镜像自地址
https://github.com/MINT-SJTU/Evo-RL.git
已同步 2026-11-20 18:17:06 +00:00
fix(human-inloop): reset to stored pose after success
- return the robot to the stored reset pose for both success and failure outcomes - clarify reset logging so it matches the generalized behavior - add a controller test covering success-triggered resets
这个提交包含在:
@ -32,7 +32,11 @@ from lerobot.configs import parser
|
|||||||
from lerobot.scripts.lerobot_record import RecordConfig, record
|
from lerobot.scripts.lerobot_record import RecordConfig, record
|
||||||
from lerobot.utils.constants import HF_LEROBOT_HOME
|
from lerobot.utils.constants import HF_LEROBOT_HOME
|
||||||
from lerobot.utils.import_utils import register_third_party_plugins
|
from lerobot.utils.import_utils import register_third_party_plugins
|
||||||
from lerobot.utils.recording_annotations import EPISODE_FAILURE, infer_collector_policy_version
|
from lerobot.utils.recording_annotations import (
|
||||||
|
EPISODE_FAILURE,
|
||||||
|
EPISODE_SUCCESS,
|
||||||
|
infer_collector_policy_version,
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
def _default_failure_reset_pose_path(cfg: RecordConfig) -> Path:
|
def _default_failure_reset_pose_path(cfg: RecordConfig) -> Path:
|
||||||
@ -82,7 +86,7 @@ def _slow_reset_all_arms_to_pose(
|
|||||||
) -> None:
|
) -> None:
|
||||||
joint_keys = [key for key in robot.action_features if key.endswith(".pos") and key in target_pose]
|
joint_keys = [key for key in robot.action_features if key.endswith(".pos") and key in target_pose]
|
||||||
if not joint_keys:
|
if not joint_keys:
|
||||||
logging.warning("No matching '.pos' joints found for failure reset pose.")
|
logging.warning("No matching '.pos' joints found for the stored reset pose.")
|
||||||
return
|
return
|
||||||
|
|
||||||
current_pose = _extract_joint_pos_from_observation(robot.get_observation())
|
current_pose = _extract_joint_pos_from_observation(robot.get_observation())
|
||||||
@ -102,7 +106,7 @@ def _slow_reset_all_arms_to_pose(
|
|||||||
teleop.send_feedback(action)
|
teleop.send_feedback(action)
|
||||||
time.sleep(step_dt_s)
|
time.sleep(step_dt_s)
|
||||||
|
|
||||||
logging.info("Episode marked as failure. Arms returned to failure reset pose in %.1fs.", duration_s)
|
logging.info("Episode ended. Arms returned to the stored reset pose in %.1fs.", duration_s)
|
||||||
|
|
||||||
|
|
||||||
class _HumanInloopFailureResetController:
|
class _HumanInloopFailureResetController:
|
||||||
@ -123,7 +127,7 @@ class _HumanInloopFailureResetController:
|
|||||||
self.failure_reset_pose = _save_failure_reset_pose(robot=robot, pose_path=self.pose_path)
|
self.failure_reset_pose = _save_failure_reset_pose(robot=robot, pose_path=self.pose_path)
|
||||||
|
|
||||||
def on_episode_outcome(self, robot: Any, teleop: Any, episode_success: str | None) -> None:
|
def on_episode_outcome(self, robot: Any, teleop: Any, episode_success: str | None) -> None:
|
||||||
if episode_success == EPISODE_FAILURE and self.failure_reset_pose is not None:
|
if episode_success in {EPISODE_FAILURE, EPISODE_SUCCESS} and self.failure_reset_pose is not None:
|
||||||
_slow_reset_all_arms_to_pose(robot=robot, teleop=teleop, target_pose=self.failure_reset_pose)
|
_slow_reset_all_arms_to_pose(robot=robot, teleop=teleop, target_pose=self.failure_reset_pose)
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
@ -43,6 +43,7 @@ from lerobot.scripts.lerobot_record import (
|
|||||||
)
|
)
|
||||||
from lerobot.scripts.lerobot_replay import DatasetReplayConfig, ReplayConfig, replay
|
from lerobot.scripts.lerobot_replay import DatasetReplayConfig, ReplayConfig, replay
|
||||||
from lerobot.scripts.lerobot_teleoperate import TeleoperateConfig, teleoperate
|
from lerobot.scripts.lerobot_teleoperate import TeleoperateConfig, teleoperate
|
||||||
|
from lerobot.utils.recording_annotations import EPISODE_SUCCESS
|
||||||
from tests.fixtures.constants import DUMMY_REPO_ID
|
from tests.fixtures.constants import DUMMY_REPO_ID
|
||||||
from tests.mocks.mock_robot import MockRobot, MockRobotConfig
|
from tests.mocks.mock_robot import MockRobot, MockRobotConfig
|
||||||
from tests.mocks.mock_teleop import MockTeleop, MockTeleopConfig
|
from tests.mocks.mock_teleop import MockTeleop, MockTeleopConfig
|
||||||
@ -277,6 +278,34 @@ def test_human_inloop_failure_reset_controller_reuses_existing_pose(tmp_path):
|
|||||||
mock_save.assert_not_called()
|
mock_save.assert_not_called()
|
||||||
|
|
||||||
|
|
||||||
|
def test_human_inloop_failure_reset_controller_resets_on_success(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_success_reset",
|
||||||
|
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.failure_reset_pose = {"motor_1.pos": 1.0, "motor_2.pos": -2.0}
|
||||||
|
|
||||||
|
with patch("lerobot.scripts.lerobot_human_inloop_record._slow_reset_all_arms_to_pose") as mock_reset:
|
||||||
|
controller.on_episode_outcome(robot=MagicMock(), teleop=MagicMock(), episode_success=EPISODE_SUCCESS)
|
||||||
|
|
||||||
|
mock_reset.assert_called_once()
|
||||||
|
assert mock_reset.call_args.kwargs["target_pose"] == {"motor_1.pos": 1.0, "motor_2.pos": -2.0}
|
||||||
|
|
||||||
|
|
||||||
def test_slow_reset_all_arms_to_pose_uses_interpolation():
|
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]))
|
robot = MockRobot(MockRobotConfig(n_motors=2, random_values=False, static_values=[0.0, 0.0]))
|
||||||
teleop = MagicMock()
|
teleop = MagicMock()
|
||||||
|
|||||||
在新工单中引用
屏蔽一个用户