镜像自地址
https://github.com/MINT-SJTU/Evo-RL.git
已同步 2026-11-20 18:17:06 +00:00
snapshot: sync mint main from current workspace 2026-03-03 16:32:56 +0800
这个提交包含在:
522
tests/test_control_robot.py
普通文件
522
tests/test_control_robot.py
普通文件
@ -0,0 +1,522 @@
|
||||
#!/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 unittest.mock import MagicMock, patch
|
||||
|
||||
from collections import deque
|
||||
|
||||
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 human_inloop_record
|
||||
from lerobot.scripts.lerobot_human_inloop_record import (
|
||||
_HumanInloopFailureResetController,
|
||||
_load_failure_reset_pose,
|
||||
_save_failure_reset_pose,
|
||||
_slow_reset_all_arms_to_pose,
|
||||
)
|
||||
from lerobot.scripts.lerobot_record import (
|
||||
ACPInferenceConfig,
|
||||
DatasetRecordConfig,
|
||||
PolicySyncDualArmExecutor,
|
||||
RecordConfig,
|
||||
_capture_policy_runtime_state,
|
||||
_predict_policy_action_with_acp_inference,
|
||||
record_loop,
|
||||
record,
|
||||
)
|
||||
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))
|
||||
在新工单中引用
屏蔽一个用户