From e64b46447efe0af16a1bf00f8bb5c9f6ec806fde Mon Sep 17 00:00:00 2001 From: ChenYuhan <2514158309@qq.com> Date: Tue, 11 Aug 2026 17:04:07 +0800 Subject: [PATCH] Improve xArm gripper communication handling --- config/gello/xarm7_gello_record_config.yaml | 4 +- rules/99-ftdi-serial.rules | 2 + .../robots/uf_robot/uf_robot.py | 73 +++++++++++++++++-- .../robots/uf_robot/uf_robot_config.py | 4 + tests/test_manual_mode.py | 29 ++++++++ 5 files changed, 103 insertions(+), 9 deletions(-) create mode 100644 rules/99-ftdi-serial.rules diff --git a/config/gello/xarm7_gello_record_config.yaml b/config/gello/xarm7_gello_record_config.yaml index c9d9fc4..8feb30c 100644 --- a/config/gello/xarm7_gello_record_config.yaml +++ b/config/gello/xarm7_gello_record_config.yaml @@ -5,6 +5,8 @@ robot: control_space: "joint" robot_ip: "192.168.1.245" gripper_type: 1 + # Append gripper initialization/read/write failures here. + gripper_error_log_path: "logs/xarm7_gripper_errors.log" # Redundant args, indicating the initial pose of xarm7. Set by 192.168.1.245:18333 # start_joints: [0, -30, 0, 0, 0, 30, 0] @@ -21,7 +23,7 @@ teleop: dataset: # root of local repo: /home//.cache/huggingface/lerobot (default) - root: "/home/uf/Data/lerobot_datas/record/ufactory/xarm6_gello_datas" + root: "/home/user/projects/lerobot_xarm7/datasets" repo_id: "ufactory/xarm6_gello_datas" single_task: "Pick up the purple grape and drop into the box on the left." fps: 30 diff --git a/rules/99-ftdi-serial.rules b/rules/99-ftdi-serial.rules new file mode 100644 index 0000000..d4d3f74 --- /dev/null +++ b/rules/99-ftdi-serial.rules @@ -0,0 +1,2 @@ +# FTDI FT232H (GELLO 示教臂串口) — 自动授予读写权限 +KERNEL=="ttyUSB*", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6014", MODE:="0666", SYMLINK+="ttyFTDI" diff --git a/src/lerobot_robot_ufactory/robots/uf_robot/uf_robot.py b/src/lerobot_robot_ufactory/robots/uf_robot/uf_robot.py index b4a4e0b..b5101de 100644 --- a/src/lerobot_robot_ufactory/robots/uf_robot/uf_robot.py +++ b/src/lerobot_robot_ufactory/robots/uf_robot/uf_robot.py @@ -5,8 +5,10 @@ import math import logging import struct import numpy as np +from datetime import datetime from enum import IntEnum from dataclasses import dataclass +from pathlib import Path from threading import Thread, Event, Lock from lerobot.robots import Robot from lerobot.cameras.utils import make_cameras_from_configs @@ -88,6 +90,8 @@ class UFRobot(Robot, Thread): self.logs = {} self._cmd_cnt = 0 + self._last_gripper_command = None + self._last_logged_controller_error = 0 self._max_joint_velocity = math.radians(self.config.max_joint_velocity) self._max_linear_velocity = self.config.max_linear_velocity @@ -302,11 +306,14 @@ class UFRobot(Robot, Thread): self.real_arm._arm._baud_checkset = True try: if self._gripper_type == GripperType.xArmGripper: - self.real_arm.set_gripper_enable(True) - self.real_arm.set_gripper_mode(0) - self.real_arm.set_gripper_speed(self._gripper_param.speed) + self._check_gripper_code("set_gripper_enable", self.real_arm.set_gripper_enable(True)) + self._check_gripper_code("set_gripper_mode", self.real_arm.set_gripper_mode(0)) + self._check_gripper_code("set_gripper_speed", self.real_arm.set_gripper_speed(self._gripper_param.speed)) if move_to_open: - self.real_arm.set_gripper_position(self._gripper_param.open_pos) + self._check_gripper_code( + "set_gripper_position", + self.real_arm.set_gripper_position(self._gripper_param.open_pos), + ) elif self._gripper_type == GripperType.xArmGripperG2: self.real_arm.set_gripper_enable(True) self.real_arm.set_gripper_mode(0) @@ -346,6 +353,7 @@ class UFRobot(Robot, Thread): def get_observation(self) -> dict[str, np.ndarray]: obs_dict = {} + self._log_controller_error_if_changed("get_observation") # Read Stretch state before_read_t = time.perf_counter() @@ -377,6 +385,8 @@ class UFRobot(Robot, Thread): if self._gripper_type > GripperType.NoGripper: if self._gripper_type == GripperType.xArmGripper: code, grippos = self.real_arm.get_gripper_position() + if code != 0 or grippos is None: + self._log_gripper_error("get_gripper_position", code, f"position={grippos}") grippos_norm = self._gripper_param.get_gripper_norm(grippos) elif self._gripper_type == GripperType.xArmGripperG2: code, grippos = self.real_arm.get_gripper_g2_position() @@ -411,11 +421,18 @@ class UFRobot(Robot, Thread): def _send_gripper_action(self, gripper_norm: float) -> None: gripper_norm = min(max(float(gripper_norm), 0.0), 1.0) + if ( + self._last_gripper_command is not None + and abs(gripper_norm - self._last_gripper_command) + < self.config.gripper_command_threshold + ): + return + if self._gripper_type == GripperType.xArmGripper: grippos = self._gripper_param.get_grippos(gripper_norm) modbus_datas = [0x08, 0x10, 0x07, 0x00, 0x00, 0x02, 0x04] modbus_datas.extend(list(struct.pack('>i', grippos))) - self.real_arm.getset_tgpio_modbus_data(modbus_datas) + result = self.real_arm.getset_tgpio_modbus_data(modbus_datas) elif self._gripper_type == GripperType.xArmGripperG2: grippos = self._gripper_param.get_grippos(gripper_norm) grippos = int((math.degrees(math.asin((grippos - 16) / 110)) + 8.33) * 18.28) @@ -423,7 +440,7 @@ class UFRobot(Robot, Thread): modbus_datas.extend(list(struct.pack('>h', self._gripper_param.speed))) modbus_datas.extend(list(struct.pack('>h', self._gripper_param.force))) modbus_datas.extend(list(struct.pack('>i', grippos))) - self.real_arm.getset_tgpio_modbus_data(modbus_datas) + result = self.real_arm.getset_tgpio_modbus_data(modbus_datas) elif self._gripper_type == GripperType.BioGripperG2: grippos = self._gripper_param.get_grippos(gripper_norm) grippos = int(grippos * 3.7342 - 265.13) @@ -431,14 +448,53 @@ class UFRobot(Robot, Thread): modbus_datas.extend(list(struct.pack('>h', self._gripper_param.speed))) modbus_datas.extend(list(struct.pack('>h', self._gripper_param.force))) modbus_datas.extend(list(struct.pack('>i', grippos))) - self.real_arm.getset_tgpio_modbus_data(modbus_datas) + result = self.real_arm.getset_tgpio_modbus_data(modbus_datas) elif self._gripper_type == GripperType.PikaGripper: grippos = self._gripper_param.get_grippos(gripper_norm) self.pika_gripper.set_gripper_distance(grippos) + result = 0 elif self._gripper_type == GripperType.RobotiqGripper: grippos = self._gripper_param.get_grippos(gripper_norm) modbus_datas = [0x09, 0x10, 0x03, 0xE8, 0x00, 0x03, 0x06, 0x09, 0x00, 0x00, grippos, self._gripper_param.speed, self._gripper_param.force] - self.real_arm.getset_tgpio_modbus_data(modbus_datas) + result = self.real_arm.getset_tgpio_modbus_data(modbus_datas) + + code = result[0] if isinstance(result, (tuple, list)) else result + if code not in (None, 0): + self._log_gripper_error("send_gripper_action", code, f"target={gripper_norm:.6f}") + return + self._last_gripper_command = gripper_norm + + def _log_gripper_error(self, operation: str, code, detail: str = "") -> None: + controller_error = getattr(self.real_arm, "error_code", None) + message = ( + f"gripper communication error: operation={operation}, code={code}, " + f"controller_error={controller_error}, {detail}" + ).rstrip(", ") + logging.error(message) + + log_path = self.config.gripper_error_log_path + if not log_path: + return + try: + path = Path(log_path).expanduser() + path.parent.mkdir(parents=True, exist_ok=True) + timestamp = datetime.now().astimezone().isoformat(timespec="milliseconds") + with path.open("a", encoding="utf-8") as stream: + stream.write(f"{timestamp} {message}\n") + except OSError: + logging.exception("Failed to write gripper error log to %s", log_path) + + def _check_gripper_code(self, operation: str, code) -> None: + if code in (None, 0): + return + self._log_gripper_error(operation, code) + raise RuntimeError(f"{operation} failed, code={code}, {self._motion_status()}") + + def _log_controller_error_if_changed(self, operation: str) -> None: + controller_error = getattr(self.real_arm, "error_code", 0) + if controller_error and controller_error != self._last_logged_controller_error: + self._log_gripper_error(operation, "controller", "controller error became active") + self._last_logged_controller_error = controller_error def _motion_status(self) -> str: """Return controller state details for a failed motion command.""" @@ -457,6 +513,7 @@ class UFRobot(Robot, Thread): def send_action(self, action: dict) -> np.ndarray: if not self._is_connected: raise ConnectionError() + self._log_controller_error_if_changed("send_action") if self.config.manual_mode: gripper_key = f"{self.prefix}gripper.pos" if ( diff --git a/src/lerobot_robot_ufactory/robots/uf_robot/uf_robot_config.py b/src/lerobot_robot_ufactory/robots/uf_robot/uf_robot_config.py index 97b76f3..0b79b96 100644 --- a/src/lerobot_robot_ufactory/robots/uf_robot/uf_robot_config.py +++ b/src/lerobot_robot_ufactory/robots/uf_robot/uf_robot_config.py @@ -16,6 +16,8 @@ class UFRobotConfig(RobotConfig): gripper_port: str = None # only used by pika gripper (gripper_type=10) gripper_speed: int = -1 # auto gripper_force: int = -1 # auto + gripper_command_threshold: float = 0.01 # normalized change required before sending a new command + gripper_error_log_path: str | None = "logs/xarm_gripper_errors.log" observe_joint_vel: bool = False # only effective in joint control mode manual_mode: bool = False # xArm joint teaching mode; records state and optional gripper actions manual_gripper_speed: float = 0.5 # normalized gripper position per second in manual mode @@ -37,5 +39,7 @@ class UFRobotConfig(RobotConfig): raise ValueError("teach_sensitivity must be between 1 and 5") if self.manual_gripper_speed < 0: raise ValueError("manual_gripper_speed must be non-negative") + if not 0 <= self.gripper_command_threshold <= 1: + raise ValueError("gripper_command_threshold must be between 0 and 1") if self.control_space == "joint" and self.joint_command_mode not in (1, 6): raise ValueError("joint_command_mode must be 1 or 6 for joint control") diff --git a/tests/test_manual_mode.py b/tests/test_manual_mode.py index b716312..9ac7d82 100644 --- a/tests/test_manual_mode.py +++ b/tests/test_manual_mode.py @@ -279,6 +279,35 @@ def test_manual_mode_initializes_gripper_without_opening_and_sends_only_gripper( robot.disconnect() +def test_gripper_command_is_only_sent_after_target_changes(monkeypatch, tmp_path): + from lerobot_robot_ufactory.robots.uf_robot import uf_robot as uf_robot_module + + arm = FakeXArm("192.168.1.245") + monkeypatch.setattr(uf_robot_module, "XArmAPI", lambda robot_ip: arm) + monkeypatch.setattr(uf_robot_module.time, "sleep", lambda _: None) + + config = UFRobotConfig( + id="test_gripper_command_threshold", + calibration_dir=tmp_path, + robot_ip=arm.robot_ip, + robot_dof=6, + control_space="joint", + gripper_type=1, + manual_mode=True, + gripper_command_threshold=0.01, + ) + robot = uf_robot_module.UFRobot(config) + robot.connect() + + robot.send_action({"gripper.pos": 0.5}) + robot.send_action({"gripper.pos": 0.505}) + robot.send_action({"gripper.pos": 0.52}) + + writes = [call for call in arm.calls if call[0] == "getset_tgpio_modbus_data"] + assert len(writes) == 2 + robot.disconnect() + + def test_manual_record_config_has_no_teleop(monkeypatch): config_path = Path("config/manual_mode/xarm7_manual_record_config.yaml").resolve() monkeypatch.setattr(