diff --git a/README.md b/README.md index 5fdb4e1..4fe67fc 100644 --- a/README.md +++ b/README.md @@ -167,7 +167,7 @@ Set `manual_mode` to `true` to enter drag mode. Press Enter to restore normal mo ### 3. Manual Drag Data Collection -Manual drag recording uses `manual_mode: true` in the robot configuration and does not configure a teleoperator. During recording, the actual joint state is written as both the observation and action in the LeRobot dataset: +Manual drag recording uses `manual_mode: true` in the robot configuration and does not configure a teleoperator. During recording, the actual joint state is written as both the observation and action in the LeRobot dataset. When a gripper is configured, hold `C` to close it slowly and `O` to open it slowly. Adjust the speed with `manual_gripper_speed`, which defaults to `0.5`: ```bash uv run uf-lerobot-record --config_path config/manual_mode/xarm7_manual_record_config.yaml diff --git a/README_ZH.md b/README_ZH.md index 95f0de4..099f769 100644 --- a/README_ZH.md +++ b/README_ZH.md @@ -166,7 +166,7 @@ uv run uf-xarm-manual-mode --config_path config/manual_mode/xarm_manual_mode_con ### 3. 人工拖拽数据采集 -人工拖拽录制使用 robot 配置中的 `manual_mode: true`,不需要配置 teleop。录制过程中,机械臂的实际关节状态会同时作为 observation 和 action 写入 LeRobot 数据集: +人工拖拽录制使用 robot 配置中的 `manual_mode: true`,不需要配置 teleop。录制过程中,机械臂的实际关节状态会作为 observation 和 action 写入 LeRobot 数据集;如果配置了夹爪,还可以按住 `C` 缓慢闭合、按住 `O` 缓慢张开。夹爪速度通过 `manual_gripper_speed` 配置,默认值为 `0.5`: ```bash uv run uf-lerobot-record --config_path config/manual_mode/xarm7_manual_record_config.yaml diff --git a/config/manual_mode/xarm7_manual_record_config.yaml b/config/manual_mode/xarm7_manual_record_config.yaml index 6abf68d..83b32f1 100644 --- a/config/manual_mode/xarm7_manual_record_config.yaml +++ b/config/manual_mode/xarm7_manual_record_config.yaml @@ -8,6 +8,8 @@ robot: # Gripper type: 1 is the xArm gripper. gripper_type: 1 manual_mode: true + # Normalized gripper position change per second while holding C/O. + manual_gripper_speed: 0.5 # Teaching sensitivity, valid range is 1-5, affecting vel of the robot. teach_sensitivity: 3 # Whether to record joint velocities in observations. 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 a068785..287aef5 100644 --- a/src/lerobot_robot_ufactory/robots/uf_robot/uf_robot.py +++ b/src/lerobot_robot_ufactory/robots/uf_robot/uf_robot.py @@ -243,6 +243,13 @@ class UFRobot(Robot, Thread): self.real_arm.set_state(0) # set to start state time.sleep(0.5) + _, err_warn = self.real_arm.get_err_warn_code() + if err_warn[0] != 0: + raise RuntimeError(f"Failed to set correct state to UF robot! Controller Error code: {err_warn[0]} !") + + if self._gripper_type > GripperType.NoGripper: + self._configure_gripper(move_to_open=not self.config.manual_mode) + if self.config.manual_mode: if self.config.teach_sensitivity is not None: code = self.real_arm.set_teach_sensitivity(self.config.teach_sensitivity) @@ -263,42 +270,6 @@ class UFRobot(Robot, Thread): ) return - _, err_warn = self.real_arm.get_err_warn_code() - if err_warn[0] != 0: - raise RuntimeError(f"Failed to set correct state to UF robot! Controller Error code: {err_warn[0]} !") - - if self._gripper_type > GripperType.NoGripper: - self.real_arm._arm._baud_checkset = True - 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.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) - self.real_arm.set_gripper_g2_position(self._gripper_param.open_pos) - elif self._gripper_type == GripperType.BioGripperG2: - _, mode = self.real_arm.get_bio_gripper_control_mode() - if mode != 1: - self.real_arm.set_bio_gripper_control_mode(1) - self.real_arm.set_bio_gripper_enable(True) - self.real_arm.open_bio_gripper() - elif self._gripper_type == GripperType.PikaGripper: - self.pika_gripper.enable() - time.sleep(0.5) - self.pika_gripper.set_gripper_distance(self._gripper_param.open_pos) - elif self._gripper_type == GripperType.RobotiqGripper: - self.real_arm.robotiq_reset() - self.real_arm.robotiq_set_activate(wait=True) - self.real_arm.robotiq_set_position(self._gripper_param.open_pos, wait=True) - self._gripper_param.grippos = self._gripper_param.open_pos - self._gripper_param.gripper_norm = self._gripper_param.open_pos - self.real_arm._arm._baud_checkset = False - _, err_warn = self.real_arm.get_err_warn_code() - if err_warn[0] != 0: - raise RuntimeError(f"Failed to set correct state to Gripper! Controller Error code: {err_warn[0]} !") - if self._control_space == "joint": self.real_arm.set_mode(6) elif self._control_space == "cartesian": @@ -316,6 +287,49 @@ class UFRobot(Robot, Thread): self.start() time.sleep(0.2) + def _configure_gripper(self, move_to_open: bool) -> None: + """Initialize the configured gripper without moving it in manual mode.""" + 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) + if move_to_open: + 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) + if move_to_open: + self.real_arm.set_gripper_g2_position(self._gripper_param.open_pos) + elif self._gripper_type == GripperType.BioGripperG2: + _, mode = self.real_arm.get_bio_gripper_control_mode() + if mode != 1: + self.real_arm.set_bio_gripper_control_mode(1) + self.real_arm.set_bio_gripper_enable(True) + if move_to_open: + self.real_arm.open_bio_gripper() + elif self._gripper_type == GripperType.PikaGripper: + self.pika_gripper.enable() + if move_to_open: + time.sleep(0.5) + self.pika_gripper.set_gripper_distance(self._gripper_param.open_pos) + elif self._gripper_type == GripperType.RobotiqGripper: + self.real_arm.robotiq_reset() + self.real_arm.robotiq_set_activate(wait=True) + if move_to_open: + self.real_arm.robotiq_set_position(self._gripper_param.open_pos, wait=True) + finally: + self.real_arm._arm._baud_checkset = False + + _, err_warn = self.real_arm.get_err_warn_code() + if err_warn[0] != 0: + raise RuntimeError(f"Failed to set correct state to Gripper! Controller Error code: {err_warn[0]} !") + + if move_to_open: + self._gripper_param.grippos = self._gripper_param.open_pos + self._gripper_param.gripper_norm = 0.0 + def calibrate(self) -> None: self._is_calibrated = True pass # CHECK! currently No-op @@ -385,10 +399,49 @@ class UFRobot(Robot, Thread): return obs_dict + def _send_gripper_action(self, gripper_norm: float) -> None: + gripper_norm = min(max(float(gripper_norm), 0.0), 1.0) + 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) + 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))) + 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) + 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))) + 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) + 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) + def send_action(self, action: dict) -> np.ndarray: if not self._is_connected: raise ConnectionError() if self.config.manual_mode: + gripper_key = f"{self.prefix}gripper.pos" + if ( + self._gripper_type > GripperType.NoGripper + and gripper_key in action + and self.real_arm.error_code == 0 + and not self.config.no_action + ): + self._send_gripper_action(action[gripper_key]) return action if self.real_arm.error_code != 0: return action @@ -428,40 +481,7 @@ class UFRobot(Robot, Thread): if self._cmd_cnt < 99999: self._cmd_cnt += 1 # CHECK!! possibility of overflow? if self._gripper_type > GripperType.NoGripper: - gripper_norm = action[f"{self.prefix}gripper.pos"] - 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) - # self.real_arm.set_gripper_position(grippos, wait=False, wait_motion=False) # CHECK! the command unit - 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))) - 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) - 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))) - 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) - 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) - # self.real_arm.robotiq_set_position( - # grippos, speed=self._gripper_param.speed, force=self._gripper_param.force, - # wait=False, wait_motion=False, - # ) + self._send_gripper_action(action[f"{self.prefix}gripper.pos"]) self.logs["write_pos_dt_s"] = time.perf_counter() - before_write_t return action 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 8a602c8..f5da02e 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 @@ -17,7 +17,8 @@ class UFRobotConfig(RobotConfig): gripper_speed: int = -1 # auto gripper_force: int = -1 # auto observe_joint_vel: bool = False # only effective in joint control mode - manual_mode: bool = False # xArm joint teaching mode; records state without sending actions + 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 teach_sensitivity: int | None = None # xArm teaching sensitivity, valid range: 1-5 # start_joints and start_tcp_pose are intentionally disabled. # Reset uses the xArm SDK initial_point instead of configuration poses. @@ -33,3 +34,5 @@ class UFRobotConfig(RobotConfig): raise ValueError("manual_mode requires control_space='joint'") if self.teach_sensitivity is not None and not 1 <= self.teach_sensitivity <= 5: 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") diff --git a/src/lerobot_robot_ufactory/scripts/uf_lerobot_record.py b/src/lerobot_robot_ufactory/scripts/uf_lerobot_record.py index 8c7fe17..f46d58b 100644 --- a/src/lerobot_robot_ufactory/scripts/uf_lerobot_record.py +++ b/src/lerobot_robot_ufactory/scripts/uf_lerobot_record.py @@ -96,9 +96,43 @@ def _current_episode_index(dataset): return dataset.num_episodes -def _manual_action_from_observation(observation, action_features): +def _manual_gripper_action_key(action_features): + return next((key for key in action_features if key.endswith("gripper.pos")), None) + + +def _manual_action_from_observation(observation, action_features, gripper_target=None): """Keep only robot action fields when mirroring manual-mode state.""" - return {key: value for key, value in observation.items() if key in action_features} + action = {key: value for key, value in observation.items() if key in action_features} + if gripper_target is not None: + gripper_key = _manual_gripper_action_key(action_features) + if gripper_key is not None and gripper_key in action: + action[gripper_key] = float(gripper_target) + return action + + +def _update_manual_gripper_key_state(key, pressed, key_state): + char = getattr(key, "char", None) + if not isinstance(char, str): + return + + char = char.lower() + if char == "c": + key_state["close"] = pressed + elif char == "o": + key_state["open"] = pressed + + +def _update_manual_gripper_target(target, key_state, speed, fps): + if target is None or fps <= 0: + return target + + close_pressed = bool(key_state.get("close", False)) + open_pressed = bool(key_state.get("open", False)) + if close_pressed == open_pressed: + return target + + direction = 1.0 if close_pressed else -1.0 + return min(max(target + direction * speed / fps, 0.0), 1.0) def _create_empty_episode_buffer(dataset, episode_index, template_episode_buffer): @@ -271,6 +305,8 @@ def record_loop( display_compressed_images: bool = False, frame_callback: callable = None, manual_mode: bool = False, + manual_gripper_keys: dict[str, bool] | None = None, + manual_gripper_speed: float = 0.5, ): if dataset is not None and dataset.fps != fps: raise ValueError(f"The dataset fps should be equal to requested fps ({dataset.fps} != {fps}).") @@ -310,6 +346,10 @@ def record_loop( # only positional cmd for now: Remove velo from observation for cmd if needed! last_robot_cmd = { k: v for k,v in last_robot_cmd.items() if not "vel" in k } + manual_gripper_keys = manual_gripper_keys or {} + manual_gripper_target = None + manual_gripper_action_key = _manual_gripper_action_key(robot.action_features) + timestamp = 0 start_episode_t = time.perf_counter() while timestamp < control_time_s: @@ -346,7 +386,24 @@ def record_loop( elif policy is None and manual_mode: # In manual mode the physical arm is the source of both the # observation and the demonstrated target state. - act = _manual_action_from_observation(obs_processed, robot.action_features) + if manual_gripper_action_key is not None and manual_gripper_target is None: + gripper_value = obs_processed.get(manual_gripper_action_key) + if gripper_value is None: + gripper_value = obs.get(manual_gripper_action_key) + if gripper_value is not None: + manual_gripper_target = min(max(float(gripper_value), 0.0), 1.0) + + manual_gripper_target = _update_manual_gripper_target( + manual_gripper_target, + manual_gripper_keys, + manual_gripper_speed, + fps, + ) + act = _manual_action_from_observation( + obs_processed, + robot.action_features, + gripper_target=manual_gripper_target, + ) act_processed_teleop = teleop_action_processor((act, obs)) elif policy is None and isinstance(teleop, Teleoperator): @@ -425,6 +482,17 @@ def _prepare_recording_episode(robot, teleop, is_uf_teleop, manual_mode): teleop.set_teleop_enabled(True, obs) +def _print_record_controls(is_recorded, manual_mode): + if is_recorded: + controls = '[ESC] Exit [←] Reset [→] Save' + else: + start_label = 'Reset / Start' if manual_mode else 'Start' + controls = f'[ESC] Exit [Space] {start_label} [←] Reset [→] Save' + if manual_mode: + controls += ' [C] Close [O] Open' + print(f'⌨ {controls}') + + def record(cfg: UFRecordConfig, async_save: bool = False) -> LeRobotDataset: init_logging() logging.info(pformat(asdict(cfg))) @@ -510,6 +578,7 @@ def record(cfg: UFRecordConfig, async_save: bool = False) -> LeRobotDataset: is_uf_teleop = isinstance(teleop, UFBaseTeleop) is_recorded = False key_dict = {} + manual_gripper_keys = {"close": False, "open": False} listener = None events = {"exit_early": False, "rerecord_episode": False, "stop_recording": False} @@ -522,6 +591,7 @@ def record(cfg: UFRecordConfig, async_save: bool = False) -> LeRobotDataset: } def on_press(key): + _update_manual_gripper_key_state(key, True, manual_gripper_keys) try: if key == keyboard.Key.right: print("Right arrow key pressed. Exiting loop...") @@ -540,12 +610,10 @@ def record(cfg: UFRecordConfig, async_save: bool = False) -> LeRobotDataset: key_dict[key] = True def on_release(key): + _update_manual_gripper_key_state(key, False, manual_gripper_keys) try: if key == keyboard.Key.enter: - if not is_recorded: - print('⌨ [ESC] Exit [Space] Start [←] Reset [→] Save') - else: - print('⌨ [ESC] Exit [←] Reset [→] Save') + _print_record_controls(is_recorded, manual_mode) # is_recorded = True except Exception as e: print(f"Error handling key release: {e}") @@ -554,7 +622,7 @@ def record(cfg: UFRecordConfig, async_save: bool = False) -> LeRobotDataset: listener, events = init_keyboard_listener(events=events, on_press=on_press, on_release=on_release) print("\n********** Episode Record Loop Start **********") - print('⌨ [ESC] Exit [Space] Start [←] Reset [→] Save') + _print_record_controls(is_recorded, manual_mode) else: input('⌨ Press Enter to start record >>> ') is_recorded = True @@ -609,6 +677,8 @@ def record(cfg: UFRecordConfig, async_save: bool = False) -> LeRobotDataset: display_data=cfg.display_data, frame_callback=frame_callback, manual_mode=manual_mode, + manual_gripper_keys=manual_gripper_keys, + manual_gripper_speed=getattr(cfg.robot, "manual_gripper_speed", 0.5), ) else: continue @@ -630,7 +700,7 @@ def record(cfg: UFRecordConfig, async_save: bool = False) -> LeRobotDataset: _set_episode_buffer(dataset, empty_episode_buffer) is_recorded = False if is_evt: - print('⌨ [ESC] Exit [Space] Start [←] Reset [→] Save') + _print_record_controls(is_recorded, manual_mode) else: input('\n⌨ Press Enter to rerecord this episode >>>>> ') is_recorded = True @@ -652,7 +722,7 @@ def record(cfg: UFRecordConfig, async_save: bool = False) -> LeRobotDataset: recorded_episodes += 1 is_recorded = False if is_evt: - print('⌨ [ESC] Exit [Space] Start [←] Reset [→] Save') + _print_record_controls(is_recorded, manual_mode) else: input('⌨ Press Enter to record at the next episode >>>>> ') is_recorded = True diff --git a/tests/test_manual_mode.py b/tests/test_manual_mode.py index 4221e29..acf2ac4 100644 --- a/tests/test_manual_mode.py +++ b/tests/test_manual_mode.py @@ -9,6 +9,8 @@ from lerobot_robot_ufactory.robots.uf_robot.uf_robot_config import UFRobotConfig from lerobot_robot_ufactory.scripts import uf_lerobot_record as record_module from lerobot_robot_ufactory.scripts.uf_lerobot_record import ( _manual_action_from_observation, + _update_manual_gripper_key_state, + _update_manual_gripper_target, _prepare_recording_episode, get_cfg, ) @@ -22,6 +24,8 @@ class FakeXArm: self.error_code = 0 self.mode = 0 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.calls = [] def motion_enable(self, **kwargs): @@ -58,6 +62,31 @@ class FakeXArm: self.calls.append(("set_linear_spd_limit_factor", factor)) return 0 + def set_gripper_enable(self, enable): + self.calls.append(("set_gripper_enable", enable)) + return 0 + + def set_gripper_mode(self, mode): + self.calls.append(("set_gripper_mode", mode)) + return 0 + + def set_gripper_speed(self, speed): + self.calls.append(("set_gripper_speed", speed)) + return 0 + + def set_gripper_position(self, position, **kwargs): + self.calls.append(("set_gripper_position", position, kwargs)) + self.gripper_position = position + return 0 + + def get_gripper_position(self): + self.calls.append(("get_gripper_position",)) + return 0, self.gripper_position + + def getset_tgpio_modbus_data(self, data): + self.calls.append(("getset_tgpio_modbus_data", data)) + return 0, [] + def get_joint_states(self, is_radian=True, num=3): positions = np.arange(6, dtype=np.float64) velocities = np.zeros(6, dtype=np.float64) @@ -167,6 +196,26 @@ def test_manual_mode_config_rejects_cartesian_control(tmp_path): ) +def test_manual_gripper_speed_is_configurable_and_non_negative(tmp_path): + config = UFRobotConfig( + id="test_manual_robot", + calibration_dir=tmp_path, + robot_dof=6, + manual_mode=True, + manual_gripper_speed=0.25, + ) + assert config.manual_gripper_speed == 0.25 + + with pytest.raises(ValueError, match="manual_gripper_speed"): + UFRobotConfig( + id="test_manual_robot", + calibration_dir=tmp_path, + robot_dof=6, + manual_mode=True, + manual_gripper_speed=-0.1, + ) + + def test_manual_action_filters_non_action_observation_fields(): observation = { "J1.pos": 1.0, @@ -182,6 +231,52 @@ def test_manual_action_filters_non_action_observation_fields(): } +def test_manual_gripper_keys_update_target_in_expected_direction_and_bounds(): + key_state = {"close": False, "open": False} + + _update_manual_gripper_key_state(type("Key", (), {"char": "C"})(), True, key_state) + assert key_state == {"close": True, "open": False} + assert _update_manual_gripper_target(0.5, key_state, speed=1.0, fps=10) == pytest.approx(0.6) + + _update_manual_gripper_key_state(type("Key", (), {"char": "C"})(), False, key_state) + _update_manual_gripper_key_state(type("Key", (), {"char": "o"})(), True, key_state) + assert _update_manual_gripper_target(0.05, key_state, speed=1.0, fps=10) == 0.0 + + _update_manual_gripper_key_state(type("Key", (), {"char": "c"})(), True, key_state) + assert _update_manual_gripper_target(0.99, key_state, speed=1.0, fps=10) == 0.99 + + +def test_manual_mode_initializes_gripper_without_opening_and_sends_only_gripper(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_manual_gripper_robot", + calibration_dir=tmp_path, + robot_ip=arm.robot_ip, + robot_dof=6, + control_space="joint", + gripper_type=1, + manual_mode=True, + ) + robot = uf_robot_module.UFRobot(config) + robot.connect() + + assert ("set_gripper_enable", True) in arm.calls + assert ("set_gripper_mode", 0) in arm.calls + assert ("set_gripper_speed", 5000) in arm.calls + assert not any(call[0] == "set_gripper_position" for call in arm.calls) + + robot.send_action({"J1.pos": 1.0, "gripper.pos": 0.5}) + + assert any(call[0] == "getset_tgpio_modbus_data" for call in arm.calls) + assert not any(call[0] == "set_servo_angle" for call in arm.calls) + 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( @@ -194,6 +289,7 @@ def test_manual_record_config_has_no_teleop(monkeypatch): assert config.robot.manual_mode is True assert config.robot.robot_dof == 7 + assert config.robot.manual_gripper_speed == 0.5 assert config.teleop is None assert config.dataset.fps == 30 @@ -277,3 +373,38 @@ def test_manual_recording_episode_resets_before_recording(): _prepare_recording_episode(robot, teleop=None, is_uf_teleop=False, manual_mode=True) assert robot.calls == ["reset_to_initial"] + + +def test_manual_record_loop_applies_keyboard_gripper_target(): + class FakeRobot: + name = "fake_manual_robot" + robot_type = name + action_features = {"J1.pos": float, "gripper.pos": float} + + def __init__(self): + self.sent_actions = [] + + def get_observation(self): + return {"J1.pos": 1.0, "gripper.pos": 0.5} + + def send_action(self, action): + self.sent_actions.append(action.copy()) + return action + + robot = FakeRobot() + action_pipeline, robot_pipeline, observation_pipeline = record_module.make_default_processors() + + record_module.record_loop( + robot=robot, + events={"exit_early": False}, + fps=10, + teleop_action_processor=action_pipeline, + robot_action_processor=robot_pipeline, + robot_observation_processor=observation_pipeline, + control_time_s=0.001, + manual_mode=True, + manual_gripper_keys={"close": True, "open": False}, + manual_gripper_speed=1.0, + ) + + assert robot.sent_actions == [{"J1.pos": 1.0, "gripper.pos": 0.6}]