ROS2: Improvements for OpenRB-150, Franka Gello Duo and reliability

ros2/README.md

@@ -90,27 +90,35 @@ colcon test
 ## Getting Started
 
 ### 1. **Run the GELLO Publisher**  
-#### Step 1: Determine your GELLO USB ID
+#### Step 1: Determine the port IDs of your communication converters
 
-To proceed, you need to know the USB ID of your GELLO device. This can be determined by running:
+To proceed, you need to know the port IDs of your communication converters (U2D2 or OpenRB-150).
+This can be determined by running:
 
 ```bash
 ls /dev/serial/by-id
 ```
 
 Example output:
+- U2D2: `usb-FTDI_USB__-__Serial_Converter_FT7WBG6` 
+- OpenRB-150: `usb-ROBOTIS_OpenRB-150_2B375CB3503059384C2E3120FF053624-if00`
+
+Use the ID to update the `com_port` parameter in your GELLO configuration file located in `/workspace/ros2/src/franka_gello_state_publisher/config/`.
+
+Rebuild the project to ensure the updated configuration is applied:
 
 ```bash
-usb-FTDI_USB__-__Serial_Converter_FT7WBG6
+cd /workspace/ros2
+colcon build
 ```
 
-In this case, the `GELLO_USB_ID` would be `/dev/serial/by-id/usb-FTDI_USB__-__Serial_Converter_FT7WBG6`.
+#### Step 2: Configure your GELLO
 
-#### Step 2: Configure your GELLO 
-
-If not done already, follow the instructions of the [`Create the GELLO configuration and determining joint ID's` section in the main README.md](../README.md#create-the-gello-configuration-and-determining-joint-ids). 
+If you are using the "Franka GELLO Duo" variant and assembled it carefully according to the assembly instructions, the default configuration file provided in `franka_gello_state_publisher/config/` should work out of the box and you can proceed with **Step 3**.
 
-Use the output of the `gello_get_offset.py` script to update the `best_offsets` and `gripper_range_rad` in the `/workspace/ros2/src/franka_gello_state_publisher/config/gello_config.yaml` file.
+If you have a different variant or still encounter configuration issues, please follow the instructions of the [`Create the GELLO configuration and determining joint ID's` section in the main README.md](../README.md#create-the-gello-configuration-and-determining-joint-ids). 
+
+Use the output of the `gello_get_offset.py` script to update the `best_offsets` and `gripper_range_rad` in your GELLO configuration file located in `/workspace/ros2/src/franka_gello_state_publisher/config/`.
 
 Rebuild the project to ensure the updated configuration is applied:
 
@@ -127,12 +135,12 @@ Create a configuration file in `src/franka_gello_state_publisher/config/` or mod
 ros2 launch franka_gello_state_publisher main.launch.py [config_file:=your_config.yaml]
 ```
 
-The `config_file` argument is **optional**. If not provided, it defaults to `example_fr3_config.yaml` in the `franka_gello_state_publisher/config/` directory.
+The `config_file` argument is **optional**. If not provided, it defaults to `example_single.yaml` in the `franka_gello_state_publisher/config/` directory.
 
 **Configuration parameters:**
 
-- `com_port`: the previously determined <GELLO_USB_ID>
-- `namespace`: ROS 2 namespace (must match the robot and the gripper).
+- `com_port`: the previously determined port ID of your communication converter
+- `namespace`: ROS 2 namespace (must match the robot and the gripper)
 - `num_joints`: 7 for Franka FR3
 - `joint_signs`: as used for calibration
 - `gripper`: true if Gello gripper state shall be used
@@ -153,6 +161,10 @@ This is done by setting the following parameters in the configuration file. Each
 
 When the GELLO publisher is started, these parameters are used to configure the Dynamixel motors. The motors then operate in their internal "Current-based Position Control" mode with a current limit set to 600mA. Check the [Dynamixel documentation](https://emanual.robotis.com/docs/en/dxl/x/xl330-m288/) for more information on the control parameters.
 
+> ⚠️ **Warning for OpenRB-150:**  
+> If using the OpenRB-150 instead of the U2D2 communication converter, do not enable torque until an external 5V power supply is connected to the board's power terminal and the jumper is set to "VIN(DXL)". Using USB power for torque operation may damage your computer's USB port.
+> Please refer to the [OpenRB-150 manual](https://emanual.robotis.com/docs/en/parts/controller/openrb-150/#when-running-5v-dynamixel-with-the-usb-power) for more information.
+
 > 💡 **Hint:**  
 > The example configuration files give a good starting point for these values:
 > - springs in joints 1, 2, 4 and gripper.

ros2/src/franka_gello_state_publisher/config/example_fr3_duo_config.yaml → ros2/src/franka_gello_state_publisher/config/example_duo.yaml

@@ -1,13 +1,20 @@
+# This is an example config file for two 7-DoF GELLO arms teleoperating two Franka FR3 robots.
+# Check the README to learn how to adapt it to your setup.
+
 LEFT:
   com_port: "usb-FTDI_USB__-__Serial_Converter_FT9MFSIO-if00-port0"
   namespace: "left"
   num_arm_joints: 7
   joint_signs: [1, 1, 1, 1, 1, -1, 1]
   gripper: true
-  best_offsets: [3.142, 3.142, 3.142, 4.712, 3.142, 4.712, 1.571]
+  best_offsets: [3.142, 3.142, 3.142, 4.712, 3.142, 4.712, 1.571] # rad
   gripper_range_rad: [2.77856, 3.50931]
   # Optional Dynamixel control parameters for all arm joints (+ gripper):
-  dynamixel_torque_enable: [     1,     1,     1,     1,     1,     1,     1,     1] # 1 on, 0 off
+  # Warning: If using the OpenRB-150 instead of the U2D2 communication converter,
+  #          do not enable torque until an external 5V power supply is connected to the board's
+  #          power terminal and the jumper is set to "VIN(DXL)". Using USB power for torque
+  #          operation may damage your computer's USB port.
+  dynamixel_torque_enable: [     0,     0,     0,     0,     0,     0,     0,     0] # 1 on, 0 off
   dynamixel_goal_position: [   0.0,   0.0,   0.0,-1.571,   0.0, 1.571,   0.0, 3.509] # rad
   dynamixel_kp_p:          [    30,    60,     0,    30,     0,     0,     0,    50]
   dynamixel_kp_i:          [     0,     0,     0,     0,     0,     0,     0,     0]
@@ -19,10 +26,14 @@ RIGHT:
   num_arm_joints: 7
   joint_signs: [1, 1, 1, 1, 1, -1, 1]
   gripper: true
-  best_offsets: [3.142, 3.142, -3.142, 4.712, 3.142, 4.712, 1.571]
+  best_offsets: [3.142, 3.142, 3.142, 4.712, 3.142, 4.712, 1.571] # rad
   gripper_range_rad: [2.77856, 3.50931]
   # Optional Dynamixel control parameters for all arm joints (+ gripper):
-  dynamixel_torque_enable: [     1,     1,     1,     1,     1,     1,     1,     1] # 1 on, 0 off
+  # Warning: If using the OpenRB-150 instead of the U2D2 communication converter,
+  #          do not enable torque until an external 5V power supply is connected to the board's
+  #          power terminal and the jumper is set to "VIN(DXL)". Using USB power for torque
+  #          operation may damage your computer's USB port.
+  dynamixel_torque_enable: [     0,     0,     0,     0,     0,     0,     0,     0] # 1 on, 0 off
   dynamixel_goal_position: [   0.0,   0.0,   0.0,-1.571,   0.0, 1.571,   0.0, 3.509] # rad
   dynamixel_kp_p:          [    30,    60,     0,    30,     0,     0,     0,    50]
   dynamixel_kp_i:          [     0,     0,     0,     0,     0,     0,     0,     0]

ros2/src/franka_gello_state_publisher/config/example_fr3_config.yaml → ros2/src/franka_gello_state_publisher/config/example_single.yaml

@@ -1,13 +1,20 @@
-robot1:
+# This is an example config file for a 7-DoF GELLO arm teleoperating a Franka FR3 robot.
+# Check the README to learn how to adapt it to your setup.
+
+SINGLE:
   com_port: "usb-FTDI_USB__-__Serial_Converter_FT9MFR6C-if00-port0"
   namespace: ""
   num_arm_joints: 7
   joint_signs: [1, 1, 1, 1, 1, -1, 1]
   gripper: true
-  best_offsets: [3.142, 3.142, -3.142, 4.712, 3.142, 4.712, 1.571]
+  best_offsets: [3.142, 3.142, -3.142, 4.712, 3.142, 4.712, 1.571] # rad
   gripper_range_rad: [2.77856, 3.50931]
   # Optional Dynamixel control parameters for all arm joints (+ gripper):
-  dynamixel_torque_enable: [     1,     1,     1,     1,     1,     1,     1,     1] # 1 on, 0 off
+  # Warning: If using the OpenRB-150 instead of the U2D2 communication converter,
+  #          do not enable torque until an external 5V power supply is connected to the board's
+  #          power terminal and the jumper is set to "VIN(DXL)". Using USB power for torque
+  #          operation may damage your computer's USB port.
+  dynamixel_torque_enable: [     0,     0,     0,     0,     0,     0,     0,     0] # 1 on, 0 off
   dynamixel_goal_position: [   0.0,   0.0,   0.0,-1.571,   0.0, 1.571,   0.0, 3.509] # rad
   dynamixel_kp_p:          [    30,    60,     0,    30,     0,     0,     0,    50]
   dynamixel_kp_i:          [     0,     0,     0,     0,     0,     0,     0,     0]

ros2/src/franka_gello_state_publisher/config/franka_gello_duo.yaml

@@ -0,0 +1,40 @@
+# This configuration works with the pre-assembled "Franka GELLO Duo" by Franka Robotics GmbH
+# See https://franka.de/documents
+
+LEFT:
+  com_port: "usb-ROBOTIS_OpenRB-150_xxxxxxxx" # REPLACE WITH YOUR ID
+  namespace: "left"
+  num_arm_joints: 7
+  joint_signs: [1, -1, 1, -1, 1, 1, 1]
+  gripper: true
+  best_offsets: [3.142, 3.142, 0.000, 0.000, 0.000, 1.571, 3.142] # rad
+  gripper_range_rad: [2.00, 3.22]
+  # Optional Dynamixel control parameters for all arm joints (+ gripper):
+  # Warning: If using the OpenRB-150 instead of the U2D2 communication converter,
+  #          do not enable torque until an external 5V power supply is connected to the board's
+  #          power terminal and the jumper is set to "VIN(DXL)". Using USB power for torque
+  #          operation may damage your computer's USB port.
+  dynamixel_torque_enable: [     0,     0,     0,     0,     0,     0,     0,     0] # 1 on, 0 off
+  dynamixel_goal_position: [   0.0,   0.0,   0.0,-1.571,   0.0, 1.571,   0.0, 3.509] # rad
+  dynamixel_kp_p:          [    30,    60,     0,    30,     0,     0,     0,    50]
+  dynamixel_kp_i:          [     0,     0,     0,     0,     0,     0,     0,     0]
+  dynamixel_kp_d:          [   250,   100,    80,    60,    30,    10,     5,     0]
+
+RIGHT:
+  com_port: "usb-ROBOTIS_OpenRB-150_xxxxxxxx" # REPLACE WITH YOUR ID
+  namespace: "right"
+  num_arm_joints: 7
+  joint_signs: [1, -1, 1, -1, 1, 1, 1]
+  gripper: true
+  best_offsets: [3.142, 3.142, 6.283, 0.000, 6.283, 1.571, 3.142] # rad
+  gripper_range_rad: [2.00, 3.22]
+  # Optional Dynamixel control parameters for all arm joints (+ gripper):
+  # Warning: If using the OpenRB-150 instead of the U2D2 communication converter,
+  #          do not enable torque until an external 5V power supply is connected to the board's
+  #          power terminal and the jumper is set to "VIN(DXL)". Using USB power for torque
+  #          operation may damage your computer's USB port.
+  dynamixel_torque_enable: [     0,     0,     0,     0,     0,     0,     0,     0] # 1 on, 0 off
+  dynamixel_goal_position: [   0.0,   0.0,   0.0,-1.571,   0.0, 1.571,   0.0, 3.509] # rad
+  dynamixel_kp_p:          [    30,    60,     0,    30,     0,     0,     0,    50]
+  dynamixel_kp_i:          [     0,     0,     0,     0,     0,     0,     0,     0]
+  dynamixel_kp_d:          [   250,   100,    80,    60,    30,    10,     5,     0]

ros2/src/franka_gello_state_publisher/franka_gello_state_publisher/dynamixel/README.md

@@ -1,6 +1,9 @@
-# Dynamixel Motor Configuration
+# Dynamixel Driver
 
 This directory contains the configuration-driven Dynamixel driver that supports multiple motor types through YAML configuration files.
+The Dynamixel motors can be connected to the system via a communication converter:
+- [U2D2](https://emanual.robotis.com/docs/en/parts/interface/u2d2/)
+- [OpenRB-150](https://emanual.robotis.com/docs/en/parts/controller/openrb-150/)
 
 ## Configuration File Structure
 

ros2/src/franka_gello_state_publisher/franka_gello_state_publisher/dynamixel/driver.py

@@ -1,4 +1,5 @@
 import time
+from glob import glob
 from pathlib import Path
 from threading import Event, Lock, Thread
 from typing import Protocol, Sequence
@@ -88,7 +89,15 @@ def read_value_by_name(self, name: str) -> Sequence[int]:
         """
         ...
 
-    def close(self):
+    def start_joint_polling(self) -> None:
+        """Start the background polling thread."""
+        ...
+
+    def stop_joint_polling(self) -> None:
+        """Stop the background polling thread."""
+        ...
+
+    def close(self) -> None:
         """Close the driver."""
         ...
 
@@ -162,7 +171,13 @@ def read_value_by_name(self, name: str) -> Sequence[int]:
     def get_joints(self) -> np.ndarray:
         return self._pulses_to_rad(self._storage_map["goal_position"].copy())
 
-    def close(self):
+    def start_joint_polling(self) -> None:
+        pass
+
+    def stop_joint_polling(self) -> None:
+        pass
+
+    def close(self) -> None:
         pass
 
 
@@ -173,6 +188,7 @@ def __init__(
         port: str = "/dev/ttyUSB0",
         baudrate: int = 57600,
         motor_type: str = "xl330",
+        autostart_polling: bool = False,
     ):
         """
         Initialize the DynamixelDriver class.
@@ -187,6 +203,8 @@ def __init__(
             The baudrate for communication (default is 57600).
         motor_type : str, optional
             The type of motor to use, e.g., "xl330" (default is "xl330").
+        autostart_polling : bool, optional
+            Whether to automatically start the joint polling thread (default is False).
 
         Raises
         ------
@@ -199,7 +217,7 @@ def __init__(
         self._ids = ids
         self._port = port
         self._baudrate = baudrate
-        self._joint_angles = None
+        self._buffered_joint_positions = None
         self._lock = Lock()
 
         # Load motor configuration
@@ -239,10 +257,15 @@ def __init__(
             self._portHandler.openPort()
             self._portHandler.setBaudRate(self._baudrate)
         except SerialException:
-            raise ConnectionError(
-                f"Could not open port {self._port}. Make sure you have specified the correct port, "
-                "that the device is connected and that you have proper permissions to access it."
-            ) from None
+            detected_ports = self._detect_com_ports()
+            if self._port in detected_ports:
+                msg = "Check that you have permissions to access it."
+            elif detected_ports:
+                ports_list = ", ".join(detected_ports)
+                msg = f"Did you specify the correct port? Detected ports: {ports_list}"
+            else:
+                msg = "Is the device connected? No supported devices were detected."
+            raise ConnectionError(f"Could not open port {self._port}. {msg}") from None
 
         # Verify connection by attempting to read the model number
         try:
@@ -257,7 +280,11 @@ def __init__(
         self.write_value_by_name("torque_enable", [0] * len(self._ids))
 
         self._stop_thread = Event()
-        self._start_reading_thread()
+        self._polling_thread = None
+        self._is_polling = False
+
+        if autostart_polling:
+            self.start_joint_polling()
 
     def _set_group(
         self,
@@ -285,7 +312,8 @@ def _set_group(
                     raise RuntimeError(f"Failed to set {name} for Dynamixel with ID {dxl_id}")
             comm_result = groupSyncWriteHandler.txPacket()
             if comm_result != COMM_SUCCESS:
-                raise RuntimeError(f"Failed to syncwrite {name}")
+                result_string = self._packetHandler.getTxRxResult(comm_result)
+                raise RuntimeError(f"Failed to syncwrite {name}, {result_string}")
             groupSyncWriteHandler.clearParam()
 
     def write_value_by_name(self, name: str, values: Sequence[int | None]):
@@ -302,7 +330,8 @@ def _read_group(self, name: str, groupSyncReadHandler, value_length: int) -> lis
         with self._lock:
             result = groupSyncReadHandler.txRxPacket()
             if result != COMM_SUCCESS:
-                raise RuntimeError(f"Failed to sync read {name}, comm result: {result}")
+                result_string = self._packetHandler.getTxRxResult(result)
+                raise RuntimeError(f"Failed to sync read {name}, {result_string}")
             values = []
             for dxl_id in self._ids:
                 if groupSyncReadHandler.isAvailable(
@@ -324,36 +353,48 @@ def read_value_by_name(self, name: str) -> list[int]:
             value_length=self._ctrl_table[name]["len"],
         )
 
-    def _start_reading_thread(self):
-        self._reading_thread = Thread(target=self._read_joint_angles_loop)
-        self._reading_thread.daemon = True
-        self._reading_thread.start()
+    def start_joint_polling(self) -> None:
+        if self._is_polling:
+            return
+        self._stop_thread.clear()
+        self._polling_thread = Thread(target=self._joint_polling_loop)
+        self._polling_thread.daemon = True
+        self._polling_thread.start()
+        self._is_polling = True
+
+    def stop_joint_polling(self) -> None:
+        if not self._is_polling:
+            return
+        self._stop_thread.set()
+        if self._polling_thread is not None:
+            self._polling_thread.join()
+        self._is_polling = False
+
+    def _joint_polling_loop(self):
+        # Continuously read joint positions and update the _buffered_joint_positions array
 
-    def _read_joint_angles_loop(self):
-        # Continuously read joint angles and update the joint_angles array
         while not self._stop_thread.is_set():
             time.sleep(0.001)
             try:
-                _joint_angles = self._read_group(
-                    "present_position",
-                    self._groupSyncReadHandlers["present_position"],
-                    self._ctrl_table["present_position"]["len"],
+                self._buffered_joint_positions = np.array(
+                    self.read_value_by_name("present_position"), dtype=int
                 )
-                self._joint_angles = np.array(_joint_angles, dtype=int)
             except RuntimeError as e:
-                print(f"warning, comm failed: {e}")
+                print(f"Warning: {e}")
                 continue
 
     def get_joints(self) -> np.ndarray:
-        # Return a copy of the joint_angles array to avoid race conditions
-        while self._joint_angles is None:
-            time.sleep(0.1)
-        _j = self._joint_angles.copy()
-        return self._pulses_to_rad(_j)
-
-    def close(self):
-        self._stop_thread.set()
-        self._reading_thread.join()
+        if self._is_polling:
+            # If polling, return _buffered_joint_positions array, use copy to avoid race conditions
+            while self._buffered_joint_positions is None:
+                time.sleep(0.01)
+            return self._pulses_to_rad(self._buffered_joint_positions.copy())
+        else:
+            joint_positions = np.array(self.read_value_by_name("present_position"), dtype=int)
+            return self._pulses_to_rad(joint_positions)
+
+    def close(self) -> None:
+        self.stop_joint_polling()
         self._portHandler.closePort()
 
     def _pulses_to_rad(self, pulses) -> np.ndarray:
@@ -363,3 +404,14 @@ def _pulses_to_rad(self, pulses) -> np.ndarray:
     def _rad_to_pulses(self, rad: float) -> int:
         """Convert radians to pulses using motor-specific configuration."""
         return int(rad / (2 * np.pi) * self._pulses_per_revolution)
+
+    def _detect_com_ports(self) -> list[str]:
+        """Detect available com_ports of supported communication converters."""
+        SUPPORTED_CONVERTERS = [
+            "usb-FTDI_USB__-__Serial_Converter",
+            "usb-ROBOTIS_OpenRB-150",
+        ]
+        matches = []
+        for converter in SUPPORTED_CONVERTERS:
+            matches.extend(glob(f"/dev/serial/by-id/{converter}*"))
+        return matches