Release/Flexiv ROS 2 Humble v1.5 (#40)

.github/workflows/humble-binary-build.yml

@@ -44,6 +44,7 @@ jobs:
           package-name: |
             flexiv_bringup
             flexiv_description
+            flexiv_gripper
             flexiv_hardware
             flexiv_moveit_config
             flexiv_msgs

README.md

@@ -1,12 +1,12 @@
 # Flexiv ROS 2
 
-[![License](https://img.shields.io/badge/License-Apache%202.0-blue.svg)](https://opensource.org/licenses/Apache-2.0) [![docs](https://img.shields.io/badge/docs-sphinx-yellow)](https://rdk.flexiv.com/manual/ros2_packages.html)
+[![License](https://img.shields.io/badge/License-Apache%202.0-blue.svg)](https://opensource.org/licenses/Apache-2.0) [![docs](https://img.shields.io/badge/docs-sphinx-yellow)](https://www.flexiv.com/software/rdk/manual/ros2_bridge.html)
 
 For ROS 2 users to easily work with [RDK](https://github.com/flexivrobotics/flexiv_rdk), the APIs of RDK are wrapped into ROS packages in `flexiv_ros2`. Key functionalities like real-time joint torque and position control are supported, and the integration with `ros2_control` framework and MoveIt! 2 is also implemented.
 
 ## References
 
-[Flexiv RDK main webpage](https://rdk.flexiv.com/) contains important information like RDK user manual and network setup.
+[Flexiv RDK main webpage](https://www.flexiv.com/software/rdk) contains important information like RDK user manual and network setup.
 
 ## Compatibility
 
@@ -105,14 +105,15 @@ This project was developed for ROS 2 Foxy (Ubuntu 20.04) and Humble (Ubuntu 22.0
 ## Usage
 
 > [!NOTE]
-> The instruction below is only a quick reference, see the [Flexiv ROS 2 Documentation](https://rdk.flexiv.com/manual/ros2_bridge.html) for more information.
+> The instruction below is only a quick reference, see the [Flexiv ROS 2 Documentation](https://www.flexiv.com/software/rdk/manual/ros2_bridge.html) for more information.
 
-The prerequisites of using ROS 2 with Flexiv Rizon robot are [enable RDK on the robot server](https://rdk.flexiv.com/manual/activate_rdk_server.html) and [establish connection](https://rdk.flexiv.com/manual/establish_connection.html) between the workstation PC and the robot.
+The prerequisites of using ROS 2 with Flexiv Rizon robot 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.
 
 The main launch file to start the robot driver is the `rizon.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: Rizon4s-123456
 - `rizon_type` (default: *rizon4*) - type of the Flexiv Rizon robot. (rizon4, rizon4s, rizon10 or rizon10s)
+- `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.
 - `start_rviz` (deafult: *true*) - starts RViz automatically with the launch file.
 - `fake_sensor_commands` (default: *false*) - enables fake command interfaces for sensors used for simulations. Used only if `use_fake_hardware` parameter is true.
@@ -186,7 +187,7 @@ ros2 launch flexiv_bringup rizon_moveit.launch.py robot_sn:=dont-care use_fake_h
 
 The robot driver (`rizon.launch.py`) publishes the following feedback states to the respective ROS topics:
 
-- `/${robot_sn}/flexiv_robot_states`: [Flexiv robot states](https://rdk.flexiv.com/api/structflexiv_1_1rdk_1_1_robot_states.html#details) including the joint- and Cartesian-space robot states. [[`flexiv_msgs/msg/RobotStates.msg`](flexiv_msgs/msg/RobotStates.msg)]
+- `/${robot_sn}/flexiv_robot_states`: [Flexiv robot states](https://www.flexiv.com/software/rdk/api/structflexiv_1_1rdk_1_1_robot_states.html) including the joint- and Cartesian-space robot states. [[`flexiv_msgs/msg/RobotStates.msg`](flexiv_msgs/msg/RobotStates.msg)]
 - `/joint_states`: Measured joint states of the robot: joint position, velocity and torque. [[`sensor_msgs/JointState.msg`](https://docs.ros.org/en/noetic/api/sensor_msgs/html/msg/JointState.html)]
 - `/flexiv_robot_states_broadcaster/tcp_pose`: Measured TCP pose expressed in world frame $^{0}T_{TCP}$ in position $[m]$ and quaternion. [[`geometry_msgs/PoseStamped.msg`](https://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/PoseStamped.html)]
 - `/flexiv_robot_states_broadcaster/external_wrench_in_tcp`: Estimated external wrench applied on TCP and expressed in TCP frame $^{TCP}F_{ext}$ in force $[N]$ and torque $[Nm]$. [[`geometry_msgs/WrenchStamped.msg`](https://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/WrenchStamped.html)]
@@ -201,3 +202,42 @@ The digital output ports on the control box can be set by publishing to the topi
 ```bash
 ros2 topic pub /gpio_controller/gpio_outputs flexiv_msgs/msg/GPIOStates "{states: [{pin: 0, state: true}, {pin: 2, state: true}]}"
 ```
+
+### Gripper Control
+
+The gripper control is implemented in the `flexiv_gripper` package to interface with the gripper that is connected to the robot.
+
+Start the `flexiv_gripper_node` with the following launch file:
+
+```bash
+ros2 launch flexiv_gripper flexiv_gripper.launch.py robot_sn:=[robot_sn]
+```
+
+Or, you can also start the gripper control with the robot driver if the gripper is Flexiv Grav:
+
+```bash
+ros2 launch flexiv_bringup rizon.launch.py robot_sn:=[robot_sn] load_gripper:=true
+```
+
+#### Gripper Actions
+
+In a new terminal, send the gripper action `move` goal to open or close the gripper:
+
+```bash
+# Closing the gripper
+ros2 action send_goal /flexiv_gripper_node/move flexiv_msgs/action/Move "{width: 0.01, velocity: 0.1, max_force: 20}"
+# Opening the gripper
+ros2 action send_goal /flexiv_gripper_node/move flexiv_msgs/action/Move "{width: 0.09, velocity: 0.1, max_force: 20}"
+```
+
+The `grasp` action enables the gripper to grasp with direct force control, but it requires the mounted gripper to support direct force control. Send a `grasp` command to the gripper:
+
+```bash
+ros2 action send_goal /flexiv_gripper_node/grasp flexiv_msgs/action/Grasp "{force: 0}"
+```
+
+To stop the gripper, send a `stop` service call:
+
+```bash
+ros2 service call /flexiv_gripper_node/stop std_srvs/srv/Trigger {}
+```

flexiv_bringup/README.md

@@ -3,7 +3,7 @@
 This package contains launch files: the main driver launcher, the MoveIt launch file and demo examples:
 
 - `rizon.launch.py` - the main launcher: starts *ros2_control* node including hardware interface, runs joint states, Flexiv robot states broadcaster, and a controller, and visualizes the current robot pose in RViZ. The default controller is `rizon_arm_controller`, a joint trajectory controller.
-- `rizon_moveit.launch.py` - runs MoveIt together with the driver. The controller for robot joints started in this launch file is *rizon_arm_controller*.
+- `rizon_moveit.launch.py` - runs MoveIt together with the main driver. The controller for robot joints started in this launch file is *rizon_arm_controller*.
 - `test_joint_trajectory_controller.launch` - sends joint trajectory goals to the *rizon_arm_controller*.
 - `sine_sweep_position.launch.py` - gets current joint states and then performs a sine-sweep motion with *forward_position_controller*.
 - `sine_sweep_impedance.launch.py` - gets current joint states and then performs a sine-sweep motion with *joint_impedance_controller*.

flexiv_bringup/config/sine_sweep_impedance_config.yaml

@@ -2,5 +2,13 @@ sine_sweep_impedance_controller:
   ros__parameters:
 
     controller_name: "joint_impedance_controller"
+    joints:
+      - joint1
+      - joint2
+      - joint3
+      - joint4
+      - joint5
+      - joint6
+      - joint7
     wait_sec_between_publish: 0.001
     speed_scaling: 1.0

flexiv_bringup/config/sine_sweep_position_config.yaml

@@ -2,5 +2,13 @@ sine_sweep_position_controller:
   ros__parameters:
 
     controller_name: "forward_position_controller"
+    joints:
+      - joint1
+      - joint2
+      - joint3
+      - joint4
+      - joint5
+      - joint6
+      - joint7
     wait_sec_between_publish: 0.001
     speed_scaling: 1.0

flexiv_bringup/launch/rizon.launch.py

@@ -1,21 +1,28 @@
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, RegisterEventHandler
+from launch.actions import (
+    DeclareLaunchArgument,
+    IncludeLaunchDescription,
+    RegisterEventHandler,
+)
 from launch.conditions import IfCondition, UnlessCondition
 from launch.event_handlers import OnProcessExit
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.actions import Node
+from launch_ros.parameter_descriptions import ParameterValue
+from launch_ros.substitutions import FindPackageShare
 from launch.substitutions import (
     Command,
     FindExecutable,
     LaunchConfiguration,
     PathJoinSubstitution,
 )
-from launch_ros.actions import Node
-from launch_ros.substitutions import FindPackageShare
 
 
 def generate_launch_description():
     rizon_type_param_name = "rizon_type"
     robot_sn_param_name = "robot_sn"
     start_rviz_param_name = "start_rviz"
+    load_gripper_param_name = "load_gripper"
     use_fake_hardware_param_name = "use_fake_hardware"
     fake_sensor_commands_param_name = "fake_sensor_commands"
     robot_controller_param_name = "robot_controller"
@@ -47,6 +54,14 @@ def generate_launch_description():
         )
     )
 
+    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(
             use_fake_hardware_param_name,
@@ -76,6 +91,7 @@ def generate_launch_description():
     rizon_type = LaunchConfiguration(rizon_type_param_name)
     robot_sn = LaunchConfiguration(robot_sn_param_name)
     start_rviz = LaunchConfiguration(start_rviz_param_name)
+    load_gripper = LaunchConfiguration(load_gripper_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)
@@ -86,28 +102,35 @@ def generate_launch_description():
     )
 
     # Get URDF via xacro
-    robot_description_content = Command(
-        [
-            PathJoinSubstitution([FindExecutable(name="xacro")]),
-            " ",
-            flexiv_urdf_xacro,
-            " ",
-            "robot_sn:=",
-            robot_sn,
-            " ",
-            "name:=",
-            "rizon",
-            " ",
-            "rizon_type:=",
-            rizon_type,
-            " ",
-            "use_fake_hardware:=",
-            use_fake_hardware,
-            " ",
-            "fake_sensor_commands:=",
-            fake_sensor_commands,
-        ]
+    robot_description_content = ParameterValue(
+        Command(
+            [
+                PathJoinSubstitution([FindExecutable(name="xacro")]),
+                " ",
+                flexiv_urdf_xacro,
+                " ",
+                "robot_sn:=",
+                robot_sn,
+                " ",
+                "name:=",
+                "rizon",
+                " ",
+                "rizon_type:=",
+                rizon_type,
+                " ",
+                "load_gripper:=",
+                load_gripper,
+                " ",
+                "use_fake_hardware:=",
+                use_fake_hardware,
+                " ",
+                "fake_sensor_commands:=",
+                fake_sensor_commands,
+            ]
+        ),
+        value_type=str,
     )
+
     robot_description = {"robot_description": robot_description_content}
 
     # RViZ
@@ -134,9 +157,26 @@ def generate_launch_description():
         package="controller_manager",
         executable="ros2_control_node",
         parameters=[robot_description, robot_controllers, {"robot_sn": robot_sn}],
+        remappings=[("joint_states", "flexiv_arm/joint_states")],
         output="both",
     )
 
+    # Joint state publisher
+    joint_state_publisher_node = Node(
+        package="joint_state_publisher",
+        executable="joint_state_publisher",
+        name="joint_state_publisher",
+        parameters=[
+            {
+                "source_list": [
+                    "flexiv_arm/joint_states",
+                    "flexiv_gripper_node/gripper_joint_states",
+                ],
+                "rate": 30,
+            }
+        ],
+    )
+
     # Robot state publisher
     robot_state_publisher_node = Node(
         package="robot_state_publisher",
@@ -173,6 +213,24 @@ def generate_launch_description():
         condition=UnlessCondition(use_fake_hardware),
     )
 
+    # Include gripper launch file
+    load_gripper_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            PathJoinSubstitution(
+                [
+                    FindPackageShare("flexiv_gripper"),
+                    "launch",
+                    "flexiv_gripper.launch.py",
+                ]
+            )
+        ),
+        launch_arguments={
+            "robot_sn": robot_sn,
+            "use_fake_hardware": use_fake_hardware,
+        }.items(),
+        condition=IfCondition(load_gripper),
+    )
+
     # Run gpio controller
     gpio_controller_spawner = Node(
         package="controller_manager",
@@ -200,9 +258,11 @@ def generate_launch_description():
 
     nodes = [
         ros2_control_node,
+        joint_state_publisher_node,
         robot_state_publisher_node,
         joint_state_broadcaster_spawner,
         flexiv_robot_states_broadcaster_spawner,
+        load_gripper_launch,
         gpio_controller_spawner,
         delay_rviz_after_joint_state_broadcaster_spawner,
         delay_robot_controller_spawner_after_joint_state_broadcaster_spawner,