Add gripper control via keyboards in manual mode
This commit is contained in:
Saberlve
parent
da5a076f4c
commit
c04fcf4427
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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.
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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")
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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}]
|
||||
|
||||
Loading…
Reference in New Issue
Block a user