domino_real: bring-up Stages 2 and 3 -- a perception check and a launcher

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 observe_scene(robot: "RealRobot", settle_s: float = 0.0) -> Any:
+    """Look at the scene and return what the cameras saw. Moves nothing.
+
+    The bare look, without ``reset_env``'s arm homing and human prompt
+    or ``execute_chunks``' motion: dwelling ``settle_s`` before
+    capturing, so dominoes disturbed by whatever happened last come to
+    rest first. Used to measure the twin against reality with no episode
+    running.
+    """
+    # pylint: disable=import-outside-toplevel,import-error
+    from babyrobot.realrobot.messages import ObserveRequest
+    return robot.observe(ObserveRequest(settle_s=float(settle_s)))
+
+
 def reset_env(robot: "RealRobot",
               joints: Optional[Sequence[float]] = None) -> Any:
     """Home the arm, wait for a human to rearrange the scene, then look.

scripts/configs/predicatorv3/stage3_domino_real.yaml

@@ -0,0 +1,71 @@
+# Stage 3 of docs/real_robot_bringup.md: dry arm, live cameras, human resets.
+#
+# Usage: python scripts/local/launch_simp.py -c predicatorv3/stage3_domino_real.yaml
+#
+# Stage 2 plus one flag (real_robot_human_reset). NOTHING MOVES -- real_robot_dry
+# builds the RealRobot with no arm -- so this adds exactly one new source of
+# failure over Stage 2: the reset cadence and the task rebuild.
+#
+# What to check:
+#   * Exactly ONE prompt for the episode, and the arm homes BEFORE it appears,
+#     i.e. before a human reaches into the workspace. In dry mode nothing
+#     actually moves, so confirm the ordering in the log -- you are checking it
+#     now so it is already trusted at Stage 6, when the arm is live.
+#   * executor.resets_done == 1, and the log line "scene reset #1; rebuilding
+#     the test task from what the cameras see".
+#   * The episode's initial state reflects how you JUST arranged the dominoes,
+#     not the capture in domino_real_scene. Rearrange them noticeably before
+#     confirming the prompt, or you cannot tell the two apart.
+#
+# ONE EPISODE PER RUN, deliberately. The doc says "run two episodes", but oracle
+# is not learning-based, so main.py takes the non-learning branch (main.py:233):
+# a single _run_testing over the env's test tasks, and this env's
+# _generate_test_tasks returns exactly one task regardless of num_test_tasks.
+# Setting num_test_tasks here would silently do nothing. Multi-episode cadence
+# comes from num_online_learning_cycles, which needs a learning approach and the
+# LLM -- that is Stage 6, not this. The across-episode bookkeeping it would
+# exercise (_reset_pending re-arming, so episode 2 owes a fresh prompt) is
+# already covered in simulation by
+# tests/envs/test_domino_real_online.py::test_each_episode_prompts_exactly_once.
+# So: run this twice if you want to see the cadence twice. What hardware adds
+# over that test is the physical loop, not the counter.
+#
+# Why oracle rather than the agent: this stage tests reset plumbing, and running
+# predicate invention alongside it would make a failure ambiguous between the
+# learner and the resets -- which is the attribution the whole ladder exists to
+# preserve. oracle needs no LLM.
+---
+includes:
+  - common.yaml
+  - envs/all.yaml
+  - approaches/all.yaml
+ENVS:
+  domino_real:
+    SKIP: False
+    FLAGS:
+      # Must be set HERE, not in settings.py: envs/all.yaml already sets it and
+      # a config value beats the settings.py default.
+      domino_real_scene: "/home/amberli/babyrobot/BabyRobotPredicator/scenes/domino_straight.json"
+      # How oracle runs the other domino envs: the push target is inferred from
+      # state rather than named by the caller. This makes Push ground as
+      # [robot] instead of [robot, domino], so a plan dumped under an agent
+      # config will NOT ground here.
+      domino_restricted_push: True
+      # -- Stage 3 ---------------------------------------------------------
+      real_robot_execute: True
+      real_robot_dry: True     # no arm is built; every call runs, nothing moves
+      real_robot_perception: "zed"   # live cameras, as in Stage 2
+      real_robot_human_reset: True   # the one new thing this stage adds
+      # Carried from Stage 2: the twin is still corrected between options, so a
+      # divergence line per boundary confirms perception stayed healthy across
+      # the reset. Harmless while dry, since nothing moves to diverge.
+      real_robot_observe_at_option_boundary: True
+      real_robot_divergence_atol: 0.02
+      # Dwell before each capture so the dominoes come to rest after a human
+      # has been rearranging them by hand.
+      real_robot_settle_s: 0.5
+APPROACHES:
+  oracle:
+    SKIP: False
+# One seed: this is a plumbing check on one physical scene, not a comparison.
+NUM_SEEDS: 1

scripts/domino_debug/check_perception.py

@@ -0,0 +1,187 @@
+"""Measure the live ZED scene against the simulated twin. NOTHING MOVES.
+
+Stage 2 of the real-robot bring-up ladder: a dry arm (``RealRobot`` is built
+with no arm at all, so this runs with the robot powered down and needs no
+polymetis controller) and live cameras. It answers the one question worth
+answering before any motion -- does what the cameras see land in the right
+place in the twin?
+
+It goes through ``make_real_robot`` and the env's own
+``state_from_observation``, i.e. the exact conversion ``RealRobotExecutor``
+uses at an option boundary, so a disagreement here is a real disagreement and
+not an artifact of a parallel code path.
+
+The three checks (run this once per check; the table is the point):
+
+  1. Static.  Untouched scene. Divergence sits at the perception noise floor.
+     Use ``--repeat`` and write the number down: it is what
+     ``real_robot_divergence_atol`` should be set from. The shipped 0.02
+     started as a guess and a Stage 2 run has since confirmed it.
+  2. Moved.   Physically slide one domino ~5cm and re-run. Divergence should
+     report ~0.05 -- and THAT domino should be the one that moved. A different
+     one moving means the capture-id -> slot mapping is wrong, which is the
+     failure this stage exists to catch.
+  3. Toppled. Lay one domino on its face. ``Toppled`` should read True for it.
+
+Watch the z column separately. A *constant* z offset across every domino is a
+table-height disagreement (``domino_real_table_z``, -0.041), not a perception
+error -- fix the number, not the geometry.
+
+Usage (from the predicators repo root, robot-ml; PYTHONHASHSEED=0):
+    PYTHONPATH=.:/path/to/BabyRobotPredicator \
+        python scripts/domino_debug/check_perception.py --repeat 5
+"""
+import argparse
+import logging
+from typing import Any, Dict, List, Optional, Tuple
+
+import numpy as np
+
+from predicators import utils
+from predicators.envs import get_or_create_env
+from predicators.envs.pybullet_domino_real import PyBulletDominoRealEnv
+from predicators.pybullet_helpers.real_robot_bridge import make_real_robot, \
+    observe_scene
+from predicators.settings import CFG
+from predicators.structs import Object, State
+from scripts.cluster_utils import SingleSeedRunConfig, generate_run_configs
+
+# A debug harness that reads env internals to report on them.
+# pylint: disable=protected-access
+
+
+def _load_config(config: str, scene: Optional[str], settle_s: float) -> None:
+    """reset_config from the stock launcher, then force the Stage 2 flags.
+
+    The launcher is the single source of truth for the env, geometry and
+    scene, exactly as probe_real_scene and replay_plan use it. Only the
+    real-robot flags are overridden here, so this tool cannot be pointed
+    at hardware by editing a config.
+    """
+    rc = list(generate_run_configs(config))[0]
+    assert isinstance(rc, SingleSeedRunConfig)
+    flags = dict(rc.flags)
+    flags.update({"env": rc.env, "approach": rc.approach, "seed": rc.seed})
+    flags.pop("log", None)
+    if scene is not None:
+        flags["domino_real_scene"] = scene
+    # Stage 2 in four flags: a robot with cameras and no arm.
+    flags["real_robot_execute"] = True
+    flags["real_robot_dry"] = True  # no arm is built; nothing can move
+    flags["real_robot_perception"] = "zed"  # live cameras
+    flags["real_robot_human_reset"] = False
+    flags["real_robot_settle_s"] = settle_s
+    utils.reset_config(flags)
+
+
+def _xyz(vec: Any) -> str:
+    """Fixed-width xyz, so the table columns line up."""
+    return "[" + " ".join(f"{float(v):6.3f}" for v in vec) + "]"
+
+
+def _domino_rows(env: PyBulletDominoRealEnv, predicted: State,
+                 perceived: State) -> List[Tuple[Object, int, Any, Any]]:
+    """(object, capture_id, predicted xyz, perceived xyz) in scene order.
+
+    Scene order is slot order is ``env._scene_ids`` order, which is the
+    mapping under test -- so report it rather than sorting by name.
+    """
+    comp = env._domino_component
+    assert comp is not None, "env has no domino component"
+    rows = []
+    for slot, capture_id in enumerate(env._scene_ids):
+        dom = comp.dominos[slot]
+        rows.append(
+            (dom, capture_id, np.array([predicted.get(dom, f) for f in "xyz"]),
+             np.array([perceived.get(dom, f) for f in "xyz"])))
+    return rows
+
+
+def _report(env: PyBulletDominoRealEnv, predicted: State, perceived: State,
+            toppled: Dict[str, bool]) -> Dict[str, float]:
+    """Print the per-domino table and return the summary numbers."""
+    rows = _domino_rows(env, predicted, perceived)
+    print(f"{'object':>10} {'id':>4} {'twin xyz':>24} {'seen xyz':>24} "
+          f"{'delta':>8} {'dz':>8}  toppled")
+    deltas, dzs = [], []
+    for dom, capture_id, twin, seen in rows:
+        delta = float(np.linalg.norm(twin - seen))
+        dz = float(seen[2] - twin[2])
+        deltas.append(delta)
+        dzs.append(dz)
+        print(f"{dom.name:>10} {capture_id:>4} {_xyz(twin):>24} "
+              f"{_xyz(seen):>24} {delta:8.4f} {dz:8.4f}  "
+              f"{toppled.get(dom.name, False)}")
+    worst = max(deltas) if deltas else float("nan")
+    # A constant dz is the table, not the perception: report the spread
+    # separately from the offset so the two cannot be confused.
+    print(f"\n  max divergence : {worst:.4f} m")
+    print(f"  mean dz        : {float(np.mean(dzs)):+.4f} m  "
+          f"(spread {float(np.max(dzs) - np.min(dzs)):.4f} m)")
+    print("  a large mean dz with a small spread is table height "
+          f"(domino_real_table_z={CFG.domino_real_table_z}), not perception")
+    return {"worst": worst, "mean_dz": float(np.mean(dzs))}
+
+
+def main() -> None:
+    """Look at the real scene N times and report it against the twin."""
+    ap = argparse.ArgumentParser(
+        description=__doc__,
+        formatter_class=argparse.RawDescriptionHelpFormatter)
+    ap.add_argument("--config", default="predicatorv3/exp_domino_real.yaml")
+    ap.add_argument("--scene",
+                    default=None,
+                    help="override CFG.domino_real_scene (the capture the "
+                    "twin is built from)")
+    ap.add_argument("--repeat",
+                    type=int,
+                    default=1,
+                    help="captures to take; >1 gives the noise floor")
+    ap.add_argument("--settle",
+                    type=float,
+                    default=0.5,
+                    help="dwell before each capture, seconds")
+    args = ap.parse_args()
+    logging.basicConfig(level=logging.INFO, format="%(message)s")
+
+    _load_config(args.config, args.scene, args.settle)
+    env = get_or_create_env(CFG.env)
+    assert isinstance(env, PyBulletDominoRealEnv), \
+        f"check_perception drives the real-scene env; got {CFG.env}"
+    # Build the task so the twin holds the captured scene, which is what the
+    # perceived scene is being measured against.
+    env.get_test_tasks()
+    toppled_pred = next(p for p in env.predicates if p.name == "Toppled")
+
+    print(f"# scene   : {CFG.domino_real_scene}")
+    print(f"# slots   : {list(enumerate(env._scene_ids))}"
+          "  (slot -> capture id)")
+    print("# NOTHING MOVES: the arm is not built, only the cameras open\n")
+
+    robot = make_real_robot()  # dry=True + zed, from the flags above
+    try:
+        worsts = []
+        for i in range(args.repeat):
+            obs = observe_scene(robot, settle_s=args.settle)
+            predicted = env.get_observation()
+            assert isinstance(predicted, State)
+            perceived = env.state_from_observation(obs, predicted)
+            toppled = {
+                d.name: bool(toppled_pred.holds(perceived, [d]))
+                for d in perceived if d.type.name == "domino"
+            }
+            print(f"--- capture {i + 1}/{args.repeat} "
+                  f"({len(obs.dominoes)} dominoes seen) ---")
+            worsts.append(_report(env, predicted, perceived, toppled)["worst"])
+            print()
+        if args.repeat > 1:
+            print(f"# noise floor over {args.repeat} captures: "
+                  f"max {max(worsts):.4f} m, "
+                  f"mean {float(np.mean(worsts)):.4f} m")
+            print("# set real_robot_divergence_atol from this, with headroom")
+    finally:
+        robot.close()  # release the cameras even if a capture threw
+
+
+if __name__ == "__main__":
+    main()