Architecture Is the Design of Seams: An Embodied Agent Skeleton That Runs, Tests, and Swaps Components

┌───────────── Training Stack (offline) ─────────────┐
│ Dataset → Preprocess → Policy/WM → Eval → Artifact │
└──────────────────────┬───────────────────────┘
      Shared contracts: State / Action Schema · ObsTransform · Artifact Manifest
┌──────────────────────┴───────────────────────┐
│                Runtime Stack (real-time)             │
│ Sensor → Estimator → StateContract → Policy → Action │
│                       ↓ buffer / interpolate / hold  │
│               Safety Gate → Controller → Robot       │
└──────────────────────────────────────────────┘

# agent/state/obs_transform.py
# Both training and deployment import from this single module; separate implementations are forbidden
import numpy as np

class ObsTransform:
    """The single authoritative preprocessing for camera observations, shared by train/deploy."""

    IMG_SIZE = (224, 224)

    def __call__(self, raw_bgr: np.ndarray) -> np.ndarray:
        img = raw_bgr[..., ::-1]              # BGR -> RGB
        img = resize(img, self.IMG_SIZE)      # bilinear, aligned with the training config
        return (img / 127.5 - 1.0).astype(np.float32)  # consistent with the checkpoint

# deploy/artifact_manifest.yaml — the complete manifest needed to reproduce one deployment
artifact:
  checkpoint: ckpt/diffusion_v17.pt
  code_commit: 8f3a2c1           # commit of the training code
  state_schema: v3              # StateContract field-definition version
  action_schema: v2             # ActionSpace / coordinate frame / dt convention version
  obs_transform: v5             # version of the preprocessing shared by train/deploy
  normalization: stats/norm_v17.npz   # normalization statistics
  robot_model: calib/ur5e_2026-08.yaml  # robot calibration
runtime:
  container_digest: sha256:71c0...  # digest of the inference image, not a tag
  python: "3.11"
  pytorch: "2.4"
  cuda: "12.4"
compat:                        # the schema range this ckpt is compatible with; validated at startup
  state_schema: [v3, v4]
  action_schema: [v2]

embodied_agent/
├── agent/
│   ├── core/            # loop, clock, lifecycle management
│   ├── perception/      # Layer 1: Sensor drivers (read → RawObservation)
│   ├── state/           # Layer 2: StateContract + StateEstimator
│   ├── policy/          # Layer 3: Protocol + implementations per route
│   ├── action/          # Layer 4: tokenizer / chunking / safety gate
│   ├── control/         # Layer 5: robot SDK adapters
│   └── safety/          # watchdog, clamping, safe poses
├── train/
│   ├── data/            # episode format, dataset, replay
│   ├── sim/             # sim backend (same RobotInterface as the real robot)
│   └── learn/           # training entry, loss, checkpoint export
├── configs/             # one yaml per robot / per task
├── tests/               # layered tests (see below)
└── deploy/              # exported artifacts, version manifest

# agent/state/contract.py
from dataclasses import dataclass
from enum import Enum

class Hypothesis(Enum):
    """A multimodal estimator may return multiple hypotheses; silently collapsing them is forbidden (cf. 9/15 Failure 1)."""
    SINGLE = "single"
    MULTI = "multi"          # requires mode_select on the policy side

@dataclass(frozen=True)
class Provenance:
    source: str              # which sensor / estimator produced it
    stamp: float             # physical-world sampling time (same clock domain as monotonic())
    frame_id: str            # coordinate frame

@dataclass(frozen=True)
class ObservationField:
    """One observation in schema-typed packaging: beyond the value, the semantic metadata is complete."""
    name: str                # "image" / "depth" / "tactile" / ...
    value: object
    dtype: str
    shape: tuple
    unit: str | None
    frame_id: str | None
    stamp: float

@dataclass(frozen=True)
class Observation:
    """Open multimodal container: adding a sensor = adding a field; the contract doesn't move."""
    fields: tuple            # tuple[ObservationField, ...]

    def get(self, name: str) -> ObservationField | None:
        for f in self.fields:
            if f.name == name:
                return f
        return None          # an absent sensor is normal; the policy must handle it

@dataclass(frozen=True)
class ProprioState:
    """Proprioception: field names are the contract; order is not; units are also the contract.
    Convention: position=rad, velocity=rad/s, effort=Nm; changing units goes through
    a schema version, never a verbal notice."""
    joint_names: tuple       # ("shoulder_pan", "shoulder_lift", ...)
    position: tuple
    velocity: tuple
    effort: tuple | None
    stamp: float

@dataclass(frozen=True)
class StateQuality:
    """State usability, not probability. Given per source; Safety gates per task definition."""
    proprio: float
    vision: float
    localization: float
    temporal: float          # multi-sensor alignment quality (see below)

    def min(self) -> float:
        return min(self.proprio, self.vision, self.localization, self.temporal)

@dataclass(frozen=True)
class StateContract:
    """The only state object a policy is allowed to consume (cf. 9/15: the policy is a contract consumer)."""
    observation: Observation
    proprio: ProprioState
    task: object                       # instruction / goal representation (None for MPC-style routes without language input)
    hypothesis: Hypothesis
    provenance: Provenance
    validity_sec: float                # validity window of this state (expired states must be gated out)
    quality: StateQuality
    schema_version: str                # field-definition version; validated at startup against the
                                       # artifact manifest's state_schema: the object must know which version it is

    def age_sec(self, now: float) -> float:
        """Data age = now - physical sampling time. If the estimator spent 100 ms computing, the age grows by 100 ms.
        `now` is passed in explicitly by the caller: the contract does not read the wall clock internally (see the Clock Protocol below)."""
        return now - self.provenance.stamp

    def is_fresh(self, now: float) -> bool:
        return self.age_sec(now) < self.validity_sec

# agent/core/clock.py
from typing import Protocol
import time

class Clock(Protocol):
    """The single source of time for the whole system. Every spot in business
    code that scatter-reads time.monotonic() is a time crack that tests cannot freeze."""
    def monotonic(self) -> float: ...

class SystemClock:
    def monotonic(self) -> float:
        return time.monotonic()

@dataclass(frozen=True)
class RawObservation:
    """Raw reading from a single sensor."""
    sensor_id: str
    sensor_timestamp: float    # hardware sampling time (note which clock domain it belongs to)
    receive_timestamp: float   # when it entered the software
    sequence_id: int
    clock_domain: str          # "monotonic" / "ros_wall" / ...
    calibration_version: str
    payload: object

Raw sensor timestamps (each in its own clock domain)
    ↓ clock normalization: converted into the monotonic domain, conversion version recorded
Sample buffer with unified timestamps
    ↓ temporal alignment: window-based alignment, interpolation/extrapolation, timeout drop
StateContract (provenance.stamp already in the monotonic domain)

# agent/perception/sensor_buffer.py, agent/state/estimator.py
@dataclass(frozen=True)
class SensorSnapshot:
    """The set of latest samples per source at time now: the input to the estimator's
    temporal alignment, not a single-sensor read."""
    samples: tuple          # tuple[RawObservation, ...], at most one per source
    window_sec: float       # alignment window: samples older than now - window must not join

class SensorBuffer(Protocol):
    """Layer 1: the driver thread keeps writing timestamped samples into a ring buffer;
    consumers take the latest non-blockingly. The production form is never a blocking
    read() — a runtime thread has no business blocking on a sensor."""
    def latest(self, now: float) -> SensorSnapshot: ...

class StateEstimator(Protocol):
    """Layer 2: the only place in the system allowed to produce a StateContract.
    Responsibilities: clock-domain normalization, temporal alignment, interpolation/
    extrapolation, drop policy, max sync window."""
    def estimate(self, snapshot: SensorSnapshot, now: float) -> StateContract: ...

# agent/action/contract.py
class ActionSpace(Enum):
    JOINT_POS = "joint_pos"
    JOINT_DELTA = "joint_delta"
    EE_POSE_DELTA = "ee_pose_delta"   # end-effector pose delta
    # ...

@dataclass(frozen=True)
class ActionSchema:
    """Spatial semantics and units: space defines semantics, representation defines
    the encoding; units and coordinate frames are contract."""
    space: ActionSpace
    representation: str         # encoding convention: joint_rad / ee_se3_log / ee_6d_rot / ...
    translation_unit: str|None  # translation unit (None for joint actions); mandatory for EE actions
    rotation_unit: str|None     # rotation representation: se3_log / quat / euler_xyz / 6d ...
    frame_id: str               # coordinate frame: joint / base / ee ...

@dataclass(frozen=True)
class Action:
    """Policy output is not a bare blob of tensor: semantics, frame, and time basis are all written into the object."""
    values: object        # action values: a chunk (H, D) or a single step (D,)
    schema: ActionSchema  # spatial semantics, encoding, units, frame
    schema_version: str   # schema fingerprint: consumers fail-before-motion based on it
    dt: float             # time interval between adjacent action points
    horizon: int          # chunk length; 1 for a single-step action

    generated_at: float   # when inference finished and the chunk was handed to Layer 4
    valid_from: float     # validity start
    valid_until: float    # hard expiry: past this, stale and must not be consumed
    state_stamp: float    # physical sampling time of the state this decision is based on
    epoch: int            # lifecycle identity: which episode / runtime epoch it belongs to
    sequence_id: int      # decision serial within the epoch: async commits must not arrive out of order (see the scheduling protocol)

# agent/policy/base.py
from typing import Protocol

class Policy(Protocol):
    """The seam of Layer 3. VLA / Diffusion / MPC are merely its implementations."""

    def reset(self, epoch: int, alloc: "SequenceAllocator") -> None:
        """Episode boundary: clear KV cache / hidden states / planning cache,
        and collect this epoch's identity and decision serial — (epoch, sequence_id) is the
        decision's true identity. A global next_seq()-style process-level mutable state
        pollutes replay and reset, and is forbidden."""
        ...

    def act(self, state: StateContract, now: float) -> Action:
        """Given a state and a world-time moment, emit an action (usually an action chunk). `now` is passed in explicitly by the Clock."""
        ...

# agent/policy/vla.py (illustrative; framework details omitted)
class VLAPolicy:
    def __init__(self, ckpt, action_tokenizer, device="cuda"):
        self.model = load_vla(ckpt).to(device).eval()
        self.tok = action_tokenizer
        self._past = None

    def reset(self, epoch=0, alloc=None):
        self._past = None     # the KV cache belongs to the epoch: cleared together with reset

    @torch.no_grad()
    def act(self, state: StateContract, now: float) -> Action:
        image = state.observation.get("image")   # -> ObservationField | None; absence must degrade gracefully
        ids = self.model.generate(
            image=None if image is None else image.value,
            instruction=state.task,
            past=self._past,
        )
        # schematic: whether the KV cache can be reused across frames depends on the model architecture
        # (whether visual tokens are replaced each frame, positional encoding, causal attention structure) — never assume it
        self._past = ids.kv_cache
        return self.tok.decode(ids.action_bins)  # -> Action(chunk); Layer 4 releases it on schedule

# agent/policy/mpc.py (world model in the loop, echoing the "hybrid architecture" trend)
class MPCPolicy:
    """Learns no explicit policy; instead rolls out with the world model to pick actions — the runtime form of the TD-MPC route."""

    def __init__(self, world_model, candidate_sampler, horizon=5, n_candidates=32):
        self.wm = world_model
        self.sampler = candidate_sampler
        self.horizon = horizon
        self.n = n_candidates

    def reset(self, epoch=0, alloc=None):
        pass  # no cross-frame state: re-encode from the current observation every frame (stateless re-plan)

    def act(self, state: StateContract, now: float) -> Action:
        z = self.wm.encode(state)
        cands = self.sampler(z, n=self.n, horizon=self.horizon)
        # schematic: compressing dynamics rollout / cost / constraint / uncertainty
        # into estimate_return() only to highlight the Policy seam; a real MPC must not hide
        # those semantics inside an unconstrained scalar scorer
        scores = [self.wm.estimate_return(z, c) for c in cands]
        best = cands[int(np.argmax(scores))]
        u0 = best[0]  # execute only the first step, replan next frame (receding horizon)
        return Action(values=u0, horizon=1, ...)   # Policy.act returns an Action, not a bare control vector

# agent/policy/diffusion.py (illustrative: timestep / noise schedule / CFG and other sampling details omitted)
class DiffusionPolicy:
    """Generative policy: denoise an action chunk over multiple steps, naturally multi-hypothesis (cf. 9/15 Failure 1)."""

    def __init__(self, denoiser, n_steps=10, clock=None):
        self.den = denoiser
        self.n_steps = n_steps
        self.clock = clock if clock is not None else SystemClock()   # Clock Protocol

    def reset(self):
        pass  # stateless, or clear the conditioning queue depending on implementation

    def act(self, state: StateContract) -> Action:
        x = torch.randn(1, CHUNK_LEN, ACTION_DIM)   # denoising starts from noise; the endpoint is not
        image = state.observation.get("image")
        for _ in range(self.n_steps):
            x = self.den(x, None if image is None else image.value, state.task)
        values = self.denormalize(x[0])   # model output -> denormalization -> action values
        now = self.clock.monotonic()      # the single source of time: no scatter-reads of time.monotonic()
        return Action(values=values,
                      schema=ActionSchema(space=ActionSpace.JOINT_POS,
                                          representation="joint_rad",
                                          translation_unit=None,
                                          rotation_unit=None,   # joint space has no rotation unit: that belongs to SE(3) actions
                                          frame_id="joint"),
                      schema_version="joint_pos@v2",
                      dt=0.02, horizon=CHUNK_LEN,
                      generated_at=now, valid_from=now,
                      valid_until=now + CHUNK_LEN * 0.02,
                      state_stamp=state.provenance.stamp,
                      sequence_id=next_seq())  # the chunk goes to Layer 4 for timed release

ActionBuffer semantics (active + scheduled dual slots, epoch barrier):
- one serial space per epoch: put() only accepts chunks of the current epoch with sequence_id > last_accepted;
  old-epoch / out-of-order late arrivals are rejected outright — after a reset, a late old decision permanently loses the right to override
- stale chunk (valid_until passed): drop — never carry an old decision forward
- future chunk (valid_from in the future): park in the scheduled slot; take over at the control boundary when valid_from arrives;
  in the meantime only a chunk with a larger sequence_id may replace it
- active chunk: a later arrival with a larger sequence_id and an already-reached valid_from takes over the active slot immediately
- takeover happens only at control boundaries — never switch mid-tick;
  while A is executing and B has not reached its effective point, keep executing A — no execution gap is allowed
- missing chunk (no decision available): enter the safe state, not silent reuse
- action discontinuity (A→B jump too large): project / rate-limit before release

# agent/control/robot.py, train/sim/task_env.py
class RobotInterface(Protocol):
    """Real robot and sim backends implement the same interface: only observe, send, reset."""
    def observe(self) -> RawObservation: ...
    def send(self, cmd: ControlCommand) -> None: ...
    def reset_robot(self) -> None: ...

class TaskEnv(Protocol):
    """Task definition: strips reward / done / success criteria out of the hardware."""
    def reset(self) -> StateContract: ...
    def step(self, action: Action) -> Transition: ...   # only Transition carries reward/done

class SimBackend:
    """MuJoCo / Isaac adapter implementing RobotInterface. Proprio field names strictly identical to the real-robot driver."""
    ...

class RealRobot:
    """Real-robot adapter implementing RobotInterface. All blocking IO moves to background threads; observe() only takes the latest."""
    ...

class PickPlaceTask:
    """One kind of TaskEnv: wraps a RobotInterface or SimBackend and is responsible for scoring reward."""
    ...

# agent/core/loop.py (schematic)
CONTROL_DT = 1.0 / 200     # command update cadence 200 Hz; the policy thread runs at the model's own pace
POLICY_DT = 1.0 / 20       # upper cadence of policy production: slowness is the model's call, overshooting is not allowed
MIN_QUALITY = 0.5          # state-usability floor: below this, no new decision is produced

class AgentLoop:
    def __init__(self, sensor, estimator, policy, state_buffer,
                 action_buffer, safety, sink, clock):
        self.sensor = sensor                # Sensor Protocol (Layer 1)
        self.estimator = estimator          # StateEstimator Protocol (Layer 2)
        self.policy = policy                # Policy Protocol (Layer 3)
        self.state_buffer = state_buffer    # Layer 2 exit: state register
        self.action_buffer = action_buffer  # Layer 4: chunk register + scheduling protocol
        self.safety = safety                # independent safety layer (SafetyGate)
        self.sink = sink                    # the sole exit: ApprovedCommandSink
        self.clock = clock                  # Clock Protocol: no scatter-reads of time.monotonic()

    def _estimator_thread(self):
        while self._running:
            now = self.clock.monotonic()
            snapshot = self.sensor.latest(now)   # non-blocking: takes the latest sample set from each source
            state = self.estimator.estimate(snapshot, now)
            self.state_buffer.publish(state)     # sole writer; consumers each take the latest-and-fresh

    def _policy_thread(self):
        while self._running:
            now = self.clock.monotonic()
            state = self.state_buffer.latest_valid(now)  # decoupled from data production
            if state is None or state.quality.min() < MIN_QUALITY:
                self.safety.enter_safe_state("stale_state")   # stale data / insufficient quality
                self._sleep_until(now + POLICY_DT)   # the error path keeps the cadence too — no busy-waiting
                continue
            action = self.policy.act(state, now)   # 100 ms+ is fine — it carries no control deadline
            self.action_buffer.put(action, now)    # the chunk goes to Layer 4 for release per the scheduling protocol

    def _control_thread(self):
        next_tick = self.clock.monotonic()
        while self._running:
            now = self.clock.monotonic()
            state = self.state_buffer.latest_valid(now)   # the control side independently re-judges freshness
            if state is None:
                self.safety.enter_safe_state("stale_state")
            else:
                cmd = self.action_buffer.current(now)  # returns None when no decision is available (protocol-defined)
                if cmd is None:
                    self.safety.enter_safe_state("no_action")
                else:
                    decision = self.safety.evaluate(state=state, action=cmd,
                                                    context={"now": now})  # production form; see the next section
                    if decision.allow:
                        self.sink.submit(decision.command)  # the sole exit: only approved commands enter the RobotInterface
                    else:
                        self.safety.enter_safe_state(decision.reason)
            next_tick += CONTROL_DT      # cadence advance is decoupled from per-tick cost — no drift
            if now > next_tick:
                self.safety.record_deadline_miss(now - next_tick)  # breach bookkeeping, handled per miss policy
                next_tick = now          # realign: skip tick / degraded / safe stop
            self._sleep_until(next_tick)

    def _sleep_until(self, deadline):
        # align to the cadence with a monotonic clock; on timeout, record a jitter metric instead of swallowing it silently
        ...

# tests/fakes.py — fake implementations in pure stdlib; pytest runs them directly
# contract fields identical in shape to agent/state/contract.py and agent/action/contract.py;
# to stay minimal, fields such as quality / SafeState are omitted — only the seams this test group pins down are kept.
from dataclasses import dataclass
from typing import Optional, Tuple
import math

CHUNK_LEN = 16
DT = 0.02


@dataclass(frozen=True)
class Provenance:
    source: str
    stamp: float                 # physical sampling time (same clock domain as the injected clock)
    frame_id: str


@dataclass(frozen=True)
class StateContract:
    proprio: Tuple[float, ...]
    provenance: Provenance
    validity_sec: float

    def is_fresh(self, now: float) -> bool:
        return (now - self.provenance.stamp) < self.validity_sec


class SequenceAllocator:
    """Decision serial allocator: one per runtime epoch, monotonic within the epoch, zeroed on reset.
    A global next_seq() pollutes replay and reset — (epoch, sequence_id) is the decision's true identity."""

    def __init__(self):
        self._next = 0

    def next(self) -> int:
        self._next += 1
        return self._next


@dataclass(frozen=True)
class Action:
    values: Tuple[float, ...]    # action chunk (CHUNK_LEN,)
    dt: float
    horizon: int
    generated_at: float          # when inference finished and the chunk was handed to Layer 4
    valid_from: float
    valid_until: float           # hard expiry: past this, the chunk is stale and must not be consumed
    state_stamp: float           # physical sampling time of the state this decision is based on
    epoch: int                   # lifecycle identity: which episode / runtime epoch it belongs to
    sequence_id: int             # decision serial: monotonic within the epoch; async commits must not arrive out of order


class FakeClock:
    """Clock injected for tests (production counterpart SystemClock; both implement the same Clock Protocol)."""

    def __init__(self):
        self.t = 0.0

    def monotonic(self):
        return self.t

    def advance(self, dt):
        self.t += dt


class FakeSensor:
    """Minimal form of the Layer-1 buffer: latest(now) takes only the newest sample and never blocks the caller."""

    def __init__(self, clock):
        self.clock = clock

    def latest(self, now):
        t = now
        return {"angle": math.sin(t), "stamp": t, "frame_id": "joint"}


class DelayedSensor(FakeSensor):
    """Simulates a sensor delayed by 0.2 s: data age exceeding validity should trigger the safe state."""

    def latest(self, now):
        raw = super().latest(now)
        return {**raw, "stamp": raw["stamp"] - 0.2}


class FailingSensor(FakeSensor):
    """Simulates a sensor that dies at fail_at: the stamp freezes on the last frame before death, and data age keeps growing."""

    def __init__(self, clock, fail_at):
        super().__init__(clock)
        self.fail_at = fail_at

    def latest(self, now):
        t = min(now, self.fail_at)
        return {"angle": math.sin(t), "stamp": t, "frame_id": "joint"}


class NaiveEstimator:
    def estimate(self, snapshot, now):
        return StateContract(
            proprio=(snapshot["angle"],),
            provenance=Provenance("fake_sensor", snapshot["stamp"], snapshot["frame_id"]),
            validity_sec=0.05,
        )


class StateBuffer:
    """Minimal form of the cross-thread seam: the estimator publishes the newest state, and the policy / control sides each take the 'newest and still fresh' one.

    Each consumer judges freshness on its own: the policy-side gate only guarantees 'the state entering the decision is fresh'; the control side must re-judge — the state a decision was based on may already be expired by the time it executes.
    """

    def __init__(self):
        self._state = None

    def publish(self, state):
        self._state = state

    def latest_valid(self, now):
        s = self._state
        if s is None or not s.is_fresh(now):
            return None        # expired/missing: hand it to Safety to enter the safe state — never silently reuse
        return s


class SinePolicy:
    """deterministic policy: the output depends only on the state, so episode replay is bit-for-bit identical."""

    def reset(self, epoch=1, allocator=None):
        self._epoch = epoch
        self._alloc = allocator if allocator is not None else SequenceAllocator()

    def act(self, state, now):
        return Action(
            values=tuple(state.proprio[0] for _ in range(CHUNK_LEN)),
            dt=DT, horizon=CHUNK_LEN,
            generated_at=now, valid_from=now,
            valid_until=now + CHUNK_LEN * DT,
            state_stamp=state.provenance.stamp,
            epoch=self._epoch,
            sequence_id=self._alloc.next(),
        )


class SlowPolicy(SinePolicy):
    """Inference latency injection: the decision sees the state at the moment it is initiated, and is committed only latency seconds later —
    the gap before it takes effect is exactly where the 'action coverage' gap comes from."""

    def __init__(self, latency=0.15):
        self.latency = latency

    def act(self, state, now):
        ready = now + self.latency
        return Action(
            values=tuple(state.proprio[0] for _ in range(CHUNK_LEN)),
            dt=DT, horizon=CHUNK_LEN,
            generated_at=ready, valid_from=ready,
            valid_until=ready + CHUNK_LEN * DT,
            state_stamp=state.provenance.stamp,
            epoch=self._epoch,
            sequence_id=self._alloc.next(),
        )


class ActionBuffer:
    """Minimal Layer 4: active + scheduled dual slots + epoch barrier.
    The precise semantics of latest-wins: a future chunk is parked until its takeover point — never creating an execution gap."""

    def __init__(self, epoch=1):
        self._epoch = epoch
        self._active: Optional[Action] = None
        self._scheduled: Optional[Action] = None
        self._last_accepted_sequence = 0
        self._active_sequence = 0
        self.events = []         # evidence hook: accepted/activated/preempted/expired/rejected_*

    def sync_epoch(self, epoch):
        """The barrier of reset_episode: decisions of the old epoch are voided immediately, and the serial restarts."""
        self._epoch = epoch
        self._active = None
        self._scheduled = None
        self._last_accepted_sequence = 0
        self._active_sequence = 0
        self.events.clear()

    def put(self, action, now) -> bool:
        """Only accepts decisions of the current epoch that are newer; old-epoch / out-of-order late arrivals are rejected outright."""
        if action.epoch != self._epoch:
            self.events.append(("rejected_stale_epoch", action.sequence_id))
            return False
        if action.sequence_id <= self._last_accepted_sequence:
            self.events.append(("rejected_out_of_order", action.sequence_id))
            return False
        self.events.append(("accepted", action.sequence_id))
        self._last_accepted_sequence = action.sequence_id
        if action.valid_from <= now:
            if self._active is not None:
                self.events.append(("preempted", self._active.sequence_id))
            self._active = action            # takes effect immediately: latest-wins
            self._active_sequence = action.sequence_id
            self._scheduled = None
            self.events.append(("activated", action.sequence_id))
        else:
            self._scheduled = action         # future: parked, takes over at the control boundary when valid_from arrives
        return True

    def current(self, now: float) -> Optional[float]:
        """Index the chunk point by time; the scheduled one is promoted at its takeover point, expired ones are voided — never reused."""
        s = self._scheduled
        if s is not None and now >= s.valid_from:
            if self._active is not None:
                self.events.append(("preempted", self._active.sequence_id))
            self._active = s
            self._active_sequence = s.sequence_id
            self._scheduled = None
            self.events.append(("activated", s.sequence_id))
        a = self._active
        if a is None:
            return None
        if now >= a.valid_until:
            self.events.append(("expired", a.sequence_id))
            self._active = None              # expired decision voided, not reused
            return None
        idx = min(int((now - a.valid_from) / a.dt), a.horizon - 1)   # ZOH: floor, not round
        return a.values[idx]


class SafetyLimiter:
    """Minimal safety layer: a velocity ceiling (a physical quantity, rad/s) + safe-state entry logging.
    The limiting is based on the measured position, not the previous command: sending 1.0 does not mean the robot reached 1.0."""

    def __init__(self, max_velocity=5.0, dt=DT):
        self.max_velocity = max_velocity     # what is limited is velocity, not position
        self._dt = dt
        self.safe_entries = []

    def check(self, state, cmd):
        pos, = cmd
        measured, = state.proprio            # whether the velocity is legal must be judged against the measured state, not the last command
        max_delta = self.max_velocity * self._dt   # velocity ceiling converted into a per-tick position increment
        delta = max(-max_delta, min(max_delta, pos - measured))
        return (measured + delta,)           # a legal position ≠ a legal velocity

    def enter_safe_state(self, reason):
        self.safe_entries.append(reason)

    def reset(self):
        self.safe_entries.clear()


class FakeController:
    def __init__(self):
        self.sent = []

    def send(self, cmd):
        self.sent.append(cmd)


class ControllerSink:
    """Test version of the CommandSink: the only object allowed to call controller.send().
    RuntimeCore only gets the sink — writing to the controller while bypassing the SafetyLimiter is structurally impossible,
    the same authority constraint as in production (see the safety gate section)."""

    def __init__(self, controller):
        self._controller = controller

    def submit(self, cmd):
        self._controller.send(cmd)


class RuntimeCore:
    """The same loop body as production: the two ticks are driven by a scheduler, not by threads.
    The epoch is owned by the core: reset_episode increments it and syncs it to the policy and the ActionBuffer."""

    def __init__(self, sensor, estimator, policy, action_buffer, safety, sink,
                 state_buffer):
        self.sensor = sensor
        self.estimator = estimator
        self.policy = policy
        self.action_buffer = action_buffer
        self.safety = safety
        self.sink = sink   # the sole exit: approved commands reach the controller through the CommandSink
        self.state_buffer = state_buffer
        self.epoch = 0

    def reset_episode(self):
        self.epoch += 1
        self.policy.reset(self.epoch, SequenceAllocator())
        self.safety.reset()
        self.action_buffer.sync_epoch(self.epoch)
        # production also resets: estimator filters, StateBuffer, controller tracking, seeds

    def prime(self, now):
        """Fill the state buffer at the episode start so the policy has state to consume on its first tick."""
        state = self.estimator.estimate(self.sensor.latest(now), now)
        if state.is_fresh(now):
            self.state_buffer.publish(state)

    def policy_tick(self, now):
        state = self.state_buffer.latest_valid(now)
        if state is None:
            self.safety.enter_safe_state("stale_state")  # policy-side freshness gate
            return
        self.action_buffer.put(self.policy.act(state, now), now)

    def control_tick(self, now):
        # in production the estimator runs in its own high-frequency loop publishing to the StateBuffer;
        # the minimal loop folds it into the control tick, and the two judge latest_valid by exactly the same criterion.
        state = self.estimator.estimate(self.sensor.latest(now), now)
        if state.is_fresh(now):
            self.state_buffer.publish(state)
        state = self.state_buffer.latest_valid(now)
        if state is None:
            self.safety.enter_safe_state("stale_state")  # control-side freshness gate, independent of the policy side
            return
        point = self.action_buffer.current(now)
        if point is None:
            self.safety.enter_safe_state("no_action")   # missing: no silent reuse
            return
        self.sink.submit(self.safety.check(state, (point,)))   # the sole exit: approved command goes through the CommandSink


def run_episode(clock, n_steps, hz=50, sensor=None, policy=None, policy_every=5):
    """FakeScheduler: the policy cadence is approximated with policy_every ticks, clock injected."""
    controller = FakeController()
    core = RuntimeCore(sensor if sensor is not None else FakeSensor(clock),
                       NaiveEstimator(),
                       policy if policy is not None else SinePolicy(),
                       ActionBuffer(), SafetyLimiter(), ControllerSink(controller),
                       StateBuffer())
    core.reset_episode()
    core.prime(clock.monotonic())
    dt = 1.0 / hz
    for i in range(n_steps):
        now = clock.monotonic()
        if i % policy_every == 0:
            core.policy_tick(now)
        core.control_tick(now)
        clock.advance(dt)
    return controller, core.safety

# tests/test_loop.py
import pytest

from fakes import (CHUNK_LEN, DT, Action, ActionBuffer, FakeClock, FakeSensor,
                   DelayedSensor, FailingSensor, NaiveEstimator, Provenance,
                   SafetyLimiter, SinePolicy, SlowPolicy, StateContract, run_episode)


def _state(angle):
    """Build a state whose measured joint position is `angle`: the input to the rate-limit tests."""
    return StateContract(proprio=(angle,),
                         provenance=Provenance("test", 0.0, "joint"),
                         validity_sec=0.05)


def test_episode_replay_bitwise_identical():
    a, _ = run_episode(FakeClock(), n_steps=100)
    b, _ = run_episode(FakeClock(), n_steps=100)
    assert a.sent == b.sent   # deterministic policy + injected clock => bit-for-bit identical


def test_stale_state_enters_safe_state():
    clock = FakeClock()
    _, safety = run_episode(clock, n_steps=10, sensor=DelayedSensor(clock),
                            policy_every=1)
    # the two gates keep their own books: the policy-side freshness gate fires 10 times (never publishes),
    # the control-side latest_valid fires 10 times (the register is empty); stale is reported before missing
    assert safety.safe_entries.count("stale_state") == 20
    assert safety.safe_entries.count("no_action") == 0


def test_safety_limits_against_measured_state():
    # velocity limiting must be done against the measured position, not the previous command:
    # sending 1.0 does not mean the robot reached 1.0 — computing Δq against the command systematically underestimates the true velocity
    safety = SafetyLimiter(max_velocity=5.0, dt=DT)   # 5 rad/s @ 50 Hz => per-tick ceiling 0.1 rad
    assert safety.check(_state(0.7), (5.0,)) == pytest.approx((0.8,))    # target 5.0: Δq=4.3, clamped to 0.7+0.1
    assert safety.check(_state(0.75), (5.0,)) == pytest.approx((0.85,))  # measured only reached 0.75: re-limit against the measurement


def test_out_of_order_commit_rejected():
    buf = ActionBuffer(epoch=1)
    clock = FakeClock()
    p = SinePolicy(); p.reset(epoch=1)
    state = NaiveEstimator().estimate(FakeSensor(clock).latest(0.0), 0.0)
    older = p.act(state, 0.0)
    newer = p.act(state, 0.0)     # simulate request #42 finishing before #41
    assert buf.put(newer, 0.0)
    assert not buf.put(older, 0.0)                # a late old decision must be rejected
    assert buf.current(0.0) == newer.values[0]


def test_stale_state_cannot_execute_buffered_action():
    # t=0.0 the state is fresh and the policy produces A (valid_until=0.32); the sensor dies at t=0.1.
    # during t=0.16~0.30 A has not expired yet, but the state is already stale — the control side must refuse to execute A.
    clock = FakeClock()
    ctrl, safety = run_episode(clock, n_steps=20, sensor=FailingSensor(clock, fail_at=0.10),
                               policy_every=100)
    assert len(ctrl.sent) == 8                 # t=0.00~0.14: state fresh, normal execution
    # t=0.16~0.38: state stale, A must never execute even though it is still unexpired in the buffer
    assert safety.safe_entries.count("stale_state") == 12
    assert safety.safe_entries.count("no_action") == 0


def test_slow_policy_yields_coverage_gap_then_recovers():
    # real latency injection: the decision is initiated at t and takes effect only at t+150ms. The control cadence is not
    # dragged down by inference, but there is no executable decision in the gap before it takes effect — the coverage gap lands explicitly in the safe state.
    clock = FakeClock()
    ctrl, safety = run_episode(clock, n_steps=40, policy_every=10,
                               policy=SlowPolicy(latency=0.15))
    assert len(ctrl.sent) == 32                # the first 8 ticks are the gap: no decision during the 150ms latency
    assert safety.safe_entries.count("no_action") == 8
    assert ctrl.sent                            # after inference completes, the control loop resumes execution


def test_epoch_barrier_rejects_stale_episode_action():
    # a late decision of Episode 41 (with a larger seq) must never override the current decision of Episode 42
    buf = ActionBuffer(epoch=1)
    clock = FakeClock()
    p41 = SinePolicy(); p41.reset(epoch=1)
    state = NaiveEstimator().estimate(FakeSensor(clock).latest(0.0), 0.0)
    p41.act(state, 0.0)
    late = p41.act(state, 0.0)          # decision #2 of episode 41, arriving late
    buf.sync_epoch(2)                    # reset_episode: episode 42 begins
    p42 = SinePolicy(); p42.reset(epoch=2)
    fresh = p42.act(state, 0.0)          # decision #1 of episode 42
    assert buf.put(fresh, 0.0)
    assert not buf.put(late, 0.0)        # old epoch arriving late: rejected even with a larger seq
    assert buf.current(0.0) == fresh.values[0]
    assert ("rejected_stale_epoch", late.sequence_id) in buf.events


def test_scheduled_chunk_takes_over_at_its_valid_from():
    # while A is executing, B (larger seq) arrives early but its valid_from is in the future:
    # A must keep executing during 0.10~0.20, and B takes over at the control boundary at 0.20 — no gap is allowed in between
    buf = ActionBuffer(epoch=1)
    a = Action(values=tuple(float(i) for i in range(CHUNK_LEN)), dt=DT, horizon=CHUNK_LEN,
               generated_at=0.0, valid_from=0.0, valid_until=0.32,
               state_stamp=0.0, epoch=1, sequence_id=10)
    b = Action(values=(2.0,) * CHUNK_LEN, dt=DT, horizon=CHUNK_LEN,
               generated_at=0.10, valid_from=0.20, valid_until=0.52,
               state_stamp=0.10, epoch=1, sequence_id=11)
    assert buf.put(a, 0.10)
    assert buf.current(0.10) == 5.0      # point 5 of A: floor((0.10-0)/0.02)
    assert buf.put(b, 0.10)
    assert buf.current(0.10) == 5.0      # B not effective yet: keep executing A, must not return None
    assert buf.current(0.19) == 9.0      # floor(9.5)=9: still A
    assert buf.current(0.20) == 2.0      # takeover point: B takes over at the control boundary
    assert buf.current(0.21) == 2.0      # floor(0.5)=0: point 0 of B


def test_chunk_indexing_uses_floor_not_round():
    # ZOH: values[i] covers [valid_from + i*dt, valid_from + (i+1)*dt), left-closed right-open
    buf = ActionBuffer(epoch=1)
    a = Action(values=tuple(float(i) for i in range(CHUNK_LEN)), dt=DT, horizon=CHUNK_LEN,
               generated_at=0.0, valid_from=0.0, valid_until=0.32,
               state_stamp=0.0, epoch=1, sequence_id=1)
    buf.put(a, 0.0)
    assert buf.current(0.015) == 0.0     # 0.75 beats: round would mis-pick 1, floor stays at 0
    assert buf.current(0.020) == 1.0     # interval boundary: enters the next point

# agent/safety/gate.py
@dataclass(frozen=True)
class SafetyDecision:
    allow: bool
    command: ControlCommand | None   # when allowed, released (possibly after project/rate-limit)
    reason: str                      # on rejection, the reason code for entering the safe state

class SoftwareSafeAction(Enum):
    """Software safe states: defined by the robot itself — an arm holding is safe; a drone landing is."""
    HOLD = "hold"                    # arm: hold the current pose
    BRAKE = "brake"                  # wheeled base: brake
    ZERO_TORQUE = "zero_torque"      # collaborative arm / quadruped: release torque
    SIT = "sit"                      # quadruped: lie down
    LAND = "land"                    # drone: land
    RETURN_HOME = "return_home"
    # note: EMERGENCY_STOP is not here — the e-stop is a hardware authority; see HardwareSafety

class HardwareSafety(Protocol):
    """Hardware authority: the e-stop chain, driver STO, independent limit switches. It does not belong to the software stack —
    it is online when the SafetyGate passes everything, and it still holds the robot when all software is dead."""
    def assert_emergency_stop(self) -> None: ...

class SafetyGate(Protocol):
    """An independent decision layer on the software control chain: no branch can get around it."""
    def evaluate(self, state: StateContract, action: Action,
                 context: dict) -> SafetyDecision: ...
    def enter_safe_state(self, reason: str) -> None: ...

# agent/control/sink.py
class CommandSink(Protocol):
    """The single entry of command authority: Safety-approved commands enter the RobotInterface
    through here. No component in the software stack (including policy and controller)
    holds a direct handle to the RobotInterface."""
    def submit(self, cmd: ControlCommand) -> None: ...

                 ┌─────────────┐
                 │   STARTING  │
                 └──────┬──────┘
                        ↓
                 ┌─────────────┐
                 │   RUNNING   │
                 └──────┬──────┘
                        │
          ┌─────────────┼─────────────┐
          ↓             ↓             ↓
    stale state   policy timeout   controller fault
          │             │             │
          └─────────────┼─────────────┘
                        ↓
                 ┌─────────────┐
                 │  DEGRADED   │
                 └──────┬──────┘
                        │ fault persists
                        ↓
                 ┌─────────────┐   fault persists / relapses   ┌──────────────┐
                 │  SAFE_STOP  │ ───────────────────────────→ │ FAULT_LATCHED │
                 └──────┬──────┘                              └──────┬───────┘
                        │ operator / recovery                       │ manual reset only
                        ↓                                           ↓
                 ┌─────────────┐                              （manual recovery
                 │   RECOVER   │                               no auto-exit）  
                 └─────────────┘

L_total = L_sensor + L_transport + L_sync + L_estimator
        + L_policy + L_queue + L_safety + L_controller  <  L_budget

┌──────────────── Runtime Supervisor ────────────────┐
│ lifecycle · heartbeat · deadline · fault · metrics │
└──┬─────────┬──────────┬─────────┬──────────┬───────┘
 Sensor    Estimator   Policy   ActionBuffer Controller