diff --git a/predicators/envs/pybullet_env.py b/predicators/envs/pybullet_env.py index 17c82de35..f9db77b31 100644 --- a/predicators/envs/pybullet_env.py +++ b/predicators/envs/pybullet_env.py @@ -72,13 +72,22 @@ class ActionExecutor(Protocol): """Drives real hardware from a simulated env's rollout. The port an env exposes for "something outside is executing what I - just simulated". Declared here, next to the two calls, and + just simulated". Declared here, next to the three calls, and implemented elsewhere -- ``pybullet_helpers.real_robot_executor`` -- so this module never imports anything that knows about a robot. An env with no executor attached is pure simulation. """ + def tasks_for(self, train_or_test: str) -> Optional[List[EnvironmentTask]]: + """Tasks to use for this split, or None to keep the env's own. + + Asked *every* time the tasks are requested, because for a real + environment "give me the train task" genuinely means "look at + the world". An executor that has nothing new to say returns None + and the env's own generated (and cached) tasks stand. + """ + def after_reset(self, train_or_test: str, task_idx: int, obs: Observation) -> None: """The env has been reset to a task's initial state.""" @@ -761,6 +770,44 @@ def attach_executor(self, executor: ActionExecutor) -> None: """ self._executor = executor + def get_train_tasks(self) -> List[EnvironmentTask]: + """The train tasks, letting an executor supply them instead. + + ``BaseEnv`` caches these, which is right for a simulated env + whose tasks are generated once. For a real one it is wrong: + every episode faces a physically different scene, and the task + has to be rebuilt from what is actually there. The executor is + asked first; see ``ActionExecutor.tasks_for``. + """ + self._maybe_replace_tasks("train") + return super().get_train_tasks() + + def get_test_tasks(self) -> List[EnvironmentTask]: + """The test tasks, letting an executor supply them instead.""" + self._maybe_replace_tasks("test") + return super().get_test_tasks() + + def _maybe_replace_tasks(self, train_or_test: str) -> None: + """Overwrite the cached tasks for a split if the executor has new + ones.""" + if self._executor is None: + return + fresh = self._executor.tasks_for(train_or_test) + if fresh is None: + return + cached = (self._train_tasks + if train_or_test == "train" else self._test_tasks) + if cached and len(fresh) < len(cached): + # Keep the list length: task *indices* have already been handed out + # against the old length, so shrinking it would turn a live index + # into an IndexError. A real environment perceives one world, so + # every index legitimately names the same freshly-perceived scene. + fresh = [fresh[0]] * len(cached) + if train_or_test == "train": + self._train_tasks = fresh + else: + self._test_tasks = fresh + # ── State Write (State → PyBullet) ────────────────────────── def sync_to_state(self, state: State) -> None: diff --git a/predicators/pybullet_helpers/real_robot_bridge.py b/predicators/pybullet_helpers/real_robot_bridge.py index 427ecf496..1ff3098ec 100644 --- a/predicators/pybullet_helpers/real_robot_bridge.py +++ b/predicators/pybullet_helpers/real_robot_bridge.py @@ -133,6 +133,20 @@ def reset_arm(robot: "RealRobot", joints: Sequence[float]) -> Sequence[float]: return reply.joints +def reset_env(robot: "RealRobot", + joints: Optional[Sequence[float]] = None) -> Any: + """Home the arm, wait for a human to rearrange the scene, then look. + Blocking, and deliberately unbounded. + + Returns the observation captured *after* the human confirmed. + """ + # pylint: disable=import-outside-toplevel,import-error + from babyrobot.realrobot.messages import ResetEnvRequest + request = ResetEnvRequest( + joints=tuple(joints) if joints is not None else None) + return robot.reset_env(request) + + def execute_chunks(robot: "RealRobot", chunks: Sequence[Sequence[Action]], layout: GripperJointLayout, @@ -143,7 +157,7 @@ def execute_chunks(robot: "RealRobot", One chunk is one unit of "execute this, then optionally look": with ``observe`` the reply carries one observation per chunk, which is - how the executor gets a look at the bench per option. With + how the executor gets a look at the scene per option. With ``observe=False`` nothing comes back and the caller's world state stays the sim's prediction. diff --git a/predicators/pybullet_helpers/real_robot_executor.py b/predicators/pybullet_helpers/real_robot_executor.py index 04ddb6e2a..122156d73 100644 --- a/predicators/pybullet_helpers/real_robot_executor.py +++ b/predicators/pybullet_helpers/real_robot_executor.py @@ -2,7 +2,7 @@ predicators rolls each option out in simulation exactly as it always has. This module ships the resulting joint trajectory to the real arm, looks at the -bench, and writes what it saw back into the simulated **twin**. The env stays +scene, and writes what it saw back into the simulated **twin**. The env stays pure simulation and never learns that a robot exists: it exposes one optional collaborator (``PyBulletEnv.attach_executor``) and calls it after each reset and each step. @@ -25,7 +25,7 @@ from predicators.envs.base_env import BaseEnv from predicators.envs.pybullet_env import PyBulletEnv from predicators.pybullet_helpers.real_robot_bridge import execute_chunks, \ - make_real_robot, reset_arm + make_real_robot, reset_arm, reset_env from predicators.settings import CFG from predicators.structs import Action, EnvironmentTask, Observation, State @@ -98,14 +98,14 @@ def __init__(self, env: PyBulletEnv, divergence_atol: float) -> None: self._env = env self._divergence_atol = divergence_atol # Largest per-object position disagreement between the twin and the - # bench at the last look, in metres; None before the first look. + # real scene at the last look, in metres; None before the first look. self.last_divergence: Optional[float] = None def absorb(self, observation: Any) -> Observation: """Write what the cameras saw into the twin; return its new reading. Divergence is measured against the twin's *pre-sync* state -- - the sim's prediction of where the bench would be -- because that + the sim's prediction of where the scene would be -- because that is the comparison that says reality went somewhere the model did not. ``_set_state``'s own reconstruction check cannot answer this: it round-trips the state it was asked to write and @@ -121,7 +121,7 @@ def absorb(self, observation: Any) -> Observation: if self.last_divergence is not None and \ self.last_divergence > self._divergence_atol: logging.warning( - "real robot: the bench is %.3f m from where the twin " + "real robot: the scene is %.3f m from where the twin " "predicted (tolerance %.3f m); the twin is being corrected, " "but the current plan was made against the prediction", self.last_divergence, self._divergence_atol) @@ -145,7 +145,8 @@ def __init__(self, robot: Any, observe_at_boundaries: bool = True, settle_s: float = 0.0, - divergence_atol: float = 0.02) -> None: + divergence_atol: float = 0.02, + human_reset: bool = True) -> None: missing = sorted(name for name in _REQUIRED_HOOKS if not callable(getattr(env, name, None))) if missing: @@ -157,35 +158,98 @@ def __init__(self, if observe_at_boundaries and not getattr(robot, "has_perception", False): raise ValueError( - "asked to look at the bench between options, but the robot " + "asked to look at the scene between options, but the robot " "has no perception configured; set real_robot_perception, or " "turn the option-boundary look off for a blind open-loop run") + if human_reset and not getattr(robot, "has_perception", False): + raise ValueError( + "human resets rebuild each episode's task from the scene, so " + "the robot needs perception; set real_robot_perception, or " + "turn real_robot_human_reset off to keep the captured scene") self._env = env self._robot = robot self._observe = observe_at_boundaries self._settle_s = settle_s + self._human_reset = human_reset self._buffer = OptionBoundaryBuffer() self._corrector = TwinCorrector(env, divergence_atol) + # The scene has to be arranged before the first episode, so a reset is + # owed from the start. Set again by every reset, i.e. once per episode. + self._reset_pending = True + # The twin's home arm configuration, captured now: an executor is + # attached right after the env is built, so the simulated arm is still + # at home. A scene reset has to send the real arm somewhere before the + # human reaches in, and this is that somewhere . + self._home_arm = self._home_arm_joints(env.get_observation()) + # How many times the scene has been (re)perceived for a task. Read by + # tests and worth logging on a hardware session. + self.resets_done = 0 @property def last_divergence(self) -> Optional[float]: - """How far the bench was from the twin at the last look.""" + """How far the scene was from the twin at the last look.""" return self._corrector.last_divergence # -- the ActionExecutor port ------------------------------------------- + def tasks_for(self, train_or_test: str) -> Optional[List[EnvironmentTask]]: + """Rebuild this split's task from the scene, resetting it first. + + **Why this happens here and not in ``reset``.** The task has to + be rebuilt before anything consumes it, and the two callers + consume it differently: + + * *Evaluation* solves at reset. ``_solve_task`` + (``main.py:774``) calls ``cogman.reset(env_task)`` with no + override policy, so ``_reset_policy`` runs + ``approach.solve(task)`` -- before ``env.reset`` is ever + called. A human reset performed in ``env.reset`` would + therefore plan against a scene that no longer exists. + * *Exploration* does not solve at reset: the online loop sets + an override policy first (``main.py:551``), so + ``_reset_policy`` takes that branch. The task still matters, + because it is what ``env.reset`` initializes the simulated + twin from -- a stale one starts every episode from the + captured scene rather than the one just arranged. + + For a real environment "give me the train task" honestly means + "look at the scene", so that is what it does. + + Returns None -- leaving the env's captured-scene task alone -- + when no reset is owed, or when human resets are off. The latter + is what keeps a fixed-plan replay reproducible. + """ + if not self._human_reset or not self._reset_pending: + return None + # Homes the arm out of the way, blocks until the human confirms, then + # perceives. + observation = reset_env(self._robot, self._home_arm) + self._reset_pending = False + self.resets_done += 1 + logging.info( + "real robot: scene reset #%d; rebuilding the %s task " + "from what the cameras see", self.resets_done, train_or_test) + domain = cast(_DomainHooks, self._env) + # One task, because a physical scene is one scene. The env keeps the + # list length stable, so task indices already handed out stay valid. + return [domain.task_from_observation(observation, train_or_test)] + def after_reset(self, train_or_test: str, task_idx: int, obs: Observation) -> None: """Home the real arm to wherever the twin just reset to. The twin is reset first because its home joint configuration is - what the option trajectories are planned from: the arm has to - start where the first option's streamed waypoints begin, or the - drift guard trips on the opening move. + what the option trajectories are planned from. + + Both train and test splits execute. Real mode is a property of + an executor being attached, not of the split. - Both splits execute. Real mode is a property of an executor - being attached, not of the split. + This is also where the next reset is owed from: an episode has + just begun, so the *following* task request has to face a + freshly arranged scene. Marking it here rather than at the end + of an episode is what makes it exactly one prompt per episode. """ del train_or_test, task_idx # every episode homes the same way + self._reset_pending = True lost = self._buffer.discard() if lost: # The previous episode ended mid-option (step limit, or an @@ -267,6 +331,7 @@ def attach_real_robot(env: BaseEnv, robot, observe_at_boundaries=CFG.real_robot_observe_at_option_boundary, settle_s=CFG.real_robot_settle_s, - divergence_atol=CFG.real_robot_divergence_atol) + divergence_atol=CFG.real_robot_divergence_atol, + human_reset=CFG.real_robot_human_reset) env.attach_executor(executor) return executor diff --git a/predicators/settings.py b/predicators/settings.py index 98977ce5a..92cf6bbd4 100644 --- a/predicators/settings.py +++ b/predicators/settings.py @@ -583,16 +583,21 @@ class GlobalSettings: # cameraless, but it always reports the captured layout), or "none" (no # cameras at all, blind open-loop run). real_robot_perception = "zed" - # Look at the bench at each option boundary and correct the twin from what + # Look at the scene at each option boundary and correct the twin from what # was seen. This is the point of running on real hardware -- the learner # sees perceived transitions rather than the simulator's guesses. real_robot_observe_at_option_boundary = True # Dwell before a capture, so the dominoes come to rest after the motion. real_robot_settle_s = 0.5 - # How far (metres) the bench may be from where the twin predicted before + # How far (metres) the scene may be from where the twin predicted before # the disagreement is worth logging. real_robot_divergence_atol = 0.02 - # --- real-world domino bench (pybullet_domino_real env) ------------------ + # Between episodes, home the arm and wait for a human to rearrange the + # scene, then rebuild that episode's task from what the cameras then see. + # False keeps the captured scene, which is what a fixed-plan + # replay wants (rebuilding would change the objects the plan named). + real_robot_human_reset = True + # --- real-world domino scene (pybullet_domino_real env) ------------------ # The reconstructed-scene JSON (robot_base frame) the pybullet_domino_real # env builds its single train/test task from; the env sizes its domino # component from this scene's role counts. @@ -604,7 +609,7 @@ class GlobalSettings: # an explicit 'role' field per domino. domino_real_start_id = 6 domino_real_target_id = 5 - # Real-bench geometry. + # Real-scene geometry. domino_real_table_z = -0.041 # real table height in the robot base frame domino_real_robot_init_tilt = np.pi domino_real_robot_init_wrist = 0.0 diff --git a/scripts/configs/predicatorv3/exp_domino_real.yaml b/scripts/configs/predicatorv3/exp_domino_real.yaml index aa72ceed5..ca61f984c 100644 --- a/scripts/configs/predicatorv3/exp_domino_real.yaml +++ b/scripts/configs/predicatorv3/exp_domino_real.yaml @@ -1,11 +1,12 @@ -# Thin launcher: real-world domino testing through the STOCK Predicators -# pipeline (replaces real_skills/run_online_learning.py's bespoke lifecycle, -# which bypassed setup_environment and hand-transcribed ~70 flags). +# Thin launcher: real-world domino testing through the Predicators +# pipeline. # Usage: python scripts/local/launch_simp.py -c predicatorv3/exp_domino_real.yaml # With real_robot_execute (default off, i.e. pure sim) # a RealRobotExecutor is attached and its rollouts drive the real Franka -- -# BOTH splits, so exploration episodes execute like evaluation ones, and the -# bench is re-perceived between options to correct the simulated twin. +# BOTH train and testsplits, so exploration episodes execute like evaluation ones, and the +# scene is re-perceived between skills to correct the simulated twin. +# Between episodes a human rearranges the dominoes and the episode's task is +# rebuilt from what the cameras see (real_robot_human_reset). # Env definition lives in envs/all.yaml (domino_real) and the approach in # approaches/all.yaml; this file only un-skips the env + approach it runs and # sets the agent-only ENVS overrides. @@ -14,6 +15,9 @@ includes: - common.yaml - envs/all.yaml - approaches/all.yaml +FLAGS: + num_online_learning_cycles: 2 +NUM_SEEDS: 1 # Agent-only env overrides: deep-merged on top of envs/all.yaml. These # excluded_predicates are dropped for "ours" runs (matches exp_domino.yaml; # oracle.yaml would keep them in). diff --git a/scripts/domino_debug/replay_plan.py b/scripts/domino_debug/replay_plan.py index d952d3fd2..e088e94b2 100644 --- a/scripts/domino_debug/replay_plan.py +++ b/scripts/domino_debug/replay_plan.py @@ -1,9 +1,9 @@ -"""Replay an EXACT solved option plan on the real-bench domino env +"""Replay an EXACT solved option plan on the real-scene domino env (deterministic, no LLM). Grounds the plan's Pick/Place/Push/Wait options with their exact parameters and rolls them through the env in TEST mode. With ``real_robot_execute=True`` a RealRobotExecutor is attached to the env and each option's joint trajectory is shipped to the Franka as that option ends. The -bench is NOT re-perceived between options here: this tool replays a fixed plan, +scene is NOT re-perceived between options here: this tool replays a fixed plan, and correcting the twin mid-replay would let the option policies see states the recorded plan was never chosen against. The default dry-run stays pure sim (optionally rendered to MP4). @@ -30,9 +30,13 @@ import numpy as np from predicators import utils +from predicators.approaches import create_approach +from predicators.cogman import CogMan, run_episode_and_get_observations from predicators.envs import get_or_create_env from predicators.envs.pybullet_domino_real import PyBulletDominoRealEnv +from predicators.execution_monitoring import create_execution_monitor from predicators.ground_truth_models import get_gt_options +from predicators.perception import create_perceiver from predicators.pybullet_helpers.real_robot_executor import attach_real_robot from predicators.settings import CFG from scripts.cluster_utils import SingleSeedRunConfig, generate_run_configs @@ -61,7 +65,7 @@ def _parse_plan(text: str) -> List[Tuple[str, List[str], List[float]]]: def main() -> None: - """Ground the plan's options and roll them through the real-bench env.""" + """Ground the plan's options and roll them through the real-scene env.""" ap = argparse.ArgumentParser( description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter) @@ -88,21 +92,27 @@ def main() -> None: if args.scene: flags["domino_real_scene"] = args.scene flags["real_robot_execute"] = bool(args.execute) - # This tool replays an EXACT plan, so it does not look at the bench even + # This tool replays an EXACT plan, so it does not look at the scene even # though the closed loop is the default elsewhere: re-syncing the twin # mid-replay would let the option policies see states the recorded plan was # never chosen against. It also keeps the tool usable with the cameras down. flags["real_robot_observe_at_option_boundary"] = False + # ...and no human reset either: that rebuilds the episode's task from a + # live look, which would rename and re-place the very objects the recorded + # plan refers to. The task must stay the captured scene the plan was + # written against. + flags["real_robot_human_reset"] = False utils.reset_config(flags) env = get_or_create_env(CFG.env) assert isinstance(env, PyBulletDominoRealEnv), \ - f"replay_plan drives the real-bench env; got {CFG.env}" + f"replay_plan drives the real-scene env; got {CFG.env}" # Attaches the arm under --execute and is a no-op otherwise, so the env # stays the same object either way. attach_real_robot(env) opts = {o.name: o for o in get_gt_options(env.get_name())} - task = env.get_test_tasks()[0].task + env_task = env.get_test_tasks()[0] + task = env_task.task by_name = {o.name: o for o in task.init} with open(args.plan, encoding="utf-8") as f: @@ -124,16 +134,32 @@ def main() -> None: policy = utils.option_plan_to_policy( plan, abstract_function=lambda s: utils.abstract(s, env.predicates)) monitor = utils.VideoMonitor(env.render) if args.out else None - traj, _ = utils.run_policy( - policy, + + # Roll out through CogMan's episode loop -- the same one main.py uses -- + # driven by an override policy, exactly as the online-learning path does + # (main.py sets the override, then resets). With an override in place + # CogMan never asks the approach to solve, so the plan being replayed is + # the plan that executes. That is also why the approach and monitor below + # are the cheap ones: neither is consulted for control, and "oracle" plus + # "trivial" avoid dragging an LLM into a debug replay. + cogman = CogMan( + create_approach("oracle", env.predicates, + get_gt_options(env.get_name()), env.types, + env.action_space, [task]), + create_perceiver(CFG.perceiver), create_execution_monitor("trivial")) + cogman.set_override_policy(policy) + cogman.set_termination_function(lambda s: False) + cogman.reset(env_task) + (_, actions), _, _ = run_episode_and_get_observations( + cogman, env, "test", 0, - termination_function=lambda s: False, max_num_steps=CFG.horizon, + terminate_on_goal_reached=False, exceptions_to_break_on={utils.OptionExecutionFailure}, monitor=monitor) - print(f"# steps={len(traj.actions)} goal_reached={env.goal_reached()}") + print(f"# steps={len(actions)} goal_reached={env.goal_reached()}") if args.out and monitor is not None: os.makedirs(os.path.join(CFG.video_dir, diff --git a/tests/envs/test_domino_real_offline_e2e.py b/tests/envs/test_domino_real_offline_e2e.py new file mode 100644 index 000000000..33905e9c1 --- /dev/null +++ b/tests/envs/test_domino_real_offline_e2e.py @@ -0,0 +1,234 @@ +"""Offline end-to-end: the real-world active-learning loop, no hardware. + +Everything here is real except the arm and the cameras: a genuine +``RealRobot(dry=True)`` with mock perception and an auto-confirming reset, the +real ``RealRobotExecutor``, the real ``CogMan``, and the real +``run_episode_and_get_observations``. Only the arm does not move and the +cameras are a stub. + +It reproduces the exact three-call sequence ``main.py`` uses per interaction +request (``main.py:551-565``):: + + cogman.set_override_policy(request.act_policy) + env_task = env.get_train_tasks()[request.train_task_idx] + cogman.reset(env_task) + run_episode_and_get_observations(cogman, env, "train", ...) + +rather than invoking ``main()`` itself, which would add argument parsing, +result directories and an LLM-backed approach without testing anything more: +the ordering property lives entirely in those four lines. This is the only +test that exercises that ordering against real babyrobot objects, so it skips +without the private submodule. +""" +# The env's caches and the domino component are what this asserts on, +# and babyrobot is imported inside the test body because it is +# optional and absent on CI. +# pylint: disable=protected-access,import-outside-toplevel,import-error +import json +from typing import Any, List, cast + +import numpy as np +import pytest +from gym.spaces import Box +from scipy.spatial.transform import Rotation + +from predicators import utils +from predicators.approaches.base_approach import BaseApproach +from predicators.cogman import CogMan, run_episode_and_get_observations +from predicators.envs.pybullet_domino.real_geometry import _REAL_TO_ENV_BODY +from predicators.envs.pybullet_domino_real import PyBulletDominoRealEnv +from predicators.execution_monitoring import create_execution_monitor +from predicators.perception import create_perceiver +from predicators.pybullet_helpers.real_robot_executor import RealRobotExecutor +from predicators.structs import Action, ParameterizedOption + +_TABLE_Z = -0.041 +_START_ID = 6 +_TARGET_ID = 5 +_EPISODES = 2 # "two online-learning cycles" + + +def _base_quat(roll: float = 0.0, yaw: float = np.pi) -> List[float]: + """The base-frame quaternion of a domino at env ``(roll, 0, yaw)``.""" + r_env = Rotation.from_euler("xyz", [roll, 0.0, yaw]).as_matrix() + r_base = Rotation.from_euler( + "z", -np.pi / 2).as_matrix() @ (r_env @ _REAL_TO_ENV_BODY.T) + return list(Rotation.from_matrix(r_base).as_quat()) + + +@pytest.fixture(name="scene_path") +def scene_path_fixture(tmp_path: Any) -> str: + """A two-domino captured scene.""" + records = [{ + "id": i, + "center_base_m": [x, 0.0, 0.03], + "quat_base_xyzw": _base_quat(), + "dims_m": [0.15, 0.07, 0.029], + } for i, x in ((_START_ID, 0.0), (_TARGET_ID, 0.2))] + path = tmp_path / "scene.json" + path.write_text(json.dumps({ + "frame": "robot_base", + "units": "m", + "dominoes": records + }), + encoding="utf-8") + return str(path) + + +def _config(scene_path: str) -> None: + """The closed-loop real config, with the arm and cameras faked.""" + utils.reset_config({ + "env": "pybullet_domino_real", + "domino_real_scene": scene_path, + "domino_real_table_z": _TABLE_Z, + "domino_real_start_id": _START_ID, + "domino_real_target_id": _TARGET_ID, + "domino_use_domino_blocks_as_target": True, + "domino_use_skill_factories": False, + "domino_real_decorate": False, + "real_robot_execute": True, + "real_robot_dry": True, + "real_robot_observe_at_option_boundary": True, + "real_robot_human_reset": True, + "real_robot_settle_s": 0.0, + "horizon": 6, + }) + + +def _two_option_policy(env: PyBulletDominoRealEnv) -> Any: + """A policy of two one-action options, one opening and one closing. + + Enough to produce two option boundaries per episode -- so two looks + -- and both gripper states, so the commands reaching the hand can be + checked for well-formedness. + """ + layout = env.gripper_joint_layout() + step = {"n": 0} + + def _action(fingers: float) -> Action: + arr = np.zeros(env.action_space.shape, dtype=np.float32) + arr[layout.left_finger_joint_idx] = fingers + arr[layout.right_finger_joint_idx] = fingers + return Action(arr) + + def policy(_state: Any) -> Action: + fingers = (layout.open_fingers + if step["n"] == 0 else layout.closed_fingers) + action = _action(fingers) + param_opt = ParameterizedOption(f"Stub{step['n']}", [], + Box(0, 1, + (1, )), lambda s, m, o, p: action, + lambda s, m, o, p: True, + lambda s, m, o, p: False) + option = param_opt.ground([], np.array([0.5], dtype=np.float32)) + option.terminal = lambda _obs: True # every action ends its option + action.set_option(option) + step["n"] += 1 + return action + + return policy + + +def test_offline_end_to_end_active_learning(scene_path: str) -> None: + """One human prompt per episode, one look per option boundary, and only + well-formed gripper commands reaching the hand.""" + pytest.importorskip("babyrobot") + from babyrobot.realrobot.observations.domino import DominoObservation, \ + DominoPose + from babyrobot.realrobot.perception import MockDominoPerception + from babyrobot.realrobot.real_robot import RealRobot + + _config(scene_path) + env = PyBulletDominoRealEnv(use_gui=False) + + prompts = {"n": 0} + + def _auto_confirm() -> None: + """Stand in for the human at the scene.""" + prompts["n"] += 1 + + seen = DominoObservation( + stamp=0.0, + dominoes=(DominoPose(id=_START_ID, + xyz=(0.0, 0.0, 0.03), + quat_xyzw=tuple(_base_quat())), + DominoPose(id=_TARGET_ID, + xyz=(0.22, 0.0, 0.03), + quat_xyzw=tuple(_base_quat())))) + + class _CountingPerception(MockDominoPerception): + """Mock perception that records how often it was asked to look.""" + + def __init__(self) -> None: + super().__init__(seen) + self.looks = 0 + + def observe(self, settle_s: float = 0.0) -> Any: + """Record the look, then report the fixed scene.""" + self.looks += 1 + return super().observe(settle_s) + + perception = _CountingPerception() + robot = RealRobot(perception=perception, + dry=True, + confirm_reset=_auto_confirm) + try: + executor = RealRobotExecutor(env, + robot, + observe_at_boundaries=True, + settle_s=0.0, + human_reset=True) + env.attach_executor(executor) + # Cast: CogMan is typed against BaseApproach, but with an override + # policy set it only ever calls the three methods the stub has. + cogman = CogMan(cast(BaseApproach, _StubApproach()), + create_perceiver("trivial"), + create_execution_monitor("trivial")) + + for _ in range(_EPISODES): + cogman.set_override_policy(_two_option_policy(env)) + cogman.set_termination_function(lambda s: False) + env_task = env.get_train_tasks()[0] + cogman.reset(env_task) + run_episode_and_get_observations(cogman, + env, + "train", + 0, + max_num_steps=2, + terminate_on_goal_reached=False) + + # One human reset per episode -- not zero (never asked) and not two + # (asked again mid-episode, which the caching defeat could cause). + assert prompts["n"] == _EPISODES + assert executor.resets_done == _EPISODES + # Each episode: one look for the reset, plus one per option boundary. + assert perception.looks == _EPISODES * 3 + # And the hand only ever saw well-formed commands. + assert robot.last_gripper_command in ("open", "close") + finally: + robot.close() + + +class _StubApproach: + """The narrowest thing CogMan will accept. + + An override policy is always set, so the approach is never asked to + solve; it exists because ``CogMan.reset`` calls these three methods. + """ + + @property + def is_learning_based(self) -> bool: + """Unused here.""" + return False + + @classmethod + def get_name(cls) -> str: + """Read by CogMan when deciding whether to render observations.""" + return "stub" + + def reset_for_new_episode(self) -> None: + """Nothing to reset.""" + + def get_execution_monitoring_info(self) -> List[Any]: + """No info to hand the monitor.""" + return [] diff --git a/tests/envs/test_domino_real_online.py b/tests/envs/test_domino_real_online.py new file mode 100644 index 000000000..9181f3422 --- /dev/null +++ b/tests/envs/test_domino_real_online.py @@ -0,0 +1,284 @@ +"""Real-world active learning: the human-gated task rebuild. + +The property under test is an *ordering* one, and it is the whole reason this +lands at task-request time rather than in ``env.reset``. The loop is + + env_task = env.get_train_tasks()[i] # (1) + cogman.reset(env_task) # (2) + run_episode_and_get_observations(...) # (3) calls env.reset(...) + +and the task must be rebuilt at (1) because both callers have already +consumed it by (3). On the evaluation path (2) *solves* -- ``_solve_task`` +resets with no override policy, so ``_reset_policy`` calls +``approach.solve`` -- and a reset at (3) would plan against a scene that no +longer exists. On the exploration path an override policy is set first, so +(2) does not solve; but the task is what ``env.reset`` initializes the twin +from, so a stale one starts the episode from the captured scene rather +than the one just arranged. + +No hardware and no babyrobot: the robot is a fake whose ``reset_env`` records +the prompt and replies with whatever the test wants the cameras to have seen. +These must never skip. +""" +# The env's task caches, component and executor slot are what these tests +# assert on, so reading them is the point. +# pylint: disable=protected-access +import json +from typing import Any, List + +import numpy as np +import pytest +from scipy.spatial.transform import Rotation + +from predicators import utils +from predicators.envs.pybullet_domino.real_geometry import _REAL_TO_ENV_BODY +from predicators.envs.pybullet_domino_real import PyBulletDominoRealEnv +from predicators.pybullet_helpers.real_robot_executor import RealRobotExecutor +from predicators.structs import EnvironmentTask, Object + +_TABLE_Z = -0.041 +_START_ID = 6 +_TARGET_ID = 5 + + +def _base_quat(roll: float = 0.0, + yaw: float = np.pi, + pitch: float = 0.0) -> List[float]: + """The base-frame quaternion of a domino at env ``(roll, pitch, yaw)``.""" + r_env = Rotation.from_euler("xyz", [roll, pitch, yaw]).as_matrix() + r_base = Rotation.from_euler( + "z", -np.pi / 2).as_matrix() @ (r_env @ _REAL_TO_ENV_BODY.T) + return list(Rotation.from_matrix(r_base).as_quat()) + + +class _StubDominoPose: + """Stands in for babyrobot's DominoPose.""" + + def __init__(self, capture_id: int, xyz: Any, quat_xyzw: Any) -> None: + self.id = capture_id + self.xyz = tuple(xyz) + self.quat_xyzw = tuple(quat_xyzw) + + +class _StubDominoObservation: + """Stands in for babyrobot's DominoObservation.""" + + def __init__(self, dominoes: Any) -> None: + self.dominoes = list(dominoes) + + +def _observation(target_base_x: float) -> _StubDominoObservation: + """A two-domino look with the target at a chosen base-frame x.""" + return _StubDominoObservation([ + _StubDominoPose(_START_ID, (0.0, 0.0, 0.03), _base_quat()), + _StubDominoPose(_TARGET_ID, (target_base_x, 0.0, 0.03), _base_quat()), + ]) + + +class _FakeRobot: + """Records human-reset prompts; replies with queued observations.""" + + has_perception = True + dry = True + + def __init__(self, observations: Any) -> None: + self.prompts = 0 + self._queue = list(observations) + self.last_returned: Any = None + + def reset_env(self, req: Any) -> Any: + """Stand in for home-arm + wait-for-human + look.""" + del req + self.prompts += 1 + self.last_returned = self._queue.pop(0) if self._queue else None + return self.last_returned + + +def _config(scene_path: str, **overrides: Any) -> None: + """The real-execution config for this env.""" + flags = { + "env": "pybullet_domino_real", + "domino_real_scene": scene_path, + "domino_real_table_z": _TABLE_Z, + "domino_real_start_id": _START_ID, + "domino_real_target_id": _TARGET_ID, + "domino_use_domino_blocks_as_target": True, + "domino_use_skill_factories": False, + "domino_real_decorate": False, + "real_robot_execute": True, + "real_robot_observe_at_option_boundary": False, + "real_robot_human_reset": True, + } + flags.update(overrides) + utils.reset_config(flags) + + +@pytest.fixture(scope="module", name="scene_path") +def scene_path_fixture(tmp_path_factory: Any) -> str: + """A captured scene with the target 20cm along base +x.""" + records = [{ + "id": i, + "center_base_m": [x, 0.0, 0.03], + "quat_base_xyzw": _base_quat(), + "dims_m": [0.15, 0.07, 0.029], + } for i, x in ((_START_ID, 0.0), (_TARGET_ID, 0.2))] + path = tmp_path_factory.mktemp("domino_online") / "scene.json" + path.write_text(json.dumps({ + "frame": "robot_base", + "units": "m", + "dominoes": records + }), + encoding="utf-8") + return str(path) + + +@pytest.fixture(scope="module", name="env") +def env_fixture(scene_path: str) -> PyBulletDominoRealEnv: + """One real twin for the module -- building PyBullet per test is slow.""" + _config(scene_path) + return PyBulletDominoRealEnv(use_gui=False) + + +@pytest.fixture(autouse=True) +def _clean_env(env: PyBulletDominoRealEnv, scene_path: str) -> Any: + """Give each test a pristine shared env. + + The env is module-scoped because building PyBullet is slow, so its + executor and its cached tasks -- the very things under test -- have + to be cleared between tests. + """ + _config(scene_path) + env._executor = None + env._train_tasks = [] + env._test_tasks = [] + yield + env._executor = None + env._train_tasks = [] + env._test_tasks = [] + + +@pytest.fixture(name="attached") +def attached_fixture(env: PyBulletDominoRealEnv, monkeypatch: Any) -> Any: + """Attach an executor whose robot traffic is recorded, not performed.""" + # reset_arm and execute_chunks would import babyrobot. The human reset is + # what these tests are about, so it is the only one that does anything. + monkeypatch.setattr( + "predicators.pybullet_helpers.real_robot_executor.reset_arm", + lambda robot, joints: tuple(joints)) + monkeypatch.setattr( + "predicators.pybullet_helpers.real_robot_executor.execute_chunks", + lambda *a, **k: []) + monkeypatch.setattr( + "predicators.pybullet_helpers.real_robot_executor.reset_env", + lambda robot, joints=None: robot.reset_env(None)) + + def _attach(robot: Any, human_reset: bool = True) -> RealRobotExecutor: + executor = RealRobotExecutor(env, + robot, + observe_at_boundaries=False, + human_reset=human_reset) + env.attach_executor(executor) + return executor + + return _attach + + +def _target(env: PyBulletDominoRealEnv) -> Object: + """The purple target domino, which sits in slot 1 of this scene.""" + comp = env._domino_component + assert comp is not None, "env has no domino component" + return comp.dominos[1] + + +# -- the ordering property --------------------------------------------------- +def test_the_task_is_rebuilt_before_the_approach_would_solve( + env: PyBulletDominoRealEnv, attached: Any) -> None: + """``get_train_tasks`` blocks for the human and returns the REBUILT task. + + That is the ordering fix: the caller solves against what it gets + back here, so what it gets back must already reflect the new scene. + """ + robot = _FakeRobot([_observation(target_base_x=0.30)]) + attached(robot) + + tasks = env.get_train_tasks() + + assert robot.prompts == 1, \ + "the human was not asked before the task was given" + assert len(tasks) == 1 + # base_x 0.30 -> world y = 0.72 + 0.30; the captured scene had 0.2. + assert tasks[0].init.get(_target(env), "y") == pytest.approx(0.72 + 0.30) + + +def test_each_episode_prompts_exactly_once(env: PyBulletDominoRealEnv, + attached: Any) -> None: + """One prompt per episode: asking again inside an episode must not re- + prompt, and the next episode must.""" + robot = _FakeRobot([_observation(0.30), _observation(0.25)]) + attached(robot) + + env.get_train_tasks() + assert robot.prompts == 1 + # Same episode: the task is already built, so no second prompt. + env.get_train_tasks() + env.get_train_tasks() + assert robot.prompts == 1 + + # An episode begins; that is what owes the next reset. + env.reset("train", 0) + env.get_train_tasks() + assert robot.prompts == 2 + + +def test_a_new_look_replaces_the_cached_task(env: PyBulletDominoRealEnv, + attached: Any) -> None: + """BaseEnv caches tasks, so without this every episode would replan against + one frozen capture. + + The second episode must see the second look. + """ + robot = _FakeRobot([_observation(0.30), _observation(0.10)]) + attached(robot) + target = _target(env) + + first = env.get_train_tasks()[0] + env.reset("train", 0) + second = env.get_train_tasks()[0] + + assert first.init.get(target, "y") == pytest.approx(0.72 + 0.30) + assert second.init.get(target, "y") == pytest.approx(0.72 + 0.10) + + +def test_test_split_is_rebuilt_too(env: PyBulletDominoRealEnv, + attached: Any) -> None: + """Evaluation episodes face a freshly arranged scene as well.""" + robot = _FakeRobot([_observation(0.28)]) + attached(robot) + + tasks = env.get_test_tasks() + + assert robot.prompts == 1 + assert isinstance(tasks[0], EnvironmentTask) + assert tasks[0].init.get(_target(env), "y") == pytest.approx(0.72 + 0.28) + + +def test_no_human_reset_keeps_the_captured_scene(env: PyBulletDominoRealEnv, + attached: Any) -> None: + """With human resets off nothing is prompted and nothing is rebuilt, which + is what a fixed-plan replay depends on.""" + robot = _FakeRobot([_observation(0.30)]) + attached(robot, human_reset=False) + + tasks = env.get_train_tasks() + + assert robot.prompts == 0 + # The captured scene put the target at base_x 0.2. + assert tasks[0].init.get(_target(env), "y") == pytest.approx(0.72 + 0.2) + + +def test_an_unattached_env_never_prompts(env: PyBulletDominoRealEnv) -> None: + """A pure-sim run -- including every env the planner builds -- must not + acquire a human in the loop.""" + tasks = env.get_train_tasks() + + assert tasks[0].init.get(_target(env), "y") == pytest.approx(0.72 + 0.2) diff --git a/tests/envs/test_real_robot_execution.py b/tests/envs/test_real_robot_execution.py index ebc5dd977..995de173f 100644 --- a/tests/envs/test_real_robot_execution.py +++ b/tests/envs/test_real_robot_execution.py @@ -3,7 +3,7 @@ The executor's own tests use a stub env to cover its logic in isolation. This file does the opposite: a **real** ``PyBulletDominoRealEnv`` with a real physics client, driven through ``env.step`` exactly as the episode loop does, -so the whole closed loop is exercised -- ship a chunk, look at the bench, +so the whole closed loop is exercised -- ship a chunk, look at the scene, convert what was seen, write it into the twin, read it back out through the agent-facing observation. @@ -94,6 +94,9 @@ def _config(scene_path, **overrides): "real_robot_observe_at_option_boundary": True, "real_robot_settle_s": 0.0, "real_robot_divergence_atol": 0.02, + # These cover per-option execution and the twin sync. The human-gated + # task rebuild is its own concern, in test_domino_real_online.py. + "real_robot_human_reset": False, } flags.update(overrides) utils.reset_config(flags) @@ -319,13 +322,13 @@ def test_closed_loop_against_a_real_dry_robot(inner, scene_path, tmp_path): reached through the real ``execute_chunks``. This is the only test that exercises the actual ``StepRequest`` / ``Segment`` construction and the gripper dedup, so a contract change on either side of the - submodule boundary surfaces here rather than on the bench. + submodule boundary surfaces here rather than on the real scene. """ pytest.importorskip("babyrobot") from babyrobot.realrobot.perception import FileDominoPerception from babyrobot.realrobot.real_robot import RealRobot - # The bench the cameras will "see": the target lying on its face. + # The scene the cameras will "see": the target lying on its face. seen = { "frame": "robot_base", diff --git a/tests/pybullet_helpers/test_real_robot_executor.py b/tests/pybullet_helpers/test_real_robot_executor.py index 6aa88b937..6fad2e39f 100644 --- a/tests/pybullet_helpers/test_real_robot_executor.py +++ b/tests/pybullet_helpers/test_real_robot_executor.py @@ -271,7 +271,7 @@ class _NoHooks(_StubEnv): # pylint: disable=abstract-method def test_construction_rejects_observing_without_perception(): - """Asking to look at the bench with no cameras fails at construction, + """Asking to look at the scene with no cameras fails at construction, rather than raising inside the first option boundary.""" with pytest.raises(ValueError) as exc: _executor(_StubEnv(), _StubRobot(has_perception=False)) @@ -279,11 +279,29 @@ def test_construction_rejects_observing_without_perception(): def test_blind_run_needs_no_perception(): - """Turning the look off is a supported (open-loop) mode, so a robot without - cameras is fine there.""" + """A blind open-loop run is supported, so a robot with no cameras is fine. + + -- provided nothing else asks to look, human resets included. + """ assert _executor(_StubEnv(), _StubRobot(has_perception=False), - observe_at_boundaries=False) is not None + observe_at_boundaries=False, + human_reset=False) is not None + + +def test_human_reset_without_cameras_is_refused(): + """A human reset rebuilds the task from a look, so it cannot be honoured + without perception. + + Better to say so at construction than to raise on the first task + request, after the human has already been asked to stand by. + """ + with pytest.raises(ValueError) as exc: + _executor(_StubEnv(), + _StubRobot(has_perception=False), + observe_at_boundaries=False, + human_reset=True) + assert "human reset" in str(exc.value) # -- reset ------------------------------------------------------------------- @@ -407,7 +425,8 @@ def test_no_sync_when_not_observing(recorder): env = _StubEnv() executor = _executor(env, _StubRobot(has_perception=False), - observe_at_boundaries=False) + observe_at_boundaries=False, + human_reset=False) executor.after_step(_act(_option(), terminal=True), env.get_observation()) @@ -416,7 +435,7 @@ def test_no_sync_when_not_observing(recorder): def test_large_divergence_is_surfaced(recorder, caplog): - """A bench that disagrees with the twin is reported rather than swallowed. + """A scene that disagrees with the twin is reported rather than swallowed. ``_set_state``'s own reconstruction check cannot catch this: it measures whether PyBullet could realize the state it was asked to