从一只会摇摆的鸭子，
理解真实世界的机器人。

# safe_policy_loop.py — 教学用最小骨架，不直接连接高扭矩舵机
import time
import numpy as np
import onnxruntime as ort

HZ, DT = 50, 1.0 / 50
MAX_STEP = np.deg2rad(2.0)  # 首次调试应更小

class MockBus:
    def read_state(self):
        return np.zeros(14), np.zeros(14), np.zeros(3)
    def write_targets(self, q):
        print("targets", np.round(q[:4], 3), "…")

bus = MockBus()
policy = ort.InferenceSession("walk.onnx", providers=["CPUExecutionProvider"])
last_action = np.zeros(14, dtype=np.float32)

while True:
    tick = time.perf_counter()
    q, dq, gyro = bus.read_state()
    command = np.zeros(13, dtype=np.float32)
    obs = np.concatenate([gyro, [0, 0, -1], q, dq, last_action, command])
    raw = policy.run(None, {policy.get_inputs()[0].name: obs[None].astype(np.float32)})[0][0]
    target = q + np.clip(raw - q, -MAX_STEP, MAX_STEP)
    bus.write_targets(target)  # 实机前增加急停、限位、掉线看门狗
    last_action = raw
    time.sleep(max(0, DT - (time.perf_counter() - tick)))