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

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/ground_truth_models/skill_factories/wait.py

@@ -25,7 +25,31 @@
 from predicators import utils
 from predicators.ground_truth_models.skill_factories.base import SkillConfig
 from predicators.structs import Action, Array, Object, ParameterizedOption, \
-    State, Type
+    State, Type, _Option
+
+
+def note_external_state_change(option: _Option, state: State) -> None:
+    """Tell ``option`` that ``state`` was set from outside, not moved into.
+
+    ``Wait`` ends once the scene holds still for several consecutive steps.
+    Writing perception into the twin replaces object poses without the
+    scene having moved, so counting that jump would zero the tally at
+    every look and ``Wait`` would never see the scene settle. This keeps
+    the tally and moves the comparison point past the jump, so the jump is
+    skipped rather than counted as motion.
+
+    A no-op for options that track no quiescence.
+    """
+    memory = option.memory
+    if "quiescence_prev" not in memory:
+        return
+    robot_obj = option.objects[0]
+    scene_objs = sorted((o for o in state if o != robot_obj), key=str)
+    if not scene_objs:
+        return
+    memory["quiescence_prev"] = state.vec(scene_objs)
+    # The cached identity names the pre-resync state, so drop it.
+    memory.pop("quiescence_sref", None)
 
 
 def create_wait_option(

predicators/pybullet_helpers/real_robot_executor.py

@@ -16,14 +16,18 @@
 """
 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
 
 from predicators import utils
 from predicators.envs.base_env import BaseEnv
 from predicators.envs.pybullet_env import PyBulletEnv
+from predicators.ground_truth_models.skill_factories.wait import \
+    note_external_state_change
 from predicators.pybullet_helpers.real_robot_bridge import execute_chunks, \
     make_real_robot, reset_arm, reset_env
 from predicators.settings import CFG
@@ -38,6 +42,24 @@
 }
 
 
+def _ends_at(option: Any, obs: Observation) -> bool:
+    """Whether ``option`` ends at ``obs``, without disturbing the option.
+
+    ``terminal`` may be stateful: ``Wait`` counts consecutive settled
+    steps in ``option.memory``, and the option's own policy is already
+    counting that series one call per step. Asking here without putting
+    the memory back would insert an extra sample per step, so ``Wait``
+    would judge the scene settled in a third of the steps it actually
+    takes.
+    """
+    saved = dict(option.memory)
+    try:
+        return cast(bool, option.terminal(obs))
+    finally:
+        option.memory.clear()
+        option.memory.update(saved)
+
+
 class _DomainHooks(Protocol):
     """The domain-specific conversions, which no base class can declare.
 
@@ -79,7 +101,7 @@ def add(self, action: Action, obs: Observation) -> Optional[List[Action]]:
         if not action.has_option():
             return None
         self._actions.append(action)
-        if not action.get_option().terminal(obs):
+        if not _ends_at(action.get_option(), obs):
             return None
         chunk, self._actions = self._actions, []
         return chunk
@@ -97,6 +119,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 +142,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.
 
@@ -271,6 +367,11 @@ def after_step(self, action: Action, obs: Observation) -> Observation:
                                       settle_s=self._settle_s)
         for observation in observations:
             obs = self._corrector.absorb(observation)
+        # The correction moved objects, but the scene did not move. Options
+        # that judge the scene settled have to be told, or every look would
+        # read as motion and they would never see it come to rest.
+        if isinstance(obs, State):
+            note_external_state_change(action.get_option(), obs)
         return obs
 
     # -- helpers -----------------------------------------------------------

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

scripts/configs/predicatorv3/stage6_rehearsal_domino_real.yaml

@@ -0,0 +1,61 @@
+# A cheap rehearsal of Stage 6 (real active learning) from
+# docs/real_robot_bringup.md -- the SHAPE of the online loop, with the two
+# expensive parts replaced so it can be run at a desk in minutes.
+#
+# Usage: python scripts/local/launch_simp.py -c predicatorv3/stage6_rehearsal_domino_real.yaml
+#
+# Stage 6 proper is exp_domino_real.yaml as shipped: live cameras, a live arm,
+# a human reset per episode, and a planning explorer that costs minutes an
+# episode. That is the right thing to run once the loop is trusted, and the
+# wrong thing to debug the loop with. This config keeps the loop -- explore,
+# learn, test, reset -- and makes one pass survivable:
+#
+#   * the explorer replays a FIXED plan instead of planning, so an episode
+#     costs nothing and does the same thing every time. A run that goes wrong
+#     is then the loop's fault rather than the planner's.
+#   * nothing moves (real_robot_dry) and no camera opens (scene_file), so it
+#     runs with the robot powered down and the ZEDs unplugged.
+#   * every look is dumped to JSON, so the rehearsal leaves the same evidence
+#     a real session would.
+#
+# What it does NOT replace is the learner: agent_po_predicate_invention_al
+# still invents predicates, which is separately expensive. Swap the approach
+# too if a cycle is still too slow to iterate on.
+#
+# POINT fixed_plan_explorer_path AT A REAL PLAN before running: a plan dumped
+# by probe_real_scene --dump-plan and already checked through replay_plan.
+---
+includes:
+  - common.yaml
+  - envs/all.yaml
+  - approaches/all.yaml
+FLAGS:
+  # One cycle is enough to exercise explore -> learn -> test. Two only doubles
+  # the wait for the same evidence.
+  num_online_learning_cycles: 1
+NUM_SEEDS: 1
+ENVS:
+  domino_real:
+    SKIP: False
+    FLAGS:
+      excluded_predicates: "InitialBlock,MovableBlock,Tilting,Upright,InFront"
+      # -- the loop, without the hardware -----------------------------------
+      real_robot_execute: True
+      real_robot_dry: True           # no arm is built; nothing can move
+      real_robot_perception: "scene_file"  # replays the capture; no cameras
+      real_robot_observe_at_option_boundary: True
+      real_robot_human_reset: False  # a rehearsal should not need a person
+      # Measured on the real scene in Stage 2, not a guess.
+      real_robot_divergence_atol: 0.02
+      # Leave the same evidence a real session would.
+      real_robot_observation_dump_dir: "logs/stage6_rehearsal_looks"
+APPROACHES:
+  agent_po_predicate_invention_al:
+    SKIP: False
+    FLAGS:
+      # The whole point: no planning explorer, no LLM in the exploration half.
+      explorer: "fixed_plan"
+      fixed_plan_explorer_path: "plans/pickplace.txt"
+      # The pre-loop test is a second full episode before anything is learned;
+      # the rehearsal is about the cycle, so skip it.
+      skip_initial_test: True