Ros2/power on position independance

.github/workflows/ros2.yml

@@ -22,5 +22,5 @@ jobs:
           runCmd: |
             cd ros2
             colcon build --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=ON -DCHECK_TIDY=ON
-            colcon test
+            colcon test --event-handlers console_direct+ --pytest-args -v
             colcon test-result --verbose

README.md

@@ -11,6 +11,14 @@ For additional resources:
 - [Hardware Repository](https://github.com/wuphilipp/gello_mechanical) - STL files and build instructions
 - [ROS 2 Support](ros2/README.md)
 
+## Supported Robots
+- **I2RT YAM**
+- **Franka FR3** (ROS 2 implementation, please refer to the separate documenation in [`ros2/README.md`](ros2/README.md))
+- **Franka FER (Panda)**
+- **UR**
+- **xArm**
+- add your own, see [Adding New Robots](#adding-new-robots)
+
 ## Quick Start
 
 ```bash
@@ -101,11 +109,6 @@ Sample configs for the YAM arm and the xarm can be found in `configs`.
 - Uses `PORT_CONFIG_MAP` dictionary
 - Maps USB serial ports to robot configurations
 
-#### ROS 2 YAML configs for Franka
-- Used for ROS 2 packages
-- Runtime configuration loading
-- Located in `ros2/src/franka_gello_state_publisher/config/gello_config.yaml`
-
 ## Adding New Robots
 
 To integrate a new robot to the Python configs:
@@ -141,7 +144,7 @@ python scripts/gello_get_offset.py \
     --port /dev/serial/by-id/usb-FTDI_USB__-__Serial_Converter_FT7WBG6
 ```
 
-**Franka FR3:**
+**Franka FER (Panda):**
 ```bash
 python scripts/gello_get_offset.py \
     --start-joints 0 0 0 -1.57 0 1.57 0 \
@@ -160,7 +163,6 @@ python scripts/gello_get_offset.py \
 **Joint Signs Reference:**
 - UR: `1 1 -1 1 1 1`
 - Panda: `1 -1 1 1 1 -1 1`
-- FR3: `1 1 1 1 1 -1 1`
 - xArm: `1 1 1 1 1 1 1`
 - YAM: `1 -1 -1 -1 1 1`
 

ros2/CHANGELOG.md

@@ -0,0 +1,49 @@
+# Changelog
+
+> This changelog covers only the changes relevant to the ROS 2 implementation of GELLO.
+
+## `ros2-v2.0.0` - 2026-02-16
+ - Published joint positions are now independent of the GELLO's joint positions on power-on.
+ - Added assembly offset calculation script which differs from the one in the non-ROS part of this repository.
+ - **Breaking**: In the publisher's config files, the `best_offsets` parameter has been renamed to 
+  `assembly_offsets` and their values need to be updated:
+    - For pre-assembled Franka GELLO Duo's, use the new values provided.
+    - For self-assembled GELLO variants, use the newly added `get_offsets.py` script to determine
+      them (see README).
+
+## `ros2-v1.2.1` - 2026-01-22
+ - Bump dependencies for compatibility with Franka FR3 robot system version 5.9.x.
+
+## `ros2-v1.2.0` - 2025-12-15
+ - Adaptations for OpenRB-150 support as an alternative to the U2D2 converter.
+ - In the publisher's default config files, virtual springs and dampers are now disabled by default, in order to prevent accidental high power draw when the OpenRB-150 board is used without external power supply.
+ - Added a configuration file for the pre-assembled Franka GELLO Duo.
+ - Renamed example config files and updated the default config file used by `franka_gello_state_publisher` accordingly.
+ - Improved Dynamixel initialization reliability by starting joint-position polling only after initialization is complete.
+
+## `ros2-v1.1.0` - 2025-11-20
+ - Improved error handling for Dynamixel driver connection failures.
+ - In dual arm systems, always terminate both publishers when one of them shuts down
+ - Signals get handled by ROS 2 launch system instead of by the publisher node itself.
+
+## `ros2-v1.0.0` - 2025-11-17
+ - A more generalized Dynamixel motor driver is introduced. To avoid breaking changes with non-ROS2 parts of this repository, the driver is now part of the `franka_gello_state_publisher` package.
+ - Refactored `franka_gello_state_publisher` into modular components.
+ - Added functionality for configurable virtual springs and dampers using the Dynamixel motors internal current-based position mode
+ - **Breaking**: In the publisher's config files, the `num_joints` parameter has been renamed to `num_arm_joints`. Please update your config files accordingly.
+
+## `ros2-v0.2.1` - 2025-11-04
+ - Fixed Dev Container build failure by updating Franka-related dependencies
+
+## `ros2-v0.2.0` - 2025-07-22
+ - Added support for controlling multiple Franka FR3 arms in ROS 2 using namespaces.
+ - Replaced all links and references to github.com/frankaemika with github.com/frankarobotics
+
+## `ros2-v0.1.1` - 2025-06-13
+ - Improve setup process of the Franka ROS 2 integration for local environments
+
+## `ros2-v0.1.0` - 2025-06-03
+ - Added ROS 2 implementation of GELLO for Franka FR3 robots:
+     - `franka_fr3_arm_controllers`: joint impedance controller for Franka FR3 robots based on GELLO input
+     - `franka_gello_state_publisher`: publishes GELLO joint states as `sensor_msgs/JointState` ROS 2 messages
+     - `franka_gripper_manager`: control of Franka Hand and Robotiq 2F-85 grippers based on GELLO input

ros2/README.md

@@ -1,6 +1,6 @@
-# GELLO ROS 2 humble integration
+# GELLO Implementation for Franka FR3 with ROS 2 (Humble) 
 
-This folder contains all required ROS 2 packages for using GELLO. 
+This folder contains all required ROS 2 packages for using GELLO with a Franka FR3 robot.
 
 ## Packages Overview
 
@@ -115,13 +115,52 @@ cd /workspace/ros2
 colcon build
 ```
 
-#### Step 2: Configure your GELLO
-
-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**.
-
-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/`.
+#### Step 2: Configure the assembly offsets of your GELLO
+
+> 🚀 **Skip this step for pre-assembled GELLOs:**  
+> If you are using the pre-assembled "Franka GELLO" (single arm) or "Franka GELLO Duo" variant, or if you assembled it carefully according to the assembly instructions, the default configuration files provided in `franka_gello_state_publisher/config/` should work out of the box. You can proceed directly to **Step 3**.
+
+If you have assembled the GELLO yourself, you need to determine the following parameters for your specific assembly:
+- `joint_signs`: Multiplicative signs for each joint. If assembled according to the instructions, this should be `1 -1 1 -1 1 1 1` for the Franka GELLO. If you notice that some joints move in the opposite direction than expected, you may need to change the corresponding sign.
+- `assembly_offsets`: Since the Dynamixel flanges allow for 4 possible mounting orientations, the zero position of the joints may be offset by a multiple of pi/2 (= 90°). This parameter allows you to compensate for these offsets.
+- `gripper_range_rad`: Similarly, the joint angles corresponding to a fully open and fully closed gripper may differ based on the assembly.
+
+Use the `get_offsets.py` script in `/ros2/src/franka_gello_state_publisher/scripts/` to determine the `assembly_offsets` and `gripper_range_rad` parameters for your specific assembly.
+
+1. Move the GELLO arm(s) to the calibration pose, depending on if it is a single arm or dual arm assembly:
+
+    <p align="left">
+      <img src="../imgs/franka_gello_single_v05_annotated.jpg" height="300px"/>
+      <img src="../imgs/franka_gello_duo_v05_annotated.jpg" height="300px"/>
+    </p>
+2. Run the script:
+    > 💡 The `start-joints` argument of the script can be adjusted if you want to use a different calibration pose. The provided values correspond to the recommended poses shown in the images above.
+    - Single arm:
+      ```bash
+      cd /workspace/ros2/src/franka_gello_state_publisher/scripts/
+      python3 get_offsets.py \
+          --start-joints 0 0 0 -1.57 0 1.57 0  \
+          --joint-signs 1 -1 1 -1 1 1 1 \
+          --port /dev/serial/by-id/<your_com_port_id>
+      ```
+    - Dual arm left:
+      ```bash
+      cd /workspace/ros2/src/franka_gello_state_publisher/scripts/
+      python3 get_offsets.py \
+          --start-joints -1.57 -0.80 1.80 -3.00 1.40 1.50 -2.10  \
+          --joint-signs 1 -1 1 -1 1 1 1 \
+          --port /dev/serial/by-id/<your_com_port_id>
+      ```
+    - Dual arm right:
+      ```bash
+      cd /workspace/ros2/src/franka_gello_state_publisher/scripts/
+      python3 get_offsets.py \
+          --start-joints 1.57 -0.80 -1.80 -3.00 -1.40 1.50 2.10  \
+          --joint-signs 1 -1 1 -1 1 1 1 \
+          --port /dev/serial/by-id/<your_com_port_id>
+      ```
+
+Use the output of the `get_offsets.py` script to update the values 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:
 
@@ -147,7 +186,7 @@ The `config_file` argument is **optional**. If not provided, it defaults to `exa
 - `num_joints`: 7 for Franka FR3
 - `joint_signs`: as used for calibration
 - `gripper`: true if Gello gripper state shall be used
-- `best_offsets` and `gripper_range_rad`: as determined with calibration routine
+- `assembly_offsets` and `gripper_range_rad`: as determined with calibration routine
 - Dynamixel control parameters: `dynamixel_...` (see below)
 
 **Virtual Springs and Dampers:**<a name="virtual-springs-dampers"></a>

ros2/src/franka_gello_state_publisher/config/example_duo.yaml

@@ -2,13 +2,13 @@
 # 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"
+  com_port: "usb-FTDI_USB__-__Serial_Converter_FT9MFSIO-if00-port0"
   num_arm_joints: 7
-  joint_signs: [1, 1, 1, 1, 1, -1, 1]
+  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] # rad
-  gripper_range_rad: [2.77856, 3.50931]
+  assembly_offsets: [0.000, 0.000, 3.142, 3.142, 3.142, 4.712, 0.000] # 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
@@ -21,13 +21,13 @@ LEFT:
   dynamixel_kp_d:          [   250,   100,    80,    60,    30,    10,     5,     0]
 
 RIGHT:
-  com_port: "usb-FTDI_USB__-__Serial_Converter_FT9MFR6C-if00-port0"
   namespace: "right"
+  com_port: "usb-FTDI_USB__-__Serial_Converter_FT9MFR6C-if00-port0"
   num_arm_joints: 7
-  joint_signs: [1, 1, 1, 1, 1, -1, 1]
+  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] # rad
-  gripper_range_rad: [2.77856, 3.50931]
+  assembly_offsets: [0.000, 0.000, 3.142, 3.142, 3.142, 4.712, 0.000] # 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

ros2/src/franka_gello_state_publisher/config/example_single.yaml

@@ -2,13 +2,13 @@
 # Check the README to learn how to adapt it to your setup.
 
 SINGLE:
-  com_port: "usb-FTDI_USB__-__Serial_Converter_FT9MFR6C-if00-port0"
   namespace: ""
+  com_port: "usb-FTDI_USB__-__Serial_Converter_FT9MFR6C-if00-port0"
   num_arm_joints: 7
-  joint_signs: [1, 1, 1, 1, 1, -1, 1]
+  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] # rad
-  gripper_range_rad: [2.77856, 3.50931]
+  assembly_offsets: [0.000, 0.000, 3.142, 3.142, 3.142, 4.712, 0.000] # 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

ros2/src/franka_gello_state_publisher/config/franka_gello_duo.yaml

@@ -2,12 +2,12 @@
 # See https://franka.de/documents
 
 LEFT:
-  com_port: "usb-ROBOTIS_OpenRB-150_xxxxxxxx" # REPLACE WITH YOUR ID
   namespace: "left"
+  com_port: "usb-ROBOTIS_OpenRB-150_xxxxxxxx" # REPLACE WITH YOUR ID
   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
+  assembly_offsets: [0.000, 0.000, 3.142, 3.142, 3.142, 4.712, 0.000] # 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,
@@ -21,12 +21,12 @@ LEFT:
   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"
+  com_port: "usb-ROBOTIS_OpenRB-150_xxxxxxxx" # REPLACE WITH YOUR ID
   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
+  assembly_offsets: [0.000, 0.000, 3.142, 3.142, 3.142, 4.712, 0.000] # 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,

ros2/src/franka_gello_state_publisher/config/franka_gello_single.yaml

@@ -0,0 +1,21 @@
+# This configuration works with the pre-assembled "Franka GELLO" (single arm) by Franka Robotics GmbH
+# See https://franka.de/documents
+
+SINGLE:
+  namespace: ""
+  com_port: "usb-ROBOTIS_OpenRB-150_xxxxxxxx" # REPLACE WITH YOUR ID
+  num_arm_joints: 7
+  joint_signs: [1, -1, 1, -1, 1, 1, 1]
+  gripper: true
+  assembly_offsets: [0.000, 0.000, 3.142, 3.142, 3.142, 4.712, 0.000] # 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]