diff --git a/.github/workflows/humble-binary-build.yml b/.github/workflows/humble-binary-build.yml index aac56a9..9a99b09 100644 --- a/.github/workflows/humble-binary-build.yml +++ b/.github/workflows/humble-binary-build.yml @@ -22,7 +22,6 @@ jobs: env: ROS_DISTRO: humble RDK_INSTALL_PREFIX: ${{ github.workspace }}/rdk_install - RMW_IMPLEMENTATION: rmw_cyclonedds_cpp steps: - name: Checkout uses: actions/checkout@v3 @@ -42,14 +41,13 @@ jobs: ros-humble-robot-state-publisher \ ros-humble-ros2-control \ ros-humble-ros2-controllers \ - ros-humble-rmw-cyclonedds-cpp \ ros-humble-test-msgs \ ros-humble-tinyxml2-vendor \ ros-humble-xacro - name: Build and install flexiv_rdk shell: bash run: | - git clone --branch v2.0 --depth 1 https://github.com/flexivrobotics/flexiv_rdk.git + git clone --branch v2.1 --depth 1 https://github.com/flexivrobotics/flexiv_rdk.git cd flexiv_rdk/thirdparty bash build_and_install_dependencies.sh "${RDK_INSTALL_PREFIX}" cd .. @@ -65,7 +63,6 @@ jobs: uses: ros-tooling/action-ros-ci@v0.4 env: CMAKE_PREFIX_PATH: ${{ env.RDK_INSTALL_PREFIX }} - RMW_IMPLEMENTATION: ${{ env.RMW_IMPLEMENTATION }} with: target-ros2-distro: ${{ env.ROS_DISTRO }} vcs-repo-file-url: ${{ github.workspace }}/flexiv.humble.repos diff --git a/README.md b/README.md index 2c549a8..b1033ab 100644 --- a/README.md +++ b/README.md @@ -22,17 +22,20 @@ For ROS 2 users to easily work with [RDK](https://github.com/flexivrobotics/flex | **Branch** | [humble](https://github.com/flexivrobotics/flexiv_ros2/tree/humble) | [jazzy](https://github.com/flexivrobotics/flexiv_ros2/tree/jazzy) | | **Release Status** | [![Humble Binary Build](https://github.com/flexivrobotics/flexiv_ros2/actions/workflows/humble-binary-build.yml/badge.svg?branch=humble)](https://github.com/flexivrobotics/flexiv_ros2/actions/workflows/humble-binary-build.yml) | [![Jazzy Binary Build](https://github.com/flexivrobotics/flexiv_ros2/actions/workflows/jazzy-binary-build.yml/badge.svg?branch=jazzy)](https://github.com/flexivrobotics/flexiv_ros2/actions/workflows/jazzy-binary-build.yml) | +> [!NOTE] +> **Release lines.** Two parallel version lines are maintained per ROS 2 distribution: +> +> | Line | Branch | RDK | Supported robots | +> | ------ | ------------------------------------------------------------------------- | -------- | ------------------------------------------------------- | +> | **v2** | [humble](https://github.com/flexivrobotics/flexiv_ros2/tree/humble) | RDK v2.x | Enlight-L, Enlight-LL, MICO-Core, MICO-Plus, MICO-Ultra | +> | **v1** | [humble-v1](https://github.com/flexivrobotics/flexiv_ros2/tree/humble-v1) | RDK v1.x | Rizon 4/4s/4M/4R/10/10s, AICO1/2 | +> +> The default `humble`/`jazzy` branches track the current **v2** line. For Rizon support on RDK v1.x, use the `humble-v1`/`jazzy-v1` branches. The two lines use incompatible RDK APIs and are not interchangeable. + ## Getting Started This project was developed for ROS 2 Humble (Ubuntu 22.04) and Jazzy (Ubuntu 24.04). Other versions of Ubuntu and ROS 2 may work, but are not officially supported. -This project uses CycloneDDS middleware (`rmw_cyclonedds_cpp`) for ROS 2 communication. - -> [!WARNING] -> Fast DDS middleware (`rmw_fastrtps_cpp`) is not supported in this project. -> It causes a compilation conflict with `flexiv_rdk` (conflicting DDS/CMake targets). -> This guide uses CycloneDDS (`rmw_cyclonedds_cpp`) in all setup and runtime examples. - 1. Install [ROS 2 Humble via Debian Packages](https://docs.ros.org/en/humble/Installation/Ubuntu-Install-Debians.html) 2. Install `colcon` and additional ROS packages: @@ -53,8 +56,7 @@ This project uses CycloneDDS middleware (`rmw_cyclonedds_cpp`) for ROS 2 communi ros-humble-joint-state-publisher \ ros-humble-joint-state-publisher-gui \ ros-humble-robot-state-publisher \ - ros-humble-rviz2 \ - ros-humble-rmw-cyclonedds-cpp + ros-humble-rviz2 ``` 3. Setup workspace: @@ -96,7 +98,6 @@ This project uses CycloneDDS middleware (`rmw_cyclonedds_cpp`) for ROS 2 communi ```bash cd ~/flexiv_ros2_ws source /opt/ros/humble/setup.bash - export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp colcon build --symlink-install --cmake-args -DCMAKE_PREFIX_PATH=~/rdk_install source install/setup.bash ``` @@ -106,7 +107,6 @@ This project uses CycloneDDS middleware (`rmw_cyclonedds_cpp`) for ROS 2 communi > > ```bash > source /opt/ros/humble/setup.bash -> export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp > source ~/flexiv_ros2_ws/install/setup.bash > ``` @@ -117,18 +117,12 @@ This project uses CycloneDDS middleware (`rmw_cyclonedds_cpp`) for ROS 2 communi The prerequisites of using ROS 2 with Flexiv robots are [enable RDK on the robot server](https://www.flexiv.com/software/rdk/manual/activate_rdk_server.html) and [establish connection](https://www.flexiv.com/software/rdk/manual/establish_connection.html) between the workstation PC and the robot. -Before running any `ros2 launch` command below, make sure CycloneDDS is selected: - -```bash -export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp -``` - All provided launch files prepend `${rdk_install_prefix}/lib` to `LD_LIBRARY_PATH` before starting Flexiv-backed nodes. The default launch argument assumes `flexiv_rdk` was installed to `~/rdk_install`, matching the build steps above. If you installed `flexiv_rdk` to a different prefix, pass `rdk_install_prefix:=/path/to/prefix` to the launch command. The launch file to start the single-arm robot driver is `flexiv.launch.py` - it loads and starts the robot hardware, joint states broadcaster, Flexiv robot states broadcasters, and robot controller and opens RViZ. The arguments for the launch file are as follows: -- `robot_sn` (*required*) - Serial number of the robot to connect to. Remove any space, for example: EnlightL-123456 -- `robot_type` (default: *EnlightL*) - type of the Flexiv single-arm robot. Supported values: *EnlightL* +- `robot_sn` (*required*) - Serial number of the robot to connect to. Remove any space, for example: Enlight-L-123456 +- `robot_type` (default: *Enlight-L*) - type of the Flexiv robot. Supported values: *Enlight-L* (single-arm), *Enlight-LL*, *MICO-Core*, *MICO-Plus*, *MICO-Ultra* (dual-arm). Dual-arm models share one RDK connection (one `robot_sn`) and expose `left_`/`right_` prefixed joints. **Note:** *MICO-Ultra*'s mobile base is not yet supported in `ros2_control` — its arms (and pan-tilt torso) are controllable, but the mobile base is not driven by this stack. - `rdk_control_mode` (default: *joint_position*) - Flexiv RDK control mode for ROS 2 joint position and velocity interfaces. Options: *joint_position* or *joint_impedance* - `load_gripper` (default: *false*) - loads the Flexiv Grav gripper as the end-effector of the robot and the gripper control node. - `use_fake_hardware` (default: *false*) - starts `FakeSystem` instead of real hardware. This is a simple simulation that mimics joint command to their states. @@ -143,13 +137,13 @@ The launch file to start the single-arm robot driver is `flexiv.launch.py` - it - Test with real robot: ```bash - ros2 launch flexiv_bringup flexiv.launch.py robot_sn:=[robot_sn] robot_type:=EnlightL + ros2 launch flexiv_bringup flexiv.launch.py robot_sn:=[robot_sn] robot_type:=Enlight-L ``` - Test with fake hardware (`ros2_control` capability): ```bash - ros2 launch flexiv_bringup flexiv.launch.py robot_sn:=EnlightL-123456 use_fake_hardware:=true + ros2 launch flexiv_bringup flexiv.launch.py robot_sn:=Enlight-L-123456 use_fake_hardware:=true ``` > [!TIP] @@ -176,7 +170,7 @@ ros2 launch flexiv_bringup flexiv_moveit.launch.py robot_sn:=[robot_sn] Test with fake hardware: ```bash -ros2 launch flexiv_bringup flexiv_moveit.launch.py robot_sn:=EnlightL-123456 use_fake_hardware:=true +ros2 launch flexiv_bringup flexiv_moveit.launch.py robot_sn:=Enlight-L-123456 use_fake_hardware:=true ``` ### Robot States @@ -194,16 +188,16 @@ The robot driver (`flexiv.launch.py`) publishes the following feedback states to The aggregated `/${robot_sn}/flexiv_robot_states` message also includes the unfiltered wrench fields `raw_tcp_wrench_local` and `raw_tcp_wrench`, which are not published as separate topics. -For single-arm launch files, `robot_type` is the model selector. +For dual-arm models (*Enlight-LL*, *MICO-Core*, *MICO-Plus*, *MICO-Ultra*) two robot-states broadcasters run, publishing per-arm states under `left_`/`right_`-prefixed names, e.g. `/left_${robot_sn}/flexiv_robot_states` and `/right_${robot_sn}/flexiv_robot_states` (and the corresponding `tcp_pose`, `tcp_twist`, wrench topics). `/joint_states` contains all 14 arm joints with `left_`/`right_` prefixes (plus the `${robot_sn}_torso_joint1/2` joints on *MICO-Plus*/*MICO-Ultra*). ### GPIO -All digital inputs can be accessed via the ROS topic `/{robot_sn}/gpio_inputs`, which publishes the current state of all 24 digital input ports exposed through the Flexiv control interface *(True: port high, false: port low)*. +All digital inputs can be accessed via the ROS topic `/{robot_sn}/gpio_inputs`, which publishes the current state of all 20 digital input ports (16 on the control box plus 2 in each wrist connector) exposed through the Flexiv control interface *(True: port high, false: port low)*. The digital output ports on the control box can be set by publishing to the topic `/{robot_sn}/gpio_outputs`. For example: ```bash -ros2 topic pub /EnlightL_123456/gpio_outputs flexiv_msgs/msg/GPIOStates "{states: [{pin: 0, state: true}, {pin: 2, state: true}]}" +ros2 topic pub /Enlight_L_123456/gpio_outputs flexiv_msgs/msg/GPIOStates "{states: [{pin: 0, state: true}, {pin: 2, state: true}]}" ``` ### Gripper Control diff --git a/flexiv.humble.repos b/flexiv.humble.repos index 4b7fb5e..ca4c13c 100644 --- a/flexiv.humble.repos +++ b/flexiv.humble.repos @@ -2,8 +2,8 @@ repositories: flexiv_description: type: git url: https://github.com/flexivrobotics/flexiv_description.git - version: humble-v2.0 + version: humble-v2.1 flexiv_rdk: type: git url: https://github.com/flexivrobotics/flexiv_rdk.git - version: v2.0 + version: v2.1 diff --git a/flexiv_bringup/config/flexiv_dual_controllers.yaml b/flexiv_bringup/config/flexiv_dual_controllers.yaml new file mode 100644 index 0000000..a97fe10 --- /dev/null +++ b/flexiv_bringup/config/flexiv_dual_controllers.yaml @@ -0,0 +1,57 @@ +controller_manager: + ros__parameters: + update_rate: 1000 # Hz + + flexiv_arm_controller: + type: joint_trajectory_controller/JointTrajectoryController + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + gpio_controller: + type: gpio_controller/GPIOController + + left_flexiv_robot_states_broadcaster: + type: flexiv_robot_states_broadcaster/FlexivRobotStatesBroadcaster + + right_flexiv_robot_states_broadcaster: + type: flexiv_robot_states_broadcaster/FlexivRobotStatesBroadcaster + +flexiv_arm_controller: + ros__parameters: + joints: + - left_$(var robot_sn)_joint1 + - left_$(var robot_sn)_joint2 + - left_$(var robot_sn)_joint3 + - left_$(var robot_sn)_joint4 + - left_$(var robot_sn)_joint5 + - left_$(var robot_sn)_joint6 + - left_$(var robot_sn)_joint7 + - right_$(var robot_sn)_joint1 + - right_$(var robot_sn)_joint2 + - right_$(var robot_sn)_joint3 + - right_$(var robot_sn)_joint4 + - right_$(var robot_sn)_joint5 + - right_$(var robot_sn)_joint6 + - right_$(var robot_sn)_joint7 + command_interfaces: + - position + state_interfaces: + - position + - velocity + state_publish_rate: 100.0 + action_monitor_rate: 20.0 + # Allow MoveIt to command a single-arm planning group (left_arm / right_arm) on this 14-joint + # controller. + allow_partial_joints_goal: true + constraints: + stopped_velocity_tolerance: 0.01 + goal_time: 0.0 + +left_flexiv_robot_states_broadcaster: + ros__parameters: + robot_sn: left_$(var robot_sn) + +right_flexiv_robot_states_broadcaster: + ros__parameters: + robot_sn: right_$(var robot_sn) diff --git a/flexiv_bringup/config/flexiv_mico_controllers.yaml b/flexiv_bringup/config/flexiv_mico_controllers.yaml new file mode 100644 index 0000000..f3416bd --- /dev/null +++ b/flexiv_bringup/config/flexiv_mico_controllers.yaml @@ -0,0 +1,61 @@ +controller_manager: + ros__parameters: + update_rate: 1000 # Hz + + flexiv_arm_controller: + type: joint_trajectory_controller/JointTrajectoryController + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + gpio_controller: + type: gpio_controller/GPIOController + + left_flexiv_robot_states_broadcaster: + type: flexiv_robot_states_broadcaster/FlexivRobotStatesBroadcaster + + right_flexiv_robot_states_broadcaster: + type: flexiv_robot_states_broadcaster/FlexivRobotStatesBroadcaster + +flexiv_arm_controller: + ros__parameters: + joints: + - left_$(var robot_sn)_joint1 + - left_$(var robot_sn)_joint2 + - left_$(var robot_sn)_joint3 + - left_$(var robot_sn)_joint4 + - left_$(var robot_sn)_joint5 + - left_$(var robot_sn)_joint6 + - left_$(var robot_sn)_joint7 + - right_$(var robot_sn)_joint1 + - right_$(var robot_sn)_joint2 + - right_$(var robot_sn)_joint3 + - right_$(var robot_sn)_joint4 + - right_$(var robot_sn)_joint5 + - right_$(var robot_sn)_joint6 + - right_$(var robot_sn)_joint7 + # Pan-tilt / waist torso (2-DoF EXT_AXIS group). Folded into the single arm controller so all + # actuated joints switch command mode together, as required by FlexivHardwareInterface. + - $(var robot_sn)_torso_joint1 + - $(var robot_sn)_torso_joint2 + command_interfaces: + - position + state_interfaces: + - position + - velocity + state_publish_rate: 100.0 + action_monitor_rate: 20.0 + # Allow MoveIt to command a joint subset (e.g. the both_arms / single-arm planning groups) + # without the torso joints. + allow_partial_joints_goal: true + constraints: + stopped_velocity_tolerance: 0.01 + goal_time: 0.0 + +left_flexiv_robot_states_broadcaster: + ros__parameters: + robot_sn: left_$(var robot_sn) + +right_flexiv_robot_states_broadcaster: + ros__parameters: + robot_sn: right_$(var robot_sn) diff --git a/flexiv_bringup/launch/flexiv.launch.py b/flexiv_bringup/launch/flexiv.launch.py index cb20299..029338d 100644 --- a/flexiv_bringup/launch/flexiv.launch.py +++ b/flexiv_bringup/launch/flexiv.launch.py @@ -5,6 +5,8 @@ DeclareLaunchArgument, EmitEvent, IncludeLaunchDescription, + LogInfo, + OpaqueFunction, RegisterEventHandler, SetEnvironmentVariable, ) @@ -25,127 +27,25 @@ ) -def generate_launch_description(): - single_arm_robot_types = [ - "EnlightL", - ] - robot_type_param_name = "robot_type" - robot_sn_param_name = "robot_sn" - rdk_control_mode_param_name = "rdk_control_mode" - start_rviz_param_name = "start_rviz" - load_gripper_param_name = "load_gripper" - gripper_name_param_name = "gripper_name" - 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" - robot_controller_param_name = "robot_controller" - rdk_install_prefix_param_name = "rdk_install_prefix" - - # Declare arguments - declared_arguments = [] - - declared_arguments.append( - DeclareLaunchArgument( - robot_type_param_name, - description="Type of the Flexiv single-arm robot.", - default_value="EnlightL", - choices=single_arm_robot_types, - ) - ) - - declared_arguments.append( - DeclareLaunchArgument( - robot_sn_param_name, - description="Serial number of the robot to connect to. Remove any space, for example: EnlightL-123456", - ) - ) - - declared_arguments.append( - 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", - choices=["joint_position", "joint_impedance"], - ) - ) - - declared_arguments.append( - DeclareLaunchArgument( - start_rviz_param_name, - default_value="true", - description="Start RViz automatically with the launch file", - ) - ) - - declared_arguments.append( - DeclareLaunchArgument( - load_gripper_param_name, - default_value="false", - description="Flag to load the Flexiv Grav gripper as the end-effector of the robot.", - ) - ) - - declared_arguments.append( - DeclareLaunchArgument( - gripper_name_param_name, - default_value="Flexiv-GN01", - description="Full name of the gripper to be controlled, can be found in Flexiv Elements -> Settings -> Device", - ) - ) - - 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 used for simple simulations. \ - Used only if 'use_fake_hardware' parameter is true.", - ) - ) - - declared_arguments.append( - DeclareLaunchArgument( - robot_controller_param_name, - default_value="flexiv_arm_controller", - description="Robot controller to start. Available: flexiv_arm_controller", - ) - ) - - declared_arguments.append( - DeclareLaunchArgument( - rdk_install_prefix_param_name, - default_value=os.path.expanduser("~/rdk_install"), - description="Prefix where flexiv_rdk and its shared-library dependencies are installed.", - ) - ) - +def launch_setup(context): # Initialize Arguments - robot_type = LaunchConfiguration(robot_type_param_name) - robot_sn = LaunchConfiguration(robot_sn_param_name) - rdk_control_mode = LaunchConfiguration(rdk_control_mode_param_name) - start_rviz = LaunchConfiguration(start_rviz_param_name) - load_gripper = LaunchConfiguration(load_gripper_param_name) - gripper_name = LaunchConfiguration(gripper_name_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) - robot_controller = LaunchConfiguration(robot_controller_param_name) - rdk_install_prefix = LaunchConfiguration(rdk_install_prefix_param_name) + robot_type = LaunchConfiguration("robot_type") + robot_sn = LaunchConfiguration("robot_sn") + robot_type_str = robot_type.perform(context) + dual_arm_robot_types = ["Enlight-LL", "MICO-Core", "MICO-Plus", "MICO-Ultra"] + pan_tilt_robot_types = ["MICO-Plus", "MICO-Ultra"] + is_dual = robot_type_str in dual_arm_robot_types + rdk_control_mode = LaunchConfiguration("rdk_control_mode") + start_rviz = LaunchConfiguration("start_rviz") + load_gripper = LaunchConfiguration("load_gripper") + gripper_name = LaunchConfiguration("gripper_name") + gripper_name_left = LaunchConfiguration("gripper_name_left") + gripper_name_right = LaunchConfiguration("gripper_name_right") + robot_controller = LaunchConfiguration("robot_controller") + rdk_install_prefix = LaunchConfiguration("rdk_install_prefix") + load_mounted_ft_sensor = LaunchConfiguration("load_mounted_ft_sensor") + use_fake_hardware = LaunchConfiguration("use_fake_hardware") + fake_sensor_commands = LaunchConfiguration("fake_sensor_commands") gripper_ready_gate_condition = PythonExpression( [ "'", @@ -156,27 +56,10 @@ def generate_launch_description(): ] ) - set_rdk_ld_library_path = SetEnvironmentVariable( - name="LD_LIBRARY_PATH", - value=[ - PathJoinSubstitution([rdk_install_prefix, "lib"]), - PythonExpression( - [ - "':' if '", - EnvironmentVariable("LD_LIBRARY_PATH", default_value=""), - "' else ''", - ] - ), - EnvironmentVariable("LD_LIBRARY_PATH", default_value=""), - ], - ) - # Get URDF via xacro flexiv_urdf_xacro = PathJoinSubstitution( [FindPackageShare("flexiv_description"), "urdf", "flexiv.urdf.xacro"] ) - - # Get URDF via xacro robot_description_content = ParameterValue( Command( [ @@ -212,14 +95,12 @@ def generate_launch_description(): ), value_type=str, ) - robot_description = {"robot_description": robot_description_content} # RViZ rviz_config_file = PathJoinSubstitution( [FindPackageShare("flexiv_description"), "rviz", "view_flexiv.rviz"] ) - rviz_node = Node( package="rviz2", executable="rviz2", @@ -229,9 +110,15 @@ def generate_launch_description(): condition=IfCondition(start_rviz), ) - # Robot controllers + # Robot controllers — select by topology (single 7-joint / dual 14-joint / MICO 16-joint). + if robot_type_str in pan_tilt_robot_types: + controllers_file = "flexiv_mico_controllers.yaml" + elif is_dual: + controllers_file = "flexiv_dual_controllers.yaml" + else: + controllers_file = "flexiv_controllers.yaml" robot_controllers = PathJoinSubstitution( - [FindPackageShare("flexiv_bringup"), "config", "flexiv_controllers.yaml"] + [FindPackageShare("flexiv_bringup"), "config", controllers_file] ) # Controller Manager @@ -258,6 +145,8 @@ def generate_launch_description(): "source_list": [ "flexiv_arm/joint_states", "flexiv_gripper_node/gripper_joint_states", + "left_flexiv_gripper_node/gripper_joint_states", + "right_flexiv_gripper_node/gripper_joint_states", ], "rate": 30, } @@ -291,50 +180,166 @@ def generate_launch_description(): ], ) - # 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), - ) - - # Include gripper launch file - load_gripper_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution( - [ - FindPackageShare("flexiv_gripper"), - "launch", - "flexiv_gripper.launch.py", - ] + # Run Flexiv robot states broadcaster(s). Single-arm robots publish one; dual-arm robots + # publish one per arm (left_ / right_), matching the selected controllers yaml. + robot_states_broadcaster_spawners = [] + if is_dual: + robot_states_broadcaster_spawners.append( + Node( + package="controller_manager", + executable="spawner", + arguments=["left_flexiv_robot_states_broadcaster"], + condition=UnlessCondition(use_fake_hardware), ) - ), - launch_arguments={ + ) + robot_states_broadcaster_spawners.append( + Node( + package="controller_manager", + executable="spawner", + arguments=["right_flexiv_robot_states_broadcaster"], + condition=UnlessCondition(use_fake_hardware), + ) + ) + else: + robot_states_broadcaster_spawners.append( + Node( + package="controller_manager", + executable="spawner", + arguments=["flexiv_robot_states_broadcaster"], + parameters=[{"robot_sn": robot_sn}], + condition=UnlessCondition(use_fake_hardware), + ) + ) + + # ---- Gripper launch + readiness gating ---- + # Grippers run only on real hardware (gripper_ready_gate_condition). Single-arm robots run one + # gripper node; dual-arm robots run one per arm group (ARM_1 = left, ARM_2 = right), each a lite + # RDK instance sharing the driver's connection. + def gripper_launch(node_name, name, joint_group=None, gripper_joint_names=None): + args = { + "gripper_node_name": node_name, "robot_sn": robot_sn, - "gripper_name": gripper_name, + "gripper_name": name, "use_fake_hardware": use_fake_hardware, "use_lite_rdk": "true", "rdk_install_prefix": rdk_install_prefix, - }.items(), - condition=IfCondition(load_gripper), - ) + } + if joint_group is not None: + args["joint_group"] = joint_group + if gripper_joint_names is not None: + args["gripper_joint_names"] = gripper_joint_names + return IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + FindPackageShare("flexiv_gripper"), + "launch", + "flexiv_gripper.launch.py", + ] + ) + ), + launch_arguments=args.items(), + condition=IfCondition(gripper_ready_gate_condition), + ) - gripper_ready_waiter = Node( - package="flexiv_gripper", - executable="wait_for_gripper_ready", - name="wait_for_gripper_ready", - parameters=[{"ready_topic": "/flexiv_gripper_node/ready"}], - output="screen", - condition=IfCondition(gripper_ready_gate_condition), - ) + def gripper_ready_waiter_node(node_name, topic): + return Node( + package="flexiv_gripper", + executable="wait_for_gripper_ready", + name=node_name, + parameters=[{"ready_topic": topic}], + output="screen", + condition=IfCondition(gripper_ready_gate_condition), + ) def launch_robot_controller_after_gripper_ready(event, context): if event.returncode == 0: return [robot_controller_spawner] - return [ - EmitEvent(event=Shutdown(reason="flexiv_gripper_node did not report ready")) + return [EmitEvent(event=Shutdown(reason="flexiv gripper did not report ready"))] + + # Nodes added directly, and the event handlers that sequence the gripper(s) -> controller. + gripper_nodes = [] + gripper_event_handlers = [] + if is_dual: + load_gripper_launch_left = gripper_launch( + "left_flexiv_gripper_node", + gripper_name_left, + "ARM_1", + gripper_joint_names=["[left_", robot_sn, "_finger_width_joint]"], + ) + load_gripper_launch_right = gripper_launch( + "right_flexiv_gripper_node", + gripper_name_right, + "ARM_2", + gripper_joint_names=["[right_", robot_sn, "_finger_width_joint]"], + ) + left_gripper_ready_waiter = gripper_ready_waiter_node( + "wait_for_left_gripper_ready", "/left_flexiv_gripper_node/ready" + ) + right_gripper_ready_waiter = gripper_ready_waiter_node( + "wait_for_right_gripper_ready", "/right_flexiv_gripper_node/ready" + ) + + def launch_right_waiter_after_left_ready(event, context): + if event.returncode == 0: + return [right_gripper_ready_waiter] + return [ + EmitEvent( + event=Shutdown( + reason="left_flexiv_gripper_node did not report ready" + ) + ) + ] + + gripper_nodes = [left_gripper_ready_waiter] + gripper_event_handlers = [ + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=joint_state_broadcaster_spawner, + on_exit=[load_gripper_launch_left, load_gripper_launch_right], + ), + condition=IfCondition(gripper_ready_gate_condition), + ), + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=left_gripper_ready_waiter, + on_exit=launch_right_waiter_after_left_ready, + ), + condition=IfCondition(gripper_ready_gate_condition), + ), + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=right_gripper_ready_waiter, + on_exit=launch_robot_controller_after_gripper_ready, + ), + condition=IfCondition(gripper_ready_gate_condition), + ), + ] + else: + load_gripper_launch = gripper_launch( + "flexiv_gripper_node", + gripper_name, + gripper_joint_names=["[", robot_sn, "_finger_width_joint]"], + ) + gripper_ready_waiter = gripper_ready_waiter_node( + "wait_for_gripper_ready", "/flexiv_gripper_node/ready" + ) + gripper_nodes = [gripper_ready_waiter] + gripper_event_handlers = [ + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=joint_state_broadcaster_spawner, + on_exit=[load_gripper_launch], + ), + condition=IfCondition(gripper_ready_gate_condition), + ), + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=gripper_ready_waiter, + on_exit=launch_robot_controller_after_gripper_ready, + ), + condition=IfCondition(gripper_ready_gate_condition), + ), ] # Run gpio controller @@ -346,7 +351,8 @@ def launch_robot_controller_after_gripper_ready(event, context): condition=UnlessCondition(use_fake_hardware), ) - # Delay start of robot_controller after `joint_state_broadcaster` + # When no gripper is loaded, start the robot controller right after joint_state_broadcaster. + # (With a gripper, the gripper_event_handlers chain starts the controller once grippers ready.) delay_robot_controller_spawner_after_joint_state_broadcaster_spawner = ( RegisterEventHandler( event_handler=OnProcessExit( @@ -357,22 +363,6 @@ def launch_robot_controller_after_gripper_ready(event, context): ) ) - delay_gripper_launch_after_joint_state_broadcaster_spawner = RegisterEventHandler( - event_handler=OnProcessExit( - target_action=joint_state_broadcaster_spawner, - on_exit=[load_gripper_launch], - ), - condition=IfCondition(gripper_ready_gate_condition), - ) - - delay_robot_controller_spawner_after_gripper_ready = RegisterEventHandler( - event_handler=OnProcessExit( - target_action=gripper_ready_waiter, - on_exit=launch_robot_controller_after_gripper_ready, - ), - condition=IfCondition(gripper_ready_gate_condition), - ) - # Delay rviz start after `robot_controller_spawner` delay_rviz_after_robot_controller_spawner = RegisterEventHandler( event_handler=OnProcessExit( @@ -381,18 +371,166 @@ def launch_robot_controller_after_gripper_ready(event, context): ) ) + # MICO-Ultra mobile base alert + mico_ultra_alert = LogInfo( + msg=( + "MICO-Ultra mobile base is NOT supported in ros2_control (v2.1). " + "The arms and pan-tilt torso are controllable; the mobile base is not " + "driven by this stack." + ), + condition=IfCondition(PythonExpression(["'", robot_type, "' == 'MICO-Ultra'"])), + ) + nodes = [ + mico_ultra_alert, ros2_control_node, joint_state_publisher_node, robot_state_publisher_node, - gripper_ready_waiter, joint_state_broadcaster_spawner, - flexiv_robot_states_broadcaster_spawner, + *robot_states_broadcaster_spawners, gpio_controller_spawner, - delay_gripper_launch_after_joint_state_broadcaster_spawner, + *gripper_nodes, + *gripper_event_handlers, delay_robot_controller_spawner_after_joint_state_broadcaster_spawner, - delay_robot_controller_spawner_after_gripper_ready, delay_rviz_after_robot_controller_spawner, ] - return LaunchDescription(declared_arguments + [set_rdk_ld_library_path] + nodes) + return nodes + + +def generate_launch_description(): + robot_types = [ + "Enlight-L", + "Enlight-LL", + "MICO-Core", + "MICO-Plus", + "MICO-Ultra", + ] + + declared_arguments = [] + + declared_arguments.append( + DeclareLaunchArgument( + "robot_type", + description="Type of the Flexiv robot. Single-arm: Enlight-L. Dual-arm: Enlight-LL, MICO-Core, MICO-Plus, MICO-Ultra.", + default_value="Enlight-L", + choices=robot_types, + ) + ) + + declared_arguments.append( + DeclareLaunchArgument( + "robot_sn", + description="Serial number of the robot to connect to. Remove any space, for example: Enlight-L-123456", + ) + ) + + declared_arguments.append( + DeclareLaunchArgument( + "rdk_control_mode", + default_value="joint_position", + description="RDK control mode for the ROS 2 control joint position and velocity interfaces. Options: joint_position, joint_impedance", + choices=["joint_position", "joint_impedance"], + ) + ) + + declared_arguments.append( + DeclareLaunchArgument( + "start_rviz", + default_value="true", + description="Start RViz automatically with the launch file", + ) + ) + + declared_arguments.append( + DeclareLaunchArgument( + "load_gripper", + default_value="false", + description="Flag to load the Flexiv Grav gripper as the end-effector of the robot.", + ) + ) + + declared_arguments.append( + DeclareLaunchArgument( + "gripper_name", + default_value="Flexiv-GN01", + description="Full name of the gripper to be controlled, can be found in Flexiv Elements -> Settings -> Device. For single-arm robots and as the default for both arms of a dual-arm robot.", + ) + ) + + declared_arguments.append( + DeclareLaunchArgument( + "gripper_name_left", + default_value=LaunchConfiguration("gripper_name"), + description="Gripper device name for the LEFT arm (ARM_1) of a dual-arm robot. Defaults to 'gripper_name'.", + ) + ) + + declared_arguments.append( + DeclareLaunchArgument( + "gripper_name_right", + default_value=LaunchConfiguration("gripper_name"), + description="Gripper device name for the RIGHT arm (ARM_2) of a dual-arm robot. Defaults to 'gripper_name'.", + ) + ) + + declared_arguments.append( + DeclareLaunchArgument( + "load_mounted_ft_sensor", + default_value="false", + description="Flag to load the mounted force torque sensor.", + ) + ) + + declared_arguments.append( + DeclareLaunchArgument( + "use_fake_hardware", + default_value="false", + description="Start robot with fake hardware mirroring command to its states.", + ) + ) + + declared_arguments.append( + DeclareLaunchArgument( + "fake_sensor_commands", + default_value="false", + description="Enable fake command interfaces for sensors used for simple simulations. \ + Used only if 'use_fake_hardware' parameter is true.", + ) + ) + + declared_arguments.append( + DeclareLaunchArgument( + "robot_controller", + default_value="flexiv_arm_controller", + description="Robot controller to start. Available: flexiv_arm_controller", + ) + ) + + declared_arguments.append( + DeclareLaunchArgument( + "rdk_install_prefix", + default_value=os.path.expanduser("~/rdk_install"), + description="Prefix where flexiv_rdk and its shared-library dependencies are installed.", + ) + ) + + set_rdk_ld_library_path = SetEnvironmentVariable( + name="LD_LIBRARY_PATH", + value=[ + PathJoinSubstitution([LaunchConfiguration("rdk_install_prefix"), "lib"]), + PythonExpression( + [ + "':' if '", + EnvironmentVariable("LD_LIBRARY_PATH", default_value=""), + "' else ''", + ] + ), + EnvironmentVariable("LD_LIBRARY_PATH", default_value=""), + ], + ) + + return LaunchDescription( + declared_arguments + + [set_rdk_ld_library_path, OpaqueFunction(function=launch_setup)] + ) diff --git a/flexiv_bringup/launch/flexiv_moveit.launch.py b/flexiv_bringup/launch/flexiv_moveit.launch.py index 7de525e..a9046ac 100644 --- a/flexiv_bringup/launch/flexiv_moveit.launch.py +++ b/flexiv_bringup/launch/flexiv_moveit.launch.py @@ -7,6 +7,7 @@ DeclareLaunchArgument, EmitEvent, IncludeLaunchDescription, + LogInfo, OpaqueFunction, RegisterEventHandler, SetEnvironmentVariable, @@ -52,10 +53,18 @@ def launch_setup(context): robot_type = LaunchConfiguration("robot_type") robot_sn = LaunchConfiguration("robot_sn") robot_sn_str = robot_sn.perform(context) + robot_type_str = robot_type.perform(context) + dual_arm_robot_types = ["Enlight-LL", "MICO-Core", "MICO-Plus", "MICO-Ultra"] + pan_tilt_robot_types = ["MICO-Plus", "MICO-Ultra"] + is_dual = robot_type_str in dual_arm_robot_types rdk_control_mode = LaunchConfiguration("rdk_control_mode") start_rviz = LaunchConfiguration("start_rviz") load_gripper = LaunchConfiguration("load_gripper") gripper_name = LaunchConfiguration("gripper_name") + gripper_name_left = LaunchConfiguration("gripper_name_left") + gripper_name_right = LaunchConfiguration("gripper_name_right") + robot_controller = LaunchConfiguration("robot_controller") + rdk_install_prefix = LaunchConfiguration("rdk_install_prefix") load_mounted_ft_sensor = LaunchConfiguration("load_mounted_ft_sensor") use_fake_hardware = LaunchConfiguration("use_fake_hardware") fake_sensor_commands = LaunchConfiguration("fake_sensor_commands") @@ -174,9 +183,15 @@ def launch_setup(context): ) ompl_planning_pipeline_config["move_group"].update(ompl_planning_yaml) - # Trajectory Execution Configuration + # Trajectory Execution Configuration. + if robot_type_str in pan_tilt_robot_types: + moveit_controllers_file = "config/moveit_controllers_mico.yaml" + elif is_dual: + moveit_controllers_file = "config/moveit_controllers_dual.yaml" + else: + moveit_controllers_file = "config/moveit_controllers.yaml" moveit_simple_controllers_yaml = load_yaml( - "flexiv_moveit_config", "config/moveit_controllers.yaml", replacements + "flexiv_moveit_config", moveit_controllers_file, replacements ) moveit_controllers = { @@ -259,9 +274,15 @@ def launch_setup(context): parameters=[robot_description], ) - # Robot controllers + # Robot controllers — select by topology (must match moveit_controllers selection above). + if robot_type_str in pan_tilt_robot_types: + controllers_file = "flexiv_mico_controllers.yaml" + elif is_dual: + controllers_file = "flexiv_dual_controllers.yaml" + else: + controllers_file = "flexiv_controllers.yaml" robot_controllers = PathJoinSubstitution( - [FindPackageShare("flexiv_bringup"), "config", "flexiv_controllers.yaml"] + [FindPackageShare("flexiv_bringup"), "config", controllers_file] ) # Run controller manager @@ -288,6 +309,8 @@ def launch_setup(context): "source_list": [ "flexiv_arm/joint_states", "flexiv_gripper_node/gripper_joint_states", + "left_flexiv_gripper_node/gripper_joint_states", + "right_flexiv_gripper_node/gripper_joint_states", ], "rate": 30, } @@ -298,11 +321,7 @@ def launch_setup(context): robot_controller_spawner = Node( package="controller_manager", executable="spawner", - arguments=[ - "flexiv_arm_controller", - "--controller-manager", - "/controller_manager", - ], + arguments=[robot_controller, "--controller-manager", "/controller_manager"], ) # Run joint state broadcaster @@ -316,50 +335,166 @@ def launch_setup(context): ], ) - # 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), - ) - - # Include gripper launch file - load_gripper_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution( - [ - FindPackageShare("flexiv_gripper"), - "launch", - "flexiv_gripper.launch.py", - ] + # Run Flexiv robot states broadcaster(s). Single-arm robots publish one; dual-arm robots + # publish one per arm (left_ / right_), matching the selected controllers yaml. + robot_states_broadcaster_spawners = [] + if is_dual: + robot_states_broadcaster_spawners.append( + Node( + package="controller_manager", + executable="spawner", + arguments=["left_flexiv_robot_states_broadcaster"], + condition=UnlessCondition(use_fake_hardware), ) - ), - launch_arguments={ + ) + robot_states_broadcaster_spawners.append( + Node( + package="controller_manager", + executable="spawner", + arguments=["right_flexiv_robot_states_broadcaster"], + condition=UnlessCondition(use_fake_hardware), + ) + ) + else: + robot_states_broadcaster_spawners.append( + Node( + package="controller_manager", + executable="spawner", + arguments=["flexiv_robot_states_broadcaster"], + parameters=[{"robot_sn": robot_sn}], + condition=UnlessCondition(use_fake_hardware), + ) + ) + + # ---- Gripper launch + readiness gating (mirrors flexiv.launch.py) ---- + # Grippers run only on real hardware (gripper_ready_gate_condition). Single-arm robots run one + # gripper node; dual-arm robots run one per arm group (ARM_1 = left, ARM_2 = right), each a lite + # RDK instance sharing the driver's connection. + def gripper_launch(node_name, name, joint_group=None, gripper_joint_names=None): + args = { + "gripper_node_name": node_name, "robot_sn": robot_sn, - "gripper_name": gripper_name, + "gripper_name": name, "use_fake_hardware": use_fake_hardware, "use_lite_rdk": "true", - "rdk_install_prefix": LaunchConfiguration("rdk_install_prefix"), - }.items(), - condition=IfCondition(load_gripper), - ) + "rdk_install_prefix": rdk_install_prefix, + } + if joint_group is not None: + args["joint_group"] = joint_group + if gripper_joint_names is not None: + args["gripper_joint_names"] = gripper_joint_names + return IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + FindPackageShare("flexiv_gripper"), + "launch", + "flexiv_gripper.launch.py", + ] + ) + ), + launch_arguments=args.items(), + condition=IfCondition(gripper_ready_gate_condition), + ) - gripper_ready_waiter = Node( - package="flexiv_gripper", - executable="wait_for_gripper_ready", - name="wait_for_gripper_ready", - parameters=[{"ready_topic": "/flexiv_gripper_node/ready"}], - output="screen", - condition=IfCondition(gripper_ready_gate_condition), - ) + def gripper_ready_waiter_node(node_name, topic): + return Node( + package="flexiv_gripper", + executable="wait_for_gripper_ready", + name=node_name, + parameters=[{"ready_topic": topic}], + output="screen", + condition=IfCondition(gripper_ready_gate_condition), + ) def launch_robot_controller_after_gripper_ready(event, context): if event.returncode == 0: return [robot_controller_spawner] - return [ - EmitEvent(event=Shutdown(reason="flexiv_gripper_node did not report ready")) + return [EmitEvent(event=Shutdown(reason="flexiv gripper did not report ready"))] + + # Nodes added directly, and the event handlers that sequence the gripper(s) -> controller. + gripper_nodes = [] + gripper_event_handlers = [] + if is_dual: + load_gripper_launch_left = gripper_launch( + "left_flexiv_gripper_node", + gripper_name_left, + "ARM_1", + gripper_joint_names=["[left_", robot_sn, "_finger_width_joint]"], + ) + load_gripper_launch_right = gripper_launch( + "right_flexiv_gripper_node", + gripper_name_right, + "ARM_2", + gripper_joint_names=["[right_", robot_sn, "_finger_width_joint]"], + ) + left_gripper_ready_waiter = gripper_ready_waiter_node( + "wait_for_left_gripper_ready", "/left_flexiv_gripper_node/ready" + ) + right_gripper_ready_waiter = gripper_ready_waiter_node( + "wait_for_right_gripper_ready", "/right_flexiv_gripper_node/ready" + ) + + def launch_right_waiter_after_left_ready(event, context): + if event.returncode == 0: + return [right_gripper_ready_waiter] + return [ + EmitEvent( + event=Shutdown( + reason="left_flexiv_gripper_node did not report ready" + ) + ) + ] + + gripper_nodes = [left_gripper_ready_waiter] + gripper_event_handlers = [ + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=joint_state_broadcaster_spawner, + on_exit=[load_gripper_launch_left, load_gripper_launch_right], + ), + condition=IfCondition(gripper_ready_gate_condition), + ), + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=left_gripper_ready_waiter, + on_exit=launch_right_waiter_after_left_ready, + ), + condition=IfCondition(gripper_ready_gate_condition), + ), + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=right_gripper_ready_waiter, + on_exit=launch_robot_controller_after_gripper_ready, + ), + condition=IfCondition(gripper_ready_gate_condition), + ), + ] + else: + load_gripper_launch = gripper_launch( + "flexiv_gripper_node", + gripper_name, + gripper_joint_names=["[", robot_sn, "_finger_width_joint]"], + ) + gripper_ready_waiter = gripper_ready_waiter_node( + "wait_for_gripper_ready", "/flexiv_gripper_node/ready" + ) + gripper_nodes = [gripper_ready_waiter] + gripper_event_handlers = [ + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=joint_state_broadcaster_spawner, + on_exit=[load_gripper_launch], + ), + condition=IfCondition(gripper_ready_gate_condition), + ), + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=gripper_ready_waiter, + on_exit=launch_robot_controller_after_gripper_ready, + ), + condition=IfCondition(gripper_ready_gate_condition), + ), ] # Servo node for realtime control @@ -391,7 +526,8 @@ def launch_robot_controller_after_gripper_ready(event, context): condition=UnlessCondition(use_fake_hardware), ) - # Delay start of robot_controller after `joint_state_broadcaster` + # When no gripper is loaded, start the robot controller right after joint_state_broadcaster. + # (With a gripper, the gripper_event_handlers chain starts the controller once grippers ready.) delay_robot_controller_spawner_after_joint_state_broadcaster_spawner = ( RegisterEventHandler( event_handler=OnProcessExit( @@ -402,22 +538,6 @@ def launch_robot_controller_after_gripper_ready(event, context): ) ) - delay_gripper_launch_after_joint_state_broadcaster_spawner = RegisterEventHandler( - event_handler=OnProcessExit( - target_action=joint_state_broadcaster_spawner, - on_exit=[load_gripper_launch], - ), - condition=IfCondition(gripper_ready_gate_condition), - ) - - delay_robot_controller_spawner_after_gripper_ready = RegisterEventHandler( - event_handler=OnProcessExit( - target_action=gripper_ready_waiter, - on_exit=launch_robot_controller_after_gripper_ready, - ), - condition=IfCondition(gripper_ready_gate_condition), - ) - # Delay move_group start after `robot_controller_spawner` delay_move_group_after_robot_controller_spawner = RegisterEventHandler( event_handler=OnProcessExit( @@ -434,18 +554,28 @@ def launch_robot_controller_after_gripper_ready(event, context): ) ) + # MICO-Ultra mobile base alert + mico_ultra_alert = LogInfo( + msg=( + "MICO-Ultra mobile base is NOT supported in ros2_control (v2.1). " + "The arms and pan-tilt torso are controllable; the mobile base is not " + "driven by this stack." + ), + condition=IfCondition(PythonExpression(["'", robot_type, "' == 'MICO-Ultra'"])), + ) + nodes = [ + mico_ultra_alert, ros2_control_node, joint_state_publisher_node, robot_state_publisher_node, - gripper_ready_waiter, joint_state_broadcaster_spawner, - flexiv_robot_states_broadcaster_spawner, + *robot_states_broadcaster_spawners, gpio_controller_spawner, servo_node, - delay_gripper_launch_after_joint_state_broadcaster_spawner, + *gripper_nodes, + *gripper_event_handlers, delay_robot_controller_spawner_after_joint_state_broadcaster_spawner, - delay_robot_controller_spawner_after_gripper_ready, delay_move_group_after_robot_controller_spawner, delay_rviz_after_robot_controller_spawner, ] @@ -456,23 +586,27 @@ def launch_robot_controller_after_gripper_ready(event, context): def generate_launch_description(): # Declare command-line arguments declared_arguments = [] - single_arm_robot_types = [ - "EnlightL", + robot_types = [ + "Enlight-L", + "Enlight-LL", + "MICO-Core", + "MICO-Plus", + "MICO-Ultra", ] declared_arguments.append( DeclareLaunchArgument( "robot_type", - description="Type of the Flexiv single-arm robot.", - default_value="EnlightL", - choices=single_arm_robot_types, + description="Type of the Flexiv robot. Single-arm: Enlight-L. Dual-arm: Enlight-LL, MICO-Core, MICO-Plus, MICO-Ultra.", + default_value="Enlight-L", + choices=robot_types, ) ) declared_arguments.append( DeclareLaunchArgument( "robot_sn", - description="Serial number of the robot to connect to. Remove any space, for example: EnlightL-123456", + description="Serial number of the robot to connect to. Remove any space, for example: Enlight-L-123456", ) ) @@ -505,7 +639,31 @@ def generate_launch_description(): DeclareLaunchArgument( "gripper_name", default_value="Flexiv-GN01", - description="Full name of the gripper to be controlled, can be found in Flexiv Elements -> Settings -> Device", + description="Full name of the gripper to be controlled, can be found in Flexiv Elements -> Settings -> Device. For single-arm robots and as the default for both arms of a dual-arm robot.", + ) + ) + + declared_arguments.append( + DeclareLaunchArgument( + "gripper_name_left", + default_value=LaunchConfiguration("gripper_name"), + description="Gripper device name for the LEFT arm (ARM_1) of a dual-arm robot. Defaults to 'gripper_name'.", + ) + ) + + declared_arguments.append( + DeclareLaunchArgument( + "gripper_name_right", + default_value=LaunchConfiguration("gripper_name"), + description="Gripper device name for the RIGHT arm (ARM_2) of a dual-arm robot. Defaults to 'gripper_name'.", + ) + ) + + declared_arguments.append( + DeclareLaunchArgument( + "robot_controller", + default_value="flexiv_arm_controller", + description="Robot controller to start. Available: flexiv_arm_controller", ) ) diff --git a/flexiv_bringup/launch/test_joint_trajectory_controller.launch.py b/flexiv_bringup/launch/test_joint_trajectory_controller.launch.py index 45e5e77..f39aa2d 100644 --- a/flexiv_bringup/launch/test_joint_trajectory_controller.launch.py +++ b/flexiv_bringup/launch/test_joint_trajectory_controller.launch.py @@ -21,7 +21,7 @@ def generate_launch_description(): [ DeclareLaunchArgument( name="robot_sn", - description="Serial number of the robot to connect to. Remove any space, for example: EnlightL-123456", + description="Serial number of the robot to connect to. Remove any space, for example: Enlight-L-123456", ), Node( package="flexiv_test_nodes", diff --git a/flexiv_controllers/flexiv_robot_states_broadcaster/CMakeLists.txt b/flexiv_controllers/flexiv_robot_states_broadcaster/CMakeLists.txt index 759d66d..0344448 100644 --- a/flexiv_controllers/flexiv_robot_states_broadcaster/CMakeLists.txt +++ b/flexiv_controllers/flexiv_robot_states_broadcaster/CMakeLists.txt @@ -1,9 +1,9 @@ cmake_minimum_required(VERSION 3.5) project(flexiv_robot_states_broadcaster LANGUAGES CXX) -# Default to C++14 if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD 20) + set(CMAKE_CXX_STANDARD_REQUIRED ON) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") diff --git a/flexiv_controllers/flexiv_robot_states_broadcaster/include/flexiv_robot_states_broadcaster/flexiv_robot_states.hpp b/flexiv_controllers/flexiv_robot_states_broadcaster/include/flexiv_robot_states_broadcaster/flexiv_robot_states.hpp index 1c74d77..41b0c4a 100644 --- a/flexiv_controllers/flexiv_robot_states_broadcaster/include/flexiv_robot_states_broadcaster/flexiv_robot_states.hpp +++ b/flexiv_controllers/flexiv_robot_states_broadcaster/include/flexiv_robot_states_broadcaster/flexiv_robot_states.hpp @@ -105,7 +105,6 @@ class FlexivRobotStates : public SemanticComponentInterfacedq; message.dtheta = flexiv_robot_states_ptr->dtheta; message.tau = flexiv_robot_states_ptr->tau; - message.tau_des = flexiv_robot_states_ptr->tau_des; message.tau_dot = flexiv_robot_states_ptr->tau_dot; message.tau_ext = flexiv_robot_states_ptr->tau_ext; message.tau_interact = flexiv_robot_states_ptr->tau_interact; diff --git a/flexiv_gripper/CMakeLists.txt b/flexiv_gripper/CMakeLists.txt index e407afb..0820b8a 100644 --- a/flexiv_gripper/CMakeLists.txt +++ b/flexiv_gripper/CMakeLists.txt @@ -1,9 +1,8 @@ cmake_minimum_required(VERSION 3.5) project(flexiv_gripper) -# Default to C++17 if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD 20) set(CMAKE_CXX_STANDARD_REQUIRED ON) endif() diff --git a/flexiv_gripper/include/flexiv_gripper/gripper_action_server.hpp b/flexiv_gripper/include/flexiv_gripper/gripper_action_server.hpp index 5fe2be8..5c1e28e 100644 --- a/flexiv_gripper/include/flexiv_gripper/gripper_action_server.hpp +++ b/flexiv_gripper/include/flexiv_gripper/gripper_action_server.hpp @@ -99,6 +99,11 @@ class GripperActionServer : public rclcpp::Node std::unique_ptr gripper_; std::unique_ptr tool_; + // Joint group whose gripper this node controls. + // Only single-arm groups (ARM_1, ARM_2) are valid for grippers. + // A dual-arm robot runs two instances of this node, one per arm group. + flexiv::rdk::JointGroup joint_group_; + rclcpp_action::Server::SharedPtr grasp_action_server_; rclcpp_action::Server::SharedPtr move_action_server_; rclcpp_action::Server::SharedPtr gripper_command_action_server_; @@ -185,7 +190,7 @@ class GripperActionServer : public rclcpp::Node while (!IsResultReady(result_future, future_wait_timeout_) && rclcpp::ok()) { if (goal_handle->is_canceling()) { - gripper_->Stop(); + gripper_->Stop(joint_group_); auto result = result_future.get(); RCLCPP_INFO( this->get_logger(), "Gripper %s action has been canceled", action_name.c_str()); diff --git a/flexiv_gripper/launch/flexiv_gripper.launch.py b/flexiv_gripper/launch/flexiv_gripper.launch.py index 43f21b2..266071f 100644 --- a/flexiv_gripper/launch/flexiv_gripper.launch.py +++ b/flexiv_gripper/launch/flexiv_gripper.launch.py @@ -19,6 +19,7 @@ def generate_launch_description(): use_fake_hardware_param_name = "use_fake_hardware" use_lite_rdk_param_name = "use_lite_rdk" gripper_joint_names_param_name = "gripper_joint_names" + joint_group_param_name = "joint_group" rdk_install_prefix_param_name = "rdk_install_prefix" # Declare arguments @@ -35,7 +36,7 @@ def generate_launch_description(): declared_arguments.append( DeclareLaunchArgument( robot_sn_param_name, - description="Serial number of the robot to connect to. Remove any space, for example: EnlightL-123456", + description="Serial number of the robot to connect to. Remove any space, for example: Enlight-L-123456", ) ) @@ -71,6 +72,15 @@ def generate_launch_description(): ) ) + declared_arguments.append( + DeclareLaunchArgument( + joint_group_param_name, + default_value="", + description="Joint group whose gripper to control (e.g. ARM_1 or ARM_2). Leave empty to " + "auto-detect on single-arm robots; must be set for dual-arm robots.", + ) + ) + declared_arguments.append( DeclareLaunchArgument( rdk_install_prefix_param_name, @@ -86,6 +96,7 @@ def generate_launch_description(): use_fake_hardware = LaunchConfiguration(use_fake_hardware_param_name) use_lite_rdk = LaunchConfiguration(use_lite_rdk_param_name) gripper_joint_names = LaunchConfiguration(gripper_joint_names_param_name) + joint_group = LaunchConfiguration(joint_group_param_name) rdk_install_prefix = LaunchConfiguration(rdk_install_prefix_param_name) set_rdk_ld_library_path = SetEnvironmentVariable( @@ -112,6 +123,7 @@ def generate_launch_description(): "gripper_name": gripper_name, "gripper_joint_names": gripper_joint_names, "use_lite_rdk": use_lite_rdk, + "joint_group": joint_group, }, gripper_config_file, ], diff --git a/flexiv_gripper/src/gripper_action_server.cpp b/flexiv_gripper/src/gripper_action_server.cpp index 7533048..3a29870 100644 --- a/flexiv_gripper/src/gripper_action_server.cpp +++ b/flexiv_gripper/src/gripper_action_server.cpp @@ -4,6 +4,42 @@ #include "flexiv_gripper/gripper_action_server.hpp" +namespace { + +/** + * @brief Resolve the joint group whose gripper this node controls. + * @param[in] group_name Requested group name ("ARM_1", "ARM_2", ...). Empty means auto-detect. + * @param[in] single_arm_groups Single-arm groups reported by the connected robot. + * @return Resolved joint group. + * @throw std::invalid_argument if the name is unknown, or if auto-detect is ambiguous (more than + * one single-arm group) or finds no group. + */ +flexiv::rdk::JointGroup ResolveJointGroup(const std::string& group_name, + const std::map& single_arm_groups) +{ + if (group_name.empty()) { + if (single_arm_groups.size() == 1) { + return single_arm_groups.begin()->first; + } + throw std::invalid_argument( + "Parameter 'joint_group' must be specified (e.g. ARM_1 or ARM_2) because the connected " + "robot reports multiple single-arm groups"); + } + for (const auto& [group, name] : flexiv::rdk::kJointGroupNames) { + if (name == group_name) { + if (single_arm_groups.count(group) == 0) { + throw std::invalid_argument("Joint group '" + group_name + + "' is not a single-arm group of the connected robot; a gripper can only be " + "controlled on a single-arm group (e.g. ARM_1 or ARM_2)"); + } + return group; + } + } + throw std::invalid_argument("Unknown joint_group '" + group_name + "'"); +} + +} + namespace flexiv_gripper { GripperActionServer::GripperActionServer(const rclcpp::NodeOptions& options) @@ -17,6 +53,7 @@ GripperActionServer::GripperActionServer(const rclcpp::NodeOptions& options) this->declare_parameter("default_max_force", kDefaultMaxForce); this->declare_parameter("gripper_joint_names", std::vector()); this->declare_parameter("use_lite_rdk", false); + this->declare_parameter("joint_group", std::string()); std::string robot_sn; if (!this->get_parameter("robot_sn", robot_sn)) { @@ -50,8 +87,7 @@ GripperActionServer::GripperActionServer(const rclcpp::NodeOptions& options) try { RCLCPP_INFO(this->get_logger(), "Connecting to robot %s with a %s RDK instance ...", robot_sn.c_str(), use_lite_rdk ? "lite" : "normal"); - robot_ = std::make_unique( - robot_sn, std::vector {}, true, use_lite_rdk); + robot_ = std::make_unique(robot_sn, true, use_lite_rdk); RCLCPP_INFO(this->get_logger(), "Successfully connected to robot"); @@ -67,8 +103,8 @@ GripperActionServer::GripperActionServer(const rclcpp::NodeOptions& options) } if (!robot_->operational()) { - RCLCPP_INFO(this->get_logger(), "Enabling robot ..."); - robot_->Enable(); + RCLCPP_INFO(this->get_logger(), "Servoing on robot ..."); + robot_->ServoOn(); while (!robot_->operational()) { std::this_thread::sleep_for(std::chrono::seconds(1)); @@ -77,27 +113,32 @@ GripperActionServer::GripperActionServer(const rclcpp::NodeOptions& options) } } + const std::string joint_group_name = this->get_parameter("joint_group").as_string(); + this->joint_group_ = ResolveJointGroup(joint_group_name, robot_->info().single_arm_groups); + RCLCPP_INFO(this->get_logger(), "Controlling gripper for joint group [%s]", + flexiv::rdk::kJointGroupNames.at(joint_group_).c_str()); + RCLCPP_INFO(this->get_logger(), "Initializing Flexiv gripper control interface"); this->gripper_ = std::make_unique(*robot_); this->tool_ = std::make_unique(*robot_); - // Enable the specified gripper as a device + // Enable the specified gripper as a device for this joint group RCLCPP_INFO(this->get_logger(), "Enabling gripper %s ...", gripper_name.c_str()); - gripper_->Enable(gripper_name); + gripper_->Enable(joint_group_, gripper_name); // Switch robot tool to gripper so the gravity compensation and TCP location is updated RCLCPP_INFO(this->get_logger(), "Switching robot tool to %s ...", gripper_name.c_str()); - tool_->Switch(gripper_name); + tool_->Switch(joint_group_, gripper_name); // Manually initialize the gripper, not all grippers need this step RCLCPP_INFO( this->get_logger(), "Initializing gripper, this process takes about 10 seconds .."); - gripper_->Init(); + gripper_->Init(joint_group_); std::this_thread::sleep_for(std::chrono::seconds(10)); RCLCPP_INFO(this->get_logger(), "Gripper initialization completed"); // Get the current gripper states - this->current_gripper_states_ = gripper_->states(); + this->current_gripper_states_ = gripper_->states().at(joint_group_); } catch (const std::exception& e) { if (use_lite_rdk) { RCLCPP_FATAL(this->get_logger(), @@ -179,7 +220,7 @@ void GripperActionServer::ExecuteMove(const std::shared_ptr& goa { auto command = [this, goal_handle]() { const auto goal = goal_handle->get_goal(); - gripper_->Move(goal->width, goal->velocity, goal->max_force); + gripper_->Move(joint_group_, goal->width, goal->velocity, goal->max_force); }; ExecuteCommand(goal_handle, GripperAction::kMove, command); } @@ -188,7 +229,7 @@ void GripperActionServer::ExecuteGrasp(const std::shared_ptr& g { auto command = [this, goal_handle]() { const auto goal = goal_handle->get_goal(); - gripper_->Grasp(goal->force); + gripper_->Grasp(joint_group_, goal->force); }; ExecuteCommand(goal_handle, GripperAction::kGrasp, command); } @@ -202,9 +243,10 @@ void GripperActionServer::ExecuteGripperCommand( std::unique_lock guard(gripper_states_mutex_); auto result = std::make_shared(); const double current_width = current_gripper_states_.width; - if (target_width > gripper_->params().max_width || target_width < 0) { + const double max_width = gripper_->params().at(joint_group_).max_width; + if (target_width > max_width || target_width < 0) { RCLCPP_ERROR(this->get_logger(), "Invalid gripper target width: %f. Max width = %f", - target_width, gripper_->params().max_width); + target_width, max_width); goal_handle->abort(result); return; } @@ -220,7 +262,7 @@ void GripperActionServer::ExecuteGripperCommand( guard.unlock(); auto command = [this, target_width]() { - gripper_->Move(target_width, kDefaultVelocity, kDefaultMaxForce); + gripper_->Move(joint_group_, target_width, kDefaultVelocity, kDefaultMaxForce); }; ExecuteGripperCommandHelper(goal_handle, command); @@ -250,7 +292,7 @@ void GripperActionServer::ExecuteGripperCommandHelper( while (!IsResultReady(result_future, future_wait_timeout_) && rclcpp::ok()) { if (goal_handle->is_canceling()) { - gripper_->Stop(); + gripper_->Stop(joint_group_); auto result = result_future.get(); RCLCPP_INFO( this->get_logger(), "Gripper %s action has been canceled", action_name.c_str()); @@ -279,7 +321,7 @@ void GripperActionServer::ExecuteGripperCommandHelper( void GripperActionServer::StopServiceCallback(const std::shared_ptr& response) { RCLCPP_INFO(this->get_logger(), "Stopping the gripper..."); - auto result = CommandExecutionResult([this]() { gripper_->Stop(); })(); + auto result = CommandExecutionResult([this]() { gripper_->Stop(joint_group_); })(); response->success = result->success; response->message = result->error; if (response->success) { @@ -295,7 +337,7 @@ void GripperActionServer::StopServiceCallback(const std::shared_ptr lock(gripper_states_mutex_); - this->current_gripper_states_ = gripper_->states(); + this->current_gripper_states_ = gripper_->states().at(joint_group_); // Modify the gripper joint states based on the mounted gripper type // The gripper joint states below is for the Flexiv Grav GN-01 gripper sensor_msgs::msg::JointState gripper_joint_states; diff --git a/flexiv_hardware/CMakeLists.txt b/flexiv_hardware/CMakeLists.txt index 73499f2..a69362a 100644 --- a/flexiv_hardware/CMakeLists.txt +++ b/flexiv_hardware/CMakeLists.txt @@ -1,9 +1,9 @@ cmake_minimum_required(VERSION 3.5) project(flexiv_hardware) -# Default to C++17 +# flexiv_rdk (linked below) requires C++20 if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD 20) set(CMAKE_CXX_STANDARD_REQUIRED ON) endif() diff --git a/flexiv_hardware/include/flexiv_hardware/flexiv_hardware_interface.hpp b/flexiv_hardware/include/flexiv_hardware/flexiv_hardware_interface.hpp index 52a1f46..12fe1f7 100644 --- a/flexiv_hardware/include/flexiv_hardware/flexiv_hardware_interface.hpp +++ b/flexiv_hardware/include/flexiv_hardware/flexiv_hardware_interface.hpp @@ -96,7 +96,7 @@ class FlexivHardwareInterface : public hardware_interface::SystemInterface std::vector target_vel_buffer_; std::vector target_torque_buffer_; - std::map nrt_joint_position_cmds_; + std::map rt_joint_position_cmds_; std::map rt_joint_torque_cmds_; // Robot states exported per active joint group. diff --git a/flexiv_hardware/src/flexiv_hardware_interface.cpp b/flexiv_hardware/src/flexiv_hardware_interface.cpp index 353920c..791c615 100644 --- a/flexiv_hardware/src/flexiv_hardware_interface.cpp +++ b/flexiv_hardware/src/flexiv_hardware_interface.cpp @@ -29,8 +29,6 @@ namespace { -constexpr double kMaxJointVelocity = 2.0; -constexpr double kMaxJointAcceleration = 3.0; constexpr uint64_t kMaxExactRobotStatesHandle = 1ULL << 53; using GroupDofList = std::vector>; @@ -85,9 +83,13 @@ std::string describe_group_layout( /** * Resolve active joint groups from Robot::states() for this interface. * - * Supported layouts: - * 1) Single-arm: exactly one group ARMS with DoF equal to expected_dof. - * 2) Dual-arm: exactly two groups ARM_1 and ARM_2 whose DoFs sum to expected_dof. + * Supported arm layouts, each optionally combined with an EXT_AXIS group (e.g. MICO-Plus, + * MICO-Ultra torso), are: 1) Single-arm: one group ARMS. 2) Dual-arm: one group each for ARM_1 and + * ARM_2. + * + * When an EXT_AXIS group is present it is placed first in the returned list, matching the + * rdk_to_ros_map_ ordering convention (external axes precede arm joints). The DoFs of all + * returned groups must sum to expected_dof. * * @param states_by_group Joint-group keyed robot states from RDK. * @param expected_dof Total DoF expected by this hardware interface mapping. @@ -100,64 +102,63 @@ GroupDofList determine_active_groups( { GroupDofList active_groups; - // Single-arm robot: one and only one group ARMS. - auto arms_it = states_by_group.find(flexiv::rdk::JointGroup::ARMS); - if (states_by_group.size() == 1 && arms_it != states_by_group.end() - && arms_it->second.q.size() == expected_dof) { - active_groups.emplace_back(flexiv::rdk::JointGroup::ARMS, expected_dof); - return active_groups; - } - if (states_by_group.size() == 1 && arms_it != states_by_group.end()) { + // Optional external-axis group. Commanded first to match the + // rdk_to_ros_map_ ordering [ext_axis..., arm_joint...]. + auto ext_it = states_by_group.find(flexiv::rdk::JointGroup::EXT_AXIS); + const bool has_ext = ext_it != states_by_group.end(); + + // Arm layout. A real robot's states() exposes overlapping views: the commandable per-arm + // single-arm groups ARM_1 (and ARM_2 on a dual-arm robot) appear *alongside* the aggregate + // views ALL and ARMS. The joint command APIs (SendJointPosition/StreamJointPosition) only + // accept single-arm and external-axis groups and reject ALL/ARMS, so select ARM_1[/ARM_2] and + // never the aggregates. ARMS is used only as a last-resort fallback for a robot that exposes + // no per-arm group at all. + auto arm1_it = states_by_group.find(flexiv::rdk::JointGroup::ARM_1); + auto arm2_it = states_by_group.find(flexiv::rdk::JointGroup::ARM_2); + const bool has_arm1 = arm1_it != states_by_group.end(); + const bool has_arm2 = arm2_it != states_by_group.end(); + + // Assemble in RDK order: external axes first, then arm(s). + GroupDofList candidate_groups; + if (has_ext) { + candidate_groups.emplace_back(flexiv::rdk::JointGroup::EXT_AXIS, ext_it->second.q.size()); + } + if (has_arm1 && has_arm2) { + // Dual-arm: two commandable single-arm groups. + candidate_groups.emplace_back(flexiv::rdk::JointGroup::ARM_1, arm1_it->second.q.size()); + candidate_groups.emplace_back(flexiv::rdk::JointGroup::ARM_2, arm2_it->second.q.size()); + } else if (has_arm1) { + // Single-arm: ARM_1 is the commandable group (ARMS is only an aggregate view). + candidate_groups.emplace_back(flexiv::rdk::JointGroup::ARM_1, arm1_it->second.q.size()); + } else { RCLCPP_ERROR(logger, - "Robot reported ARMS group with %zu joints, but hardware interface expects %zu. " - "Layout: %s", - arms_it->second.q.size(), expected_dof, describe_group_layout(states_by_group).c_str()); + "Unsupported joint-group combination returned by robot states: %s. Expected a " + "commandable ARM_1[/ARM_2] group (optionally with EXT_AXIS); the aggregate ALL/ARMS " + "views are not commandable. Expected total DoF %zu.", + describe_group_layout(states_by_group).c_str(), expected_dof); return active_groups; } - // Dual-arm robot: one and only one group for each arm. - auto arm1_it = states_by_group.find(flexiv::rdk::JointGroup::ARM_1); - auto arm2_it = states_by_group.find(flexiv::rdk::JointGroup::ARM_2); - if (states_by_group.size() == 2 && arm1_it != states_by_group.end() - && arm2_it != states_by_group.end()) { - const size_t arm1_dof = arm1_it->second.q.size(); - const size_t arm2_dof = arm2_it->second.q.size(); - if (arm1_dof > 0 && arm2_dof > 0 && arm1_dof + arm2_dof == expected_dof) { - active_groups.emplace_back(flexiv::rdk::JointGroup::ARM_1, arm1_dof); - active_groups.emplace_back(flexiv::rdk::JointGroup::ARM_2, arm2_dof); + size_t total_dof = 0; + for (const auto& [group, group_dof] : candidate_groups) { + if (group_dof == 0) { + RCLCPP_ERROR(logger, "Joint group '%s' reported 0 joints. Layout: %s", + joint_group_name_string(group).c_str(), + describe_group_layout(states_by_group).c_str()); return active_groups; } + total_dof += group_dof; } - if (states_by_group.size() == 2 && arm1_it != states_by_group.end() - && arm2_it != states_by_group.end()) { + + if (total_dof != expected_dof) { RCLCPP_ERROR(logger, - "Robot reported ARM_1 with %zu joints and ARM_2 with %zu joints, but hardware " - "interface expects %zu total joints. Layout: %s", - arm1_it->second.q.size(), arm2_it->second.q.size(), expected_dof, - describe_group_layout(states_by_group).c_str()); + "Robot joint groups report %zu total joints, but hardware interface expects %zu. " + "Layout: %s", + total_dof, expected_dof, describe_group_layout(states_by_group).c_str()); return active_groups; } - for (const auto& [group, states] : states_by_group) { - if (group == flexiv::rdk::JointGroup::ARMS || group == flexiv::rdk::JointGroup::ARM_1 - || group == flexiv::rdk::JointGroup::ARM_2) { - continue; - } - - RCLCPP_WARN(logger, - "Robot reported unsupported joint group '%s' while resolving active groups " - "(q=%zu, dtheta=%zu, tau=%zu)", - joint_group_name_string(group).c_str(), states.q.size(), states.dtheta.size(), - states.tau.size()); - } - - RCLCPP_ERROR(logger, - "Unsupported joint-group combination returned by robot states: %s. Supported layouts " - "are [ARMS] or [ARM_1, ARM_2] with expected total DoF %zu.", - describe_group_layout(states_by_group).c_str(), expected_dof); - - // Any other group combination is unsupported in this interface. - active_groups.clear(); + active_groups = std::move(candidate_groups); return active_groups; } @@ -231,7 +232,7 @@ hardware_interface::CallbackReturn FlexivHardwareInterface::on_init( target_torque_buffer_.resize(info_.joints.size(), 0.0); hw_states_gpio_in_.resize(flexiv::rdk::kIOPorts, std::numeric_limits::quiet_NaN()); hw_commands_gpio_out_.resize(flexiv::rdk::kIOPorts, std::numeric_limits::quiet_NaN()); - nrt_joint_position_cmds_.clear(); + rt_joint_position_cmds_.clear(); rt_joint_torque_cmds_.clear(); stop_modes_ = {}; start_modes_ = {}; @@ -416,9 +417,9 @@ hardware_interface::CallbackReturn FlexivHardwareInterface::on_init( try { auto rdk_control_mode_str = info_.hardware_parameters.at("rdk_control_mode"); if (rdk_control_mode_str == "joint_position") { - rdk_control_mode_ = flexiv::rdk::Mode::NRT_JOINT_POSITION; + rdk_control_mode_ = flexiv::rdk::Mode::RT_JOINT_POSITION; } else if (rdk_control_mode_str == "joint_impedance") { - rdk_control_mode_ = flexiv::rdk::Mode::NRT_JOINT_IMPEDANCE; + rdk_control_mode_ = flexiv::rdk::Mode::RT_JOINT_IMPEDANCE; } else { RCLCPP_FATAL(getLogger(), "Parameter 'rdk_control_mode' has invalid value '%s'. Options: joint_position, " @@ -474,7 +475,13 @@ std::vector FlexivHardwareInterface::export_ std::vector groups; if (robot_) { - groups = robot_->groups(); + for (const auto& [group, name] : robot_->info().all_groups) { + if (group == flexiv::rdk::JointGroup::ALL + || group == flexiv::rdk::JointGroup::UNKNOWN) { + continue; + } + groups.push_back(group); + } } if (groups.empty()) { groups.push_back(flexiv::rdk::JointGroup::ARMS); @@ -553,27 +560,33 @@ hardware_interface::CallbackReturn FlexivHardwareInterface::on_activate( RCLCPP_INFO(getLogger(), "Fault on robot server is cleared"); } - // Check the DoF of the robot - if (robot_->info().DoF != info_.joints.size()) { - RCLCPP_FATAL(getLogger(), "Robot has %ld DoF. Expected %ld (from URDF).", - robot_->info().DoF, info_.joints.size()); + const auto robot_info = robot_->info(); + size_t robot_total_dof = 0; + for (const auto& [group, name] : robot_info.single_arm_groups) { + const auto dof_it = robot_info.DoF.find(group); + if (dof_it != robot_info.DoF.end()) { + robot_total_dof += dof_it->second; + } + } + const auto ext_dof_it = robot_info.DoF.find(flexiv::rdk::JointGroup::EXT_AXIS); + if (ext_dof_it != robot_info.DoF.end()) { + robot_total_dof += ext_dof_it->second; + } + if (robot_total_dof != info_.joints.size()) { + RCLCPP_FATAL(getLogger(), "Robot has %zu commandable DoF. Expected %zu (from URDF).", + robot_total_dof, info_.joints.size()); return hardware_interface::CallbackReturn::ERROR; } - // Enable the robot - RCLCPP_INFO(getLogger(), "Enabling robot ..."); - robot_->Enable(); + // Servo on the robot (release brakes and become operational) + RCLCPP_INFO(getLogger(), "Servoing on robot ..."); + robot_->ServoOn(); // Wait for the robot to become operational while (!robot_->operational()) { std::this_thread::sleep_for(std::chrono::seconds(1)); } RCLCPP_INFO(getLogger(), "Robot is now operational"); - - // Unlock external axes if any - if (robot_->info().DoF_e > 0) { - robot_->LockExternalAxes(false); - } } catch (const std::exception& e) { RCLCPP_FATAL(getLogger(), "Could not enable robot."); RCLCPP_FATAL(getLogger(), e.what()); @@ -621,7 +634,6 @@ hardware_interface::return_type FlexivHardwareInterface::read( for (const auto& [group, group_dof] : active_groups) { const auto& group_states = states_by_group.at(group); - hw_flexiv_robot_states_by_group_[group] = group_states; if (group_states.q.size() < group_dof || group_states.dtheta.size() < group_dof || group_states.tau.size() < group_dof) { RCLCPP_ERROR(getLogger(), @@ -644,6 +656,17 @@ hardware_interface::return_type FlexivHardwareInterface::read( return hardware_interface::return_type::ERROR; } + // Refresh every exported per-group robot-states handle the robot reports, so the + // robot-states broadcaster(s) publish fresh data regardless of which group name they read: + // a single-arm broadcaster reads the ARMS-named (robot_sn) handle while commands use ARM_1, + // and dual-arm broadcasters read the ARM_1/ARM_2 (left_/right_) handles. + for (auto& [group, group_states] : hw_flexiv_robot_states_by_group_) { + const auto it = states_by_group.find(group); + if (it != states_by_group.end()) { + group_states = it->second; + } + } + // Read joint states // Map RDK states (RDK order) to Hardware Interface states (ROS order) for (size_t rdk_idx = 0; rdk_idx < dof; ++rdk_idx) { @@ -716,43 +739,42 @@ hardware_interface::return_type FlexivHardwareInterface::write( } } - bool rebuild_nrt_joint_position_cmds - = nrt_joint_position_cmds_.size() != active_groups.size(); - if (!rebuild_nrt_joint_position_cmds) { - auto cmd_it = nrt_joint_position_cmds_.begin(); + // Stream real-time joint position commands. + bool rebuild_rt_joint_position_cmds + = rt_joint_position_cmds_.size() != active_groups.size(); + if (!rebuild_rt_joint_position_cmds) { + auto cmd_it = rt_joint_position_cmds_.begin(); for (const auto& [group, group_dof] : active_groups) { - if (cmd_it == nrt_joint_position_cmds_.end() || cmd_it->first != group + if (cmd_it == rt_joint_position_cmds_.end() || cmd_it->first != group || cmd_it->second.q_d.size() != group_dof || cmd_it->second.dq_d.size() != group_dof - || cmd_it->second.dq_max.size() != group_dof - || cmd_it->second.ddq_max.size() != group_dof) { - rebuild_nrt_joint_position_cmds = true; + || cmd_it->second.ddq_d.size() != group_dof) { + rebuild_rt_joint_position_cmds = true; break; } ++cmd_it; } } - if (rebuild_nrt_joint_position_cmds) { - nrt_joint_position_cmds_.clear(); + if (rebuild_rt_joint_position_cmds) { + rt_joint_position_cmds_.clear(); for (const auto& [group, group_dof] : active_groups) { - auto& cmd = nrt_joint_position_cmds_[group]; + auto& cmd = rt_joint_position_cmds_[group]; cmd.q_d.resize(group_dof); cmd.dq_d.resize(group_dof); - cmd.dq_max.assign(group_dof, kMaxJointVelocity); - cmd.ddq_max.assign(group_dof, kMaxJointAcceleration); + cmd.ddq_d.assign(group_dof, 0.0); } } if (active_groups.size() == 1 && active_groups.front().first == flexiv::rdk::JointGroup::ARMS) { - auto& cmd = nrt_joint_position_cmds_.at(flexiv::rdk::JointGroup::ARMS); + auto& cmd = rt_joint_position_cmds_.at(flexiv::rdk::JointGroup::ARMS); std::copy(target_pos.begin(), target_pos.end(), cmd.q_d.begin()); std::copy(target_vel.begin(), target_vel.end(), cmd.dq_d.begin()); } else { size_t offset = 0; for (const auto& [group, group_dof] : active_groups) { - auto& cmd = nrt_joint_position_cmds_.at(group); + auto& cmd = rt_joint_position_cmds_.at(group); const auto begin = static_cast::difference_type>(offset); std::copy_n(target_pos.begin() + begin, group_dof, cmd.q_d.begin()); std::copy_n(target_vel.begin() + begin, group_dof, cmd.dq_d.begin()); @@ -760,7 +782,7 @@ hardware_interface::return_type FlexivHardwareInterface::write( } } - robot_->SendJointPosition(nrt_joint_position_cmds_); + robot_->StreamJointPosition(rt_joint_position_cmds_); } else if (torque_controller_running_ && robot_->mode() == flexiv::rdk::Mode::RT_JOINT_TORQUE && !is_eff_nan) { auto states_by_group = robot_->states(); diff --git a/flexiv_moveit_config/config/joint_limits.yaml b/flexiv_moveit_config/config/joint_limits.yaml index 08857c8..0a4d853 100644 --- a/flexiv_moveit_config/config/joint_limits.yaml +++ b/flexiv_moveit_config/config/joint_limits.yaml @@ -10,37 +10,37 @@ default_acceleration_scaling_factor: 0.1 joint_limits: $(var robot_sn)_joint1: has_velocity_limits: true - max_velocity: 2.0944 # EnlightL + max_velocity: 2.0944 # Enlight-L has_acceleration_limits: true max_acceleration: 2.0 $(var robot_sn)_joint2: has_velocity_limits: true - max_velocity: 2.0944 # EnlightL + max_velocity: 2.0944 # Enlight-L has_acceleration_limits: true max_acceleration: 2.0 $(var robot_sn)_joint3: has_velocity_limits: true - max_velocity: 2.4435 # EnlightL + max_velocity: 2.4435 # Enlight-L has_acceleration_limits: true max_acceleration: 2.0 $(var robot_sn)_joint4: has_velocity_limits: true - max_velocity: 2.4435 # EnlightL + max_velocity: 2.4435 # Enlight-L has_acceleration_limits: true max_acceleration: 2.0 $(var robot_sn)_joint5: has_velocity_limits: true - max_velocity: 4.8869 # EnlightL + max_velocity: 4.8869 # Enlight-L has_acceleration_limits: true max_acceleration: 2.0 $(var robot_sn)_joint6: has_velocity_limits: true - max_velocity: 4.8869 # EnlightL + max_velocity: 4.8869 # Enlight-L has_acceleration_limits: true max_acceleration: 2.0 $(var robot_sn)_joint7: has_velocity_limits: true - max_velocity: 4.8869 # EnlightL + max_velocity: 4.8869 # Enlight-L has_acceleration_limits: true max_acceleration: 2.0 $(var robot_sn)_finger_width_joint: @@ -48,3 +48,88 @@ joint_limits: max_velocity: 0.5 has_acceleration_limits: true max_acceleration: 1.0 + + # Dual-arm joint limits (Enlight-LL, MICO-Core, MICO-Plus, MICO-Ultra) + left_$(var robot_sn)_joint1: + has_velocity_limits: true + max_velocity: 2.0944 # Enlight-L + has_acceleration_limits: true + max_acceleration: 2.0 + left_$(var robot_sn)_joint2: + has_velocity_limits: true + max_velocity: 2.0944 # Enlight-L + has_acceleration_limits: true + max_acceleration: 2.0 + left_$(var robot_sn)_joint3: + has_velocity_limits: true + max_velocity: 2.4435 # Enlight-L + has_acceleration_limits: true + max_acceleration: 2.0 + left_$(var robot_sn)_joint4: + has_velocity_limits: true + max_velocity: 2.4435 # Enlight-L + has_acceleration_limits: true + max_acceleration: 2.0 + left_$(var robot_sn)_joint5: + has_velocity_limits: true + max_velocity: 4.8869 # Enlight-L + has_acceleration_limits: true + max_acceleration: 2.0 + left_$(var robot_sn)_joint6: + has_velocity_limits: true + max_velocity: 4.8869 # Enlight-L + has_acceleration_limits: true + max_acceleration: 2.0 + left_$(var robot_sn)_joint7: + has_velocity_limits: true + max_velocity: 4.8869 # Enlight-L + has_acceleration_limits: true + max_acceleration: 2.0 + right_$(var robot_sn)_joint1: + has_velocity_limits: true + max_velocity: 2.0944 # Enlight-L + has_acceleration_limits: true + max_acceleration: 2.0 + right_$(var robot_sn)_joint2: + has_velocity_limits: true + max_velocity: 2.0944 # Enlight-L + has_acceleration_limits: true + max_acceleration: 2.0 + right_$(var robot_sn)_joint3: + has_velocity_limits: true + max_velocity: 2.4435 # Enlight-L + has_acceleration_limits: true + max_acceleration: 2.0 + right_$(var robot_sn)_joint4: + has_velocity_limits: true + max_velocity: 2.4435 # Enlight-L + has_acceleration_limits: true + max_acceleration: 2.0 + right_$(var robot_sn)_joint5: + has_velocity_limits: true + max_velocity: 4.8869 # Enlight-L + has_acceleration_limits: true + max_acceleration: 2.0 + right_$(var robot_sn)_joint6: + has_velocity_limits: true + max_velocity: 4.8869 # Enlight-L + has_acceleration_limits: true + max_acceleration: 2.0 + right_$(var robot_sn)_joint7: + has_velocity_limits: true + max_velocity: 4.8869 # Enlight-L + has_acceleration_limits: true + max_acceleration: 2.0 + + # 2-DoF torso joint limits (MICO-Plus, MICO-Ultra). Required by MoveIt's + # time-optimal trajectory generation, which has no acceleration limit otherwise. + $(var robot_sn)_torso_joint1: + has_velocity_limits: true + max_velocity: 1.0 # MICO torso + has_acceleration_limits: true + max_acceleration: 1.0 + $(var robot_sn)_torso_joint2: + has_velocity_limits: true + max_velocity: 1.0 # MICO torso + has_acceleration_limits: true + max_acceleration: 1.0 diff --git a/flexiv_moveit_config/config/kinematics.yaml b/flexiv_moveit_config/config/kinematics.yaml index e527d51..a53b756 100644 --- a/flexiv_moveit_config/config/kinematics.yaml +++ b/flexiv_moveit_config/config/kinematics.yaml @@ -2,3 +2,14 @@ $(var robot_sn)_arm: kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin kinematics_solver_search_resolution: 0.005 kinematics_solver_timeout: 0.05 + +# Dual-arm groups (Enlight-LL, MICO-Core, MICO-Plus, MICO-Ultra) +left_$(var robot_sn)_arm: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 0.05 + +right_$(var robot_sn)_arm: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 0.05 diff --git a/flexiv_moveit_config/config/moveit_controllers.yaml b/flexiv_moveit_config/config/moveit_controllers.yaml index 8ead2c4..a6c3883 100644 --- a/flexiv_moveit_config/config/moveit_controllers.yaml +++ b/flexiv_moveit_config/config/moveit_controllers.yaml @@ -1,3 +1,5 @@ +# Single-arm (Enlight-L). Dual / MICO robots use moveit_controllers_dual.yaml / +# moveit_controllers_mico.yaml, selected by flexiv_moveit.launch.py based on robot_type. controller_names: - flexiv_arm_controller - flexiv_gripper_node diff --git a/flexiv_moveit_config/config/moveit_controllers_dual.yaml b/flexiv_moveit_config/config/moveit_controllers_dual.yaml new file mode 100644 index 0000000..e2d3560 --- /dev/null +++ b/flexiv_moveit_config/config/moveit_controllers_dual.yaml @@ -0,0 +1,40 @@ +# Dual-arm (Enlight-LL, MICO-Core). One 14-joint flexiv_arm_controller covers the left_arm, +# right_arm and both_arms planning groups (allow_partial_joints_goal is enabled controller-side). +controller_names: + - flexiv_arm_controller + - left_flexiv_gripper_node + - right_flexiv_gripper_node + +flexiv_arm_controller: + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + default: true + joints: + - left_$(var robot_sn)_joint1 + - left_$(var robot_sn)_joint2 + - left_$(var robot_sn)_joint3 + - left_$(var robot_sn)_joint4 + - left_$(var robot_sn)_joint5 + - left_$(var robot_sn)_joint6 + - left_$(var robot_sn)_joint7 + - right_$(var robot_sn)_joint1 + - right_$(var robot_sn)_joint2 + - right_$(var robot_sn)_joint3 + - right_$(var robot_sn)_joint4 + - right_$(var robot_sn)_joint5 + - right_$(var robot_sn)_joint6 + - right_$(var robot_sn)_joint7 + +left_flexiv_gripper_node: + action_ns: gripper_action + type: GripperCommand + default: true + joints: + - left_$(var robot_sn)_finger_width_joint + +right_flexiv_gripper_node: + action_ns: gripper_action + type: GripperCommand + default: true + joints: + - right_$(var robot_sn)_finger_width_joint diff --git a/flexiv_moveit_config/config/moveit_controllers_mico.yaml b/flexiv_moveit_config/config/moveit_controllers_mico.yaml new file mode 100644 index 0000000..3b44cd1 --- /dev/null +++ b/flexiv_moveit_config/config/moveit_controllers_mico.yaml @@ -0,0 +1,43 @@ +# MICO-Plus / MICO-Ultra. One 16-joint flexiv_arm_controller covers the two arms plus the 2-DoF +# torso (pan-tilt / waist). The arm planning groups (left_arm, right_arm, both_arms) command a +# joint subset via allow_partial_joints_goal (enabled controller-side). +controller_names: + - flexiv_arm_controller + - left_flexiv_gripper_node + - right_flexiv_gripper_node + +flexiv_arm_controller: + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + default: true + joints: + - left_$(var robot_sn)_joint1 + - left_$(var robot_sn)_joint2 + - left_$(var robot_sn)_joint3 + - left_$(var robot_sn)_joint4 + - left_$(var robot_sn)_joint5 + - left_$(var robot_sn)_joint6 + - left_$(var robot_sn)_joint7 + - right_$(var robot_sn)_joint1 + - right_$(var robot_sn)_joint2 + - right_$(var robot_sn)_joint3 + - right_$(var robot_sn)_joint4 + - right_$(var robot_sn)_joint5 + - right_$(var robot_sn)_joint6 + - right_$(var robot_sn)_joint7 + - $(var robot_sn)_torso_joint1 + - $(var robot_sn)_torso_joint2 + +left_flexiv_gripper_node: + action_ns: gripper_action + type: GripperCommand + default: true + joints: + - left_$(var robot_sn)_finger_width_joint + +right_flexiv_gripper_node: + action_ns: gripper_action + type: GripperCommand + default: true + joints: + - right_$(var robot_sn)_finger_width_joint diff --git a/flexiv_moveit_config/config/ompl_planning.yaml b/flexiv_moveit_config/config/ompl_planning.yaml index 40672c6..e08e11f 100644 --- a/flexiv_moveit_config/config/ompl_planning.yaml +++ b/flexiv_moveit_config/config/ompl_planning.yaml @@ -174,3 +174,94 @@ $(var robot_sn)_grav_gripper: - SPARSkConfigDefault - SPARStwokConfigDefault - TrajOptDefault + +# Dual-arm groups (Enlight-LL, MICO-Core, MICO-Plus, MICO-Ultra) +left_$(var robot_sn)_arm: + planner_configs: + - SBLkConfigDefault + - ESTkConfigDefault + - LBKPIECEkConfigDefault + - BKPIECEkConfigDefault + - KPIECEkConfigDefault + - RRTkConfigDefault + - RRTConnectkConfigDefault + - RRTstarkConfigDefault + - TRRTkConfigDefault + - PRMkConfigDefault + - PRMstarkConfigDefault + - FMTkConfigDefault + - BFMTkConfigDefault + - PDSTkConfigDefault + - STRIDEkConfigDefault + - BiTRRTkConfigDefault + - LBTRRTkConfigDefault + - BiESTkConfigDefault + - ProjESTkConfigDefault + - LazyPRMkConfigDefault + - LazyPRMstarkConfigDefault + - SPARSkConfigDefault + - SPARStwokConfigDefault + - TrajOptDefault + +right_$(var robot_sn)_arm: + planner_configs: + - SBLkConfigDefault + - ESTkConfigDefault + - LBKPIECEkConfigDefault + - BKPIECEkConfigDefault + - KPIECEkConfigDefault + - RRTkConfigDefault + - RRTConnectkConfigDefault + - RRTstarkConfigDefault + - TRRTkConfigDefault + - PRMkConfigDefault + - PRMstarkConfigDefault + - FMTkConfigDefault + - BFMTkConfigDefault + - PDSTkConfigDefault + - STRIDEkConfigDefault + - BiTRRTkConfigDefault + - LBTRRTkConfigDefault + - BiESTkConfigDefault + - ProjESTkConfigDefault + - LazyPRMkConfigDefault + - LazyPRMstarkConfigDefault + - SPARSkConfigDefault + - SPARStwokConfigDefault + - TrajOptDefault + +# 2-DoF pan-tilt / waist torso group (MICO-Plus, MICO-Ultra). Joint-space planning only; +# harmless for other robot types whose SRDF has no such group. +$(var robot_sn)_torso: + planner_configs: + - RRTConnectkConfigDefault + - RRTstarkConfigDefault + - PRMkConfigDefault + - PRMstarkConfigDefault + +both_arms: + planner_configs: + - SBLkConfigDefault + - ESTkConfigDefault + - LBKPIECEkConfigDefault + - BKPIECEkConfigDefault + - KPIECEkConfigDefault + - RRTkConfigDefault + - RRTConnectkConfigDefault + - RRTstarkConfigDefault + - TRRTkConfigDefault + - PRMkConfigDefault + - PRMstarkConfigDefault + - FMTkConfigDefault + - BFMTkConfigDefault + - PDSTkConfigDefault + - STRIDEkConfigDefault + - BiTRRTkConfigDefault + - LBTRRTkConfigDefault + - BiESTkConfigDefault + - ProjESTkConfigDefault + - LazyPRMkConfigDefault + - LazyPRMstarkConfigDefault + - SPARSkConfigDefault + - SPARStwokConfigDefault + - TrajOptDefault diff --git a/flexiv_moveit_config/srdf/flexiv.srdf.xacro b/flexiv_moveit_config/srdf/flexiv.srdf.xacro index 36f4df9..e1b19eb 100644 --- a/flexiv_moveit_config/srdf/flexiv.srdf.xacro +++ b/flexiv_moveit_config/srdf/flexiv.srdf.xacro @@ -1,21 +1,110 @@ - + - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/flexiv_msgs/CMakeLists.txt b/flexiv_msgs/CMakeLists.txt index 7332d1c..8e0206b 100644 --- a/flexiv_msgs/CMakeLists.txt +++ b/flexiv_msgs/CMakeLists.txt @@ -7,21 +7,6 @@ find_package(std_msgs REQUIRED) find_package(geometry_msgs REQUIRED) find_package(builtin_interfaces REQUIRED) -# This package is built against the Flexiv RDK install prefix, which also provides -# a newer Fast-CDR than ROS 2 Humble's FastRTPS generators expect. The driver is -# CycloneDDS-only, so generate introspection typesupport and skip FastRTPS code. -set(STATIC_ROSIDL_TYPESUPPORT_C rosidl_typesupport_introspection_c) -set(STATIC_ROSIDL_TYPESUPPORT_CPP rosidl_typesupport_introspection_cpp) -set(_flexiv_msgs_rosidl_extensions "") -foreach(_extension ${AMENT_EXTENSIONS_rosidl_generate_idl_interfaces}) - if(NOT _extension MATCHES "^rosidl_typesupport_fastrtps_(c|cpp):") - list(APPEND _flexiv_msgs_rosidl_extensions "${_extension}") - endif() -endforeach() -set(AMENT_EXTENSIONS_rosidl_generate_idl_interfaces ${_flexiv_msgs_rosidl_extensions}) -unset(_flexiv_msgs_rosidl_extensions) -unset(_extension) - set(msg_files msg/Digital.msg msg/GPIOStates.msg diff --git a/flexiv_msgs/msg/RobotStates.msg b/flexiv_msgs/msg/RobotStates.msg index f78bb1b..daa0574 100644 --- a/flexiv_msgs/msg/RobotStates.msg +++ b/flexiv_msgs/msg/RobotStates.msg @@ -19,9 +19,6 @@ float64[] dtheta # Measured joint torques float64[] tau -# Desired joint torques -float64[] tau_des - # Numerical derivative of measured joint torques float64[] tau_dot diff --git a/flexiv_test_nodes/README.md b/flexiv_test_nodes/README.md index 68ec80f..397ca60 100644 --- a/flexiv_test_nodes/README.md +++ b/flexiv_test_nodes/README.md @@ -38,8 +38,8 @@ ros2 launch flexiv_test_nodes robot_states_publisher.launch.py robot_sn:=[robot_ **Parameters:** - `robot_sn`: Robot serial number (required) -- `network_interface`: Network interface name (optional, auto-detect if empty) - `publish_rate`: Publish rate in Hz (default: 100) +- `joint_group`: Which RDK joint group's states to publish (default: `ALL`). RDK returns states per joint group; `ALL` is the whole robot (7 joints single-arm, 14 dual-arm). For an individual arm of a dual-arm robot use `ARM_1` (left) or `ARM_2` (right). ### 2. Robot States Monitor diff --git a/flexiv_test_nodes/flexiv_test_nodes/robot_states_monitor.py b/flexiv_test_nodes/flexiv_test_nodes/robot_states_monitor.py index 27c1c39..c87e9c7 100755 --- a/flexiv_test_nodes/flexiv_test_nodes/robot_states_monitor.py +++ b/flexiv_test_nodes/flexiv_test_nodes/robot_states_monitor.py @@ -20,7 +20,7 @@ def __init__(self): super().__init__("state_monitor") # Declare a parameter for the robot serial number - self.declare_parameter("robot_sn", "EnlightL-000000") + self.declare_parameter("robot_sn", "Enlight-L-000001") robot_sn = self.get_parameter("robot_sn").get_parameter_value().string_value # Construct topic name dynamically based on robot SN diff --git a/flexiv_test_nodes/flexiv_test_nodes/robot_states_publisher.py b/flexiv_test_nodes/flexiv_test_nodes/robot_states_publisher.py index 02d22ef..e74d75c 100755 --- a/flexiv_test_nodes/flexiv_test_nodes/robot_states_publisher.py +++ b/flexiv_test_nodes/flexiv_test_nodes/robot_states_publisher.py @@ -21,38 +21,40 @@ class RobotStatesPublisher(Node): """ROS2 node that publishes Flexiv robot states from RDK to ROS2 topics.""" - def __init__(self, robot_sn: str, network_interface: str = ""): + def __init__(self, robot_sn: str): super().__init__("rdk_state_publisher") # Declare parameters self.declare_parameter("robot_sn", robot_sn) - self.declare_parameter("network_interface", network_interface) self.declare_parameter("publish_rate", 100) # Default 100 Hz + # RDK v2.1 robot.states() returns a dict keyed by JointGroup. Select which group's states + # to publish on the single topic. "ALL" = the whole robot (7 joints single-arm, 14 dual); + # use "ARM_1"/"ARM_2" for an individual arm of a dual-arm robot. + self.declare_parameter("joint_group", "ALL") # Get parameters self.robot_sn = self.get_parameter("robot_sn").value - self.network_interface = self.get_parameter("network_interface").value self.publish_rate = self.get_parameter("publish_rate").value + self.joint_group_name = self.get_parameter("joint_group").value + try: + self.joint_group = getattr(flexivrdk.JointGroup, self.joint_group_name) + except AttributeError: + self.get_logger().error( + f"Invalid joint_group '{self.joint_group_name}', falling back to 'ALL'" + ) + self.joint_group_name = "ALL" + self.joint_group = flexivrdk.JointGroup.ALL self.get_logger().info( f"Initializing Robot States Publisher for robot {self.robot_sn}" ) - if self.network_interface: - self.get_logger().info( - f"Network interface whitelist: [{self.network_interface}]" - ) - else: - self.get_logger().info("Using all available network interfaces") self.get_logger().info(f"Publish rate: {self.publish_rate} Hz") + self.get_logger().info(f"Publishing joint group: {self.joint_group_name}") - # Initialize RDK Robot instance + # Initialize RDK Robot instance. RDK v2.1's Robot constructor is + # Robot(robot_sn, verbose=True, lite=False) — network-interface whitelisting was removed. try: - if self.network_interface: - # Use specific network interface - self.robot = flexivrdk.Robot(self.robot_sn, [self.network_interface]) - else: - # Use all available network interfaces - self.robot = flexivrdk.Robot(self.robot_sn) + self.robot = flexivrdk.Robot(self.robot_sn) self.get_logger().info("Successfully connected to robot via RDK") except Exception as e: self.get_logger().error(f"Failed to connect to robot: {str(e)}") @@ -165,8 +167,17 @@ def publish_robot_states(self): """Main callback function to read robot states and publish as ROS2 message.""" try: - # Get robot states from RDK - robot_states = self.robot.states() + # Get robot states from RDK. v2.1 returns a dict keyed by JointGroup; select the + # configured group (default ALL = whole robot). + all_states = self.robot.states() + if self.joint_group not in all_states: + self.get_logger().error( + f"Joint group '{self.joint_group_name}' not reported by the robot. " + f"Available: {[str(g) for g in all_states.keys()]}", + throttle_duration_sec=1.0, + ) + return + robot_states = all_states[self.joint_group] # Create ROS2 RobotStates message msg = RobotStates() @@ -179,13 +190,12 @@ def publish_robot_states(self): msg.robot_timestamp.sec = robot_states.timestamp[0] msg.robot_timestamp.nanosec = robot_states.timestamp[1] - # Joint-space states (all arrays are size 7 for Flexiv robots) + # Joint-space states (array size = DoF of the selected joint group) msg.q = list(robot_states.q) # Joint positions (link-side) msg.theta = list(robot_states.theta) # Joint positions (motor-side) msg.dq = list(robot_states.dq) # Joint velocities (link-side) msg.dtheta = list(robot_states.dtheta) # Joint velocities (motor-side) msg.tau = list(robot_states.tau) # Joint torques - msg.tau_des = list(robot_states.tau_des) # Desired joint torques msg.tau_dot = list(robot_states.tau_dot) # Joint torque derivatives msg.tau_ext = list(robot_states.tau_ext) # External joint torques msg.tau_interact = list( @@ -259,13 +269,7 @@ def main(args=None): "--robot-sn", type=str, required=True, - help="Robot serial number (e.g., EnlightL-123456)", - ) - parser.add_argument( - "--network-interface", - type=str, - default="", - help="Network interface name (e.g., eth0, enp0s31f6). Leave empty to use all interfaces.", + help="Robot serial number (e.g., Enlight-L-123456)", ) # Parse known args (ROS2 args are handled separately) @@ -276,10 +280,7 @@ def main(args=None): try: # Create and spin the node - node = RobotStatesPublisher( - robot_sn=parsed_args.robot_sn, - network_interface=parsed_args.network_interface, - ) + node = RobotStatesPublisher(robot_sn=parsed_args.robot_sn) # Spin the node rclpy.spin(node) diff --git a/flexiv_test_nodes/launch/robot_states_publisher.launch.py b/flexiv_test_nodes/launch/robot_states_publisher.launch.py index 01568ef..ad49ecf 100644 --- a/flexiv_test_nodes/launch/robot_states_publisher.launch.py +++ b/flexiv_test_nodes/launch/robot_states_publisher.launch.py @@ -1,20 +1,19 @@ """Launch file for Robot States Publisher Node. Example usage: - # Auto-detect network interface: ros2 launch flexiv_test_nodes robot_states_publisher.launch.py \ - robot_sn:=EnlightL-123456 - - # With specific network interface: - ros2 launch flexiv_test_nodes robot_states_publisher.launch.py \ - robot_sn:=EnlightL-123456 \ - network_interface:=eth0 + robot_sn:=Enlight-L-123456 # Custom publish rate: ros2 launch flexiv_test_nodes robot_states_publisher.launch.py \ - robot_sn:=EnlightL-123456 \ + robot_sn:=Enlight-L-123456 \ publish_rate:=200 + # Publish a single arm of a dual-arm robot: + ros2 launch flexiv_test_nodes robot_states_publisher.launch.py \ + robot_sn:=Enlight-LL-123456 \ + joint_group:=ARM_1 + Author: Flexiv Robotics License: Apache-2.0 """ @@ -30,13 +29,7 @@ def generate_launch_description(): # Declare launch arguments robot_sn_arg = DeclareLaunchArgument( - "robot_sn", description="Robot serial number (e.g., EnlightL-123456)" - ) - - network_interface_arg = DeclareLaunchArgument( - "network_interface", - default_value="", - description="Network interface name (e.g., eth0, enp0s31f6). Leave empty to auto-detect.", + "robot_sn", description="Robot serial number (e.g., Enlight-L-123456)" ) publish_rate_arg = DeclareLaunchArgument( @@ -45,6 +38,13 @@ def generate_launch_description(): description="Publishing rate in Hz (default: 100)", ) + joint_group_arg = DeclareLaunchArgument( + "joint_group", + default_value="ALL", + description="RDK joint group whose states to publish: ALL (whole robot), ARM_1 (left), " + "or ARM_2 (right).", + ) + # Create the Robot States Publisher node robot_states_publisher_node = Node( package="flexiv_test_nodes", @@ -54,23 +54,21 @@ def generate_launch_description(): parameters=[ { "robot_sn": LaunchConfiguration("robot_sn"), - "network_interface": LaunchConfiguration("network_interface"), "publish_rate": LaunchConfiguration("publish_rate"), + "joint_group": LaunchConfiguration("joint_group"), } ], arguments=[ "--robot-sn", LaunchConfiguration("robot_sn"), - "--network-interface", - LaunchConfiguration("network_interface"), ], ) return LaunchDescription( [ robot_sn_arg, - network_interface_arg, publish_rate_arg, + joint_group_arg, robot_states_publisher_node, ] )