文件
Evo-RL/tests/test_control_robot.py
Elvin 3a87382ba4 chore(ci): make quality checks pass across repo
- fix ruff violations in dataset, recording, and value modules

- fix typos findings and markdown/workflow lint issues

- update pages workflow checkout to disable credential persistence

- apply pre-commit auto-formatting and EOF normalization

- include requested repo housekeeping updates (.gitignore and README)
2026-03-04 11:02:06 +08:00

520 行
17 KiB
Python

#!/usr/bin/env python
# Copyright 2025 The HuggingFace Inc. team. All rights reserved.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
import json
from collections import deque
from unittest.mock import MagicMock, patch
import numpy as np
import pytest
import torch
from lerobot.datasets.lerobot_dataset import LeRobotDataset
from lerobot.scripts.lerobot_calibrate import CalibrateConfig, calibrate
from lerobot.scripts.lerobot_human_inloop_record import (
_HumanInloopFailureResetController,
_load_failure_reset_pose,
_save_failure_reset_pose,
_slow_reset_all_arms_to_pose,
human_inloop_record,
)
from lerobot.scripts.lerobot_record import (
ACPInferenceConfig,
DatasetRecordConfig,
PolicySyncDualArmExecutor,
RecordConfig,
_capture_policy_runtime_state,
_predict_policy_action_with_acp_inference,
record,
record_loop,
)
from lerobot.scripts.lerobot_replay import DatasetReplayConfig, ReplayConfig, replay
from lerobot.scripts.lerobot_teleoperate import TeleoperateConfig, teleoperate
from tests.fixtures.constants import DUMMY_REPO_ID
from tests.mocks.mock_robot import MockRobot, MockRobotConfig
from tests.mocks.mock_teleop import MockTeleop, MockTeleopConfig
def test_calibrate():
robot_cfg = MockRobotConfig()
cfg = CalibrateConfig(robot=robot_cfg)
calibrate(cfg)
def test_teleoperate():
robot_cfg = MockRobotConfig()
teleop_cfg = MockTeleopConfig()
cfg = TeleoperateConfig(
robot=robot_cfg,
teleop=teleop_cfg,
teleop_time_s=0.1,
)
teleoperate(cfg)
def test_record_and_resume(tmp_path):
robot_cfg = MockRobotConfig()
teleop_cfg = MockTeleopConfig()
dataset_cfg = DatasetRecordConfig(
repo_id=DUMMY_REPO_ID,
single_task="Dummy task",
root=tmp_path / "record",
num_episodes=1,
episode_time_s=0.1,
reset_time_s=0,
push_to_hub=False,
)
cfg = RecordConfig(
robot=robot_cfg,
dataset=dataset_cfg,
teleop=teleop_cfg,
play_sounds=False,
)
dataset = record(cfg)
assert dataset.fps == 30
assert dataset.meta.total_episodes == dataset.num_episodes == 1
assert dataset.meta.total_frames == dataset.num_frames == 3
assert dataset.meta.total_tasks == 1
cfg.resume = True
# Mock the revision to prevent Hub calls during resume
with (
patch("lerobot.datasets.lerobot_dataset.get_safe_version") as mock_get_safe_version,
patch("lerobot.datasets.lerobot_dataset.snapshot_download") as mock_snapshot_download,
):
mock_get_safe_version.return_value = "v3.0"
mock_snapshot_download.return_value = str(tmp_path / "record")
dataset = record(cfg)
assert dataset.meta.total_episodes == dataset.num_episodes == 2
assert dataset.meta.total_frames == dataset.num_frames == 6
assert dataset.meta.total_tasks == 1
def test_record_adds_episode_success_and_collector_policy_id(tmp_path):
robot_cfg = MockRobotConfig()
teleop_cfg = MockTeleopConfig()
root = tmp_path / "record_with_annotations"
dataset_cfg = DatasetRecordConfig(
repo_id=DUMMY_REPO_ID,
single_task="Dummy task",
root=root,
num_episodes=1,
episode_time_s=0.1,
reset_time_s=0,
push_to_hub=False,
)
cfg = RecordConfig(
robot=robot_cfg,
dataset=dataset_cfg,
teleop=teleop_cfg,
play_sounds=False,
enable_episode_outcome_labeling=True,
default_episode_success="failure",
enable_collector_policy_id=True,
)
dataset = record(cfg)
assert "complementary_info.collector_policy_id" in dataset.features
reloaded = LeRobotDataset(DUMMY_REPO_ID, root=root)
assert reloaded[0]["complementary_info.collector_policy_id"] == "human"
assert "episode_success" in reloaded.meta.episodes.column_names
assert reloaded.meta.episodes[0]["episode_success"] == "failure"
def test_human_inloop_record_works_without_policy_and_saves_annotations(tmp_path):
robot_cfg = MockRobotConfig()
teleop_cfg = MockTeleopConfig()
root = tmp_path / "hil_no_policy"
dataset_cfg = DatasetRecordConfig(
repo_id=DUMMY_REPO_ID,
single_task="Dummy task",
root=root,
num_episodes=1,
episode_time_s=0.1,
reset_time_s=0,
push_to_hub=False,
)
cfg = RecordConfig(
robot=robot_cfg,
dataset=dataset_cfg,
teleop=teleop_cfg,
play_sounds=False,
)
dataset = human_inloop_record(cfg)
assert cfg.intervention_state_machine_enabled is False
assert cfg.collector_policy_id_policy == "human"
assert "complementary_info.collector_policy_id" in dataset.features
reloaded = LeRobotDataset(DUMMY_REPO_ID, root=root)
assert reloaded[0]["complementary_info.collector_policy_id"] == "human"
assert "episode_success" in reloaded.meta.episodes.column_names
assert reloaded.meta.episodes[0]["episode_success"] == "failure"
def test_record_loop_sets_leader_manual_control_during_reset():
class MockTeleopWithManualControl(MockTeleop):
def __init__(self, config):
super().__init__(config)
self.manual_control_calls = []
def set_manual_control(self, enabled: bool) -> None:
self.manual_control_calls.append(enabled)
robot = MockRobot(MockRobotConfig())
teleop = MockTeleopWithManualControl(MockTeleopConfig())
robot.connect()
teleop.connect()
try:
record_loop(
robot=robot,
events={
"exit_early": True,
"rerecord_episode": False,
"stop_recording": False,
"toggle_intervention": False,
"episode_outcome": None,
},
fps=30,
teleop_action_processor=lambda x: x[0],
robot_action_processor=lambda x: x[0],
robot_observation_processor=lambda x: x,
teleop=teleop,
policy=None,
control_time_s=0.1,
)
finally:
if teleop.is_connected:
teleop.disconnect()
if robot.is_connected:
robot.disconnect()
assert teleop.manual_control_calls == [True]
def test_save_and_load_failure_reset_pose(tmp_path):
robot = MockRobot(MockRobotConfig(n_motors=2, random_values=False, static_values=[12.5, -3.0]))
robot.connect()
pose_path = tmp_path / "failure_reset_pose.json"
try:
saved_pose = _save_failure_reset_pose(robot=robot, pose_path=pose_path)
finally:
if robot.is_connected:
robot.disconnect()
with open(pose_path) as f:
payload = json.load(f)
assert saved_pose == {"motor_1.pos": 12.5, "motor_2.pos": -3.0}
assert payload["joint_pos"] == {"motor_1.pos": 12.5, "motor_2.pos": -3.0}
def test_load_failure_reset_pose_from_json(tmp_path):
pose_path = tmp_path / "failure_reset_pose.json"
payload = {
"robot_type": "mock_robot",
"joint_pos": {
"motor_1.pos": 12.5,
"motor_2.pos": -3.0,
"non_joint_key": 999,
},
}
with open(pose_path, "w") as f:
json.dump(payload, f)
loaded_pose = _load_failure_reset_pose(pose_path)
assert loaded_pose == {"motor_1.pos": 12.5, "motor_2.pos": -3.0}
def test_human_inloop_failure_reset_controller_reuses_existing_pose(tmp_path):
robot_cfg = MockRobotConfig()
teleop_cfg = MockTeleopConfig()
dataset_cfg = DatasetRecordConfig(
repo_id=DUMMY_REPO_ID,
single_task="Dummy task",
root=tmp_path / "hil_with_policy",
num_episodes=1,
episode_time_s=0.1,
reset_time_s=0,
push_to_hub=False,
)
cfg = RecordConfig(
robot=robot_cfg,
dataset=dataset_cfg,
teleop=teleop_cfg,
play_sounds=False,
)
controller = _HumanInloopFailureResetController(cfg)
controller.pose_path = tmp_path / "existing_failure_reset_pose.json"
with open(controller.pose_path, "w") as f:
json.dump({"joint_pos": {"motor_1.pos": 1.0, "motor_2.pos": -2.0}}, f)
with (
patch("builtins.input") as mock_input,
patch("lerobot.scripts.lerobot_human_inloop_record._save_failure_reset_pose") as mock_save,
):
controller.on_record_connected(robot=MagicMock(), teleop=MagicMock())
assert controller.failure_reset_pose == {"motor_1.pos": 1.0, "motor_2.pos": -2.0}
mock_input.assert_not_called()
mock_save.assert_not_called()
def test_slow_reset_all_arms_to_pose_uses_interpolation():
robot = MockRobot(MockRobotConfig(n_motors=2, random_values=False, static_values=[0.0, 0.0]))
teleop = MagicMock()
robot.connect()
robot.send_action = MagicMock(wraps=robot.send_action)
target_pose = {"motor_1.pos": 11.0, "motor_2.pos": -22.0}
try:
_slow_reset_all_arms_to_pose(
robot=robot,
teleop=teleop,
target_pose=target_pose,
duration_s=0.2,
)
finally:
if robot.is_connected:
robot.disconnect()
final_action = robot.send_action.call_args_list[-1].args[0]
assert final_action == {"motor_1.pos": 11.0, "motor_2.pos": -22.0}
assert robot.send_action.call_count > 1
teleop.set_manual_control.assert_called_once_with(False)
teleop.send_feedback.assert_called()
def test_record_and_replay(tmp_path):
robot_cfg = MockRobotConfig()
teleop_cfg = MockTeleopConfig()
record_dataset_cfg = DatasetRecordConfig(
repo_id=DUMMY_REPO_ID,
single_task="Dummy task",
root=tmp_path / "record_and_replay",
num_episodes=1,
episode_time_s=0.1,
push_to_hub=False,
)
record_cfg = RecordConfig(
robot=robot_cfg,
dataset=record_dataset_cfg,
teleop=teleop_cfg,
play_sounds=False,
)
replay_dataset_cfg = DatasetReplayConfig(
repo_id=DUMMY_REPO_ID,
episode=0,
root=tmp_path / "record_and_replay",
)
replay_cfg = ReplayConfig(
robot=robot_cfg,
dataset=replay_dataset_cfg,
play_sounds=False,
)
record(record_cfg)
# Mock the revision to prevent Hub calls during replay
with (
patch("lerobot.datasets.lerobot_dataset.get_safe_version") as mock_get_safe_version,
patch("lerobot.datasets.lerobot_dataset.snapshot_download") as mock_snapshot_download,
):
mock_get_safe_version.return_value = "v3.0"
mock_snapshot_download.return_value = str(tmp_path / "record_and_replay")
replay(replay_cfg)
def test_policy_sync_dual_arm_executor():
robot = MagicMock()
robot.send_action.return_value = {"motor_1.pos": 10.0}
teleop = MagicMock()
executor = PolicySyncDualArmExecutor(robot=robot, teleop=teleop, parallel_dispatch=True)
action = {"motor_1.pos": 10.0}
sent_action = executor.send_action(action)
executor.shutdown()
assert sent_action == action
robot.send_action.assert_called_once_with(action)
teleop.send_feedback.assert_called_once_with(action)
def test_record_config_rejects_cfg_without_acp_enable():
robot_cfg = MockRobotConfig()
teleop_cfg = MockTeleopConfig()
dataset_cfg = DatasetRecordConfig(
repo_id=DUMMY_REPO_ID,
single_task="Dummy task",
num_episodes=1,
episode_time_s=0.1,
reset_time_s=0,
push_to_hub=False,
)
with pytest.raises(ValueError, match="acp_inference.use_cfg=true"):
RecordConfig(
robot=robot_cfg,
dataset=dataset_cfg,
teleop=teleop_cfg,
play_sounds=False,
acp_inference=ACPInferenceConfig(enable=False, use_cfg=True, cfg_beta=0.6),
)
def test_record_config_rejects_negative_cfg_beta():
robot_cfg = MockRobotConfig()
teleop_cfg = MockTeleopConfig()
dataset_cfg = DatasetRecordConfig(
repo_id=DUMMY_REPO_ID,
single_task="Dummy task",
num_episodes=1,
episode_time_s=0.1,
reset_time_s=0,
push_to_hub=False,
)
with pytest.raises(ValueError, match="cfg_beta"):
RecordConfig(
robot=robot_cfg,
dataset=dataset_cfg,
teleop=teleop_cfg,
play_sounds=False,
acp_inference=ACPInferenceConfig(enable=True, use_cfg=False, cfg_beta=-0.1),
)
def test_acp_inference_without_cfg_appends_positive_prompt():
class _StaticPolicy:
def __init__(self, value: float):
self.value = value
self.tasks = []
def select_action(self, batch):
self.tasks.append(batch["task"])
return torch.tensor([[self.value, self.value, self.value]], dtype=torch.float32)
observation_frame = {"observation.state": np.array([0.0, 0.0, 0.0], dtype=np.float32)}
policy = _StaticPolicy(value=2.0)
action = _predict_policy_action_with_acp_inference(
observation_frame=observation_frame,
policy=policy,
device=torch.device("cpu"),
preprocessor=lambda x: x,
postprocessor=lambda x: x,
use_amp=False,
task="Pick and place",
robot_type="mock_robot",
acp_inference=ACPInferenceConfig(enable=True, use_cfg=False, cfg_beta=0.6),
)
assert torch.allclose(action, torch.tensor([[2.0, 2.0, 2.0]], dtype=torch.float32))
assert policy.tasks[-1] == "Pick and place\nAdvantage: positive"
def test_acp_inference_with_cfg_blends_cond_and_uncond_actions():
class _StaticPolicy:
def __init__(self):
self.tasks = []
def select_action(self, batch):
self.tasks.append(batch["task"])
value = 3.0 if "Advantage: positive" in batch["task"] else 1.0
return torch.tensor([[value, value, value]], dtype=torch.float32)
observation_frame = {"observation.state": np.array([0.0, 0.0, 0.0], dtype=np.float32)}
policy = _StaticPolicy()
cond_state = {}
uncond_state = {}
action = _predict_policy_action_with_acp_inference(
observation_frame=observation_frame,
policy=policy,
device=torch.device("cpu"),
preprocessor=lambda x: x,
postprocessor=lambda x: x,
use_amp=False,
task="Pick and place",
robot_type="mock_robot",
acp_inference=ACPInferenceConfig(enable=True, use_cfg=True, cfg_beta=0.5),
cond_runtime_state=cond_state,
uncond_runtime_state=uncond_state,
)
assert torch.allclose(action, torch.tensor([[2.0, 2.0, 2.0]], dtype=torch.float32))
assert policy.tasks == [
"Pick and place\nAdvantage: positive",
"Pick and place",
]
def test_acp_inference_with_cfg_uses_isolated_branch_queues():
class _QueuePolicy:
def __init__(self):
self._action_queue = deque(maxlen=2)
def select_action(self, batch):
if len(self._action_queue) == 0:
base = 10.0 if "Advantage: positive" in batch["task"] else 0.0
self._action_queue.extend(
[
torch.tensor([[base + 1.0, base + 1.0, base + 1.0]], dtype=torch.float32),
torch.tensor([[base + 2.0, base + 2.0, base + 2.0]], dtype=torch.float32),
]
)
return self._action_queue.popleft()
observation_frame = {"observation.state": np.array([0.0, 0.0, 0.0], dtype=np.float32)}
policy = _QueuePolicy()
cond_state = _capture_policy_runtime_state(policy)
uncond_state = _capture_policy_runtime_state(policy)
action_1 = _predict_policy_action_with_acp_inference(
observation_frame=observation_frame,
policy=policy,
device=torch.device("cpu"),
preprocessor=lambda x: x,
postprocessor=lambda x: x,
use_amp=False,
task="Pick and place",
robot_type="mock_robot",
acp_inference=ACPInferenceConfig(enable=True, use_cfg=True, cfg_beta=0.5),
cond_runtime_state=cond_state,
uncond_runtime_state=uncond_state,
)
action_2 = _predict_policy_action_with_acp_inference(
observation_frame=observation_frame,
policy=policy,
device=torch.device("cpu"),
preprocessor=lambda x: x,
postprocessor=lambda x: x,
use_amp=False,
task="Pick and place",
robot_type="mock_robot",
acp_inference=ACPInferenceConfig(enable=True, use_cfg=True, cfg_beta=0.5),
cond_runtime_state=cond_state,
uncond_runtime_state=uncond_state,
)
assert torch.allclose(action_1, torch.tensor([[6.0, 6.0, 6.0]], dtype=torch.float32))
assert torch.allclose(action_2, torch.tensor([[7.0, 7.0, 7.0]], dtype=torch.float32))