real robot: a fixed-plan explorer, and a record of every look

Three pieces for exercising the online loop on hardware, where the cost is
wall-clock rather than compute.

fixed_plan explorer. The planning explorers cost minutes per episode, so a
full cycle is expensive to run for the sake of testing the loop around it.
This one replays a plan file every episode and costs nothing, so a run that
goes wrong is the loop's fault and not the planner's. It reads replay_plan's
format, which means a plan dumped by probe_real_scene --dump-plan and already
verified through replay_plan can be handed straight to the loop. Grounded per
episode, because a human reset rebuilds the task and hands back fresh Objects.

Every look is logged, not only the ones over tolerance. A run whose looks all
behaved previously said nothing at all, so there was no way to distinguish
"perception was healthy" from "nobody checked". The line carries the
per-object breakdown, which is what tells a single knocked domino apart from a
table-height offset shared by all of them -- the max alone cannot.

Looks can be dumped to JSON (real_robot_observation_dump_dir, off by default).
Each file records what was perceived beside what the twin predicted, so a
session can be re-examined offline without the robot. Write failures are
logged and swallowed: losing a dump must not take the arm down mid-episode.

The learner is untouched.

Author: amburger66

predicators/explorers/fixed_plan_explorer.py

@@ -0,0 +1,96 @@
+"""An explorer that replays one fixed option plan from a file.
+
+A stand-in for the planning explorers when what is being tested is the online
+loop itself rather than what the agent chooses. The LLM-backed explorers cost
+minutes per episode, which makes a full cycle expensive to exercise on
+hardware; this one costs nothing and does the same thing every episode, so a
+run that goes wrong is the loop's fault and not the planner's.
+
+The plan file is ``replay_plan.py``'s format -- one grounded option per line,
+``-> {...}`` subgoals optional and ignored::
+
+    Pick(robot:robot, domino_1:domino)[0.0657]
+    Place(robot:robot)[0.70, 1.16, 0.55, 1.75]
+    Wait(robot:robot)[]
+
+so a plan dumped by ``probe_real_scene --dump-plan`` and verified through
+``replay_plan`` can be handed straight to the loop.
+"""
+
+import logging
+import re
+from typing import Dict, List, Tuple
+
+import numpy as np
+
+from predicators import utils
+from predicators.explorers import BaseExplorer
+from predicators.settings import CFG
+from predicators.structs import ExplorationStrategy, Object, State, _Option
+
+# Same grammar as scripts/domino_debug/replay_plan.py.
+_LINE = re.compile(r"^\s*(\w+)\s*\(([^)]*)\)\s*\[([^\]]*)\]")
+
+
+def _parse_plan(text: str) -> List[Tuple[str, List[str], List[float]]]:
+    """[(option_name, [obj_names], [param_floats]), ...] from the plan text."""
+    steps: List[Tuple[str, List[str], List[float]]] = []
+    for raw in text.splitlines():
+        line = raw.split("->", 1)[0].strip()
+        if not line or line.startswith("#"):
+            continue
+        match = _LINE.match(line)
+        if not match:
+            continue
+        objs = [
+            a.split(":", 1)[0].strip() for a in match.group(2).split(",")
+            if a.strip()
+        ]
+        floats = [float(v) for v in match.group(3).split(",") if v.strip()]
+        steps.append((match.group(1), objs, floats))
+    return steps
+
+
+class FixedPlanExplorer(BaseExplorer):
+    """Replays the plan at ``CFG.fixed_plan_explorer_path`` every episode."""
+
+    @classmethod
+    def get_name(cls) -> str:
+        return "fixed_plan"
+
+    def _ground(self, state: State) -> List[_Option]:
+        """Ground the plan's options against the objects in ``state``.
+
+        Grounded per episode rather than once, because a human reset
+        rebuilds the task and hands back fresh ``Object`` instances.
+        """
+        path = CFG.fixed_plan_explorer_path
+        assert path, "fixed_plan explorer needs fixed_plan_explorer_path"
+        with open(path, encoding="utf-8") as f:
+            steps = _parse_plan(f.read())
+        assert steps, f"no plan steps parsed from {path}"
+        options: Dict[str, object] = {o.name: o for o in self._options}
+        by_name: Dict[str, Object] = {o.name: o for o in state}
+        plan = []
+        for name, obj_names, params in steps:
+            option = options[name]
+            objs = [by_name[n] for n in obj_names]
+            plan.append(
+                option.ground(  # type: ignore[attr-defined]
+                    objs, np.array(params, dtype=np.float32)))
+        return plan
+
+    def _get_exploration_strategy(self, train_task_idx: int,
+                                  timeout: int) -> ExplorationStrategy:
+        del timeout  # the plan is fixed; there is nothing to search
+        state = self._train_tasks[train_task_idx].init
+        plan = self._ground(state)
+        logging.info("fixed_plan explorer: %s", [o.simple_str() for o in plan])
+        policy = utils.option_plan_to_policy(
+            plan,
+            abstract_function=lambda s: utils.abstract(s, self._predicates))
+        # Never terminate early: the plan running out raises
+        # OptionExecutionFailure, which the interaction loop already handles,
+        # and stopping sooner would cut the episode short of the plan.
+        termination_function = lambda _: False
+        return policy, termination_function

predicators/pybullet_helpers/real_robot_executor.py

@@ -16,8 +16,10 @@
 """
 from __future__ import annotations
 
+import json
 import logging
-from typing import Any, List, Optional, Protocol, cast
+import os
+from typing import Any, Dict, List, Optional, Protocol, Tuple, cast
 
 import numpy as np
 
@@ -97,6 +99,8 @@ class TwinCorrector:
     def __init__(self, env: PyBulletEnv, divergence_atol: float) -> None:
         self._env = env
         self._divergence_atol = divergence_atol
+        # Looks so far, used to name the dumps in the order they happened.
+        self._look_count = 0
         # Largest per-object position disagreement between the twin and the
         # real scene at the last look, in metres; None before the first look.
         self.last_divergence: Optional[float] = None
@@ -118,20 +122,92 @@ def absorb(self, observation: Any) -> Observation:
         domain = cast(_DomainHooks, self._env)
         perceived = domain.state_from_observation(observation, predicted)
         self.last_divergence = _max_position_divergence(predicted, perceived)
+        self._look_count += 1
+        per_object = _per_object_divergence(predicted, perceived)
+        # Log every look, not only the ones over tolerance: a run whose looks
+        # all behaved should still say so, and the per-object breakdown is
+        # what distinguishes one bad capture from a systematic offset.
+        logging.info(
+            "real robot: look %d, worst %.4f m (tolerance %.3f m); %s",
+            self._look_count, self.last_divergence or float("nan"),
+            self._divergence_atol, ", ".join(f"{obj.name} {dist:.4f}"
+                                             for obj, dist in per_object))
         if self.last_divergence is not None and \
                 self.last_divergence > self._divergence_atol:
             logging.warning(
                 "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)
+        _dump_look(self._look_count, predicted, perceived, per_object,
+                   self.last_divergence)
         self._env.sync_to_state(perceived)
         # No need to refresh the env's cached observation by hand:
         # PyBulletEnv.get_observation re-reads the state out of PyBullet, so
         # this picks the corrected world up (and re-caches it).
         return self._env.get_observation()
 
 
+def _per_object_divergence(predicted: State,
+                           perceived: State) -> List[Tuple[Any, float]]:
+    """Per-object ``(object, distance)``, worst first.
+
+    ``_max_position_divergence`` answers "how bad", which is what the
+    tolerance is checked against; this answers "which object", which is
+    what tells a knocked domino apart from a table-height offset shared
+    by all of them.
+    """
+    out = []
+    for obj in predicted.data:
+        if obj not in perceived.data:
+            continue
+        if not {"x", "y", "z"}.issubset(obj.type.feature_names):
+            continue
+        delta = np.array(
+            [predicted.get(obj, f) - perceived.get(obj, f) for f in "xyz"])
+        out.append((obj, float(np.linalg.norm(delta))))
+    return sorted(out, key=lambda pair: pair[1], reverse=True)
+
+
+def _dump_look(index: int, predicted: State, perceived: State,
+               per_object: List[Tuple[Any,
+                                      float]], worst: Optional[float]) -> None:
+    """Write one look to ``CFG.real_robot_observation_dump_dir`` as JSON.
+
+    Records the twin's prediction beside what was perceived, so a
+    session can be re-examined offline -- including the looks that
+    raised no warning. Failing to write must never take the arm down
+    mid-episode, so any error here is logged and swallowed.
+    """
+    out_dir = CFG.real_robot_observation_dump_dir
+    if not out_dir:
+        return
+
+    def _poses(state: State) -> Dict[str, List[float]]:
+        return {
+            obj.name: [float(state.get(obj, f)) for f in "xyz"]
+            for obj in state.data
+            if {"x", "y", "z"}.issubset(obj.type.feature_names)
+        }
+
+    record = {
+        "look": index,
+        "worst_divergence": worst,
+        "divergence_atol": CFG.real_robot_divergence_atol,
+        "predicted": _poses(predicted),
+        "perceived": _poses(perceived),
+        "per_object": {obj.name: dist
+                       for obj, dist in per_object},
+    }
+    try:
+        os.makedirs(out_dir, exist_ok=True)
+        path = os.path.join(out_dir, f"look_{index:04d}.json")
+        with open(path, "w", encoding="utf-8") as f:
+            json.dump(record, f, indent=2, sort_keys=True)
+    except OSError as exc:  # pragma: no cover - disk trouble only
+        logging.warning("real robot: could not write %s (%s)", out_dir, exc)
+
+
 class RealRobotExecutor:
     """Ships each option's trajectory to the arm and corrects the twin.
 

predicators/settings.py

@@ -585,6 +585,16 @@ class GlobalSettings:
     # How far (metres) the scene may be from where the twin predicted before
     # the disagreement is worth logging.
     real_robot_divergence_atol = 0.02
+    # Write every option-boundary look to this directory as JSON: what was
+    # perceived, what the twin predicted, and the per-object disagreement.
+    # Empty disables it. The divergence WARNING only fires above tolerance, so
+    # without this a run leaves no record of the looks that behaved -- and no
+    # way to tell a systematic offset from one bad capture after the fact.
+    real_robot_observation_dump_dir = ""
+    # Plan file the "fixed_plan" explorer replays every episode, in
+    # replay_plan.py's format. Lets the online loop be exercised without
+    # paying for a planning explorer.
+    fixed_plan_explorer_path = ""
     # 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

tests/explorers/test_fixed_plan_explorer.py

@@ -0,0 +1,75 @@
+"""Test cases for the fixed plan explorer class."""
+import pytest
+
+from predicators import utils
+from predicators.envs.cover import CoverEnv
+from predicators.explorers import create_explorer
+from predicators.explorers.fixed_plan_explorer import _parse_plan
+from predicators.ground_truth_models import get_gt_options
+
+
+def _write_plan(tmp_path, text):
+    path = tmp_path / "plan.txt"
+    path.write_text(text, encoding="utf-8")
+    return str(path)
+
+
+def test_parse_plan_matches_replay_plans_format():
+    """The explorer reads what probe_real_scene --dump-plan writes, so a plan
+    verified through replay_plan can be handed straight to the loop.
+
+    That includes the '#' header those files carry and the '-> {...}'
+    subgoal tail a solved plan may carry.
+    """
+    steps = _parse_plan("# scene : whatever.json\n"
+                        "# solved : False\n"
+                        "\n"
+                        "Pick(robot:robot, domino_1:domino)[0.0657]\n"
+                        "Place(robot:robot)[0.70, 1.16] -> {Holding(x)}\n"
+                        "Wait(robot:robot)[]\n")
+    assert steps == [
+        ("Pick", ["robot", "domino_1"], [0.0657]),
+        ("Place", ["robot"], [0.70, 1.16]),
+        ("Wait", ["robot"], []),
+    ]
+
+
+def test_fixed_plan_explorer_replays_the_file(tmp_path):
+    """The explorer's policy executes the plan on the file, not a search."""
+    # cover's only option is PickPlace(), one param and no objects.
+    plan_path = _write_plan(tmp_path, "PickPlace()[0.75]\n")
+    utils.reset_config({
+        "env": "cover",
+        "seed": 0,
+        "explorer": "fixed_plan",
+        "fixed_plan_explorer_path": plan_path,
+    })
+    env = CoverEnv()
+    train_tasks = [t.task for t in env.get_train_tasks()]
+    explorer = create_explorer("fixed_plan", env.predicates,
+                               get_gt_options("cover"), env.types,
+                               env.action_space, train_tasks)
+    policy, termination_function = explorer.get_exploration_strategy(0, 500)
+
+    # Never terminates on its own; the plan running out is what ends it.
+    assert not termination_function(train_tasks[0].init)
+    act = policy(train_tasks[0].init)
+    assert env.action_space.contains(act.arr)
+    assert act.get_option().name == "PickPlace"
+
+
+def test_fixed_plan_explorer_needs_a_path():
+    """A missing path fails loudly rather than exploring some other way."""
+    utils.reset_config({
+        "env": "cover",
+        "seed": 0,
+        "explorer": "fixed_plan",
+        "fixed_plan_explorer_path": "",
+    })
+    env = CoverEnv()
+    train_tasks = [t.task for t in env.get_train_tasks()]
+    explorer = create_explorer("fixed_plan", env.predicates,
+                               get_gt_options("cover"), env.types,
+                               env.action_space, train_tasks)
+    with pytest.raises(AssertionError, match="fixed_plan_explorer_path"):
+        explorer.get_exploration_strategy(0, 500)

tests/pybullet_helpers/test_real_robot_executor.py

@@ -13,6 +13,7 @@
 """
 import ast
 import inspect
+import json
 from typing import Any, List, Optional, cast
 
 import numpy as np
@@ -23,7 +24,8 @@
 from predicators.envs.pybullet_env import PyBulletEnv
 from predicators.pybullet_helpers.real_robot_bridge import GripperJointLayout
 from predicators.pybullet_helpers.real_robot_executor import \
-    OptionBoundaryBuffer, RealRobotExecutor, attach_real_robot
+    OptionBoundaryBuffer, RealRobotExecutor, _dump_look, \
+    _per_object_divergence, attach_real_robot
 from predicators.structs import Action, Object, ParameterizedOption, State, \
     Type
 
@@ -486,3 +488,72 @@ def test_attach_rejects_a_non_pybullet_env():
     with pytest.raises(TypeError) as exc:
         attach_real_robot(cast(Any, object()), _StubRobot())
     assert "PyBullet" in str(exc.value)
+
+
+# -- per-look observability --------------------------------------------------
+
+_OTHER = Object("block1", _BLOCK_TYPE)
+
+
+def _two_object_state(x0: float, x1: float) -> State:
+    """A two-object state, so a per-object breakdown has something to break
+    down."""
+    return State({
+        _BLOCK: np.array([x0, 0.0, 0.0]),
+        _OTHER: np.array([x1, 0.0, 0.0]),
+    })
+
+
+def test_per_object_divergence_names_the_object_and_orders_by_distance():
+    """The max alone cannot tell a knocked object from a shared offset.
+
+    ``_max_position_divergence`` answers "how bad", which is what the
+    tolerance tests; this answers "which", which is what a human reads.
+    """
+    predicted = _two_object_state(0.0, 0.0)
+    perceived = _two_object_state(0.01, 0.05)
+
+    result = _per_object_divergence(predicted, perceived)
+
+    assert [obj.name for obj, _ in result] == ["block1", "block0"]
+    assert result[0][1] == pytest.approx(0.05)
+    assert result[1][1] == pytest.approx(0.01)
+
+
+def test_dump_look_writes_both_sides_of_the_comparison(tmp_path):
+    """A dumped look records the prediction beside the perception.
+
+    Recording only the divergence would leave a session unable to say
+    WHERE things were, which is what makes a capture re-examinable
+    offline.
+    """
+    utils.reset_config({
+        "seed": 0,
+        "real_robot_observation_dump_dir": str(tmp_path),
+    })
+    predicted = _two_object_state(0.0, 0.0)
+    perceived = _two_object_state(0.01, 0.05)
+
+    _dump_look(3, predicted, perceived,
+               _per_object_divergence(predicted, perceived), 0.05)
+
+    written = sorted(tmp_path.glob("*.json"))
+    assert [f.name for f in written] == ["look_0003.json"]
+    record = json.loads(written[0].read_text(encoding="utf-8"))
+    assert record["look"] == 3
+    assert record["worst_divergence"] == pytest.approx(0.05)
+    assert record["predicted"]["block1"] == [0.0, 0.0, 0.0]
+    assert record["perceived"]["block1"] == [0.05, 0.0, 0.0]
+    assert record["per_object"]["block1"] == pytest.approx(0.05)
+
+
+def test_dump_look_is_off_by_default(tmp_path):
+    """Dumping is opt-in: an unset directory writes nothing at all."""
+    utils.reset_config({"seed": 0, "real_robot_observation_dump_dir": ""})
+    predicted = _two_object_state(0.0, 0.0)
+    perceived = _two_object_state(0.01, 0.0)
+
+    _dump_look(1, predicted, perceived,
+               _per_object_divergence(predicted, perceived), 0.01)
+
+    assert not list(tmp_path.iterdir())