diff --git a/.github/workflows/ros2.yml b/.github/workflows/ros2.yml index aba09f41..cd08daf8 100644 --- a/.github/workflows/ros2.yml +++ b/.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 diff --git a/README.md b/README.md index 3aa316f5..5c986240 100644 --- a/README.md +++ b/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` diff --git a/imgs/franka_gello_duo_v05_annotated.jpg b/imgs/franka_gello_duo_v05_annotated.jpg new file mode 100644 index 00000000..5f1d802f Binary files /dev/null and b/imgs/franka_gello_duo_v05_annotated.jpg differ diff --git a/imgs/franka_gello_single_v05_annotated.jpg b/imgs/franka_gello_single_v05_annotated.jpg new file mode 100644 index 00000000..090a3562 Binary files /dev/null and b/imgs/franka_gello_single_v05_annotated.jpg differ diff --git a/ros2/CHANGELOG.md b/ros2/CHANGELOG.md new file mode 100644 index 00000000..4e2cb5bc --- /dev/null +++ b/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 diff --git a/ros2/README.md b/ros2/README.md index d00116ef..8f61e7d6 100644 --- a/ros2/README.md +++ b/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: + +

+ + +

+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/ + ``` + - 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/ + ``` + - 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/ + ``` + +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:** diff --git a/ros2/src/franka_gello_state_publisher/config/example_duo.yaml b/ros2/src/franka_gello_state_publisher/config/example_duo.yaml index 461d33d9..a5cbe523 100644 --- a/ros2/src/franka_gello_state_publisher/config/example_duo.yaml +++ b/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 diff --git a/ros2/src/franka_gello_state_publisher/config/example_single.yaml b/ros2/src/franka_gello_state_publisher/config/example_single.yaml index e0a2cd81..9d294a88 100644 --- a/ros2/src/franka_gello_state_publisher/config/example_single.yaml +++ b/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 diff --git a/ros2/src/franka_gello_state_publisher/config/franka_gello_duo.yaml b/ros2/src/franka_gello_state_publisher/config/franka_gello_duo.yaml index 8e120ed6..1d7e7358 100644 --- a/ros2/src/franka_gello_state_publisher/config/franka_gello_duo.yaml +++ b/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, diff --git a/ros2/src/franka_gello_state_publisher/config/franka_gello_single.yaml b/ros2/src/franka_gello_state_publisher/config/franka_gello_single.yaml new file mode 100644 index 00000000..a0fc3814 --- /dev/null +++ b/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] diff --git a/ros2/src/franka_gello_state_publisher/franka_gello_state_publisher/gello_hardware.py b/ros2/src/franka_gello_state_publisher/franka_gello_state_publisher/gello_hardware.py index 18639be8..39548d7b 100644 --- a/ros2/src/franka_gello_state_publisher/franka_gello_state_publisher/gello_hardware.py +++ b/ros2/src/franka_gello_state_publisher/franka_gello_state_publisher/gello_hardware.py @@ -14,7 +14,7 @@ class GelloHardwareParams(TypedDict): joint_signs: List[int] gripper: bool gripper_range_rad: List[float] - best_offsets: List[float] + assembly_offsets: List[float] dynamixel_kp_p: List[int] dynamixel_kp_i: List[int] dynamixel_kp_d: List[int] @@ -71,6 +71,67 @@ def __setitem__(self, param_name: str, value: List[int]) -> None: class GelloHardware: """Hardware interface for GELLO teleoperation device.""" + # From https://frankarobotics.github.io/docs/robot_specifications.html#limits-for-franka-research-3-fr3 + JOINT_POSITION_LIMITS = np.array( + [ + [-2.9007, 2.9007], # -166/166 deg + [-1.8361, 1.8361], # -105/105 deg + [-2.9007, 2.9007], # -166/166 deg + [-3.0770, -0.1169], # -176/-7 deg + [-2.8763, 2.8763], # -165/165 deg + [0.4398, 4.6216], # -25/265 deg + [-3.0508, 3.0508], # -175/175 deg + ] + ) + MID_JOINT_POSITIONS = JOINT_POSITION_LIMITS.mean(axis=1) + + OPERATING_MODE = 5 # CURRENT_BASED_POSITION_MODE + CURRENT_LIMIT = 600 # mA + + @staticmethod + def normalize_joint_positions( + raw_positions: np.ndarray, + assembly_offsets: np.ndarray, + joint_signs: np.ndarray, + ) -> np.ndarray: + """ + Normalize joint positions to working range centered on mid_positions. + + Raw joint positions (in rad) are based on the motor's internal position register. + On power up, this register resets to [0, 2*Pi], losing tracking of full rotations (multi-turn). + Furthermore, these raw values are offset by the physical assembly position and may need to be + inverted based on motor direction. + + This function converts raw motor positions to absolute positions by: + 1. Removing physical assembly offsets + 2. Applying joint direction signs + 3. Wrapping to range [mid-pi, mid+pi) to resolve multi-turn ambiguity from the motor's power-on reset + + Parameters + ---------- + raw_positions : np.ndarray + Raw motor positions in radians + assembly_offsets : np.ndarray + Physical assembly position offsets + joint_signs : np.ndarray + Direction multipliers for each joint + + Returns + ------- + np.ndarray + Normalized joint positions in radians + + """ + return ( + np.mod( + (raw_positions - assembly_offsets) * joint_signs + - GelloHardware.MID_JOINT_POSITIONS, + 2 * np.pi, + ) + - np.pi + + GelloHardware.MID_JOINT_POSITIONS + ) + def __init__( self, hardware_config: GelloHardwareParams, @@ -84,12 +145,25 @@ def __init__( self._gripper = hardware_config["gripper"] self._num_total_joints = self._num_arm_joints + (1 if self._gripper else 0) self._gripper_range_rad = hardware_config["gripper_range_rad"] - self._best_offsets = np.array(hardware_config["best_offsets"]) + self._assembly_offsets = np.array(hardware_config["assembly_offsets"]) self._initialize_driver() - OPERATING_MODE = 5 # CURRENT_BASED_POSITION_MODE - CURRENT_LIMIT = 600 # mA + self._initial_arm_joints_raw = self._driver.get_joints()[: self._num_arm_joints] + + # Normalize the raw joint positions initially. After this, all position updates will be done + # incrementally to maintain continuity. + initial_arm_joints = self.normalize_joint_positions( + self._initial_arm_joints_raw, + self._assembly_offsets, + self._joint_signs, + ) + + # Store raw initial joint positions to compute joint position deltas on update + self._prev_arm_joints_raw = self._initial_arm_joints_raw.copy() + # Store processed initial joint positions for updating the processed position with the deltas + self._prev_arm_joints = initial_arm_joints.copy() + self._dynamixel_control_config = DynamixelControlConfig( kp_p=hardware_config["dynamixel_kp_p"].copy(), kp_i=hardware_config["dynamixel_kp_i"].copy(), @@ -98,8 +172,8 @@ def __init__( goal_position=self._goal_position_to_pulses( hardware_config["dynamixel_goal_position"] ).copy(), - goal_current=[CURRENT_LIMIT] * self._num_total_joints, - operating_mode=[OPERATING_MODE] * self._num_total_joints, + goal_current=[self.CURRENT_LIMIT] * self._num_total_joints, + operating_mode=[self.OPERATING_MODE] * self._num_total_joints, ) self._initialize_parameters() @@ -143,17 +217,44 @@ def update_dynamixel_control_parameter( "goal_position", self._dynamixel_control_config["goal_position"] ) - def read_joint_states(self) -> tuple[np.ndarray, float]: - """Read current joint positions and gripper state.""" - gello_joints_raw = self._driver.get_joints() - gello_arm_joints_raw = gello_joints_raw[: self._num_arm_joints] - gello_arm_joints = (gello_arm_joints_raw - self._best_offsets) * self._joint_signs + def get_joint_and_gripper_positions(self) -> tuple[np.ndarray, float]: + """Return a tuple containing the processed joint positions and gripper position percentage.""" + joints_raw = self._driver.get_joints() + arm_joints_raw = joints_raw[: self._num_arm_joints] + gripper_position_raw = joints_raw[-1] + return self.process_arm_joint_positions(arm_joints_raw), self.process_gripper_position( + gripper_position_raw + ) + + def process_arm_joint_positions(self, arm_joints_raw: np.ndarray) -> np.ndarray: + """ + Calculate arm joint positions from raw positions. - gripper_position = 0.0 - if self._gripper: - gripper_position = self._gripper_readout_to_percent(gello_joints_raw[-1]) + Applies deltas to previous positions to maintain continuity and clamps + to the robot's joint limits. + """ + # Compute joint position deltas and apply to previous processed positions + arm_joints_delta = (arm_joints_raw - self._prev_arm_joints_raw) * self._joint_signs + arm_joints = self._prev_arm_joints + arm_joints_delta + + # Store for next update + self._prev_arm_joints = arm_joints.copy() + self._prev_arm_joints_raw = arm_joints_raw.copy() + + arm_joints_clipped = np.clip( + arm_joints, self.JOINT_POSITION_LIMITS[:, 0], self.JOINT_POSITION_LIMITS[:, 1] + ) + return arm_joints_clipped - return gello_arm_joints, gripper_position + def process_gripper_position(self, gripper_position_raw: float) -> float: + """Convert and clamp raw gripper position to percentage (0-1). Return 0.0 if no gripper is present.""" + if not self._gripper: + return 0.0 + gripper_position_percent = (gripper_position_raw - self._gripper_range_rad[0]) / ( + self._gripper_range_rad[1] - self._gripper_range_rad[0] + ) + gripper_position_clipped = max(0.0, min(1.0, gripper_position_percent)) + return gripper_position_clipped def disable_torque(self) -> None: """Disable torque on all joints.""" @@ -161,13 +262,20 @@ def disable_torque(self) -> None: def _goal_position_to_pulses(self, goals: list[float]) -> list[int]: """Convert goal positions from radians to dynamixel pulses.""" - arm_goals_raw = (goals[: self._num_arm_joints] * self._joint_signs) + self._best_offsets - goals_raw = np.append(arm_goals_raw, goals[-1]) if self._gripper else arm_goals_raw - return [self._driver._rad_to_pulses(rad) for rad in goals_raw] + arm_goals = np.array(goals[: self._num_arm_joints]) - def _gripper_readout_to_percent(self, gripper_position: float) -> float: - """Convert gripper position to percentage (0-1).""" - gripper_position_percent = (gripper_position - self._gripper_range_rad[0]) / ( - self._gripper_range_rad[1] - self._gripper_range_rad[0] + # Apply the inverse mapping of the initialization process to convert arm goals back to raw motor commands: + # 1. Compute 'initial_rotations': The number of full 2*pi turns the motor was offset by at startup. + # 2. Reconstruct 'arm_goals_raw': Combine the rotation offset, goal position, and assembly offsets, + # applying the correct joint signs to match the motor's coordinate system. + initial_rotations = np.floor_divide( + self._initial_arm_joints_raw - self._assembly_offsets - self.MID_JOINT_POSITIONS, + 2 * np.pi, ) - return max(0.0, min(1.0, gripper_position_percent)) + arm_goals_raw = ( + initial_rotations * 2 * np.pi + arm_goals + self._assembly_offsets + ) * self._joint_signs + np.pi + + goals_raw = np.append(arm_goals_raw, goals[-1]) if self._gripper else arm_goals_raw + goals_raw_pulses = [self._driver._rad_to_pulses(rad) for rad in goals_raw] + return goals_raw_pulses diff --git a/ros2/src/franka_gello_state_publisher/franka_gello_state_publisher/gello_parameter_config.py b/ros2/src/franka_gello_state_publisher/franka_gello_state_publisher/gello_parameter_config.py index c5180689..187e98bb 100644 --- a/ros2/src/franka_gello_state_publisher/franka_gello_state_publisher/gello_parameter_config.py +++ b/ros2/src/franka_gello_state_publisher/franka_gello_state_publisher/gello_parameter_config.py @@ -78,7 +78,7 @@ def __init__(self): ), ParameterConfig( ParameterDescriptor( - name="best_offsets", + name="assembly_offsets", type=ParameterType.PARAMETER_DOUBLE_ARRAY, description="Joint offset calibration", read_only=True, diff --git a/ros2/src/franka_gello_state_publisher/franka_gello_state_publisher/gello_publisher.py b/ros2/src/franka_gello_state_publisher/franka_gello_state_publisher/gello_publisher.py index a71d3474..3a6c345a 100644 --- a/ros2/src/franka_gello_state_publisher/franka_gello_state_publisher/gello_publisher.py +++ b/ros2/src/franka_gello_state_publisher/franka_gello_state_publisher/gello_publisher.py @@ -63,7 +63,7 @@ def publish_joint_jog(self) -> None: "fr3_joint6", "fr3_joint7", ] - [gello_arm_joints, gripper_position] = self.gello_hardware.read_joint_states() + [gello_arm_joints, gripper_position] = self.gello_hardware.get_joint_and_gripper_positions() arm_joint_states = JointState() arm_joint_states.header.stamp = self.get_clock().now().to_msg() diff --git a/ros2/src/franka_gello_state_publisher/launch/main.launch.py b/ros2/src/franka_gello_state_publisher/launch/main.launch.py index 513a5d3d..6743ab90 100755 --- a/ros2/src/franka_gello_state_publisher/launch/main.launch.py +++ b/ros2/src/franka_gello_state_publisher/launch/main.launch.py @@ -37,7 +37,7 @@ def generate_robot_nodes(context): {"joint_signs": config["joint_signs"]}, {"gripper": config["gripper"]}, {"gripper_range_rad": config["gripper_range_rad"]}, - {"best_offsets": config["best_offsets"]}, + {"assembly_offsets": config["assembly_offsets"]}, {"dynamixel_kp_p": config["dynamixel_kp_p"]}, {"dynamixel_kp_i": config["dynamixel_kp_i"]}, {"dynamixel_kp_d": config["dynamixel_kp_d"]}, diff --git a/ros2/src/franka_gello_state_publisher/scripts/get_offsets.py b/ros2/src/franka_gello_state_publisher/scripts/get_offsets.py new file mode 100644 index 00000000..f96fc5c3 --- /dev/null +++ b/ros2/src/franka_gello_state_publisher/scripts/get_offsets.py @@ -0,0 +1,104 @@ +import os +from dataclasses import dataclass +from typing import Tuple +from textwrap import indent + +import numpy as np +import tyro + +from franka_gello_state_publisher.dynamixel.driver import DynamixelDriver +from franka_gello_state_publisher.gello_hardware import GelloHardware + +# The offset in radians from the gripper open position to the closed position. +GRIPPER_OPEN_TO_CLOSED_RAD = -1.22 +DEFAULT_BAUDRATE = 57600 + + +@dataclass +class Args: + port: str = "/dev/ttyUSB0" + """The port that GELLO is connected to.""" + + start_joints: Tuple[float, ...] = (0.0, 0.0, 0.0, -1.571, 0.0, 1.571, 0.0) + """The joint angles that the GELLO is placed in at (in radians).""" + + joint_signs: Tuple[int, ...] = (1, -1, 1, -1, 1, 1, 1) + """Sign multipliers for each joint to account for motor direction.""" + + gripper: bool = True + """Whether or not the gripper is attached.""" + + def __post_init__(self): + assert len(self.joint_signs) == len(self.start_joints) + for idx, j in enumerate(self.joint_signs): + assert j == -1 or j == 1, f"Joint idx: {idx} should be -1 or 1, but got {j}." + + @property + def num_arm_joints(self) -> int: + return len(self.start_joints) + + @property + def num_total_joints(self) -> int: + extra_joints = 1 if self.gripper else 0 + return self.num_arm_joints + extra_joints + + +def determine_offsets( + arm_joints_raw: np.ndarray, start_joints: np.ndarray, joint_signs: np.ndarray +) -> np.ndarray: + """ + Calculate assembly offsets by comparing current pose to expected initialization pose and rounding to nearest 90 degrees. + + Parameters + ---------- + arm_joints_raw : np.ndarray + Raw joint positions read from the driver + start_joints : np.ndarray + Expected joint positions in radians for the initialization pose + joint_signs : np.ndarray + Signs to apply to each joint angle to account for motor direction + + Returns + ------- + np.ndarray + Assembly offsets in radians, normalized to positive values. + + """ + arm_joints_normalized = GelloHardware.normalize_joint_positions( + arm_joints_raw, + np.zeros(len(arm_joints_raw)), # zero offsets since we are calculating them here + joint_signs, + ) + pose_differences = arm_joints_normalized - start_joints + offsets = np.round(pose_differences / (np.pi / 2)) * (np.pi / 2) + offsets_normalized = np.mod(offsets, 2 * np.pi) + return offsets_normalized + + +def main(args: Args) -> None: + joint_ids = list(range(1, args.num_total_joints + 1)) + driver = DynamixelDriver(joint_ids, port=args.port, baudrate=DEFAULT_BAUDRATE) + joints_raw = driver.get_joints() + arm_joints_raw = np.array(joints_raw[: args.num_arm_joints]) + assembly_offsets = determine_offsets( + arm_joints_raw, np.array(args.start_joints), np.array(args.joint_signs) + ) + + gripper_range_rad = None + if args.gripper and len(joints_raw) > args.num_arm_joints: + gripper_open = joints_raw[-1] + gripper_range_rad = [gripper_open + GRIPPER_OPEN_TO_CLOSED_RAD, gripper_open] + + print("Update your config files with the following values:\n") + print(indent(f'com_port: "{os.path.basename(args.port)}"', " ")) + print(indent(f"num_arm_joints: {args.num_arm_joints}", " ")) + print(indent(f"joint_signs: {list(args.joint_signs)}", " ")) + print(indent(f"gripper: {str(args.gripper).lower()}", " ")) + print(indent(f"assembly_offsets: {list(np.round(assembly_offsets, 3))} # rad", " ")) + if args.gripper: + print(indent(f"gripper_range_rad: {list(np.round(gripper_range_rad, 3))}", " ")) + print() + + +if __name__ == "__main__": + main(tyro.cli(Args))