diff --git a/README.md b/README.md index cb683d2..814c2fa 100644 --- a/README.md +++ b/README.md @@ -67,11 +67,14 @@ Predefined configs are provided under `config/`: - `robot.robot_ip` — xArm controller IP (e.g. `192.168.1.245`) - `robot.robot_dof` — `7` -- `robot.gripper_type` — `1` for the xArm gripper +- `robot.gripper_type` — `2` for the xArm Gripper G2 +- `robot.gripper_speed` — G2 opening/closing speed in mm/s (`15`–`225`; configured as `100`) +- `robot.gripper_force` — G2 gripping force (`1`–`100`; configured as `50`) - `teleop.port` — GELLO serial port (`/dev/serial/by-id/...`) - `teleop.joint_ids` / `teleop.joint_signs` — per-arm servo mapping and direction - `teleop.start_joints` — GELLO calibration reference, should match the xArm SDK initial point (degrees) - `teleop.gripper_id` — GELLO gripper servo ID (`8`; `-1` disables it) +- `teleop.gripper_open_deg` / `teleop.gripper_close_deg` — GELLO servo calibration angles; these are independent of the G2's 0–84 mm opening range - `teleop.realtime_control_fps` — independent GELLO-to-xArm command loop rate; it is separate from `dataset.fps` - `dataset.root` / `dataset.repo_id` — where the dataset is stored - `dataset.single_task` — task description saved with each frame diff --git a/README_ZH.md b/README_ZH.md index 033d09d..bdeedbd 100644 --- a/README_ZH.md +++ b/README_ZH.md @@ -67,11 +67,14 @@ ls /dev/serial/by-id/ - `robot.robot_ip` — xArm 控制器 IP（如 `192.168.1.245`） - `robot.robot_dof` — `7` -- `robot.gripper_type` — `1` 表示 xArm 夹爪 +- `robot.gripper_type` — `2` 表示 xArm Gripper G2 +- `robot.gripper_speed` — G2 开合速度，单位 mm/s（范围 `15`–`225`，当前配置为 `100`） +- `robot.gripper_force` — G2 夹持力（范围 `1`–`100`，当前配置为 `50`） - `teleop.port` — GELLO 串口路径（`/dev/serial/by-id/...`） - `teleop.joint_ids` / `teleop.joint_signs` — 各型号机械臂的舵机映射与方向 - `teleop.start_joints` — GELLO 校准参考值（角度），应与 xArm SDK 初始点一致 - `teleop.gripper_id` — GELLO 夹爪舵机 ID（`8`；`-1` 表示无夹爪） +- `teleop.gripper_open_deg` / `teleop.gripper_close_deg` — GELLO 舵机的开闭标定角，与 G2 的 0–84 mm 行程相互独立 - `teleop.realtime_control_fps` — GELLO 到 xArm 的独立实时控制频率，与 `dataset.fps` 分开 - `dataset.root` / `dataset.repo_id` — 数据集保存位置 - `dataset.single_task` — 随每一帧保存的任务描述 diff --git a/config/gello/xarm5_gello_record_config.yaml b/config/gello/xarm5_gello_record_config.yaml deleted file mode 100644 index 2e31ebf..0000000 --- a/config/gello/xarm5_gello_record_config.yaml +++ /dev/null @@ -1,28 +0,0 @@ -robot: - type: uf::robot - id: "uf_robot" - robot_dof: 5 - control_space: "joint" - robot_ip: "192.168.1.245" - gripper_type: 1 - enable_logs: false - -# make sure to edit with your correct configurations! -teleop: - type: uf::gello_teleop - realtime_control_fps: 60 - port: "/dev/serial/by-id/usb-FTDI_USB__-__Serial_Converter_FTAJZYC7-if00-port0" - joint_ids: [1, 2, 4, 6, 7] - joint_signs: [1, 1, -1, 1, 1] - start_joints: [0, 0, -90, 90, 0] - gripper_id: 8 - -dataset: - # Dataset path relative to the directory where the command is started. - root: "datasets/xarm5_gello_datas" - repo_id: "ufactory/xarm5_gello_datas" - single_task: "Pick up the purple grape and drop into the box on the left." - fps: 30 - episode_time_s: 60 # max duration for one episode - reset_time_s: 20 # time for resetting env between episodes - push_to_hub: False diff --git a/config/gello/xarm6_gello_record_config.yaml b/config/gello/xarm6_gello_record_config.yaml deleted file mode 100644 index 7eee0fb..0000000 --- a/config/gello/xarm6_gello_record_config.yaml +++ /dev/null @@ -1,28 +0,0 @@ -robot: - type: uf::robot - id: "uf_robot" - robot_dof: 6 - control_space: "joint" - robot_ip: "192.168.1.245" - gripper_type: 1 - enable_logs: false - -# make sure to edit with your correct configurations! -teleop: - type: uf::gello_teleop - realtime_control_fps: 60 - port: "/dev/serial/by-id/usb-FTDI_USB__-__Serial_Converter_FTAJZYC7-if00-port0" - joint_ids: [1, 2, 4, 5, 6, 7] - joint_signs: [1, 1, -1, 1, 1, 1] - start_joints: [0, 0, -90, 0, 90, 0] - gripper_id: 8 - -dataset: - # Dataset path relative to the directory where the command is started. - root: "datasets/xarm6_gello_datas" - repo_id: "ufactory/xarm6_gello_datas" - single_task: "Pick up the purple grape and drop into the box on the left." - fps: 30 - episode_time_s: 60 # max duration for one episode - reset_time_s: 20 # time for resetting env between episodes - push_to_hub: False diff --git a/config/gello/xarm7_gello_cartesian_record_config.yaml b/config/gello/xarm7_gello_cartesian_record_config.yaml index 48a7947..45de406 100644 --- a/config/gello/xarm7_gello_cartesian_record_config.yaml +++ b/config/gello/xarm7_gello_cartesian_record_config.yaml @@ -4,7 +4,12 @@ robot: robot_dof: 7 control_space: "cartesian" robot_ip: "192.168.1.245" - gripper_type: 1 + # 2: xArm Gripper G2 (0-84 mm opening range). + gripper_type: 2 + # xArm Gripper G2 speed, 15-225 mm/s; 100 is the SDK default. + gripper_speed: 100 + # xArm Gripper G2 gripping force, 1-100 percent; 50 is the SDK default. + gripper_force: 50 max_linear_velocity: 200 min_tcp_z_mm: -2.0 gripper_error_log_path: "logs/xarm7_gripper_errors.log" diff --git a/config/gello/xarm7_gello_pick_potato_chips.yaml b/config/gello/xarm7_gello_pick_potato_chips.yaml new file mode 100644 index 0000000..713a8a4 --- /dev/null +++ b/config/gello/xarm7_gello_pick_potato_chips.yaml @@ -0,0 +1,76 @@ +robot: + type: uf::robot + id: "uf_robot" + robot_dof: 7 + control_space: "joint" + robot_ip: "192.168.1.245" + # 2: xArm Gripper G2 (0-84 mm opening range). + gripper_type: 2 + enable_logs: false + # Up to 60 Hz goal updates; unchanged targets are filtered below. + gripper_command_interval_s: 0.0166667 + # xArm Gripper G2 speed, 15-225 mm/s; 100 is the SDK default. + gripper_speed: 50 + # xArm Gripper G2 gripping force, 1-100 percent; 50 is the SDK default. + gripper_force: 1 + # Use the high-frequency servo interface for lower-latency GELLO tracking. + joint_command_mode: 1 + max_joint_velocity: 120 + # TCP z floor in the xArm base coordinate system (mm). + min_tcp_z_mm: -2.0 + # CPU-local FK/Jacobian projection keeps ServoJ free of synchronous SDK queries. + tcp_z_guard_backend: "local_projection" + tcp_z_soft_margin_mm: 0.5 + local_kinematics_max_error_mm: 2.0 + controller_safety_boundary: true + # Append gripper initialization/read/write failures here. + gripper_error_log_path: "logs/xarm7_gripper_errors.log" + cameras: + camera: + type: intelrealsense + serial_number_or_name: "242622070583" + width: 640 + height: 480 + fps: 30 + camera2: + type: intelrealsense + serial_number_or_name: "148522072685" + width: 640 + height: 480 + fps: 30 + # Redundant args, indicating the initial pose of xarm7. Set by 192.168.1.245:18333 + # start_joints: [0, -30, 0, 0, 0, 30, 0] +# make sure to edit with your correct configurations! +teleop: + type: uf::gello_teleop + id: "gello_teleop" + # Independent GELLO -> xArm command loop; dataset.fps remains the recording rate. + realtime_control_fps: 60 + port: "/dev/serial/by-id/usb-FTDI_USB__-__Serial_Converter_FTB9HYVD-if00-port0" + joint_ids: [1, 2, 3, 4, 5, 6, 7] + joint_signs: [1, 1, 1, 1, 1, 1, 1] + gripper_id: 8 + gripper_open_deg: 198.28125 + gripper_close_deg: 155.75 + +# Lossy browser preview of the frames already captured for the dataset. +# JPEG encoding and network I/O run in the background; preview frames are +# dropped instead of delaying the 30 Hz recorder or 60 Hz GELLO control loop. +web_preview: + enabled: false + host: "0.0.0.0" + port: 8765 + fps: 8 + width: 480 + jpeg_quality: 65 + +dataset: + # Dataset path relative to the directory where the command is started. + root: "datasets/xarm7_gello_pick_potato_chips" + repo_id: "ufactory/xarm7_gello_pick_potato_chips" + single_task: "Pick up the potato chips and drop." + # Cameras deliver 30 Hz; arm ServoJ remains independently fixed at 60 Hz. + fps: 30 + episode_time_s: 60 # max duration for one episode + reset_time_s: 20 # time for resetting env between episodes + push_to_hub: False diff --git a/config/gello/xarm7_gello_record_config.yaml b/config/gello/xarm7_gello_record_config.yaml index b693487..a93d484 100644 --- a/config/gello/xarm7_gello_record_config.yaml +++ b/config/gello/xarm7_gello_record_config.yaml @@ -4,13 +4,15 @@ robot: robot_dof: 7 control_space: "joint" robot_ip: "192.168.1.245" - gripper_type: 1 + # 2: xArm Gripper G2 (0-84 mm opening range). + gripper_type: 2 enable_logs: false - # Up to 60 Hz goal updates; the first command after idle is immediate. - # Keeping the gripper below its 5000 maximum speed avoids C19 in testing. + # Up to 60 Hz goal updates; unchanged targets are filtered below. gripper_command_interval_s: 0.0166667 - # Avoid the previous maximum-speed (5000) default on the tool RS485 device. - gripper_speed: 1500 + # xArm Gripper G2 speed, 15-225 mm/s; 100 is the SDK default. + gripper_speed: 100 + # xArm Gripper G2 gripping force, 1-100 percent; 50 is the SDK default. + gripper_force: 50 # Use the high-frequency servo interface for lower-latency GELLO tracking. joint_command_mode: 1 max_joint_velocity: 120 @@ -64,8 +66,8 @@ web_preview: dataset: # Dataset path relative to the directory where the command is started. - root: "datasets/xarm7_gello_joint_safe_datas" - repo_id: "ufactory/xarm7_gello_joint_safe_datas" + root: "datasets/xarm7_gello_joint_safe_datas1" + repo_id: "ufactory/xarm7_gello_joint_safe_datas1" single_task: "Pick up the purple grape and drop into the box on the left." # Cameras deliver 30 Hz; arm ServoJ remains independently fixed at 60 Hz. fps: 30 diff --git a/config/gello/xarm7_gello_test.yaml b/config/gello/xarm7_gello_test.yaml new file mode 100644 index 0000000..66741f4 --- /dev/null +++ b/config/gello/xarm7_gello_test.yaml @@ -0,0 +1,76 @@ +robot: + type: uf::robot + id: "uf_robot" + robot_dof: 7 + control_space: "joint" + robot_ip: "192.168.1.245" + # 2: xArm Gripper G2 (0-84 mm opening range). + gripper_type: 2 + enable_logs: false + # Up to 60 Hz goal updates; unchanged targets are filtered below. + gripper_command_interval_s: 0.0166667 + # xArm Gripper G2 speed, 15-225 mm/s; 100 is the SDK default. + gripper_speed: 50 + # xArm Gripper G2 gripping force, 1-100 percent; 50 is the SDK default. + gripper_force: 1 + # Use the high-frequency servo interface for lower-latency GELLO tracking. + joint_command_mode: 1 + max_joint_velocity: 120 + # TCP z floor in the xArm base coordinate system (mm). + min_tcp_z_mm: -2.0 + # CPU-local FK/Jacobian projection keeps ServoJ free of synchronous SDK queries. + tcp_z_guard_backend: "local_projection" + tcp_z_soft_margin_mm: 0.5 + local_kinematics_max_error_mm: 2.0 + controller_safety_boundary: true + # Append gripper initialization/read/write failures here. + gripper_error_log_path: "logs/xarm7_gripper_errors.log" + cameras: + camera: + type: intelrealsense + serial_number_or_name: "242622070583" + width: 640 + height: 480 + fps: 30 + camera2: + type: intelrealsense + serial_number_or_name: "148522072685" + width: 640 + height: 480 + fps: 30 + # Redundant args, indicating the initial pose of xarm7. Set by 192.168.1.245:18333 + # start_joints: [0, -30, 0, 0, 0, 30, 0] +# make sure to edit with your correct configurations! +teleop: + type: uf::gello_teleop + id: "gello_teleop" + # Independent GELLO -> xArm command loop; dataset.fps remains the recording rate. + realtime_control_fps: 60 + port: "/dev/serial/by-id/usb-FTDI_USB__-__Serial_Converter_FTB9HYVD-if00-port0" + joint_ids: [1, 2, 3, 4, 5, 6, 7] + joint_signs: [1, 1, 1, 1, 1, 1, 1] + gripper_id: 8 + gripper_open_deg: 198.28125 + gripper_close_deg: 155.75 + +# Lossy browser preview of the frames already captured for the dataset. +# JPEG encoding and network I/O run in the background; preview frames are +# dropped instead of delaying the 30 Hz recorder or 60 Hz GELLO control loop. +web_preview: + enabled: false + host: "0.0.0.0" + port: 8765 + fps: 8 + width: 480 + jpeg_quality: 65 + +dataset: + # Dataset path relative to the directory where the command is started. + root: "datasets/xarm7_gello_test" + repo_id: "ufactory/xarm7_gello_test" + single_task: "Test episode for GELLO control." + # Cameras deliver 30 Hz; arm ServoJ remains independently fixed at 60 Hz. + fps: 30 + episode_time_s: 360 # max duration for one episode + reset_time_s: 20 # time for resetting env between episodes + push_to_hub: False diff --git a/config/manual_mode/xarm7_manual_record_config.yaml b/config/manual_mode/xarm7_manual_record_config.yaml index fa18c96..a70832e 100644 --- a/config/manual_mode/xarm7_manual_record_config.yaml +++ b/config/manual_mode/xarm7_manual_record_config.yaml @@ -6,7 +6,9 @@ robot: # IP address of the xArm controller. robot_ip: "192.168.1.245" # Gripper type: 1 is the xArm gripper. - gripper_type: 1 + gripper_type: 2 + # xArm Gripper G2 gripping force, 1-100 percent; 50 is the SDK default. + gripper_force: 50 enable_logs: false manual_mode: true # Normalized gripper position change per second while holding C/O. diff --git a/config/pika/xarm7_pika_record_config.yaml b/config/pika/xarm7_pika_record_config.yaml index 51b648a..d7eeaf0 100644 --- a/config/pika/xarm7_pika_record_config.yaml +++ b/config/pika/xarm7_pika_record_config.yaml @@ -5,6 +5,8 @@ robot: control_space: "cartesian" robot_ip: "192.168.1.85" gripper_type: 10 + # xArm Gripper G2 gripping force, 1-100 percent; 50 is the SDK default. + gripper_force: 50 enable_logs: false max_linear_velocity: 200 cameras: diff --git a/config/spacemouse/xarm7_spacemouse_record_config.yaml b/config/spacemouse/xarm7_spacemouse_record_config.yaml index d1fae3d..88ff343 100644 --- a/config/spacemouse/xarm7_spacemouse_record_config.yaml +++ b/config/spacemouse/xarm7_spacemouse_record_config.yaml @@ -5,6 +5,8 @@ robot: control_space: "cartesian" robot_ip: "192.168.1.127" gripper_type: 0 + # xArm Gripper G2 gripping force, 1-100 percent; 50 is the SDK default. + gripper_force: 50 enable_logs: false cameras: diff --git a/config/umi/dual_xarm6_umi_record_config.yaml b/config/umi/dual_xarm6_umi_record_config.yaml index 18f2943..65ae3ab 100644 --- a/config/umi/dual_xarm6_umi_record_config.yaml +++ b/config/umi/dual_xarm6_umi_record_config.yaml @@ -12,6 +12,8 @@ robot: control_space: "cartesian" robot_ip: "192.168.1.29" gripper_type: 2 + # xArm Gripper G2 gripping force, 1-100 percent; 50 is the SDK default. + gripper_force: 50 enable_logs: false max_linear_velocity: 200 cameras: @@ -29,6 +31,8 @@ robot: control_space: "cartesian" robot_ip: "192.168.1.83" gripper_type: 2 + # xArm Gripper G2 gripping force, 1-100 percent; 50 is the SDK default. + gripper_force: 50 enable_logs: false max_linear_velocity: 200 cameras: diff --git a/config/umi/xarm6_umi_record_config.yaml b/config/umi/xarm6_umi_record_config.yaml index afc0a93..e22c7da 100644 --- a/config/umi/xarm6_umi_record_config.yaml +++ b/config/umi/xarm6_umi_record_config.yaml @@ -5,6 +5,8 @@ robot: control_space: "cartesian" robot_ip: "192.168.1.83" gripper_type: 2 + # xArm Gripper G2 gripping force, 1-100 percent; 50 is the SDK default. + gripper_force: 50 enable_logs: false max_linear_velocity: 250 cameras: diff --git a/docs/gello_xarm7_smooth_safe_recording_zh.md b/docs/gello_xarm7_smooth_safe_recording_zh.md index 7c0cc6b..e104c1b 100644 --- a/docs/gello_xarm7_smooth_safe_recording_zh.md +++ b/docs/gello_xarm7_smooth_safe_recording_zh.md @@ -12,7 +12,7 @@ - 控制器 Safety Boundary 仍然开启，作为本地保护之外的最后一道硬保护。 - 相机和数据集按真实的 30 Hz 记录，机械臂控制仍保持独立的 60 Hz。 - 每条 action 带发送时刻；每次 RT joint state 采样也带时刻。写入数据集时，为 state 选择当时已经发送的最近 action，绝不拿“未来的 action”配较早的 state。 -- xArm Gripper 保持 60 Hz 的目标检查能力，但只有变化超过阈值才真正发送；夹爪速度从最大值 5000 降到 1500，解决 Error 19。 +- xArm Gripper G2 使用 SDK 专用接口和物理单位；目标最多按 60 Hz 检查，但只有变化超过阈值才真正发送。初始速度采用 SDK 默认的 100 mm/s。 当前使用的配置是： @@ -28,7 +28,9 @@ robot: gripper_command_interval_s: 0.0166667 gripper_command_threshold: 0.01 - gripper_speed: 1500 + gripper_type: 2 + gripper_speed: 100 + gripper_force: 50 teleop: realtime_control_fps: 60 @@ -118,7 +120,7 @@ logs/gello_record_sync_<时间>.csv 30 Hz 数据集每帧记录的是该采样时刻有效的 60 Hz 控制命令，这是正常的降采样，不是不同步。 -## 四、夹爪 Error 19 是怎么消除的 +## 四、xArm Gripper G2 配置与 Error 19 排查 现象是只要连续控制夹爪，就出现： @@ -127,18 +129,16 @@ set_rs485_data -> code=1 controller_error=19 ``` -排查过程里先试过降低夹爪命令频率：2 Hz、5 Hz、6.7 Hz 都不报错，但低频带来明显跟手延迟。继续对照实验后发现，真正与故障一致的变量不是频率，而是夹爪速度： +旧配置将实际安装的 G2 错写成了 `gripper_type: 1`，因此走的是老款 xArm Gripper 的 0–800 pulse、50–5000 r/min 参数和 `set_gripper_position` 路径。旧实验中“5000 降到 1500”的结论不适用于 G2，不能继续作为 G2 的速度依据。 -- 报错时使用默认最大速度 5000。 -- 速度降到 1500 后，从 2 Hz 一直提高到 20 Hz 都稳定。 -- 最后恢复到最高 60 Hz，仍然稳定。 - -运行期发送也改成 SDK 专用的非阻塞 `set_gripper_position`，不再手工拼通用 RS485 数据包；持续 ServoJ 时关闭 `wait_motion`，否则 SDK 会等待机械臂停止。 +现在使用 `gripper_type: 2`。G2 的位置范围是 0–84 mm，速度范围是 15–225 mm/s，夹持力范围是 1–100。运行期直接调用 SDK 的非阻塞 `set_gripper_g2_position`，由 SDK 完成几何换算和 Modbus 帧构造；持续 ServoJ 时关闭 `wait_motion`，避免等待机械臂停止。 最终参数为： ```yaml -gripper_speed: 1500 +gripper_type: 2 +gripper_speed: 100 +gripper_force: 50 gripper_command_interval_s: 0.0166667 gripper_command_threshold: 0.01 ``` @@ -151,9 +151,9 @@ gripper_command_threshold: 0.01 logs/xarm7_gripper_errors.log ``` -成功记录包含目标值、脉冲位置、调用耗时和返回码。最终实验中单次调用通常只需要约 1.3–2.2 ms，60 Hz、速度 1500 时没有再次出现 C19。 +成功记录包含归一化目标、G2 开口位置、调用耗时和返回码。由于旧日志来自错误的夹爪类型配置，切换到 G2 后必须重新进行连续开合实机验收，确认没有新增 Error 19。 -如果以后更换夹爪、线缆或固件后 C19 再次出现，应先把速度降下来验证。如果低速也报错，就应该检查腕部线缆、接头、末端供电和末端 IO 板固件，而不是无限降低控制频率。 +如果仍出现 C19，应先降低 G2 的 mm/s 速度并降低发送频率做对照；低速低频仍报错时，再检查腕部线缆、接头、末端供电和末端 IO 板固件。 ## 五、运行和验收 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 e5378af..0bb8252 100644 --- a/src/lerobot_robot_ufactory/robots/uf_robot/uf_robot.py +++ b/src/lerobot_robot_ufactory/robots/uf_robot/uf_robot.py @@ -154,6 +154,9 @@ class UFRobot(Robot, Thread): elif self._gripper_type == GripperType.xArmGripperG2: speed = 225 if self.config.gripper_speed < 0 else min(max(15, self.config.gripper_speed), 225) gripper_speed = int(((speed * 60) / 9.88235 + 140) / 0.4) + # Keep the SDK-facing speed in mm/s. GripperParam.speed retains + # the original low-level register conversion used by this repo. + self._gripper_g2_speed = speed gripper_force = 50 if self.config.gripper_force < 0 else min(max(1, self.config.gripper_force), 100) self._gripper_param = GripperParam('xArmGripperG2', open_pos=84, close_pos=0, speed=gripper_speed, force=gripper_force) elif self._gripper_type == GripperType.BioGripperG2: @@ -732,10 +735,19 @@ class UFRobot(Robot, Thread): ), ) elif self._gripper_type == GripperType.xArmGripperG2: - self.real_arm.set_gripper_enable(True) - self.real_arm.set_gripper_mode(0) + 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)) if move_to_open: - self.real_arm.set_gripper_g2_position(self._gripper_param.open_pos) + self._check_gripper_code( + "set_gripper_g2_position", + self.real_arm.set_gripper_g2_position( + self._gripper_param.open_pos, + speed=self._gripper_g2_speed, + force=self._gripper_param.force, + wait=True, + check_baud=False, + ), + ) elif self._gripper_type == GripperType.BioGripperG2: _, mode = self.real_arm.get_bio_gripper_control_mode() if mode != 1: @@ -809,6 +821,9 @@ class UFRobot(Robot, Thread): grippos_norm = self._gripper_param.get_gripper_norm(grippos) elif self._gripper_type == GripperType.xArmGripperG2: code, grippos = self.real_arm.get_gripper_g2_position() + if code != 0 or not isinstance(grippos, (int, float, np.number)): + self._log_gripper_error("get_gripper_g2_position", code, f"position={grippos}") + grippos = None grippos_norm = self._gripper_param.get_gripper_norm(grippos) elif self._gripper_type == GripperType.BioGripperG2: code, grippos = self.real_arm.get_bio_gripper_g2_position() @@ -942,12 +957,15 @@ class UFRobot(Robot, Thread): ) 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) - modbus_datas = [0x08, 0x10, 0x0C, 0x00, 0x00, 0x05, 0x0A, 0x00, 0x01] - 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))) - result = self.real_arm.getset_tgpio_modbus_data(modbus_datas) + result = self.real_arm.set_gripper_g2_position( + grippos, + speed=self._gripper_g2_speed, + force=self._gripper_param.force, + wait=False, + wait_motion=False, + check_baud=False, + check_err=False, + ) elif self._gripper_type == GripperType.BioGripperG2: grippos = self._gripper_param.get_grippos(gripper_norm) grippos = int(grippos * 3.7342 - 265.13) @@ -972,7 +990,7 @@ class UFRobot(Robot, Thread): else None ) if code not in (None, 0): - detail = f"target={gripper_norm:.6f}, pulse={grippos}" + detail = f"target={gripper_norm:.6f}, position={grippos}" if command_dt_ms is not None: detail += f", dt_ms={command_dt_ms:.3f}" self._log_gripper_error( @@ -985,7 +1003,7 @@ class UFRobot(Robot, Thread): self._log_gripper_command(gripper_norm, grippos, command_dt_ms) self._last_gripper_command = gripper_norm - def _log_gripper_command(self, target: float, pulse: int, dt_ms: float) -> None: + def _log_gripper_command(self, target: float, position: int, dt_ms: float) -> None: log_path = self.config.gripper_error_log_path if not log_path: return @@ -996,7 +1014,7 @@ class UFRobot(Robot, Thread): with path.open("a", encoding="utf-8") as stream: stream.write( f"{timestamp} gripper command: target={target:.6f}, " - f"pulse={pulse}, dt_ms={dt_ms:.3f}, code=0\n" + f"position={position}, dt_ms={dt_ms:.3f}, code=0\n" ) except OSError: logging.exception("Failed to write gripper command log to %s", log_path) 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 09a8eb2..945d495 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 @@ -59,6 +59,11 @@ class UFRobotConfig(RobotConfig): raise ValueError("gripper_command_threshold must be between 0 and 1") if not math.isfinite(self.gripper_command_interval_s) or self.gripper_command_interval_s < 0: raise ValueError("gripper_command_interval_s must be finite and non-negative") + if self.gripper_type == 2: + if self.gripper_speed != -1 and not 15 <= self.gripper_speed <= 225: + raise ValueError("xArm Gripper G2 gripper_speed must be -1 or between 15 and 225 mm/s") + if self.gripper_force != -1 and not 1 <= self.gripper_force <= 100: + raise ValueError("xArm Gripper G2 gripper_force must be -1 or between 1 and 100") 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") if self.min_tcp_z_mm is not None and not math.isfinite(self.min_tcp_z_mm): diff --git a/tests/test_manual_mode.py b/tests/test_manual_mode.py index b6e6693..538da82 100644 --- a/tests/test_manual_mode.py +++ b/tests/test_manual_mode.py @@ -4,6 +4,7 @@ from types import SimpleNamespace import numpy as np import pytest +import yaml from lerobot.datasets.lerobot_dataset import LeRobotDataset from lerobot_robot_ufactory.robots.uf_robot.uf_robot_config import UFRobotConfig @@ -28,6 +29,7 @@ class FakeXArm: self.initial_point = [0.0, -30.0, 0.0, 0.0, 0.0, 30.0] self._arm = type("FakeArmTransport", (), {"_baud_checkset": False})() self.gripper_position = 800 + self.gripper_g2_position = 84 self.calls = [] def motion_enable(self, **kwargs): @@ -85,6 +87,15 @@ class FakeXArm: self.calls.append(("get_gripper_position",)) return 0, self.gripper_position + def set_gripper_g2_position(self, position, **kwargs): + self.calls.append(("set_gripper_g2_position", position, kwargs)) + self.gripper_g2_position = position + return 0 + + def get_gripper_g2_position(self): + self.calls.append(("get_gripper_g2_position",)) + return 0, self.gripper_g2_position + def getset_tgpio_modbus_data(self, data): self.calls.append(("getset_tgpio_modbus_data", data)) return 0, [] @@ -263,6 +274,100 @@ def test_gripper_rs485_commands_are_rate_limited(monkeypatch, tmp_path): robot.disconnect() +def test_xarm_gripper_g2_uses_sdk_units_and_dedicated_api(monkeypatch, tmp_path, caplog): + 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_g2", + calibration_dir=tmp_path, + robot_ip=arm.robot_ip, + robot_dof=6, + control_space="joint", + gripper_type=2, + gripper_speed=100, + gripper_force=50, + gripper_command_interval_s=0.0, + gripper_error_log_path=None, + ) + robot = uf_robot_module.UFRobot(config) + assert robot._gripper_g2_speed == 100 + assert robot._gripper_param.speed == int(((100 * 60) / 9.88235 + 140) / 0.4) + robot.connect() + + g2_writes = [call for call in arm.calls if call[0] == "set_gripper_g2_position"] + assert g2_writes == [ + ( + "set_gripper_g2_position", + 84, + { + "speed": 100, + "force": 50, + "wait": True, + "check_baud": False, + }, + ) + ] + + robot._send_gripper_action(0.5) + assert [call for call in arm.calls if call[0] == "set_gripper_g2_position"][-1] == ( + "set_gripper_g2_position", + 42, + { + "speed": 100, + "force": 50, + "wait": False, + "wait_motion": False, + "check_baud": False, + "check_err": False, + }, + ) + assert not any(call[0] == "getset_tgpio_modbus_data" for call in arm.calls) + + observation = robot.get_observation() + assert observation["gripper.pos"] == pytest.approx(0.5) + + arm.get_gripper_g2_position = lambda: (1, None) + observation = robot.get_observation() + assert observation["gripper.pos"] == pytest.approx(0.5) + assert "get_gripper_g2_position" in caplog.text + robot.disconnect() + + +def test_xarm_gripper_g2_rejects_legacy_speed_and_invalid_force(tmp_path): + with pytest.raises(ValueError, match="15 and 225 mm/s"): + UFRobotConfig( + id="test_gripper_g2_speed", + calibration_dir=tmp_path, + robot_dof=6, + gripper_type=2, + gripper_speed=1500, + ) + + with pytest.raises(ValueError, match="between 1 and 100"): + UFRobotConfig( + id="test_gripper_g2_force", + calibration_dir=tmp_path, + robot_dof=6, + gripper_type=2, + gripper_force=0, + ) + + +def test_gello_configs_select_xarm_gripper_g2(): + config_dir = Path("config/gello") + config_paths = sorted(config_dir.glob("*.yaml")) + assert config_paths + + for config_path in config_paths: + config = yaml.safe_load(config_path.read_text(encoding="utf-8")) + assert config["robot"]["gripper_type"] == 2, config_path + assert 15 <= config["robot"]["gripper_speed"] <= 225, config_path + assert 1 <= config["robot"]["gripper_force"] <= 100, config_path + + def test_manual_mode_config_rejects_cartesian_control(tmp_path): with pytest.raises(ValueError, match="control_space='joint'"): UFRobotConfig(