roboapi 0.4.2__py3-none-any.whl

roboapi/runtime.py

@@ -0,0 +1,911 @@
+"""Policy loop, manual motion and the shared operation gate."""
+
+from __future__ import annotations
+
+from dataclasses import asdict, replace
+import logging
+import threading
+import time
+from pathlib import Path
+
+import numpy as np
+
+from . import geometry as geo
+from . import config
+from .config import Config
+from .calibration import Calibration
+from .semantics import ACTION_DT, HOME_POSE7, TRAINING_FRAME
+from .motion import dispatch, wait_for_pose, wait_until, workspace_targets
+from .operations import OperationGate, exclusive
+from .records import RunRecords
+from .tasks import TaskArchive
+from .embodiment import Embodiment, build_embodiment
+from .errors import CalibrationRequired, ControlNotReady, LoopRunning
+from .policy import ActionChunk, OpenAIChatPolicyClient, PolicyUnavailable
+from .policy.settings import (
+    policy_token_configured, validate_policy_endpoint, write_policy_token,
+)
+
+logger = logging.getLogger(__name__)
+
+IDLE, RUNNING, PAUSED = "idle", "running", "paused"
+
+
+class Runtime:
+    """The one background loop, plus the guard that keeps manual motion off the arm
+    while it runs. Everything the MCP server and the CLI call goes through here."""
+
+    def __init__(self, emb: Embodiment, policy, *, config_path: str | Path | None = None) -> None:
+        self.emb = emb
+        self.cfg = emb.cfg
+        self.config_path = Path(config_path).expanduser().resolve() if config_path is not None else None
+        self.policy = policy
+        self.records = RunRecords(self.cfg.runtime)
+        self.tasks = TaskArchive(Path(self.cfg.runtime.log_dir).expanduser() / "tasks")
+        self.emb.camera.on_event = self.records.event
+        self._observation_after = None
+        self._control_trace = []
+        self._lock = threading.RLock()
+        self.operations = OperationGate()
+        self._mode = IDLE
+        previous_task = self.tasks.current()
+        self._instruction: str | None = previous_task["instruction"] if previous_task else None
+        self._applied_instruction: str | None = None
+        self._camera_failures = 0
+        self._run_source: str | None = None
+        self._abort = threading.Event()
+        # Aborts a BLOCKING tool (move_waypoints) rather than the loop. Separate from _abort
+        # because those run with mode == IDLE, where the loop and its abort do not exist.
+        self._thread: threading.Thread | None = None
+        self._started_at = 0.0
+        self._chunk_started_at = 0.0
+        self._chunks = 0
+        self._late = 0
+        self._stop_reason: str | None = None
+        self._stop_requested: str | None = None
+        self._arm_fault_exit = False
+        self._direction_check: str | None = None
+        self.calibration = Calibration(self)
+
+    # -- introspection -----------------------------------------------------
+    @property
+    def mode(self) -> str:
+        return self._mode
+
+    def status(self) -> dict:
+        """One call must be worth polling for: mode, instruction, progress, health, why it stopped."""
+        with self._lock:
+            mode, instruction, run_source = self._mode, self._instruction, self._run_source
+            applied_instruction = self._applied_instruction
+        running = mode in (RUNNING, PAUSED)
+        task = self.tasks.current()
+        return {
+            "timestamp": time.time(),
+            "mode": mode,
+            "instruction": instruction,
+            "instruction_pending": running and instruction != applied_instruction,
+            "run_source": run_source,
+            "task_id": task["id"] if task else None,
+            "task_elapsed_s": task["elapsed_s"] if task else 0.0,
+            "chunk_elapsed_time": round(time.monotonic() - self._chunk_started_at, 3) if running else 0.0,
+            "elapsed_s": round(time.monotonic() - self._started_at, 2) if running else 0.0,
+            "chunks_done": self._chunks,
+            "late_frames": self._late,
+            "stop_reason": self._stop_reason,
+            "policy_ready": self.policy.ready,
+            "control_ready": self.emb.control_ready,
+            "inter_chunk_pause_s": self.cfg.runtime.inter_chunk_pause_s,
+            "workspace": self.workspace_config(),
+            **self.emb.status(),
+            "calibration": self.calibration.status(),
+            **self.records.snapshot(),
+            "session": self.policy.session_status,
+        }
+
+    @exclusive
+    def clear_monitor_errors(self) -> dict:
+        """Dismiss previous run errors without changing a live hardware fault or moving the arm."""
+        with self._lock:
+            if self._mode != IDLE:
+                raise LoopRunning("stop the policy before clearing previous errors")
+            self._stop_reason = None
+            self.records.clear_errors()
+        return self.status()
+
+
+    def workspace_config(self) -> dict:
+        return {**asdict(self.cfg.workspace), "home_xyz_m": list(HOME_POSE7[:3])}
+
+    @exclusive
+    def configure_workspace(self, *, enabled, min_xyz_m, max_xyz_m) -> dict:
+        """Save idle installation limits without connecting or moving any device."""
+        if self.mode != IDLE or self.calibration.phase not in {"acquire", "complete"}:
+            raise LoopRunning("Stop and finish calibration before changing the workspace")
+        if self.config_path is None:
+            raise ValueError("Workspace settings require a local installation configuration path")
+        workspace = config.WorkspaceConfig(enabled, min_xyz_m, max_xyz_m)
+        workspace.validate()
+        config.save(replace(self.cfg, workspace=workspace), self.config_path)
+        self.cfg.workspace = workspace
+        self.records.event("Workspace settings saved")
+        return self.workspace_config()
+
+    def _require_workspace_start(self) -> None:
+        workspace = self.cfg.workspace
+        # A frame round-trip must not turn a point on the face into an outside start.
+        if workspace.enabled and not workspace.contains(self.emb.arm.get_eef_pose()[:3], atol=1e-9):
+            raise ControlNotReady("Robot is outside the workspace. Return Home before starting motion.")
+
+    def policy_config(self) -> dict:
+        """Safe policy metadata; never returns the credential or a prefix of it."""
+        cfg = self.cfg.policy
+        token_file = str(Path(cfg.token_file).expanduser()) if cfg.token_file else None
+        return {
+            "type": cfg.type,
+            "url": cfg.url or None,
+            "model": cfg.model or None,
+            "token_file": token_file,
+            "api_key_configured": bool(cfg.token_file and policy_token_configured(cfg.token_file)),
+            "policy_ready": self.policy.ready,
+            "inter_chunk_pause_s": self.cfg.runtime.inter_chunk_pause_s,
+        }
+
+    @exclusive
+    def configure_policy(self, *, url: str, model: str, inter_chunk_pause_s: float,
+                         api_key: str | None = None) -> dict:
+        """Verify and save the idle model connection and run settings together."""
+        cfg = self.cfg.policy
+        if cfg.type != "openai":
+            raise ValueError("policy connection setup requires policy.type='openai'")
+        if not cfg.token_file:
+            raise ValueError("policy.token_file is not configured")
+        if self.config_path is None:
+            raise ValueError("policy settings require a local installation configuration path")
+        url, model = validate_policy_endpoint(url, model)
+        updated = replace(self.cfg, policy=replace(cfg, url=url, model=model),
+                          runtime=replace(self.cfg.runtime, inter_chunk_pause_s=inter_chunk_pause_s))
+        updated.validate()
+        with self._lock:
+            if self._mode != IDLE:
+                raise LoopRunning("stop the policy before changing its settings")
+            path = Path(cfg.token_file).expanduser()
+            previous = path.read_bytes() if api_key is not None and path.exists() else None
+            previous_mode = path.stat().st_mode & 0o777 if previous is not None else None
+            if api_key is not None:
+                write_policy_token(cfg.token_file, api_key)
+            candidate = None
+            try:
+                candidate = OpenAIChatPolicyClient(
+                    url, cfg.token_file, model,
+                    image_height=cfg.image_height,
+                    timeout_s=cfg.timeout_s,
+                    use_env_proxy=cfg.use_env_proxy,
+                )
+                candidate.connect()
+                config.save(updated, self.config_path)
+            except Exception:
+                if candidate is not None:
+                    try:
+                        candidate.close()
+                    except Exception:
+                        logger.warning("rejected policy connection did not close cleanly")
+                if api_key is not None:
+                    if previous is None:
+                        path.unlink(missing_ok=True)
+                    else:
+                        path.write_bytes(previous)
+                        path.chmod(previous_mode or 0o600)
+                raise
+            previous_client = self.policy
+            cfg.url, cfg.model = url, model
+            self.cfg.runtime.inter_chunk_pause_s = inter_chunk_pause_s
+            self.policy = candidate
+            try:
+                previous_client.close()
+            except Exception:
+                logger.warning("previous policy connection did not close cleanly")
+        self.records.event("Model settings saved")
+        return {"ok": True, **self.policy_config()}
+
+    def observation(self, with_image: bool = True) -> tuple[dict, np.ndarray | None]:
+        """Status + the arm/gripper reading + optionally the frame, sampled together.
+
+        Two separate calls would hand the agent a picture from one instant and a pose from
+        another, with no way to see the skew. `image_age_ms` reports the residual.
+        """
+        obs = self.status()
+        if not self.emb.connected:
+            if self.emb.feedback is not None and self.emb.feedback.live and self.emb.calibrated:
+                pose = self.emb.arm.adapter.to_task(self.emb.feedback.snapshot()["pose"])
+                values = geo.pose7_to_xyzrpy(pose)
+                obs.update(eef_xyz_m=values[:3].round(4).tolist(),
+                           eef_rpy_deg=values[3:].round(2).tolist(), eef_frame=TRAINING_FRAME)
+            return obs, self.emb.get_camera()[0] if with_image and obs["camera_connected"] else None
+        # Split and unit-named on purpose. A bare 7-vector invites an LLM to send radians,
+        # or a quaternion it cannot author; xyz-in-metres and rpy-in-degrees it can both read
+        # back and write. pose7d stays the internal representation everywhere else.
+        pose = geo.pose7_to_xyzrpy(self.emb.arm.get_eef_pose())
+        obs["eef_xyz_m"] = pose[0:3].round(4).tolist()
+        obs["eef_rpy_deg"] = pose[3:6].round(2).tolist()
+        obs["eef_frame"] = TRAINING_FRAME
+        obs["gripper_state"] = round(float(self.emb.gripper.get_ratio()), 4)
+        image = None
+        if with_image:
+            image, age = self.emb.camera.read_after(
+                self._observation_after, self.cfg.camera.stale_after_s,
+            )
+            obs["image_age_ms"] = round(age * 1e3, 1)
+        return obs, image
+
+    # -- loop control ------------------------------------------------------
+    @exclusive
+    def start(self, instruction: str, *, source: str = "mcp", new_task: bool = False) -> dict:
+        if source not in {"mcp", "web"}:
+            raise ValueError("run source must be mcp or web")
+        self._validate_instruction(instruction)
+        with self._lock:
+            if self._mode != IDLE:
+                raise LoopRunning(f"loop_running: already {self._mode}. Call stop() first.")
+            self._stop_requested = None
+            self._abort.clear()
+            self.emb.require_alive()
+            self._require_workspace_start()
+            self.policy.prepare_session()
+            self._instruction = instruction
+            self._applied_instruction = None
+            self._camera_failures = getattr(self.emb.camera, "failure_count", 0)
+            self._run_source = source
+            self._stop_reason = None
+            self._arm_fault_exit = False
+            self._chunks = 0
+            self._late = 0
+            self._started_at = time.monotonic()
+            self._chunk_started_at = self._started_at
+            self._mode = RUNNING
+            self._begin_task(instruction, new_task=new_task)
+            self.records.event("Run started")
+            self._thread = threading.Thread(target=self._run, name="policy-loop", daemon=True)
+            self._thread.start()
+        return self.status()
+
+    @staticmethod
+    def _validate_instruction(instruction: str) -> None:
+        if not isinstance(instruction, str) or not instruction.strip():
+            raise ValueError("instruction must not be empty")
+
+    def set_instruction(self, instruction: str, *, source: str = "mcp") -> dict:
+        """Takes effect at the NEXT inference: the loop reads it once per iteration.
+
+        Latency includes the rest of the complete chunk, frame waits, and one
+        inference. The instruction does not truncate a chunk. Use stop() to interrupt execution.
+
+        Meant for a running loop: start() takes the instruction itself, and overwrites
+        anything set beforehand.
+        """
+        self._validate_instruction(instruction)
+        with self._lock:
+            if self._mode not in (RUNNING, PAUSED):
+                raise LoopRunning("loop_not_running: start the policy before changing its instruction")
+            self._require_source(source)
+            self._instruction = instruction
+            self.records.event("Instruction submitted")
+        return self.status()
+
+    def _require_source(self, source: str) -> None:
+        if source not in {"mcp", "web"}:
+            raise ValueError("run source must be mcp or web")
+        if self._mode != IDLE and source != self._run_source:
+            raise LoopRunning(f"policy started through {self._run_source}; stop it before using {source}")
+
+    def _begin_task(self, instruction: str, *, new_task: bool = False) -> None:
+        try:
+            self.tasks.begin(instruction, self.cfg.policy.model, self._run_source,
+                             self.emb.camera, new_task=new_task)
+        except Exception:
+            logger.exception("task recording could not start")
+            self.records.event("Task recording unavailable; robot control remains active", "warning")
+
+    def _finish_task(self, reason: str) -> None:
+        try:
+            events = self.records.snapshot()["inference_events"]
+            infer_ms = events[-1]["infer_ms"] if events else None
+            self.tasks.finish(reason, infer_ms=infer_ms)
+        except Exception:
+            logger.exception("task recording could not finish")
+            self.records.event("Could not save task recording", "warning")
+
+    def pause(self, *, source: str = "mcp") -> dict:
+        with self._lock:
+            self._require_source(source)
+            if self._mode != RUNNING:
+                return self.status()
+            self._mode = PAUSED
+            self.policy.record_lifecycle("pause")
+            self.records.event("Run paused")
+        self._hold_measured()
+        with self._lock:
+            if self._mode == PAUSED:
+                self._finish_task("paused")
+        return self.status()
+
+    @exclusive
+    def resume(self, *, source: str = "mcp") -> dict:
+        self.emb.require_control()
+        with self._lock:
+            self._require_source(source)
+            if self._mode != PAUSED:
+                return self.status()
+            self._require_workspace_start()
+            self._mode = RUNNING
+            self._begin_task(self._instruction)
+            self.policy.record_lifecycle("resume")
+            self.records.event("Run resumed")
+        return self.status()
+
+    def stop(self, reason: str = "stopped") -> dict:
+        """Stop whatever is driving the arm: the policy loop, or a blocking move.
+
+        The blocking-move abort signal is set BEFORE the idle early-return, because a
+        calibration Home runs with mode == IDLE. Drivers that support vendor cancellation
+        poll this signal and return to measured-state hold before the operation unwinds.
+        """
+        self.operations.interrupt()
+        # Hold and signal before waiting on metadata or model setup under _lock.
+        self._stop_requested = reason
+        self._abort.set()
+        self._hold_measured()
+        thread = self._thread
+        with self._lock:
+            if self._mode == IDLE:
+                return self.status()
+            self._stop_reason = reason
+            self._arm_fault_exit = False
+            self._abort.set()
+            self._finish_task(reason)
+        if thread is not None and thread is not threading.current_thread():
+            thread.join(timeout=5.0)
+        self._hold_measured()
+        return self.status()
+
+    def _hold_measured(self) -> None:
+        self.emb.hold()
+
+    def _run(self) -> None:
+        try:
+            if not self._abort.is_set():
+                self.policy.begin_session()
+            while not self._abort.is_set():
+                if self._interrupted() and self._abort.is_set():
+                    break
+                if self._mode == PAUSED:
+                    # Hold: stop feeding new targets. The arm stays on the last one -- on
+                    # Franka this must NOT be done by ending the joint session, which
+                    # ramps to damping-only and lets the arm sag.
+                    time.sleep(0.02)
+                    continue
+                self._chunk_started_at = time.monotonic()
+                step_started = time.perf_counter()
+                step_at = time.time()
+                self.records.begin_inference(step_at, step_started)
+                try:
+                    result = self.step(None, should_stop=self._interrupted,
+                                           on_phase=self.records.set_phase)
+                except PolicyUnavailable as exc:
+                    self.records.record_inference(step_at, step_started, exc=exc)
+                    with self._lock:
+                        if not self._abort.is_set():
+                            self._stop_reason = f"policy_unavailable: {exc}"
+                            self._arm_fault_exit = False
+                    break
+                except Exception as exc:
+                    self.records.record_inference(step_at, step_started, exc=exc)
+                    # A driver can fail mid-chunk. If the rig is not alive that is a hardware
+                    # fault and takes the same path as one seen at the top of the loop;
+                    # anything else is a real bug and must not be swallowed as a fault.
+                    with self._lock:
+                        # A step can fail while an earlier stop is unwinding it.
+                        # Preserve that cause and collect native errors only for arm exits.
+                        if self._abort.is_set():
+                            break
+                        if self.emb.alive:
+                            raise
+                        camera_failed = getattr(self.emb.camera, "failure_count", 0) != self._camera_failures
+                        self._arm_fault_exit = (
+                            not camera_failed and not self.emb.arm.live and bool(self.emb.arm.last_error)
+                        )
+                        detail = (getattr(self.emb.camera, "last_error", None) or "camera disconnected"
+                                  if camera_failed else self.emb.last_error or str(exc))
+                        self._stop_reason = f"not_alive: {detail}"
+                    break
+                self.records.record_inference(step_at, step_started, result=result)
+                if result["chunk_len"]:
+                    self._chunks += 1
+                self._late += result["late_frames"]
+        except PolicyUnavailable as exc:
+            with self._lock:
+                if not self._abort.is_set():
+                    self._stop_reason = f"policy_unavailable: {exc}"
+                    self._arm_fault_exit = False
+        except Exception as exc:  # never let the loop thread die silently
+            logger.exception("policy loop failed")
+            with self._lock:
+                if not self._abort.is_set():
+                    self._stop_reason = f"error: {exc}"
+                    self._arm_fault_exit = False
+        finally:
+            # Local hold always precedes server cleanup, including a transport
+            # failure or stop that interrupted an action partway through.
+            try:
+                self._hold_measured()
+            except Exception:
+                logger.warning("local hold could not be confirmed at policy exit")
+            if self._arm_fault_exit:
+                wait_for_fault = getattr(self.emb.arm, "wait_for_fault", None)
+                if wait_for_fault is not None:
+                    try:
+                        # The native exception can arrive while the driver's worker
+                        # unwinds. Finish that collection before recording this run.
+                        error = wait_for_fault()
+                        with self._lock:
+                            if self._arm_fault_exit and error:
+                                self._stop_reason = f"not_alive: {error}"
+                    except Exception:
+                        logger.warning("Could not collect the final arm fault", exc_info=True)
+            try:
+                if self._stop_requested is not None:
+                    self._stop_reason = self._stop_requested
+                self._finish_task(self._stop_reason or "stopped")
+                self.policy.end_session(self._stop_reason or "stopped")
+            finally:
+                with self._lock:
+                    self._mode = IDLE
+                    self._run_source = None
+                    if self._stop_reason is None:
+                        self._stop_reason = "stopped"
+                    self.records.event(f"Run stopped: {self._stop_reason}",
+                                       "error" if self._stop_reason.startswith(("error:", "not_alive:", "policy_unavailable:")) else "info")
+
+    def _interrupted(self) -> bool:
+        """Interrupt frames and waits on STOP or pause.
+
+        After a pause-abort the loop parks at the top and, on resume, re-infers from a fresh
+        observation rather than replaying a plan made before the world was held still.
+        """
+        with self._lock:
+            if self._abort.is_set():
+                return True
+            camera_failed = getattr(self.emb.camera, "failure_count", 0) != self._camera_failures
+            if camera_failed or not self.emb.alive:
+                detail = (getattr(self.emb.camera, "last_error", None) or "camera disconnected"
+                          if camera_failed else self.emb.last_error or "device unavailable")
+                self._arm_fault_exit = (
+                    not camera_failed and not self.emb.arm.live and bool(self.emb.arm.last_error)
+                )
+                self._stop_reason = f"not_alive: {detail}"
+                self._abort.set()
+            return self._abort.is_set() or self._mode == PAUSED
+
+    # -- guarded manual-motion surface -------------------------------------
+    def _require_idle(self) -> None:
+        self.emb.require_control()
+        if self._mode != IDLE:
+            raise LoopRunning(f"loop_running: the policy loop is {self._mode}. Call stop() first.")
+
+    _WAYPOINT_KEYS = frozenset({"x", "y", "z", "roll", "pitch", "yaw", "gripper", "relative", "blend"})
+
+    @exclusive
+    def move_waypoints(self, waypoints: list[dict]) -> dict:
+        """Agent waypoints -> absolute targets, chained: each is completed from the one before
+        it (the first from the arm), so omitted axes hold and `relative` offsets accumulate.
+        Validation is here, at the trust boundary, before anything moves."""
+        self._require_idle()
+        if not waypoints:
+            raise ValueError("waypoints must not be empty")
+        pose = self.emb.arm.get_eef_pose()
+        grip = float(self.emb.gripper.get_ratio())
+        targets, grips, blends = [], [], []
+        for i, wp in enumerate(waypoints):
+            unknown = set(wp) - self._WAYPOINT_KEYS
+            if unknown:
+                raise ValueError(f"waypoint {i}: unknown fields {sorted(unknown)}; "
+                                 f"allowed: {sorted(self._WAYPOINT_KEYS)}")
+            axes = {k: wp.get(k) for k in ("x", "y", "z", "roll", "pitch", "yaw")}
+            pose = geo.target_pose(pose, **axes, relative=bool(wp.get("relative", False)))
+            # Relative waypoints start from the previous effective target, including at a wall.
+            pose = workspace_targets([pose], self.cfg.workspace)[0]
+            if wp.get("gripper") is not None:
+                grip = float(wp["gripper"])
+                if not 0.0 <= grip <= 1.0:
+                    raise ValueError(f"waypoint {i}: gripper must be an opening ratio in [0, 1], got {grip}")
+            blend = self.cfg.arm.blend_radius if wp.get("blend") is None else float(wp["blend"])
+            if blend < 0.0:
+                raise ValueError(f"waypoint {i}: blend must be >= 0 m, got {blend}")
+            targets.append(pose)
+            grips.append(grip)
+            blends.append(blend)
+        targets = np.array(targets)
+        # Cleared on entry, so a stop() from before this call does not kill it; a stop() from
+        # here on is caught by the per-frame check inside the dispatch.
+        self.operations.reset_abort()
+        return self._move_path(targets, np.array(grips), np.array(blends),
+                                       should_stop=self.operations.abort.is_set)
+
+    def move_to_pose(self, gripper=None, **kwargs) -> dict:
+        """One waypoint: the simple form of move_waypoints, on the same path."""
+        out = self.move_waypoints([{**kwargs, "gripper": gripper}])
+        wp = out["waypoints"][0]
+        return {**out, "target": wp["target"], "distance_m": out["path_m"],
+                "gripper": None if gripper is None else wp["gripper"]}
+
+    def _at_direction_home(self) -> bool:
+        position, angle = geo.pose_delta(HOME_POSE7, self.emb.arm.get_eef_pose())
+        # Keep each short gesture close to its recorded starting pose.
+        return bool(position <= 0.005 and angle <= np.deg2rad(2))
+
+    def direction_check_status(self) -> dict:
+        """Read current Home readiness without connecting or moving hardware."""
+        at_home = False
+        try:
+            if (self.mode == IDLE and self.calibration.phase == "complete"
+                    and self.emb.calibrated and self.emb.control_ready and self.emb.alive):
+                at_home = self._at_direction_home()
+        except Exception:
+            # Disconnected/stale feedback is normal while the device page polls.
+            pass
+        return {"at_home": at_home, "active": self._direction_check}
+
+    @exclusive
+    def check_direction(self, direction: str) -> dict:
+        """Check task translation and rotation using the policy's full-pose executor.
+
+        Each gesture moves 5 cm and turns 30 degrees, then returns to calibrated
+        Home, keeping the measured gripper opening. Horizontal gestures tilt the
+        downward-facing tool toward travel; the upward gesture turns about task Z.
+        An explicit 'home' action returns without a gesture.
+        Stop interrupts the single path, including all remaining return targets.
+        """
+        gestures = {"forward": {"y": -0.05, "roll": -30},
+                    "left": {"x": 0.05, "pitch": -30},
+                    "up": {"z": 0.05, "yaw": 30}}
+        if direction not in (*gestures, "home"):
+            raise ValueError("direction must be forward, left, up or home")
+        self.operations.reset_abort()
+        self._require_idle()
+        if not self.emb.calibrated or self.calibration.phase != "complete":
+            raise CalibrationRequired("Save coordinate calibration before checking directions")
+        self.emb.require_alive()
+        self.emb.arm.check_motion_ready()
+        if direction != "home" and not self._at_direction_home():
+            raise ControlNotReady("Return to Home before checking a direction")
+        targets = [HOME_POSE7]
+        if direction != "home":
+            targets.insert(0, geo.target_pose(HOME_POSE7, **gestures[direction], relative=True))
+        grip = float(self.emb.gripper.get_ratio())
+        self._direction_check = direction
+        try:
+            result = self._move_path(targets, [grip] * len(targets), [0.0] * len(targets),
+                                     should_stop=self.operations.abort.is_set, position_only=False,
+                                     workspace_entry=direction == "home")
+            return {**result, "direction": direction,
+                    "at_home": self.direction_check_status()["at_home"]}
+        except BaseException:
+            try:
+                self.emb.hold()
+            except Exception:
+                logger.exception("Could not hold after a failed direction check")
+            raise
+        finally:
+            self._direction_check = None
+
+    @exclusive
+    def reset(self) -> dict:
+        self._require_idle()
+        self.operations.reset_abort()
+        result = self._return_home(should_stop=self.operations.abort.is_set)
+        self._clear_resolved_errors()
+        self.records.event("Returned Home")
+        return result
+
+    @exclusive
+    def recover(self) -> dict:
+        self.emb.require_control()
+        self.stop("recover")
+        self.operations.reset_abort()
+        result = self._recover_and_home(should_stop=self.operations.abort.is_set)
+        self._clear_resolved_errors()
+        self.records.event("Devices recovered and returned Home")
+        return result
+
+    def _clear_resolved_errors(self) -> None:
+        if self.emb.alive and not self.emb.last_error and not self.operations.abort.is_set():
+            with self._lock:
+                self._stop_reason = None
+                self.records.clear_errors()
+
+    @exclusive
+    def preview(self) -> dict:
+        if self.mode != IDLE or self.calibration.phase not in {"acquire", "complete"}:
+            raise LoopRunning("finish the current operation before connecting preview")
+        self.emb.connect_preview()
+        for name, error in self.emb.preview_errors.items():
+            self.records.event(f"{name.capitalize()} preview unavailable: {error}", "error")
+        for name, notice in self.emb.preview_notices.items():
+            self.records.event(f"{name.capitalize()}: {notice}")
+        state = self.emb.status()
+        self.records.event("Preview connected" if state["arm_connected"] or state["camera_connected"] else "Preview unavailable")
+        return self.status()
+
+    @exclusive
+    def release(self) -> dict:
+        if self.mode != IDLE:
+            raise LoopRunning("finish the current operation before releasing devices")
+        calibration = self.calibration
+        calibration.close()
+        calibration.home_pose = calibration.draft = calibration.device = None
+        calibration.plane_poses = []
+        calibration.guiding_axis = None
+        calibration.phase = "complete" if calibration.profile else "acquire"
+        calibration.error = calibration.notice = None
+        self.emb.control_ready = False
+        self.disconnect()
+        self.records.event("Devices disconnected")
+        return self.status()
+
+    def connect(self) -> None:
+        if self.emb.connected:
+            return
+        self.records.open()
+        try:
+            self.policy.connect()
+        except PolicyUnavailable as exc:
+            self._close_policy()
+            logger.warning("policy is not ready; MCP setup remains available: %s", exc)
+        except Exception:
+            self._close_policy()
+            raise
+        try:
+            self.emb.connect()
+        except Exception:
+            self._close_policy()
+            raise
+
+    def _close_policy(self) -> None:
+        try:
+            self.policy.close()
+        except Exception:
+            logger.warning("policy connection did not close cleanly", exc_info=True)
+
+    def disconnect(self) -> None:
+        try:
+            self.emb.disconnect()
+        finally:
+            self._close_policy()
+
+    def close(self) -> None:
+        try:
+            self.stop("shutdown")
+        finally:
+            try:
+                self.calibration.close()
+            finally:
+                try:
+                    self.disconnect()
+                finally:
+                    self.tasks.close()
+
+    def _record_action(self, chunk, result, image_age, image):
+        self.records.write(chunk, result, image_age, image,
+                           state=self.emb.get_state(), control_trace=self._control_trace)
+        self._control_trace = []
+
+
+    def call_model(self, instruction: str | None, image: np.ndarray | None = None) -> ActionChunk:
+        self.emb.require_alive()
+        if image is None:
+            image, _ = self.emb.camera.read()
+        pose, grip = self.emb.arm.get_eef_pose(), self.emb.gripper.get_ratio()
+        if instruction is None:
+            with self._lock:
+                instruction = self._instruction or ""
+                if instruction != self._applied_instruction:
+                    task = self.tasks.current()
+                    if self._mode == RUNNING and task and task["instruction"] != instruction.strip():
+                        self._finish_task("instruction changed")
+                        self._begin_task(instruction)
+                    self._applied_instruction = instruction
+                    self.records.event("Instruction applied")
+        return self.policy.infer(pose, grip, image, instruction)
+
+    def apply_action(self, chunk: ActionChunk, should_stop=None, *, position_only=False,
+                     workspace_entry=False) -> dict:
+        """Dispatch the complete chunk on monotonic ABSOLUTE deadlines.
+
+        Deadline i is ``t0 + i*dt``, so scheduling error never accumulates; a late frame runs
+        immediately rather than being skipped. Skipping would jump over intermediate waypoints
+        and the inner loop would accelerate to catch up -- exactly the lunge we do not want.
+        `late` is counted and surfaced to diagnose persistent dispatch delays on hardware.
+
+        `should_stop` is checked per frame, so stop() and pause() both take effect within one
+        frame instead of waiting out a chunk. Both are human interventions; making the operator
+        watch 1.67 s of motion after asking for a hold is the wrong answer.
+        """
+        executed = 0
+        result = None
+
+        def progress(count):
+            nonlocal executed
+            executed = count
+
+        try:
+            self.emb.require_alive()
+            if self.mode == IDLE and not workspace_entry:
+                self._require_workspace_start()
+            result = self._dispatch(chunk.arm, chunk.gripper, should_stop,
+                                    on_executed=progress, position_only=position_only,
+                                    workspace_entry=workspace_entry)
+            return result
+        finally:
+            if chunk.session_id is not None:
+                self.policy.record_execution(chunk, executed,
+                                             completed=result is not None and not result["aborted"])
+
+    def _dispatch(self, poses, grips, should_stop=None, *, on_executed=None, position_only=False,
+                  workspace_entry=False) -> dict:
+        self.emb.arm.drain_trace()
+        try:
+            result = dispatch(self.emb.arm, poses, grips, gripper=self.emb.gripper,
+                              should_stop=should_stop, on_executed=on_executed,
+                              command_lock=self.emb.command_lock, position_only=position_only,
+                              workspace=None if workspace_entry else self.cfg.workspace)
+            self._observation_after = result["action_period_completed_at_monotonic"] or time.monotonic()
+            return result
+        finally:
+            self._control_trace = self.emb.arm.drain_trace()
+
+    def _move_path(self, targets7, grips, blends, should_stop=None, *, position_only=False,
+                   workspace_entry=False) -> dict:
+        """Drive through absolute pose7d waypoints as ONE motion, compiled to a chunk.
+
+        Deliberately NOT one `servo_pose` per waypoint: `servoL` assumes small increments at
+        rate, so a far target makes the UR sprint at its velocity ceiling; on Franka it would
+        saturate the bounded IK and discard the intended path and timing. Streaming an
+        interpolated path instead keeps the whole move inside the compliant control loop.
+        That matters more here than
+        anywhere else: this is the one path where a pose comes straight from a language model.
+
+        The track is an `ActionChunk` and goes through `apply_action` and the jsonl log, so an
+        agent-authored motion and a policy chunk are the same thing to everything downstream.
+        """
+        self.emb.require_alive()
+        cfg = self.cfg
+        start = self.emb.arm.get_eef_pose()
+        targets7 = workspace_targets(targets7, cfg.workspace)
+        image, image_age = self.emb.camera.read()
+        plan = geo.plan_path(
+            start, targets7, self.emb.gripper.get_ratio(), grips, blends,
+            speed=cfg.arm.cartesian_speed, angular_speed=cfg.arm.cartesian_angular_speed,
+            gripper_travel_s=cfg.gripper.full_travel_s, dt=ACTION_DT,
+        )
+        chunk = ActionChunk(plan.poses, plan.grips, 0.0, "waypoints")
+        result = self.apply_action(chunk, should_stop=should_stop, position_only=position_only,
+                                   workspace_entry=workspace_entry)
+        done = result["executed"]
+        out = {
+            **result,
+            "from": geo.pose7_to_xyzrpy(start).round(4).tolist(),
+            "waypoints": [
+                {"target": geo.pose7_to_xyzrpy(t).round(4).tolist(),
+                 "gripper": round(float(g), 4), "blend": round(float(r), 4)}
+                for t, g, r in zip(targets7, grips, plan.blends)
+            ],
+            "reached": int(plan.passed[done - 1]) if done else 0,
+            "path_m": round(plan.path_m, 4),
+            "planned_s": round(plan.planned_s, 2),
+        }
+        self._record_action(chunk, out, image_age, image)
+        return out
+
+    def step(self, instruction: str | None, should_stop=None, on_phase=None) -> dict:
+        """One serial iteration: infer, execute, then hold before the next fresh sample."""
+        observation = self.emb.camera.read_after(
+            self._observation_after, self.cfg.camera.stale_after_s, should_stop,
+        )
+        if observation is None:
+            return {"executed": 0, "late_frames": 0, "aborted": True, "elapsed_s": 0.0,
+                    "infer_ms": 0.0, "chunk_len": 0}
+        image, image_age = observation
+        if on_phase is not None:
+            on_phase("inference")
+        chunk = self.call_model(instruction, image)
+        try:
+            output = chunk.summary(self.emb.arm.get_eef_pose(), self.emb.gripper.get_ratio())
+            if on_phase is not None:
+                on_phase("execution", request_ms=chunk.request_ms)
+        except Exception:
+            if chunk.session_id is not None:
+                self.policy.record_execution(chunk, 0, completed=False)
+            raise
+        action_started = time.perf_counter()
+        result = self.apply_action(chunk, should_stop=should_stop)
+        action_ms = (time.perf_counter() - action_started) * 1e3
+        pause_s = self.cfg.runtime.inter_chunk_pause_s
+        if result["executed"] and not result["aborted"] and pause_s:
+            if on_phase is not None:
+                on_phase("inter_chunk_pause")
+            deadline = self._observation_after + pause_s
+            wait_until(deadline, should_stop)
+            # Reject frames begun during the pause. call_model samples arm/gripper only
+            # after this boundary's new camera frame arrives on the next iteration.
+            self._observation_after = time.monotonic()
+        self._record_action(chunk, result, image_age, image)
+        return {**result, "infer_ms": chunk.infer_ms, "chunk_len": len(chunk),
+                "_monitor": {"action_ms": round(action_ms, 1),
+                             "request_ms": round(chunk.request_ms, 1) if chunk.request_ms is not None else None,
+                             "image_age_ms": round(image_age * 1e3, 1),
+                             "request_id": chunk.request_id, "output": output}}
+
+    def _return_home(self, should_stop=None) -> dict:
+        """Open the gripper and return along the Cartesian path to training Home."""
+        self.emb.require_control()
+        self.emb.arm.check_motion_ready()
+        try:
+            self.emb.gripper.move_blocking(self.cfg.arm.home_gripper, should_stop=should_stop)
+            result = self._move_path([HOME_POSE7], [self.cfg.arm.home_gripper], [0.0],
+                                     should_stop=should_stop, position_only=True, workspace_entry=True)
+            if result["aborted"]:
+                raise RuntimeError("Return Home was stopped")
+            wait_for_pose(self.emb.arm, HOME_POSE7, should_stop)
+        except BaseException:
+            try:
+                self.emb.hold()
+            except Exception:
+                logger.exception("Could not confirm the stop after a failed Home return")
+            raise
+        self._observation_after = time.monotonic()
+        result = {"home_pose7": list(HOME_POSE7), "eef_frame": TRAINING_FRAME,
+                  "gripper": self.cfg.arm.home_gripper}
+        if (should_stop is None or not should_stop()) and (zero_ft := getattr(self.emb.arm, "zero_ft", None)):
+            try:
+                zero_ft()
+            except Exception as exc:
+                result["warning"] = f"Home reached; force sensor zeroing failed: {exc}"
+                self.records.event(result["warning"], "warning")
+        return result
+
+    def _recover_and_home(self, should_stop=None) -> dict:
+        """Recover the driver, then follow the Cartesian path to training Home.
+
+        Reconnect all components when control was released, so camera and policy
+        resources and the embodiment lifecycle are restored together.
+        """
+        self.emb.require_control()
+        if should_stop is not None and should_stop():
+            raise RuntimeError("recovery_cancelled")
+        policy_ready = self.policy.ready
+        if not self.emb.connected:
+            self.connect()
+        elif not self.emb.camera.live or not policy_ready:
+            # A stale camera/policy is part of the transaction, not an isolated arm fault.
+            # Reconnect all parts in the same order used at startup and roll back together.
+            self.disconnect()
+            self.connect()
+        else:
+            if self.cfg.arm.type == "ur":
+                self.emb.arm.recover(should_stop=should_stop)
+            else:
+                self.emb.arm.recover()
+            if should_stop is not None and should_stop():
+                raise RuntimeError("recovery_cancelled")
+            self.emb.gripper.recover()
+        result = self._return_home(should_stop=should_stop)
+        return {**result, "alive": self.emb.alive, "last_error": self.emb.last_error}
+
+
+def build(cfg: Config, *, config_path: str | Path | None = None) -> Runtime:
+    emb = build_embodiment(cfg)
+    policy = OpenAIChatPolicyClient(
+        cfg.policy.url, cfg.policy.token_file, cfg.policy.model,
+        image_height=cfg.policy.image_height, timeout_s=cfg.policy.timeout_s,
+        use_env_proxy=cfg.policy.use_env_proxy,
+    )
+    return Runtime(emb, policy, config_path=config_path)