具身智能的工程架构：一个能跑、能测、能换组件的 Agent 骨架

┌───────────── 训练时栈（离线） ─────────────┐
│ Dataset → Preprocess → Policy/WM → Eval → Artifact │
└──────────────────────┬───────────────────────┘
      共享契约：State / Action Schema · ObsTransform · Artifact Manifest
┌──────────────────────┴───────────────────────┐
│                运行时栈（实时）                │
│ Sensor → Estimator → StateContract → Policy → Action │
│                       ↓ 缓冲 / 插值 / 保持             │
│               Safety Gate → Controller → Robot       │
└──────────────────────────────────────────────┘

# agent/state/obs_transform.py
# 训练时和部署时都从这一个模块 import，禁止各自实现
import numpy as np

class ObsTransform:
    """相机观测的唯一权威预处理，训练/部署共用。"""

    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)      # 双线性，对齐训练配置
        return (img / 127.5 - 1.0).astype(np.float32)  # 与 checkpoint 一致

# deploy/artifact_manifest.yaml —— 复现一次部署所需的完整清单
artifact:
  checkpoint: ckpt/diffusion_v17.pt
  code_commit: 8f3a2c1           # 训练代码的 commit
  state_schema: v3              # StateContract 字段定义版本
  action_schema: v2             # ActionSpace / 坐标系 / dt 约定版本
  obs_transform: v5             # 训练/部署共用的预处理版本
  normalization: stats/norm_v17.npz   # 归一化统计量
  robot_model: calib/ur5e_2026-08.yaml  # 机器人标定
runtime:
  container_digest: sha256:71c0...  # 推理镜像 digest，不是 tag
  python: "3.11"
  pytorch: "2.4"
  cuda: "12.4"
compat:                        # 这份 ckpt 兼容的 schema 范围，启动时校验
  state_schema: [v3, v4]
  action_schema: [v2]

embodied_agent/
├── agent/
│   ├── core/            # loop、时钟、生命周期管理
│   ├── perception/      # 第 1 层：Sensor 驱动（read → RawObservation）
│   ├── state/           # 第 2 层：StateContract + StateEstimator
│   ├── policy/          # 第 3 层：Protocol + 各路线实现
│   ├── action/          # 第 4 层：tokenizer / chunking / safety gate
│   ├── control/         # 第 5 层：机器人 SDK 适配
│   └── safety/          # watchdog、限幅、安全位姿
├── train/
│   ├── data/            # episode 格式、dataset、replay
│   ├── sim/             # 仿真后端（与真机同一 RobotInterface）
│   └── learn/           # 训练入口、loss、checkpoint 导出
├── configs/             # 每机器人/每任务一份 yaml
├── tests/               # 分层测试（见下文）
└── deploy/              # 导出 artifact、版本清单

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

class Hypothesis(Enum):
    """多模态 estimator 可能给出多个假设，禁止无声折叠（呼应 9/15 Failure 1）。"""
    SINGLE = "single"
    MULTI = "multi"          # 需要 policy 侧做 mode_select

@dataclass(frozen=True)
class Provenance:
    source: str              # 哪个传感器/estimator 产出
    stamp: float             # 物理世界采样时刻（与 monotonic() 同时钟域）
    frame_id: str            # 坐标系

@dataclass(frozen=True)
class ObservationField:
    """一路观测的 schema 化封装：值之外，语义元数据齐全。"""
    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:
    """开放式多模态容器：加传感器 = 加 field，不动 contract。"""
    fields: tuple            # tuple[ObservationField, ...]

    def get(self, name: str) -> ObservationField | None:
        for f in self.fields:
            if f.name == name:
                return f
        return None          # 传感器缺席是正常情况，policy 必须能处理

@dataclass(frozen=True)
class ProprioState:
    """本体感受：字段名是 contract，顺序不是；单位也是 contract。
    约定 position=rad、velocity=rad/s、effort=Nm，换单位走 schema 版本，不走口头通知。"""
    joint_names: tuple       # ("shoulder_pan", "shoulder_lift", ...)
    position: tuple
    velocity: tuple
    effort: tuple | None
    stamp: float

@dataclass(frozen=True)
class StateQuality:
    """状态可用性，不是概率。分来源给，Safety 按任务定义 gate。"""
    proprio: float
    vision: float
    localization: float
    temporal: float          # 多传感器对齐质量（见下文）

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

@dataclass(frozen=True)
class StateContract:
    """policy 唯一允许消费的状态对象（呼应 9/15：policy 是 contract consumer）。"""
    observation: Observation
    proprio: ProprioState
    task: object                       # 指令/目标表征（MPC 等无语言输入的可为 None）
    hypothesis: Hypothesis
    provenance: Provenance
    validity_sec: float                # 数据有效期（过期必须 gate 掉）
    quality: StateQuality
    schema_version: str                # 字段定义版本，启动时与 artifact manifest 的
                                       # state_schema 校验：对象要知道自己是哪一版

    def age_sec(self, now: float) -> float:
        """数据年龄 = now - 物理采样时刻。estimator 算了 100ms，年龄就长 100ms。
        now 由调用方显式传入：contract 内部不读 wall clock（见下文的 Clock Protocol）。"""
        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):
    """全系统唯一的时间来源。业务代码里散读 time.monotonic() 的位置，
    每一个都是测试里无法冻结的时间裂缝。"""
    def monotonic(self) -> float: ...

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

@dataclass(frozen=True)
class RawObservation:
    """单路传感器的原始读数。"""
    sensor_id: str
    sensor_timestamp: float    # 硬件采样时刻（注明属于哪个 clock domain）
    receive_timestamp: float   # 进入软件的时刻
    sequence_id: int
    clock_domain: str          # "monotonic" / "ros_wall" / ...
    calibration_version: str
    payload: object

原始传感器时间戳（各自 clock domain）
    ↓ clock normalization：统一换算进 monotonic 域，记录换算版本
带统一时间戳的样本缓冲
    ↓ temporal alignment：按窗口对齐、插值/外推、超时丢弃
StateContract（provenance.stamp 已在 monotonic 域）

# agent/perception/sensor_buffer.py、agent/state/estimator.py
@dataclass(frozen=True)
class SensorSnapshot:
    """各源在 now 时刻的最新样本集合：estimator 做时间对齐的输入，不是单路读数。"""
    samples: tuple          # tuple[RawObservation, ...]，每源至多一条
    window_sec: float       # 对齐窗口：早于 now - window 的样本不允许参与对齐

class SensorBuffer(Protocol):
    """第 1 层：驱动线程持续把带时间戳的样本写进环形缓冲，消费方非阻塞取最新。
    生产形态绝不是 blocking read()——runtime 线程没有资格阻塞在传感器上。"""
    def latest(self, now: float) -> SensorSnapshot: ...

class StateEstimator(Protocol):
    """第 2 层：系统里唯一允许产出 StateContract 的地方。
    职责：时钟域归一、时间对齐、插值/外推、drop policy、最大同步窗口。"""
    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"   # 末端位姿增量
    # ...

@dataclass(frozen=True)
class ActionSchema:
    """空间语义与单位：space 定语义，representation 定编码，单位与坐标系是 contract。"""
    space: ActionSpace
    representation: str         # 编码约定：joint_rad / ee_se3_log / ee_6d_rot / ...
    translation_unit: str|None  # 平移单位（关节动作为 None）；EE 动作必须显式
    rotation_unit: str|None     # 旋转表示：se3_log / quat / euler_xyz / 6d ...
    frame_id: str               # 坐标系：joint / base / ee ...

@dataclass(frozen=True)
class Action:
    """策略输出不是一坨裸 tensor：语义、坐标系、时间口径都写在对象里。"""
    values: object        # 动作值：chunk (H, D) 或单步 (D,)
    schema: ActionSchema  # 空间语义、编码、单位、坐标系
    schema_version: str   # schema 指纹：consumer 侧据此 fail-before-motion
    dt: float             # 相邻动作点的时间间隔
    horizon: int          # chunk 长度，单步动作为 1

    generated_at: float   # 推理完成、提交给第 4 层的时刻
    valid_from: float     # 生效起点
    valid_until: float    # 失效时刻：超过即 stale，不允许再被消费
    state_stamp: float    # 决策所依据状态的物理采样时刻
    epoch: int            # 生命周期身份：属于哪一次 episode / runtime epoch
    sequence_id: int      # epoch 内决策序号：异步提交不许乱序（见调度协议）

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

class Policy(Protocol):
    """第 3 层的接缝。VLA / Diffusion / MPC 都只是它的实现。"""

    def reset(self, epoch: int, alloc: "SequenceAllocator") -> None:
        """episode 边界：清空 KV cache / 隐藏状态 / 规划缓存，
        同时领取本 epoch 的身份与决策序号——(epoch, sequence_id) 才是决策的真实身份。
        全局 next_seq() 这类进程级可变状态会污染回放与 reset，禁止使用。"""
        ...

    def act(self, state: StateContract, now: float) -> Action:
        """给定状态与世界时刻，输出一个动作（通常是一个动作 chunk）。now 由 Clock 显式传入。"""
        ...

# agent/policy/vla.py（示意，框架细节从略）
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     # KV cache 是 epoch 的从属物，随 reset 一并清零

    @torch.no_grad()
    def act(self, state: StateContract, now: float) -> Action:
        image = state.observation.get("image")   # -> ObservationField | None，缺席要会降级
        ids = self.model.generate(
            image=None if image is None else image.value,
            instruction=state.task,
            past=self._past,
        )
        # schematic：KV cache 能否跨帧复用取决于模型架构
        # （visual token 是否逐帧替换、位置编码、causal attention 结构），不能默认
        self._past = ids.kv_cache
        return self.tok.decode(ids.action_bins)  # -> Action(chunk)，第 4 层负责按时序放出

# agent/policy/mpc.py（世界模型在环，呼应"混合架构"趋势）
class MPCPolicy:
    """不学显式策略，用世界模型 rollout 挑动作——TD-MPC 路线的运行时形态。"""

    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  # 无跨帧状态：每帧从当前观测重新编码（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：把 dynamics rollout / cost / constraint / uncertainty
        # 压缩进 estimate_return() 只为突出 Policy 接缝；真实 MPC 不该把这些语义
        # 藏成一个无约束的 scalar scorer
        scores = [self.wm.estimate_return(z, c) for c in cands]
        best = cands[int(np.argmax(scores))]
        u0 = best[0]  # 只执行第一步，下一帧重新规划（receding horizon）
        return Action(values=u0, horizon=1, ...)   # Policy.act 的返回类型是 Action，不是裸控制向量

# agent/policy/diffusion.py（示意：省略了 timestep / noise schedule / CFG 等采样细节）
class DiffusionPolicy:
    """生成式策略：多步去噪出一个动作 chunk，天然输出多假设（呼应 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  # 无状态，或按实现清空条件队列

    def act(self, state: StateContract) -> Action:
        x = torch.randn(1, CHUNK_LEN, ACTION_DIM)   # 去噪起点是噪声，终点不是
        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])   # 模型输出 -> 反归一化 -> 动作值
        now = self.clock.monotonic()      # 唯一时间来源：不散读 time.monotonic()
        return Action(values=values,
                      schema=ActionSchema(space=ActionSpace.JOINT_POS,
                                          representation="joint_rad",
                                          translation_unit=None,
                                          rotation_unit=None,   # 关节空间没有旋转单位：它属于 SE(3) 动作
                                          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())  # chunk 交给第 4 层按时序放出

ActionBuffer semantics（active + scheduled 双槽，epoch 屏障）:
- 每个 epoch 一份序号空间：put() 只接受本 epoch 且 sequence_id > last_accepted 的 chunk，
  旧 epoch / 乱序晚到一律拒绝——reset 之后，迟到的旧决策永远失去覆盖权
- stale chunk（valid_until 已过）: 丢弃，绝不沿用旧决策
- future chunk（valid_from 在未来）: 进 scheduled 槽暂存，到 valid_from 在控制边界接管；
  期间只有 sequence_id 更大的 chunk 可以替换它
- 已生效 chunk: sequence_id 更大且 valid_from 已到的后来者立即接管 active 槽
- 接管只发生在控制边界（control boundary），不在控制拍中间切换；
  A 执行中 B 未到生效点，继续执行 A——不允许出现执行空洞
- missing chunk（无可用决策）: 进入 safe state，而不是静默沿用
- action discontinuity（A→B 跳变过大）: project / rate-limit 后再放行

# agent/control/robot.py、train/sim/task_env.py
class RobotInterface(Protocol):
    """真机与仿真后端实现同一个接口：只会观察、发送、复位。"""
    def observe(self) -> RawObservation: ...
    def send(self, cmd: ControlCommand) -> None: ...
    def reset_robot(self) -> None: ...

class TaskEnv(Protocol):
    """任务定义：把 reward / done / 成功判据从硬件里剥出来。"""
    def reset(self) -> StateContract: ...
    def step(self, action: Action) -> Transition: ...   # Transition 里才有 reward/done

class SimBackend:
    """MuJoCo / Isaac 适配 RobotInterface。proprio 字段名与真机驱动严格一致。"""
    ...

class RealRobot:
    """真机适配 RobotInterface。所有阻塞 IO 移到后台线程，observe() 只取最新。"""
    ...

class PickPlaceTask:
    """TaskEnv 的一种：包着一个 RobotInterface 或 SimBackend，负责评 reward。"""
    ...

# agent/core/loop.py（schematic）
CONTROL_DT = 1.0 / 200     # 命令更新节拍 200 Hz；policy 线程按模型自身节奏跑
POLICY_DT = 1.0 / 20       # policy 产出的上限节拍：慢由模型决定，快不许越过它
MIN_QUALITY = 0.5          # 状态可用性下限：低于此值不产新决策

class AgentLoop:
    def __init__(self, sensor, estimator, policy, state_buffer,
                 action_buffer, safety, sink, clock):
        self.sensor = sensor                # Sensor Protocol（第 1 层）
        self.estimator = estimator          # StateEstimator Protocol（第 2 层）
        self.policy = policy                # Policy Protocol（第 3 层）
        self.state_buffer = state_buffer    # 第 2 层出口：状态寄存器
        self.action_buffer = action_buffer  # 第 4 层：chunk 寄存器 + 调度协议
        self.safety = safety                # 独立安全层（SafetyGate）
        self.sink = sink                    # 唯一出口：ApprovedCommandSink
        self.clock = clock                  # Clock Protocol：不散读 time.monotonic()

    def _estimator_thread(self):
        while self._running:
            now = self.clock.monotonic()
            snapshot = self.sensor.latest(now)   # 非阻塞：取各源最新样本集
            state = self.estimator.estimate(snapshot, now)
            self.state_buffer.publish(state)     # 唯一写者；消费方各自取最新且新鲜

    def _policy_thread(self):
        while self._running:
            now = self.clock.monotonic()
            state = self.state_buffer.latest_valid(now)  # 与数据生产解耦
            if state is None or state.quality.min() < MIN_QUALITY:
                self.safety.enter_safe_state("stale_state")   # 数据过期/质量不足
                self._sleep_until(now + POLICY_DT)   # 错误路径也守节拍，不许忙等
                continue
            action = self.policy.act(state, now)   # 允许 100ms+，不背控制的 deadline
            self.action_buffer.put(action, now)    # chunk 交给第 4 层按调度协议放出

    def _control_thread(self):
        next_tick = self.clock.monotonic()
        while self._running:
            now = self.clock.monotonic()
            state = self.state_buffer.latest_valid(now)   # 控制侧独立再判新鲜度
            if state is None:
                self.safety.enter_safe_state("stale_state")
            else:
                cmd = self.action_buffer.current(now)  # 无可用决策则返回 None（协议已定）
                if cmd is None:
                    self.safety.enter_safe_state("no_action")
                else:
                    decision = self.safety.evaluate(state=state, action=cmd,
                                                    context={"now": now})  # 生产形态，见下节
                    if decision.allow:
                        self.sink.submit(decision.command)  # 唯一出口：批准的命令才进 RobotInterface
                    else:
                        self.safety.enter_safe_state(decision.reason)
            next_tick += CONTROL_DT      # 节拍推进与单次耗时解耦，不漂移
            if now > next_tick:
                self.safety.record_deadline_miss(now - next_tick)  # 违约记账，按 miss policy 处置
                next_tick = now          # 重对齐：skip tick / degraded / safe stop
            self._sleep_until(next_tick)

    def _sleep_until(self, deadline):
        # monotonic 时钟对齐节拍；超时时记录 jitter 指标而非静默吞掉
        ...

# tests/fakes.py —— 纯 stdlib 的假实现，pytest 直接跑
# 契约字段与 agent/state/contract.py、agent/action/contract.py 同形；
# 为保持最小，quality / SafeState 等字段从略，只保留本组测试要钉住的接缝。
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                 # 物理采样时刻（与注入时钟同时钟域）
    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:
    """决策序号分配器：每个 runtime epoch 一份，epoch 内单调、reset 即清零。
    全局 next_seq() 会污染 replay 与 reset——(epoch, sequence_id) 才是决策的真实身份。"""

    def __init__(self):
        self._next = 0

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


@dataclass(frozen=True)
class Action:
    values: Tuple[float, ...]    # 动作 chunk（CHUNK_LEN,）
    dt: float
    horizon: int
    generated_at: float          # 推理完成、提交给第 4 层的时刻
    valid_from: float
    valid_until: float           # 失效时刻：超过即 stale，不允许再被消费
    state_stamp: float           # 决策所依据状态的物理采样时刻
    epoch: int                   # 生命周期身份：属于哪一次 episode / runtime epoch
    sequence_id: int             # 决策序号：epoch 内单调，异步提交不许乱序


class FakeClock:
    """测试注入的时钟（生产对应 SystemClock；二者实现同一个 Clock Protocol）。"""

    def __init__(self):
        self.t = 0.0

    def monotonic(self):
        return self.t

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


class FakeSensor:
    """第 1 层 buffer 的最小形态：latest(now) 只取最新样本，绝不阻塞调用方。"""

    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):
    """模拟延迟 0.2s 的传感器：数据年龄超过 validity 时应触发 safe state。"""

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


class FailingSensor(FakeSensor):
    """模拟 fail_at 时刻死掉的传感器：stamp 冻结在死前最后一帧，数据年龄持续增长。"""

    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:
    """跨线程接缝的最小形态：estimator 发布最新状态，policy / 控制两侧各自取'最新且仍新鲜'的。

    两个消费侧各自做 freshness 判定：policy 侧的 gate 只保证'进入决策的状态是新鲜的'；
    控制侧必须重新判定——决策所依据的状态到执行时可能已经过期。
    """

    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        # 过期/缺失：交给 Safety 进入 safe state，绝不静默沿用
        return s


class SinePolicy:
    """deterministic policy：输出只由状态决定，episode 回放可逐位一致。"""

    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):
    """推理延迟注入：决策在发起时刻看见状态，latency 秒后才提交生效——
    生效前的空窗正是'动作覆盖率'缺口的来源。"""

    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:
    """第 4 层最小形态：active + scheduled 两槽 + epoch 屏障。
    latest-wins 的准确语义：future chunk 暂存到点接管，绝不制造执行空洞。"""

    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 钩子：accepted/activated/preempted/expired/rejected_*

    def sync_epoch(self, epoch):
        """reset_episode 的屏障：旧 epoch 的决策立即作废，序号重新起算。"""
        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:
        """只接受本 epoch 且更新的决策；旧 epoch / 乱序晚到直接拒。"""
        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            # 立即生效：latest-wins
            self._active_sequence = action.sequence_id
            self._scheduled = None
            self.events.append(("activated", action.sequence_id))
        else:
            self._scheduled = action         # future：暂存，到 valid_from 在控制边界接管
        return True

    def current(self, now: float) -> Optional[float]:
        """按时刻取 chunk 点；scheduled 到点提升，过期作废，绝不沿用。"""
        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              # 过期决策作废，不沿用
            return None
        idx = min(int((now - a.valid_from) / a.dt), a.horizon - 1)   # ZOH：floor，不是 round
        return a.values[idx]


class SafetyLimiter:
    """最小安全层：速度上限（物理量，rad/s）+ safe-state 入口记录。
    限幅基于实测位置，不是上一拍 command：发了 1.0 不代表机器人到了 1.0。"""

    def __init__(self, max_velocity=5.0, dt=DT):
        self.max_velocity = max_velocity     # 限的是速度，不是位置
        self._dt = dt
        self.safe_entries = []

    def check(self, state, cmd):
        pos, = cmd
        measured, = state.proprio            # 速度是否合法要对实测算，不是对 last command
        max_delta = self.max_velocity * self._dt   # 速度上限换算成本拍位置增量
        delta = max(-max_delta, min(max_delta, pos - measured))
        return (measured + delta,)           # 位置合法 ≠ 速度合法

    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:
    """测试版 CommandSink：唯一允许调用 controller.send() 的对象。
    RuntimeCore 只拿得到 sink——绕过 SafetyLimiter 直写 controller 在结构上不可能，
    与生产同一条 authority 约束（见正文安全门一节）。"""

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

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


class RuntimeCore:
    """与生产 loop 同一个循环体：两个 tick 由调度器驱动，而不是由线程驱动。
    epoch 由 core 拥有：reset_episode 递增并同步给 policy 与 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   # 唯一出口：批准的命令经 CommandSink 进 controller
        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)
        # 生产还要重置：estimator 滤波器、StateBuffer、controller tracking、种子

    def prime(self, now):
        """episode 起点先把状态缓冲灌起来，policy 首拍才有状态可消费。"""
        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 侧 freshness gate
            return
        self.action_buffer.put(self.policy.act(state, now), now)

    def control_tick(self, now):
        # 生产里 estimator 跑在自己的高频 loop 里向 StateBuffer 发布；
        # 最小 loop 把它折进控制节拍，二者对 latest_valid 的判据完全一致。
        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")  # 控制侧 freshness gate，独立于 policy 侧
            return
        point = self.action_buffer.current(now)
        if point is None:
            self.safety.enter_safe_state("no_action")   # missing：不静默沿用
            return
        self.sink.submit(self.safety.check(state, (point,)))   # 唯一出口：approved command 经 CommandSink


def run_episode(clock, n_steps, hz=50, sensor=None, policy=None, policy_every=5):
    """FakeScheduler：policy 节奏用 policy_every 拍近似，时钟注入。"""
    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):
    """构造一个实测关节位置为 angle 的状态：限速测试的输入。"""
    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 + 注入时钟 => 逐位一致


def test_stale_state_enters_safe_state():
    clock = FakeClock()
    _, safety = run_episode(clock, n_steps=10, sensor=DelayedSensor(clock),
                            policy_every=1)
    # 双 gate 各自记账：policy 侧 freshness gate 触发 10 次（从不发布），
    # 控制侧 latest_valid 触发 10 次（寄存器为空）；stale 优先于 missing 报告
    assert safety.safe_entries.count("stale_state") == 20
    assert safety.safe_entries.count("no_action") == 0


def test_safety_limits_against_measured_state():
    # 速度限幅必须对实测位置做，不是对上一拍 command：
    # 发了 1.0 不代表机器人到了 1.0——基于 command 算 Δq 会系统性低估真实速度
    safety = SafetyLimiter(max_velocity=5.0, dt=DT)   # 5 rad/s @ 50 Hz => 单拍上限 0.1 rad
    assert safety.check(_state(0.7), (5.0,)) == pytest.approx((0.8,))    # 目标 5.0：Δq=4.3，截到 0.7+0.1
    assert safety.check(_state(0.75), (5.0,)) == pytest.approx((0.85,))  # 实测只走到 0.75：重新对实测限幅


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)     # 模拟 request #42 比 #41 先完成
    assert buf.put(newer, 0.0)
    assert not buf.put(older, 0.0)                # 旧决策晚到，必须被拒
    assert buf.current(0.0) == newer.values[0]


def test_stale_state_cannot_execute_buffered_action():
    # t=0.0 状态新鲜，policy 产出 A（valid_until=0.32）；t=0.1 传感器死掉。
    # t=0.16~0.30 期间 A 仍未过期，但状态已 stale——控制侧必须拒绝执行 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：状态新鲜，正常执行
    # t=0.16~0.38：状态 stale，A 虽在缓冲里未过期也绝不执行
    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():
    # 真·延迟注入：决策在 t 发起、t+150ms 才生效。控制节拍不被推理拖慢，
    # 但生效前的空窗里没有可执行决策——coverage 缺口显式落成 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                # 前 8 拍是缺口：150ms 延迟里没有决策
    assert safety.safe_entries.count("no_action") == 8
    assert ctrl.sent                            # 推理完成后控制环恢复执行


def test_epoch_barrier_rejects_stale_episode_action():
    # Episode 41 的迟到决策（seq 更大）绝不能覆盖 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)          # episode 41 的第 2 号决策，异步晚到
    buf.sync_epoch(2)                    # reset_episode：episode 42 开始
    p42 = SinePolicy(); p42.reset(epoch=2)
    fresh = p42.act(state, 0.0)          # episode 42 的第 1 号决策
    assert buf.put(fresh, 0.0)
    assert not buf.put(late, 0.0)        # 旧 epoch 晚到：即使 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():
    # A 执行中，B（seq 更大）提前到达但 valid_from 在未来：
    # 0.10~0.20 必须继续执行 A，到 0.20 由 B 在控制边界接管——中间不允许出现空洞
    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      # A 的第 5 点：floor((0.10-0)/0.02)
    assert buf.put(b, 0.10)
    assert buf.current(0.10) == 5.0      # B 还没生效：继续 A，不得返回 None
    assert buf.current(0.19) == 9.0      # floor(9.5)=9：仍是 A
    assert buf.current(0.20) == 2.0      # 到点：B 接管控制边界
    assert buf.current(0.21) == 2.0      # floor(0.5)=0：B 的第 0 点


def test_chunk_indexing_uses_floor_not_round():
    # ZOH：values[i] 覆盖 [valid_from + i*dt, valid_from + (i+1)*dt)，左闭右开
    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 拍：round 会错取 1，floor 留在 0
    assert buf.current(0.020) == 1.0     # 区间边界：进入下一点

# agent/safety/gate.py
@dataclass(frozen=True)
class SafetyDecision:
    allow: bool
    command: ControlCommand | None   # allow 时放行（可能经 project/rate-limit）
    reason: str                      # 拒绝时进 safe state 的原因码

class SoftwareSafeAction(Enum):
    """软件安全态：由机器人自己定义——机械臂 hold 是安全，无人机 land 才是。"""
    HOLD = "hold"                    # 机械臂：保持当前位姿
    BRAKE = "brake"                  # 轮式底盘：制动
    ZERO_TORQUE = "zero_torque"      # 协作臂/四足：卸力
    SIT = "sit"                      # 四足：趴下
    LAND = "land"                    # 无人机：降落
    RETURN_HOME = "return_home"
    # 注意：EMERGENCY_STOP 不在这里——急停是硬件 authority，见 HardwareSafety

class HardwareSafety(Protocol):
    """硬件 authority：急停链、驱动器 STO、独立限位。它不属于软件栈——
    SafetyGate 全体通过时它也在线，软件全挂时它仍然兜得住机器人。"""
    def assert_emergency_stop(self) -> None: ...

class SafetyGate(Protocol):
    """软件控制链上独立的判定层：任何分支都绕不过去。"""
    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):
    """command authority 的唯一入口：Safety 批准的命令从这里进入 RobotInterface。
    软件栈里没有任何组件（包括 policy 与 controller）持有 RobotInterface 的直连句柄。"""
    def submit(self, cmd: ControlCommand) -> None: ...

                 ┌─────────────┐
                 │   STARTING  │
                 └──────┬──────┘
                        ↓
                 ┌─────────────┐
                 │   RUNNING   │
                 └──────┬──────┘
                        │
          ┌─────────────┼─────────────┐
          ↓             ↓             ↓
    stale state   policy timeout   controller fault
          │             │             │
          └─────────────┼─────────────┘
                        ↓
                 ┌─────────────┐
                 │  DEGRADED   │
                 └──────┬──────┘
                        │ fault persists
                        ↓
                 ┌─────────────┐   fault persists / 反复未愈   ┌──────────────┐
                 │  SAFE_STOP  │ ───────────────────────────→ │ FAULT_LATCHED │
                 └──────┬──────┘                              └──────┬───────┘
                        │ operator / recovery                       │ manual reset only
                        ↓                                           ↓
                 ┌─────────────┐                              （人工介入，
                 │   RECOVER   │                               不允许自动退出）
                 └─────────────┘

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