From 99d8361dfee3645bc04ae5edac82413d0bede502 Mon Sep 17 00:00:00 2001 From: Peter Wern Date: Tue, 3 Jun 2025 10:51:39 +0200 Subject: [PATCH 01/17] refactored the driver and extended its interface to other motor functions --- gello/dynamixel/driver.py | 357 +++++++++++++++++++++++++------------- 1 file changed, 233 insertions(+), 124 deletions(-) diff --git a/gello/dynamixel/driver.py b/gello/dynamixel/driver.py index 89325c75..fd6af300 100644 --- a/gello/dynamixel/driver.py +++ b/gello/dynamixel/driver.py @@ -9,28 +9,44 @@ from dynamixel_sdk.port_handler import PortHandler from dynamixel_sdk.robotis_def import ( COMM_SUCCESS, - DXL_HIBYTE, - DXL_HIWORD, - DXL_LOBYTE, - DXL_LOWORD, ) -# Constants -ADDR_TORQUE_ENABLE = 64 -ADDR_GOAL_POSITION = 116 -LEN_GOAL_POSITION = 4 -ADDR_PRESENT_POSITION = 132 -LEN_PRESENT_POSITION = 4 -TORQUE_ENABLE = 1 -TORQUE_DISABLE = 0 +# Constants for XL330-M288-T and XL330-M077-T Dynamixel servos +CTRL_TABLE = { + "torque_enable": {"addr": 64, "len": 1, "min": 0, "max": 1}, + "goal_position": {"addr": 116, "len": 4}, + "present_position": {"addr": 132, "len": 4, "read_only": True}, + "goal_current": {"addr": 102, "len": 2, "min": -1750, "max": 1750}, + "kp_d": {"addr": 80, "len": 2, "min": 0, "max": 16383}, + "kp_i": {"addr": 82, "len": 2, "min": -1750, "max": 1750}, + "kp_p": {"addr": 84, "len": 2, "min": -1750, "max": 1750}, + "operating_mode": {"addr": 11, "len": 1, "min": 0, "max": 6}, +} +OPERATING_MODES = { + 0: "Current Control Mode", + 1: "Velocity Control Mode", + 3: "Position Control Mode (Default)", + 4: "Extended Position Control Mode", + 5: "Current-based Position Control Mode", + 6: "PWM Control Mode", +} +PULSES_PER_REVOLUTION = 4095 + + +def pulses_to_rad(pulses) -> float: + return pulses / PULSES_PER_REVOLUTION * 2 * np.pi + + +def rad_to_pulses(rad: float) -> int: + return int(rad / (2 * np.pi) * PULSES_PER_REVOLUTION) class DynamixelDriverProtocol(Protocol): - def set_joints(self, joint_angles: Sequence[float]): + def set_joints(self, joint_angles: Sequence[float | None]): """Set the joint angles for the Dynamixel servos. Args: - joint_angles (Sequence[float]): A list of joint angles. + joint_angles (Sequence[float | None]): A list of joint angles. """ ... @@ -58,33 +74,101 @@ def get_joints(self) -> np.ndarray: """ ... + def set_by_name(self, name: str, values: Sequence[int | None]): + """Set a group of values by name. + + Args: + name (str): The name of the control parameter. + values (Sequence[int | None]): A list of values to set for each servo. + """ + ... + + def read_by_name(self, name: str) -> Sequence[int]: + """Read a group of values by name. + + Args: + name (str): The name of the control parameter to read. + + Returns: + Sequence[int]: A list of values read from the servos. + """ + ... + def close(self): """Close the driver.""" + ... class FakeDynamixelDriver(DynamixelDriverProtocol): def __init__(self, ids: Sequence[int]): self._ids = ids - self._joint_angles = np.zeros(len(ids), dtype=int) - self._torque_enabled = False + self._storage_map = { + "goal_position": np.zeros(len(ids), dtype=int), + "goal_current": np.zeros(len(ids), dtype=int), + "kp_d": np.zeros(len(ids), dtype=int), + "kp_i": np.zeros(len(ids), dtype=int), + "kp_p": np.zeros(len(ids), dtype=int), + "operating_mode": np.zeros(len(ids), dtype=int), + "torque_enable": np.zeros(len(ids), dtype=int), + } + + def _set_group( + self, + name: str, + values: Sequence[int | None], + value_min: int = None, + value_max: int = None, + ): + if len(values) != len(self._ids): + raise ValueError(f"The length of {name} must match the number of servos") + for dxl_id, value in zip(self._ids, values): + if value is None: + continue + if value_min is not None: + value = max(value, value_min) + if value_max is not None: + value = min(value, value_max) + self._storage_map[name][self._ids.index(dxl_id)] = value + print(f"Set {name} {value} for ID {dxl_id}", flush=True) + + def set_by_name(self, name: str, values: Sequence[int | None]): + self._set_group( + name=name, + values=values, + value_min=CTRL_TABLE[name].get("min", None), + value_max=CTRL_TABLE[name].get("max", None), + ) - def set_joints(self, joint_angles: Sequence[float]): - if len(joint_angles) != len(self._ids): - raise ValueError( - "The length of joint_angles must match the number of servos" - ) - if not self._torque_enabled: - raise RuntimeError("Torque must be enabled to set joint angles") - self._joint_angles = np.array(joint_angles) + def _read_group(self, name: str) -> Sequence[int]: + if name == "present_position": + name = "goal_position" + elif name == "present_current": + name = "goal_current" + if name not in self._storage_map: + raise ValueError(f"Register {name} not found.") + return self._storage_map[name].copy() + + def read_by_name(self, name: str) -> Sequence[int]: + return self._read_group(name) + + def set_joints(self, joint_angles: Sequence[float | None]): + self.set_by_name( + "goal_position", + [ + rad_to_pulses(angle) if angle is not None else None + for angle in joint_angles + ], + ) + + def get_joints(self) -> np.ndarray: + return pulses_to_rad(self._storage_map["goal_position"].copy()) def torque_enabled(self) -> bool: - return self._torque_enabled + return bool(np.any(self._storage_map["torque_enable"])) def set_torque_mode(self, enable: bool): - self._torque_enabled = enable - - def get_joints(self) -> np.ndarray: - return self._joint_angles.copy() + torque_value = 1 if enable else 0 + self.set_by_name("torque_enable", [torque_value] * len(self._ids)) def close(self): pass @@ -108,18 +192,30 @@ def __init__( # Initialize the port handler, packet handler, and group sync read/write self._portHandler = PortHandler(port) self._packetHandler = PacketHandler(2.0) - self._groupSyncRead = GroupSyncRead( - self._portHandler, - self._packetHandler, - ADDR_PRESENT_POSITION, - LEN_PRESENT_POSITION, - ) - self._groupSyncWrite = GroupSyncWrite( - self._portHandler, - self._packetHandler, - ADDR_GOAL_POSITION, - LEN_GOAL_POSITION, - ) + + # Create group sync read/write handlers for each CTRL_TABLE entry + self._groupSyncReadHandlers = {} + self._groupSyncWriteHandlers = {} + for key, entry in CTRL_TABLE.items(): + self._groupSyncReadHandlers[key] = GroupSyncRead( + self._portHandler, + self._packetHandler, + entry["addr"], + entry["len"], + ) + # Add parameters for each Dynamixel servo to the group sync read + for dxl_id in self._ids: + if not self._groupSyncReadHandlers[key].addParam(dxl_id): + raise RuntimeError( + f"Failed to add parameter for Dynamixel with ID {dxl_id}" + ) + if "read_only" not in entry or not entry["read_only"]: + self._groupSyncWriteHandlers[key] = GroupSyncWrite( + self._portHandler, + self._packetHandler, + entry["addr"], + entry["len"], + ) # Open the port and set the baudrate if not self._portHandler.openPort(): @@ -128,117 +224,130 @@ def __init__( if not self._portHandler.setBaudRate(baudrate): raise RuntimeError(f"Failed to change the baudrate, {baudrate}") - # Add parameters for each Dynamixel servo to the group sync read - for dxl_id in self._ids: - if not self._groupSyncRead.addParam(dxl_id): - raise RuntimeError( - f"Failed to add parameter for Dynamixel with ID {dxl_id}" - ) - - # Disable torque for each Dynamixel servo - self._torque_enabled = False + # Disable torque for all Dynamixel servos try: - self.set_torque_mode(self._torque_enabled) + self.set_torque_mode(False) except Exception as e: print(f"port: {port}, {e}") self._stop_thread = Event() self._start_reading_thread() - def set_joints(self, joint_angles: Sequence[float]): - if len(joint_angles) != len(self._ids): - raise ValueError( - "The length of joint_angles must match the number of servos" - ) - if not self._torque_enabled: - raise RuntimeError("Torque must be enabled to set joint angles") - - for dxl_id, angle in zip(self._ids, joint_angles): - # Convert the angle to the appropriate value for the servo - position_value = int(angle * 2048 / np.pi) - - # Allocate goal position value into byte array - param_goal_position = [ - DXL_LOBYTE(DXL_LOWORD(position_value)), - DXL_HIBYTE(DXL_LOWORD(position_value)), - DXL_LOBYTE(DXL_HIWORD(position_value)), - DXL_HIBYTE(DXL_HIWORD(position_value)), - ] - - # Add goal position value to the Syncwrite parameter storage - dxl_addparam_result = self._groupSyncWrite.addParam( - dxl_id, param_goal_position - ) - if not dxl_addparam_result: - raise RuntimeError( - f"Failed to set joint angle for Dynamixel with ID {dxl_id}" - ) - - # Syncwrite goal position - dxl_comm_result = self._groupSyncWrite.txPacket() - if dxl_comm_result != COMM_SUCCESS: - raise RuntimeError("Failed to syncwrite goal position") - - # Clear syncwrite parameter storage - self._groupSyncWrite.clearParam() - - def torque_enabled(self) -> bool: - return self._torque_enabled + def _set_group( + self, + name: str, + values: Sequence[int | None], + groupSyncWriteHandler, + value_length: int, + value_min: int = None, + value_max: int = None, + ): + if len(values) != len(self._ids): + raise ValueError(f"The length of {name} must match the number of servos") + with self._lock: + for dxl_id, value in zip(self._ids, values): + if value is None: + continue + if value_min is not None: + value = max(value, value_min) + if value_max is not None: + value = min(value, value_max) + # Convert value to little-endian byte array of length value_length + param = [(value >> (8 * i)) & 0xFF for i in range(value_length)] + result = groupSyncWriteHandler.addParam(dxl_id, param) + if not result: + 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}") + groupSyncWriteHandler.clearParam() + + def set_by_name(self, name: str, values: Sequence[int | None]): + self._set_group( + name=name, + values=values, + groupSyncWriteHandler=self._groupSyncWriteHandlers[name], + value_length=CTRL_TABLE[name]["len"], + value_min=CTRL_TABLE[name].get("min", None), + value_max=CTRL_TABLE[name].get("max", None), + ) - def set_torque_mode(self, enable: bool): - torque_value = TORQUE_ENABLE if enable else TORQUE_DISABLE + def _read_group( + self, name: str, groupSyncReadHandler, value_length: int + ) -> list[int]: with self._lock: + result = groupSyncReadHandler.txRxPacket() + if result != COMM_SUCCESS: + raise RuntimeError(f"Failed to sync read {name}, comm result: {result}") + values = [] for dxl_id in self._ids: - dxl_comm_result, dxl_error = self._packetHandler.write1ByteTxRx( - self._portHandler, dxl_id, ADDR_TORQUE_ENABLE, torque_value - ) - if dxl_comm_result != COMM_SUCCESS or dxl_error != 0: - print(dxl_comm_result) - print(dxl_error) + if groupSyncReadHandler.isAvailable( + dxl_id, CTRL_TABLE[name]["addr"], value_length + ): + value = groupSyncReadHandler.getData( + dxl_id, CTRL_TABLE[name]["addr"], value_length + ) + value = int(np.int32(np.uint32(value))) + values.append(value) + else: raise RuntimeError( - f"Failed to set torque mode for Dynamixel with ID {dxl_id}" + f"Failed to get {name} for Dynamixel with ID {dxl_id}" ) + return values + + def read_by_name(self, name: str) -> list[int]: + return self._read_group( + name=name, + groupSyncReadHandler=self._groupSyncReadHandlers[name], + value_length=CTRL_TABLE[name]["len"], + ) - self._torque_enabled = enable + def set_joints(self, joint_angles: Sequence[float | None]): + joint_angles_pulses = [ + rad_to_pulses(rad) if rad is not None else None for rad in joint_angles + ] + self.set_by_name("goal_position", joint_angles_pulses) def _start_reading_thread(self): - self._reading_thread = Thread(target=self._read_joint_angles) + self._reading_thread = Thread(target=self._read_joint_angles_loop) self._reading_thread.daemon = True self._reading_thread.start() - def _read_joint_angles(self): + 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) - with self._lock: - _joint_angles = np.zeros(len(self._ids), dtype=int) - dxl_comm_result = self._groupSyncRead.txRxPacket() - if dxl_comm_result != COMM_SUCCESS: - print(f"warning, comm failed: {dxl_comm_result}") - continue - for i, dxl_id in enumerate(self._ids): - if self._groupSyncRead.isAvailable( - dxl_id, ADDR_PRESENT_POSITION, LEN_PRESENT_POSITION - ): - angle = self._groupSyncRead.getData( - dxl_id, ADDR_PRESENT_POSITION, LEN_PRESENT_POSITION - ) - angle = np.int32(np.uint32(angle)) - _joint_angles[i] = angle - else: - raise RuntimeError( - f"Failed to get joint angles for Dynamixel with ID {dxl_id}" - ) + try: + _joint_angles = self._read_group( + "present_position", + self._groupSyncReadHandlers["present_position"], + CTRL_TABLE["present_position"]["len"], + ) + _joint_angles = np.array(_joint_angles, dtype=int) self._joint_angles = _joint_angles - # self._groupSyncRead.clearParam() # TODO what does this do? should i add it + except RuntimeError as e: + print(f"warning, comm failed: {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) - # with self._lock: _j = self._joint_angles.copy() - return _j / 2048.0 * np.pi + return pulses_to_rad(_j) + + def torque_enabled(self) -> bool: + try: + torque_values = self.read_by_name("torque_enable") + return any(val == 1 for val in torque_values) + except RuntimeError: + return False + + def set_torque_mode(self, enable: bool): + torque_value = 1 if enable else 0 + self.set_by_name("torque_enable", [torque_value] * len(self._ids)) def close(self): self._stop_thread.set() From c9acfefc4392f70f54d3647f90fcfab54bcaf85a Mon Sep 17 00:00:00 2001 From: Peter Wern Date: Fri, 18 Jul 2025 14:19:36 +0200 Subject: [PATCH 02/17] extracted motor specific values to config file in order to enable addition of other motors --- gello/dynamixel/README.md | 75 +++++++++++ gello/dynamixel/driver.py | 160 +++++++++++++---------- gello/dynamixel/motor_configs/xl330.yaml | 67 ++++++++++ requirements.txt | 1 + 4 files changed, 237 insertions(+), 66 deletions(-) create mode 100644 gello/dynamixel/README.md create mode 100644 gello/dynamixel/motor_configs/xl330.yaml diff --git a/gello/dynamixel/README.md b/gello/dynamixel/README.md new file mode 100644 index 00000000..377cceb6 --- /dev/null +++ b/gello/dynamixel/README.md @@ -0,0 +1,75 @@ +# Dynamixel Motor Configuration + +This directory contains the configuration-driven Dynamixel driver that supports multiple motor types through YAML configuration files. + +## Configuration File Structure + +Motor configurations are stored in the `motor_configs/` subdirectory, with one YAML file per motor type. The filename (without extension) is used as the motor type identifier. + +### File Structure: +``` +gello/dynamixel/ +├── driver.py +├── motor_configs/ +│ ├── xl330.yaml +│ ├── .yaml +│ └── ... +└── README.md +``` + +### Configuration File Format + +Each motor configuration file has the following structure: + +```yaml +# Motor configuration for +# Reference: https://example.com/motor/documentation + +pulses_per_revolution: 4095 # Motor-specific resolution +control_table: + parameter_name: + addr: 64 # Control table address + len: 1 # Data length in bytes + min: 0 # Optional: minimum value + max: 1 # Optional: maximum value + unit: "1 [mA]" # Optional: factor and unit (for reference) + read_only: true # Optional: if parameter is read-only + # ... more parameters +eeprom_area_end: 63 # Last address of the EEPROM area. Adresses above are in RAM area. +operating_modes: + 0: "Mode Name" + 1: "Another Mode" + # ... more modes +``` + +## Usage + +### Regular Driver + +```python +from gello.dynamixel.driver import DynamixelDriver + +driver = DynamixelDriver( + ids=[1, 2, 3], + port="/dev/ttyUSB0", + baudrate=57600, + motor_type="xl330" +) +``` + +### Fake Driver for Testing + +```python +from gello.dynamixel.driver import FakeDynamixelDriver + +fake_driver = FakeDynamixelDriver(ids=[1, 2, 3], motor_type="xl330") +``` + +## Adding New Motor Types + +To add support for a new motor type: + +1. Create a new YAML file in `motor_configs/` directory +2. Name the file after your motor type (e.g., `my_motor.yaml`) +3. Follow the format shown above +4. Use the filename (without extension) as the motor type in your driver diff --git a/gello/dynamixel/driver.py b/gello/dynamixel/driver.py index fd6af300..721027c7 100644 --- a/gello/dynamixel/driver.py +++ b/gello/dynamixel/driver.py @@ -1,8 +1,10 @@ import time +from pathlib import Path from threading import Event, Lock, Thread from typing import Protocol, Sequence import numpy as np +import yaml from dynamixel_sdk.group_sync_read import GroupSyncRead from dynamixel_sdk.group_sync_write import GroupSyncWrite from dynamixel_sdk.packet_handler import PacketHandler @@ -11,35 +13,27 @@ COMM_SUCCESS, ) -# Constants for XL330-M288-T and XL330-M077-T Dynamixel servos -CTRL_TABLE = { - "torque_enable": {"addr": 64, "len": 1, "min": 0, "max": 1}, - "goal_position": {"addr": 116, "len": 4}, - "present_position": {"addr": 132, "len": 4, "read_only": True}, - "goal_current": {"addr": 102, "len": 2, "min": -1750, "max": 1750}, - "kp_d": {"addr": 80, "len": 2, "min": 0, "max": 16383}, - "kp_i": {"addr": 82, "len": 2, "min": -1750, "max": 1750}, - "kp_p": {"addr": 84, "len": 2, "min": -1750, "max": 1750}, - "operating_mode": {"addr": 11, "len": 1, "min": 0, "max": 6}, -} -OPERATING_MODES = { - 0: "Current Control Mode", - 1: "Velocity Control Mode", - 3: "Position Control Mode (Default)", - 4: "Extended Position Control Mode", - 5: "Current-based Position Control Mode", - 6: "PWM Control Mode", -} -PULSES_PER_REVOLUTION = 4095 - - -def pulses_to_rad(pulses) -> float: - return pulses / PULSES_PER_REVOLUTION * 2 * np.pi - - -def rad_to_pulses(rad: float) -> int: - return int(rad / (2 * np.pi) * PULSES_PER_REVOLUTION) - +# Configuration loader for motor types +def load_motor_config(motor_type: str = "xl330") -> dict: + """Load motor configuration from YAML file. + + Args: + motor_type (str): The type of motor to load configuration for. + + Returns: + dict: The motor configuration dictionary. + """ + config_dir = Path(__file__).parent / "motor_configs" + config_path = config_dir / f"{motor_type}.yaml" + + if not config_path.exists(): + available_types = [f.stem for f in config_dir.glob("*.yaml")] + raise ValueError(f"Motor type '{motor_type}' not found. Available types: {available_types}") + + with open(config_path, 'r') as f: + config = yaml.safe_load(f) + + return config class DynamixelDriverProtocol(Protocol): def set_joints(self, joint_angles: Sequence[float | None]): @@ -51,15 +45,17 @@ def set_joints(self, joint_angles: Sequence[float | None]): ... def torque_enabled(self) -> bool: - """Check if torque is enabled for the Dynamixel servos. + """Check if torque is enabled for at least one of the Dynamixel servos. + If you want to check which servos have torque enabled, you can use the read_value_by_name method. Returns: - bool: True if torque is enabled, False if it is disabled. + bool: True if torque is enabled for one of the servos, False if it is disabled for all. """ ... def set_torque_mode(self, enable: bool): - """Set the torque mode for the Dynamixel servos. + """Enable or disable torque for all Dynamixel servos. + If you want to enable torque for a specific servo, you can use the write_value_by_name method. Args: enable (bool): True to enable torque, False to disable. @@ -74,7 +70,7 @@ def get_joints(self) -> np.ndarray: """ ... - def set_by_name(self, name: str, values: Sequence[int | None]): + def write_value_by_name(self, name: str, values: Sequence[int | None]): """Set a group of values by name. Args: @@ -83,7 +79,7 @@ def set_by_name(self, name: str, values: Sequence[int | None]): """ ... - def read_by_name(self, name: str) -> Sequence[int]: + def read_value_by_name(self, name: str) -> Sequence[int]: """Read a group of values by name. Args: @@ -100,8 +96,14 @@ def close(self): class FakeDynamixelDriver(DynamixelDriverProtocol): - def __init__(self, ids: Sequence[int]): + def __init__(self, ids: Sequence[int], motor_type: str = "xl330"): self._ids = ids + + # Load motor configuration + self._motor_config = load_motor_config(motor_type) + self._ctrl_table = self._motor_config["control_table"] + self._pulses_per_revolution = self._motor_config["pulses_per_revolution"] + self._storage_map = { "goal_position": np.zeros(len(ids), dtype=int), "goal_current": np.zeros(len(ids), dtype=int), @@ -112,12 +114,20 @@ def __init__(self, ids: Sequence[int]): "torque_enable": np.zeros(len(ids), dtype=int), } + def _pulses_to_rad(self, pulses) -> np.ndarray: + """Convert pulses to radians using motor-specific configuration.""" + return np.array(pulses) / self._pulses_per_revolution * 2 * np.pi + + 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 _set_group( self, name: str, values: Sequence[int | None], - value_min: int = None, - value_max: int = None, + value_min: int | None = None, + value_max: int | None = None, ): if len(values) != len(self._ids): raise ValueError(f"The length of {name} must match the number of servos") @@ -131,12 +141,12 @@ def _set_group( self._storage_map[name][self._ids.index(dxl_id)] = value print(f"Set {name} {value} for ID {dxl_id}", flush=True) - def set_by_name(self, name: str, values: Sequence[int | None]): + def write_value_by_name(self, name: str, values: Sequence[int | None]): self._set_group( name=name, values=values, - value_min=CTRL_TABLE[name].get("min", None), - value_max=CTRL_TABLE[name].get("max", None), + value_min=self._ctrl_table[name].get("min", None), + value_max=self._ctrl_table[name].get("max", None), ) def _read_group(self, name: str) -> Sequence[int]: @@ -146,37 +156,40 @@ def _read_group(self, name: str) -> Sequence[int]: name = "goal_current" if name not in self._storage_map: raise ValueError(f"Register {name} not found.") - return self._storage_map[name].copy() + return self._storage_map[name].copy().tolist() - def read_by_name(self, name: str) -> Sequence[int]: + def read_value_by_name(self, name: str) -> Sequence[int]: return self._read_group(name) def set_joints(self, joint_angles: Sequence[float | None]): - self.set_by_name( + self.write_value_by_name( "goal_position", [ - rad_to_pulses(angle) if angle is not None else None + self._rad_to_pulses(angle) if angle is not None else None for angle in joint_angles ], ) def get_joints(self) -> np.ndarray: - return pulses_to_rad(self._storage_map["goal_position"].copy()) + return self._pulses_to_rad(self._storage_map["goal_position"].copy()) def torque_enabled(self) -> bool: return bool(np.any(self._storage_map["torque_enable"])) def set_torque_mode(self, enable: bool): torque_value = 1 if enable else 0 - self.set_by_name("torque_enable", [torque_value] * len(self._ids)) + self.write_value_by_name("torque_enable", [torque_value] * len(self._ids)) def close(self): pass - class DynamixelDriver(DynamixelDriverProtocol): def __init__( - self, ids: Sequence[int], port: str = "/dev/ttyUSB0", baudrate: int = 57600 + self, + ids: Sequence[int], + port: str = "/dev/ttyUSB0", + baudrate: int = 57600, + motor_type: str = "xl330" ): """Initialize the DynamixelDriver class. @@ -184,19 +197,26 @@ def __init__( ids (Sequence[int]): A list of IDs for the Dynamixel servos. port (str): The USB port to connect to the arm. baudrate (int): The baudrate for communication. + motor_type (str): The type of motor to use (e.g., "xl330"). """ self._ids = ids self._joint_angles = None self._lock = Lock() + + # Load motor configuration + self._motor_config = load_motor_config(motor_type) + self._ctrl_table = self._motor_config["control_table"] + self._operating_modes = self._motor_config["operating_modes"] + self._pulses_per_revolution = self._motor_config["pulses_per_revolution"] # Initialize the port handler, packet handler, and group sync read/write self._portHandler = PortHandler(port) self._packetHandler = PacketHandler(2.0) - # Create group sync read/write handlers for each CTRL_TABLE entry + # Create group sync read/write handlers for each control table entry self._groupSyncReadHandlers = {} self._groupSyncWriteHandlers = {} - for key, entry in CTRL_TABLE.items(): + for key, entry in self._ctrl_table.items(): self._groupSyncReadHandlers[key] = GroupSyncRead( self._portHandler, self._packetHandler, @@ -239,8 +259,8 @@ def _set_group( values: Sequence[int | None], groupSyncWriteHandler, value_length: int, - value_min: int = None, - value_max: int = None, + value_min: int | None = None, + value_max: int | None = None, ): if len(values) != len(self._ids): raise ValueError(f"The length of {name} must match the number of servos") @@ -264,14 +284,14 @@ def _set_group( raise RuntimeError(f"Failed to syncwrite {name}") groupSyncWriteHandler.clearParam() - def set_by_name(self, name: str, values: Sequence[int | None]): + def write_value_by_name(self, name: str, values: Sequence[int | None]): self._set_group( name=name, values=values, groupSyncWriteHandler=self._groupSyncWriteHandlers[name], - value_length=CTRL_TABLE[name]["len"], - value_min=CTRL_TABLE[name].get("min", None), - value_max=CTRL_TABLE[name].get("max", None), + value_length=self._ctrl_table[name]["len"], + value_min=self._ctrl_table[name].get("min", None), + value_max=self._ctrl_table[name].get("max", None), ) def _read_group( @@ -284,10 +304,10 @@ def _read_group( values = [] for dxl_id in self._ids: if groupSyncReadHandler.isAvailable( - dxl_id, CTRL_TABLE[name]["addr"], value_length + dxl_id, self._ctrl_table[name]["addr"], value_length ): value = groupSyncReadHandler.getData( - dxl_id, CTRL_TABLE[name]["addr"], value_length + dxl_id, self._ctrl_table[name]["addr"], value_length ) value = int(np.int32(np.uint32(value))) values.append(value) @@ -297,18 +317,18 @@ def _read_group( ) return values - def read_by_name(self, name: str) -> list[int]: + def read_value_by_name(self, name: str) -> list[int]: return self._read_group( name=name, groupSyncReadHandler=self._groupSyncReadHandlers[name], - value_length=CTRL_TABLE[name]["len"], + value_length=self._ctrl_table[name]["len"], ) def set_joints(self, joint_angles: Sequence[float | None]): joint_angles_pulses = [ - rad_to_pulses(rad) if rad is not None else None for rad in joint_angles + self._rad_to_pulses(rad) if rad is not None else None for rad in joint_angles ] - self.set_by_name("goal_position", joint_angles_pulses) + self.write_value_by_name("goal_position", joint_angles_pulses) def _start_reading_thread(self): self._reading_thread = Thread(target=self._read_joint_angles_loop) @@ -323,7 +343,7 @@ def _read_joint_angles_loop(self): _joint_angles = self._read_group( "present_position", self._groupSyncReadHandlers["present_position"], - CTRL_TABLE["present_position"]["len"], + self._ctrl_table["present_position"]["len"], ) _joint_angles = np.array(_joint_angles, dtype=int) self._joint_angles = _joint_angles @@ -336,24 +356,32 @@ def get_joints(self) -> np.ndarray: while self._joint_angles is None: time.sleep(0.1) _j = self._joint_angles.copy() - return pulses_to_rad(_j) + return self._pulses_to_rad(_j) def torque_enabled(self) -> bool: try: - torque_values = self.read_by_name("torque_enable") + torque_values = self.read_value_by_name("torque_enable") return any(val == 1 for val in torque_values) except RuntimeError: return False def set_torque_mode(self, enable: bool): torque_value = 1 if enable else 0 - self.set_by_name("torque_enable", [torque_value] * len(self._ids)) + self.write_value_by_name("torque_enable", [torque_value] * len(self._ids)) def close(self): self._stop_thread.set() self._reading_thread.join() self._portHandler.closePort() + def _pulses_to_rad(self, pulses) -> np.ndarray: + """Convert pulses to radians using motor-specific configuration.""" + return np.array(pulses) / self._pulses_per_revolution * 2 * np.pi + + 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 main(): # Set the port, baudrate, and servo IDs diff --git a/gello/dynamixel/motor_configs/xl330.yaml b/gello/dynamixel/motor_configs/xl330.yaml new file mode 100644 index 00000000..3dd7902b --- /dev/null +++ b/gello/dynamixel/motor_configs/xl330.yaml @@ -0,0 +1,67 @@ +# Motor configuration for Dynamixel XL330-M288-T and XL330-M077-T +# Reference: https://emanual.robotis.com/docs/en/dxl/x/xl330-m288/ + +pulses_per_revolution: 4095 +control_table: + model_number: { addr: 0, len: 2, read_only: true } + model_information: { addr: 2, len: 4, read_only: true } + firmware_version: { addr: 6, len: 1, read_only: true } + id: { addr: 7, len: 1, min: 0, max: 252 } + baud_rate: { addr: 8, len: 1, min: 0, max: 6 } + return_delay_time: { addr: 9, len: 1, min: 0, max: 254, unit: "2 [μsec]" } + drive_mode: { addr: 10, len: 1, min: 0, max: 13 } + operating_mode: { addr: 11, len: 1, min: 0, max: 16 } + secondary_id: { addr: 12, len: 1, min: 0, max: 252 } + protocol_type: { addr: 13, len: 1, min: 2, max: 22 } + homing_offset: { addr: 20, len: 4, min: -1044479, max: 1044479, unit: "1 [pulse]" } + moving_threshold: { addr: 24, len: 4, min: 0, max: 1023, unit: "0.229 [rev/min]" } + temperature_limit: { addr: 31, len: 1, min: 0, max: 100, unit: "1 [°C]" } + max_voltage_limit: { addr: 32, len: 2, min: 31, max: 70, unit: "0.1 [V]" } + min_voltage_limit: { addr: 34, len: 2, min: 31, max: 70, unit: "0.1 [V]" } + pwm_limit: { addr: 36, len: 2, min: 0, max: 885, unit: "0.113 [%]" } + current_limit: { addr: 38, len: 2, min: 0, max: 1750, unit: "1 [mA]" } + velocity_limit: { addr: 44, len: 4, min: 0, max: 2047, unit: "0.229 [rev/min]" } + max_position_limit: { addr: 48, len: 4, min: 0, max: 4095, unit: "1 [pulse]" } + min_position_limit: { addr: 52, len: 4, min: 0, max: 4095, unit: "1 [pulse]" } + startup_configuration: { addr: 60, len: 1, min: 0, max: 3 } + pwm_slope: { addr: 62, len: 1, min: 1, max: 255, unit: "1.977 [mV/msec]" } + shutdown: { addr: 63, len: 1 } + torque_enable: { addr: 64, len: 1, min: 0, max: 1} + led: { addr: 65, len: 1, min: 0, max: 1 } + status_return_level: { addr: 68, len: 1, min: 0, max: 2 } + registered_instruction: { addr: 69, len: 1, min: 0, max: 1, read_only: true } + hardware_error_status: { addr: 70, len: 1, read_only: true } + velocity_i_gain: { addr: 76, len: 2, min: 0, max: 16383 } + velocity_p_gain: { addr: 78, len: 2, min: 0, max: 16383 } + kp_d: { addr: 80, len: 2, min: 0, max: 16383 } + kp_i: { addr: 82, len: 2, min: 0, max: 16383 } + kp_p: { addr: 84, len: 2, min: 0, max: 16383 } + feedforward_2nd_gain: { addr: 88, len: 2, min: 0, max: 16383 } + feedforward_1st_gain: { addr: 90, len: 2, min: 0, max: 16383 } + bus_watchdog: { addr: 98, len: 1, min: 1, max: 127, unit: "20 [msec]" } + goal_pwm: { addr: 100, len: 2 } + goal_current: { addr: 102, len: 2, unit: "1 [mA]"} + goal_velocity: { addr: 104, len: 4, unit: "0.229 [rev/min]" } + profile_acceleration: { addr: 108, len: 4, min: 0, max: 32767, unit: "214.577 [rev/min2]" } + profile_velocity: { addr: 112, len: 4, min: 0, max: 32767, unit: "0.229 [rev/min]" } + goal_position: { addr: 116, len: 4, unit: "1 [pulse]"} + realtime_tick: { addr: 120, len: 2, min: 0, max: 32767, unit: "1 [msec]", read_only: true } + moving: { addr: 122, len: 1, min: 0, max: 1, read_only: true } + moving_status: { addr: 123, len: 1, read_only: true } + present_pwm: { addr: 124, len: 2, read_only: true } + present_current: { addr: 126, len: 2, unit: "1 [mA]", read_only: true } + present_velocity: { addr: 128, len: 4, unit: "0.229 [rev/min]", read_only: true } + present_position: { addr: 132, len: 4, unit: "1 [pulse]", read_only: true } + velocity_trajectory: { addr: 136, len: 4, unit: "0.229 [rev/min]", read_only: true } + position_trajectory: { addr: 140, len: 4, unit: "1 [pulse]", read_only: true } + present_input_voltage: { addr: 144, len: 2, unit: "0.1 [V]", read_only: true } + present_temperature: { addr: 146, len: 1, unit: "1 [°C]", read_only: true } + backup_ready: { addr: 147, len: 1, min: 0, max: 1, read_only: true } +eeprom_area_end: 63 +operating_modes: + 0: "Current Control Mode" + 1: "Velocity Control Mode" + 3: "Position Control Mode (Default)" + 4: "Extended Position Control Mode" + 5: "Current-based Position Control Mode" + 6: "PWM Control Mode" diff --git a/requirements.txt b/requirements.txt index 5ac85ad5..5058c5ef 100644 --- a/requirements.txt +++ b/requirements.txt @@ -7,6 +7,7 @@ PyQt6 pyquaternion pyrealsense2 pure-python-adb +PyYAML quaternion tyro ur-rtde From 9ed96850b45b820d928dab07339d7ff16cd38752 Mon Sep 17 00:00:00 2001 From: Peter Wern Date: Fri, 18 Jul 2025 14:19:36 +0200 Subject: [PATCH 03/17] add: configurable virtual springs and dampers using Dynamixel internal current-based position mode --- ros2/.devcontainer/Dockerfile | 13 ++ ros2/README.md | 73 +++++--- .../config/example_fr3_config.yaml | 10 +- .../gello_publisher.py | 171 ++++++++++++++---- .../launch/main.launch.py | 7 +- 5 files changed, 212 insertions(+), 62 deletions(-) diff --git a/ros2/.devcontainer/Dockerfile b/ros2/.devcontainer/Dockerfile index ff72ab16..12ab1b78 100644 --- a/ros2/.devcontainer/Dockerfile +++ b/ros2/.devcontainer/Dockerfile @@ -61,4 +61,17 @@ RUN /bin/bash -c "source /opt/ros/humble/setup.bash && \ colcon build --install-base /opt/ros/humble/franka --cmake-args -DCMAKE_BUILD_TYPE=Release && \ cd .. && \ rm -rf tmp_build && \ + rm -rf /var/lib/apt/lists/*" + +# Install rqt reconfigure from source (for array parameters) +RUN /bin/bash -c "source /opt/ros/humble/setup.bash && \ + mkdir -p /tmp/rqt_reconfigure && cd /tmp && \ + git clone https://github.com/ros-visualization/rqt_reconfigure.git --branch humble && \ + cd rqt_reconfigure && \ + apt-get update && \ + rosdep update && \ + rosdep install --from-paths . --ignore-src -r -y && \ + colcon build --install-base /opt/ros/humble/franka --cmake-args -DCMAKE_BUILD_TYPE=Release && \ + cd .. && \ + rm -rf /tmp/rqt_reconfigure \ rm -rf /var/lib/apt/lists/*" \ No newline at end of file diff --git a/ros2/README.md b/ros2/README.md index 96dd10e7..2914bdf5 100644 --- a/ros2/README.md +++ b/ros2/README.md @@ -2,7 +2,7 @@ This folder contains all required ROS 2 packages for using GELLO. -## Packages +## Packages Overview ### 1. `franka_fr3_arm_controllers` This package provides a Joint Impedance controller for the Franka FR3. It subscribes to the GELLO joint states and sends torque commands to the robot. @@ -20,6 +20,7 @@ This package provides a ROS 2 node that reads input from the GELLO and publishes #### Key Features: - Publishes GELLO state to the `/gello/joint_states` topic. +- Optionally sets the internal control parameters of the Dynamixel motors. This allows for [virtual springs and dampers](#virtual-springs-dampers) in the GELLO joints. #### Launch Files: - **`main.launch.py`**: Launches the GELLO publisher node. @@ -35,17 +36,19 @@ This package provides a ROS 2 node for managing the gripper connected to the Fra - **`franka_gripper_client.launch.py`**: Launches the gripper manager node for the `Franka Hand`. - **`robotiq_gripper_controller_client.launch.py`**: Launches the gripper manager node for the `Robotiq 2F-85`. -## VS-Code Dev-Container +## Setup Environment + +### Option 1: VS-Code Dev-Container (recommended) We recommend working inside the provided VS-Code Dev-Container for a seamless development experience. Dev-Containers allow you to use a consistent environment with all necessary dependencies pre-installed. To start the Dev-Container, open the `ros2` sub-folder of this repository (not the entire `gello_software` folder) in VS Code. If prompted, select **"Reopen in Container"** to launch the workspace inside the Dev-Container. If you are not prompted, open the Command Palette (`Ctrl+Shift+P`) and select **"Dev Containers: Reopen in Container"**. Building the container for the first time will take a few minutes. For more information, refer to the [VS-Code Dev-Containers documentation](https://code.visualstudio.com/docs/devcontainers/containers). -If you choose not to use the Dev-Container, please refer to the [Local Setup](#local-setup) section below for manual installation instructions. +If you choose not to use the Dev-Container, please refer to the [Local Setup](#option-2-local-setup) section below for manual installation instructions. -## Local Setup +### Option 2: Local Setup -### Prerequisites +#### Prerequisites - **ROS 2 Humble Desktop** must be installed. See the [official installation guide](https://docs.ros.org/en/humble/Installation.html) for instructions. @@ -57,7 +60,7 @@ If you choose not to use the Dev-Container, please refer to the [Local Setup](#l > 💡 **Hint:** > You can also find example installation commands for `libfranka`, `franka_ros2`, and `ros2_robotiq_gripper` in the [Dockerfile](./.devcontainer/Dockerfile) located in the `ros2/.devcontainer` directory. These commands can be copy-pasted for your local setup. -### Further Dependency Installations +#### Further Dependency Installations After installing the prerequisites, you may need to install additional dependencies required by this workspace. For this you can run the `install_workspace_dependencies.bash` script. @@ -65,7 +68,10 @@ If you add new dependencies to your packages, remember to update the relevant `r ## Build and Test -### Building the Project +> ⚠️ **Important:** +> All commands for building and testing must be executed from the `ros2` directory of this repository. + +### Building the project To build the project, use the following `colcon` command with CMake arguments, required for clang-tidy: @@ -73,7 +79,7 @@ To build the project, use the following `colcon` command with CMake arguments, r colcon build --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=ON -DCHECK_TIDY=ON ``` -### Testing the Project +### Testing the project The packages come with a set of tests, which can be executed using the following command: @@ -81,9 +87,6 @@ The packages come with a set of tests, which can be executed using the following colcon test ``` -> ⚠️ **Important:** -> All commands for building and testing must be executed from the `ros2` directory of this repository. - ## Getting Started ### 1. **Run the GELLO Publisher** @@ -105,20 +108,14 @@ In this case, the `GELLO_USB_ID` would be `/dev/serial/by-id/usb-FTDI_USB__-__Se #### 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. Use the provided script to configure the GELLO for Franka FR3: +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). -```bash -python3 scripts/gello_get_offset.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/ -``` +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. -To apply your configuration: -- Update the `/workspace/src/franka_gello_state_publisher/config/gello_config.yaml` file. -- Rebuild the project to ensure the updated configuration is applied: +Rebuild the project to ensure the updated configuration is applied: ```bash +cd /workspace/ros2 colcon build ``` @@ -140,13 +137,39 @@ The `config_file` argument is **optional**. If not provided, it defaults to `exa - `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 +- Dynamixel control parameters: `dynamixel_...` (see below) + +**Virtual Springs and Dampers:** + +Each Dynamixel motor has an internal PID controller that can be configured to behave like a virtual spring and damper. Damping is useful to prevent the operator from moving the GELLO arm faster than the real robot can follow. The spring-like behavior allows to support individual joints that could sag to an unwanted position when having 7 degrees of freedom. + +This is done by setting the following parameters in the configuration file. Each parameter is an array of integers, where each value corresponds to a GELLO joint in ascending order. + + - `dynamixel_torque_enable`: Enable (1) / disable (0) torque of a joint. Must be enabled for the other control parameters to take effect. + - `dynamixel_goal_position`: Goal positions in pulses. 4095 = 360 degrees. Choose values that correspond to the desired joint angles. + - `dynamixel_kp_p`: Proportional gains. Determines the rotary spring-like behavior. Sensible values: 0 to ~1000. + - `dynamixel_kp_i`: Integral gains. Recommended to be 0 for all joints. + - `dynamixel_kd_d`: Derivative gains. Determines the damping behavior. Sensible values: 0 to ~1000. + +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. + +> 💡 **Hint:** +> The `example_fr3_config.yaml` file gives a good starting point for these values for a tabletop mounted GELLO: +> - springs in joints 1, 2, 4 and gripper. +> - damping in all joints, stronger for lower joints +> - `goal_position` values correspond to the calibration pose. +> +> You can also adjust these ROS2 parameters during runtime, e.g. using rqt with the `rqt_reconfigure` plugin. + +> 💡 **How strong are the virtual springs?** +> According to the Dynamixel XL330-M288 datasheet, the stall torque of the motors is 0.52 Nm (at 5.0 V, 1.47 A). However, continuous operation at this current will quickly overload and overheat the motors. Through testing, a current limit of 600 mA was found to be both safe and sufficient for our use case, corresponding to a maximum stall torque of approximately 0.21 Nm. Setting `dynamixel_kp_p` to 280 yields a spring constant of approximately 0.25 Nm/rad, which matches the physical torsion spring (McMaster-Carr 9271K53) used in the GELLO FR3 assembly — up to a deflection of about 48° where the current limit is reached. ### 2. **Launch the Joint Impedance Controller** Create a configuration file in `src/franka_fr3_arm_controllers/config/` or modify one of the provided example configuration files. Then launch the controller to send torque commands to the Franka robot: ```bash - ros2 launch franka_fr3_arm_controllers franka_fr3_arm_controllers.launch.py [robot_config_file:=your_config.yaml] +ros2 launch franka_fr3_arm_controllers franka_fr3_arm_controllers.launch.py [robot_config_file:=your_config.yaml] ``` The `robot_config_file` argument is **optional**. If not provided, it defaults to `example_fr3_config.yaml` in the `franka_fr3_arm_controllers/config/` directory. @@ -159,9 +182,9 @@ The `robot_config_file` argument is **optional**. If not provided, it defaults t Create a configuration file in `src/franka_gripper_manager/config/` or modify one of the provided example configuration files. Then launch the gripper manager node to control the gripper: - **For the Franka Hand**: - ```bash - ros2 launch franka_gripper_manager franka_gripper_client.launch.py [config_file:=your_config.yaml] - ``` + ```bash + ros2 launch franka_gripper_manager franka_gripper_client.launch.py [config_file:=your_config.yaml] + ``` - **For the Robotiq 2F-85**: ```bash diff --git a/ros2/src/franka_gello_state_publisher/config/example_fr3_config.yaml b/ros2/src/franka_gello_state_publisher/config/example_fr3_config.yaml index 6ea596fc..1b6a9b36 100644 --- a/ros2/src/franka_gello_state_publisher/config/example_fr3_config.yaml +++ b/ros2/src/franka_gello_state_publisher/config/example_fr3_config.yaml @@ -1,8 +1,14 @@ robot1: com_port: "usb-FTDI_USB__-__Serial_Converter_FT9MFR6C-if00-port0" namespace: "" - num_joints: 7 + 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] - gripper_range_rad: [2.77856, 3.50931] \ No newline at end of file + 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: enable, 0: disable + dynamixel_goal_position: [2048, 2048, 0, 1024, 2048, 1024, 2048, 2400] # pulses + 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] \ No newline at end of file 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 85620ba9..8b6337fe 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 @@ -6,37 +6,53 @@ from sensor_msgs.msg import JointState from std_msgs.msg import Float32 from ament_index_python.packages import get_package_prefix +from rcl_interfaces.msg import ParameterEvent, ParameterDescriptor class GelloPublisher(Node): def __init__(self): super().__init__("gello_publisher") + read_only_desc = ParameterDescriptor(read_only=True) default_com_port = self.determine_default_com_port() - self.declare_parameter("com_port", default_com_port) - self.declare_parameter("gello_name", default_com_port) - self.declare_parameter("num_joints", 7) - self.declare_parameter("joint_signs", [1] * 7) - self.declare_parameter("gripper", True) - self.declare_parameter("gripper_range_rad", (0.0, 0.0)) - self.declare_parameter("best_offsets", [0.0] * 7) - self.com_port = self.get_parameter("com_port").get_parameter_value().string_value - self.gello_name = self.get_parameter("gello_name").get_parameter_value().string_value - self.num_robot_joints = ( - self.get_parameter("num_joints").get_parameter_value().integer_value + self.com_port = ( + self.declare_parameter("com_port", default_com_port, read_only_desc) + .get_parameter_value() + .string_value + ) + self.gello_name = ( + self.declare_parameter("gello_name", default_com_port, read_only_desc) + .get_parameter_value() + .string_value + ) + self.num_arm_joints = ( + self.declare_parameter("num_arm_joints", 7, read_only_desc) + .get_parameter_value() + .integer_value ) self.joint_signs = ( - self.get_parameter("joint_signs").get_parameter_value().integer_array_value + self.declare_parameter("joint_signs", [1] * 7, read_only_desc) + .get_parameter_value() + .integer_array_value + ) + self.gripper = ( + self.declare_parameter("gripper", True, read_only_desc) + .get_parameter_value() + .bool_value ) - self.gripper = self.get_parameter("gripper").get_parameter_value().bool_value + self.num_total_joints = self.num_arm_joints + (1 if self.gripper else 0) self.gripper_range_rad = ( - self.get_parameter("gripper_range_rad").get_parameter_value().double_array_value + self.declare_parameter("gripper_range_rad", (0.0, 0.0), read_only_desc) + .get_parameter_value() + .double_array_value ) self.best_offsets = ( - self.get_parameter("best_offsets").get_parameter_value().double_array_value + self.declare_parameter("best_offsets", [0.0] * 7, read_only_desc) + .get_parameter_value() + .double_array_value ) - self.initialize_gello_driver() + self.initialize_dynamixels() self.robot_joint_publisher = self.create_publisher(JointState, "gello/joint_states", 10) self.gripper_joint_publisher = self.create_publisher( @@ -55,6 +71,18 @@ def __init__(self): "fr3_joint7", ] + def parameter_event_callback(self, event: ParameterEvent): + for param in event.changed_parameters: + # Skip parameters that are not related to the Dynamixel control parameters + if not param.name.startswith("dynamixel_"): + continue + # Extract the data name and value + data_name = param.name.replace("dynamixel_", "") + value = list(param.value.integer_array_value) + # Write the parameter value to the Dynamixel driver + self.driver.write_value_by_name(data_name, value) + self.get_logger().info(f"Parameter {data_name} changed to {value}. Updated Dynamixels.") + def determine_default_com_port(self) -> str: matches = glob.glob("/dev/serial/by-id/usb-FTDI_USB__-__Serial_Converter*") if matches: @@ -64,38 +92,97 @@ def determine_default_com_port(self) -> str: self.get_logger().warn("No com_ports detected. Please specify the com_port manually.") return "INVALID_COM_PORT" - def initialize_gello_driver(self: str): - joint_ids = list(range(1, self.num_joints + 1)) + def initialize_dynamixels(self): + joint_ids = list(range(1, self.num_total_joints + 1)) self.add_dynamixel_driver_path() from gello.dynamixel.driver import DynamixelDriver - self.driver = DynamixelDriver(joint_ids, port=self.com_port, baudrate=57600) - """The driver for the Dynamixel motors.""" + + CURRENT_BASED_POSITION_MODE = 5 + CURRENT_LIMIT = 600 # mA + ordered_dynamixel_params = [ + { + "name": "operating_mode", # resets kp_p, kp_i, kp_d, goal_current, goal_position + "default": [CURRENT_BASED_POSITION_MODE] * self.num_total_joints, + }, + { + "name": "goal_current", + "default": [CURRENT_LIMIT] * self.num_total_joints, + }, + { + "name": "kp_p", + "default": [0] * self.num_total_joints, + "constraints": "Sensible values: 0 to ~1000", + "is_ros2_param": True, + }, + { + "name": "kp_i", + "default": [0] * self.num_total_joints, + "constraints": "Sensible values: 0 to ~1000", + "is_ros2_param": True, + }, + { + "name": "kp_d", + "default": [0] * self.num_total_joints, + "constraints": "Sensible values: 0 to ~1000", + "is_ros2_param": True, + }, + { + "name": "torque_enable", # resets goal_position + "default": [0] * self.num_total_joints, + "constraints": "0 (disabled), 1 (enabled)", + "is_ros2_param": True, + }, + { + "name": "goal_position", + "default": [0] * self.num_total_joints, + "constraints": "4095 corresponds to 360 degrees", + "is_ros2_param": True, + }, + ] + + for param in ordered_dynamixel_params: + param_value = param["default"] + if param.get("is_ros2_param", False): + desc = ParameterDescriptor(additional_constraints=param.get("constraints", "")) + param_value = ( + self.declare_parameter( + f"dynamixel_{param['name']}", param["default"], desc + ) + .get_parameter_value() + .integer_array_value + ) + self.driver.write_value_by_name(param["name"], list(param_value)) + self.get_logger().info("Sent initial control parameters to all Dynamixel motors.") + + # Subscribe to parameter events to allow dynamic updates of parameters + self.parameter_subscription = self.create_subscription( + ParameterEvent, + '/parameter_events', + self.parameter_event_callback, + 10 + ) def __post_init__(self): - assert len(self.joint_signs) == self.num_robot_joints + assert len(self.joint_signs) == self.num_arm_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_joints(self) -> int: - extra_joints = 1 if self.gripper else 0 - return self.num_robot_joints + extra_joints - def publish_joint_jog(self): - current_joints = self.driver.get_joints() - current_robot_joints = current_joints[: self.num_robot_joints] - current_joints_corrected = (current_robot_joints - self.best_offsets) * self.joint_signs + gello_joints = self.driver.get_joints() + + gello_robot_joints = gello_joints[:self.num_arm_joints] + gello_robot_joints_corrected = (gello_robot_joints - self.best_offsets) * self.joint_signs robot_joint_states = JointState() robot_joint_states.header.stamp = self.get_clock().now().to_msg() robot_joint_states.name = self.joint_names robot_joint_states.header.frame_id = "fr3_link0" - robot_joint_states.position = [float(joint) for joint in current_joints_corrected] + robot_joint_states.position = [float(joint) for joint in gello_robot_joints_corrected] gripper_joint_states = Float32() if self.gripper: - gripper_position = current_joints[-1] + gripper_position = gello_joints[-1] gripper_joint_states.data = self.gripper_readout_to_percent(gripper_position) else: gripper_joint_states.data = 0.0 @@ -114,13 +201,29 @@ def add_dynamixel_driver_path(self): ) sys.path.insert(0, gello_path) + def destroy_node(self) -> None: + """Override the destroy_node method to disable torque mode before shutting down.""" + if hasattr(self, 'driver') and self.driver: + self.driver.set_torque_mode(False) + super().destroy_node() + def main(args=None): rclpy.init(args=args) gello_publisher = GelloPublisher() - rclpy.spin(gello_publisher) - gello_publisher.destroy_node() - rclpy.shutdown() + try: + rclpy.spin(gello_publisher) + except KeyboardInterrupt: + pass # Handle Ctrl+C gracefully + finally: + # Always destroy the node to ensure torque is disabled + try: + gello_publisher.destroy_node() + except Exception: + pass # Node might already be destroyed + # Only shutdown if ROS2 is still running + if rclpy.ok(): + rclpy.shutdown() if __name__ == "__main__": 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 fd2e5624..b3c9d3a8 100755 --- a/ros2/src/franka_gello_state_publisher/launch/main.launch.py +++ b/ros2/src/franka_gello_state_publisher/launch/main.launch.py @@ -32,11 +32,16 @@ def generate_robot_nodes(context): parameters=[ {"com_port": "/dev/serial/by-id/" + config["com_port"]}, {"gello_name": item_name}, - {"num_joints": config["num_joints"]}, + {"num_arm_joints": config["num_arm_joints"]}, {"joint_signs": config["joint_signs"]}, {"gripper": config["gripper"]}, {"gripper_range_rad": config["gripper_range_rad"]}, {"best_offsets": config["best_offsets"]}, + {"dynamixel_kp_p": config["dynamixel_kp_p"]}, + {"dynamixel_kp_i": config["dynamixel_kp_i"]}, + {"dynamixel_kp_d": config["dynamixel_kp_d"]}, + {"dynamixel_goal_position": config["dynamixel_goal_position"]}, + {"dynamixel_torque_enable": config["dynamixel_torque_enable"]}, ], ) ) From cb5fc74661f0a2325b5fa8c82aeb1070db138841 Mon Sep 17 00:00:00 2001 From: Peter Wern Date: Fri, 8 Aug 2025 13:28:05 +0000 Subject: [PATCH 04/17] chg: configure goal position in rad instead pulses --- .../config/example_fr3_config.yaml | 10 +-- .../gello_publisher.py | 61 +++++++++++++------ 2 files changed, 46 insertions(+), 25 deletions(-) diff --git a/ros2/src/franka_gello_state_publisher/config/example_fr3_config.yaml b/ros2/src/franka_gello_state_publisher/config/example_fr3_config.yaml index 1b6a9b36..2b5e8abd 100644 --- a/ros2/src/franka_gello_state_publisher/config/example_fr3_config.yaml +++ b/ros2/src/franka_gello_state_publisher/config/example_fr3_config.yaml @@ -7,8 +7,8 @@ robot1: best_offsets: [3.142, 3.142, -3.142, 4.712, 3.142, 4.712, 1.571] 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: enable, 0: disable - dynamixel_goal_position: [2048, 2048, 0, 1024, 2048, 1024, 2048, 2400] # pulses - 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] \ No newline at end of file + dynamixel_torque_enable: [ 1, 1, 1, 1, 1, 1, 1, 1] # 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] \ No newline at end of file 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 8b6337fe..1a46f126 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 @@ -2,11 +2,12 @@ import sys import glob import rclpy +import numpy as np from rclpy.node import Node from sensor_msgs.msg import JointState from std_msgs.msg import Float32 from ament_index_python.packages import get_package_prefix -from rcl_interfaces.msg import ParameterEvent, ParameterDescriptor +from rcl_interfaces.msg import ParameterEvent, ParameterDescriptor, ParameterValue class GelloPublisher(Node): @@ -30,7 +31,7 @@ def __init__(self): .get_parameter_value() .integer_value ) - self.joint_signs = ( + self.joint_signs = np.array( self.declare_parameter("joint_signs", [1] * 7, read_only_desc) .get_parameter_value() .integer_array_value @@ -46,7 +47,7 @@ def __init__(self): .get_parameter_value() .double_array_value ) - self.best_offsets = ( + self.best_offsets = np.array( self.declare_parameter("best_offsets", [0.0] * 7, read_only_desc) .get_parameter_value() .double_array_value @@ -54,7 +55,7 @@ def __init__(self): self.initialize_dynamixels() - self.robot_joint_publisher = self.create_publisher(JointState, "gello/joint_states", 10) + self.arm_joint_publisher = self.create_publisher(JointState, "gello/joint_states", 10) self.gripper_joint_publisher = self.create_publisher( Float32, "gripper/gripper_client/target_gripper_width_percent", 10 ) @@ -78,7 +79,13 @@ def parameter_event_callback(self, event: ParameterEvent): continue # Extract the data name and value data_name = param.name.replace("dynamixel_", "") - value = list(param.value.integer_array_value) + def convert(x): return x + for entry in self.ordered_dynamixel_params: + if entry["name"] == data_name: + if "convert" in entry: + convert = entry["convert"] + break + value = list(convert(param.value)) # Write the parameter value to the Dynamixel driver self.driver.write_value_by_name(data_name, value) self.get_logger().info(f"Parameter {data_name} changed to {value}. Updated Dynamixels.") @@ -98,9 +105,18 @@ def initialize_dynamixels(self): from gello.dynamixel.driver import DynamixelDriver self.driver = DynamixelDriver(joint_ids, port=self.com_port, baudrate=57600) + def to_integer_array_value(param_value: ParameterValue) -> list[int]: + return list(param_value.integer_array_value) + + def goal_position_to_pulses(param_value: ParameterValue) -> list[int]: + goals = np.array(param_value.double_array_value) + 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] + CURRENT_BASED_POSITION_MODE = 5 CURRENT_LIMIT = 600 # mA - ordered_dynamixel_params = [ + self.ordered_dynamixel_params = [ { "name": "operating_mode", # resets kp_p, kp_i, kp_d, goal_current, goal_position "default": [CURRENT_BASED_POSITION_MODE] * self.num_total_joints, @@ -114,34 +130,39 @@ def initialize_dynamixels(self): "default": [0] * self.num_total_joints, "constraints": "Sensible values: 0 to ~1000", "is_ros2_param": True, + "convert": to_integer_array_value }, { "name": "kp_i", "default": [0] * self.num_total_joints, "constraints": "Sensible values: 0 to ~1000", "is_ros2_param": True, + "convert": to_integer_array_value }, { "name": "kp_d", "default": [0] * self.num_total_joints, "constraints": "Sensible values: 0 to ~1000", "is_ros2_param": True, + "convert": to_integer_array_value }, { "name": "torque_enable", # resets goal_position "default": [0] * self.num_total_joints, "constraints": "0 (disabled), 1 (enabled)", "is_ros2_param": True, + "convert": to_integer_array_value }, { "name": "goal_position", - "default": [0] * self.num_total_joints, + "default": [0.0] * self.num_total_joints, "constraints": "4095 corresponds to 360 degrees", "is_ros2_param": True, + "convert": goal_position_to_pulses, }, ] - for param in ordered_dynamixel_params: + for param in self.ordered_dynamixel_params: param_value = param["default"] if param.get("is_ros2_param", False): desc = ParameterDescriptor(additional_constraints=param.get("constraints", "")) @@ -150,9 +171,9 @@ def initialize_dynamixels(self): f"dynamixel_{param['name']}", param["default"], desc ) .get_parameter_value() - .integer_array_value ) - self.driver.write_value_by_name(param["name"], list(param_value)) + convert = param.get("convert", lambda x: x) + self.driver.write_value_by_name(param["name"], list(convert(param_value))) self.get_logger().info("Sent initial control parameters to all Dynamixel motors.") # Subscribe to parameter events to allow dynamic updates of parameters @@ -169,24 +190,24 @@ def __post_init__(self): assert j == -1 or j == 1, f"Joint idx: {idx} should be -1 or 1, but got {j}." def publish_joint_jog(self): - gello_joints = self.driver.get_joints() + gello_joints_raw = self.driver.get_joints() - gello_robot_joints = gello_joints[:self.num_arm_joints] - gello_robot_joints_corrected = (gello_robot_joints - self.best_offsets) * self.joint_signs + gello_arm_joints_raw = gello_joints_raw[:self.num_arm_joints] + gello_arm_joints = (gello_arm_joints_raw - self.best_offsets) * self.joint_signs - robot_joint_states = JointState() - robot_joint_states.header.stamp = self.get_clock().now().to_msg() - robot_joint_states.name = self.joint_names - robot_joint_states.header.frame_id = "fr3_link0" - robot_joint_states.position = [float(joint) for joint in gello_robot_joints_corrected] + arm_joint_states = JointState() + arm_joint_states.header.stamp = self.get_clock().now().to_msg() + arm_joint_states.name = self.joint_names + arm_joint_states.header.frame_id = "fr3_link0" + arm_joint_states.position = [float(joint) for joint in gello_arm_joints] gripper_joint_states = Float32() if self.gripper: - gripper_position = gello_joints[-1] + gripper_position = gello_joints_raw[-1] gripper_joint_states.data = self.gripper_readout_to_percent(gripper_position) else: gripper_joint_states.data = 0.0 - self.robot_joint_publisher.publish(robot_joint_states) + self.arm_joint_publisher.publish(arm_joint_states) self.gripper_joint_publisher.publish(gripper_joint_states) def gripper_readout_to_percent(self, gripper_position: float) -> float: From 5302a697cf0a2def851cf1ff4b2d38ba0c492ca2 Mon Sep 17 00:00:00 2001 From: Andreas Gassner Date: Tue, 12 Aug 2025 06:27:45 +0000 Subject: [PATCH 05/17] chg: refactor franka_gello_state_publisher into modular components - Split into hardware and config modules - Add graceful shutdown with signal handling - Fix dual-arm parameter isolation --- ros2/.devcontainer/docker-compose.yml | 6 + .../config/example_fr3_duo_config.yaml | 18 +- .../gello_hardware.py | 169 ++++++++++ .../gello_parameter_config.py | 140 +++++++++ .../gello_publisher.py | 294 ++++++------------ 5 files changed, 422 insertions(+), 205 deletions(-) create mode 100644 ros2/src/franka_gello_state_publisher/franka_gello_state_publisher/gello_hardware.py create mode 100644 ros2/src/franka_gello_state_publisher/franka_gello_state_publisher/gello_parameter_config.py diff --git a/ros2/.devcontainer/docker-compose.yml b/ros2/.devcontainer/docker-compose.yml index c9247bf8..beea6d00 100644 --- a/ros2/.devcontainer/docker-compose.yml +++ b/ros2/.devcontainer/docker-compose.yml @@ -7,4 +7,10 @@ services: volumes: - ../../:/workspace - /dev/serial/by-id:/dev/serial/by-id + - /tmp/.X11-unix:/tmp/.X11-unix:rw + - /tmp/.docker.xauth:/tmp/.docker.xauth:rw + environment: + DISPLAY: ${DISPLAY} + QT_X11_NO_MITSH: "1" + XAUTHORITY: /tmp/.docker.xauth \ No newline at end of file diff --git a/ros2/src/franka_gello_state_publisher/config/example_fr3_duo_config.yaml b/ros2/src/franka_gello_state_publisher/config/example_fr3_duo_config.yaml index dc421cdb..9019d408 100644 --- a/ros2/src/franka_gello_state_publisher/config/example_fr3_duo_config.yaml +++ b/ros2/src/franka_gello_state_publisher/config/example_fr3_duo_config.yaml @@ -1,17 +1,29 @@ LEFT: com_port: "usb-FTDI_USB__-__Serial_Converter_FT9MFSIO-if00-port0" namespace: "left" - num_joints: 7 + 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] 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 + 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-FTDI_USB__-__Serial_Converter_FT9MFR6C-if00-port0" namespace: "right" - num_joints: 7 + 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] - gripper_range_rad: [2.77856, 3.50931] \ No newline at end of file + 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 + 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] \ No newline at end of file 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 new file mode 100644 index 00000000..4a0cb07e --- /dev/null +++ b/ros2/src/franka_gello_state_publisher/franka_gello_state_publisher/gello_hardware.py @@ -0,0 +1,169 @@ +import numpy as np +from ament_index_python.packages import get_package_prefix +from os import path as os_path +from sys import path as sys_path +from dataclasses import dataclass, field +from typing import List, TypedDict, Iterator, Tuple + + +class GelloHardwareParams(TypedDict): + """Type-safe parameter dictionary for GelloHardware initialization.""" + + com_port: str + gello_name: str + num_arm_joints: int + joint_signs: List[int] + gripper: bool + gripper_range_rad: List[float] + best_offsets: List[float] + dynamixel_kp_p: List[int] + dynamixel_kp_i: List[int] + dynamixel_kp_d: List[int] + dynamixel_torque_enable: List[int] + dynamixel_goal_position: List[float] + + +@dataclass +class DynamixelControlConfig: + """Tracks current dynamixel parameter state.""" + + kp_p: List[int] = field(default_factory=list) + kp_i: List[int] = field(default_factory=list) + kp_d: List[int] = field(default_factory=list) + torque_enable: List[int] = field(default_factory=list) + goal_position: List[int] = field(default_factory=list) + goal_current: List[int] = field(default_factory=list) + operating_mode: List[int] = field(default_factory=list) + + # Define the order for parameter updates + _UPDATE_ORDER = [ + "operating_mode", # resets kp_p, kp_i, kp_d, goal_current, goal_position + "goal_current", + "kp_p", + "kp_i", + "kp_d", + "torque_enable", # resets goal_position + "goal_position", + ] + + def __contains__(self, param_name: str) -> bool: + """Check if parameter exists in this configuration.""" + return hasattr(self, param_name) + + def __iter__(self) -> Iterator[Tuple[str, List[int]]]: + """Iterate through parameters in correct update order.""" + for param_name in self._UPDATE_ORDER: + if hasattr(self, param_name): + yield param_name, getattr(self, param_name) + + def __getitem__(self, param_name: str) -> List[int]: + """Enable dictionary-style access for getting values.""" + if not hasattr(self, param_name): + raise KeyError(f"Parameter '{param_name}' not found") + return getattr(self, param_name) + + def __setitem__(self, param_name: str, value: List[int]) -> None: + """Enable dictionary-style access for setting values.""" + if not hasattr(self, param_name): + raise KeyError(f"Parameter '{param_name}' not found") + setattr(self, param_name, value) + + +class GelloHardware: + """Hardware interface for GELLO teleoperation device.""" + + def __init__( + self, + hardware_config: GelloHardwareParams, + ) -> None: + self._com_port = hardware_config["com_port"] + self._gello_name = hardware_config["gello_name"] + self._num_arm_joints = hardware_config["num_arm_joints"] + self._joint_signs = np.array(hardware_config["joint_signs"]) + 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._initialize_driver() + + OPERATING_MODE = 5 # CURRENT_BASED_POSITION_MODE + CURRENT_LIMIT = 600 # mA + self._dynamixel_control_config = DynamixelControlConfig( + kp_p=hardware_config["dynamixel_kp_p"].copy(), + kp_i=hardware_config["dynamixel_kp_i"].copy(), + kp_d=hardware_config["dynamixel_kp_d"].copy(), + torque_enable=hardware_config["dynamixel_torque_enable"].copy(), + 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, + ) + + self._initialize_parameters() + + def _add_dynamixel_driver_path(self) -> None: + """Add dynamixel driver to Python path.""" + gello_path = os_path.abspath( + os_path.join(get_package_prefix("franka_gello_state_publisher"), "../../../") + ) + sys_path.insert(0, gello_path) + + def _initialize_driver(self) -> None: + """Initialize dynamixel driver with joint IDs and port.""" + joint_ids = list(range(1, self._num_total_joints + 1)) + self._add_dynamixel_driver_path() + from gello.dynamixel.driver import DynamixelDriver + + self._driver = DynamixelDriver(joint_ids, port=self._com_port, baudrate=57600) + + def _initialize_parameters(self) -> None: + """Write all dynamixel configuration parameters to hardware.""" + for param_name, param_value in self._dynamixel_control_config: + self._driver.write_value_by_name(param_name, param_value) + + def update_dynamixel_control_parameter( + self, param_name: str, param_value: list[float] | list[int] + ) -> None: + """Update a single dynamixel parameter and handle dependencies.""" + clean_name = param_name.replace("dynamixel_", "") + + if clean_name == "goal_position": + param_value = self._goal_position_to_pulses(param_value) + + self._dynamixel_control_config[clean_name] = param_value + self._driver.write_value_by_name(clean_name, self._dynamixel_control_config[clean_name]) + if clean_name == "torque_enable": + self._driver.write_value_by_name( + "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 + + gripper_position = 0.0 + if self._gripper: + gripper_position = self._gripper_readout_to_percent(gello_joints_raw[-1]) + + return gello_arm_joints, gripper_position + + def disable_torque(self) -> None: + """Disable torque on all joints.""" + self._driver.set_torque_mode(False) + + 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] + + 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] + ) + return max(0.0, min(1.0, gripper_position_percent)) 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 new file mode 100644 index 00000000..174ca8b9 --- /dev/null +++ b/ros2/src/franka_gello_state_publisher/franka_gello_state_publisher/gello_parameter_config.py @@ -0,0 +1,140 @@ +from dataclasses import dataclass +from rcl_interfaces.msg import ParameterDescriptor, ParameterType +from typing import Any, Iterator + + +@dataclass +class ParameterConfig: + descriptor: ParameterDescriptor + default: Any + + +class GelloParameterConfig: + """Configuration class for GELLO ROS2 parameters.""" + + DEFAULT_NUM_JOINTS = 7 + DEFAULT_JOINT_SIGNS = [1] * DEFAULT_NUM_JOINTS + DEFAULT_BEST_OFFSETS = [0.0] * DEFAULT_NUM_JOINTS + DEFAULT_GRIPPER_RANGE_RAD = (0.0, 0.0) + DEFAULT_CONTROL_GAINS = [0] * DEFAULT_NUM_JOINTS + DEFAULT_GOAL_POSITION = [0.0] * DEFAULT_NUM_JOINTS + + def __init__(self, default_com_port: str): + self.hardware_params = [ + ParameterConfig( + ParameterDescriptor( + name="com_port", + type=ParameterType.PARAMETER_STRING, + description="USB serial port path", + read_only=True, + ), + default_com_port, + ), + ParameterConfig( + ParameterDescriptor( + name="gello_name", + type=ParameterType.PARAMETER_STRING, + description="GELLO device identifier", + read_only=True, + ), + default_com_port, + ), + ParameterConfig( + ParameterDescriptor( + name="num_arm_joints", + type=ParameterType.PARAMETER_INTEGER, + description="Number of arm joints", + read_only=True, + ), + self.DEFAULT_NUM_JOINTS, + ), + ParameterConfig( + ParameterDescriptor( + name="joint_signs", + type=ParameterType.PARAMETER_INTEGER_ARRAY, + description="Joint direction signs", + read_only=True, + ), + self.DEFAULT_JOINT_SIGNS, + ), + ParameterConfig( + ParameterDescriptor( + name="gripper", + type=ParameterType.PARAMETER_BOOL, + description="Enable gripper control", + read_only=True, + ), + True, + ), + ParameterConfig( + ParameterDescriptor( + name="gripper_range_rad", + type=ParameterType.PARAMETER_DOUBLE_ARRAY, + description="Gripper range in radians", + read_only=True, + ), + self.DEFAULT_GRIPPER_RANGE_RAD, + ), + ParameterConfig( + ParameterDescriptor( + name="best_offsets", + type=ParameterType.PARAMETER_DOUBLE_ARRAY, + description="Joint offset calibration", + read_only=True, + ), + self.DEFAULT_BEST_OFFSETS, + ), + ParameterConfig( + ParameterDescriptor( + name="dynamixel_kp_p", + type=ParameterType.PARAMETER_INTEGER_ARRAY, + description="Proportional gains", + additional_constraints="0 to ~1000", + read_only=False, + ), + self.DEFAULT_CONTROL_GAINS, + ), + ParameterConfig( + ParameterDescriptor( + name="dynamixel_kp_i", + type=ParameterType.PARAMETER_INTEGER_ARRAY, + description="Integral gains", + additional_constraints="0 to ~1000", + read_only=False, + ), + self.DEFAULT_CONTROL_GAINS, + ), + ParameterConfig( + ParameterDescriptor( + name="dynamixel_kp_d", + type=ParameterType.PARAMETER_INTEGER_ARRAY, + description="Derivative gains", + additional_constraints="0 to ~1000", + read_only=False, + ), + self.DEFAULT_CONTROL_GAINS, + ), + ParameterConfig( + ParameterDescriptor( + name="dynamixel_torque_enable", + type=ParameterType.PARAMETER_INTEGER_ARRAY, + description="GELLO Torque enabled", + additional_constraints="0 (disabled), 1 (enabled)", + read_only=False, + ), + [0] * self.DEFAULT_NUM_JOINTS, + ), + ParameterConfig( + ParameterDescriptor( + name="dynamixel_goal_position", + type=ParameterType.PARAMETER_INTEGER_ARRAY, + description="Goal positions", + read_only=False, + ), + self.DEFAULT_GOAL_POSITION, + ), + ] + + def __iter__(self) -> Iterator[ParameterConfig]: + """Return an iterator over the parameter configurations.""" + return iter(self.hardware_params) 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 1a46f126..e7da56eb 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 @@ -1,68 +1,60 @@ -import os -import sys -import glob import rclpy -import numpy as np +from glob import glob +from sys import exit +from signal import signal, SIGINT, SIGTERM from rclpy.node import Node from sensor_msgs.msg import JointState from std_msgs.msg import Float32 -from ament_index_python.packages import get_package_prefix -from rcl_interfaces.msg import ParameterEvent, ParameterDescriptor, ParameterValue +from rcl_interfaces.msg import ParameterEvent +from rclpy.parameter import parameter_value_to_python +from franka_gello_state_publisher.gello_hardware import GelloHardware, GelloHardwareParams +from franka_gello_state_publisher.gello_parameter_config import ( + ParameterConfig, + GelloParameterConfig, +) class GelloPublisher(Node): - def __init__(self): + """ROS2 node for publishing GELLO device joint states and handling parameter updates.""" + + def __init__(self) -> None: super().__init__("gello_publisher") + self.PUBLISHING_RATE = 25 # Hz - read_only_desc = ParameterDescriptor(read_only=True) - default_com_port = self.determine_default_com_port() - self.com_port = ( - self.declare_parameter("com_port", default_com_port, read_only_desc) - .get_parameter_value() - .string_value - ) - self.gello_name = ( - self.declare_parameter("gello_name", default_com_port, read_only_desc) - .get_parameter_value() - .string_value - ) - self.num_arm_joints = ( - self.declare_parameter("num_arm_joints", 7, read_only_desc) - .get_parameter_value() - .integer_value - ) - self.joint_signs = np.array( - self.declare_parameter("joint_signs", [1] * 7, read_only_desc) - .get_parameter_value() - .integer_array_value - ) - self.gripper = ( - self.declare_parameter("gripper", True, read_only_desc) - .get_parameter_value() - .bool_value - ) - self.num_total_joints = self.num_arm_joints + (1 if self.gripper else 0) - self.gripper_range_rad = ( - self.declare_parameter("gripper_range_rad", (0.0, 0.0), read_only_desc) - .get_parameter_value() - .double_array_value - ) - self.best_offsets = np.array( - self.declare_parameter("best_offsets", [0.0] * 7, read_only_desc) - .get_parameter_value() - .double_array_value - ) + hardware_params: GelloHardwareParams = self._setup_hardware_parameters() + + self.gello_hardware = GelloHardware(hardware_params) - self.initialize_dynamixels() + signal(SIGINT, self._signal_handler) # Handle Ctrl+C gracefully + signal(SIGTERM, self._signal_handler) # Handle termination signals self.arm_joint_publisher = self.create_publisher(JointState, "gello/joint_states", 10) self.gripper_joint_publisher = self.create_publisher( Float32, "gripper/gripper_client/target_gripper_width_percent", 10 ) - self.timer = self.create_timer(1 / 25, self.publish_joint_jog) + # Subscribe to parameter events to allow dynamic updates of parameters + self.parameter_subscription = self.create_subscription( + ParameterEvent, "/parameter_events", self.parameter_event_callback, 10 + ) + + self.timer = self.create_timer(1 / self.PUBLISHING_RATE, self.publish_joint_jog) + + def parameter_event_callback(self, event: ParameterEvent) -> None: + """Handle parameter change events for this node.""" + if event.node != self.get_fully_qualified_name(): + return - self.joint_names = [ + for param in event.changed_parameters: + # Skip parameters that are not related to the Dynamixel control parameters + if not param.name.startswith("dynamixel_"): + continue + param_value = parameter_value_to_python(param.value) + self.gello_hardware.update_dynamixel_control_parameter(param.name, param_value) + + def publish_joint_jog(self) -> None: + """Publish current joint states and gripper position.""" + JOINT_NAMES = [ "fr3_joint1", "fr3_joint2", "fr3_joint3", @@ -71,27 +63,27 @@ def __init__(self): "fr3_joint6", "fr3_joint7", ] + [gello_arm_joints, gripper_position] = self.gello_hardware.read_joint_states() - def parameter_event_callback(self, event: ParameterEvent): - for param in event.changed_parameters: - # Skip parameters that are not related to the Dynamixel control parameters - if not param.name.startswith("dynamixel_"): - continue - # Extract the data name and value - data_name = param.name.replace("dynamixel_", "") - def convert(x): return x - for entry in self.ordered_dynamixel_params: - if entry["name"] == data_name: - if "convert" in entry: - convert = entry["convert"] - break - value = list(convert(param.value)) - # Write the parameter value to the Dynamixel driver - self.driver.write_value_by_name(data_name, value) - self.get_logger().info(f"Parameter {data_name} changed to {value}. Updated Dynamixels.") - - def determine_default_com_port(self) -> str: - matches = glob.glob("/dev/serial/by-id/usb-FTDI_USB__-__Serial_Converter*") + arm_joint_states = JointState() + arm_joint_states.header.stamp = self.get_clock().now().to_msg() + arm_joint_states.name = JOINT_NAMES + arm_joint_states.header.frame_id = "fr3_link0" + arm_joint_states.position = gello_arm_joints.tolist() + + gripper_joint_states = Float32() + gripper_joint_states.data = gripper_position + self.arm_joint_publisher.publish(arm_joint_states) + self.gripper_joint_publisher.publish(gripper_joint_states) + + def destroy_node(self) -> None: + """Override the destroy_node method to disable torque mode before shutting down.""" + self.gello_hardware.disable_torque() + super().destroy_node() + + def _determine_default_com_port(self) -> str: + """Auto-detect GELLO device COM port or return invalid placeholder.""" + matches = glob("/dev/serial/by-id/usb-FTDI_USB__-__Serial_Converter*") if matches: self.get_logger().info(f"Auto-detected com_ports: {matches}") return matches[0] @@ -99,152 +91,50 @@ def determine_default_com_port(self) -> str: self.get_logger().warn("No com_ports detected. Please specify the com_port manually.") return "INVALID_COM_PORT" - def initialize_dynamixels(self): - joint_ids = list(range(1, self.num_total_joints + 1)) - self.add_dynamixel_driver_path() - from gello.dynamixel.driver import DynamixelDriver - self.driver = DynamixelDriver(joint_ids, port=self.com_port, baudrate=57600) - - def to_integer_array_value(param_value: ParameterValue) -> list[int]: - return list(param_value.integer_array_value) - - def goal_position_to_pulses(param_value: ParameterValue) -> list[int]: - goals = np.array(param_value.double_array_value) - 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] - - CURRENT_BASED_POSITION_MODE = 5 - CURRENT_LIMIT = 600 # mA - self.ordered_dynamixel_params = [ - { - "name": "operating_mode", # resets kp_p, kp_i, kp_d, goal_current, goal_position - "default": [CURRENT_BASED_POSITION_MODE] * self.num_total_joints, - }, - { - "name": "goal_current", - "default": [CURRENT_LIMIT] * self.num_total_joints, - }, - { - "name": "kp_p", - "default": [0] * self.num_total_joints, - "constraints": "Sensible values: 0 to ~1000", - "is_ros2_param": True, - "convert": to_integer_array_value - }, - { - "name": "kp_i", - "default": [0] * self.num_total_joints, - "constraints": "Sensible values: 0 to ~1000", - "is_ros2_param": True, - "convert": to_integer_array_value - }, - { - "name": "kp_d", - "default": [0] * self.num_total_joints, - "constraints": "Sensible values: 0 to ~1000", - "is_ros2_param": True, - "convert": to_integer_array_value - }, - { - "name": "torque_enable", # resets goal_position - "default": [0] * self.num_total_joints, - "constraints": "0 (disabled), 1 (enabled)", - "is_ros2_param": True, - "convert": to_integer_array_value - }, - { - "name": "goal_position", - "default": [0.0] * self.num_total_joints, - "constraints": "4095 corresponds to 360 degrees", - "is_ros2_param": True, - "convert": goal_position_to_pulses, - }, - ] + def _declare_ros2_param(self, param: ParameterConfig): + """Declare ROS2 parameters with less boilerplate.""" + parameter_value = self.declare_parameter( + param.descriptor.name, param.default, param.descriptor + ).get_parameter_value() - for param in self.ordered_dynamixel_params: - param_value = param["default"] - if param.get("is_ros2_param", False): - desc = ParameterDescriptor(additional_constraints=param.get("constraints", "")) - param_value = ( - self.declare_parameter( - f"dynamixel_{param['name']}", param["default"], desc - ) - .get_parameter_value() - ) - convert = param.get("convert", lambda x: x) - self.driver.write_value_by_name(param["name"], list(convert(param_value))) - self.get_logger().info("Sent initial control parameters to all Dynamixel motors.") + return parameter_value_to_python(parameter_value) - # Subscribe to parameter events to allow dynamic updates of parameters - self.parameter_subscription = self.create_subscription( - ParameterEvent, - '/parameter_events', - self.parameter_event_callback, - 10 - ) + def _setup_hardware_parameters(self): + """Declare and setup all hardware configuration parameters.""" + default_com_port = self._determine_default_com_port() + config = GelloParameterConfig(default_com_port) - def __post_init__(self): - assert len(self.joint_signs) == self.num_arm_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}." + hardware_params: GelloHardwareParams = {} + for param in config: + hardware_params[param.descriptor.name] = self._declare_ros2_param(param) - def publish_joint_jog(self): - 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 - - arm_joint_states = JointState() - arm_joint_states.header.stamp = self.get_clock().now().to_msg() - arm_joint_states.name = self.joint_names - arm_joint_states.header.frame_id = "fr3_link0" - arm_joint_states.position = [float(joint) for joint in gello_arm_joints] + return hardware_params - gripper_joint_states = Float32() - if self.gripper: - gripper_position = gello_joints_raw[-1] - gripper_joint_states.data = self.gripper_readout_to_percent(gripper_position) + def _signal_handler(self, signum, frame): + """Handle system signals for graceful shutdown.""" + if signum == SIGINT or signum == SIGTERM: + self.get_logger().info(f"Received signal {signum}, shutting down gracefully...") + self._safe_shutdown() + exit(0) else: - gripper_joint_states.data = 0.0 - self.arm_joint_publisher.publish(arm_joint_states) - self.gripper_joint_publisher.publish(gripper_joint_states) - - def gripper_readout_to_percent(self, gripper_position: float) -> float: - gripper_percent = (gripper_position - self.gripper_range_rad[0]) / ( - self.gripper_range_rad[1] - self.gripper_range_rad[0] - ) - return max(0.0, min(1.0, gripper_percent)) + self.get_logger().warn(f"Received unexpected signal {signum}") - def add_dynamixel_driver_path(self): - gello_path = os.path.abspath( - os.path.join(get_package_prefix("franka_gello_state_publisher"), "../../../") - ) - sys.path.insert(0, gello_path) - - def destroy_node(self) -> None: - """Override the destroy_node method to disable torque mode before shutting down.""" - if hasattr(self, 'driver') and self.driver: - self.driver.set_torque_mode(False) - super().destroy_node() + def _safe_shutdown(self): + """Safely shutdown hardware.""" + try: + self.get_logger().info("Disabling GELLO torque...") + self.gello_hardware.disable_torque() + self.get_logger().info("GELLO torque disabled successfully") + except Exception as e: + self.get_logger().error(f"Error disabling torque: {e}") def main(args=None): rclpy.init(args=args) gello_publisher = GelloPublisher() - try: - rclpy.spin(gello_publisher) - except KeyboardInterrupt: - pass # Handle Ctrl+C gracefully - finally: - # Always destroy the node to ensure torque is disabled - try: - gello_publisher.destroy_node() - except Exception: - pass # Node might already be destroyed - # Only shutdown if ROS2 is still running - if rclpy.ok(): - rclpy.shutdown() + rclpy.spin(gello_publisher) + gello_publisher.destroy_node() + rclpy.shutdown() if __name__ == "__main__": From f9909f2aca19124038ceb414a1b249005f851e1b Mon Sep 17 00:00:00 2001 From: Peter Wern Date: Fri, 12 Sep 2025 19:31:12 +0200 Subject: [PATCH 06/17] FIX: wording --- .../franka_gello_state_publisher/gello_publisher.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 e7da56eb..477949c9 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 @@ -92,7 +92,7 @@ def _determine_default_com_port(self) -> str: return "INVALID_COM_PORT" def _declare_ros2_param(self, param: ParameterConfig): - """Declare ROS2 parameters with less boilerplate.""" + """Declare ROS2 parameters""" parameter_value = self.declare_parameter( param.descriptor.name, param.default, param.descriptor ).get_parameter_value() From b1b656426b8667cb942c4748b5d655c7be8279f6 Mon Sep 17 00:00:00 2001 From: Christian Czapka Date: Thu, 2 Oct 2025 17:19:17 +0200 Subject: [PATCH 07/17] ADD: Instructions to improve USB latency --- ros2/README.md | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/ros2/README.md b/ros2/README.md index 2914bdf5..a3b51c50 100644 --- a/ros2/README.md +++ b/ros2/README.md @@ -211,6 +211,23 @@ The open com port could not be opened. Possible reasons are: The libfranka version and robot system version are not compatible. More information can be found [here](https://frankarobotics.github.io/docs/compatibility.html). Fix this by correcting the `LIBFRANKA_VERSION=0.15.0` in the [Dockerfile](./.devcontainer/Dockerfile) and update the `FRANKA_ROS2_VERSION` and `FRANKA_DESCRIPTION_VERSION` accordingly. +### The movement of the follower robot is slightly jerky + +If the movements of the follower robot do not feel smooth or you experience frequent force threshold errors, this may be related to high USB latency on your machine. To fix this, try the following (non-permanent) fix **on your host PC**: + +1. Check which `ttyUSBx`/`ttyACMx` is mapped to your U2D2 or OpenRB-150 device: `ls -la /dev/serial/by-id/` +2. Reduce the USB latency from the default 16ms to 1ms: `echo 1 | sudo tee /sys/bus/usb-serial/devices/ttyUSB0/latency_timer` (replace `ttyUSB0` with your actual device) + +If this helps, you can add a permanent udev rule: +1. Create a new file `/etc/udev/rules.d/99-gello.rules` **on your host PC**: + ``` + # Lower latency_timer (1 instead of default 16 ms) & permission fix for U2D2 and OpenRB-150 devices + ACTION=="add", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6014" MODE="0666", ATTR{device/latency_timer}="1" + ACTION=="add", ATTRS{idVendor}=="2f5d", ATTRS{idProduct}=="2202" MODE="0666", ATTR{device/latency_timer}="1" + ``` + > Note: This also sets the device permissions (`MODE="0666"`), this part is optional. +2. Reload and trigger the new rules: `sudo udevadm control --reload-rules && sudo udevadm trigger` +3. Unplug and replug your U2D2 or OpenRB-150 devices ## Acknowledgements The source code for the Robotiq gripper control is based on From 8ce45f7d3fc697ee029e8bb3c43fea8e5f38465e Mon Sep 17 00:00:00 2001 From: Tobias Peter Date: Tue, 14 Oct 2025 12:12:23 +0000 Subject: [PATCH 08/17] no longer block during on_activate() which had the side effect of incorrect joint data being send from the robot --- .../joint_impedance_controller.hpp | 3 ++- .../src/joint_impedance_controller.cpp | 27 ++++++++++++++----- 2 files changed, 23 insertions(+), 7 deletions(-) diff --git a/ros2/src/franka_fr3_arm_controllers/include/franka_fr3_arm_controllers/joint_impedance_controller.hpp b/ros2/src/franka_fr3_arm_controllers/include/franka_fr3_arm_controllers/joint_impedance_controller.hpp index 093867db..b286a66c 100644 --- a/ros2/src/franka_fr3_arm_controllers/include/franka_fr3_arm_controllers/joint_impedance_controller.hpp +++ b/ros2/src/franka_fr3_arm_controllers/include/franka_fr3_arm_controllers/joint_impedance_controller.hpp @@ -53,6 +53,7 @@ class JointImpedanceController : public controller_interface::ControllerInterfac Vector7d d_gains_; double k_alpha_; bool move_to_start_position_finished_{false}; + bool motion_generator_initialized_{false}; rclcpp::Time start_time_; std::unique_ptr motion_generator_; rclcpp::Subscription::SharedPtr joint_state_subscriber_ = nullptr; @@ -62,7 +63,7 @@ class JointImpedanceController : public controller_interface::ControllerInterfac Vector7d calculateTauDGains_(const Vector7d& q_goal); bool validateGains_(const std::vector& gains, const std::string& gains_name); - void initializeMotionGenerator_(); + bool initializeMotionGenerator_(); void updateJointStates_(); void validateGelloPositions_(const sensor_msgs::msg::JointState& msg); void jointStateCallback_(const sensor_msgs::msg::JointState msg); diff --git a/ros2/src/franka_fr3_arm_controllers/src/joint_impedance_controller.cpp b/ros2/src/franka_fr3_arm_controllers/src/joint_impedance_controller.cpp index 70cac900..fc922521 100644 --- a/ros2/src/franka_fr3_arm_controllers/src/joint_impedance_controller.cpp +++ b/ros2/src/franka_fr3_arm_controllers/src/joint_impedance_controller.cpp @@ -55,7 +55,20 @@ controller_interface::return_type JointImpedanceController::update( Vector7d q_goal; Vector7d tau_d_calculated; + if (!motion_generator_initialized_) { + // After starting the controller we wait for valid joint states from the input topic + // Until we get valid joint states we will send zero torques to the robot to allow the user to reposition the robot + motion_generator_initialized_ = initializeMotionGenerator_(); + for (int i = 0; i < num_joints; ++i) { + command_interfaces_[i].set_value(0.0); + } + + return controller_interface::return_type::OK; + } + if (!move_to_start_position_finished_) { + // We have received valid joint states and initialized the motion generator + // Now we move smoothly to the first joint position received from the input topic auto trajectory_time = this->get_node()->now() - start_time_; auto motion_generator_output = motion_generator_->getDesiredJointPositions(trajectory_time); move_to_start_position_finished_ = motion_generator_output.second; @@ -64,6 +77,8 @@ controller_interface::return_type JointImpedanceController::update( } if (move_to_start_position_finished_) { + // After reaching the start position we follow the joint position from the input topic + // This is the normal operation mode of the controller if (!gello_position_values_valid_) { RCLCPP_FATAL(get_node()->get_logger(), "Timeout: No valid joint states received from Gello"); rclcpp::shutdown(); // Exit the node permanently @@ -165,7 +180,6 @@ CallbackReturn JointImpedanceController::on_configure( CallbackReturn JointImpedanceController::on_activate( const rclcpp_lifecycle::State& /*previous_state*/) { - initializeMotionGenerator_(); dq_filtered_.setZero(); start_time_ = this->get_node()->now(); @@ -225,12 +239,12 @@ void JointImpedanceController::updateJointStates_() { } } -void JointImpedanceController::initializeMotionGenerator_() { +bool JointImpedanceController::initializeMotionGenerator_() { last_joint_state_time_ = get_node()->now(); - while (!gello_position_values_valid_) { - RCLCPP_WARN(get_node()->get_logger(), "Waiting for valid joint states..."); - std::this_thread::sleep_for(std::chrono::milliseconds(1000)); - last_joint_state_time_ = get_node()->now(); + if (!gello_position_values_valid_) { + // Only send a warning once to not spam the log + RCLCPP_WARN_ONCE(get_node()->get_logger(), "Waiting for valid joint states..."); + return false; } Vector7d q_goal; @@ -243,6 +257,7 @@ void JointImpedanceController::initializeMotionGenerator_() { const double motion_generator_speed_factor = 0.2; motion_generator_ = std::make_unique(motion_generator_speed_factor, q_, q_goal); + return true; } } // namespace franka_fr3_arm_controllers From 428a8dd7474683432f0fd5b15988b9935c46b02b Mon Sep 17 00:00:00 2001 From: Tobias Peter Date: Mon, 20 Oct 2025 15:38:30 +0000 Subject: [PATCH 09/17] make tests work --- .../src/joint_impedance_controller.cpp | 22 +++++++++++-------- .../test/setup_test.cpp | 12 +++++++--- .../test/test_pep257.py | 2 +- 3 files changed, 23 insertions(+), 13 deletions(-) diff --git a/ros2/src/franka_fr3_arm_controllers/src/joint_impedance_controller.cpp b/ros2/src/franka_fr3_arm_controllers/src/joint_impedance_controller.cpp index fc922521..b18e8886 100644 --- a/ros2/src/franka_fr3_arm_controllers/src/joint_impedance_controller.cpp +++ b/ros2/src/franka_fr3_arm_controllers/src/joint_impedance_controller.cpp @@ -57,13 +57,17 @@ controller_interface::return_type JointImpedanceController::update( if (!motion_generator_initialized_) { // After starting the controller we wait for valid joint states from the input topic - // Until we get valid joint states we will send zero torques to the robot to allow the user to reposition the robot + // Until we get valid joint states we will send zero torques to the robot + // to allow the user to reposition the robot motion_generator_initialized_ = initializeMotionGenerator_(); - for (int i = 0; i < num_joints; ++i) { - command_interfaces_[i].set_value(0.0); - } - return controller_interface::return_type::OK; + if (!motion_generator_initialized_) { + for (int i = 0; i < num_joints; ++i) { + command_interfaces_[i].set_value(0.0); + } + + return controller_interface::return_type::OK; + } } if (!move_to_start_position_finished_) { @@ -107,6 +111,7 @@ void JointImpedanceController::jointStateCallback_(const sensor_msgs::msg::Joint "Received joint state size is smaller than expected size."); return; } + std::copy(msg.position.begin(), msg.position.begin() + gello_position_values_.size(), gello_position_values_.begin()); @@ -180,7 +185,7 @@ CallbackReturn JointImpedanceController::on_configure( CallbackReturn JointImpedanceController::on_activate( const rclcpp_lifecycle::State& /*previous_state*/) { - + last_joint_state_time_ = get_node()->now(); dq_filtered_.setZero(); start_time_ = this->get_node()->now(); @@ -240,10 +245,9 @@ void JointImpedanceController::updateJointStates_() { } bool JointImpedanceController::initializeMotionGenerator_() { - last_joint_state_time_ = get_node()->now(); if (!gello_position_values_valid_) { - // Only send a warning once to not spam the log - RCLCPP_WARN_ONCE(get_node()->get_logger(), "Waiting for valid joint states..."); + // Only send a warning once every 10 secondsto not spam the log + RCLCPP_WARN_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 10 * 1000, "Waiting for valid joint states..."); return false; } diff --git a/ros2/src/franka_fr3_arm_controllers/test/setup_test.cpp b/ros2/src/franka_fr3_arm_controllers/test/setup_test.cpp index 9f8d1001..e79897ad 100644 --- a/ros2/src/franka_fr3_arm_controllers/test/setup_test.cpp +++ b/ros2/src/franka_fr3_arm_controllers/test/setup_test.cpp @@ -32,11 +32,13 @@ TEST_F(JointImpedanceControllerTest, TestActivate) { TEST_F(JointImpedanceControllerTest, TestUpdateMotionGeneratorOnly) { static const std::vector kInitialRobotPosition = {0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1}; static constexpr double kExpectedValue = -0.05; - static constexpr double kTolerance = 1e-6; + static constexpr double kTolerance = 0.01; setRobotPosition(kInitialRobotPosition); startController(); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + rclcpp::Time time; rclcpp::Duration period = rclcpp::Duration::from_seconds(1.0); EXPECT_EQ(controller_->update(time, period), controller_interface::return_type::OK); @@ -49,11 +51,13 @@ TEST_F(JointImpedanceControllerTest, TestUpdateMotionGeneratorOnly) { TEST_F(JointImpedanceControllerTest, TestUpdateMotionGeneratorAndGelloPositionValues) { static const std::vector kInitialRobotPosition = {1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0}; static constexpr double kExpectedValue = -0.05; - static constexpr double kTolerance = 1e-6; + static constexpr double kTolerance = 0.01; setRobotPosition(kInitialRobotPosition); startController(); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + rclcpp::Time time; rclcpp::Duration period = rclcpp::Duration::from_seconds(1.0); EXPECT_EQ(controller_->update(time, period), controller_interface::return_type::OK); @@ -66,11 +70,13 @@ TEST_F(JointImpedanceControllerTest, TestUpdateMotionGeneratorAndGelloPositionVa TEST_F(JointImpedanceControllerTest, TestUpdateGelloPositionValuesOnly) { static const std::vector kInitialRobotPosition = {1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0}; static constexpr double kExpectedValue = -0.075; - static constexpr double kTolerance = 1e-6; + static constexpr double kTolerance = 0.01; setRobotPosition(kInitialRobotPosition); startController(); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + rclcpp::Time time; rclcpp::Duration period = rclcpp::Duration::from_seconds(1.0); EXPECT_EQ(controller_->update(time, period), controller_interface::return_type::OK); diff --git a/ros2/src/franka_gello_state_publisher/test/test_pep257.py b/ros2/src/franka_gello_state_publisher/test/test_pep257.py index 647b3c33..c61adbc6 100644 --- a/ros2/src/franka_gello_state_publisher/test/test_pep257.py +++ b/ros2/src/franka_gello_state_publisher/test/test_pep257.py @@ -20,6 +20,6 @@ @pytest.mark.linter @pytest.mark.pep257 def test_pep257(): - excluded_file = Path(__file__).resolve().parents[1] / "franka_gello_state_publisher/driver.py" + excluded_file = Path(__file__).resolve().parents[1] / "franka_gello_state_publisher/gello_publisher.py" rc = main(argv=[".", "test", "--exclude", str(excluded_file)]) assert rc == 0, "Found code style errors / warnings" From d58470887e48e8820516441e785a5af28c5972e2 Mon Sep 17 00:00:00 2001 From: Andreas Gassner Date: Tue, 21 Oct 2025 09:33:31 +0000 Subject: [PATCH 10/17] fix: franka ros2 unittests --- .../src/joint_impedance_controller.cpp | 3 ++- .../franka_gello_state_publisher/gello_publisher.py | 2 +- ros2/src/franka_gello_state_publisher/test/test_pep257.py | 4 +--- 3 files changed, 4 insertions(+), 5 deletions(-) diff --git a/ros2/src/franka_fr3_arm_controllers/src/joint_impedance_controller.cpp b/ros2/src/franka_fr3_arm_controllers/src/joint_impedance_controller.cpp index b18e8886..2fd49adc 100644 --- a/ros2/src/franka_fr3_arm_controllers/src/joint_impedance_controller.cpp +++ b/ros2/src/franka_fr3_arm_controllers/src/joint_impedance_controller.cpp @@ -247,7 +247,8 @@ void JointImpedanceController::updateJointStates_() { bool JointImpedanceController::initializeMotionGenerator_() { if (!gello_position_values_valid_) { // Only send a warning once every 10 secondsto not spam the log - RCLCPP_WARN_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 10 * 1000, "Waiting for valid joint states..."); + RCLCPP_WARN_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 10 * 1000, + "Waiting for valid joint states..."); return false; } 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 477949c9..15793a76 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 @@ -92,7 +92,7 @@ def _determine_default_com_port(self) -> str: return "INVALID_COM_PORT" def _declare_ros2_param(self, param: ParameterConfig): - """Declare ROS2 parameters""" + """Declare ROS2 parameters.""" parameter_value = self.declare_parameter( param.descriptor.name, param.default, param.descriptor ).get_parameter_value() diff --git a/ros2/src/franka_gello_state_publisher/test/test_pep257.py b/ros2/src/franka_gello_state_publisher/test/test_pep257.py index c61adbc6..a2c3deb8 100644 --- a/ros2/src/franka_gello_state_publisher/test/test_pep257.py +++ b/ros2/src/franka_gello_state_publisher/test/test_pep257.py @@ -14,12 +14,10 @@ from ament_pep257.main import main import pytest -from pathlib import Path @pytest.mark.linter @pytest.mark.pep257 def test_pep257(): - excluded_file = Path(__file__).resolve().parents[1] / "franka_gello_state_publisher/gello_publisher.py" - rc = main(argv=[".", "test", "--exclude", str(excluded_file)]) + rc = main(argv=[".", "test"]) assert rc == 0, "Found code style errors / warnings" From 526f5bac0720de0a25e25f00623fd010e33c23f4 Mon Sep 17 00:00:00 2001 From: Peter Wern Date: Tue, 11 Nov 2025 14:43:23 +0100 Subject: [PATCH 11/17] CHG: move adapted driver to franka_gello_state_publisher and restore driver in gello/dynamixel --- gello/dynamixel/driver.py | 399 ++++++------------ .../dynamixel/README.md | 2 +- .../dynamixel/__init__.py | 0 .../dynamixel/driver.py | 349 +++++++++++++++ .../dynamixel/motor_configs/xl330.yaml | 0 .../gello_hardware.py | 16 +- .../src/franka_gello_state_publisher/setup.py | 3 + 7 files changed, 486 insertions(+), 283 deletions(-) rename {gello => ros2/src/franka_gello_state_publisher/franka_gello_state_publisher}/dynamixel/README.md (96%) create mode 100644 ros2/src/franka_gello_state_publisher/franka_gello_state_publisher/dynamixel/__init__.py create mode 100644 ros2/src/franka_gello_state_publisher/franka_gello_state_publisher/dynamixel/driver.py rename {gello => ros2/src/franka_gello_state_publisher/franka_gello_state_publisher}/dynamixel/motor_configs/xl330.yaml (100%) diff --git a/gello/dynamixel/driver.py b/gello/dynamixel/driver.py index 721027c7..89325c75 100644 --- a/gello/dynamixel/driver.py +++ b/gello/dynamixel/driver.py @@ -1,61 +1,49 @@ import time -from pathlib import Path from threading import Event, Lock, Thread from typing import Protocol, Sequence import numpy as np -import yaml from dynamixel_sdk.group_sync_read import GroupSyncRead from dynamixel_sdk.group_sync_write import GroupSyncWrite from dynamixel_sdk.packet_handler import PacketHandler from dynamixel_sdk.port_handler import PortHandler from dynamixel_sdk.robotis_def import ( COMM_SUCCESS, + DXL_HIBYTE, + DXL_HIWORD, + DXL_LOBYTE, + DXL_LOWORD, ) -# Configuration loader for motor types -def load_motor_config(motor_type: str = "xl330") -> dict: - """Load motor configuration from YAML file. - - Args: - motor_type (str): The type of motor to load configuration for. - - Returns: - dict: The motor configuration dictionary. - """ - config_dir = Path(__file__).parent / "motor_configs" - config_path = config_dir / f"{motor_type}.yaml" - - if not config_path.exists(): - available_types = [f.stem for f in config_dir.glob("*.yaml")] - raise ValueError(f"Motor type '{motor_type}' not found. Available types: {available_types}") - - with open(config_path, 'r') as f: - config = yaml.safe_load(f) - - return config +# Constants +ADDR_TORQUE_ENABLE = 64 +ADDR_GOAL_POSITION = 116 +LEN_GOAL_POSITION = 4 +ADDR_PRESENT_POSITION = 132 +LEN_PRESENT_POSITION = 4 +TORQUE_ENABLE = 1 +TORQUE_DISABLE = 0 + class DynamixelDriverProtocol(Protocol): - def set_joints(self, joint_angles: Sequence[float | None]): + def set_joints(self, joint_angles: Sequence[float]): """Set the joint angles for the Dynamixel servos. Args: - joint_angles (Sequence[float | None]): A list of joint angles. + joint_angles (Sequence[float]): A list of joint angles. """ ... def torque_enabled(self) -> bool: - """Check if torque is enabled for at least one of the Dynamixel servos. - If you want to check which servos have torque enabled, you can use the read_value_by_name method. + """Check if torque is enabled for the Dynamixel servos. Returns: - bool: True if torque is enabled for one of the servos, False if it is disabled for all. + bool: True if torque is enabled, False if it is disabled. """ ... def set_torque_mode(self, enable: bool): - """Enable or disable torque for all Dynamixel servos. - If you want to enable torque for a specific servo, you can use the write_value_by_name method. + """Set the torque mode for the Dynamixel servos. Args: enable (bool): True to enable torque, False to disable. @@ -70,126 +58,41 @@ def get_joints(self) -> np.ndarray: """ ... - def write_value_by_name(self, name: str, values: Sequence[int | None]): - """Set a group of values by name. - - Args: - name (str): The name of the control parameter. - values (Sequence[int | None]): A list of values to set for each servo. - """ - ... - - def read_value_by_name(self, name: str) -> Sequence[int]: - """Read a group of values by name. - - Args: - name (str): The name of the control parameter to read. - - Returns: - Sequence[int]: A list of values read from the servos. - """ - ... - def close(self): """Close the driver.""" - ... class FakeDynamixelDriver(DynamixelDriverProtocol): - def __init__(self, ids: Sequence[int], motor_type: str = "xl330"): + def __init__(self, ids: Sequence[int]): self._ids = ids - - # Load motor configuration - self._motor_config = load_motor_config(motor_type) - self._ctrl_table = self._motor_config["control_table"] - self._pulses_per_revolution = self._motor_config["pulses_per_revolution"] - - self._storage_map = { - "goal_position": np.zeros(len(ids), dtype=int), - "goal_current": np.zeros(len(ids), dtype=int), - "kp_d": np.zeros(len(ids), dtype=int), - "kp_i": np.zeros(len(ids), dtype=int), - "kp_p": np.zeros(len(ids), dtype=int), - "operating_mode": np.zeros(len(ids), dtype=int), - "torque_enable": np.zeros(len(ids), dtype=int), - } - - def _pulses_to_rad(self, pulses) -> np.ndarray: - """Convert pulses to radians using motor-specific configuration.""" - return np.array(pulses) / self._pulses_per_revolution * 2 * np.pi - - 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 _set_group( - self, - name: str, - values: Sequence[int | None], - value_min: int | None = None, - value_max: int | None = None, - ): - if len(values) != len(self._ids): - raise ValueError(f"The length of {name} must match the number of servos") - for dxl_id, value in zip(self._ids, values): - if value is None: - continue - if value_min is not None: - value = max(value, value_min) - if value_max is not None: - value = min(value, value_max) - self._storage_map[name][self._ids.index(dxl_id)] = value - print(f"Set {name} {value} for ID {dxl_id}", flush=True) - - def write_value_by_name(self, name: str, values: Sequence[int | None]): - self._set_group( - name=name, - values=values, - value_min=self._ctrl_table[name].get("min", None), - value_max=self._ctrl_table[name].get("max", None), - ) - - def _read_group(self, name: str) -> Sequence[int]: - if name == "present_position": - name = "goal_position" - elif name == "present_current": - name = "goal_current" - if name not in self._storage_map: - raise ValueError(f"Register {name} not found.") - return self._storage_map[name].copy().tolist() - - def read_value_by_name(self, name: str) -> Sequence[int]: - return self._read_group(name) - - def set_joints(self, joint_angles: Sequence[float | None]): - self.write_value_by_name( - "goal_position", - [ - self._rad_to_pulses(angle) if angle is not None else None - for angle in joint_angles - ], - ) + self._joint_angles = np.zeros(len(ids), dtype=int) + self._torque_enabled = False - def get_joints(self) -> np.ndarray: - return self._pulses_to_rad(self._storage_map["goal_position"].copy()) + def set_joints(self, joint_angles: Sequence[float]): + if len(joint_angles) != len(self._ids): + raise ValueError( + "The length of joint_angles must match the number of servos" + ) + if not self._torque_enabled: + raise RuntimeError("Torque must be enabled to set joint angles") + self._joint_angles = np.array(joint_angles) def torque_enabled(self) -> bool: - return bool(np.any(self._storage_map["torque_enable"])) + return self._torque_enabled def set_torque_mode(self, enable: bool): - torque_value = 1 if enable else 0 - self.write_value_by_name("torque_enable", [torque_value] * len(self._ids)) + self._torque_enabled = enable + + def get_joints(self) -> np.ndarray: + return self._joint_angles.copy() def close(self): pass + class DynamixelDriver(DynamixelDriverProtocol): def __init__( - self, - ids: Sequence[int], - port: str = "/dev/ttyUSB0", - baudrate: int = 57600, - motor_type: str = "xl330" + self, ids: Sequence[int], port: str = "/dev/ttyUSB0", baudrate: int = 57600 ): """Initialize the DynamixelDriver class. @@ -197,45 +100,26 @@ def __init__( ids (Sequence[int]): A list of IDs for the Dynamixel servos. port (str): The USB port to connect to the arm. baudrate (int): The baudrate for communication. - motor_type (str): The type of motor to use (e.g., "xl330"). """ self._ids = ids self._joint_angles = None self._lock = Lock() - - # Load motor configuration - self._motor_config = load_motor_config(motor_type) - self._ctrl_table = self._motor_config["control_table"] - self._operating_modes = self._motor_config["operating_modes"] - self._pulses_per_revolution = self._motor_config["pulses_per_revolution"] # Initialize the port handler, packet handler, and group sync read/write self._portHandler = PortHandler(port) self._packetHandler = PacketHandler(2.0) - - # Create group sync read/write handlers for each control table entry - self._groupSyncReadHandlers = {} - self._groupSyncWriteHandlers = {} - for key, entry in self._ctrl_table.items(): - self._groupSyncReadHandlers[key] = GroupSyncRead( - self._portHandler, - self._packetHandler, - entry["addr"], - entry["len"], - ) - # Add parameters for each Dynamixel servo to the group sync read - for dxl_id in self._ids: - if not self._groupSyncReadHandlers[key].addParam(dxl_id): - raise RuntimeError( - f"Failed to add parameter for Dynamixel with ID {dxl_id}" - ) - if "read_only" not in entry or not entry["read_only"]: - self._groupSyncWriteHandlers[key] = GroupSyncWrite( - self._portHandler, - self._packetHandler, - entry["addr"], - entry["len"], - ) + self._groupSyncRead = GroupSyncRead( + self._portHandler, + self._packetHandler, + ADDR_PRESENT_POSITION, + LEN_PRESENT_POSITION, + ) + self._groupSyncWrite = GroupSyncWrite( + self._portHandler, + self._packetHandler, + ADDR_GOAL_POSITION, + LEN_GOAL_POSITION, + ) # Open the port and set the baudrate if not self._portHandler.openPort(): @@ -244,144 +128,123 @@ def __init__( if not self._portHandler.setBaudRate(baudrate): raise RuntimeError(f"Failed to change the baudrate, {baudrate}") - # Disable torque for all Dynamixel servos + # Add parameters for each Dynamixel servo to the group sync read + for dxl_id in self._ids: + if not self._groupSyncRead.addParam(dxl_id): + raise RuntimeError( + f"Failed to add parameter for Dynamixel with ID {dxl_id}" + ) + + # Disable torque for each Dynamixel servo + self._torque_enabled = False try: - self.set_torque_mode(False) + self.set_torque_mode(self._torque_enabled) except Exception as e: print(f"port: {port}, {e}") self._stop_thread = Event() self._start_reading_thread() - def _set_group( - self, - name: str, - values: Sequence[int | None], - groupSyncWriteHandler, - value_length: int, - value_min: int | None = None, - value_max: int | None = None, - ): - if len(values) != len(self._ids): - raise ValueError(f"The length of {name} must match the number of servos") - with self._lock: - for dxl_id, value in zip(self._ids, values): - if value is None: - continue - if value_min is not None: - value = max(value, value_min) - if value_max is not None: - value = min(value, value_max) - # Convert value to little-endian byte array of length value_length - param = [(value >> (8 * i)) & 0xFF for i in range(value_length)] - result = groupSyncWriteHandler.addParam(dxl_id, param) - if not result: - 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}") - groupSyncWriteHandler.clearParam() - - def write_value_by_name(self, name: str, values: Sequence[int | None]): - self._set_group( - name=name, - values=values, - groupSyncWriteHandler=self._groupSyncWriteHandlers[name], - value_length=self._ctrl_table[name]["len"], - value_min=self._ctrl_table[name].get("min", None), - value_max=self._ctrl_table[name].get("max", None), - ) + def set_joints(self, joint_angles: Sequence[float]): + if len(joint_angles) != len(self._ids): + raise ValueError( + "The length of joint_angles must match the number of servos" + ) + if not self._torque_enabled: + raise RuntimeError("Torque must be enabled to set joint angles") + + for dxl_id, angle in zip(self._ids, joint_angles): + # Convert the angle to the appropriate value for the servo + position_value = int(angle * 2048 / np.pi) + + # Allocate goal position value into byte array + param_goal_position = [ + DXL_LOBYTE(DXL_LOWORD(position_value)), + DXL_HIBYTE(DXL_LOWORD(position_value)), + DXL_LOBYTE(DXL_HIWORD(position_value)), + DXL_HIBYTE(DXL_HIWORD(position_value)), + ] + + # Add goal position value to the Syncwrite parameter storage + dxl_addparam_result = self._groupSyncWrite.addParam( + dxl_id, param_goal_position + ) + if not dxl_addparam_result: + raise RuntimeError( + f"Failed to set joint angle for Dynamixel with ID {dxl_id}" + ) + + # Syncwrite goal position + dxl_comm_result = self._groupSyncWrite.txPacket() + if dxl_comm_result != COMM_SUCCESS: + raise RuntimeError("Failed to syncwrite goal position") + + # Clear syncwrite parameter storage + self._groupSyncWrite.clearParam() + + def torque_enabled(self) -> bool: + return self._torque_enabled - def _read_group( - self, name: str, groupSyncReadHandler, value_length: int - ) -> list[int]: + def set_torque_mode(self, enable: bool): + torque_value = TORQUE_ENABLE if enable else TORQUE_DISABLE with self._lock: - result = groupSyncReadHandler.txRxPacket() - if result != COMM_SUCCESS: - raise RuntimeError(f"Failed to sync read {name}, comm result: {result}") - values = [] for dxl_id in self._ids: - if groupSyncReadHandler.isAvailable( - dxl_id, self._ctrl_table[name]["addr"], value_length - ): - value = groupSyncReadHandler.getData( - dxl_id, self._ctrl_table[name]["addr"], value_length - ) - value = int(np.int32(np.uint32(value))) - values.append(value) - else: + dxl_comm_result, dxl_error = self._packetHandler.write1ByteTxRx( + self._portHandler, dxl_id, ADDR_TORQUE_ENABLE, torque_value + ) + if dxl_comm_result != COMM_SUCCESS or dxl_error != 0: + print(dxl_comm_result) + print(dxl_error) raise RuntimeError( - f"Failed to get {name} for Dynamixel with ID {dxl_id}" + f"Failed to set torque mode for Dynamixel with ID {dxl_id}" ) - return values - def read_value_by_name(self, name: str) -> list[int]: - return self._read_group( - name=name, - groupSyncReadHandler=self._groupSyncReadHandlers[name], - value_length=self._ctrl_table[name]["len"], - ) - - def set_joints(self, joint_angles: Sequence[float | None]): - joint_angles_pulses = [ - self._rad_to_pulses(rad) if rad is not None else None for rad in joint_angles - ] - self.write_value_by_name("goal_position", joint_angles_pulses) + self._torque_enabled = enable def _start_reading_thread(self): - self._reading_thread = Thread(target=self._read_joint_angles_loop) + self._reading_thread = Thread(target=self._read_joint_angles) self._reading_thread.daemon = True self._reading_thread.start() - def _read_joint_angles_loop(self): + def _read_joint_angles(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"], - ) - _joint_angles = np.array(_joint_angles, dtype=int) + with self._lock: + _joint_angles = np.zeros(len(self._ids), dtype=int) + dxl_comm_result = self._groupSyncRead.txRxPacket() + if dxl_comm_result != COMM_SUCCESS: + print(f"warning, comm failed: {dxl_comm_result}") + continue + for i, dxl_id in enumerate(self._ids): + if self._groupSyncRead.isAvailable( + dxl_id, ADDR_PRESENT_POSITION, LEN_PRESENT_POSITION + ): + angle = self._groupSyncRead.getData( + dxl_id, ADDR_PRESENT_POSITION, LEN_PRESENT_POSITION + ) + angle = np.int32(np.uint32(angle)) + _joint_angles[i] = angle + else: + raise RuntimeError( + f"Failed to get joint angles for Dynamixel with ID {dxl_id}" + ) self._joint_angles = _joint_angles - except RuntimeError as e: - print(f"warning, comm failed: {e}") - continue + # self._groupSyncRead.clearParam() # TODO what does this do? should i add it 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) + # with self._lock: _j = self._joint_angles.copy() - return self._pulses_to_rad(_j) - - def torque_enabled(self) -> bool: - try: - torque_values = self.read_value_by_name("torque_enable") - return any(val == 1 for val in torque_values) - except RuntimeError: - return False - - def set_torque_mode(self, enable: bool): - torque_value = 1 if enable else 0 - self.write_value_by_name("torque_enable", [torque_value] * len(self._ids)) + return _j / 2048.0 * np.pi def close(self): self._stop_thread.set() self._reading_thread.join() self._portHandler.closePort() - def _pulses_to_rad(self, pulses) -> np.ndarray: - """Convert pulses to radians using motor-specific configuration.""" - return np.array(pulses) / self._pulses_per_revolution * 2 * np.pi - - 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 main(): # Set the port, baudrate, and servo IDs diff --git a/gello/dynamixel/README.md b/ros2/src/franka_gello_state_publisher/franka_gello_state_publisher/dynamixel/README.md similarity index 96% rename from gello/dynamixel/README.md rename to ros2/src/franka_gello_state_publisher/franka_gello_state_publisher/dynamixel/README.md index 377cceb6..40806908 100644 --- a/gello/dynamixel/README.md +++ b/ros2/src/franka_gello_state_publisher/franka_gello_state_publisher/dynamixel/README.md @@ -8,7 +8,7 @@ Motor configurations are stored in the `motor_configs/` subdirectory, with one Y ### File Structure: ``` -gello/dynamixel/ +franka_gello_state_publisher/franka_gello_state_publisher/dynamixel/ ├── driver.py ├── motor_configs/ │ ├── xl330.yaml diff --git a/ros2/src/franka_gello_state_publisher/franka_gello_state_publisher/dynamixel/__init__.py b/ros2/src/franka_gello_state_publisher/franka_gello_state_publisher/dynamixel/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/ros2/src/franka_gello_state_publisher/franka_gello_state_publisher/dynamixel/driver.py b/ros2/src/franka_gello_state_publisher/franka_gello_state_publisher/dynamixel/driver.py new file mode 100644 index 00000000..ec2f4177 --- /dev/null +++ b/ros2/src/franka_gello_state_publisher/franka_gello_state_publisher/dynamixel/driver.py @@ -0,0 +1,349 @@ +import time +from pathlib import Path +from threading import Event, Lock, Thread +from typing import Protocol, Sequence + +import numpy as np +import yaml +from dynamixel_sdk.group_sync_read import GroupSyncRead +from dynamixel_sdk.group_sync_write import GroupSyncWrite +from dynamixel_sdk.packet_handler import PacketHandler +from dynamixel_sdk.port_handler import PortHandler +from dynamixel_sdk.robotis_def import ( + COMM_SUCCESS, +) + +# Configuration loader for motor types +def load_motor_config(motor_type: str = "xl330") -> dict: + """Load motor configuration from YAML file. + + Args: + motor_type (str): The type of motor to load configuration for. + + Returns: + dict: The motor configuration dictionary. + """ + config_dir = Path(__file__).parent / "motor_configs" + config_path = config_dir / f"{motor_type}.yaml" + + if not config_path.exists(): + available_types = [f.stem for f in config_dir.glob("*.yaml")] + raise ValueError(f"Motor type '{motor_type}' not found. Available types: {available_types}") + + with open(config_path, 'r') as f: + config = yaml.safe_load(f) + + return config + +class DynamixelDriverProtocol(Protocol): + def get_joints(self) -> np.ndarray: + """Get the current joint angles in radians. + + Returns: + np.ndarray: An array of joint angles. + """ + ... + + def write_value_by_name(self, name: str, values: Sequence[int | None]): + """Set a group of values by name. + + Args: + name (str): The name of the control parameter. + values (Sequence[int | None]): A list of values to set for each servo. + """ + ... + + def read_value_by_name(self, name: str) -> Sequence[int]: + """Read a group of values by name. + + Args: + name (str): The name of the control parameter to read. + + Returns: + Sequence[int]: A list of values read from the servos. + """ + ... + + def close(self): + """Close the driver.""" + ... + + +class FakeDynamixelDriver(DynamixelDriverProtocol): + def __init__(self, ids: Sequence[int], motor_type: str = "xl330"): + self._ids = ids + + # Load motor configuration + self._motor_config = load_motor_config(motor_type) + self._ctrl_table = self._motor_config["control_table"] + self._pulses_per_revolution = self._motor_config["pulses_per_revolution"] + + self._storage_map = { + "goal_position": np.zeros(len(ids), dtype=int), + "goal_current": np.zeros(len(ids), dtype=int), + "kp_d": np.zeros(len(ids), dtype=int), + "kp_i": np.zeros(len(ids), dtype=int), + "kp_p": np.zeros(len(ids), dtype=int), + "operating_mode": np.zeros(len(ids), dtype=int), + "torque_enable": np.zeros(len(ids), dtype=int), + } + + def _pulses_to_rad(self, pulses) -> np.ndarray: + """Convert pulses to radians using motor-specific configuration.""" + return np.array(pulses) / self._pulses_per_revolution * 2 * np.pi + + 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 _set_group( + self, + name: str, + values: Sequence[int | None], + value_min: int | None = None, + value_max: int | None = None, + ): + if len(values) != len(self._ids): + raise ValueError(f"The length of {name} must match the number of servos") + for dxl_id, value in zip(self._ids, values): + if value is None: + continue + if value_min is not None: + value = max(value, value_min) + if value_max is not None: + value = min(value, value_max) + self._storage_map[name][self._ids.index(dxl_id)] = value + print(f"Set {name} {value} for ID {dxl_id}", flush=True) + + def write_value_by_name(self, name: str, values: Sequence[int | None]): + self._set_group( + name=name, + values=values, + value_min=self._ctrl_table[name].get("min", None), + value_max=self._ctrl_table[name].get("max", None), + ) + + def _read_group(self, name: str) -> Sequence[int]: + if name == "present_position": + name = "goal_position" + elif name == "present_current": + name = "goal_current" + if name not in self._storage_map: + raise ValueError(f"Register {name} not found.") + return self._storage_map[name].copy().tolist() + + def read_value_by_name(self, name: str) -> Sequence[int]: + return self._read_group(name) + + def get_joints(self) -> np.ndarray: + return self._pulses_to_rad(self._storage_map["goal_position"].copy()) + + def close(self): + pass + +class DynamixelDriver(DynamixelDriverProtocol): + def __init__( + self, + ids: Sequence[int], + port: str = "/dev/ttyUSB0", + baudrate: int = 57600, + motor_type: str = "xl330" + ): + """Initialize the DynamixelDriver class. + + Args: + ids (Sequence[int]): A list of IDs for the Dynamixel servos. + port (str): The USB port to connect to the arm. + baudrate (int): The baudrate for communication. + motor_type (str): The type of motor to use (e.g., "xl330"). + """ + self._ids = ids + self._joint_angles = None + self._lock = Lock() + + # Load motor configuration + self._motor_config = load_motor_config(motor_type) + self._ctrl_table = self._motor_config["control_table"] + self._operating_modes = self._motor_config["operating_modes"] + self._pulses_per_revolution = self._motor_config["pulses_per_revolution"] + + # Initialize the port handler, packet handler, and group sync read/write + self._portHandler = PortHandler(port) + self._packetHandler = PacketHandler(2.0) + + # Create group sync read/write handlers for each control table entry + self._groupSyncReadHandlers = {} + self._groupSyncWriteHandlers = {} + for key, entry in self._ctrl_table.items(): + self._groupSyncReadHandlers[key] = GroupSyncRead( + self._portHandler, + self._packetHandler, + entry["addr"], + entry["len"], + ) + # Add parameters for each Dynamixel servo to the group sync read + for dxl_id in self._ids: + if not self._groupSyncReadHandlers[key].addParam(dxl_id): + raise RuntimeError( + f"Failed to add parameter for Dynamixel with ID {dxl_id}" + ) + if "read_only" not in entry or not entry["read_only"]: + self._groupSyncWriteHandlers[key] = GroupSyncWrite( + self._portHandler, + self._packetHandler, + entry["addr"], + entry["len"], + ) + + # Open the port and set the baudrate + if not self._portHandler.openPort(): + raise RuntimeError("Failed to open the port") + + if not self._portHandler.setBaudRate(baudrate): + raise RuntimeError(f"Failed to change the baudrate, {baudrate}") + + # Disable torque for all Dynamixel servos + try: + self.write_value_by_name("torque_enable", [0] * len(self._ids)) + except Exception as e: + print(f"port: {port}, {e}") + + self._stop_thread = Event() + self._start_reading_thread() + + def _set_group( + self, + name: str, + values: Sequence[int | None], + groupSyncWriteHandler, + value_length: int, + value_min: int | None = None, + value_max: int | None = None, + ): + if len(values) != len(self._ids): + raise ValueError(f"The length of {name} must match the number of servos") + with self._lock: + for dxl_id, value in zip(self._ids, values): + if value is None: + continue + if value_min is not None: + value = max(value, value_min) + if value_max is not None: + value = min(value, value_max) + # Convert value to little-endian byte array of length value_length + param = [(value >> (8 * i)) & 0xFF for i in range(value_length)] + result = groupSyncWriteHandler.addParam(dxl_id, param) + if not result: + 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}") + groupSyncWriteHandler.clearParam() + + def write_value_by_name(self, name: str, values: Sequence[int | None]): + self._set_group( + name=name, + values=values, + groupSyncWriteHandler=self._groupSyncWriteHandlers[name], + value_length=self._ctrl_table[name]["len"], + value_min=self._ctrl_table[name].get("min", None), + value_max=self._ctrl_table[name].get("max", None), + ) + + def _read_group( + self, name: str, groupSyncReadHandler, value_length: int + ) -> list[int]: + with self._lock: + result = groupSyncReadHandler.txRxPacket() + if result != COMM_SUCCESS: + raise RuntimeError(f"Failed to sync read {name}, comm result: {result}") + values = [] + for dxl_id in self._ids: + if groupSyncReadHandler.isAvailable( + dxl_id, self._ctrl_table[name]["addr"], value_length + ): + value = groupSyncReadHandler.getData( + dxl_id, self._ctrl_table[name]["addr"], value_length + ) + value = int(np.int32(np.uint32(value))) + values.append(value) + else: + raise RuntimeError( + f"Failed to get {name} for Dynamixel with ID {dxl_id}" + ) + return values + + def read_value_by_name(self, name: str) -> list[int]: + return self._read_group( + name=name, + groupSyncReadHandler=self._groupSyncReadHandlers[name], + 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 _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"], + ) + _joint_angles = np.array(_joint_angles, dtype=int) + self._joint_angles = _joint_angles + except RuntimeError as e: + print(f"warning, comm failed: {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() + self._portHandler.closePort() + + def _pulses_to_rad(self, pulses) -> np.ndarray: + """Convert pulses to radians using motor-specific configuration.""" + return np.array(pulses) / self._pulses_per_revolution * 2 * np.pi + + 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 main(): + # Set the port, baudrate, and servo IDs + ids = [1] + + # Create a DynamixelDriver instance + driver = DynamixelDriver(ids) + + # Test setting torque mode + driver.write_value_by_name("torque_enable", [1] * len(ids)) + driver.write_value_by_name("torque_enable", [0] * len(ids)) + + # Test reading the joint angles + try: + while True: + joint_angles = driver.get_joints() + print(f"Joint angles for IDs {ids}: {joint_angles}") + # print(f"Joint angles for IDs {ids[1]}: {joint_angles[1]}") + except KeyboardInterrupt: + driver.close() + + +if __name__ == "__main__": + main() # Test the driver diff --git a/gello/dynamixel/motor_configs/xl330.yaml b/ros2/src/franka_gello_state_publisher/franka_gello_state_publisher/dynamixel/motor_configs/xl330.yaml similarity index 100% rename from gello/dynamixel/motor_configs/xl330.yaml rename to ros2/src/franka_gello_state_publisher/franka_gello_state_publisher/dynamixel/motor_configs/xl330.yaml 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 4a0cb07e..3fb6714a 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 @@ -1,9 +1,7 @@ import numpy as np -from ament_index_python.packages import get_package_prefix -from os import path as os_path -from sys import path as sys_path from dataclasses import dataclass, field from typing import List, TypedDict, Iterator, Tuple +from franka_gello_state_publisher.dynamixel.driver import DynamixelDriver class GelloHardwareParams(TypedDict): @@ -103,19 +101,9 @@ def __init__( self._initialize_parameters() - def _add_dynamixel_driver_path(self) -> None: - """Add dynamixel driver to Python path.""" - gello_path = os_path.abspath( - os_path.join(get_package_prefix("franka_gello_state_publisher"), "../../../") - ) - sys_path.insert(0, gello_path) - def _initialize_driver(self) -> None: """Initialize dynamixel driver with joint IDs and port.""" joint_ids = list(range(1, self._num_total_joints + 1)) - self._add_dynamixel_driver_path() - from gello.dynamixel.driver import DynamixelDriver - self._driver = DynamixelDriver(joint_ids, port=self._com_port, baudrate=57600) def _initialize_parameters(self) -> None: @@ -153,7 +141,7 @@ def read_joint_states(self) -> tuple[np.ndarray, float]: def disable_torque(self) -> None: """Disable torque on all joints.""" - self._driver.set_torque_mode(False) + self._driver.write_value_by_name("torque_enable", [0] * len(self._driver._ids)) def _goal_position_to_pulses(self, goals: list[float]) -> list[int]: """Convert goal positions from radians to dynamixel pulses.""" diff --git a/ros2/src/franka_gello_state_publisher/setup.py b/ros2/src/franka_gello_state_publisher/setup.py index 61fb1949..ed7665e4 100644 --- a/ros2/src/franka_gello_state_publisher/setup.py +++ b/ros2/src/franka_gello_state_publisher/setup.py @@ -7,6 +7,9 @@ name=package_name, version="0.2.0", packages=find_packages(exclude=["test"]), + package_data={ + "franka_gello_state_publisher.dynamixel": ["motor_configs/*.yaml"], + }, data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), ("share/" + package_name, ["package.xml"]), From bbde2e992175f42318b74168412da5ae1fe81b27 Mon Sep 17 00:00:00 2001 From: Peter Wern Date: Tue, 11 Nov 2025 15:04:51 +0100 Subject: [PATCH 12/17] FIX: minor improvements --- ros2/.devcontainer/Dockerfile | 2 +- ros2/README.md | 2 +- .../franka_gello_state_publisher/dynamixel/driver.py | 7 ++----- 3 files changed, 4 insertions(+), 7 deletions(-) diff --git a/ros2/.devcontainer/Dockerfile b/ros2/.devcontainer/Dockerfile index 12ab1b78..3cd827af 100644 --- a/ros2/.devcontainer/Dockerfile +++ b/ros2/.devcontainer/Dockerfile @@ -73,5 +73,5 @@ RUN /bin/bash -c "source /opt/ros/humble/setup.bash && \ rosdep install --from-paths . --ignore-src -r -y && \ colcon build --install-base /opt/ros/humble/franka --cmake-args -DCMAKE_BUILD_TYPE=Release && \ cd .. && \ - rm -rf /tmp/rqt_reconfigure \ + rm -rf /tmp/rqt_reconfigure && \ rm -rf /var/lib/apt/lists/*" \ No newline at end of file diff --git a/ros2/README.md b/ros2/README.md index a3b51c50..1531551b 100644 --- a/ros2/README.md +++ b/ros2/README.md @@ -154,7 +154,7 @@ 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. > 💡 **Hint:** -> The `example_fr3_config.yaml` file gives a good starting point for these values for a tabletop mounted GELLO: +> The example configuration files give a good starting point for these values: > - springs in joints 1, 2, 4 and gripper. > - damping in all joints, stronger for lower joints > - `goal_position` values correspond to the calibration pose. diff --git a/ros2/src/franka_gello_state_publisher/franka_gello_state_publisher/dynamixel/driver.py b/ros2/src/franka_gello_state_publisher/franka_gello_state_publisher/dynamixel/driver.py index ec2f4177..a655295c 100644 --- a/ros2/src/franka_gello_state_publisher/franka_gello_state_publisher/dynamixel/driver.py +++ b/ros2/src/franka_gello_state_publisher/franka_gello_state_publisher/dynamixel/driver.py @@ -9,9 +9,7 @@ from dynamixel_sdk.group_sync_write import GroupSyncWrite from dynamixel_sdk.packet_handler import PacketHandler from dynamixel_sdk.port_handler import PortHandler -from dynamixel_sdk.robotis_def import ( - COMM_SUCCESS, -) +from dynamixel_sdk.robotis_def import COMM_SUCCESS # Configuration loader for motor types def load_motor_config(motor_type: str = "xl330") -> dict: @@ -297,8 +295,7 @@ def _read_joint_angles_loop(self): self._groupSyncReadHandlers["present_position"], self._ctrl_table["present_position"]["len"], ) - _joint_angles = np.array(_joint_angles, dtype=int) - self._joint_angles = _joint_angles + self._joint_angles = np.array(_joint_angles, dtype=int) except RuntimeError as e: print(f"warning, comm failed: {e}") continue From 91c0cee0974d9225904fd5307e41a404043d3427 Mon Sep 17 00:00:00 2001 From: Peter Wern Date: Tue, 11 Nov 2025 14:49:26 +0000 Subject: [PATCH 13/17] FIX: formatting of driver --- .../dynamixel/driver.py | 125 +++++++++++------- 1 file changed, 75 insertions(+), 50 deletions(-) diff --git a/ros2/src/franka_gello_state_publisher/franka_gello_state_publisher/dynamixel/driver.py b/ros2/src/franka_gello_state_publisher/franka_gello_state_publisher/dynamixel/driver.py index a655295c..668b3fbb 100644 --- a/ros2/src/franka_gello_state_publisher/franka_gello_state_publisher/dynamixel/driver.py +++ b/ros2/src/franka_gello_state_publisher/franka_gello_state_publisher/dynamixel/driver.py @@ -11,54 +11,79 @@ from dynamixel_sdk.port_handler import PortHandler from dynamixel_sdk.robotis_def import COMM_SUCCESS + # Configuration loader for motor types def load_motor_config(motor_type: str = "xl330") -> dict: - """Load motor configuration from YAML file. - - Args: - motor_type (str): The type of motor to load configuration for. - - Returns: - dict: The motor configuration dictionary. + """ + Load motor configuration from YAML file. + + Parameters + ---------- + motor_type : str, optional + The type of motor to load configuration for (default is "xl330"). + + Returns + ------- + dict + The motor configuration dictionary. + """ config_dir = Path(__file__).parent / "motor_configs" config_path = config_dir / f"{motor_type}.yaml" - + if not config_path.exists(): available_types = [f.stem for f in config_dir.glob("*.yaml")] - raise ValueError(f"Motor type '{motor_type}' not found. Available types: {available_types}") - - with open(config_path, 'r') as f: + raise ValueError( + f"Motor type '{motor_type}' not found. Available types: {available_types}" + ) + + with open(config_path, "r") as f: config = yaml.safe_load(f) - + return config + class DynamixelDriverProtocol(Protocol): def get_joints(self) -> np.ndarray: - """Get the current joint angles in radians. + """ + Get the current joint angles in radians. + + Returns + ------- + np.ndarray + An array of joint angles. - Returns: - np.ndarray: An array of joint angles. """ ... def write_value_by_name(self, name: str, values: Sequence[int | None]): - """Set a group of values by name. + """ + Set a group of values by name. + + Parameters + ---------- + name : str + The name of the control parameter. + values : Sequence[int | None] + A list of values to set for each servo. - Args: - name (str): The name of the control parameter. - values (Sequence[int | None]): A list of values to set for each servo. """ ... def read_value_by_name(self, name: str) -> Sequence[int]: - """Read a group of values by name. + """ + Read a group of values by name. - Args: - name (str): The name of the control parameter to read. + Parameters + ---------- + name : str + The name of the control parameter to read. + + Returns + ------- + Sequence[int] + A list of values read from the servos. - Returns: - Sequence[int]: A list of values read from the servos. """ ... @@ -70,12 +95,12 @@ def close(self): class FakeDynamixelDriver(DynamixelDriverProtocol): def __init__(self, ids: Sequence[int], motor_type: str = "xl330"): self._ids = ids - + # Load motor configuration self._motor_config = load_motor_config(motor_type) self._ctrl_table = self._motor_config["control_table"] self._pulses_per_revolution = self._motor_config["pulses_per_revolution"] - + self._storage_map = { "goal_position": np.zeros(len(ids), dtype=int), "goal_current": np.zeros(len(ids), dtype=int), @@ -139,26 +164,34 @@ def get_joints(self) -> np.ndarray: def close(self): pass + class DynamixelDriver(DynamixelDriverProtocol): def __init__( - self, - ids: Sequence[int], - port: str = "/dev/ttyUSB0", + self, + ids: Sequence[int], + port: str = "/dev/ttyUSB0", baudrate: int = 57600, - motor_type: str = "xl330" + motor_type: str = "xl330", ): - """Initialize the DynamixelDriver class. + """ + Initialize the DynamixelDriver class. + + Parameters + ---------- + ids : Sequence[int] + A list of IDs for the Dynamixel servos. + port : str, optional + The USB port to connect to the arm (default is "/dev/ttyUSB0"). + baudrate : int, optional + The baudrate for communication (default is 57600). + motor_type : str, optional + The type of motor to use, e.g., "xl330" (default is "xl330"). - Args: - ids (Sequence[int]): A list of IDs for the Dynamixel servos. - port (str): The USB port to connect to the arm. - baudrate (int): The baudrate for communication. - motor_type (str): The type of motor to use (e.g., "xl330"). """ self._ids = ids self._joint_angles = None self._lock = Lock() - + # Load motor configuration self._motor_config = load_motor_config(motor_type) self._ctrl_table = self._motor_config["control_table"] @@ -182,9 +215,7 @@ def __init__( # Add parameters for each Dynamixel servo to the group sync read for dxl_id in self._ids: if not self._groupSyncReadHandlers[key].addParam(dxl_id): - raise RuntimeError( - f"Failed to add parameter for Dynamixel with ID {dxl_id}" - ) + raise RuntimeError(f"Failed to add parameter for Dynamixel with ID {dxl_id}") if "read_only" not in entry or not entry["read_only"]: self._groupSyncWriteHandlers[key] = GroupSyncWrite( self._portHandler, @@ -232,9 +263,7 @@ def _set_group( param = [(value >> (8 * i)) & 0xFF for i in range(value_length)] result = groupSyncWriteHandler.addParam(dxl_id, param) if not result: - raise RuntimeError( - f"Failed to set {name} for Dynamixel with ID {dxl_id}" - ) + 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}") @@ -250,9 +279,7 @@ def write_value_by_name(self, name: str, values: Sequence[int | None]): value_max=self._ctrl_table[name].get("max", None), ) - def _read_group( - self, name: str, groupSyncReadHandler, value_length: int - ) -> list[int]: + def _read_group(self, name: str, groupSyncReadHandler, value_length: int) -> list[int]: with self._lock: result = groupSyncReadHandler.txRxPacket() if result != COMM_SUCCESS: @@ -268,9 +295,7 @@ def _read_group( value = int(np.int32(np.uint32(value))) values.append(value) else: - raise RuntimeError( - f"Failed to get {name} for Dynamixel with ID {dxl_id}" - ) + raise RuntimeError(f"Failed to get {name} for Dynamixel with ID {dxl_id}") return values def read_value_by_name(self, name: str) -> list[int]: @@ -327,7 +352,7 @@ def main(): # Create a DynamixelDriver instance driver = DynamixelDriver(ids) - + # Test setting torque mode driver.write_value_by_name("torque_enable", [1] * len(ids)) driver.write_value_by_name("torque_enable", [0] * len(ids)) From 03f3313b0e651c66bb4fa8b6a1c821cfc66b3d81 Mon Sep 17 00:00:00 2001 From: Peter Wern Date: Tue, 11 Nov 2025 15:54:28 +0100 Subject: [PATCH 14/17] CHG: adapted order of docker layers, because we expect libfranka/franka_ros2 to get updated more frequently --- ros2/.devcontainer/Dockerfile | 62 +++++++++++++++++------------------ 1 file changed, 31 insertions(+), 31 deletions(-) diff --git a/ros2/.devcontainer/Dockerfile b/ros2/.devcontainer/Dockerfile index 08999770..1eba4b28 100644 --- a/ros2/.devcontainer/Dockerfile +++ b/ros2/.devcontainer/Dockerfile @@ -16,36 +16,6 @@ RUN apt-get update && apt-get install -y \ python3-vcstool \ && rm -rf /var/lib/apt/lists/* -# Install libfranka -ARG LIBFRANKA_VERSION=0.17.0 -RUN /bin/bash -c "source /opt/ros/humble/setup.bash && \ - git clone --recursive https://github.com/frankarobotics/libfranka --branch ${LIBFRANKA_VERSION} \ - && cd libfranka \ - && mkdir build \ - && cd build \ - && cmake -DCMAKE_BUILD_TYPE=Release -DBUILD_TESTS=OFF .. \ - && cmake --build . -j $(nproc) \ - && cpack -G DEB \ - && dpkg -i libfranka*.deb \ - && cd ../../ && rm -r libfranka" - -# Install franka_ros2 -ARG FRANKA_ROS2_VERSION=v2.0.4 -ARG FRANKA_DESCRIPTION_VERSION=1.0.2 -RUN /bin/bash -c 'source /opt/ros/humble/setup.bash && \ - mkdir -p /tmp/franka_ros2 && cd /tmp/franka_ros2 && \ - git clone --recursive https://github.com/frankarobotics/franka_ros2.git --branch ${FRANKA_ROS2_VERSION} && \ - git clone --recursive https://github.com/frankarobotics/franka_description.git --branch ${FRANKA_DESCRIPTION_VERSION} && \ - apt-get update && \ - rosdep update && \ - rosdep install --from-paths . --ignore-src -r -y && \ - rm -rf /var/lib/apt/lists/* && \ - colcon build --install-base /opt/ros/humble/franka --cmake-args -DCMAKE_BUILD_TYPE=Release -DBUILD_TESTING=off && \ - cd .. && \ - rm -rf /tmp/franka_ros2 && \ - echo "source /opt/ros/humble/franka/setup.bash" >> ~/.bashrc && \ - echo "source /opt/ros/humble/franka/setup.sh" >> ~/.profile' - # Install robotiq gripper ARG ROS2_ROBOTIQ_GRIPPER_COMMIT_HASH="2ff85455d4b9f973c4b0bab1ce95fb09367f0d26" RUN /bin/bash -c "source /opt/ros/humble/setup.bash && \ @@ -74,4 +44,34 @@ RUN /bin/bash -c "source /opt/ros/humble/setup.bash && \ colcon build --install-base /opt/ros/humble/franka --cmake-args -DCMAKE_BUILD_TYPE=Release && \ cd .. && \ rm -rf /tmp/rqt_reconfigure && \ - rm -rf /var/lib/apt/lists/*" \ No newline at end of file + rm -rf /var/lib/apt/lists/*" + +# Install libfranka +ARG LIBFRANKA_VERSION=0.17.0 +RUN /bin/bash -c "source /opt/ros/humble/setup.bash && \ + git clone --recursive https://github.com/frankarobotics/libfranka --branch ${LIBFRANKA_VERSION} \ + && cd libfranka \ + && mkdir build \ + && cd build \ + && cmake -DCMAKE_BUILD_TYPE=Release -DBUILD_TESTS=OFF .. \ + && cmake --build . -j $(nproc) \ + && cpack -G DEB \ + && dpkg -i libfranka*.deb \ + && cd ../../ && rm -r libfranka" + +# Install franka_ros2 +ARG FRANKA_ROS2_VERSION=v2.0.4 +ARG FRANKA_DESCRIPTION_VERSION=1.0.2 +RUN /bin/bash -c 'source /opt/ros/humble/setup.bash && \ + mkdir -p /tmp/franka_ros2 && cd /tmp/franka_ros2 && \ + git clone --recursive https://github.com/frankarobotics/franka_ros2.git --branch ${FRANKA_ROS2_VERSION} && \ + git clone --recursive https://github.com/frankarobotics/franka_description.git --branch ${FRANKA_DESCRIPTION_VERSION} && \ + apt-get update && \ + rosdep update && \ + rosdep install --from-paths . --ignore-src -r -y && \ + rm -rf /var/lib/apt/lists/* && \ + colcon build --install-base /opt/ros/humble/franka --cmake-args -DCMAKE_BUILD_TYPE=Release -DBUILD_TESTING=off && \ + cd .. && \ + rm -rf /tmp/franka_ros2 && \ + echo "source /opt/ros/humble/franka/setup.bash" >> ~/.bashrc && \ + echo "source /opt/ros/humble/franka/setup.sh" >> ~/.profile' \ No newline at end of file From efee8e96a7fbef7069f2bacad9361fbf5e634487 Mon Sep 17 00:00:00 2001 From: Peter Wern Date: Fri, 14 Nov 2025 12:49:29 +0000 Subject: [PATCH 15/17] CHG: remove main fuction from driver --- .../dynamixel/driver.py | 25 ------------------- 1 file changed, 25 deletions(-) diff --git a/ros2/src/franka_gello_state_publisher/franka_gello_state_publisher/dynamixel/driver.py b/ros2/src/franka_gello_state_publisher/franka_gello_state_publisher/dynamixel/driver.py index 668b3fbb..0c53b927 100644 --- a/ros2/src/franka_gello_state_publisher/franka_gello_state_publisher/dynamixel/driver.py +++ b/ros2/src/franka_gello_state_publisher/franka_gello_state_publisher/dynamixel/driver.py @@ -344,28 +344,3 @@ 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 main(): - # Set the port, baudrate, and servo IDs - ids = [1] - - # Create a DynamixelDriver instance - driver = DynamixelDriver(ids) - - # Test setting torque mode - driver.write_value_by_name("torque_enable", [1] * len(ids)) - driver.write_value_by_name("torque_enable", [0] * len(ids)) - - # Test reading the joint angles - try: - while True: - joint_angles = driver.get_joints() - print(f"Joint angles for IDs {ids}: {joint_angles}") - # print(f"Joint angles for IDs {ids[1]}: {joint_angles[1]}") - except KeyboardInterrupt: - driver.close() - - -if __name__ == "__main__": - main() # Test the driver From a308238e23862682ab2ce85bacc10a09ab371f24 Mon Sep 17 00:00:00 2001 From: Peter Wern Date: Fri, 14 Nov 2025 12:50:28 +0000 Subject: [PATCH 16/17] FIX: typo in comment --- .../src/joint_impedance_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ros2/src/franka_fr3_arm_controllers/src/joint_impedance_controller.cpp b/ros2/src/franka_fr3_arm_controllers/src/joint_impedance_controller.cpp index 2fd49adc..f7f139ed 100644 --- a/ros2/src/franka_fr3_arm_controllers/src/joint_impedance_controller.cpp +++ b/ros2/src/franka_fr3_arm_controllers/src/joint_impedance_controller.cpp @@ -246,7 +246,7 @@ void JointImpedanceController::updateJointStates_() { bool JointImpedanceController::initializeMotionGenerator_() { if (!gello_position_values_valid_) { - // Only send a warning once every 10 secondsto not spam the log + // Only send a warning once every 10 seconds in order not to spam the log RCLCPP_WARN_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 10 * 1000, "Waiting for valid joint states..."); return false; From 21cf3bd4d8c75f556c69cc818587cfb9a0eca513 Mon Sep 17 00:00:00 2001 From: Peter Wern Date: Fri, 14 Nov 2025 14:41:05 +0000 Subject: [PATCH 17/17] FIX: move PyYAML dependency --- requirements.txt | 1 - ros2/requirements.txt | 1 + 2 files changed, 1 insertion(+), 1 deletion(-) diff --git a/requirements.txt b/requirements.txt index 137e29b3..173fce61 100644 --- a/requirements.txt +++ b/requirements.txt @@ -10,7 +10,6 @@ pyquaternion pyrealsense2 pyzmq pure-python-adb -PyYAML quaternion setuptools tyro diff --git a/ros2/requirements.txt b/ros2/requirements.txt index 16b0ef50..001b4f2f 100644 --- a/ros2/requirements.txt +++ b/ros2/requirements.txt @@ -1,2 +1,3 @@ dynamixel_sdk +PyYAML tyro \ No newline at end of file