Package SG90 self-righting pipeline for review

This commit is contained in:
ming01.wang 2026-11-13 17:06:09 +08:00
commit 5a45862f35
2312 changed files with 4760693 additions and 0 deletions

10
.gitignore vendored Normal file
View File

@ -0,0 +1,10 @@
__pycache__/
*.pyc
.DS_Store
.venv/
build/
transport/
*.pem
*.key
.env
.env.*

68
README.md Normal file
View File

@ -0,0 +1,68 @@
# SG90 双轴不倒翁:设计、仿真、实机反馈
两只 **360° 连续旋转 SG90**、一个底板 IMU(QMI8658C)和外置 RDK X5,实现倒地后的自主起身。这里汇总机械设计、MuJoCo 环境、动作搜索、策略训练、校准网页、板端控制和失败证据,供协作者复现并优化。
**这是尚未完成可靠全向起身的研究项目。** 冻结的 v3 主模型在严格诊断仿真中为 **55/963(5.71%)**;实机成功记录使用过模型与分段撑地的组合,不能当作统一纯模型成功。当前物理参数族未通过实机轨迹校准。最新加高 CAD 也尚未同步进入训练几何。
![最新 CAD 设计预览](hardware_design/selfrising_sg90_large_plate_20261111/fit_v4_taller_rounded_20261113/output/cad_assembly.png)
## 建议阅读顺序
1. [完整 pipeline](docs/PIPELINE.md):每一步的输入、入口、输出和未解决问题。
2. [优化交接说明](docs/OPTIMIZATION_HANDOFF.md):协作者应该先改什么,如何证明有效。
3. [结果与版本对照](docs/RESULTS.md):区分旧位置策略、速度策略、v3、纯模型与辅助控制。
4. [离线复现](docs/REPRODUCING.md):先跑一个成功样本和一个失败样本,再进入训练。
5. [硬件与部署](docs/HARDWARE.md):连续旋转控制接口、校准和代码导航。
6. [来源、收录范围与权利说明](docs/PROVENANCE.md)。
```mermaid
flowchart LR
A[需求与机械 CAD] --> B[打印与实物装配]
B --> C[IMU / 舵机标定]
C --> D[仿真与实机轨迹对齐]
D --> E[动作搜索 / 教师数据]
E --> F[BC / PPO 策略]
F --> G[冻结权重 / 独立测试]
G --> H[X5 离线推理与部署]
H --> I[实机记录 / 失败分析]
I --> D
```
## 五分钟开始审阅
Python **3.10+** 可恢复并校验封存文件;逐轨迹精确复现要求原记录的 **Python 3.10.20** 与依赖版本。
```sh
python3 scripts/materialize_reproduction.py --out build/v3
python3 -B build/v3/verify_package.py
python3.10 -m venv .venv
. .venv/bin/activate
python -m pip install -r requirements.txt
python -B build/v3/infer.py --observation build/v3/examples/first_observation.json
OMP_NUM_THREADS=1 OPENBLAS_NUM_THREADS=1 python -B build/v3/run_demo.py \
--sample build/v3/samples/primary_failure.json --out build/failure_replay
```
四个原始测试 JSON 各约 300 MB,仓库中用 gzip 无损保存。恢复后约需 1.3 GB 空间;原文件清单、逐文件 SHA-256 和运行代码保持封存字节。上述命令只操作本地仿真,不连接板卡。
## 目录
| 目录 | 用途 |
|---|---|
| `hardware_design/` | CAD 参数代码、STEP、STL、装配及转动审查,含 v4 加高版 |
| `training/continuous_sg90_v3_20261113/` | v3 环境、参数诊断、动作搜索、BC/PPO、测试及数据 |
| `training/sg90_360_handoff/` | +Y 局部成功与跨摆法失败对照 |
| `training/continuous_sg90_20261112/` | 早期速度策略来源与环境 |
| `calibration/` | IMU 校准网页、状态与点动接口、假硬件测试 |
| `deployment/` | X5 历史控制代码、旧权重、运行限制与六轮实机收据 |
| `reproduction/` | 完整 v3 冻结复现包,含权重、原始测试轨迹、教师数据和 8 段回放 |
| `delivery/` | 较早位置/速度策略的精简审阅快照,完整 v3 从 reproduction 恢复 |
| `docs/` | 交接、复现与结果口径 |
原路径层次保留,方便查明调用关系与证据来源;本仓库没有重新发明一套训练框架。主模型的 SHA-256 为 `200a2f30635f21836e0b7b807238e4cbfcd9cc7dda4ff238b8a20c9560a96839`;板端历史旧模型为 `7626bbef2099172970966085bcd8d08d85e304113c3f120b323834e348c74e0f`,两者用途不同。
## 开始优化
提交改动时说明:问题与假设、改动文件、冻结权重、每个实际倒向的有效/排除/失败数,以及同种子基线对照。146M、147M 等已揭示测试集合不能继续称为未见测试集。不要把教师挑出的成功动作或训练窗口分数当作策略成功率。
本次整理不启动真机、不重训,也不改变既有控制限制。校准精度、最新 CAD 的真实装配和新权重实机起身仍待验证。

11490
SOURCE_MANIFEST.json Normal file

File diff suppressed because it is too large Load Diff

112
calibration/README.md Normal file
View File

@ -0,0 +1,112 @@
# X5 双舵机实机标定工作台
## 当前:SG90 实测校准与单次爬起工作台
入口仍为 <http://127.0.0.1:8767/>,从仓库根运行 `python3 -m calibration.imu_server --port 8767`。此页使用 `/home/sunrise/x5_imu_calibration_20261112/v2/motion_cli.py`;未调用旧16输入恢复驱动,也未复用旧MG90S/竖直yaw标定。页面加载、刷新和相机开启不会输出PWM,动作须用户主动点击并完成现场确认。
已完成的三姿态、候选及板端保存回执持久化到 `imu_data_20261112/session.json`,重启恢复且不重放旧任务。动作请求、前后图及现场观察分别留存;运行中重启会将结果标未知,板端仍有租约/超时保护。维护升级前必须等待任务空闲并从 `/api/imu/export` 保存最新完整会话,不能丢掉用户已做的校准。
按网页依次完成:
1. IMU已固定并校准后,用户确认平放静止且在现场观察,主动运行一次无PWM前置验证。回执绑定本次模型/安装/运行版本;旧逐次读取曾出现丢帧,不将旧失败或新技术诊断冒充用户验收。是否通过以当前板端回执为准。
2. 先卸下舵盘和相连负载,仅对固定下轴32或上轴33做一次1500→1450/1550→1500微秒点测。首次1500也可能明显转动,不会自动归中。结束停PWM后,保持轴不转,按CAD装回机械零位;操作者用量角器确认后记录1500μs=0°。这是主动现场确认,不是预填的测量值。
3. 装回后每次只选已有实测点旁边±50μs。上轴正角使原竖直杆向body+X偏;下轴正角使杆向body−Y偏,测一轴时另一轴保持零位。方向按钮只保存观察,不能替代实测角。每次目标保持时使用与关节转动平面一致的量角器读数;前图只记录准备情境,不自动验证角度,真实phase/after图另行留存。
4. 板端将每份人工量角绑定到真实、清理确认的jog任务,保存增量历史。两轴实测范围均覆盖−60°…+60°后,由实测点内插生成SG90 profile,范围外不外推。行程不足或碰撞应停止;不能为了达到60°而顶死。校准不完整时爬起被阻断,有限点动仍可完成校准。
5. 保持独立舵机供电并共地,将机构轻放倒、静止,不从高处抛摔。新前图、当前现场确认和板端门禁都通过后,用户点击一次爬起;3秒倒计时可取消,板端动作最长8秒,不循环、不自动重试。
本地复用原 `Bridge` 首浏览器心跳、互斥、2.5秒续租、25秒传输超时、板端停止和清理流程;增加两个覆写点以绑定新协议,旧入口默认行为不变。运动时700ms浏览器保活;后台、画面丢失、观察取消、停止或超时触发停机。前图15秒有效且不可复用;相机不可用时允许用户主动上传确为当前的照片并持续现场观察,标记接收时间而不伪造拍摄时间。停止PWM不等于切断舵机电源。
页面显示真实IMU、下发目标、推理、阶段和清理回读。`imu_upright_candidate` 只显示“姿态达标,请现场确认是否站稳”;仅用户主动记录底板着地并站稳后标为用户观察确认,不自动声称实机成功率。91.69%为既有仿真数据,非实机保证。
验证:`python3 -m unittest calibration.test_imu calibration.test_imu_motion calibration.test_backend calibration.test_server`,以及 `node calibration/test_imu_motion_frontend.cjs`、`node calibration/test_imu_frontend.cjs`、`node calibration/test_frontend.cjs`。替身测试不输出PWM;真正的点动/量角/爬起留给现场用户主动执行。
## 2026-11-12:独立 IMU 安装校准页
以下为此前无PWM版本的记录;当前功能与安全门禁以本文件上节和当前页面为准。
本次入口:<http://127.0.0.1:8767/>,启动命令:
```sh
python3 -m calibration.imu_server --port 8767
```
此入口不加载旧动作页面、不调用旧 `Bridge.prepare/start`,只复用现有受限 SSH 传输连接本次部署的 `board_tool.py`。没有舵机/PWM API。加载页面只读状态;用户主动开始的 8 秒窗口才采集六轴,网页显示约 20 Hz,传感器配置为 224.2 Hz、±8g、±1024°/s,结束逐位恢复并回读列出的可读配置。同步锁存需操作 AHB 时钟门控;门控原始隐藏状态不可读,退出时通过 CAL1_L=0 显式重新启用,因此 `hidden_state_restored=false`,不声称恢复全部硬件状态。结果显示实际接收频率、传感器计数跨度、丢帧和恢复范围。窗口结束的数值明确标记为历史样本。
按页面依次静置:顶面 +Z 朝上、+X 端竖直朝上、+Y 端竖直朝上,手托底板而非长杆。三份数据均通过静止筛查后拟合右手正交 `sensor_to_body`;姿态彼此不近似正交、符号反了或拟合误差超过15°时拒绝候选。仅平放估计陀螺仪零偏,保留1g;未进行完整六面加速度标定。详见 [安装和四线接法](../deployment/IMU_INSTALL_X5_20261112.md)。
候选先保存在本机 `calibration/imu_data_20261112/`,不会自动套用旧安装参数。用户勾选并点击保存后,板端基于三份原始证据 SHA、实采参考数据和模型身份重新验证,写入新 `/home/sunrise/x5_imu_calibration_20261112/calibration_profile.json`;本目录旧 profile 若有则先备份,20261109 旧目录保持不变。`calibration_candidate_sha256` 标识校准内容,`model_candidate_sha256` 标识冻结模型入口,两者不同。
板端96条离线数值检查已通过;真实8秒采样出现过2–4个计数缺口,实时采样尚未验收,网页会明确显示该状态。主动影子诊断保留遇任意丢帧即失败的严格门禁,可能因此中止;`shadow_ready` 或校准文件存在仅表示有配置,不代表实时链路通过。
保存后的 8 秒影子推理只建立196维观测并计算网络输出,两轴命令历史是虚拟的,不向舵机发送。训练模型91.69%仿真成功率不等于真机恢复成功;舵机零位、方向、全行程及机构安装仍需后续实测门禁。本次不启用旧页面的动作按钮。
本机服务只绑定回环,保留 Host/Origin/CSRF 检查。测试为 `python3 -m unittest calibration.test_imu -v`、`node calibration/test_imu_frontend.cjs`:包括纯合成三姿态、拒绝运动API、候选门禁及完整网页保存→模拟SSH→真实板端校验器→临时配置回读。该保存测试只写临时目录,没有伪造或保存实机校准。
本机入口:<http://127.0.0.1:8766/calibration>。原仿真结果页保留在 `/`,新增标定入口。服务仅绑定回环地址,不公开到局域网。
2026-11-11 自助点测:相机与单轴操作并排显示,IMU、测角和历史证据收进「更多标定」。选舵机和目标,完成三项现场确认,点击「3 秒后开始一次往返」;倒计时可用右上角停止取消,倒计时结束仍重新检查设备、相机和确认状态。每轮停止后,可点「动了,并且回位/动了,没有回位/没有动作」直接保存到显示的任务记录,页面刷新会保留已保存的观察。回位指回到本轮首次指令姿态;不指启动前姿态,也不据此认定角度标定通过。无需聊天逐次批准,原锁、租约、板端超时和清理保留;未启用策略动作或双轴联动。自助下拉不提供 550/2450 μs 极端探测。
页面载入与刷新只读状态,不启动 PWM 或 IMU 采样。设备身份每 5 秒只读刷新;实时六轴数据仅在用户主动开启的 8 秒采样中更新。历史 439 帧、零偏/安装矩阵候选、上层录像已加载,不需要从头重做。
## 使用
1. 打开相机并允许浏览器使用摄像头,选择正确设备,完整拍到杆尖、两轴、底板及运动区域。相机预览来自浏览器所在电脑;内置浏览器不支持时用 Chrome 打开同一地址。
2. 需要重采时,选择静止、前边抬高或左边抬高,倒计时后采样 8 秒。采样结束恢复原寄存器;新记录不自动替换或应用历史候选参数。
3. 单轴测试前确认当前姿态、完整取景和清场。动作严格为 1500 → 1450 或 1550 → 1500 微秒,分别保持 2 / 3 / 2 秒。首次归位可能大幅运动,1500 微秒并非已验证竖直中位。
4. 前后图片自动保存。用户依次点击杆底、杆尖计算二维画面投影角,另行记录看到运动/没看到/未知。拍不到杆时保持未知;前后差包含首次归位,不能当成 50 微秒产生的增量角。
5. 顶部停止按钮停止当前请求;清理是否完成以板端回读为准。停止 PWM 不等于切断舵机电源。JSON 和 Markdown 导出包括原始阶段回读、候选值、来源 SHA 及图像引用。
## 服务启动
已有 USB SSH 用户授权保存在根目录交接文件。本机已配置专用 SSH 密钥和已核验的主机公钥,保存在 `~/.config/x5-calibration/`;私钥仅本机可读,不保存板卡密码。从仓库根运行,无需预先手动建立 SSH 会话:
```sh
python3 -m calibration.server --port 8766 --debug-only
```
服务通过 OpenSSH `ControlMaster=auto` 自动建立会话,空闲复用 60 秒;会话消失后,下次状态刷新会重新建立。连接维护循环间隔 5 秒(不含连接超时耗时),首次准备失败也会重试。已准备环境重连只读取状态,不重放任何运动任务;首次脚本准备会检查板卡身份、独占锁及 PWM 空闲。断线中的任务仍按租约停止,未确认的清理保持未知。
板卡改为静态 `192.168.128.100` 时,为上述服务命令增加 `CALIBRATION_SSH_HOST=192.168.128.100`,并先将新地址对应的主机公钥核验后加入专用 `known_hosts`;不会自动接受未知或变更的主机密钥。默认控制 socket 按主机区分。仍兼容 `CALIBRATION_SSH_CONTROL`,密钥和主机公钥文件分别可通过 `CALIBRATION_SSH_IDENTITY`、`CALIBRATION_SSH_KNOWN_HOSTS` 指定。这些配置不修改电脑或板卡 IP。
本机专用公钥在板端限制为 USB 子网 `192.168.128.0/24` 来源,禁用端口转发、代理转发和交互终端;切换到其他网络须另行核对授权。服务停止、USB 未识别、板卡未开机或物理网络不通时,不能仅靠 SSH 自动恢复。首次在其他电脑使用仍需安装该电脑自己的公钥并核验板卡主机公钥。
2026-11-10 本次硬件调试使用同一启动命令加 `--debug-only`:仅上传 IMU/单轴 runner,屏蔽旧恢复就绪状态,并在服务端拒绝 recovery 请求。旧 profile 的近似起身批准不会启用本轮恢复入口。
启动前确认 8766 空闲,勿结束其他服务。启动会上传并校验专用 runner 与既有采样器,仅写 `/home/sunrise/x5_calibration_web_20261109/`。不会输出 PWM、改变设备树或自动采样。已有活动任务时不能替换 runner。
## 保护与证据
- 同源 Host/Origin 校验和每次服务启动生成的请求令牌;无跨域控制接口。
- 单轴固定指令范围,当前取景确认、15 秒内的新前图,重复 UUID 拒绝重放。
- 本地任务互斥与板端文件锁;发现已导出 PWM 即拒绝抢占。
- 收到首次浏览器保活后才允许下发任务;无首次保活或先停止时不接触运动控制。
- 浏览器每 700 ms 保活,本地与板端分别检查 2.5 秒租约;板端 15 秒超时,本地 SSH 25 秒超时。
- 清理失败或 SSH 断连显示未知/失败,不将请求接受或 PWM 回读当作机械到位。
- `deployment/x5_mg90s_20261109_verify/web-calibration/` 保存新任务 JSON、图片、观察、验收记录;历史原文件不变。
测试:`python3 -m unittest calibration.test_backend calibration.test_server -v` 和 `node calibration/test_frontend.cjs`(动作门禁、竞态、投影测量、相机授权/取消、自助倒计时取消及未回位记录)。硬件动作测试使用替身,超时测试使用独立进程;并非实机运动验收。
已实读 X5 身份、IMU 0x6B / WHO_AM_I 0x05 和空闲 PWM,已核对板端 runner SHA。新网页的实时相机权限、8 秒采样和动作闭环仍需现场操作验证。本次未输出 PWM 或重新采样。X5 直供余量、全行程、关节实际角度及旧策略的真实观测适配仍未验证,本页不提供爬起入口。
2026-11-09 晚间续作:增加逐步测量用的固定 900..2100 μs(100 μs 间隔,排除1500)目标;旧1450/1550流程不变。新目标以400 μs/s、每步最多20 μs往返,最长指令流程10秒,保留15秒硬上限和浏览器心跳停机。列表不是实测安全行程或已校准角度;执行前先核对零位,并逐个邻近目标观察。
## 2026-11-11 排针定位澄清
从排针末端 `DO / GND` 那一排开始数,每一对两针算一排;不能把 GPIO6 功能名理解成第六排,也不依赖实物上是否印全 GPIO 数字。
| 从 DO/GND 端数 | 同排文字 | 本次用途 |
|---|---|---|
| 1 | DO / GND | 不接信号 |
| 2 | DIN / GP | 不接信号 |
| 3 | GP / WS | 不接信号 |
| 4 | GND / PWM | PWM 是上轴信号,物理33 |
| 5 | PWM / GP | PWM 是下轴信号,物理32 |
| 6 | GND / GP | 不是本次下轴信号针 |
同排文字顺序不代表固定左右:正面/背面视角会镜像。定位端点、排数和PWM文字后再插线,断电改线。
用户已明确选择空载、现场口头反馈的调试方式。runner新增显式 `observation_mode=user_feedback`,仅允许机械空载、有人观察、场地/初始归位条件确认的单轴1450/1550us小幅任务,不伪造相机确认或照片。原相机模式、恢复模式、全行程探测仍按原门禁执行;锁、租约、硬超时与清理不变。
### 简化的现场确认
实机区使用一个明确的“本次准备”确认,包含供电共地、当前画面、现场观察、活动区及归中风险。同轴同阶段的成功点动和选择下个小步不清除此确认;换轴、换阶段、急停、切后台和断线会清除。阶段确认只显示当前适用的一项。测量后自动选择最新记录点;服务端原有输入验证、实时画面、租约、急停、限位与实测覆盖检查保持有效。

1
calibration/__init__.py Normal file
View File

@ -0,0 +1 @@
"""Local, user-triggered hardware calibration; importing never contacts hardware."""

328
calibration/bridge.py Normal file
View File

@ -0,0 +1,328 @@
"""Local SSH bridge; no connection or hardware action occurs on construction."""
import base64
import copy
from datetime import datetime
import hashlib
import json
import os
from pathlib import Path
import shlex
import subprocess
import tempfile
import threading
import time
from zoneinfo import ZoneInfo
from .remote_runner import validate
ROOT = Path(__file__).resolve().parent.parent
OUTPUT = ROOT / 'deployment/x5_mg90s_20261109_verify/web-calibration'
REMOTE = '/home/sunrise/x5_calibration_web_20261109'
ACTIVE = ('queued', 'running', 'stopping')
TRANSPORT_TIMEOUT = 25
FIRST_HEARTBEAT_TIMEOUT = 2.5
class BeforeLaunchStopped(RuntimeError):
pass
def now():
return datetime.now(ZoneInfo('Asia/Shanghai')).isoformat()
class Bridge:
def __init__(self, output=OUTPUT, control=None, debug_only=False):
self.debug_only = debug_only
self.runner_path = REMOTE + '/remote_runner.py'
self.output = Path(output)
self.output.mkdir(parents=True, exist_ok=True)
self.host = os.environ.get('CALIBRATION_SSH_HOST', '192.168.128.10')
config = Path.home() / '.config/x5-calibration'
socket_name = f'x5-calibration-{os.getuid()}-{hashlib.sha256(self.host.encode()).hexdigest()[:8]}.sock'
self.control = control or os.environ.get('CALIBRATION_SSH_CONTROL') or str(Path(tempfile.gettempdir()) / socket_name)
self.identity = os.environ.get('CALIBRATION_SSH_IDENTITY', str(config / 'id_ed25519'))
self.known_hosts = os.environ.get('CALIBRATION_SSH_KNOWN_HOSTS', str(config / 'known_hosts'))
self.lock = threading.RLock()
self.prepared = False
self.process = None
self._worker_active = False
self.job = None
self.device = {'connected': False, 'read_at': None, 'error': '尚未读取实时状态',
'identity': None, 'imu': {}, 'pwm': None}
self.jobs = []
self.last_heartbeat = 0
self.stop_requested = threading.Event()
self.first_heartbeat = threading.Event()
for path in sorted(self.output.glob('job-*.json')):
try:
job = json.loads(path.read_text())
if job['status'] in ACTIVE:
job.update(status='failed', error='Local service restarted; previous outcome unknown',
cleanup_verified=False)
self._save(job)
self.jobs.append(job)
except (ValueError, KeyError, OSError):
continue
if self.jobs:
self.job = max(self.jobs, key=lambda j: j.get('created_epoch', 0))
def _ssh(self, command):
return ['ssh', '-S', self.control, '-o', 'BatchMode=yes',
'-o', 'ControlMaster=auto', '-o', 'ControlPersist=60',
'-i', self.identity, '-o', 'IdentitiesOnly=yes',
'-o', 'UserKnownHostsFile=' + self.known_hosts, '-o', 'StrictHostKeyChecking=yes',
'-o', 'ConnectTimeout=3', '-o', 'ServerAliveInterval=1', '-o', 'ServerAliveCountMax=2',
'sunrise@' + self.host, command]
def _save(self, job):
path = self.output / ('job-' + job['id'] + '.json')
temporary = path.with_suffix('.tmp')
temporary.write_text(json.dumps(job, ensure_ascii=False, indent=2))
temporary.replace(path)
def prepare(self):
"""Upload only the runner and existing sampler; never executes a calibration."""
paths = [ROOT / 'calibration/remote_runner.py',
ROOT / 'deployment/x5_mg90s_20261109_verify/imu_sampling/sample_qmi8658.py']
recovery_dir = ROOT / 'deployment/imu_recovery_20261109'
recovery_files = [recovery_dir / name for name in ('recovery_hardware.py', 'recovery_loop.py',
'recovery_candidate.py', 'imu_orientation.py', 'qmi_i2c.py')]
if not self.debug_only and all(p.exists() for p in recovery_files):
paths.extend(recovery_files)
paths.append(OUTPUT / 'job-df96a8aa-3c86-4abd-8273-768d37b99b54.json')
profile = recovery_dir / 'calibration_profile.json'
if profile.exists():
paths.append(profile)
files = {p.name: base64.b64encode(p.read_bytes()).decode() for p in paths}
# File names are server-owned constants, never supplied by the browser.
code = ('import base64,json,pathlib,sys,hashlib,os,fcntl; '
"assert pathlib.Path('/proc/device-tree/model').read_text().rstrip('\\x00') == 'D-Robotics RDK X5 V1.0'; "
'files=json.load(sys.stdin); root=pathlib.Path(' + repr(REMOTE) + '); '
'expected={name:hashlib.sha256(base64.b64decode(data)).hexdigest() for name,data in files.items()}; '
'existing={name:hashlib.sha256((root/name).read_bytes()).hexdigest() if (root/name).is_file() else None for name in files}; '
'matched=existing==expected; '
'print(json.dumps(existing)) if matched else None; '
'sys.exit(0) if matched else None; '
"lock=os.open('/tmp/x5-calibration-web.lock',os.O_CREAT|os.O_RDONLY,0o600); "
'fcntl.flock(lock,fcntl.LOCK_EX|fcntl.LOCK_NB); '
"assert not list(pathlib.Path('/sys/class/pwm').glob('pwmchip*/pwm[0-9]*')), 'PWM is already exported'; "
'root.mkdir(exist_ok=True); '
'[(root/name).write_bytes(base64.b64decode(data)) for name,data in files.items()]; '
'print(json.dumps({name:hashlib.sha256((root/name).read_bytes()).hexdigest() for name in files}))')
with self.lock:
if self._worker_active or self.job and self.job['status'] in ACTIVE:
raise ValueError('Cannot replace runner while a task is active')
run = subprocess.run(self._ssh('python3 -c ' + shlex.quote(code)), input=json.dumps(files),
capture_output=True, text=True, timeout=10, check=True)
observed = json.loads(run.stdout)
expected = {p.name: hashlib.sha256(p.read_bytes()).hexdigest() for p in paths}
if observed != expected:
raise RuntimeError('Remote runner hash verification failed')
self.prepared = True
return {'uploaded': expected, 'motion_started': False}
def refresh(self):
with self.lock:
if self._worker_active or self.job and self.job['status'] in ACTIVE:
return self.status()
try:
run = subprocess.run(self._ssh('sudo -n python3 ' + REMOTE + '/remote_runner.py status'),
capture_output=True, text=True, timeout=7, check=True)
device = json.loads(run.stdout)
if device.get('identity') != 'D-Robotics RDK X5 V1.0':
raise RuntimeError('Unexpected board identity')
if self.debug_only:
device.update(recovery_ready=False, recovery_error='本次仅启用 IMU 与单轴调试')
device.update(read_at=time.time(), read_at_iso=now(), error=None)
except Exception as exc:
device = {'connected': False, 'identity': None, 'imu': {}, 'pwm': None,
'read_at': time.time(), 'read_at_iso': now(), 'error': str(exc)}
with self.lock:
self.device = device
return self.status()
def status(self):
with self.lock:
device = copy.deepcopy(self.device)
device['age_s'] = time.time() - device['read_at'] if device['read_at'] else None
job = copy.deepcopy(self.job)
if job and job['status'] in ACTIVE:
job['elapsed_s'] = max(0, time.monotonic() - self.started_mono)
summaries = [{k: j.get(k) for k in ('id', 'type', 'pose', 'pin', 'pulse_us',
'status', 'started_at', 'error', 'cleanup_verified')}
for j in self.jobs]
return {'device': device, 'job': job, 'jobs': summaries, 'prepared': self.prepared}
def _validate_payload(self, payload):
return validate(payload)
def start(self, payload):
payload = self._validate_payload(payload)
if self.debug_only and payload['type'] == 'recovery':
raise ValueError('Recovery is disabled in this debugging session')
with self.lock:
if any(j['id'] == payload['request_id'] for j in self.jobs):
raise ValueError('Request already recorded; replay will not execute hardware')
if self._worker_active or self.job and self.job['status'] in ACTIVE:
raise ValueError('Another calibration is active')
if not self.prepared:
raise ValueError('Remote runner is not prepared')
if not self.device.get('connected') or not self.device.get('read_at') or time.time() - self.device['read_at'] > 30:
raise ValueError('Refresh device status before starting')
if self.device.get('pwm'):
raise ValueError('PWM is already exported; inspect the existing owner')
job = dict(payload, id=payload['request_id'], status='queued', phase='awaiting_first_heartbeat',
started_at=now(), created_epoch=time.time(), events=[], elapsed_s=0,
latest_sample=None, sample_received_at=None, result=None, error=None,
cleanup_verified=False, physical_success='unknown')
self.jobs.append(job)
self.job = job
self._save(job)
self.last_heartbeat = self.started_mono = time.monotonic()
self.stop_requested.clear()
self.first_heartbeat.clear()
self._worker_active = True
threading.Thread(target=self._execute, args=(job, payload), daemon=True).start()
return copy.deepcopy(job)
def heartbeat(self, job_id):
with self.lock:
if not self.job or self.job['id'] != job_id:
raise ValueError('Unknown active job')
if self.job['status'] in ACTIVE:
if not self.first_heartbeat.is_set() and time.monotonic() - self.started_mono > FIRST_HEARTBEAT_TIMEOUT:
raise ValueError('First browser heartbeat window expired')
self.last_heartbeat = time.monotonic()
self.first_heartbeat.set()
return copy.deepcopy(self.job)
def stop(self):
with self.lock:
if self.job and self.job['status'] in ACTIVE:
self.stop_requested.set()
self.job['phase'] = 'stop_requested'
self._save(self.job)
return copy.deepcopy(self.job)
def _execute(self, job, payload):
terminal_received = False
stderr = []
proc = None
feeder_done = threading.Event()
transport_timeout = threading.Event()
command_sent = False
def watchdog():
if feeder_done.wait(TRANSPORT_TIMEOUT):
return
transport_timeout.set()
proc.terminate()
try:
proc.wait(timeout=1)
except subprocess.TimeoutExpired:
proc.kill()
def feed():
while not feeder_done.wait(0.5):
with self.lock:
expired = time.monotonic() - self.last_heartbeat > 2.5
stop = self.stop_requested.is_set() or expired or time.monotonic() - self.started_mono > 15
try:
proc.stdin.write(json.dumps({'action': 'stop' if stop else 'heartbeat'}) + '\n')
proc.stdin.flush()
except (OSError, ValueError):
return
if stop:
return
try:
# Start alone cannot authorize motion: require a response from its live owner.
while not self.first_heartbeat.wait(.025):
if self.stop_requested.is_set():
raise BeforeLaunchStopped('Stopped before first browser heartbeat; no motion command sent')
if time.monotonic() - self.started_mono >= FIRST_HEARTBEAT_TIMEOUT:
raise BeforeLaunchStopped('No first browser heartbeat; no motion command sent')
if self.stop_requested.is_set():
raise BeforeLaunchStopped('Stopped before SSH launch; no motion command sent')
proc = subprocess.Popen(self._ssh('sudo -n python3 -u ' + self.runner_path + ' run'),
stdin=subprocess.PIPE, stdout=subprocess.PIPE, stderr=subprocess.PIPE,
text=True, bufsize=1)
with self.lock:
self.process = proc
if self.stop_requested.is_set() or time.monotonic() - self.last_heartbeat > 2.5:
raise BeforeLaunchStopped('Client lease ended before launch; no motion command sent')
command_sent = True
proc.stdin.write(json.dumps(payload) + '\n')
proc.stdin.flush()
threading.Thread(target=feed, daemon=True).start()
threading.Thread(target=watchdog, daemon=True).start()
threading.Thread(target=lambda: stderr.append(proc.stderr.read(16384)), daemon=True).start()
for line in proc.stdout:
event = json.loads(line)
event['received_at'] = time.time()
with self.lock:
job['events'].append(event)
job['elapsed_s'] = event.get('elapsed_s', job['elapsed_s'])
if event['event'] == 'started':
job.update(status='running', phase='preflight')
self.device = {**event['device'], 'read_at': time.time(), 'read_at_iso': now()}
elif event['event'] == 'phase':
job['phase'] = event['phase']
elif event['event'] == 'sample':
job.update(phase='sampling', latest_sample=event['sample'],
sample_received_at=event['received_at'])
elif event['event'] == 'control':
job['latest_control'] = {k: v for k, v in event['control'].items()
if k != 'observation'}
elif event['event'] == 'result':
terminal_received = True
job.update(status=event['status'], result=event.get('result'),
error=event.get('error'), cleanup_verified=event.get('cleanup_verified', False),
phase='finished')
if job['status'] == 'completed' and not job['cleanup_verified']:
job.update(status='failed', error='Cleanup was not verified')
self._save(job)
code = proc.wait(timeout=3)
if transport_timeout.is_set():
raise RuntimeError('SSH transport exceeded 25s; board cleanup is unconfirmed')
if not terminal_received or code != 0:
raise RuntimeError('SSH/runner ended without verified completion: ' + ''.join(stderr)[-1000:])
except BeforeLaunchStopped as exc:
with self.lock:
job.update(status='stopped', phase='cancelled_before_motion', error=str(exc),
cleanup_verified=True, result={'motion_attempted': False, 'cleanup_required': False})
except Exception as exc:
with self.lock:
job.update(status='failed', phase='transport_failed', error=str(exc),
cleanup_verified=not command_sent)
finally:
feeder_done.set()
if proc is not None:
try:
proc.stdin.close()
except (OSError, ValueError):
pass
if proc.poll() is None:
proc.terminate()
try:
proc.wait(timeout=1)
except subprocess.TimeoutExpired:
proc.kill()
proc.wait(timeout=1)
proc.stdout.close()
proc.stderr.close()
with self.lock:
self.process = None
job['finished_at'] = now()
job['elapsed_s'] = time.monotonic() - self.started_mono
self._save(job)
self._worker_active = False
# Refresh is a read-only confirmation; transport failure remains unknown.
self.refresh()
def export(self):
with self.lock:
return {'exported_at': now(), 'device': self.status()['device'],
'jobs': copy.deepcopy(self.jobs), 'parameters_applied': False,
'policy_hardware_ready': False}

View File

@ -0,0 +1,72 @@
// The continuous-speed panel shows the same USB camera used for bench recordings.
(() => {
const image = $('deskFrame'), status = $('deskStatus'), toggle = $('deskToggle');
let visible = false, paused = false, timer, pending, url;
function blank(message) {
delete image.dataset.capturedAt;
image.hidden = true;
image.removeAttribute('src');
if (url) URL.revokeObjectURL(url);
url = null;
status.textContent = message;
}
async function next() {
if (!visible || paused || document.hidden || !state?.continuous_deployment) return;
const request = new AbortController();
pending = request;
const timeout = setTimeout(() => request.abort(), 1000);
let delay = 125;
try {
const response = await fetch('/api/imu/camera.jpg', {
headers: {'X-Cal-Token': state.csrf_token}, signal: request.signal, cache: 'no-store'
});
if (!response.ok) throw Error();
const capturedAt = Number(response.headers.get('X-Frame-Time')) * 1000;
const data = await response.blob();
if (pending !== request) return;
if (!capturedAt || Date.now() - capturedAt > 1200) throw Error();
const last = url;
url = URL.createObjectURL(data);
image.src = url;
image.dataset.capturedAt = String(capturedAt);
image.hidden = false;
if (last) URL.revokeObjectURL(last);
status.textContent = '实时 · Deli 桌面相机 · ' + new Date(capturedAt).toLocaleTimeString();
} catch {
if (pending !== request) return;
blank('等待相机画面,正在重连。请确认 Deli 摄像头已接入电脑。');
delay = 500;
} finally {
clearTimeout(timeout);
if (pending === request) {
pending = null;
timer = setTimeout(next, delay);
}
}
}
function update() {
clearTimeout(timer);
pending?.abort();
pending = null;
blank(paused ? '实时画面已暂停。' : '正在连接桌面相机…');
toggle.textContent = paused ? '恢复画面' : '暂停画面';
next();
}
toggle.addEventListener('click', () => { paused = !paused; update(); });
new IntersectionObserver(([entry]) => {
visible = entry.isIntersecting;
update();
}).observe($('speedWork'));
document.addEventListener('visibilitychange', update);
window.addEventListener('pagehide', () => { visible = false; update(); });
window.addEventListener('pageshow', event => {
if (event.persisted) {
const bounds = $('speedWork').getBoundingClientRect();
visible = bounds.height > 0 && bounds.bottom > 0 && bounds.top < window.innerHeight;
update();
}
});
})();

View File

@ -0,0 +1,61 @@
"""On-demand live view of the desk camera shared by local browser tabs."""
import os
from pathlib import Path
import shutil
import subprocess
import tempfile
import threading
import time
class DeskCamera:
def __init__(self):
self.directory = tempfile.TemporaryDirectory(prefix='x5-desk-camera-')
self.path = Path(self.directory.name) / 'frame.jpg'
self.lock = threading.Lock()
self.process = None
self.last_request = 0
def _stop(self):
if self.process is not None:
self.process.terminate()
try:
self.process.wait(timeout=1)
except subprocess.TimeoutExpired:
self.process.kill()
self.process.wait()
self.process = None
self.path.unlink(missing_ok=True)
def release_if_idle(self):
with self.lock:
if self.process is not None and time.monotonic() - self.last_request > 6:
self._stop()
def close(self):
with self.lock:
self._stop()
self.directory.cleanup()
def frame(self):
with self.lock:
self.last_request = time.monotonic()
if self.process is None or self.process.poll() is not None:
self._stop()
self.started = self.last_request
self.process = subprocess.Popen([
shutil.which('ffmpeg') or '/opt/homebrew/bin/ffmpeg', '-nostdin', '-loglevel', 'error',
'-f', 'avfoundation', '-pixel_format', 'uyvy422', '-video_size', '1280x720',
'-framerate', '30', '-i', 'Deli-2K-Camera:none', '-an', '-vf', 'fps=8',
'-q:v', '5', '-f', 'image2', '-update', '1', '-atomic_writing', '1',
'-y', str(self.path)], stdin=subprocess.DEVNULL, stdout=subprocess.DEVNULL)
try:
with self.path.open('rb') as image:
modified = os.fstat(image.fileno()).st_mtime
if time.time() - modified < 1:
return image.read(), modified
except FileNotFoundError:
modified = 0
if self.last_request - self.started > 6 and time.time() - modified > 5:
self._stop()
return None

116
calibration/imu.html Normal file

File diff suppressed because one or more lines are too long

145
calibration/imu_motion.js Normal file
View File

@ -0,0 +1,145 @@
'use strict';
let motionOwned=null,motionOperation=0,motionCountdown=0,motionTimer=null,motionHeartbeatBusy=false,motionLaunching=false,activeObservationMode=null,awaitedMotion=null;
let cameraStream=null,cameraGeneration=0,cameraPending=false,cameraFrameAt=0,cameraLastTime=-1;
let uploadedBefore=null,jogTarget=1550,pointSignature='',lastMotionFinished=null,phasePhotoPending=new Set();
const motionChecks=['benchReady'];
let servoViewInitialized=false;
function startServoReview(){
if(pending||motionCountdown||activeMotion(motionJob())||busySensor())return;
$('jogAxis').value='32';
const zero=(state?.motion_status?.calibration_points?.['32']||[]).some(p=>p.pulse_us===1500&&p.measured_deg===0);
$('jogStage').value=zero?'mounted_measurement':'unloaded_center';
clearMotionChecks();pointSignature='';jogTarget=1450;renderMotion();$('pointBase').value='1500';renderMotion();
}
$('servoReview').addEventListener('click',startServoReview);
const cadDemoData=JSON.parse($('cadDemoFrames').textContent);
let cadDemoAxis=null,cadDemoIndex=8,cadDemoStep=1,cadDemoTimer=null;
function stopCadDemo(){if(cadDemoTimer!==null)clearInterval(cadDemoTimer);cadDemoTimer=null;text('cadDemoPlay','播放 CAD 转动')}
function renderCadDemo(){
const axis=$('jogAxis').value==='33'?'33':'32',frames=cadDemoData.axes[axis];
if(axis!==cadDemoAxis){stopCadDemo();cadDemoAxis=axis;cadDemoIndex=8;cadDemoStep=1;}
const busy=pending||motionCountdown>0||activeMotion(state?.motion?.job);
if(busy)stopCadDemo();
$('cadDemoPlay').disabled=$('cadDemoZero').disabled=$('cadDemoSlider').disabled=busy||!frames?.length;
if(!frames?.length)return;
const angle=cadDemoData.angles[cadDemoIndex];
if($('cadDemoImage').src!==frames[cadDemoIndex])$('cadDemoImage').src=frames[cadDemoIndex];
$('cadDemoSlider').value=String(cadDemoIndex);text('cadDemoAngle',(angle>0?'+':'')+angle+'° · CAD 示意');
text('cadDemoTitle',axis==='32'?'下轴 CAD 转动演示 · 青框和上部整体倾斜':'上轴 CAD 转动演示 · 青框不动,橙臂和杆倾斜');
text('cadDemoInstruction',axis==='32'?'量青框相对底板的角度。正角度朝板上 -Y 端倾,负角度朝 +Y 端倾;另一轴保持零位。':'量橙臂相对青框的角度。正角度朝板上 +X 端倾,负角度朝 -X 端倾;另一轴保持零位。');
}
$('cadDemoPlay').addEventListener('click',()=>{
renderCadDemo();if($('cadDemoPlay').disabled)return;
if(cadDemoTimer!==null){stopCadDemo();return;}
text('cadDemoPlay','暂停 CAD 转动');
cadDemoTimer=setInterval(()=>{
if(document.visibilityState!=='visible'){stopCadDemo();return;}
if(cadDemoIndex===16)cadDemoStep=-1;else if(cadDemoIndex===0)cadDemoStep=1;
cadDemoIndex+=cadDemoStep;renderCadDemo();
},150);
});
$('cadDemoZero').addEventListener('click',()=>{stopCadDemo();cadDemoIndex=8;renderCadDemo()});
$('cadDemoSlider').addEventListener('input',()=>{stopCadDemo();cadDemoIndex=Math.max(0,Math.min(16,Math.round(Number($('cadDemoSlider').value)||0)));renderCadDemo()});
const activeMotion=job=>['queued','running','stopping'].includes(job?.status);
function motionJob(){return state?.motion?.job}
function physicalCameraReady(){const v=$('motionCamera');return document.visibilityState==='visible'&&cameraStream&&cameraStream.getVideoTracks().some(t=>t.readyState==='live'&&!t.muted)&&!v.paused&&v.readyState>=2&&performance.now()-cameraFrameAt<1800}
function photoReady(){return $('modePhoto').checked&&uploadedBefore&&!uploadedBefore.job_id&&Date.now()/1000-uploadedBefore.captured_at<12}
function observationReady(){return $('modePhoto').checked?photoReady():physicalCameraReady()}
function physicalChecked(){return motionChecks.every(id=>$(id).checked)&&document.visibilityState==='visible'}
function axisPoints(){const points=state?.motion_status?.calibration_points||{};return points[String($('jogAxis').value)]||[]}
function pulseOf(p){return Number(p.pulse_us)}
function angleOf(p){return Number(p.measured_deg)}
function clearMotionChecks(){motionChecks.forEach(id=>$(id).checked=false);$('fallenStationary').checked=false;$('jogUnloaded').checked=false;$('jogMounted').checked=false}
function busySensor(){return state?.job?.status==='running'}
function renderContinuousDeployment(){
const c=state.continuous_deployment;$('continuousPanel').hidden=!c;
document.body?.classList.toggle('continuous-mode',!!c);
if(!c)return false;
text('motionHeading','双舵机校准与实机实验');
if(typeof renderSpeedBench==='function')renderSpeedBench();
text('continuousState',c.passed?'新策略已部署到 X5;真实 IMU 无 PWM 连续推理已通过('+c.controls+' 次控制 / '+c.samples+' 帧采样)。本页支持单轴校准与单次双轴实验。':'新策略部署检查尚未完成;不会启动舵机。');
if(c.supply_hold){text('continuousState','新策略的板端推理检查已通过;当前供电电压未确认,全部舵机输出已暂停。');text('continuousNext','请先关闭舵机电池盒。两节18650的串并联和稳压输出尚未核实,不能直接按5V使用;确认舵机端为4.8–6V后再继续调试。')}
if(!c.supply_hold && c.last_physical_trial){
const t=c.last_physical_trial;
text('continuousState','X5 已部署 360° 速度模型。上次双轴输出 '+t.pwm_window_s.toFixed(2)+' 秒。');
text('continuousNext',t.corrected_lower && t.termination === 'ValueError: Control sensor cache older than 10ms'?'本轮未爬起。IMU 样本超过 10 毫秒,控制程序已停止双轴输出。':t.imu_upright_candidate?'上次 IMU 检测到直立,实机结果以现场画面为准。':(c.lower_probe_verified?'上次双轴试跑未站稳。下轴已按实拍点测修正方向,等待双轴验证。':'上次双轴试跑未检测到站稳。'));
}
text('continuousDetails',JSON.stringify(c,null,2));
['jogStart','recoverStart','quickZeroLower','quickZeroUpper','recordZero','recordAngle','servoSave','shadow','servoReview'].forEach(id=>$(id).disabled=true);
text('cadDemoInstruction','这里只演示装配的关节方向。360°舵机按速度控制,不用示意角度做舵机定位标定。');
return true;
}
function renderMotion(){renderCadDemo();if(!state)return;
if(renderContinuousDeployment())return;
if(!servoViewInitialized&&state.saved?.saved&&state.motion_status?.installed){servoViewInitialized=true;startServoReview();}
const j=motionJob(),ms=state.motion_status||{},busy=pending||motionCountdown>0||activeMotion(j)||busySensor(),connected=networkHealthy&&state.device?.connected===true&&Date.now()/1000-state.device.read_at<20;
$('servoReview').disabled=busy;
const resumeCoverage=ms.calibration_coverage||{};
text('servoResumeState',state.saved?.saved?'IMU 安装校准已保存,不用重做。下轴 '+(resumeCoverage['32']?.measured_count||0)+' 个实测点,上轴 '+(resumeCoverage['33']?.measured_count||0)+' 个实测点。先核对零位,再补两轴角度范围。':'尚未读取到已保存的 IMU 安装校准;保留现有记录,请先确认连接。');
const installed=ms.installed===true,checked=connected&&installed&&physicalChecked()&&observationReady(),points=axisPoints(),zero=points.some(p=>pulseOf(p)===1500&&Math.abs(angleOf(p))<.01),mounted=$('jogStage').value==='mounted_measurement';
for(const [pin,id] of [[32,'quickZeroLower'],[33,'quickZeroUpper']]){const zero=(ms.calibration_points?.[String(pin)]||[]).some(p=>pulseOf(p)===1500&&angleOf(p)===0);$(id).disabled=busy||!connected||!installed||zero;$(id).textContent=zero?(pin===32?'下轴零位已记录':'上轴零位已记录'):(pin===32?'下框已摆正,记录下轴0°':'杆已竖直,记录上轴0°');}
const reasons=ms.blockers||ms.reasons||[];text('motionGate',!installed?'本轮动作模块尚未就绪;等待读取连接和板端版本,当前不能启动动作。':ms.recover_ready===true?'板端前置门禁已通过;仍需当前画面和本次准备确认,再主动执行一次。':'动作模块已安装;爬起尚未就绪。'+(Array.isArray(reasons)?reasons.join(';'):String(reasons||'')));
$('shadow').disabled=busy||!connected||!installed||!state.saved||!$('shadowConfirm').checked||!$('shadowConfirm').checked;text('preflightResult',state.preflight?JSON.stringify(state.preflight,null,2):'等待主动检查;无 PWM。已有 IMU 校准会保留。');
if(installed)text('runtimeState',ms.sensor_stream_validated===true?'本页现场无 PWM 验证已通过,并绑定当前模型、安装与运行版本。爬起还需两轴实测标定及当前前图;不等于实机起身已验证。':'本页现场无 PWM 验证尚未完成或已失效;请平放静止并主动运行一次。旧接口的失败记录属于历史,当前是否就绪以本轮板端回执为准。');
$('cameraOpen').disabled=cameraPending||activeMotion(j)||pending;$('modePhoto').disabled=$('modeCamera').disabled=activeMotion(j)||pending;$('currentPhoto').disabled=activeMotion(j)||pending;$('cameraClose').disabled=!cameraStream&&!cameraPending;
text('cameraStatus',cameraPending?'等待浏览器相机权限;可取消等待。':physicalCameraReady()?'LIVE · 真实相机画面':cameraStream?'画面停止或陈旧;会请求停止动作。':photoReady()?'已上传当前前图(15秒内单次使用)':'相机未打开;可上传当前全景照片并现场持续观察。');
const before=(state.frames||[]).find(f=>f.job_id===j?.id&&f.role==='before'),after=(state.frames||[]).filter(f=>f.job_id===j?.id&&f.role==='after').slice(-1)[0];if(uploadedBefore&&!uploadedBefore.job_id)showFrame('beforePreview',uploadedBefore);else if(before)showFrame('beforePreview',before);if(after)showFrame('afterPreview',after);else $('afterPreview').hidden=true;
const pin=Number($('jogAxis').value);text('axisPositive',pin===32?'下轴正角:右手绕 body +X,原本竖直的杆向 body −Y 偏;上轴保持零位。':'上轴正角:右手绕随下轴转动的局部 +Y,原本竖直的杆向 body +X 偏;下轴保持零位。');
const sig=JSON.stringify([pin,points.map(p=>[pulseOf(p),angleOf(p)])]);if(sig!==pointSignature){pointSignature=sig;const old=$('pointBase').value,opts=points.length?points:[{pulse_us:1500,measured_deg:0}];$('pointBase').replaceChildren(...opts.map(p=>{const o=document.createElement('option');o.value=String(pulseOf(p));o.textContent=pulseOf(p)+' μs · '+angleOf(p)+'°'+(points.length?' 实测':' 待确认零位');return o}));const latest=points.slice().sort((a,b)=>(a.seq||0)-(b.seq||0)).slice(-1)[0];if(latest)$('pointBase').value=String(pulseOf(latest));else if(opts.some(p=>String(pulseOf(p))===old))$('pointBase').value=old;}
$('unloadedConfirmRow').hidden=mounted;$('mountedConfirmRow').hidden=!mounted;text('targetPulse',jogTarget);$('jogAxis').disabled=$('jogStage').disabled=$('pointBase').disabled=busy;$('pulseMinus').disabled=$('pulsePlus').disabled=busy;
const jogStageOK=mounted?zero&&$('jogMounted').checked:$('jogUnloaded').checked&&[1450,1550].includes(jogTarget);
$('jogStart').disabled=busy||!checked||!jogStageOK;text('jogGate',busy?'等待当前任务结束,或按急停取消。':!checked?'请完成当前前图/实时画面及本次准备确认。':mounted&&!zero?'先完成卸载归中,按CAD装回后保存本轴1500 μs的真实0°点。':!jogStageOK?'请确认本阶段已卸载,或已装回零位并检查行程。':'可以开始本轴一次点测;首个1500 μs也会转动。');
const finished=j&&j.type==='jog'&&j.status==='completed'&&j.cleanup_verified===true&&j.pin===pin;const frame=(state.frames||[]).find(f=>f.job_id===j?.id&&f.role==='phase');
text('measurementJob',j?.type==='jog'?'任务 '+j.id+' · '+(j.pin===32?'下轴':'上轴')+' · 目标 '+j.target_us+' μs · '+j.status:'先完成一次本轴点测。');
$('recordZero').disabled=busy||!finished||j.calibration_stage!=='unloaded_center'||!$('zeroMounted').checked;
const measured=$('measuredAngle').value.trim();$('recordAngle').disabled=busy||!finished||j.calibration_stage!=='mounted_measurement'||!$('zeroMounted').checked||!measured||!Number.isFinite(Number(measured));
['directionPositive','directionNegative','directionNone'].forEach(id=>$(id).disabled=busy||!finished);
const allPoints=state.motion_status?.calibration_points||{};text('calibrationPoints',[['32','下轴'],['33','上轴']].map(([key,name])=>name+'实测:\n'+((allPoints[key]||[]).map(p=>p.pulse_us+' μs → '+p.measured_deg+'°').join('\n')||'尚无记录')).join('\n\n'));const direction=(state.observations||[]).filter(o=>o.job_id===j?.id&&o.physical_result).slice(-1)[0];text('directionResult',direction?'已记录:'+direction.note:'方向观察尚未记录;不代替实际量角。');const coverage=ms.calibration_coverage||{};text('servoProfileState',JSON.stringify({profile:ms.servo_profile,coverage},null,2));
$('servoSave').disabled=busy||!connected||!installed||!$('servoSaveConfirm').checked||!$('benchReady').checked;
$('recoverStart').disabled=busy||!checked||ms.recover_ready!==true||!$('fallenStationary').checked;text('recoverGate',ms.recover_ready!==true?'尚未通过板端爬起门禁;先完成无PWM验证及两轴真实−60°…+60°覆盖。':!checked?'请更新当前全景并完成现场确认。':!$('fallenStationary').checked?'请轻放倒并静止,再确认当前前图对应此姿态。':'可主动开始一次最长8秒爬起尝试,不会自动重试。');
const windowEvent=(j?.events||[]).filter(e=>e.event==='phase'&&e.phase==='policy_running').slice(-1)[0];if(motionCountdown)text('motionCountdown',motionCountdown+' 秒后开始;急停可取消');else if(activeMotion(j)&&windowEvent)text('motionCountdown','指令窗口剩余约 '+Math.max(0,(windowEvent.max_pwm_seconds||8)-(Date.now()/1000-windowEvent.received_at)).toFixed(1)+' 秒(软件回读计时)');else text('motionCountdown',activeMotion(j)?'板端耗时 '+Number(j.elapsed_s||0).toFixed(1)+' 秒':'');
const phaseLabels={awaiting_first_heartbeat:'等待本页保活,尚未启动',preflight:'板端前置检查',quiet_initialization:'静止初始化',fresh_stationary_bias:'采集本轮静止零偏',policy_running:'单次爬起策略输出',ramp:'慢速过渡到目标',initial_command:'首次1500 μs归中',increment:'目标保持,请观察量角',return_command:'返回1500 μs',sampling:'实际IMU采样/控制',recovering:'爬起策略输出',finished:'流程结束',transport_failed:'连接中断,清理待确认'};text('motionPhase',j?'任务 '+j.id+' · '+j.type+' · '+j.status+' · 阶段 '+(phaseLabels[j.phase]||j.phase||'等待板端')+(j.error?' · '+j.error:''):'没有动作任务;不会自动启动。');
const result=j?.result||{},sensor=result.sensor||{};const compact={termination_reason:result.termination_reason||result.stop_reason||j?.error,metrics:result.metrics,imu_upright_candidate:result.imu_upright_candidate,control_count:result.controls?.length||0,sample_count:result.samples?.length||sensor.sample_count,readback_count:result.readbacks?.length||0,sensor_summary:{host_sample_hz:sensor.host_sample_hz,counter_drops:sensor.counter_drops,max_host_gap_s:sensor.max_host_gap_s,stream_error:sensor.stream_error,cleanup_verified:sensor.cleanup_verified},cleanup_verified:j?.cleanup_verified};text('motionLive',j?JSON.stringify({sample:j.latest_sample,control:j.latest_control,result:compact},null,2):'等待用户主动操作。');if($('motionDetailsPanel').open)text('motionDetails',JSON.stringify({module:ms,job:j||null},null,2));
if(frame){$('targetPreview').src=frame.url;$('targetPreview').hidden=false;text('targetPhotoState','真实目标阶段照片 · '+frame.id+';按相机拍摄,不等于硬件同步测角。')}else{$('targetPreview').hidden=true;text('targetPhotoState',finished?'目标阶段照片缺失;不要把前图说成目标角照片。':'等待目标保持阶段;相机模式会自动留图。')}
$('recordStanding').disabled=busy||!j||j.type!=='recover'||!$('standingObserved').checked;
const observed=(state.observations||[]).filter(o=>o.job_id===j?.id&&o.standing_confirmed===true).slice(-1)[0];text('standingResult',observed?'已记录用户现场确认:底板着地并站稳。不是自动判定的实机成功。':j?.result?.imu_upright_candidate===true?'IMU姿态达标,请现场确认底板是否着地并站稳。':'IMU姿态不能独立证明接触地面和站稳;尚无用户站稳确认。');
if(activeMotion(j)&&motionOwned===j.id&&activeObservationMode==='camera'&&!physicalCameraReady())stopMotion();
captureTargetPhase(j).catch(e=>text('targetPhotoState','目标照片未保存:'+e.message));
if(j&&!activeMotion(j)&&awaitedMotion===j.id){motionOwned=null;awaitedMotion=null;$('fallenStationary').checked=false;if(lastMotionFinished!==j.id){lastMotionFinished=j.id;if(physicalCameraReady())captureMotionFrame('after',j.id).then(f=>showFrame('afterPreview',f)).catch(e=>text('notice','后图缺失:'+e.message));}}
}
function showFrame(id,frame){$(id).src=frame.url;$(id).hidden=false}
async function openPhysicalCamera(){if(cameraPending||activeMotion(motionJob()))return;const generation=++cameraGeneration;closePhysicalCamera(false);cameraPending=true;renderMotion();try{const stream=await navigator.mediaDevices.getUserMedia({video:{width:{ideal:1280},height:{ideal:720}},audio:false});if(generation!==cameraGeneration){stream.getTracks().forEach(t=>t.stop());return}cameraStream=stream;$('motionCamera').srcObject=stream;await $('motionCamera').play();if(generation!==cameraGeneration)return;stream.getVideoTracks()[0].addEventListener('ended',()=>{cameraFrameAt=0;if(motionOwned)stopMotion()});if('requestVideoFrameCallback' in $('motionCamera')){const tick=()=>{if(generation!==cameraGeneration||!cameraStream)return;cameraFrameAt=performance.now();$('motionCamera').requestVideoFrameCallback(tick)};$('motionCamera').requestVideoFrameCallback(tick)}}catch(e){if(generation===cameraGeneration)text('notice','相机未打开:'+e.message+';可上传当前全景照片并现场观察。')}finally{if(generation===cameraGeneration)cameraPending=false;renderMotion()}}
function closePhysicalCamera(invalidate=true){if(invalidate){clearMotionChecks();cameraGeneration++;cameraPending=false;if(motionOwned&&!$('modePhoto').checked)stopMotion()}if(cameraStream)cameraStream.getTracks().forEach(t=>t.stop());cameraStream=null;cameraFrameAt=0;$('motionCamera').srcObject=null;renderMotion()}
async function captureMotionFrame(role,jobId){if(!physicalCameraReady())throw Error('相机画面缺失或陈旧');const v=$('motionCamera'),c=document.createElement('canvas'),scale=Math.min(1,1280/v.videoWidth);c.width=Math.round(v.videoWidth*scale);c.height=Math.round(v.videoHeight*scale);c.getContext('2d').drawImage(v,0,0,c.width,c.height);return request('frame',{role,job_id:jobId,phase:role==='phase'?'increment':undefined,source:'camera',image:c.toDataURL('image/jpeg',.85)})}
async function uploadPhoto(file,role){if(!file)throw Error('请选择当前照片');if(file.size>20000000)throw Error('原始照片超过20MB');const encoded=await new Promise((resolve,reject)=>{const r=new FileReader();r.onload=()=>resolve(r.result);r.onerror=reject;r.readAsDataURL(file)}),image=await new Promise((resolve,reject)=>{const i=new Image();i.onload=()=>resolve(i);i.onerror=()=>reject(Error('照片无法解码'));i.src=encoded});const c=document.createElement('canvas'),scale=Math.min(1,1280/image.naturalWidth);c.width=Math.round(image.naturalWidth*scale);c.height=Math.round(image.naturalHeight*scale);c.getContext('2d').drawImage(image,0,0,c.width,c.height);const job=motionJob();if(role==='after'&&(!job||activeMotion(job)))throw Error('先等待当前动作结束');const frame=await request('frame',{role,source:'uploaded_current_photo',image:c.toDataURL('image/jpeg',.85),job_id:role==='after'?job.id:undefined});if(role==='before'){uploadedBefore=frame;$('modePhoto').checked=true;$('modeCamera').checked=false;showFrame('beforePreview',frame)}else showFrame('afterPreview',frame);await poll();renderMotion()}
async function captureTargetPhase(job){if(!job||job.type!=='jog'||job.status!=='running'||job.phase!=='increment'||motionOwned!==job.id||!physicalCameraReady()||phasePhotoPending.has(job.id))return;const event=(job.events||[]).filter(e=>e.event==='phase'&&e.phase==='increment').slice(-1)[0];if(!event||Date.now()/1000-event.received_at<.65)return;phasePhotoPending.add(job.id);await captureMotionFrame('phase',job.id);await poll()}
async function beginMotion(kind){if(pending||motionCountdown||activeMotion(motionJob())||busySensor())return;renderMotion();if($(kind==='recover'?'recoverStart':'jogStart').disabled)return;const operation=++motionOperation;pending=true;motionLaunching=true;motionCountdown=3;render();motionTimer=setInterval(async()=>{if(operation!==motionOperation||!motionCountdown)return;motionCountdown--;renderMotion();if(motionCountdown)return;clearInterval(motionTimer);try{if(operation!==motionOperation||!physicalChecked()||!observationReady())throw Error('当前前图/现场确认已变化,取消尚未开始的动作');const frame=$('modePhoto').checked?uploadedBefore:await captureMotionFrame('before');if(operation!==motionOperation||!physicalChecked()||!observationReady())throw Error('已停止或取景丢失,取消尚未开始的动作');showFrame('beforePreview',frame);const ms=state.motion_status||{},payload={type:kind,request_id:crypto.randomUUID(),before_frame_id:frame.id,observation_mode:$('modePhoto').checked?'photo_operator':'camera',camera_confirmed:$('benchReady').checked,clearance_confirmed:$('benchReady').checked,reference_confirmed:$('benchReady').checked,operator_observing:$('benchReady').checked,external_power_confirmed:$('benchReady').checked};if(kind==='jog')Object.assign(payload,{pin:Number($('jogAxis').value),target_us:jogTarget,calibration_stage:$('jogStage').value,mechanically_unloaded:$('jogUnloaded').checked,mounted_clearance_confirmed:$('jogMounted').checked});else Object.assign(payload,{servo_profile_sha256:ms.servo_profile?.sha256,installation_profile_sha256:ms.installation_profile?.sha256,gently_placed_fallen_stationary:$('fallenStationary').checked});const job=await request('motion/start',payload);if(operation!==motionOperation){await request('motion/stop',{});throw Error('启动期间已停止,等待板端清理')}motionOwned=job.id;awaitedMotion=job.id;activeObservationMode=payload.observation_mode;if(uploadedBefore?.id===frame.id)uploadedBefore.job_id=job.id;state.motion={...(state.motion||{}),job};$('measuredAngle').value='';$('standingObserved').checked=false;await request('motion/heartbeat',{id:job.id});text('notice','一次任务已提交,正在等待板端阶段/清理回读。')}catch(e){text('notice',e.message);clearMotionChecks()}finally{pending=false;motionLaunching=false;render();}},1000)}
async function stopMotion(){if(typeof cancelSpeedLease==='function')cancelSpeedLease();motionOperation++;if(motionLaunching)pending=false;motionLaunching=false;motionCountdown=0;if(motionTimer)clearInterval(motionTimer);motionOwned=null;clearMotionChecks();text('motionCountdown','停止请求中;等待板端清理回读');try{await request('motion/stop',{});await poll()}catch(e){text('notice','停止未确认:'+e.message+';等待板端租约超时,必要时切断舵机电源。')}render()}
async function recordPoint(zero){const job=motionJob();if(!job||job.type!=='jog'||job.status!=='completed'||!job.cleanup_verified||!$('zeroMounted').checked)return;try{if(zero&&physicalCameraReady()){const after=await captureMotionFrame('after',job.id);showFrame('afterPreview',after)}const frame=(state.frames||[]).find(f=>f.job_id===job.id&&f.role==='before');if(!frame)throw Error('此任务缺少真实照片');const angle=zero?0:Number($('measuredAngle').value);if(!zero&&(!$('measuredAngle').value.trim()||!Number.isFinite(angle)))throw Error('请输入量角器读到的实际角度');await action('motion/calibration-point',{request_id:job.id,pin:job.pin,pulse_us:zero?1500:job.target_us,measured_deg:angle,zero_mounted_confirmed:$('zeroMounted').checked,operator_measured:true,frame_id:frame.id});$('measuredAngle').value='';text('notice','已保存用户现场量角记录;前图只用于准备情境关联,不自动验证角度。下一步只允许在已记录点旁边±50 μs。')}catch(e){text('notice',e.message)}}
async function recordDirection(direction){const job=motionJob();if(!job||job.type!=='jog'||activeMotion(job))return;await action('motion/observation',{job_id:job.id,physical_result:direction==='uncertain'?'uncertain':'moved',note:'用户观察目标相对归中姿态方向:'+direction+';方向观察不代替实际关节角测量。'})}
$('motionDetailsPanel').addEventListener('toggle',renderMotion);$('cameraOpen').addEventListener('click',openPhysicalCamera);$('cameraClose').addEventListener('click',()=>closePhysicalCamera());$('currentPhoto').addEventListener('change',e=>uploadPhoto(e.target.files[0],'before').catch(e=>text('notice',e.message)));$('afterPhoto').addEventListener('change',e=>uploadPhoto(e.target.files[0],'after').catch(e=>text('notice',e.message)));
[...motionChecks,'modePhoto','modeCamera','jogUnloaded','jogMounted','zeroMounted','servoSaveConfirm','fallenStationary','standingObserved'].forEach(id=>$(id).addEventListener('change',renderMotion));$('measuredAngle').addEventListener('input',renderMotion);
['jogAxis','jogStage'].forEach(id=>$(id).addEventListener('change',()=>{jogTarget=1550;pointSignature='';$('zeroMounted').checked=false;$('measuredAngle').value='';clearMotionChecks();renderMotion()}));
$('pulseMinus').addEventListener('click',()=>{jogTarget=Math.max(500,Number($('pointBase').value||1500)-50);$('fallenStationary').checked=false;renderMotion()});$('pulsePlus').addEventListener('click',()=>{jogTarget=Math.min(2500,Number($('pointBase').value||1500)+50);$('fallenStationary').checked=false;renderMotion()});
$('jogStart').addEventListener('click',()=>beginMotion('jog'));$('recoverStart').addEventListener('click',()=>beginMotion('recover'));$('motionStop').addEventListener('click',stopMotion);$('recordZero').addEventListener('click',()=>recordPoint(true));$('recordAngle').addEventListener('click',()=>recordPoint(false));$('directionPositive').addEventListener('click',()=>recordDirection('沿图示正向'));$('directionNegative').addEventListener('click',()=>recordDirection('沿图示反向'));$('directionNone').addEventListener('click',()=>recordDirection('uncertain'));
$('servoSave').addEventListener('click',()=>action('motion/save-servo',{operator_confirmed:$('servoSaveConfirm').checked,external_power_confirmed:$('benchReady').checked}));$('shadowConfirm').addEventListener('change',renderMotion);$('recordStanding').addEventListener('click',()=>action('motion/observation',{job_id:motionJob()?.id,standing_confirmed:$('standingObserved').checked,note:'用户现场确认底板接触地面且已站稳;不是IMU自动判定。'}));
setInterval(()=>{if(cameraStream&&!('requestVideoFrameCallback' in $('motionCamera'))&&$('motionCamera').currentTime!==cameraLastTime){cameraLastTime=$('motionCamera').currentTime;cameraFrameAt=performance.now()}renderMotion()},400);
setInterval(async()=>{if(motionOwned&&!motionHeartbeatBusy){const id=motionOwned;motionHeartbeatBusy=true;try{if(!physicalChecked()||(activeObservationMode==='camera'&&!physicalCameraReady()))throw Error('现场观察或画面丢失');await request('motion/heartbeat',{id})}catch(e){if(motionOwned===id){text('notice','保活失败,已请求停止:'+e.message);await stopMotion()}}finally{motionHeartbeatBusy=false}}},700);
document.addEventListener('visibilitychange',()=>{if(document.visibilityState!=='visible'){stopCadDemo();clearMotionChecks();if(motionOwned||motionCountdown||pending)stopMotion()}});window.addEventListener('pagehide',()=>{motionOperation++;if(motionOwned&&state?.csrf_token)fetch('/api/imu/motion/stop',{method:'POST',headers:{'Content-Type':'application/json','X-Cal-Token':state.csrf_token},body:'{}',keepalive:true}).catch(()=>{})});renderMotion();
async function recordExistingZero(pin){
if(pending||activeMotion(motionJob())||busySensor())return;
pending=true;render();
try{
const full=await request('export');
const job=(full.motion?.jobs||[]).filter(j=>j.type==='jog'&&j.pin===pin&&j.status==='completed'&&j.cleanup_verified===true).sort((a,b)=>(b.created_epoch||0)-(a.created_epoch||0))[0];
if(!job)throw Error('这个轴没有已完成的回中记录,请先做一次小幅点动。');
const frame=(full.frames||[]).find(f=>f.job_id===job.id&&f.role==='before');
if(!frame)throw Error('回中任务缺少关联照片,不能使用这条记录。');
await request('motion/calibration-point',{request_id:job.id,pin,pulse_us:1500,measured_deg:0,frame_id:frame.id,zero_mounted_confirmed:true,operator_measured:true});
state=await request('state');$('jogAxis').value=String(pin);$('jogStage').value='mounted_measurement';jogTarget=1550;pointSignature='';clearMotionChecks();
text('quickZeroState','已按你的当前位置确认记录零位。接下来小幅点动并测安全范围;没有执行舵机动作。');
}catch(e){text('quickZeroState',e.message)}finally{pending=false;render();}
}
$('quickZeroLower').addEventListener('click',()=>recordExistingZero(32));
$('quickZeroUpper').addEventListener('click',()=>recordExistingZero(33));

632
calibration/imu_server.py Normal file
View File

@ -0,0 +1,632 @@
"""Loopback X5 calibration and explicitly requested, leased SG90 attempts."""
import argparse
import base64
import copy
from datetime import datetime, timezone
import hashlib
from http.server import BaseHTTPRequestHandler, ThreadingHTTPServer
import json
from pathlib import Path
import secrets
import shlex
import subprocess
import threading
import time
import uuid
import numpy as np
from .desk_camera import DeskCamera
from .bridge import Bridge
from .server import atomic_json
from .motion_bridge import MotionBridge
from .speed_bench import SpeedBench
ROOT = Path(__file__).resolve().parent.parent
REMOTE = '/home/sunrise/x5_imu_calibration_20261112'
DATA = ROOT / 'calibration/imu_data_20261112'
POSES = ('flat', 'x_up', 'y_up')
MOTION_SCRIPT = 'v2/motion_cli.py'
def digest(value):
return hashlib.sha256(json.dumps(value, sort_keys=True, separators=(',', ':'),
allow_nan=False).encode()).hexdigest()
def stationary(capture):
if (capture.get('status') != 'sampled' or capture.get('sensor_config') !=
{'odr_hz': 224.2, 'gyro_dps': 1024, 'accel_g': 8}):
raise ValueError('采样状态或传感器配置与本次模型不一致')
if capture.get('configuration_restored') is not True or capture.get('pwm_written') is not False:
raise ValueError('采样未确认恢复可读配置及无 PWM 写入')
samples = capture.get('samples', [])
if len(samples) < 100:
raise ValueError('静止数据不足100帧,请重新采样')
accel = np.asarray([s['accel_g'] for s in samples], dtype=float)
gyro = np.asarray([s['gyro_dps'] for s in samples], dtype=float)
times = np.asarray([s['elapsed_s'] for s in samples], dtype=float)
if accel.shape != (len(samples), 3) or gyro.shape != accel.shape or not all(
np.isfinite(a).all() for a in (accel, gyro, times)):
raise ValueError('六轴数据必须为有限三维向量')
dt = np.diff(times)
norms = np.linalg.norm(accel, axis=1)
if (times[-1] - times[0] < 1 or np.any(dt <= 0) or np.max(dt) > .15
or np.min(norms) < .85 or np.max(norms) > 1.15
or np.max(np.std(accel, axis=0)) >= .02
or np.max(np.std(gyro, axis=0)) >= .5
or np.max(np.abs(np.mean(gyro, axis=0))) >= 10):
raise ValueError('未通过静止筛查:保持该姿态不动,避免手抖/桌面振动后重采')
return np.mean(accel, axis=0), np.mean(gyro, axis=0)
def candidate(captures, model_candidate_sha256=None):
if any(p not in captures for p in POSES):
return None
means = {p: stationary(captures[p]['result']) for p in POSES}
unit = {p: a / np.linalg.norm(a) for p, (a, _) in means.items()}
measured = np.column_stack([unit['x_up'], unit['y_up'], unit['flat']])
dots = [float(np.dot(measured[:, a], measured[:, b])) for a, b in ((0, 1), (0, 2), (1, 2))]
if np.linalg.det(measured) <= 0 or max(abs(d) for d in dots) > np.sin(np.deg2rad(15)):
raise ValueError('三个姿态方向不正交或正负号反了;确认 +X/+Y 端朝上,重新采样')
u, _, vt = np.linalg.svd(measured)
rotation = (u @ vt).T
errors = [float(np.rad2deg(np.arccos(np.clip(np.dot(rotation @ measured[:, i],
np.eye(3)[:, i]), -1, 1)))) for i in range(3)]
if max(errors) > 15 or any(np.max(np.abs(g - means['flat'][1])) > 1 for _, g in means.values()):
raise ValueError('姿态拟合误差或静止零偏变化过大,请重采')
value = {'schema': 'qmi8658c_installation_v1',
'gyro_bias_dps': means['flat'][1].tolist(),
'sensor_to_body': rotation.tolist(),
'reference_accel_g': means['flat'][0].tolist(),
'reference_samples': captures['flat']['result']['samples'],
'source_capture_ids': {p: captures[p]['id'] for p in POSES},
'source_capture_sha256': {p: captures[p]['result']['output_sha256'] for p in POSES},
'sensor_config': {'odr_hz': 224.2, 'gyro_dps': 1024, 'accel_g': 8},
'pose_fit_error_deg': dict(zip(('x_up', 'y_up', 'flat'), errors)),
'accelerometer_bias_calibrated': False,
'model_candidate_sha256': model_candidate_sha256,
'created_at': datetime.now(timezone.utc).isoformat()}
value['calibration_candidate_sha256'] = digest(value)
return value
class App:
def __init__(self, transport, data=DATA, continuous=None):
self.transport, self.data = transport, Path(data)
self.continuous=copy.deepcopy(continuous)
self.autonomous = {}
self.data.mkdir(parents=True, exist_ok=True)
self.token = secrets.token_urlsafe(32)
self.lock = threading.RLock()
self.device = {'connected': False, 'error': '正在读取 X5 状态', 'read_at': None}
self.captures, self.proposal, self.saved, self.job = {}, None, None, None
self.candidate_error = None
self.frames, self.observations, self.preflight = [], [], None
self.motion_status = {}
session = self.data / 'session.json'
if session.is_file():
saved = json.loads(session.read_text())
self.captures = saved.get('captures', {})
self.proposal = saved.get('candidate')
self.saved = saved.get('saved')
self.job = saved.get('job')
self.frames = saved.get('frames', [])
self.observations = saved.get('observations', [])
self.preflight = saved.get('preflight')
if self.job and self.job['status'] == 'running':
self.job.update(status='failed', error='服务重启,旧任务结果未知;不会自动重放')
self.motion = (SpeedBench(transport, self.data/'speed-motion', continuous,
ROOT/'deployment/x5_continuous_speed_20261112/bench_receipts')
if continuous else MotionBridge(transport, self.data/'motion'))
def persist(self):
with self.lock:
atomic_json(self.data / 'session.json', self.export())
def export(self):
with self.lock:
return copy.deepcopy({'captures': self.captures, 'candidate': self.proposal,
'saved': self.saved, 'job': self.job, 'frames': self.frames,
'observations': self.observations, 'preflight': self.preflight,
'motion': self.motion.export()})
def command(self, args, payload=None, timeout=15, script='board_tool.py', allow_failure=False):
cmd = 'sudo -n python3 ' + REMOTE + '/' + script + ' ' + shlex.join(args)
run = subprocess.run(self.transport._ssh(cmd), input=None if payload is None else json.dumps(payload),
text=True, capture_output=True, timeout=timeout, check=False)
try:
value = json.loads(run.stdout)
except ValueError:
raise RuntimeError((run.stderr or run.stdout or '板端未返回JSON结果')[-2000:])
if not isinstance(value, dict):
raise RuntimeError('板端结果必须为JSON对象')
if run.returncode and not allow_failure:
raise RuntimeError(str(value.get('error') or value.get('errors') or run.stderr or '板端命令失败')[-2000:])
return value
def refresh(self):
with self.lock:
if self.job and self.job['status'] == 'running' or self.motion._worker_active:
return
try:
value = self.command(['status'], timeout=10)
if value.get('identity') != 'D-Robotics RDK X5 V1.0':
raise ValueError('连接的设备不是已核验的 RDK X5 V1.0')
value.update(connected=True, read_at=time.time(), error=None)
if self.continuous:
result = subprocess.run(self.transport._ssh("systemctl is-active x5-selfright.service; cat /run/x5-selfright.json 2>/dev/null || true"),capture_output=True,text=True,timeout=5,check=True)
line, _, data = result.stdout.partition('\n')
self.autonomous = dict(json.loads(data) if data.strip() else {}, active=line.strip() in ('active','activating','deactivating'))
except Exception as exc:
value = {'connected': False, 'read_at': time.time(), 'error': str(exc)}
with self.lock:
self.device = value
try:
motion = self.command(['status'], script=MOTION_SCRIPT, timeout=10)
except Exception as exc:
motion = {'installed': False, 'error': str(exc)}
with self.lock:
self.motion_status = motion
self.motion.device = copy.deepcopy(self.device)
self.motion.prepared = motion.get('installed') is True
remote_profile = motion.get('installation_profile')
candidate_matches = (not self.proposal or remote_profile and
remote_profile.get('calibration_candidate_sha256') == self.proposal.get('calibration_candidate_sha256'))
if not self.saved and candidate_matches and remote_profile and remote_profile.get('sha256'):
self.saved = dict(saved=True, sha256=remote_profile['sha256'],
profile_path=REMOTE+'/calibration_profile.json', restored_from_board=True)
def state(self):
with self.lock:
return copy.deepcopy({'device': self.device, 'job': self.job,
'captures': {p: {k: v for k, v in record.items() if k != 'result'} for p, record in self.captures.items()},
'candidate': None if self.proposal is None else {k: v for k, v in self.proposal.items() if k != 'reference_samples'},
'candidate_error': self.candidate_error, 'saved': self.saved,
'motion': self.motion.status(), 'motion_status': self.motion_status,
'frames': self.frames, 'observations': self.observations, 'preflight': self.preflight,
'csrf_token': self.token, 'server_time': time.time(),
'continuous_deployment':self.continuous, 'autonomous':self.autonomous,
'last_speed_trial': self.motion.last_trial() if self.continuous else None,
'speed_axes': self.motion.axes if self.continuous else {},
'speed_replay': (self.motion.job['id'] if self.continuous and self.motion.job and
(self.motion.output/(self.motion.job['id']+'.mp4')).is_file() else None),
'mode': 'continuous_speed_web' if self.continuous else 'guarded_sg90_bench',
'motion_enabled': self.motion.prepared and (not self.continuous or bool(self.continuous.get('passed') and not self.continuous.get('supply_hold')))})
def start(self, payload):
kind, pose = payload.get('type'), payload.get('pose')
if self.continuous and kind in ('shadow','preflight'):
raise ValueError('旧定位策略的验证已停用;新360速度策略的板端验收见当前部署结果')
if kind not in ('capture', 'shadow', 'preflight') or payload.get('pose_confirmed') is not True:
raise ValueError('请先确认按图摆放并保持不动')
if kind == 'preflight' and payload.get('operator_observing') is not True:
raise ValueError('请确认操作者在现场观察并保持静止')
if kind == 'capture' and pose not in POSES:
raise ValueError('未知姿态')
with self.lock:
if self.job and self.job['status'] == 'running' or self.motion._worker_active:
raise ValueError('已有采样/推理任务,请等待结束')
if not self.device.get('connected') or time.time() - self.device.get('read_at', 0) > 20:
raise ValueError('设备未连接或状态已过期,请先刷新状态')
if self.device.get('pwm'):
raise ValueError('检测到已导出的 PWM,请先检查既有任务')
if kind in ('shadow', 'preflight') and not self.saved:
raise ValueError('请先完成三个姿态并明确保存本次安装校准')
ident = str(uuid.uuid4())
self.job = {'id': ident, 'type': kind, 'pose': pose, 'status': 'running',
'started_at': time.time(), 'latest_sample': None, 'error': None}
target = self.execute_preflight if kind == 'preflight' else self.execute
self.persist()
threading.Thread(target=target, args=(copy.deepcopy(self.job),), daemon=True).start()
return self.state()
def execute_preflight(self, job):
try:
result = self.command(['arm-check'], payload={'request_id':job['id'], 'stationary_confirmed':True, 'operator_observing':True}, script=MOTION_SCRIPT, timeout=25, allow_failure=True)
atomic_json(self.data / (job['id']+'.json'), result)
summary = {k:v for k,v in result.items() if k not in ('samples','controls')}
summary.update(sample_count=len(result.get('samples', [])),
control_count=len(result.get('controls', [])), local_evidence_file=job['id']+'.json')
with self.lock:
self.preflight = summary
passed = (result.get('passed') is True and result.get('status') == 'passed'
and result.get('cleanup_verified') is True and result.get('pwm_written') is False)
self.job.update(status='completed' if passed else 'failed', result=summary,
error=None if passed else (result.get('error') or '前置检查或清理未通过,请查看具体阻断项'),
completed_at=time.time())
except Exception as exc:
with self.lock:
self.job.update(status='failed', error=str(exc), completed_at=time.time())
finally:
self.persist()
self.refresh()
def execute(self, job):
args = [job['type'], '--seconds', '8', '--output', job['id'] + '.json', '--stream']
if job['type'] == 'shadow':
args += ['--calibration', REMOTE + '/calibration_profile.json']
command = 'sudo -n python3 -u ' + REMOTE + '/board_tool.py ' + shlex.join(args)
proc, errors, terminal = None, [], None
try:
proc = subprocess.Popen(self.transport._ssh(command), stdout=subprocess.PIPE,
stderr=subprocess.PIPE, text=True, bufsize=1)
timer = threading.Timer(25, proc.kill)
timer.start()
threading.Thread(target=lambda: errors.append(proc.stderr.read(16000)), daemon=True).start()
try:
for line in proc.stdout:
event = json.loads(line)
if event.get('event') == 'sample':
with self.lock:
self.job['latest_sample'] = event['sample']
self.job['sample_received_at'] = time.time()
elif event.get('event') == 'control':
with self.lock:
self.job['latest_control'] = {k: v for k, v in event['control'].items() if k != 'observation'}
elif event.get('event') == 'result':
terminal = event['result']
code = proc.wait(timeout=2)
finally:
timer.cancel()
if terminal is None:
raise RuntimeError('板端任务未正常完成:' + ''.join(errors)[-1000:])
atomic_json(self.data / (job['id'] + '.json'), terminal)
summary = {k: v for k, v in terminal.items() if k not in ('samples', 'controls')}
summary['control_count'] = len(terminal.get('controls', []))
with self.lock:
self.job['result'] = summary
if code != 0 or terminal.get('status') == 'failed':
raise RuntimeError('板端任务未通过:' + terminal.get('error', ''.join(errors)[-1000:]))
if terminal.get('pwm_written') is not False or terminal.get('configuration_restored') is not True:
raise RuntimeError('板端未确认可读配置恢复/无 PWM 写入,结果不可用于校准')
with self.lock:
if job['type'] == 'capture':
stationary(terminal)
self.captures[job['pose']] = {'id': job['id'], 'sample_count': len(terminal['samples']),
'captured_at': time.time(), 'result': terminal, 'sha256': digest(terminal)}
self.proposal, self.saved, self.candidate_error = None, None, None
try:
self.proposal = candidate(self.captures, self.device.get('model', {}).get('candidate_sha256'))
except ValueError as exc:
self.candidate_error = str(exc)
if self.proposal:
atomic_json(self.data / 'candidate.json', self.proposal)
self.job.update(status='completed', result=summary,
completed_at=time.time())
except Exception as exc:
with self.lock:
self.job.update(status='failed', error=str(exc), completed_at=time.time())
finally:
if proc and proc.poll() is None:
proc.kill()
self.refresh()
self.persist()
def save(self, payload):
with self.lock:
if (not self.proposal or payload.get('calibration_candidate_sha256') != self.proposal['calibration_candidate_sha256']
or payload.get('installation_confirmed') is not True):
raise ValueError('请确认安装固定,并保存当前候选;候选已变化时必须重新检查')
if self.job and self.job['status'] == 'running' or self.motion._worker_active:
raise ValueError('任务运行时不能修改校准')
value = self.command(['save-profile'], self.proposal)
if value.get('pwm_written') is not False:
raise RuntimeError('板端保存回执无效')
self.saved = value
atomic_json(self.data / 'saved-profile-receipt.json', value)
self.persist()
return value
def frame(self, payload):
role = payload.get('role')
if role not in ('before', 'after', 'phase') or payload.get('source') not in ('camera', 'uploaded_current_photo'):
raise ValueError('需要实际相机或用户上传的当前前后照片')
encoded = payload.get('image', '')
if not isinstance(encoded, str) or not encoded.startswith('data:image/jpeg;base64,'):
raise ValueError('需要 JPEG 图像')
raw = base64.b64decode(encoded.split(',', 1)[1], validate=True)
if len(raw) > 2_000_000 or not raw.startswith(b'\xff\xd8\xff') or not raw.endswith(b'\xff\xd9'):
raise ValueError('JPEG 文件无效或超过2MB')
with self.lock:
job_id = payload.get('job_id')
if role == 'before' and job_id:
raise ValueError('前图不能预先绑定到任务')
if role == 'after' and not any(j['id'] == job_id and j['status'] not in ('queued','running','stopping')
for j in self.motion.jobs):
raise ValueError('后图必须关联已结束的动作任务')
if role == 'phase':
job = self.motion.status().get('job')
if (not job or job['id'] != job_id or job['status'] != 'running'
or job.get('phase') != 'increment' or payload.get('phase') != 'increment'):
raise ValueError('目标保持阶段已变化,不能将照片绑定到旧阶段')
event = next((e for e in reversed(job.get('events', []))
if e.get('event') == 'phase' and e.get('phase') == 'increment'), None)
if not event or time.time()-event['received_at'] < .5:
raise ValueError('目标尚未保持足够时间')
ident = str(uuid.uuid4())
value = dict(id=ident, role=role, job_id=job_id, source=payload['source'],
captured_at=time.time(), timestamp_kind='server_receive_time_not_camera_exposure_time',
sha256=hashlib.sha256(raw).hexdigest(), url='/frames/'+ident+'.jpg')
if role == 'phase':
value.update(phase='increment', hardware_synchronized=False)
(self.data/(ident+'.jpg')).write_bytes(raw)
self.frames.append(value)
self.persist()
return value
def start_motion(self, payload):
if self.continuous:
raise ValueError('当前是360连续旋转舵机,旧角度点测/爬起接口已停用;先完成速度模式停止点与方向验证')
with self.lock:
if self.job and self.job['status'] == 'running':
raise ValueError('请先等待当前传感器任务结束')
frame = next((f for f in self.frames if f['id'] == payload.get('before_frame_id')), None)
if (not frame or frame['role'] != 'before' or frame.get('job_id')
or time.time()-frame['captured_at'] > 15):
raise ValueError('需要15秒内当前前图;旧前图或另一任务前图不能复用')
if payload.get('observation_mode') == 'photo_operator' and frame['source'] != 'uploaded_current_photo':
raise ValueError('请主动上传当前照片并确认现场持续观察')
if payload.get('observation_mode') == 'camera' and frame['source'] != 'camera':
raise ValueError('实时相机模式需要实际相机前图')
job = self.motion.start(payload)
frame['job_id'] = job['id']
self.persist()
return job
def autonomous_command(self, action):
if action not in ('start','stop') or not self.continuous:
raise ValueError('Unknown autonomous command')
if action == 'start' and (self.motion._worker_active or (self.job or {}).get('status')=='running'):
raise ValueError('请先等待当前实验结束')
cmd = 'sudo -n systemctl ' + ('enable --now' if action=='start' else 'disable --now') + ' x5-selfright.service'
proc = subprocess.run(self.transport._ssh(cmd), capture_output=True,text=True,timeout=25)
if proc.returncode:
raise RuntimeError(proc.stderr[-500:])
self.autonomous = dict(active=action=='start',phase='starting' if action=='start' else 'stopped')
return self.autonomous
def stop_motion(self):
legacy=self.motion.stop()
if not self.continuous:return legacy
self.autonomous_command('stop')
command='sudo -n python3 /home/sunrise/x5_continuous_speed_20261112/bench_pulse.py stop'
run=subprocess.run(self.transport._ssh(command),capture_output=True,text=True,timeout=8)
if run.returncode:raise RuntimeError('速度模式停止请求未确认:'+run.stderr[-500:])
return dict(legacy=legacy,continuous=json.loads(run.stdout))
def save_servo(self, payload):
if self.continuous:raise ValueError('连续旋转舵机不能保存为旧角度映射')
with self.lock:
if self.motion._worker_active or self.job and self.job['status'] == 'running':
raise ValueError('任务执行期间不能修改舵机校准')
result = self.command(['save-servo-profile'], payload, script=MOTION_SCRIPT)
atomic_json(self.data/'servo-profile-receipt.json', result)
self.refresh()
self.persist()
return result
def calibration_point(self, payload):
if self.continuous:raise ValueError('连续旋转模式不使用1500微秒等于0度的定位标定')
with self.lock:
if self.motion._worker_active:
raise ValueError('先等待动作完成及 PWM 清理回读,再保存量角记录')
job = next((j for j in self.motion.jobs if j['id'] == payload.get('request_id')), None)
if not job or job['type'] != 'jog' or job['status'] != 'completed' or job.get('cleanup_verified') is not True:
raise ValueError('需要本次已完成且清理确认的真实单轴任务')
frame = next((f for f in self.frames if f['id'] == payload.get('frame_id')
and f.get('job_id') == job['id']), None)
if not frame:
raise ValueError('量角记录必须引用此任务的真实照片')
if payload.get('operator_measured') is not True or payload.get('zero_mounted_confirmed') is not True:
raise ValueError('请按 CAD 对齐零位并使用量角器测量,不能用未测默认值')
result = self.command(['save-calibration-point'], payload, script=MOTION_SCRIPT)
self.refresh()
self.persist()
return result
def observe_motion(self, payload):
with self.lock:
job = next((j for j in self.motion.jobs if j['id'] == payload.get('job_id')), None)
if not job or job['status'] in ('queued','running','stopping'):
raise ValueError('请选择已结束的真实动作任务')
if payload.get('standing_confirmed') is not True and payload.get('physical_result') not in ('moved','no_motion','uncertain'):
raise ValueError('请明确记录现场看到的结果')
if payload.get('standing_confirmed') is True and job['type'] != 'recover':
raise ValueError('站稳观察只能关联本次爬起任务')
record = {k: payload.get(k) for k in ('job_id','standing_confirmed','physical_result','note')}
if not isinstance(record.get('note') or '', str) or len(record.get('note') or '') > 2000:
raise ValueError('记录说明过长')
record.update(recorded_at=time.time(), source='user_observation_not_automatic_success')
self.observations.append(record)
self.persist()
return record
def handler_for(app, port):
hosts = {f'127.0.0.1:{port}', f'localhost:{port}'}
class Handler(BaseHTTPRequestHandler):
def log_message(self, fmt, *args):
print(self.command, self.path, fmt % args, flush=True)
def reply(self, status, value, kind='application/json; charset=utf-8', headers=()):
body = value if isinstance(value, bytes) else json.dumps(value, ensure_ascii=False, allow_nan=False).encode()
self.send_response(status)
for name, val in [('Content-Type', kind), ('Content-Length', str(len(body))),
('Cache-Control', 'no-store'), ('X-Content-Type-Options', 'nosniff'),
('X-Frame-Options', 'DENY'), *headers]:
self.send_header(name, val)
self.end_headers()
try:
self.wfile.write(body)
except (BrokenPipeError, ConnectionResetError):
pass
def trusted(self):
if (self.headers.get('Host') not in hosts or
self.headers.get('Origin') not in {None, *('http://' + h for h in hosts)} or
self.command == 'POST' and (self.headers.get('Sec-Fetch-Site') == 'cross-site' or
not secrets.compare_digest(self.headers.get('X-Cal-Token', ''), app.token))):
self.reply(403, {'error': '仅允许从本机当前校准页面操作'})
return False
return True
def do_GET(self):
if not self.trusted():
return
if self.path in ('/', '/imu', '/imu/'):
return self.reply(200, (ROOT / 'calibration/imu.html').read_bytes(), 'text/html; charset=utf-8')
if self.path == '/api/imu/state':
return self.reply(200, app.state())
if self.path == '/api/imu/export':
return self.reply(200, app.export())
if self.path == '/speed-bench.js':
return self.reply(200, (ROOT/'calibration/speed_bench.js').read_bytes(), 'text/javascript; charset=utf-8')
if self.path == '/desk-camera.js':
return self.reply(200, (ROOT/'calibration/desk_camera.js').read_bytes(), 'text/javascript; charset=utf-8')
if self.path == '/api/imu/camera.jpg':
if not secrets.compare_digest(self.headers.get('X-Cal-Token', ''), app.token) or self.headers.get('Sec-Fetch-Site') == 'cross-site':
return self.reply(403, {'error': '请在本机实机页面打开相机'})
try:
frame = self.server.camera.frame()
except OSError:
frame = None
if frame is None:
return self.reply(503, {'error': '等待桌面相机的新画面'})
image, captured_at = frame
return self.reply(200, image, 'image/jpeg', [('X-Frame-Time', str(captured_at))])
if self.path == '/imu-motion.js':
return self.reply(200, (ROOT/'calibration/imu_motion.js').read_bytes(), 'text/javascript; charset=utf-8')
if self.path.startswith('/frames/'):
name = self.path.removeprefix('/frames/')
with app.lock:
allowed = {f['id']+'.jpg' for f in app.frames}
if name in allowed:
return self.reply(200, (app.data/name).read_bytes(), 'image/jpeg')
return self.reply(404, {'error': '未知照片'})
if self.path.startswith('/speed-recordings/') and app.continuous:
name = self.path.removeprefix('/speed-recordings/')
if not any(name == j['id']+'.mp4' for j in app.motion.jobs):
return self.reply(404, {'error': '没有这次实验录像'})
clip = app.motion.output / name
if clip.is_file():
return self.reply(200, clip.read_bytes(), 'video/mp4')
return self.reply(404, {'error': '录像还未生成'})
recordings = {
'/autonomous-other-side.mp4':'auto-29a7c0b1-6e62-4aee-ab1b-676441f19c75.mp4',
'/autonomous-support.mp4':'auto-9098872b-1678-48b0-997b-5e0a4de90b57.mp4',
'/support-start.jpg':'support-start.jpg',
'/upper-support-steps.mp4': 'upper-support-steps.mp4',
'/upper-loaded-1550.mp4': '1ecac18c-86ee-4059-83e9-c0a4a6d5211c-load.mp4',
'/upper-loaded-1550.jpg': '1ecac18c-86ee-4059-83e9-c0a4a6d5211c-measurement.jpg',
'/lower-probe.mp4': 'a6f56223-835e-4cab-bc13-adbda6b60f1f-camera.mp4',
'/corrected-trial.mp4': '9acd1a04-a96b-4fea-a9ca-35a67937baaa-camera.mp4',
'/lower-flat-latest.mp4': '6748b741-2cdc-46c0-ae84-a71ff79b9401-camera.mp4',
'/lower90-comparison.mp4': 'lower90-comparison.mp4',
'/lower45-clockwise.mp4': 'lower45-clockwise.mp4',
'/lower45-comparison.jpg': 'lower45-comparison.jpg',
}
if self.path in recordings:
clip = ROOT/'deployment/x5_continuous_speed_20261112/bench_receipts'/recordings[self.path]
if clip.is_file():
return self.reply(200, clip.read_bytes(), 'image/jpeg' if clip.suffix == '.jpg' else 'video/mp4')
return self.reply(404, {'error': '实拍文件不可用'})
if self.path == '/imu-placement.png':
return self.reply(200, (ROOT / 'hardware_design/selfrising_sg90_large_plate_imu_20261111/output/imu_placement.png').read_bytes(), 'image/png')
if self.path == '/installation.md':
return self.reply(200, (ROOT / 'deployment/IMU_INSTALL_X5_20261112.md').read_bytes(), 'text/markdown; charset=utf-8')
if self.path == '/bench-test.md':
return self.reply(200, (ROOT / 'deployment/x5_imu_calibration_20261112/v2/BENCH_TEST.md').read_bytes(), 'text/markdown; charset=utf-8')
self.reply(404, {'error': '未知路径'})
def do_POST(self):
if not self.trusted():
return
try:
size = int(self.headers.get('Content-Length', 0))
if self.headers.get('Transfer-Encoding') or not 0 < size <= 3_000_000:
return self.reply(413, {'error': '请求体大小无效'})
if self.headers.get_content_type() != 'application/json':
raise ValueError('需要 JSON 请求')
payload = json.loads(self.rfile.read(size), parse_constant=lambda _: (_ for _ in ()).throw(ValueError('非有限数字')))
if not isinstance(payload, dict):
raise ValueError('需要 JSON 对象')
if self.path == '/api/imu/start':
return self.reply(200, app.start(payload))
if self.path == '/api/imu/save':
return self.reply(200, app.save(payload))
if self.path=='/api/imu/autonomous/start' and payload=={}:
with app.lock:
return self.reply(200,app.autonomous_command('start'))
if self.path in ('/api/imu/speed/start', '/api/imu/speed/observe'):
if app.autonomous.get('active'):
raise ValueError('X5 自主控制中;单次调试前请先停止自主运行')
if not app.continuous or app.continuous.get('supply_hold') or not app.continuous.get('passed'):
raise ValueError('360° 实验台未就绪')
with app.lock:
if (app.job or {}).get('status') == 'running':
raise ValueError('请先等 IMU 采样结束')
if self.path.endswith('/observe'):
return self.reply(200, app.motion.observe(payload))
if self.server.camera.frame() is None:
raise ValueError('实时相机还没有新画面')
if not app.saved or app.saved.get('sha256') != app.continuous.get('installation_sha256'):
raise ValueError('IMU 安装校准与此模型不一致')
return self.reply(200, app.motion.start(payload))
methods = {'/api/imu/frame': app.frame, '/api/imu/motion/start': app.start_motion,
'/api/imu/motion/heartbeat': lambda p: app.motion.heartbeat(p.get('id')),
'/api/imu/motion/stop': lambda p: app.stop_motion(),
'/api/imu/motion/calibration-point': app.calibration_point,
'/api/imu/motion/save-servo': app.save_servo,
'/api/imu/motion/observation': app.observe_motion}
if self.path in methods:
return self.reply(200, methods[self.path](payload))
if self.path == '/api/imu/refresh' and payload == {}:
app.refresh()
return self.reply(200, app.state())
self.reply(404, {'error': '未知接口;本页不提供舵机动作'})
except (ValueError, KeyError, TypeError) as exc:
self.reply(400, {'error': str(exc)})
except Exception as exc:
self.reply(409, {'error': str(exc)})
return Handler
def maintain(app):
while True:
app.refresh()
time.sleep(5)
class CalibrationHTTPServer(ThreadingHTTPServer):
def service_actions(self):
self.camera.release_if_idle()
def main():
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument('--port', type=int, default=8767)
args = parser.parse_args()
# Only reuse SSH transport: the old bridge's prepare/start are never invoked.
marker=ROOT/'deployment/x5_continuous_speed_20261112/DEPLOYED.json'
app = App(Bridge(output=DATA / 'transport', debug_only=True),
continuous=json.loads(marker.read_text()) if marker.exists() else None)
http = CalibrationHTTPServer(('127.0.0.1', args.port), handler_for(app, args.port))
http.camera = DeskCamera()
if app.continuous:
app.motion.camera = http.camera
http.daemon_threads = True
threading.Thread(target=maintain, args=(app,), daemon=True).start()
print(f'IMU-only calibration: http://127.0.0.1:{args.port}/', flush=True)
try:
http.serve_forever()
finally:
http.camera.close()
app.motion.stop()
app.persist()
http.server_close()
if __name__ == '__main__':
main()

145
calibration/index.html Normal file

File diff suppressed because one or more lines are too long

View File

@ -0,0 +1,79 @@
"""New SG90 protocol using the existing leased, bounded SSH task executor."""
import re
import uuid
from .bridge import Bridge
REMOTE = '/home/sunrise/x5_imu_calibration_20261112/v2/motion_cli.py'
def validate_motion(payload):
if not isinstance(payload, dict) or payload.get('type') not in ('jog', 'recover'):
raise ValueError('Only the current SG90 jog/recover protocol is permitted')
try:
request_id = str(uuid.UUID(payload.get('request_id', '')))
except (ValueError, TypeError, AttributeError):
raise ValueError('request_id must be a UUID')
flags = ('camera_confirmed', 'clearance_confirmed', 'reference_confirmed',
'operator_observing', 'external_power_confirmed')
if any(payload.get(k) is not True for k in flags):
raise ValueError('请确认当前取景、现场观察、独立供电、清场及首次归位风险')
if payload.get('observation_mode') not in ('camera', 'photo_operator'):
raise ValueError('需要实时相机或主动上传的当前照片与现场观察')
try:
uuid.UUID(payload.get('before_frame_id', ''))
except (ValueError, TypeError, AttributeError):
raise ValueError('需要当前动作前图')
value = {k: payload[k] for k in (*flags, 'type', 'observation_mode', 'before_frame_id')}
value['request_id'] = request_id
if payload['type'] == 'jog':
if (type(payload.get('pin')) is not int or payload['pin'] not in (32, 33)
or type(payload.get('target_us')) is not int or not 500 <= payload['target_us'] <= 2500
or payload['target_us'] % 50):
raise ValueError('单轴目标需在受限50 μs标定网格内;板端还会核验真人记录的相邻点')
stage = payload.get('calibration_stage')
if stage == 'unloaded_center':
if payload['target_us'] not in (1450, 1550) or payload.get('mechanically_unloaded') is not True:
raise ValueError('首次点测仅允许卸下舵盘负载后的单轴 1450/1550 μs')
value['mechanically_unloaded'] = True
elif stage == 'mounted_measurement':
if payload.get('mounted_clearance_confirmed') is not True:
raise ValueError('请确认已按CAD安装零位、当前机构有安全行程')
value['mounted_clearance_confirmed'] = True
else:
raise ValueError('请选择卸载归中或装回后的逐点量角阶段')
value.update(pin=payload['pin'], target_us=payload['target_us'], calibration_stage=stage)
else:
if payload.get('gently_placed_fallen_stationary') is not True:
raise ValueError('请轻放倒并静止,不能从高处抛摔')
value['gently_placed_fallen_stationary'] = True
for key in ('servo_profile_sha256', 'installation_profile_sha256'):
if not isinstance(payload.get(key), str) or not re.fullmatch('[a-f0-9]{64}', payload[key]):
raise ValueError('需要本次模型对应的安装与 SG90 实测校准')
value[key] = payload[key]
return value
class MotionBridge(Bridge):
def __init__(self, transport, output):
super().__init__(output=output)
self.transport = transport
self.runner_path = REMOTE
def _ssh(self, command):
return self.transport._ssh(command)
def _validate_payload(self, payload):
return validate_motion(payload)
def refresh(self):
# App performs read-only refresh for both IMU and motion status.
return self.status()
def export(self):
value = super().export()
value.pop('parameters_applied', None)
value.pop('policy_hardware_ready', None)
value['protocol'] = 'sg90_v2_explicit_leased_attempts'
value['physical_outcome_source'] = 'separate_user_observation'
return value

View File

@ -0,0 +1,299 @@
"""Bounded X5 calibration runner. Status is read-only; run requires stdin lease."""
import fcntl
import importlib.util
import json
import os
from pathlib import Path
import select
import signal
import sys
import threading
import time
import uuid
BOARD = 'D-Robotics RDK X5 V1.0'
LEASE_SECONDS = 2.5
MAX_SECONDS = 15
LOCK_FILE = Path('/tmp/x5-calibration-web.lock')
CALIBRATION_PULSES = (550, 1450, 1550, 2450) + tuple(p for p in range(900, 2101, 100) if p != 1500)
class Cancelled(RuntimeError):
pass
def validate(payload):
if not isinstance(payload, dict):
raise ValueError('Request must be an object')
try:
request_id = str(uuid.UUID(payload.get('request_id', '')))
except (ValueError, TypeError, AttributeError):
raise ValueError('request_id must be a UUID')
if payload.get('type') == 'imu':
if payload.get('pose') not in ('stationary', 'front-up', 'left-up'):
raise ValueError('Unknown IMU pose')
return dict(type='imu', request_id=request_id, pose=payload['pose'])
if payload.get('type') not in ('servo', 'recovery'):
raise ValueError('Only imu, servo calibration and one recovery attempt are supported')
if payload['type'] == 'servo' and (type(payload.get('pin')) is not int or payload['pin'] not in (32, 33)):
raise ValueError('Only physical pin 32 or 33 is permitted')
if payload['type'] == 'servo' and (type(payload.get('pulse_us')) is not int or payload['pulse_us'] not in CALIBRATION_PULSES):
raise ValueError('Target must be one of the fixed calibration pulse values')
if payload.get('observation_mode') == 'user_feedback':
# Explicit bench-only alternative to camera observation; never enables recovery or sweeps.
if (payload['type'] != 'servo' or payload['pulse_us'] not in (1450, 1550)
or any(payload.get(k) is not True for k in
('mechanically_unloaded', 'operator_observing', 'clearance_confirmed', 'reference_confirmed'))):
raise ValueError('User-observed testing requires an unloaded servo and a 50us step')
return {k: payload[k] for k in ('type', 'pin', 'pulse_us', 'observation_mode',
'mechanically_unloaded', 'operator_observing',
'clearance_confirmed', 'reference_confirmed')} | {'request_id': request_id}
if payload.get('observation_mode') is not None:
raise ValueError('Unknown observation mode')
if any(payload.get(k) is not True for k in
('clearance_confirmed', 'reference_confirmed', 'camera_confirmed')):
raise ValueError('Current camera, clearance and reference posture must be confirmed')
frame = payload.get('before_frame_id')
if not isinstance(frame, str) or not frame or len(frame) > 150:
raise ValueError('A current before_frame_id is required')
if payload['type'] == 'recovery':
if payload.get('known_fallen_back') is not True:
raise ValueError('This controller requires the verified fallen-back side')
return {k: payload[k] for k in ('type', 'clearance_confirmed', 'reference_confirmed',
'camera_confirmed', 'before_frame_id', 'known_fallen_back')} | {
'request_id': request_id, 'initial_targets_deg': [0, 0]}
return {k: payload[k] for k in ('type', 'pin', 'pulse_us', 'clearance_confirmed',
'reference_confirmed', 'camera_confirmed', 'before_frame_id')} | {'request_id': request_id}
def load_sampler():
spec = importlib.util.spec_from_file_location('qmi_sample', Path(__file__).with_name('sample_qmi8658.py'))
module = importlib.util.module_from_spec(spec)
spec.loader.exec_module(module)
return module
def channels(root=Path('/sys/class/pwm')):
return [p for chip in root.glob('pwmchip*') for p in chip.glob('pwm[0-9]*')]
def controller(root=Path('/sys/class/pwm')):
matches = [p for p in root.glob('pwmchip*')
if (p / 'device/of_node').resolve().name == 'pwm@34170000']
if len(matches) != 1:
raise RuntimeError('PWM controller pwm@34170000 not uniquely identified')
return matches[0]
def read_pwm(path):
return {k: int((path / k).read_text()) if (path / k).exists() else None
for k in ('enable', 'period', 'duty_cycle')}
def status():
identity = Path('/proc/device-tree/model').read_text().strip('\x00\n')
if identity != BOARD:
raise RuntimeError('Unexpected board identity: ' + identity)
imu = {'address': '0x6b', 'bus': 5, 'who_am_i': None}
try:
imu['who_am_i'] = hex(load_sampler().I2C().read(0)[0])
except Exception as exc:
imu['error'] = str(exc)
recovery_ready, recovery_error = False, 'Target-angle calibration is not available'
if Path(__file__).with_name('calibration_profile.json').exists():
try:
from recovery_hardware import load_configuration
load_configuration()
recovery_ready, recovery_error = True, None
except Exception as exc:
recovery_error = str(exc)
return {'connected': True, 'identity': identity, 'imu': imu,
'recovery_ready': recovery_ready, 'recovery_error': recovery_error,
'pwm': [{'path': str(p), **read_pwm(p)} for p in channels()],
'pwm_controller': str(controller()), 'motion_started': False}
def servo(payload, gpio, emit, chip=None, sleep=time.sleep):
pin = payload['pin']
chip = chip or controller()
path = chip / ('pwm0' if pin == 32 else 'pwm1')
pwm = None
started = time.monotonic()
result = {'pin': pin, 'pulses_us': [1500, payload['pulse_us'], 1500],
'holds_s': [2, 3, 2], 'readbacks': [], 'cleanup_verified': False,
'mechanical_success': 'unknown', 'angle_mapping': 'not_calibrated'}
extended = abs(payload['pulse_us'] - 1500) > 50
result['ramp_us_per_s'] = 400 if extended else None
gpio.setmode(gpio.BOARD)
try:
# Reuse the tested startup order; start() alone skips enable at zero duty.
pwm = gpio.PWM(pin, 50)
pwm.ChangeDutyCycle(1500 / 200)
pwm.start(1500 / 200)
previous = 1500
for index, (pulse, hold) in enumerate(zip(result['pulses_us'], result['holds_s'])):
if index:
if extended:
emit('ramp', from_us=previous, target_us=pulse, rate_us_per_s=400)
while previous != pulse:
step = max(-20, min(20, pulse - previous))
sleep(abs(step) / 400)
previous += step
pwm.ChangeDutyCycle(previous / 200)
else:
pwm.ChangeDutyCycle(pulse / 200)
actual = read_pwm(path)
if actual != {'enable': 1, 'period': 20000000, 'duty_cycle': pulse * 1000}:
raise RuntimeError('PWM readback mismatch: ' + repr(actual))
phase = ('initial_command', 'increment', 'return_command')[index]
record = {'phase': phase, 'pulse_us': pulse, 'hold_s': hold,
'elapsed_s': time.monotonic() - started, 'pwm': actual}
result['readbacks'].append(record)
emit('phase', **record)
sleep(hold)
finally:
errors = []
try:
if pwm is not None:
pwm.stop()
except Exception as exc:
errors.append('stop: ' + str(exc))
if pwm is not None:
try:
gpio.cleanup(pin)
except Exception as exc:
errors.append('cleanup: ' + str(exc))
# Only this task's channel is eligible for fallback cleanup, never another pin.
if path.exists() and pwm is not None:
errors.append('Normal cleanup left the channel exported')
try:
(path / 'enable').write_text('0')
(chip / 'unexport').write_text(path.name[3:])
result['emergency_cleanup'] = True
except Exception as exc:
errors.append('fallback: ' + str(exc))
result['cleanup_verified'] = not path.exists()
result['cleanup_errors'] = errors
emit('cleanup', **result)
if not result['cleanup_verified'] or errors:
raise RuntimeError('PWM cleanup needs inspection: ' + repr(result))
return result
def imu(emit):
sampler = load_sampler()
original_decode = sampler.decode
started = time.monotonic()
def decode(data):
sample = original_decode(data)
emit('sample', sample={**sample, 'elapsed_s': time.monotonic() - started})
return sample
sampler.decode = decode
result = sampler.capture(sampler.I2C(), 8)
result['cleanup_verified'] = result.get('configuration_restored') is True
result['parameters_applied'] = False
return result
def run(payload, emit):
payload = validate(payload)
# O_EXCL is a durable replay barrier, including retries after SSH failure.
ledger = Path(__file__).with_name('requests')
ledger.mkdir(exist_ok=True)
with (ledger / (payload['request_id'] + '.json')).open('x') as f:
json.dump(payload, f)
lock = LOCK_FILE.open('a')
fcntl.flock(lock, fcntl.LOCK_EX | fcntl.LOCK_NB)
done = threading.Event()
reason = {'message': None}
cancellation_raised = False
def abort(signum, frame):
nonlocal cancellation_raised
# A second signal must not interrupt restoration already running in finally.
if cancellation_raised:
return
cancellation_raised = True
raise Cancelled(reason['message'] or ('Board timeout' if signum == signal.SIGALRM else 'Cancelled'))
def lease():
last = time.monotonic()
while not done.wait(0.05):
ready, _, _ = select.select([sys.stdin], [], [], 0)
if ready:
line = sys.stdin.readline()
if not line:
reason['message'] = 'SSH input disconnected'
break
try:
message = json.loads(line)
except ValueError:
reason['message'] = 'Invalid lease message'
break
if message == {'action': 'heartbeat'}:
last = time.monotonic()
else:
reason['message'] = 'User requested stop'
break
if time.monotonic() - last > LEASE_SECONDS:
reason['message'] = 'Browser lease expired'
break
if not done.is_set():
os.kill(os.getpid(), signal.SIGUSR1)
for sig in (signal.SIGTERM, signal.SIGHUP, signal.SIGUSR1, signal.SIGALRM):
signal.signal(sig, abort)
signal.alarm(MAX_SECONDS)
threading.Thread(target=lease, daemon=True).start()
result = None
try:
current = status()
if current['pwm']:
raise RuntimeError('PWM already exported; another controller may own it')
emit('started', device=current, phase='preflight')
if payload['type'] == 'servo':
import Hobot.GPIO as GPIO
result = servo(payload, GPIO, emit)
elif payload['type'] == 'recovery':
from recovery_hardware import recovery
result = recovery(payload, emit)
else:
result = imu(emit)
terminal = ('stopped' if reason['message'] else
'failed' if result.get('status') == 'failed' else 'completed')
emit('result', status=terminal, result=result,
cleanup_verified=result.get('cleanup_verified', False), error=reason['message'])
except Exception as exc:
clean = not channels() if payload['type'] == 'servo' else bool(result and result.get('cleanup_verified'))
emit('result', status='stopped' if isinstance(exc, Cancelled) else 'failed',
error=str(exc), cleanup_verified=clean, result=result)
finally:
done.set()
signal.alarm(0)
fcntl.flock(lock, fcntl.LOCK_UN)
lock.close()
def main():
start = time.monotonic()
def emit(event, **fields):
print(json.dumps({'event': event, 'elapsed_s': time.monotonic() - start, **fields}), flush=True)
try:
if len(sys.argv) == 2 and sys.argv[1] == 'status':
print(json.dumps(status()), flush=True)
elif len(sys.argv) == 2 and sys.argv[1] == 'run':
payload = json.loads(sys.stdin.readline(8192))
run(payload, emit)
else:
raise ValueError('Use status or run')
except Exception as exc:
emit('result', status='failed', error=str(exc), cleanup_verified=False)
raise SystemExit(1)
if __name__ == '__main__':
main()

314
calibration/server.py Normal file
View File

@ -0,0 +1,314 @@
"""Local calibration UI, with the existing simulation page preserved at /."""
import argparse
import base64
import hashlib
from http.server import BaseHTTPRequestHandler, ThreadingHTTPServer
import json
import math
import mimetypes
from pathlib import Path
import secrets
import threading
import time
from urllib.parse import unquote, urlsplit
import uuid
ROOT = Path(__file__).resolve().parent.parent
VERIFY = ROOT / 'deployment/x5_mg90s_20261109_verify'
SIM = ROOT / 'hardware_design/x5_sg90_v1/simulation'
DATA = VERIFY / 'web-calibration'
EVIDENCE = {
'hardware-calibration.json', 'imu_sampling/mounting-estimate.json',
'imu_sampling/RESULT.md', 'camera-angle-1/RESULT.md',
'camera-angle-1/movement.mp4', 'camera-angle-1/angle-result.json',
'camera-angle-1/execution-result.json', 'camera-angle-1/manifest.json',
*('camera-angle-1/frame-' + t + 's.png' for t in ('2.5', '3.5', '4.0', '5.5', '7.5', '8.5')),
}
def read_json(path, default=None):
return json.loads(path.read_text()) if path.exists() else default
def atomic_json(path, value):
tmp = path.with_suffix('.tmp')
tmp.write_text(json.dumps(value, ensure_ascii=False, indent=2))
tmp.replace(path)
def history():
sources = ['hardware-calibration.json', 'imu_sampling/mounting-estimate.json',
'camera-angle-1/angle-result.json', 'camera-angle-1/execution-result.json']
imu = {}
for pose in ('stationary', 'front-up', 'left-up'):
rel = 'imu_sampling/' + pose + '-1.json'
data = read_json(VERIFY / rel, {})
imu[pose] = {k: data.get(k) for k in ('status', 'summary', 'configuration_restored')}
imu[pose]['source'] = rel
sources.append(rel)
return {'calibration': read_json(VERIFY / sources[0], {}),
'mounting': read_json(VERIFY / sources[1], {}),
'camera': read_json(VERIFY / sources[2], {}), 'imu': imu,
'provenance': {p: hashlib.sha256((VERIFY / p).read_bytes()).hexdigest()
for p in sources if (VERIFY / p).exists()},
'kind': 'historical_evidence', 'applied_to_motor_control': False}
class App:
def __init__(self, bridge, data=DATA):
self.bridge, self.data = bridge, data
data.mkdir(parents=True, exist_ok=True)
self.lock = threading.RLock()
self.token = secrets.token_urlsafe(32)
self.historical = history()
self.artifacts = read_json(data / 'artifacts.json', {'frames': [], 'observations': []})
def save(self):
atomic_json(self.data / 'artifacts.json', self.artifacts)
def state(self):
with self.lock:
state = self.bridge.status()
return {**state, 'recovery_ready': state['device'].get('recovery_ready') is True,
'history': self.historical,
**self.artifacts, 'csrf_token': self.token, 'server_time': time.time()}
def frame(self, p):
if p.get('role') not in ('before', 'after', 'reference', 'phase'):
raise ValueError('Invalid frame role')
phase_event = None
if p['role'] == 'phase':
job = self.bridge.status().get('job')
if (not job or job['id'] != p.get('job_id') or job['type'] != 'servo'
or job['status'] != 'running' or job.get('phase') != p.get('phase')
or p.get('phase') not in ('initial_command', 'increment', 'return_command')):
raise ValueError('阶段已切换或任务已结束,不能把当前图绑定到旧阶段')
phase_event = next((e for e in reversed(job.get('events', []))
if e.get('event') == 'phase' and e.get('phase') == p['phase']), None)
if not phase_event or time.time() - phase_event['received_at'] < 0.75:
raise ValueError('阶段尚未保持足够时间')
if p['role'] in ('before', 'reference') and p.get('job_id'):
raise ValueError('Before/reference frames cannot be pre-bound to a job')
if p['role'] == 'after':
job = next((j for j in self.bridge.export().get('jobs', []) if j['id'] == p.get('job_id')), None)
if not job or job['type'] not in ('servo', 'recovery') or job['status'] not in ('completed', 'stopped', 'failed'):
raise ValueError('后图必须关联已结束的舵机任务')
encoded = p.get('image', '')
if not isinstance(encoded, str) or not encoded.startswith('data:image/jpeg;base64,'):
raise ValueError('Expected JPEG data URL')
raw = base64.b64decode(encoded.split(',', 1)[1], validate=True)
if len(raw) > 2_000_000 or not raw.startswith(b'\xff\xd8\xff') or not raw.endswith(b'\xff\xd9'):
raise ValueError('Invalid or oversized JPEG')
ident = str(uuid.uuid4())
frame = {'id': ident, 'role': p['role'], 'job_id': p.get('job_id'),
'captured_at': time.time(), 'timestamp_kind': 'server_receive_time',
'sha256': hashlib.sha256(raw).hexdigest(), 'url': '/calibration/frames/' + ident + '.jpg'}
if phase_event:
frame.update(phase=p['phase'], phase_event=phase_event, hardware_synchronized=False)
with self.lock:
(self.data / (ident + '.jpg')).write_bytes(raw)
self.artifacts['frames'].append(frame)
self.save()
return frame
def start(self, p):
with self.lock:
frame = None
if p.get('type') == 'recovery' and self.bridge.status()['device'].get('recovery_ready') is not True:
raise ValueError('两轴目标角和行程尚未完成标定,恢复试验未启用')
if p.get('type') in ('servo', 'recovery'):
frame = next((f for f in self.artifacts['frames']
if f['id'] == p.get('before_frame_id')), None)
if not frame or frame['role'] != 'before' or frame.get('job_id') or time.time() - frame['captured_at'] > 15:
raise ValueError('请重新拍摄当前完整取景;前图已过期或已用于另一动作。')
job = self.bridge.start(p)
if frame:
frame['job_id'] = job['id']
self.save()
return job
def observation(self, p):
if p.get('physical_result') not in ('moved', 'no_motion', 'uncertain'):
raise ValueError('Invalid physical observation')
jobs = self.bridge.export().get('jobs', [])
if not any(j.get('id') == p.get('job_id') for j in jobs):
raise ValueError('Unknown job')
angles = p.get('angles') or {}
if not isinstance(angles, dict) or angles.keys() - {'before', 'after'}:
raise ValueError('Invalid angle observations')
for role, a in angles.items():
if a is None:
continue
if not isinstance(a, dict):
raise ValueError('Invalid angle record')
angle = a.get('angle_deg')
if angle is not None and (type(angle) not in (int, float) or not math.isfinite(angle) or abs(angle) > 360):
raise ValueError('Invalid projected angle')
if not any(f['id'] == a.get('frame_id') and f.get('job_id') == p['job_id']
and f['role'] == role for f in self.artifacts['frames']):
raise ValueError('Angle must reference this job and frame role')
points = a.get('points', [])
if not isinstance(points, list) or len(points) not in (0, 2):
raise ValueError('Expected two image points')
for point in points:
if not isinstance(point, dict) or any(type(point.get(k)) not in (int, float)
or not math.isfinite(point[k]) or point[k] < 0 for k in ('x', 'y')):
raise ValueError('Image coordinates must be finite nonnegative numbers')
note = p.get('note', '')
if not isinstance(note, str) or len(note) > 2000:
raise ValueError('Note too long')
result = {'job_id': p['job_id'], 'physical_result': p['physical_result'],
'angles': angles, 'note': note, 'recorded_at': time.time(),
'source': 'user_observation', 'angle_kind': '2D_projection_not_joint_angle'}
with self.lock:
self.artifacts['observations'].append(result)
self.save()
return result
def export(self):
with self.lock:
return {'schema': 'x5_calibration_web_v1', 'exported_at': time.time(),
'history': self.historical, **self.bridge.export(), **self.artifacts,
'policy_ready': False, 'parameters_applied': False,
'limits': ['1500us is not a verified upright reference',
'Image angles are 2D projections; no calibrated joint gain',
'X5 servo supply margin and full travel remain unverified']}
def report(self):
data = self.export()
return '# X5 实机标定记录\n\n历史证据、板端指令结果及用户观察分别保存。参数均未应用到闭环控制。\n\n' + \
'1500 微秒不等于长杆竖直;画面角度不是关节角。当前不具备实机爬起条件。\n\n' + \
'```json\n' + json.dumps(data, ensure_ascii=False, indent=2) + '\n```\n'
def handler_for(app, port):
hosts = {f'127.0.0.1:{port}', f'localhost:{port}'}
class Handler(BaseHTTPRequestHandler):
def log_message(self, fmt, *args):
print(self.command, urlsplit(self.path).path, fmt % args, flush=True)
def reply(self, code, body, kind='application/json; charset=utf-8'):
if not isinstance(body, bytes):
body = json.dumps(body, ensure_ascii=False, allow_nan=False).encode()
self.send_response(code)
self.send_header('Content-Type', kind)
self.send_header('Content-Length', str(len(body)))
self.send_header('Cache-Control', 'no-store')
self.send_header('X-Content-Type-Options', 'nosniff')
self.send_header('X-Frame-Options', 'DENY')
self.send_header('Permissions-Policy', 'camera=(self), microphone=()')
self.end_headers()
try:
self.wfile.write(body)
except (BrokenPipeError, ConnectionResetError):
pass
def trusted(self):
if self.headers.get('Host') not in hosts or self.headers.get('Origin') not in {None, *('http://' + h for h in hosts)}:
self.reply(403, {'error': '仅允许本机同源访问'})
return False
if self.command == 'POST' and (self.headers.get('Sec-Fetch-Site') == 'cross-site' or
not secrets.compare_digest(self.headers.get('X-Cal-Token', ''), app.token)):
self.reply(403, {'error': '请从当前标定页面操作;请求令牌无效'})
return False
return True
def file(self, file):
if not file.is_file():
return self.reply(404, {'error': '文件不存在'})
self.reply(200, file.read_bytes(), mimetypes.guess_type(file.name)[0] or 'application/octet-stream')
def do_GET(self):
if not self.trusted():
return
path = unquote(urlsplit(self.path).path)
if path in ('/calibration', '/calibration/'):
return self.file(ROOT / 'calibration/index.html')
if path == '/api/calibration/state':
return self.reply(200, app.state())
if path == '/api/calibration/export':
return self.reply(200, app.export())
if path == '/api/calibration/report':
return self.reply(200, app.report().encode(), 'text/markdown; charset=utf-8')
if path.startswith('/calibration/evidence/'):
name = path.removeprefix('/calibration/evidence/')
return self.file(VERIFY / name) if name in EVIDENCE else self.reply(404, {'error': '未列入证据白名单'})
if path.startswith('/calibration/frames/'):
name = path.removeprefix('/calibration/frames/')
with app.lock:
allowed = {f['id'] + '.jpg' for f in app.artifacts['frames']}
return self.file(app.data / name) if name in allowed else self.reply(404, {'error': '未知照片'})
target = (SIM / (path.lstrip('/') or 'index.html')).resolve()
if target.is_relative_to(SIM) and not any(p.startswith('.') for p in Path(path).parts):
return self.file(target)
self.reply(404, {'error': '未知路径'})
def do_POST(self):
if not self.trusted():
return
try:
size = int(self.headers.get('Content-Length', '0'))
if self.headers.get('Transfer-Encoding') or not 0 < size <= 3_000_000:
return self.reply(413, {'error': '请求体大小无效'})
if self.headers.get_content_type() != 'application/json':
raise ValueError('Expected application/json')
def invalid_constant(value):
raise ValueError('Non-finite JSON number: ' + value)
payload = json.loads(self.rfile.read(size), parse_constant=invalid_constant)
if not isinstance(payload, dict):
raise ValueError('Expected JSON object')
action = urlsplit(self.path).path
methods = {'/api/calibration/start': app.start, '/api/calibration/frame': app.frame,
'/api/calibration/observation': app.observation,
'/api/calibration/heartbeat': lambda p: app.bridge.heartbeat(p.get('id'))}
if action in methods:
return self.reply(200, methods[action](payload))
if action in ('/api/calibration/refresh', '/api/calibration/stop'):
if payload:
raise ValueError('No parameters allowed')
result = app.bridge.refresh() if action.endswith('refresh') else app.bridge.stop()
return self.reply(200, result)
self.reply(404, {'error': '未知接口'})
except (ValueError, TypeError, KeyError) as exc:
self.reply(400, {'error': str(exc)})
except RuntimeError as exc:
self.reply(409, {'error': str(exc)})
except Exception as exc:
self.reply(500, {'error': type(exc).__name__ + ': ' + str(exc)})
return Handler
def maintain_connection(bridge):
while True:
if not bridge.prepared:
try:
bridge.prepare()
except Exception as exc:
print('Runner preparation failed; will retry:', exc, flush=True)
bridge.refresh()
time.sleep(5)
def main():
from calibration.bridge import Bridge
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument('--port', type=int, default=8766)
parser.add_argument('--debug-only', action='store_true', help='Allow IMU and single-servo tasks only')
args = parser.parse_args()
bridge = Bridge(debug_only=args.debug_only)
app = App(bridge)
http = ThreadingHTTPServer(('127.0.0.1', args.port), handler_for(app, args.port))
http.daemon_threads = True
threading.Thread(target=maintain_connection, args=(bridge,), daemon=True).start()
print(f'Calibration: http://127.0.0.1:{args.port}/calibration (no automatic motion)', flush=True)
try:
http.serve_forever()
finally:
bridge.stop()
http.server_close()
if __name__ == '__main__':
main()

149
calibration/speed_bench.js Normal file
View File

@ -0,0 +1,149 @@
let speedOwner = null, speedStarting = false, speedGeneration = 0, speedBeatBusy = false;
let speedPromptKey = '';
const speedCameraReady = () => !document.hidden && !$('deskFrame').hidden &&
Date.now() - Number($('deskFrame').dataset.capturedAt) < 1500;
const speedActive = job => ['queued','running','stopping'].includes(job?.status);
function speedFailure(error) {
if (!error) return '';
if (error.includes('does not see a fallen base')) return '底板处于直立范围,未启动舵机。将底板轻放侧倒后可再试。';
if (error.includes('1000dps guard')) return '陀螺仪读数超过 1000°/秒,已触发停机保护;本轮最终姿态请看回放。';
if (error.includes('older than 10ms')) return 'IMU 数据延迟超过 10 毫秒,板端已中止。';
return error;
}
function trialMotion(r) {
if (!r) return '';
if(r.termination==='estimated_half_turn_from_start')return '旧程序估计有一轴接近半圈,提前停止了双轴。';
if(r.termination==='no_stable_fall')return '等候结束,没有检测到稳定侧倒姿态,舵机未启动。';
if(!r.commands)return '';
const parts=[r.upper_reversed?'本次已反向上轴。':'本次沿用校准方向。'];
if(r.detected_side){
const sides={measured:'原侧',mirror:'镜像侧',edge_y_pos:'机身 +Y 边',edge_y_neg:'机身 −Y 边',edge_x_pos:'机身 +X 边',edge_x_neg:'机身 −X 边'};
parts.push('IMU 自动识别为'+(sides[r.detected_side] || '斜向侧倒')+'。');
}
if(r.support_stages?.length)parts.push('上轴撑地点动 '+r.support_stages.map(s=>s.target_us+' µs × '+s.hold_s.toFixed(2)+' 秒').join('、')+'。');
if(r.termination==='mirror_no_lift')parts.push('镜像侧未形成有效抬升,已停止。');
if(r.measured_support && !r.support_stages?.length)parts.push('本轮未进入实测撑地方向。');
if(r.limited_ticks){
const axes=[['32','下轴'],['33','上轴']].filter(([pin])=>r.limited_ticks[pin]>0).map(([,name])=>name);
if(axes.length)parts.push(axes.join('、')+'触发行程估算限速。');
}
if(r.settled_metrics)parts.push(r.settled_metrics.imu_upright_candidate?'IMU 检测到直立,请用回放确认站稳。':'IMU 未检测到站稳。');
return parts.join('');
}
function renderSpeedBench() {
const axes = state.speed_axes || {}, job = state.motion?.job;
const summary = (pin, label) => {
const a = axes[pin] || {};
return label+' '+(a.neutral ? a.neutral+' μs' : '待测停止点')+' / '+
({cw:'顺时针',ccw:'逆时针'}[a.direction] || '待测方向');
};
const calibrated = [32,33].every(p => axes[p]?.neutral && axes[p]?.direction);
const connected = networkHealthy && state.device?.connected && Date.now()/1000-state.device.read_at<20;
const busy = speedStarting || speedActive(job) || state.job?.status==='running';
const autoState=state.autonomous || {};
const phases={starting:'正在启动',wait_upright:'请将底板平放至直立姿态,静止 1 秒后自动就绪',armed:'正在监听倾倒,站稳后会继续监听',recovering:'检测到倾倒,X5 正在尝试爬起',fault:'已因故障停止',stopped:'已停止'};
const pose=autoState.resting_pose;
const edges={measured:'已实测的原侧',mirror:'已实测的镜像侧',edge_y_pos:'+Y 侧',edge_y_neg:'−Y 侧',edge_x_pos:'+X 侧',edge_x_neg:'−X 侧',inverted:'倒扣',upright:'直立'};
const previous=autoState.last_trial;
text('autonomousState', !connected?'等候连接 X5':(autoState.active || autoState.phase==='fault' ? (phases[autoState.phase] || '正在读取'):'已停止')+
(Number.isFinite(autoState.tilt_deg)?',底板倾角 '+autoState.tilt_deg.toFixed(1)+'°':'')+
(pose?'('+(edges[pose] || '斜向侧倒')+')':'')+
(previous?'。上次'+(previous.support_steps?'试撑 '+previous.support_steps+' 段,':'')+'发送 '+previous.commands+' 次双轴指令,'+(previous.settled?.imu_upright_candidate?'站稳':'未确认站稳')+(previous.error?'。'+speedFailure(previous.error):''):'')+(autoState.error?'。'+autoState.error:''));
$('autonomousStart').hidden=!connected || autoState.active || busy;
const ready = !autoState.active && !busy && connected && state.saved?.sha256 === state.continuous_deployment.installation_sha256 &&
state.continuous_deployment.passed && !state.continuous_deployment.supply_hold && speedCameraReady();
text('speedProfile',summary(32,'下轴')+';'+summary(33,'上轴')+'。方向从各自舵盘螺钉侧看。');
$('speedDirection').disabled = $('speedStopTest').disabled = !ready;
$('speedAuto').disabled = $('speedRun').disabled = !ready || !calibrated;
$('speedAxis').disabled = $('speedNeutral').disabled = $('sideCorrection').disabled = busy;
const r=job?.result;
let msg = !connected?'等待 X5 连接':!speedCameraReady()?'等待桌面相机实时画面':
!calibrated?'请补齐两轴停止点和方向。':'已载入校准,可直接运行一次实验。';
if (speedStarting || speedActive(job)) msg = job?.phase==='stop_requested'?'已请求急停,等板端确认。':'正在执行,请观察机构。';
if (speedActive(job) && job.phase==='waiting_for_fall')msg='等候倾倒,请轻放侧倒后松手。';
else if (!speedStarting && !speedActive(job) && job) {
const kind = job.type==='speed_trial'?'双轴实验':(job.pin===32?'下轴':'上轴')+'点测';
const metrics=r?.settled_metrics || r?.end_metrics;
msg=kind+(job.status==='completed'?'已结束':'已中止')+'。'+
(r?.pwm_window_s>0?'输出 '+r.pwm_window_s.toFixed(2)+' 秒。':'')+
(r?.commands?'已下发 '+r.commands+' 次双轴指令。':'')+
(metrics && Number.isFinite(metrics.up_z)?'结束倾角约 '+Math.round(Math.acos(Math.max(-1,Math.min(1,metrics.up_z)))*180/Math.PI)+'°。':'')+
(job.type==='speed_trial'?trialMotion(r):'')+(job.cleanup_verified?'已确认输出关闭。':'输出关闭状态未确认。')+speedFailure(job.error);
}
text('speedResult',msg);
const last=state.last_speed_trial;
$('lastWebTrial').hidden=!last;
if(last){
const result=last.result || {};
text('lastTrialResult',(last.status==='completed'?'实验结束。':speedActive(last)?'实验进行中。':'实验中止。')+
speedFailure(last.error)+trialMotion(result)+(last.cleanup_verified?'已确认输出关闭。':'')+
(result.commands?'双轴指令 '+result.commands+' 次。':''));
$('lastTrialVideo').hidden=!last.replay;
if(last.replay && $('lastTrialVideo').dataset.job!==last.id){
$('lastTrialVideo').src='/speed-recordings/'+last.id+'.mp4';
$('lastTrialVideo').dataset.job=last.id;
}
}
const clip=state.speed_replay;
$('speedRecording').hidden=!clip;
if(clip && $('speedVideo').dataset.job!==clip){
$('speedVideo').dataset.job=clip;
$('speedVideo').src='/speed-recordings/'+clip+'.mp4';
$('speedRecording').open=false;
}
const key=job?.target_us===1550?'direction':'neutral';
const ask=job?.type==='speed_probe' && job.status==='completed' && job.cleanup_verified &&
axes[job.pin]?.[key+'_job']!==job.id && !busy;
$('speedSeen').hidden=!ask;
if(ask)$('speedAuto').disabled=$('speedRun').disabled=true;
if(ask && speedPromptKey!==job.id) {
speedPromptKey=job.id;
text('speedPrompt',(job.pin===32?'下轴':'上轴')+(key==='direction'?'从舵盘螺钉侧看,刚才怎么转?':'在测试脉宽下有没有主动转动?'));
const choices=key==='direction'?[['ccw','逆时针'],['cw','顺时针'],['none','没看见转动']]:[['still','没动,保存停止点'],['moving','还在转,换一个值再测']];
$('speedChoices').replaceChildren(...choices.map(([seen,label])=>{
const button=document.createElement('button');button.textContent=label;
button.onclick=async()=>{try{await request('speed/observe',{id:job.id,seen});await poll()}catch(e){text('speedError',e.message)}};
return button;
}));
}
}
function cancelSpeedLease(){speedGeneration++;speedOwner=null;speedStarting=false}
async function startSpeed(trial, direction, auto=false) {
if(speedStarting || speedActive(state?.motion?.job) || !speedCameraReady())return;
const generation=++speedGeneration;
const payload={request_id:crypto.randomUUID(),onsite:true,type:trial?'speed_trial':'speed_probe'};
if(auto)Object.assign(payload,{auto_fall:true,measured_support:true});
else if(trial)Object.assign(payload,{fallen:true,match_last_side:$('sideCorrection').value==='match',measured_support:$('sideCorrection').value==='support'});
else Object.assign(payload,{pin:Number($('speedAxis').value),target_us:direction?1550:Number($('speedNeutral').value)});
speedStarting=true;text('speedError','');renderSpeedBench();
try {
const j=await request('speed/start',payload);
if(generation!==speedGeneration || !speedCameraReady())return; // An unacknowledged launch expires on the server.
speedOwner=j.id;state.motion.job=j;
await speedHeartbeat();
} catch(e){text('speedError',e.message)}
finally{speedStarting=false;renderSpeedBench()}
}
async function speedHeartbeat() {
const id=speedOwner;
if(!id || speedBeatBusy)return;
speedBeatBusy=true;
try {
if(!speedCameraReady())throw Error('实时画面已离开');
const j=await request('motion/heartbeat',{id});
if(speedOwner===id){state.motion.job=j;if(!speedActive(j))speedOwner=null;}
} catch(e){if(speedOwner===id){text('speedError',e.message);await stopMotion()}}
finally{speedBeatBusy=false;}
}
$('autonomousStart').onclick=async()=>{try{await request('autonomous/start',{});await poll()}catch(e){text('autonomousState',e.message)}};
$('speedDirection').onclick=()=>startSpeed(false,true);
$('speedStopTest').onclick=()=>startSpeed(false,false);
$('speedAuto').onclick=()=>startSpeed(true,false,true);
$('speedRun').onclick=()=>startSpeed(true,false);
setInterval(speedHeartbeat,350);
document.addEventListener('visibilitychange',()=>{if(document.hidden&&(speedOwner||speedStarting))stopMotion()});
window.addEventListener('pagehide',()=>{if(speedOwner||speedStarting){
cancelSpeedLease();fetch('/api/imu/motion/stop',{method:'POST',headers:{'Content-Type':'application/json','X-Cal-Token':state.csrf_token},body:'{}',keepalive:true}).catch(()=>{});
}});

153
calibration/speed_bench.py Normal file
View File

@ -0,0 +1,153 @@
"""Web controls for the two continuous-rotation SG90s."""
import json
import uuid
import time
import threading
import subprocess
import shutil
from .motion_bridge import MotionBridge
from .server import atomic_json
STOP_POINTS = (1490, 1495, 1500, 1505, 1510)
# 1550 us was observed CCW from both horn screws on this assembly.
REFERENCE = {'32': ('c1d3b728-cee5-47cf-85cf-471c044d1116', '24ca8407-6c5a-4254-a17c-29bce79b7876'),
'33': ('0174f4ce-d0f4-4060-ac15-5139594132f6', '47c89b49-443e-4027-bb7a-d72a6c3442ca')}
class SpeedBench(MotionBridge):
def __init__(self, transport, output, deployment, receipts):
super().__init__(transport, output)
self.runner_path = '/home/sunrise/x5_continuous_speed_20261112/bench_pulse.py'
self.profile_path = self.output / 'speed-calibration.json'
self.identity = (deployment.get('actor_sha256'), deployment.get('installation_sha256'))
self.axes = {}
if self.profile_path.exists():
doc = json.loads(self.profile_path.read_text())
if tuple(doc['identity']) == self.identity:
self.axes = doc['axes']
elif self.identity == ('7626bbef2099172970966085bcd8d08d85e304113c3f120b323834e348c74e0f',
'7685b881cf3a47b204c8e4b7dc6083008f89ae53f0979c0bb9d9c483cc13ff6b'):
for pin, ids in REFERENCE.items():
for ident, pulse, key, value in zip(ids, (1500, 1550), ('neutral', 'direction'), (1500, 'ccw')):
path = receipts / (ident + '.json')
if path.is_file():
r = json.loads(path.read_text())['result']
if (r.get('request_id'), r.get('pin'), r.get('target_us'), r.get('status'), r.get('cleanup_verified')) == (ident, int(pin), pulse, 'completed', True):
self.axes.setdefault(pin, {}).update({key: value, key+'_job': ident})
self.save_axes()
camera = None
def _execute(self, job, payload):
done = threading.Event()
if self.camera is not None:
threading.Thread(target=self._record, args=(job['id'], done), daemon=True).start()
try:
super()._execute(job, payload)
finally:
done.set()
def _record(self, ident, done):
path = self.output / (ident + '.partial.mp4')
try:
with subprocess.Popen([shutil.which('ffmpeg') or '/opt/homebrew/bin/ffmpeg', '-nostdin',
'-loglevel', 'error', '-use_wallclock_as_timestamps', '1', '-f', 'mjpeg', '-i', 'pipe:0',
'-an', '-c:v', 'libx264', '-preset', 'ultrafast', '-pix_fmt', 'yuv420p',
'-movflags', '+faststart', str(path)], stdin=subprocess.PIPE) as encoder:
previous = None
deadline = time.monotonic() + 25
tail = None
while time.monotonic() < deadline:
if done.is_set():
if tail is None:
tail = time.monotonic() + .6
elif time.monotonic() >= tail:
break
frame = self.camera.frame()
if frame and frame[1] != previous:
encoder.stdin.write(frame[0])
encoder.stdin.flush()
previous = frame[1]
time.sleep(.1)
encoder.stdin.close()
if encoder.wait(timeout=5) == 0 and previous is not None:
path.replace(self.output / (ident + '.mp4'))
except (OSError, subprocess.SubprocessError):
pass
finally:
path.unlink(missing_ok=True)
def save_axes(self):
atomic_json(self.profile_path, dict(identity=self.identity, axes=self.axes))
def _validate_payload(self, payload):
if payload.get('onsite') is not True:
raise ValueError('请确认手已离开、线松、机构有活动空间')
ident = str(uuid.UUID(payload['request_id']))
task = dict(request_id=ident, operator_observing=True, held_clearance_confirmed=True,
independent_power_confirmed=True, web_v1=True)
if payload.get('type') == 'speed_probe':
pin, pulse = payload.get('pin'), payload.get('target_us')
if type(pin) is not int or pin not in (32, 33) or type(pulse) is not int or pulse not in (*STOP_POINTS, 1550):
raise ValueError('未知轴或点测值')
return dict(task, type='speed_probe', pin=pin, target_us=pulse)
auto = payload.get('auto_fall', False)
if type(auto) is not bool:
raise ValueError('无效的倾倒检测模式')
if payload.get('type') != 'speed_trial' or (not auto and payload.get('fallen') is not True):
raise ValueError('请先将底板侧放稳,再开始一次实验')
mapping = []
for pin in ('32', '33'):
a = self.axes.get(pin, {})
if a.get('neutral') not in STOP_POINTS or a.get('direction') not in ('ccw','cw'):
raise ValueError('请先测完两轴的停止点和方向')
mapping.append([a['neutral'], (50 if pin == '32' else -50) * (1 if a['direction']=='ccw' else -1)])
match = payload.get('match_last_side', False)
if type(match) is not bool:
raise ValueError('上轴对照选项无效')
support = payload.get('measured_support', False)
if type(support) is not bool:
raise ValueError('撑地选项无效')
return dict(task, auto_fall=auto, measured_support=auto or support, match_last_side=match and not auto, type='speed_trial', mode='recovery_trial', fallen_and_clear=True,
leads_clear=True, speed_mapping=mapping)
def start(self, payload):
with self.lock:
j = self.job or {}
if j.get('type') == 'speed_probe' and j['status'] == 'completed':
key = 'direction_job' if j['target_us'] == 1550 else 'neutral_job'
if self.axes.get(str(j['pin']), {}).get(key) != j['id']:
raise ValueError('请先记录刚才单轴点测的观察结果')
return super().start(payload)
def last_trial(self):
with self.lock:
j = max((x for x in self.jobs if x['type'] == 'speed_trial'),
key=lambda x:x['created_epoch'], default=None)
if not j:
return None
return dict(id=j['id'], status=j['status'], error=j.get('error'),
result=j.get('result'), cleanup_verified=j['cleanup_verified'],
replay=(self.output/(j['id']+'.mp4')).is_file())
def observe(self, payload):
with self.lock:
j = self.job
if (self._worker_active or not j or j['id'] != payload.get('id') or j['type'] != 'speed_probe'
or j['status'] != 'completed' or not j.get('cleanup_verified')):
raise ValueError('请先完成对应的单轴点测')
observed = payload.get('seen')
pulse = j['target_us']
key = 'direction' if pulse == 1550 else 'neutral'
valid = ('ccw', 'cw', 'none') if key == 'direction' else ('still', 'moving')
if observed not in valid:
raise ValueError('请选择实际看到的动作')
axis = self.axes.setdefault(str(j['pin']), {})
if observed in ('none', 'moving'):
axis.pop(key, None)
else:
axis[key] = observed if key == 'direction' else pulse
axis[key+'_job'] = j['id']
self.save_axes()
return dict(saved=True)

386
calibration/test_backend.py Normal file
View File

@ -0,0 +1,386 @@
"""Offline validation only: fake GPIO/sysfs and isolated subprocess watchdogs."""
import base64
import contextlib
import io
import json
from pathlib import Path
import shutil
import shlex
import subprocess
import sys
import tempfile
import time
import unittest
from unittest import mock
import uuid
from calibration import remote_runner as runner
from calibration.bridge import Bridge
def request(**changes):
return dict(type='servo', request_id=str(uuid.uuid4()), pin=33, pulse_us=1550,
clearance_confirmed=True, reference_confirmed=True,
camera_confirmed=True, before_frame_id='frame-checked', **changes)
class GPIO:
BOARD = 'BOARD'
def __init__(self, root):
self.root, self.calls = root, []
def setmode(self, mode):
self.calls.append(('mode', mode))
def PWM(self, pin, hz):
outer = self
path = self.root / ('pwm0' if pin == 32 else 'pwm1')
path.mkdir()
for name, value in [('enable', 0), ('period', 20000000), ('duty_cycle', 0)]:
(path / name).write_text(str(value))
class PWM:
def ChangeDutyCycle(self, duty):
outer.calls.append(('duty', duty))
(path / 'duty_cycle').write_text(str(round(duty * 200000)))
def start(self, duty):
outer.calls.append(('start', duty))
# Match the library defect: zero old duty does not enable output.
if int((path / 'duty_cycle').read_text()):
(path / 'enable').write_text('1')
def stop(self):
outer.calls.append(('stop',))
(path / 'enable').write_text('0')
return PWM()
def cleanup(self, pin):
self.calls.append(('cleanup', pin))
shutil.rmtree(self.root / ('pwm0' if pin == 32 else 'pwm1'))
class BackendTests(unittest.TestCase):
def test_bridge_uses_configured_board_address(self):
for env, host in [({}, '192.168.128.10'),
({'CALIBRATION_SSH_HOST': '192.168.128.100'}, '192.168.128.100')]:
with self.subTest(host=host), tempfile.TemporaryDirectory() as tmp, \
mock.patch.dict('os.environ', env, clear=True), \
mock.patch('calibration.bridge.Path.is_socket', return_value=False):
bridge = Bridge(output=tmp, control='/tmp/test-control.sock')
self.assertEqual(bridge._ssh('true')[-2:], ['sunrise@' + host, 'true'])
def test_ssh_recreates_missing_session_without_relaxing_authentication(self):
with tempfile.TemporaryDirectory() as tmp:
bridge = Bridge(output=tmp, control=str(Path(tmp) / 'missing.sock'))
command = bridge._ssh('true')
config = subprocess.check_output([command[0], '-G', *command[1:-1]],
text=True, stderr=subprocess.DEVNULL)
settings = dict(line.split(' ', 1) for line in config.splitlines())
self.assertEqual(settings['controlmaster'], 'auto')
self.assertEqual(settings['controlpersist'], '60')
self.assertEqual(settings['batchmode'], 'yes')
self.assertEqual(settings['stricthostkeychecking'], 'true')
self.assertEqual(settings['identitiesonly'], 'yes')
self.assertIn(bridge.identity, settings['identityfile'])
self.assertIn(bridge.known_hosts, settings['userknownhostsfile'])
def test_refresh_after_transport_failure_never_replays_motion(self):
with tempfile.TemporaryDirectory() as tmp:
bridge = Bridge(output=tmp, control=str(Path(tmp) / 'missing.sock'))
bridge.prepared = True
device = dict(identity=runner.BOARD, connected=True, pwm=[])
responses = [subprocess.TimeoutExpired('ssh', 7),
mock.Mock(stdout=json.dumps(device))]
with mock.patch('calibration.bridge.subprocess.run', side_effect=responses) as run:
self.assertFalse(bridge.refresh()['device']['connected'])
self.assertTrue(bridge.refresh()['device']['connected'])
self.assertIsNone(bridge.job)
self.assertIsNone(bridge.process)
self.assertEqual(len(run.call_args_list), 2)
self.assertTrue(all(c.args[0][-1].endswith('remote_runner.py status')
for c in run.call_args_list))
def test_prepare_reuses_matching_files_without_taking_hardware_lock(self):
with tempfile.TemporaryDirectory() as tmp:
root = Path(tmp)
remote = root / 'remote'
remote.mkdir()
model = root / 'model'
model.write_text(runner.BOARD + '\x00')
bridge = Bridge(output=root / 'output', debug_only=True)
bridge._ssh = lambda command: [command]
def remote_run(command, **kwargs):
for name, encoded in json.loads(kwargs['input']).items():
(remote / name).write_bytes(base64.b64decode(encoded))
output = io.StringIO()
def path(name):
return model if name == '/proc/device-tree/model' else remote
with mock.patch('pathlib.Path', side_effect=path), \
mock.patch('os.open', side_effect=AssertionError('Matching files must not take hardware ownership')), \
mock.patch('sys.stdin', io.StringIO(kwargs['input'])), \
contextlib.redirect_stdout(output), self.assertRaises(SystemExit) as exit_status:
exec(shlex.split(command[0])[-1], {})
self.assertEqual(exit_status.exception.code, 0)
return mock.Mock(stdout=output.getvalue())
with mock.patch('calibration.bridge.subprocess.run', side_effect=remote_run):
result = bridge.prepare()
self.assertTrue(bridge.prepared)
self.assertFalse(result['motion_started'])
def test_user_feedback_is_explicit_unloaded_and_small_step_only(self):
payload = request()
payload.pop('camera_confirmed'); payload.pop('before_frame_id')
payload.update(observation_mode='user_feedback', mechanically_unloaded=True, operator_observing=True)
valid = runner.validate(payload)
self.assertEqual(valid['observation_mode'], 'user_feedback')
self.assertNotIn('camera_confirmed', valid)
self.assertNotIn('before_frame_id', valid)
for k,v in [('mechanically_unloaded',False),('operator_observing',False),
('clearance_confirmed',False),('reference_confirmed',False),
('pulse_us',1600),('type','recovery'),('observation_mode','unknown')]:
with self.subTest(key=k,value=v), self.assertRaises(ValueError):
runner.validate({**payload,k:v})
def test_debug_session_rejects_recovery_even_if_old_profile_is_ready(self):
with tempfile.TemporaryDirectory() as tmp:
bridge = Bridge(output=tmp, debug_only=True)
bridge.prepared = True
bridge.device.update(connected=True, read_at=time.time(), pwm=[], recovery_ready=True)
payload = request()
payload.update(type='recovery', known_fallen_back=True)
with self.assertRaisesRegex(ValueError, 'disabled'):
bridge.start(payload)
self.assertIsNone(bridge.job)
with mock.patch('calibration.bridge.threading.Thread'):
self.assertEqual(bridge.start(request())['type'], 'servo')
def test_debug_refresh_masks_old_recovery_readiness(self):
with tempfile.TemporaryDirectory() as tmp:
bridge = Bridge(output=tmp, debug_only=True)
bridge._ssh = lambda command: ['unused']
device = dict(identity=runner.BOARD, connected=True, recovery_ready=True, pwm=[])
with mock.patch('calibration.bridge.subprocess.run', return_value=mock.Mock(stdout=json.dumps(device))):
state = bridge.refresh()
self.assertTrue(state['device']['connected'])
self.assertFalse(state['device']['recovery_ready'])
def test_validation_limits_targets_to_calibration_values(self):
for key, value in [('pin', 31), ('pin', True), ('pulse_us', 2200), ('pulse_us', 1501), ('pulse_us', 1550.0),
('camera_confirmed', False), ('reference_confirmed', 1),
('before_frame_id', ''), ('type', 'policy'), ('request_id', 'not-uuid')]:
payload = request()
payload[key] = value
with self.subTest(key=key, value=value), self.assertRaises(ValueError):
runner.validate(payload)
self.assertEqual(runner.validate(request())['pin'], 33)
def test_extended_calibration_is_ramped_bounded_and_cleans_on_interrupt(self):
for target in (550, 900, 2100, 2450):
with self.subTest(target=target), tempfile.TemporaryDirectory() as tmp:
gpio, sleeps = GPIO(Path(tmp)), []
payload = request()
payload['pulse_us'] = target
runner.validate(payload)
result = runner.servo(payload, gpio, lambda *a, **kw: None,
chip=Path(tmp), sleep=sleeps.append)
pulses = [round(c[1] * 200) for c in gpio.calls if c[0] == 'duty']
self.assertEqual((pulses[0], pulses[-1]), (1500, 1500))
self.assertIn(target, pulses)
self.assertLessEqual(max(abs(b-a) for a,b in zip(pulses,pulses[1:])), 20)
self.assertAlmostEqual(sum(sleeps), 7 + 2 * abs(target - 1500) / 400)
self.assertLess(sum(sleeps), runner.MAX_SECONDS)
self.assertTrue(result['cleanup_verified'])
with tempfile.TemporaryDirectory() as tmp:
gpio = GPIO(Path(tmp))
payload['pulse_us'] = 1600
with self.assertRaises(runner.Cancelled):
runner.servo(payload, gpio, lambda *a, **kw: None, chip=Path(tmp),
sleep=mock.Mock(side_effect=[None, runner.Cancelled('stop during ramp')]))
self.assertFalse((Path(tmp) / 'pwm1').exists())
def test_start_order_readbacks_and_cleanup(self):
with tempfile.TemporaryDirectory() as tmp:
gpio, events = GPIO(Path(tmp)), []
result = runner.servo(request(), gpio, lambda kind, **kw: events.append((kind, kw)),
chip=Path(tmp), sleep=lambda _: None)
self.assertEqual(gpio.calls[:3], [('mode', 'BOARD'), ('duty', 7.5), ('start', 7.5)])
self.assertEqual([e[1]['pulse_us'] for e in events if e[0] == 'phase'], [1500, 1550, 1500])
self.assertTrue(result['cleanup_verified'])
self.assertEqual(gpio.calls[-2:], [('stop',), ('cleanup', 33)])
def test_recovery_requires_known_pose_and_current_camera_confirmation(self):
payload = request()
payload.update(type='recovery', known_fallen_back=True)
validated = runner.validate(payload)
self.assertEqual(validated['initial_targets_deg'], [0, 0])
self.assertNotIn('pulse_us', validated)
for field in ('known_fallen_back', 'camera_confirmed', 'clearance_confirmed', 'reference_confirmed'):
with self.subTest(field=field), self.assertRaises(ValueError):
runner.validate({**payload, field: False})
def test_cancel_or_io_error_still_cleans_target(self):
for error in (runner.Cancelled('stop'), OSError('broken readback')):
with self.subTest(error=error), tempfile.TemporaryDirectory() as tmp:
gpio = GPIO(Path(tmp))
with self.assertRaises(type(error)):
runner.servo(request(), gpio, lambda *a, **k: None, chip=Path(tmp),
sleep=mock.Mock(side_effect=error))
self.assertFalse((Path(tmp) / 'pwm1').exists())
self.assertEqual(gpio.calls[-1], ('cleanup', 33))
def test_failed_claim_does_not_cleanup_someone_elses_channel(self):
with tempfile.TemporaryDirectory() as tmp:
gpio = GPIO(Path(tmp))
foreign = Path(tmp) / 'pwm1'
foreign.mkdir()
(foreign / 'enable').write_text('1')
gpio.PWM = mock.Mock(side_effect=RuntimeError('Already owned'))
with self.assertRaises(RuntimeError):
runner.servo(request(), gpio, lambda *a, **k: None, chip=Path(tmp))
self.assertTrue(foreign.exists())
self.assertEqual((foreign / 'enable').read_text(), '1')
self.assertNotIn(('cleanup', 33), gpio.calls)
def test_fallback_cleanup_is_not_a_completed_success(self):
with tempfile.TemporaryDirectory() as tmp:
gpio, events = GPIO(Path(tmp)), []
gpio.cleanup = lambda pin: None
with self.assertRaisesRegex(RuntimeError, 'cleanup needs inspection'):
runner.servo(request(), gpio, lambda kind, **kw: events.append((kind, kw)),
chip=Path(tmp), sleep=lambda _: None)
cleanup = next(e[1] for e in events if e[0] == 'cleanup')
self.assertTrue(cleanup['emergency_cleanup'])
self.assertTrue(cleanup['cleanup_errors'])
def test_controller_selected_by_device_tree(self):
with tempfile.TemporaryDirectory() as tmp:
root = Path(tmp)
for name, node in [('pwmchip0', 'unrelated'), ('pwmchip8', 'pwm@34170000')]:
(root / name / 'device').mkdir(parents=True)
(root / node).mkdir()
(root / name / 'device/of_node').symlink_to(root / node)
self.assertEqual(runner.controller(root).name, 'pwmchip8')
def test_bridge_duplicate_busy_stale_and_restart(self):
with tempfile.TemporaryDirectory() as tmp:
bridge = Bridge(output=tmp)
bridge.prepared = True
bridge.device.update(connected=True, read_at=time.time(), pwm=[])
payload = request()
with mock.patch('calibration.bridge.threading.Thread'):
job = bridge.start(payload)
self.assertEqual(job['status'], 'queued')
self.assertFalse(job['cleanup_verified'])
with self.assertRaisesRegex(ValueError, 'already recorded'):
bridge.start(payload)
with self.assertRaisesRegex(ValueError, 'active'):
bridge.start(request())
restarted = Bridge(output=tmp)
self.assertEqual(restarted.job['status'], 'failed')
self.assertFalse(restarted.job['cleanup_verified'])
restarted.prepared = True
restarted.device.update(connected=True, read_at=time.time() - 100, pwm=[])
with self.assertRaisesRegex(ValueError, 'Refresh'):
restarted.start(request())
self.assertFalse(restarted.export()['parameters_applied'])
def test_blocked_transport_has_independent_local_timeout(self):
with tempfile.TemporaryDirectory() as tmp:
bridge = Bridge(output=tmp)
bridge.prepared = True
bridge.device.update(connected=True, read_at=time.time(), pwm=[])
bridge._ssh = lambda command: [sys.executable, '-c', 'import time; time.sleep(10)']
bridge.refresh = lambda: bridge.status()
with mock.patch('calibration.bridge.TRANSPORT_TIMEOUT', .2):
job = bridge.start(request())
bridge.heartbeat(job['id'])
deadline = time.monotonic() + 3
while bridge.job['status'] in ('queued', 'running') and time.monotonic() < deadline:
time.sleep(.02)
self.assertEqual(bridge.job['status'], 'failed')
self.assertIn('transport', bridge.job['error'])
self.assertFalse(bridge.job['cleanup_verified'])
def test_first_heartbeat_required_and_stop_before_launch_never_contacts_runner(self):
for stop in (False, True):
with self.subTest(stop=stop), tempfile.TemporaryDirectory() as tmp:
bridge = Bridge(output=tmp)
bridge.prepared = True
bridge.device.update(connected=True, read_at=time.time(), pwm=[])
bridge.refresh = lambda: bridge.status()
bridge._ssh = mock.Mock(side_effect=AssertionError('No remote run may start'))
with mock.patch('calibration.bridge.FIRST_HEARTBEAT_TIMEOUT', .15):
job = bridge.start(request())
self.assertEqual(job['status'], 'queued')
with self.assertRaisesRegex(ValueError, 'Unknown active job'):
bridge.heartbeat(str(uuid.uuid4()))
self.assertFalse(bridge.first_heartbeat.is_set())
if stop:
bridge.stop()
# A delayed first heartbeat cannot revive a stopped request.
bridge.heartbeat(job['id'])
deadline = time.monotonic() + 2
while bridge._worker_active and time.monotonic() < deadline:
time.sleep(.01)
self.assertFalse(bridge._worker_active)
self.assertEqual(bridge.job['status'], 'stopped')
self.assertFalse(bridge.job['result']['motion_attempted'])
bridge._ssh.assert_not_called()
def test_process_lease_and_hard_timeout_cleanup(self):
# Use the actual runner lock, lease thread, signals, replay ledger and finally path;
# substitute only the hardware action, which is a cancellable wait.
source = """
import json,pathlib,sys,time
from calibration import remote_runner as r
r.__file__=sys.argv[1]+'/remote_runner.py'
r.LOCK_FILE=pathlib.Path(sys.argv[1]+'/board.lock')
r.MAX_SECONDS=int(sys.argv[2])
r.LEASE_SECONDS=float(sys.argv[3])
r.status=lambda: {'pwm':[]}
r.channels=lambda: []
def servo(payload, gpio, emit):
try: time.sleep(5)
finally:
time.sleep(1.2)
emit('fake_cleanup', tested=True)
r.servo=servo
import types
sys.modules['Hobot']=types.ModuleType('Hobot')
sys.modules['Hobot.GPIO']=types.ModuleType('Hobot.GPIO')
def emit(event,**kw): print(json.dumps({'event':event,**kw}),flush=True)
payload=json.loads(sys.stdin.readline())
r.run(payload,emit)
"""
for hard, lease, expectation in [(1, 0.25, 'stopped'), (1, 5, 'stopped')]:
with self.subTest(hard=hard), tempfile.TemporaryDirectory() as tmp:
proc = subprocess.Popen([sys.executable, '-c', source, tmp, str(hard), str(lease)],
stdin=subprocess.PIPE, stdout=subprocess.PIPE,
stderr=subprocess.PIPE, text=True)
proc.stdin.write(json.dumps(request()) + '\n')
proc.stdin.flush()
deadline = time.monotonic() + 4
while proc.poll() is None and time.monotonic() < deadline:
time.sleep(.02)
if proc.poll() is None:
proc.kill()
self.fail('Board guard did not terminate')
output = [json.loads(line) for line in proc.stdout.read().splitlines()]
self.assertEqual(proc.returncode, 0, proc.stderr.read())
self.assertTrue(any(e['event'] == 'fake_cleanup' for e in output))
self.assertEqual(output[-1]['status'], expectation)
self.assertTrue(output[-1]['cleanup_verified'])
proc.stdin.close()
proc.stdout.close()
proc.stderr.close()
if __name__ == '__main__':
unittest.main()

View File

@ -0,0 +1,79 @@
const fs=require('fs'),vm=require('vm'),assert=require('assert');
const nodes=new Map(),calls=[];
function element(){return {style:{},className:'',textContent:'',disabled:false,checked:false,value:'',dataset:{},classList:{add(){},remove(){}},append(){},replaceChildren(){},addEventListener(){},getContext(){return {clearRect(){},drawImage(){},beginPath(){},arc(){},fill(){},fillText(){},moveTo(){},lineTo(){},stroke(){}}},getBoundingClientRect(){return {left:0,top:0,width:1280,height:720}}};}
const context={console,URL,Date,Math,Number,Set,Map,JSON,Promise,crypto:require('crypto').webcrypto,performance:require('perf_hooks').performance,AbortController,
location:{href:'http://127.0.0.1:8766/calibration',origin:'http://127.0.0.1:8766'},
document:{visibilityState:'visible',getElementById(id){if(!nodes.has(id))nodes.set(id,element());return nodes.get(id)},createElement:element,querySelectorAll(){return []},addEventListener(){}},window:{addEventListener(){}},navigator:{},Image:class{},setTimeout(){return 1},clearTimeout(){},setInterval(){return 1},clearInterval(){},fetch:async(url,opt={})=>{calls.push([url,opt.method||'GET']);return {ok:true,json:async()=>({device:null,job:null,history:{},csrf_token:'test'})};}};
vm.createContext(context);
const html=fs.readFileSync('calibration/index.html','utf8');vm.runInContext(html.match(/<script>([\s\S]*?)<\/script>/)[1],context);
(async()=>{
await new Promise(setImmediate);
assert(calls.every(c=>c[1]==='GET'),'Page load must be GET only');
vm.runInContext(`networkOK=true;state={device:{connected:true,read_at:Date.now()/1000}};stream={getVideoTracks:()=>[{readyState:'live',muted:false}]};$('camera').paused=false;$('camera').readyState=4;frameAt=performance.now();['cameraCheck','clearanceCheck','referenceCheck'].forEach(id=>$(id).checked=true);updateGate();`,context);
assert(!nodes.get('servoStart').disabled,'Fresh camera+device and checks may enable start');
vm.runInContext(`countdown={pose:'stationary',left:3};updateGate()`,context);assert(nodes.get('servoStart').disabled,'Countdown must exclude servo motion');
vm.runInContext(`countdown=0;state.device.read_at-=31;updateGate()`,context);assert(nodes.get('servoStart').disabled,'Stale device must block motion');
vm.runInContext(`state.device.read_at=Date.now()/1000;document.visibilityState='hidden';updateGate()`,context);assert(nodes.get('servoStart').disabled,'Hidden page must block motion');
vm.runInContext(`document.visibilityState='visible';comparison.before.image={};comparison.before.points=[{x:100,y:100},{x:100,y:0}];comparison.after.image={};comparison.after.points=[{x:100,y:100},{x:200,y:100}];drawMeasure('before');drawMeasure('after');`,context);
assert.strictEqual(vm.runInContext('comparison.before.angle',context),0);assert.strictEqual(vm.runInContext('comparison.after.angle',context),90);
let saved;
context.capturePayload=p=>saved=p;
await vm.runInContext(`selectedJob={id:'test',type:'servo'};comparison.before.frame=null;comparison.after.frame=null;request=async(path,p)=>{if(path==='observation')capturePayload(p);return {}};refreshState=async()=>{};saveObservation();`,context);
assert.strictEqual(saved.angles.before,null);assert.strictEqual(saved.angles.after,null);
let started=0,resolveFrame;
context.deferFrame=()=>new Promise(r=>resolveFrame=r);context.countStart=()=>started++;
const promise=vm.runInContext(`state.job=null;pending=false;frameAt=performance.now();['cameraCheck','clearanceCheck','referenceCheck'].forEach(id=>$(id).checked=true);updateGate();captureFrame=()=>deferFrame();request=async(path,p)=>{if(path==='start')countStart();return {}};startServo();`,context);
await vm.runInContext('stopTask()',context);resolveFrame({id:'before-test',url:null});await promise;
assert.strictEqual(started,0,'Stop during photo upload must prevent later start');
let finishMedia,closedLate=0;
context.deferMedia=()=>new Promise(resolve=>finishMedia=resolve);
vm.runInContext(`pending=false;state.job=null;cameraPending=false;stream=null;navigator.mediaDevices={getUserMedia:()=>deferMedia()};`,context);
const cameraPromise=vm.runInContext('openCamera()',context);
assert(nodes.get('cameraBtn').disabled,'Waiting camera permission blocks repeated request');
assert(nodes.get('cameraState').textContent.includes('等待'),'Pending permission is visible immediately');
assert(!nodes.get('cameraOff').disabled,'Pending permission can be cancelled');
vm.runInContext('closeCamera()',context);
finishMedia({getTracks:()=>[{stop:()=>closedLate++}]});
await cameraPromise;
assert.strictEqual(closedLate,1,'Late stream after cancellation must be closed');
assert.strictEqual(vm.runInContext('cameraPending',context),false);
vm.runInContext(`pending=false;countdown=0;ownedJob=null;state={device:{connected:true,read_at:Date.now()/1000},job:null};networkOK=true;document.visibilityState='visible';stream={getVideoTracks:()=>[{readyState:'live',muted:false}]};$('camera').paused=false;$('camera').readyState=4;frameAt=performance.now();['cameraCheck','clearanceCheck','referenceCheck','fallenBackCheck'].forEach(id=>$(id).checked=true);updateGate();`,context);
assert(nodes.get('recoveryStart').disabled,'Recovery defaults to disabled without explicit backend readiness');
vm.runInContext(`state.recovery_ready='true';updateGate()`,context);assert(nodes.get('recoveryStart').disabled,'Readiness must be boolean true');
vm.runInContext(`state.recovery_ready=true;$('fallenBackCheck').checked=false;updateGate()`,context);assert(nodes.get('recoveryStart').disabled,'Known starting pose requires explicit confirmation');
vm.runInContext(`$('fallenBackCheck').checked=true;$('referenceCheck').checked=false;updateGate()`,context);assert(nodes.get('recoveryStart').disabled,'Recovery also needs original three confirmations');
vm.runInContext(`$('referenceCheck').checked=true;updateGate()`,context);assert(!nodes.get('recoveryStart').disabled,'Ready recovery can be actively requested');
let recoveryPayload;
context.keepRecovery=p=>recoveryPayload=p;
await vm.runInContext(`captureFrame=async()=>({id:'recovery-before',url:null});request=async(path,p)=>{if(path==='start'){keepRecovery(p);return {...p,id:'recovery-job',status:'running'}}return {}};startServo('recovery');`,context);
assert.strictEqual(recoveryPayload.type,'recovery');assert.strictEqual(recoveryPayload.known_fallen_back,true);assert.strictEqual(recoveryPayload.before_frame_id,'recovery-before');
assert(!('pin' in recoveryPayload)&&!('pulse_us' in recoveryPayload),'Recovery must not reuse servo dropdown inputs');
assert.strictEqual(vm.runInContext('ownedJob',context),'recovery-job');
vm.runInContext(`state.job={id:'recovery-job',type:'recovery',status:'completed',cleanup_verified:true,result:{base_upright_observed:true}};jobRender();`,context);
assert(nodes.get('jobResult').textContent.includes('底板姿态达标'),'Describe only the base posture criterion');
assert(nodes.get('jobResult').textContent.includes('实物恢复结果仍需相机观察'),'Do not infer physical recovery success');
vm.runInContext(`state.job.result={};jobRender();clearChecks()`,context);assert(nodes.get('jobResult').textContent.includes('结果未知'));assert(!nodes.get('fallenBackCheck').checked,'Clear recovery confirmation after motion/stop');
let countdownTick,launches=0,intervals=0;
context.setInterval=fn=>{countdownTick=fn;intervals++;return 99};
context.countLaunch=()=>launches++;
vm.runInContext(`pending=false;ownedJob=null;countdown=0;state.job=null;startServo=async()=>countLaunch();frameAt=performance.now();['cameraCheck','clearanceCheck','referenceCheck'].forEach(id=>$(id).checked=true);startServoCountdown();startServoCountdown();`,context);
assert.strictEqual(intervals,1,'Repeated click must not add a second countdown');
assert.strictEqual(launches,0,'Click must leave time to move away');
countdownTick();countdownTick();assert.strictEqual(launches,0);
countdownTick();assert.strictEqual(launches,1,'Only the third tick launches one trial');
vm.runInContext(`frameAt=performance.now();['cameraCheck','clearanceCheck','referenceCheck'].forEach(id=>$(id).checked=true);startServoCountdown();`,context);
await vm.runInContext('stopTask()',context);countdownTick();
assert.strictEqual(launches,1,'Stop during countdown must cancel motion');
vm.runInContext(`state.job=null;selectedJob={id:'upper-return-test',type:'servo',pin:33,pulse_us:1550,status:'completed'};request=async(path,p)=>{if(path==='observation')capturePayload(p);return {}};feedbackRender();`,context);
assert(!nodes.get('feedbackNotReturned').disabled);
await vm.runInContext(`saveFeedback('not_returned')`,context);
assert.strictEqual(saved.job_id,'upper-return-test');assert.strictEqual(saved.physical_result,'moved');
assert(saved.note.includes('没有回到'),'Non-return must be persisted separately from no motion');
const previousSave=saved;
await vm.runInContext(`state.job={id:'active',status:'running'};feedbackRender();saveFeedback('returned');`,context);
assert(nodes.get('feedbackReturned').disabled);assert.strictEqual(saved,previousSave,'Running trial must block feedback for another record');
console.log('PASS: self-service countdown, duplicate click, stop cancellation and non-return feedback');
console.log('PASS: recovery readiness, all confirmations, independent payload, lease ownership, conservative result, reset');
console.log('PASS: camera pending feedback, duplicate block, cancellation and late-stream closure');
console.log('PASS: initial GET only, camera gate, countdown mutex, stale device, hidden page, 2D angles, missing-photo observation, stop/upload race');
})().catch(e=>{console.error(e);process.exitCode=1});

185
calibration/test_imu.py Normal file
View File

@ -0,0 +1,185 @@
"""Synthetic calibration/API checks. Never opens hardware or saves a live profile."""
import copy
from contextlib import nullcontext
import hashlib
from http.client import HTTPConnection
from http.server import ThreadingHTTPServer
import importlib
import io
import json
from pathlib import Path
import subprocess
import sys
import tempfile
import threading
import unittest
from unittest.mock import Mock, patch
import uuid
import numpy as np
from calibration.imu_server import App, candidate, handler_for, stationary
def captures(root):
# Clearly synthetic: exact three static faces, 50 Hz host timestamps.
rotation = np.array([[0, -1, 0], [1, 0, 0], [0, 0, 1]], dtype=float)
records = {}
for pose, body in [('flat', [0, 0, 1]), ('x_up', [1, 0, 0]), ('y_up', [0, 1, 0])]:
ident = str(uuid.uuid4())
accel = (rotation.T @ body).tolist()
result = {'samples': [{'elapsed_s': i*.02, 'sensor_tick': i, 'accel_g': accel,
'gyro_dps': [.1, -.2, .3]} for i in range(120)],
'sensor_config': {'odr_hz': 224.2, 'gyro_dps': 1024, 'accel_g': 8},
'summary': {'max_host_gap_s': .02, 'sample_count': 120},
'status': 'sampled', 'configuration_restored': True, 'pwm_written': False}
path = root / (ident + '.json')
path.write_text(json.dumps(result))
result = dict(result, output_path=str(path), output_sha256=hashlib.sha256(path.read_bytes()).hexdigest())
records[pose] = {'id': ident, 'result': result}
return records, rotation
class CalibrationTests(unittest.TestCase):
def test_real_rotation_and_bias_from_synthetic_data(self):
with tempfile.TemporaryDirectory() as root:
data, rotation = captures(Path(root))
result = candidate(data, 'model-sha')
np.testing.assert_allclose(result['sensor_to_body'], rotation, atol=1e-12)
np.testing.assert_allclose(result['gyro_bias_dps'], [.1, -.2, .3])
np.testing.assert_allclose(result['reference_accel_g'], [0, 0, 1])
self.assertFalse(result['accelerometer_bias_calibrated'])
self.assertEqual(result['model_candidate_sha256'], 'model-sha')
self.assertIsNone(candidate({'flat': data['flat']}))
data['y_up']['result']['samples'] = copy.deepcopy(data['x_up']['result']['samples'])
with self.assertRaises(ValueError):
candidate(data)
def test_nonstationary_bad_cleanup_and_nonfinite_rejected(self):
with tempfile.TemporaryDirectory() as root:
data, _ = captures(Path(root))
result = data['flat']['result']
for update in ({'configuration_restored': False}, {'pwm_written': True},
{'samples': []}):
with self.assertRaises(ValueError):
stationary(dict(result, **update))
result['samples'][0]['gyro_dps'][0] = float('nan')
with self.assertRaises(ValueError):
stationary(result)
def test_failed_capture_keeps_dropped_tick_and_partial_restore_evidence(self):
with tempfile.TemporaryDirectory() as root:
app = App(Mock(), root)
job = {'id': str(uuid.uuid4()), 'type': 'capture', 'pose': 'flat', 'status': 'running'}
app.job = copy.deepcopy(job)
result = {'status': 'failed', 'error': 'Dropped sensor tick', 'pwm_written': False,
'configuration_restored': True, 'hidden_state_restored': False,
'restoration_scope': 'readable configuration only', 'samples': [],
'summary': {'host_capture_hz': 220., 'dropped_sensor_samples': 2}}
process = Mock(stdout=io.StringIO(json.dumps({'event': 'result', 'result': result})+'\n'),
stderr=io.StringIO(''))
process.wait.return_value = 1
process.poll.return_value = 1
with patch('calibration.imu_server.subprocess.Popen', return_value=process), \
patch.object(app, 'refresh'):
app.execute(job)
self.assertEqual(app.job['status'], 'failed')
self.assertEqual(app.job['result']['summary']['dropped_sensor_samples'], 2)
self.assertFalse(app.job['result']['hidden_state_restored'])
self.assertEqual(app.captures, {})
self.assertEqual(json.loads((Path(root)/(job['id']+'.json')).read_text()), result)
def test_save_transport_uses_actual_board_validator_and_readback(self):
board_dir = Path(__file__).resolve().parents[1] / 'deployment/x5_imu_calibration_20261112'
sys.path.insert(0, str(board_dir))
try:
board = importlib.import_module('board_tool')
with tempfile.TemporaryDirectory() as root:
root = Path(root)
evidence = root / 'evidence'
evidence.mkdir()
data, _ = captures(evidence)
model = json.loads((board_dir / 'MODEL_MANIFEST.json').read_text())
proposal = candidate(data, model['candidate_sha256'])
transport = Mock()
transport._ssh.side_effect = lambda command: ['fake-ssh', command]
app = App(transport, root / 'local')
app.proposal = proposal
def fake_ssh(argv, **kwargs):
self.assertIn('save-profile', argv[1])
# Exercise the real board serializer/validator against temporary evidence only.
receipt = board.save_profile(json.loads(kwargs['input']))
return subprocess.CompletedProcess(argv, 0, json.dumps(receipt), '')
with patch.object(board, 'HERE', root), patch.object(board, 'EVIDENCE', evidence), \
patch.object(board, 'verify_model', return_value=model), \
patch.object(board, 'locked', return_value=nullcontext()), \
patch('calibration.imu_server.subprocess.run', side_effect=fake_ssh):
receipt = app.save({'calibration_candidate_sha256': proposal['calibration_candidate_sha256'],
'installation_confirmed': True})
self.assertTrue(receipt['saved'])
readback = json.loads((root / 'calibration_profile.json').read_text())
self.assertEqual(readback, proposal)
self.assertEqual(receipt['sha256'], hashlib.sha256((root / 'calibration_profile.json').read_bytes()).hexdigest())
with self.assertRaises(ValueError):
app.save({'calibration_candidate_sha256': 'stale', 'installation_confirmed': True})
finally:
sys.path.remove(str(board_dir))
class HTTPTests(unittest.TestCase):
def setUp(self):
self.tmp = tempfile.TemporaryDirectory()
self.transport = Mock()
self.app = App(self.transport, self.tmp.name)
self.http = ThreadingHTTPServer(('127.0.0.1', 0), handler_for(self.app, 0))
self.port = self.http.server_port
self.http.RequestHandlerClass = handler_for(self.app, self.port)
self.thread = threading.Thread(target=self.http.serve_forever, daemon=True)
self.thread.start()
def tearDown(self):
self.http.shutdown()
self.http.server_close()
self.thread.join()
self.tmp.cleanup()
def request(self, method, path, payload=None, headers=None):
client = HTTPConnection('127.0.0.1', self.port, timeout=2)
h = {'Content-Type': 'application/json', 'X-Cal-Token': self.app.token, **(headers or {})}
client.request(method, path, None if payload is None else json.dumps(payload), h)
response = client.getresponse()
result = response.status, response.read()
client.close()
return result
def test_page_load_is_read_only_and_motion_endpoints_absent(self):
for path in ('/', '/api/imu/state', '/api/imu/export', '/imu-placement.png'):
self.assertEqual(self.request('GET', path)[0], 200)
self.transport._ssh.assert_not_called()
for path in ('/api/calibration/start', '/api/imu/servo', '/../calibration/server.py'):
self.assertEqual(self.request('POST', path, {})[0], 404)
self.assertFalse(json.loads(self.request('GET', '/api/imu/state')[1])['motion_enabled'])
def test_camera_requires_local_page_and_reports_missing_frame(self):
path = '/api/imu/camera.jpg'
self.http.camera = Mock()
for headers in ({'X-Cal-Token': ''}, {'Sec-Fetch-Site': 'cross-site'}):
self.assertEqual(self.request('GET', path, headers=headers)[0], 403)
self.http.camera.frame.assert_not_called()
self.http.camera.frame.side_effect = [None, (b'jpeg', 123.5)]
self.assertEqual(self.request('GET', path)[0], 503)
self.assertEqual(self.request('GET', path), (200, b'jpeg'))
self.transport._ssh.assert_not_called()
def test_bad_origin_and_missing_confirmation_rejected(self):
for headers in ({'Origin': 'https://evil.invalid'}, {'Host': 'evil.invalid'},
{'X-Cal-Token': 'stale'}, {'Sec-Fetch-Site': 'cross-site'}):
self.assertEqual(self.request('POST', '/api/imu/start', {}, headers)[0], 403)
self.assertEqual(self.request('POST', '/api/imu/start', {'type': 'capture', 'pose': 'flat'})[0], 400)
self.assertEqual(self.request('POST', '/api/imu/start', {'type': 'servo', 'pose_confirmed': True})[0], 400)
if __name__ == '__main__':
unittest.main()

View File

@ -0,0 +1,42 @@
const fs=require('fs'),vm=require('vm'),assert=require('assert');
const nodes=new Map(),calls=[];
function element(){return{disabled:false,checked:false,textContent:'',className:'',addEventListener(){}}}
const context={AbortController,setTimeout,clearTimeout,console,Date,Math,Number,JSON,Array,Infinity,setInterval(){},
document:{getElementById(id){if(!nodes.has(id))nodes.set(id,element());return nodes.get(id)}},
fetch:async(url,options={})=>{calls.push([url,options]);return{ok:true,json:async()=>({device:{connected:false},csrf_token:'test'})}}};
vm.createContext(context);
const script=fs.readFileSync('calibration/imu.html','utf8').match(/<script>([\s\S]*?)<\/script>/)[1];
vm.runInContext(script,context);
(async()=>{
await new Promise(setImmediate);
assert(calls.every(([,options])=>!options.method),'Page load must be GET-only');
vm.runInContext(`state={csrf_token:'test',device:{connected:true,read_at:Date.now()/1000,identity:'X5',imu:{},model:{installed:true}},captures:{}};render()`,context);
assert(nodes.get('capture-flat').disabled,'Pose must be explicitly confirmed');
vm.runInContext(`state.device.model.offline_validation={status:'passed',count:96};state.device.model.realtime_stream_validated=false;state.device.model.shadow_ready=true;render()`,context);
assert(nodes.get('deployment').textContent.includes('离线 96 条检查通过'));
assert(nodes.get('runtimeState').textContent.includes('实时采样尚未通过'));
assert(nodes.get('runtimeState').textContent.includes('不代表实机起身通过'));
assert(nodes.get('shadow').disabled,'Profile-presence status must not bypass local explicit save/confirmation');
vm.runInContext(`$('confirm-flat').checked=true;render()`,context);
assert(!nodes.get('capture-flat').disabled);
assert(nodes.get('save').disabled&&nodes.get('shadow').disabled);
vm.runInContext(`state.candidate={calibration_candidate_sha256:'calibration-hash'};render();$('saveConfirm').checked=true;render()`,context);
assert(!nodes.get('save').disabled);
assert(nodes.get('shadow').disabled,'Unsaved candidate must not enable inference');
vm.runInContext(`state.saved={saved:true};$('shadowConfirm').checked=true;render()`,context);
assert(!nodes.get('shadow').disabled);
vm.runInContext(`state.job={type:'capture',status:'running',started_at:Date.now()/1000,latest_sample:{accel_g:[0,0,1],gyro_dps:[0,0,0]},sample_received_at:Date.now()/1000};render()`,context);
assert(nodes.get('capture-flat').disabled&&nodes.get('save').disabled&&nodes.get('shadow').disabled);
assert(nodes.get('sampleAge').textContent.includes('当前采样窗口'));
vm.runInContext(`state.job.status='completed';state.job.result={summary:{host_capture_hz:223.9,sensor_tick_span_hz:224.2,dropped_sensor_samples:0},configuration_restored:true,hidden_state_restored:false,restoration_scope:'listed readable registers only',ahb_clock_gating_initial:'unknown (not readable)',ahb_clock_gating_cleanup:'explicitly re-enabled'};render()`,context);
assert(nodes.get('job').textContent.includes('可读配置'));
assert(nodes.get('captureQuality').textContent.includes('223.9 Hz'));
assert(nodes.get('captureQuality').textContent.includes('不声称全状态恢复'));
vm.runInContext(`state.job.status='failed';state.job.result.ahb_clock_gating_cleanup='failed or unverified';render()`,context);
assert(nodes.get('captureQuality').textContent.includes('failed or unverified'),'Failed hidden cleanup must not be shown as successful');
assert(nodes.get('sampleAge').textContent.includes('历史样本'));
vm.runInContext(`state.device.read_at-=30;render()`,context);
assert(nodes.get('capture-flat').disabled&&nodes.get('save').disabled&&nodes.get('shadow').disabled,'Stale state must block tasks');
assert(!script.includes('/api/calibration/'),'New page must not call legacy motion endpoints');
console.log('PASS: GET-only load, pose confirmation, save gate, shadow gate, running mutex, stale samples/state, no legacy motion API');
})().catch(error=>{console.error(error);process.exitCode=1});

View File

@ -0,0 +1,157 @@
"""Leased-motion integration checks using temporary files and no hardware."""
import base64
import copy
from concurrent.futures import ThreadPoolExecutor
import json
from pathlib import Path
import tempfile
import threading
import time
import unittest
from unittest.mock import Mock, patch
import uuid
from calibration.imu_server import App
from calibration.server import atomic_json
from calibration.motion_bridge import MotionBridge, validate_motion
def motion_payload(**updates):
return dict(type='jog',request_id=str(uuid.uuid4()),pin=32,target_us=1550,
calibration_stage='unloaded_center',mechanically_unloaded=True,
observation_mode='photo_operator',before_frame_id=str(uuid.uuid4()),
camera_confirmed=True,clearance_confirmed=True,reference_confirmed=True,
operator_observing=True,external_power_confirmed=True,**updates)
class MotionTests(unittest.TestCase):
def test_continuous_mode_blocks_stale_angle_actions_without_touching_board(self):
with tempfile.TemporaryDirectory() as root:
transport=Mock();app=App(transport,root,continuous={'passed':True,'actuator_type':'continuous_speed'})
for call in (lambda:app.start_motion(motion_payload()),lambda:app.calibration_point({}),
lambda:app.save_servo({}),lambda:app.start({'type':'preflight'})):
with self.assertRaises(ValueError):call()
transport._ssh.assert_not_called()
self.assertFalse(app.state()['motion_enabled'])
self.assertEqual(app.state()['mode'],'continuous_speed_web')
def test_namespace_bootstrap_and_mounted_flags(self):
payload=motion_payload()
self.assertEqual(validate_motion(payload)['type'],'jog')
for update in ({'type':'recovery'},{'target_us':1600},{'mechanically_unloaded':False},
{'pin':True},{'operator_observing':False},{'target_us':99999}):
with self.assertRaises(ValueError):validate_motion(dict(payload,**update))
mounted=dict(payload,calibration_stage='mounted_measurement',mounted_clearance_confirmed=True,target_us=1600,
mechanically_unloaded=False)
self.assertTrue(validate_motion(mounted)['mounted_clearance_confirmed'])
self.assertNotIn('mechanically_unloaded',validate_motion(mounted))
def test_recover_requires_real_hashes_fallen_confirmation_and_new_runner(self):
payload=dict(motion_payload(),type='recover',gently_placed_fallen_stationary=True,
servo_profile_sha256='a'*64,installation_profile_sha256='b'*64)
self.assertEqual(validate_motion(payload)['type'],'recover')
for update in ({'servo_profile_sha256':None},{'gently_placed_fallen_stationary':False}):
with self.assertRaises(ValueError):validate_motion(dict(payload,**update))
with tempfile.TemporaryDirectory() as root:
bridge=MotionBridge(Mock(),root)
self.assertTrue(bridge.runner_path.endswith('/v2/motion_cli.py'))
self.assertFalse(bridge.prepared)
def test_session_preserves_installation_and_never_replays_live_task(self):
with tempfile.TemporaryDirectory() as root:
app=App(Mock(),root)
app.captures={'flat':{'id':'real-history','result':{'samples':[{'a':'preserved'}]}}}
app.proposal={'schema':'qmi8658c_installation_v1','calibration_candidate_sha256':'source'}
app.saved={'saved':True,'sha256':'a'*64}
app.job={'id':'previous','type':'shadow','status':'running'}
app.persist()
restored=App(Mock(),root)
self.assertEqual(restored.captures,app.captures)
self.assertEqual(restored.saved,app.saved)
self.assertEqual(restored.proposal,app.proposal)
self.assertEqual(restored.job['status'],'failed')
self.assertFalse(restored.motion._worker_active)
def test_old_board_profile_does_not_auto_apply_a_new_unsaved_candidate(self):
with tempfile.TemporaryDirectory() as root:
app=App(Mock(),root)
device={'identity':'D-Robotics RDK X5 V1.0','pwm':[]}
status={'installed':True,'installation_profile':{'sha256':'a'*64}}
app.proposal={'calibration_candidate_sha256':'new-unsaved'}
with patch.object(app,'command',side_effect=[copy.deepcopy(device),status]):app.refresh()
self.assertIsNone(app.saved)
app.proposal=None
with patch.object(app,'command',side_effect=[copy.deepcopy(device),status]):app.refresh()
self.assertTrue(app.saved['restored_from_board'])
def test_concurrent_session_writes_are_serial_and_keep_all_updates(self):
with tempfile.TemporaryDirectory() as root:
app=App(Mock(),root)
guard=threading.Lock();counts={'active':0,'maximum':0}
def write(path,value):
with guard:
counts['active']+=1
counts['maximum']=max(counts['maximum'],counts['active'])
try:
time.sleep(.001)
atomic_json(path,value)
finally:
with guard:counts['active']-=1
def update(i):
with app.lock:app.observations.append({'test_id':i})
app.persist()
with patch('calibration.imu_server.atomic_json',side_effect=write),ThreadPoolExecutor(max_workers=8) as pool:
list(pool.map(update,range(40)))
self.assertEqual(counts['maximum'],1)
result=json.loads((Path(root)/'session.json').read_text())
self.assertEqual({r['test_id'] for r in result['observations']},set(range(40)))
def test_fresh_image_binding_replay_barrier_and_no_motion_on_page_load(self):
with tempfile.TemporaryDirectory() as root:
transport=Mock();app=App(transport,root)
image='data:image/jpeg;base64,'+base64.b64encode(b'\xff\xd8\xfffixture\xff\xd9').decode()
frame=app.frame({'role':'before','source':'uploaded_current_photo','image':image})
payload=motion_payload();payload['before_frame_id']=frame['id']
with patch.object(app.motion,'start',return_value=dict(payload,id=payload['request_id'],status='queued')) as start:
with patch('calibration.imu_server.time.time',return_value=frame['captured_at']+16):
with self.assertRaises(ValueError):app.start_motion(payload)
start.assert_not_called()
app.start_motion(payload)
with self.assertRaises(ValueError):app.start_motion(payload)
self.assertEqual(start.call_count,1)
transport._ssh.assert_not_called()
def test_user_standing_confirmation_is_separate_from_imu_candidate(self):
with tempfile.TemporaryDirectory() as root:
app=App(Mock(),root)
job={'id':str(uuid.uuid4()),'type':'recover','status':'completed','cleanup_verified':True,
'result':{'imu_upright_candidate':True}}
app.motion.jobs.append(job)
self.assertEqual(app.observations,[])
value=app.observe_motion({'job_id':job['id'],'standing_confirmed':True})
self.assertEqual(value['source'],'user_observation_not_automatic_success')
job['type']='jog'
with self.assertRaises(ValueError):app.observe_motion({'job_id':job['id'],'standing_confirmed':True})
def test_preflight_requires_onsite_confirmation_and_routes_only_new_cli(self):
with tempfile.TemporaryDirectory() as root:
app=App(Mock(),root)
app.job={'id':str(uuid.uuid4()),'type':'preflight','status':'running'}
with patch.object(app,'command',return_value={'passed':True,'status':'passed','cleanup_verified':True,'pwm_written':False}) as run,\
patch.object(app,'refresh'):
app.execute_preflight(copy.deepcopy(app.job))
args,kw=run.call_args
self.assertEqual(args[0],['arm-check'])
self.assertEqual(kw['script'],'v2/motion_cli.py')
self.assertTrue(kw['payload']['stationary_confirmed'])
self.assertTrue(kw['payload']['operator_observing'])
self.assertEqual(app.job['status'],'completed')
with self.assertRaises(ValueError):app.start({'type':'preflight','pose_confirmed':True})
with patch.object(app,'command',return_value={'passed':True,'status':'passed','cleanup_verified':False,'pwm_written':False}),\
patch.object(app,'refresh'):
app.execute_preflight(copy.deepcopy(app.job))
self.assertEqual(app.job['status'],'failed')
self.assertTrue(app.job['error'])
if __name__=='__main__':unittest.main()

View File

@ -0,0 +1,85 @@
const fs=require('fs'),vm=require('vm'),assert=require('assert');
const html=fs.readFileSync('calibration/imu.html','utf8'),knownIds=new Set([...html.matchAll(/\bid="([^"]+)"/g)].map(m=>m[1]));
const nodes=new Map(),calls=[],intervals=new Map(),events={};let timerId=0;
function element(){return{disabled:false,checked:false,textContent:'',className:'',value:'',hidden:false,children:[],paused:false,readyState:4,currentTime:0,
addEventListener(name,fn){this['on'+name]=fn},replaceChildren(...children){this.children=children;if(!children.some(c=>c.value===this.value))this.value=children[0]?.value||''},
play:async()=>{},getContext(){return{drawImage(){}}},toDataURL(){return'data:image/jpeg;base64,/9j/TEST/9k='}}}
const get=id=>{assert(knownIds.has(id),'JavaScript references an ID missing from actual HTML: '+id);if(!nodes.has(id)){const n=element();if(id==='jogAxis')n.value='32';if(id==='jogStage')n.value='unloaded_center';if(id==='cadDemoFrames')n.textContent=html.match(/<script type="application\/json" id="cadDemoFrames">([\s\S]*?)<\/script>/)[1];if(id==='pointBase')n.value='1500';if(id==='modeCamera')n.checked=true;nodes.set(id,n)}return nodes.get(id)};
let server={device:{connected:false},motion:{job:null},motion_status:{installed:false},csrf_token:'test',captures:{}};
const context={AbortController,setTimeout,clearTimeout,console,Date,Math,Number,JSON,Array,Set,Infinity,crypto:require('crypto').webcrypto,performance:require('perf_hooks').performance,
document:{visibilityState:'visible',getElementById:get,createElement:element,addEventListener(name,fn){events[name]=fn}},window:{addEventListener(name,fn){events[name]=fn}},navigator:{mediaDevices:{}},
setInterval(fn,ms){const id=++timerId;intervals.set(id,{fn,ms});return id},clearInterval(id){intervals.delete(id)},
fetch:async(url,options={})=>{calls.push([url,options]);let result={};if(!options.method)result=server;else if(url.endsWith('/motion/start')){const p=JSON.parse(options.body);result={...p,id:p.request_id,status:'queued',events:[],created_epoch:Date.now()/1000};server.motion={job:result}}else if(url.endsWith('/motion/heartbeat'))result=server.motion.job;return{ok:true,json:async()=>result}}};
vm.createContext(context);
vm.runInContext(html.match(/<script>([\s\S]*?)<\/script>/)[1],context);
vm.runInContext(fs.readFileSync('calibration/imu_motion.js','utf8'),context);
const run=code=>vm.runInContext(code,context);
function ready(){server={csrf_token:'test',device:{connected:true,read_at:Date.now()/1000,model:{installed:true}},captures:{},saved:{saved:true},motion:{job:null},motion_status:{installed:true,recover_ready:true,sensor_stream_validated:true,calibration_points:{},installation_profile:{sha256:'a'.repeat(64)},servo_profile:{sha256:'b'.repeat(64)}}};context.fixture=server;run(`state=fixture;servoViewInitialized=true;networkHealthy=true;pending=false;motionOwned=null;motionCountdown=0;awaitedMotion=null;$('jogStage').value='unloaded_center';jogTarget=1550;$('modePhoto').checked=true;$('modeCamera').checked=false;uploadedBefore={id:crypto.randomUUID(),captured_at:Date.now()/1000,url:'/synthetic.jpg'};motionChecks.forEach(id=>$(id).checked=true);$('jogUnloaded').checked=true;$('fallenStationary').checked=true;render();`)}
(async()=>{
await new Promise(setImmediate);assert(calls.every(([,o])=>!o.method),'Loading the page must never start hardware');
const cadCalls=calls.length;assert(!html.includes('127.0.0.1:8768'),'Calibration demo must not need a second page/server');
assert(html.indexOf('id="jogAxis"')<html.indexOf('id="cadDemoPanel"')&&html.indexOf('id="cadDemoPanel"')<html.indexOf('id="jogStart"'),'Show CAD next to axis selection before physical point testing');
assert.strictEqual(get('cadDemoAngle').textContent,'0° · CAD 示意');
get('cadDemoPlay').onclick();const cadTick=[...intervals.values()].find(v=>v.ms===150).fn;cadTick();
assert.strictEqual(get('cadDemoAngle').textContent,'+2.5° · CAD 示意');
get('cadDemoSlider').value='16';get('cadDemoSlider').oninput();assert.strictEqual(get('cadDemoAngle').textContent,'+20° · CAD 示意');
const lowerPose=get('cadDemoImage').src;get('jogAxis').value='33';get('jogAxis').onchange();
assert(get('cadDemoTitle').textContent.startsWith('上轴'));assert.strictEqual(get('cadDemoAngle').textContent,'0° · CAD 示意');
get('cadDemoSlider').value='16';get('cadDemoSlider').oninput();assert.notStrictEqual(get('cadDemoImage').src,lowerPose,'Upper axis must use its own CAD rigid-body transforms');
get('cadDemoZero').onclick();assert.strictEqual(get('cadDemoAngle').textContent,'0° · CAD 示意');
assert.strictEqual(get('measuredAngle').value,'','CAD example must not fill real measurements');assert.strictEqual(run('jogTarget'),1550,'CAD example must not change PWM target');
get('cadDemoPlay').onclick();run('motionCountdown=3;renderMotion()');assert.strictEqual(run('cadDemoTimer'),null,'Physical countdown stops the illustration');assert(get('cadDemoPlay').disabled);
run('motionCountdown=0');get('jogAxis').value='32';get('jogAxis').onchange();
assert(calls.slice(cadCalls).every(([,o])=>!o.method),'CAD playback, scrubbing and zero view must never send hardware writes');
ready();
run(`state.saved.saved=true;state.motion_status.calibration_points={'32':[{pulse_us:1500,measured_deg:0,seq:1},{pulse_us:1550,measured_deg:.2,seq:2}]};servoViewInitialized=false;renderMotion()`);
assert.strictEqual(get('jogStage').value,'mounted_measurement','Saved zero resumes measurement rather than asking for IMU calibration');
assert.strictEqual(get('pointBase').value,'1500');assert.strictEqual(run('jogTarget'),1450);
assert(get('servoResumeState').textContent.includes('IMU 安装校准已保存'));
const reviewWrites=calls.filter(([,o])=>o.method).length;get('servoReview').onclick();
assert.strictEqual(calls.filter(([,o])=>o.method).length,reviewWrites,'Review must not reset data or actuate');
assert.strictEqual(run('state.motion_status.calibration_points["32"].length'),2,'Review preserves prior evidence');
ready();
const originalFetch=context.fetch,zeroId=require('crypto').randomUUID(),zeroFrame=require('crypto').randomUUID();let zeroPayload=null;
context.fetch=async(url,options={})=>{
if(url.endsWith('/export'))return{ok:true,json:async()=>({motion:{jobs:[{id:zeroId,type:'jog',pin:32,status:'completed',cleanup_verified:true,created_epoch:1}]},frames:[{id:zeroFrame,job_id:zeroId,role:'before'}]})};
if(url.endsWith('/motion/calibration-point')){zeroPayload=JSON.parse(options.body);return{ok:true,json:async()=>({saved:true})};}
return originalFetch(url,options);
};
const zeroStarts=calls.filter(([u])=>u.endsWith('/motion/start')).length;await run('recordExistingZero(32)');
assert(zeroPayload&&zeroPayload.pin===32&&zeroPayload.pulse_us===1500&&zeroPayload.measured_deg===0);
assert(zeroPayload.zero_mounted_confirmed&&zeroPayload.operator_measured);
assert.strictEqual(calls.filter(([u])=>u.endsWith('/motion/start')).length,zeroStarts,'Recording an explicitly confirmed existing zero must not actuate');
assert.strictEqual(get('jogStage').value,'mounted_measurement');context.fetch=originalFetch;
ready();assert(!get('jogStart').disabled,'Bootstrap is usable with real confirmations');
assert.strictEqual(run('motionChecks.length'),1,'One explicit combined preparation confirmation');
assert(!knownIds.has('motionPowerCheck'),'No hidden legacy confirmation boxes');
assert(get('mountedConfirmRow').hidden&&!get('unloadedConfirmRow').hidden,'Show only the current stage confirmation');
run(`state.motion.job={id:'finished-small-step',type:'jog',status:'completed',cleanup_verified:true,pin:32,calibration_stage:'unloaded_center',events:[]};awaitedMotion='finished-small-step';motionOwned='finished-small-step';renderMotion()`);
assert(get('benchReady').checked&&get('jogUnloaded').checked,'Successful small steps retain the same stage preparation');
get('pulsePlus').onclick();assert(get('benchReady').checked,'Choosing the next small step must not ask preparation again');
run(`state.motion_status.calibration_points={'32':[{pulse_us:1500,measured_deg:0,seq:1},{pulse_us:1550,measured_deg:5,seq:2}]};renderMotion()`);
assert.strictEqual(get('pointBase').value,'1550','Continue from the latest measured point automatically');
get('jogStage').value='mounted_measurement';get('jogStage').onchange();assert(!get('benchReady').checked,'Changing assembly stage requires a fresh preparation confirmation');
ready();
await run(`beginMotion('jog')`);assert.strictEqual(run('motionCountdown'),3);assert.strictEqual(calls.filter(([u])=>u.endsWith('/motion/start')).length,0);
const countdown=[...intervals.values()].find(v=>v.ms===1000).fn;await run('stopMotion()');await countdown();
assert.strictEqual(run('pending'),false,'Cancelling countdown must unblock later explicit actions');assert.strictEqual(calls.filter(([u])=>u.endsWith('/motion/start')).length,0,'Cancelled countdown must never launch');
ready();await run(`beginMotion('jog')`);const tick=[...intervals.values()].filter(v=>v.ms===1000).slice(-1)[0].fn;await tick();await tick();assert.strictEqual(calls.filter(([u])=>u.endsWith('/motion/start')).length,0);await tick();
const starts=calls.filter(([u])=>u.endsWith('/motion/start'));assert.strictEqual(starts.length,1);const payload=JSON.parse(starts[0][1].body);assert.strictEqual(payload.type,'jog');assert.strictEqual(payload.pin,32);assert.strictEqual(payload.target_us,1550);assert.strictEqual(payload.calibration_stage,'unloaded_center');assert(payload.mechanically_unloaded&&payload.operator_observing);assert(calls.some(([u])=>u.endsWith('/motion/heartbeat')),'Only after explicit start, acquire first browser lease');
assert(get('modeCamera').disabled,'Observation mode cannot be switched to bypass a live camera requirement');
const beforeReadFailure=calls.filter(([u])=>u.endsWith('/motion/stop')).length,normalFetch=context.fetch;context.fetch=async(u,o)=>{if(u.endsWith('/state'))throw Error('state channel failed');return normalFetch(u,o)};await run('poll()');await new Promise(setImmediate);assert(calls.filter(([u])=>u.endsWith('/motion/stop')).length>beforeReadFailure,'Loss of state monitoring must stop even if heartbeat endpoint still responds');assert.strictEqual(run('motionOwned'),null);assert.strictEqual(run('networkHealthy'),false);context.fetch=normalFetch;ready();run(`state.motion.job={id:'another-live-task',type:'jog',status:'running',events:[]};motionOwned='another-live-task';activeObservationMode='photo_operator';`);
const stops=calls.filter(([u])=>u.endsWith('/motion/stop')).length;context.document.visibilityState='hidden';events.visibilitychange();await new Promise(setImmediate);assert(calls.filter(([u])=>u.endsWith('/motion/stop')).length>stops,'Background tab must stop');context.document.visibilityState='visible';
ready();run(`state.motion_status.recover_ready=false;state.device.model.shadow_ready=true;renderMotion()`);assert(get('recoverStart').disabled,'A profile-presence bit must not bypass current runtime/calibration gates');
ready();run(`state.motion.job={id:'recovery-candidate',type:'recover',status:'completed',cleanup_verified:true,result:{imu_upright_candidate:true},events:[]};renderMotion()`);assert(get('standingResult').textContent.includes('请现场确认'));assert(!calls.some(([u])=>u.endsWith('/motion/observation')),'IMU candidate must not auto-create physical success');
run(`state.motion.job.result.samples=[{raw:'RAW_SENTINEL'}];state.motion.job.result.controls=[{observation:Array(196).fill(1)}];state.motion.job.result.sensor={host_sample_hz:231.1,counter_drops:0};renderMotion()`);assert(!get('motionLive').textContent.includes('RAW_SENTINEL'));assert(get('motionLive').textContent.includes('231.1'));run(`$('motionDetailsPanel').open=true;renderMotion()`);assert(get('motionDetails').textContent.includes('RAW_SENTINEL'),'Full raw result stays in explicit details');
let resolveCamera,closed=0;context.navigator.mediaDevices.getUserMedia=()=>new Promise(resolve=>resolveCamera=resolve);
ready();run(`$('modePhoto').checked=false;$('modeCamera').checked=true;`);const opening=run('openPhysicalCamera()');assert(run('cameraPending'));run('closePhysicalCamera()');resolveCamera({getTracks:()=>[{stop(){closed++}}]});await opening;assert.strictEqual(closed,1,'Late permission result after cancel must be closed');
ready();run(`state.continuous_deployment={passed:true,controls:133,samples:2214};renderMotion()`);
assert(!get('continuousPanel').hidden);assert(get('continuousState').textContent.includes('133'));
for(const id of ['jogStart','recoverStart','quickZeroLower','quickZeroUpper','recordZero','recordAngle','servoSave','shadow'])assert(get(id).disabled,'Continuous mode blocks old position actions: '+id);
const staleAngleStarts=calls.filter(([u])=>u.endsWith('/motion/start')).length;await run(`beginMotion('jog')`);assert.strictEqual(calls.filter(([u])=>u.endsWith('/motion/start')).length,staleAngleStarts);
console.log('PASS: embedded CAD playback/axis switch/scrubbing with no hardware writes, GET-only load, usable bootstrap, countdown/stop race, first lease, background stop, immutable observation mode, runtime gate, IMU/user separation, camera cancellation');
})().catch(error=>{console.error(error);process.exitCode=1});

142
calibration/test_server.py Normal file
View File

@ -0,0 +1,142 @@
import base64
from http.client import HTTPConnection
from http.server import ThreadingHTTPServer
import json
from pathlib import Path
import tempfile
import threading
import unittest
from unittest.mock import Mock, patch
import uuid
from calibration.server import App, handler_for, maintain_connection
class ConnectionTests(unittest.TestCase):
def test_initial_connection_failure_retries_preparation_without_starting_motion(self):
bridge = Mock(prepared=False)
def prepare():
if bridge.prepare.call_count == 1:
raise ConnectionError('USB not present yet')
bridge.prepared = True
bridge.prepare.side_effect = prepare
with patch('calibration.server.time.sleep', side_effect=[None, StopIteration]), \
self.assertRaises(StopIteration):
maintain_connection(bridge)
self.assertTrue(bridge.prepared)
self.assertEqual(bridge.prepare.call_count, 2)
self.assertEqual(bridge.refresh.call_count, 2)
bridge.start.assert_not_called()
class FakeBridge:
def __init__(self):
self.jobs = []
self.starts = 0
def status(self):
return {'device': {'connected': False}, 'job': self.jobs[-1] if self.jobs else None}
def export(self):
return {'jobs': self.jobs}
def start(self, p):
self.starts += 1
job = dict(p, id=p['request_id'], status='running')
self.jobs.append(job)
return job
def stop(self):
return None
def refresh(self):
return self.status()
class HTTPTests(unittest.TestCase):
def setUp(self):
self.tmp = tempfile.TemporaryDirectory()
self.bridge = FakeBridge()
self.app = App(self.bridge, Path(self.tmp.name))
self.http = ThreadingHTTPServer(('127.0.0.1', 0), handler_for(self.app, 0))
self.port = self.http.server_port
self.http.RequestHandlerClass = handler_for(self.app, self.port)
self.thread = threading.Thread(target=self.http.serve_forever, daemon=True)
self.thread.start()
def tearDown(self):
self.http.shutdown()
self.http.server_close()
self.thread.join()
self.tmp.cleanup()
def request(self, method, path, data=None, headers=None):
client = HTTPConnection('127.0.0.1', self.port, timeout=2)
h = {'Content-Type': 'application/json', 'X-Cal-Token': self.app.token}
h.update(headers or {})
client.request(method, path, None if data is None else json.dumps(data), h)
res = client.getresponse()
result = res.status, res.read()
client.close()
return result
def test_get_never_moves_and_old_simulation_is_retained(self):
for path in ('/', '/api/calibration/state', '/api/calibration/export', '/api/calibration/report', '/result.json'):
self.assertEqual(self.request('GET', path)[0], 200)
self.assertEqual(self.bridge.starts, 0)
export = json.loads(self.request('GET', '/api/calibration/export')[1])
self.assertNotIn('csrf_token', export)
self.assertFalse(export['parameters_applied'])
self.assertIn('camera', export['history'])
def test_origin_host_and_token_rejection(self):
for headers in ({'Origin': 'https://evil.invalid'}, {'Host': 'evil.invalid'},
{'X-Cal-Token': 'stale'}, {'Sec-Fetch-Site': 'cross-site'}):
self.assertEqual(self.request('POST', '/api/calibration/start', {}, headers)[0], 403)
self.assertEqual(self.bridge.starts, 0)
def test_path_traversal_and_unlisted_evidence(self):
for path in ('/../CALIBRATION_WEB_HANDOFF_20261109.md', '/%2e%2e/%2e%2e/server.py',
'/calibration/evidence/../../server.py', '/calibration/frames/../artifacts.json'):
self.assertEqual(self.request('GET', path)[0], 404)
def test_frame_freshness_binding_and_after_order(self):
image = 'data:image/jpeg;base64,' + base64.b64encode(b'\xff\xd8\xfftest\xff\xd9').decode()
ident = str(uuid.uuid4())
self.assertEqual(self.request('POST', '/api/calibration/frame',
dict(image=image, role='after', job_id=ident))[0], 400)
frame = json.loads(self.request('POST', '/api/calibration/frame', dict(image=image, role='before'))[1])
p = dict(type='servo', pin=32, request_id=ident, before_frame_id=frame['id'])
with patch('calibration.server.time.time', return_value=frame['captured_at'] + 16):
self.assertEqual(self.request('POST', '/api/calibration/start', p)[0], 400)
self.assertEqual(self.bridge.starts, 0)
self.assertEqual(self.request('POST', '/api/calibration/start', p)[0], 200)
self.assertEqual(self.request('POST', '/api/calibration/start', p)[0], 400)
self.assertEqual(self.bridge.starts, 1)
after = dict(image=image, role='after', job_id=ident)
self.assertEqual(self.request('POST', '/api/calibration/frame', after)[0], 400)
phase = dict(image=image, role='phase', job_id=ident, phase='initial_command')
self.assertEqual(self.request('POST', '/api/calibration/frame', phase)[0], 400)
self.bridge.jobs[0].update(phase='initial_command', events=[{'event': 'phase',
'phase': 'initial_command', 'received_at': 0}])
self.assertEqual(self.request('POST', '/api/calibration/frame', phase)[0], 200)
self.bridge.jobs[0]['phase'] = 'increment'
self.assertEqual(self.request('POST', '/api/calibration/frame', phase)[0], 400)
self.bridge.jobs[0]['status'] = 'completed'
self.assertEqual(self.request('POST', '/api/calibration/frame', after)[0], 200)
self.assertEqual(self.request('POST', '/api/calibration/observation',
dict(job_id=ident, physical_result='moved', angles={
'after': {'angle_deg': 5, 'frame_id': frame['id']}}))[0], 400)
self.assertEqual(self.request('POST', '/api/calibration/observation',
dict(job_id=ident, physical_result='uncertain', angles={
'before': None, 'after': None}))[0], 200)
self.assertEqual(self.request('POST', '/api/calibration/observation',
dict(job_id=ident, physical_result='moved', angles={
'before': {'angle_deg': 5, 'frame_id': frame['id'],
'points': [{'x': 'bad', 'y': 0}, {'x': 2, 'y': 4}]}}))[0], 400)
if __name__ == '__main__':
unittest.main()

View File

@ -0,0 +1,147 @@
{
"scope": "SIMULATION_ONLY_CONTINUOUS_SPEED",
"actor_sha256": "7626bbef2099172970966085bcd8d08d85e304113c3f120b323834e348c74e0f",
"hardware_tested": false,
"files": {
"README.md": "bb4cb1b3f3e42190ec6a223e35f2d2cebe30d167a8fa26612ec5e633fd4ecda5",
"SELECTION.json": "56b7eaa3c82b4abdfe55dc113485ed0a03a04f78bdfa21b712f0198ead36fc79",
"SOURCE_MANIFEST.json": "d01d25e8ee96423f3d282e116542ab57957467b1a55d92e91b5be0c2b730c39a",
"__pycache__/environment.cpython-310.pyc": "a351cf2397a285770bb5bad8570292f64594fa9472b981b90cb8bf0c7fce94aa",
"__pycache__/imu_orientation.cpython-310.pyc": "0fa8e8256320bf573f5412c09ac4b746561c9bda87f4af9a80ea727ea2a58cbf",
"__pycache__/policy.cpython-310.pyc": "adcf5a5b2746d2647d6082dbcd2ba0a58df512b7c5b9b86ae3980a7d5ff21471",
"__pycache__/position_physical.cpython-310.pyc": "5a7a6a4a65fa18fd7642c72d778739de39e272d55ba91a5a6ad43fe88665959d",
"__pycache__/sensor_model.cpython-310.pyc": "6e6cc3611360bfefdd47b5c0cc5ad0c94b910b17a252c597d5b5b1a3a84d93f0",
"actor_input_audit.json": "15fe604c67f20a3fd4e4a5662dcf52e0cde971e756a5342bd116a52d7dedd1af",
"audit_initials.py": "cdad6523c2f1a4c39d4dd692edfebb662846de8fd506bd631534153858b941fb",
"build_fast.py": "cad20efb56e41012eab79fb4f8facda7bcdce65475a0f43b3202551d28292aac",
"continuous_dynamics.json": "317aba7548857f4823e56a08618f5de51ef44534912517e0d199a81712ebeac1",
"dependencies.json": "9c2c103c9268aa32d1f1b5373d708de1329ee364c41d05bbe4014ce2da80abeb",
"dynamics.json": "65aea4901caa7eccf52c66947fd994d46ae467859dd67b6fad1d2b36df1ff023",
"environment.py": "b82a7d0c0124c7bc40f7cb4d01d85e753b3d4f0dfde70db0dbdcb79cb5ae7013",
"evaluate.py": "6e753f9d3123898b62ae3868eb48ecacabf632f40807eb3253fb4dc83573977a",
"imu_orientation.py": "fc6048e029b7059832ddda3731d5151ab089b7396006ae31ba71a25edaa8772f",
"legacy_integrity.json": "1d8afdcda85be85d2c55ba586f5571d2471d9874a0fa246045488976cf76d21f",
"meshes/factory_horn1_ENVELOPE.stl": "92ea863dc6e57578de392e3f9531c97d691672f340da7fcee4efca8090f87f21",
"meshes/factory_horn2_ENVELOPE.stl": "4060ecbb51b70757522642d22cb211bc0c8b8fe72fc688df39f0db5943bd0773",
"meshes/horn_strip_1.stl": "76d77e25745ff84b243a0973ed7cc58c23cc8fa09c3b7bf837c207d945f8b722",
"meshes/horn_strip_2.stl": "fd8a9dc02f5b381e1742370551bce8fe31bba73dd3ecc0507eae0d1238faa11d",
"meshes/horn_strip_3.stl": "9b2109cb0bc54471bf134548475fa8ec29c008fbc5d7d69c3b04b802ccd12e35",
"meshes/horn_strip_4.stl": "9ad3f5a3770967bd970e1e7bb95de878abf2f348bd81d86f7336ba473747ce87",
"meshes/idler_bolt1.stl": "6ed55893808e44358e7f9fdc7cb93cc4a8f65cb4493fb0eb876648169dcdc084",
"meshes/idler_bolt2.stl": "87450e6e8b48b4a4bd19cfb4c9db43b5be8108385cbb892de81da8115cc18f46",
"meshes/idler_bushing1.stl": "233dd51c800635114f0450af03195631a33a0846416b96ae948ddc41de8c1307",
"meshes/idler_bushing2.stl": "6b9c71825b7cc3336816d18c0c50103e3b10693c62d6f999c751a9198eb7c9bf",
"meshes/idler_nut1.stl": "777834ce173302a9a1a467de1825cbba85f8a83fec3c279bbe79a7fcf62bfcb7",
"meshes/idler_nut2.stl": "9ba57dcc7ce869f9840a9948ea143d3cae8c0ec8bcd7f060cd0c7c5890a76a7d",
"meshes/idler_washer1.stl": "9298a52f4cf865d7651c4e1a11ce46c6078624af66f34b5a9004e2e435f411e7",
"meshes/idler_washer2.stl": "fd9b093a4ff0c1030de3aa815411f5ca1b97cb3234fec1eb2d1a473a0c95d321",
"meshes/imu_components_ENVELOPE.stl": "30f0b8e78601002b52cdfada8fad480f8ac7113e5b6ceef29a980f70ecd09026",
"meshes/imu_header_8pin_ENVELOPE.stl": "0c4539b0ec22fbef1ec67a58b31055f0a02ad0fca8be25b3592d1274fba83245",
"meshes/imu_holder_TRIAL.stl": "c0576c815deb43e4805939fc20a42f207fbdf9a091e57d8fa4ede6a867afc34b",
"meshes/imu_nut_1.stl": "7c65367a120f2777baf18350a16b1f9689ccbd42e839d818fae5feba82424f11",
"meshes/imu_nut_2.stl": "43dc8517b8f5c9d50b93419732cf42dd2fb4196e705de3ea8a9be7e84d131a4c",
"meshes/imu_pcb_22x17_ENVELOPE.stl": "2d69bb5823b3051730d5e48a13a2d4be6201987255d2dfa66a7a1471384c84fa",
"meshes/imu_screw_1.stl": "d3f6844119e807163351a70becd8bd455605ea719b3951eba6bba238769795c3",
"meshes/imu_screw_2.stl": "40a85687c4eccc74796b5ffa83f04328776a5cfa7c86ee3dde7d2348f51c8a56",
"meshes/imu_washer_1.stl": "9c144aa77c84da588b3eed18f7eb60405db698669da0e007776badceaca2352b",
"meshes/imu_washer_2.stl": "88eae70feffbde2e74b6d483297a86103b2f77699d694a3b37dcad4bfd36ef53",
"meshes/large_plate.stl": "cd797375c946c2973712035c78f74ca3da3a808f77de5a94462de28feca5f9cc",
"meshes/pitch_arm.stl": "26d08674a2381f1b2ffe84068fec0a47cb95347543d0f5cf655e11f566f5600c",
"meshes/plate_nut_1.stl": "53b4dd7491240e307057c3ff4a64a8bd0e470f83d8b005c15bceae64ccb52f04",
"meshes/plate_nut_2.stl": "4ce1eec26c8edd5b04011c195fb67ea31a111c5632e6783bc3ac3e0be174da2c",
"meshes/plate_nut_3.stl": "86d8b1aee6baab9d5bb853b7331afe549fca1409cfabd2e0bf63204f3134f52f",
"meshes/plate_nut_4.stl": "ea64d3f603f63931e544ef0240b46465553bc6b6f17bfa05021882a383886932",
"meshes/plate_screw_1.stl": "ad4e3c97054a9a655cbac763185575804e63e8fcd4495830d293b8847857294f",
"meshes/plate_screw_2.stl": "a985bc7f275689f054aad2562a2060b0737e369aa2216fedffeb40ee51e772d6",
"meshes/plate_screw_3.stl": "9c876e54d865a1aa98ad9e8c557105392ce08428ad060ae2e5d930f0372eaf0b",
"meshes/plate_screw_4.stl": "cb43d0c158a3461fe31c0de93d278a9788926aca31e3013f139694f12bca305c",
"meshes/plate_washer_1.stl": "768ed4e2e8269420f2772c42a0dac607d17a2991c1938b2b7da1d7a617ccace1",
"meshes/plate_washer_2.stl": "775e588c36ba3d9a640bba54a97e05c7b9fbb1752994f435dcad89098d75d737",
"meshes/plate_washer_3.stl": "810e765b1a268c27650c22545a99c35dab591ea9f2e5f091dbccfd0ced05dc41",
"meshes/plate_washer_4.stl": "a3ba32664e324b3b0eeb937a313e8156ad1f61e7a9c86ad9499549d65f952e7f",
"meshes/roll_frame.stl": "200203bcd121f9b1cd3172489b31c1cbf93c20bbaebc94d477301f81c068dcde",
"meshes/servo1.stl": "14238a5a21cfa642abbc493f78316b4e7a1e4d0be2fd81e41c99370cab420f7d",
"meshes/servo2.stl": "e9dd6de0002af8a1648527f31d45183bbb6b1a34f9d713b8ba5e1fb639c563a9",
"meshes/sg90_mount.stl": "7487cd3771a024419beeaebb4ce224060ca84357205f72f2ec1e6fd569accf21",
"model.xml": "4d271da65bd1dfad0ec5401398e9cad83e7d490a35d6f2628215bbc11c46f8d1",
"physics.xml": "94214f8148db2b1f68f368fe1b6fcae3c1947b169c8d8699b5cddefdfd12e80a",
"policy.py": "3cfbcd1135c260dd71cf6fd30b89e9b57e7f1c98366332a92d2e68010544df2b",
"position_physical.py": "9c6c327192e4b24fd63b9a4ea28ec1550ea7737c8deb4204884e1e465d95cfa2",
"post_stop_audit.json": "9b03453cb56ee046c4ee5c4446a04693d7cd6e8d437b907499148dd3cda8dfa1",
"probe_contacts/demonstrations.npz": "1921f12fd4692b3ecdb26829211bf2c0079cd3eba181a87834e68c365fb017b9",
"probe_contacts/report.json": "5410dfb32eba22f02a9dbcc270d73b7fe434bcbf8ded7baaea7b4c18bbcfc38a",
"replay.py": "4e32f065e56ad9b20db4ace386b1e022142bc2dd911efa8a240ad65fdd8e4f04",
"replay_support.py": "22815726ac40e1b03c3bc1c7984f35e5db63dd795fb9b0abef8f4a9028ada807",
"replays/back_70040000_success.mp4": "f218fbf601f11feb4d75ee4384e6ed7ee7cdc6d44e5494f05a38b60341368453",
"replays/back_70040000_success.npz": "6871477fdf59ace6d82b1f28875f1e7f9f28c3e0ff081a35629b7382f3e0d012",
"replays/back_70040000_success.png": "c8950d76287aefa68be406e403b668294a1fad7284b0fce24ae98b11d7c7cadb",
"replays/back_70040001_failure.mp4": "c896ba9888e06582a0609d48f9bbf29780cb8709f17dee35f71056330043fa94",
"replays/back_70040001_failure.npz": "5371d6b31898b16494bb4d15519d390c4a098fd8f6d120e924d2c6da3cd27a90",
"replays/back_70040001_failure.png": "5c9f2284c231fb6cdd12ae909767a6ac1de4ca1978fbb118afc1e9f26ef1a630",
"replays/back_left_70030000_success.mp4": "8a3b12d69cc76960a29b22e67e0837fef4c4dac2a0ce91679be8f26759c28913",
"replays/back_left_70030000_success.npz": "618193b293c71e983fa9929eba4e5714622e7348b8de642b1862c4b036c86470",
"replays/back_left_70030000_success.png": "9fb45488e5c18f74f393df88ab24a36e9f030849069528546196c1341e428ac9",
"replays/back_left_70030076_failure.mp4": "f9707452ac7f16435f651939b55bd09169789b0c86b34cffcdcfce3f64c5d87e",
"replays/back_left_70030076_failure.npz": "a7c4ad814682cf7fd1dde9c5d06285eb059c7f553346b53e9ae29b780e0d4081",
"replays/back_left_70030076_failure.png": "03b14b1b653b407d112d667580c31c1c68640a3600bb644a95e0ea480637f783",
"replays/back_right_70050000_success.mp4": "ebba6daef2d44dded9a965c92111756b269ca06b86f4839b3feeb6b6096a41f0",
"replays/back_right_70050000_success.npz": "b8dae0a930b01556e465841332b7b26570db1bd53d8b1bc961291a42786002fe",
"replays/back_right_70050000_success.png": "2bb8b7c22848f07e79b504af6aa5601f2a5fb2a4853523e4d698a3677d4a2b34",
"replays/back_right_70050150_failure.mp4": "7d9de68ca890321d6056e2fb7fed3e0696b298fa2c99b7160ab547a89f4ead65",
"replays/back_right_70050150_failure.npz": "c8fdfa75e5c13883ee3afe1fcca44035edb727f1b8c0aba8d479a8aa2ecb580a",
"replays/back_right_70050150_failure.png": "098e8f89edb231f6f03b365dc6b7baae5efb8378cedf7be8120eb91d6397a335",
"replays/front_70000000_success.mp4": "5419d1780d08819abe872f281be01cbddf71ba034f2b48f8a0605acb4958e95a",
"replays/front_70000000_success.npz": "92d06f9b490de93e048f57bea06509b52455221683a8b33b02e3cc936cc20725",
"replays/front_70000000_success.png": "7922723dddb37ff0867faf322f7042976030ec20948130476f8cc25dbe860415",
"replays/front_70000068_failure.mp4": "bdf2f90399cbf6555388b50b8211a1645a5ff3785e1cca12c48a990eb37f17fa",
"replays/front_70000068_failure.npz": "00390f9656b8a3e3e8cde773ce2548b17c037232bb1d794f05f69f31cca6f045",
"replays/front_70000068_failure.png": "5721c01c23e0cdb589bbbf9d92a592c68735e8be7e00a0465009f0f47b3b3806",
"replays/front_left_70010000_success.mp4": "1b3c7497f8acdb43091492b809a8206be530bdae68b702e1450e7c91c6112db2",
"replays/front_left_70010000_success.npz": "daf35d7820178b8b4b485f002a0fa6d824c13e47736ad916a2eb10f43e464541",
"replays/front_left_70010000_success.png": "a20c20ba58f00899340e1b5737b609cbe1542257e68fd47d0ace4d333b815b8d",
"replays/front_left_70010058_failure.mp4": "3754cc4a794d543df19f8c337939cc2e9902e5f1590cc79161bf7798e6427169",
"replays/front_left_70010058_failure.npz": "b88bd9d700cbf627a4b48127fcc8aab5e1bcca04022b05ab1a0e5705d23b42bf",
"replays/front_left_70010058_failure.png": "7c7cbb5c42b234bcb9da042eec7defa42459514b3aff5a69e9f26c86adcf8964",
"replays/front_right_70070000_success.mp4": "476dfc854682476b6dedabd892d3f4cf24497920d30cc35dcf78f92554411783",
"replays/front_right_70070000_success.npz": "914baf5be47e2300cfd137af471c5cb7432f1ebabcedb8d558af271a970833b0",
"replays/front_right_70070000_success.png": "88d8f1a9863a2e18bad206b2bf81920f15f4177975f0ebe8bb23c7ebf9690fda",
"replays/left_70020000_success.mp4": "b0556206d5b2d8df77e65f376a373145f989593dd4fb52d8f3c9dda60718eff4",
"replays/left_70020000_success.npz": "be820d9529dcbbb6d57630555ff19e6f7f6a366c90d646861bf535abe679a2da",
"replays/left_70020000_success.png": "e3947d10d1b7332dc843a8bc95039720d03b3cc18d3989ba76d63bb58ac8d2d6",
"replays/left_70020032_failure.mp4": "9e17f3889441525c75e632f4cd18150dc52370380a454a722f2db97640db9573",
"replays/left_70020032_failure.npz": "f13e4d88dd349716bd73b2ad32f4ed8d1b652af00e411797c6d61e0a00972403",
"replays/left_70020032_failure.png": "60e67a37393a54e50e81a64c8b6eb9ff14a78c6063162dd85d85afd5af41415d",
"replays/manifest.json": "d04fe14bee2eb201bfd200d69f12d84b85c4049c8108f45bfffc7b09ff7b819f",
"replays/right_70060001_success.mp4": "5d741572a234de6064ba08c2736c2e4ac634d74cb0f18118fd233a893869e043",
"replays/right_70060001_success.npz": "e53f03f5e013beb646645c2602d82142c0f4c7d4ae458bd99cf2e0af5c24e414",
"replays/right_70060001_success.png": "40fe563caaed140b37a7eee19e9fed3b14db2048ef5d2463a89d7ba037f7fe75",
"report/data.json": "9ea8cfa689586420a6368bd433ae832c620c6aa97c2d6fbeb536e9be3a08e8f9",
"report/index.html": "55c79339bee8d72e2b9d297cc7e22a046aa4d5760a0e005dbeb11fc21cce1c52",
"report.py": "e78a44f68eb3e61ea80707f20508912cf69a5d7c9da0bfc4b85009d1516d3c08",
"requirements-runtime.txt": "b773c28b8e0d3b69a551f5ffc0ef6077103b2404d0fc3103ef096a26475d5b46",
"requirements.txt": "78d58a7c0883b6a10a4088f36efa6bd04ba4577a15b541c3d4fef2e19dd7ef97",
"run_demo.py": "091ddf811770b645f0c363916daf95dd65740cb64def8332f99fe33accafb65b",
"selected/actor.npz": "7626bbef2099172970966085bcd8d08d85e304113c3f120b323834e348c74e0f",
"selected/identity.json": "47904e435a888963dbda09f9ca5f90e0c60611e7303af3d67172b77d7e026919",
"selected/policy.zip": "2e426cd99bbf3d125ac81b177aeeb5bfcf24a74997f34d33715d291544ff2402",
"self_collision.json": "3370662c1056b40a68de45ecce2f8a25809246117840914aa6b1ea0448aa6615",
"sensor_contract.json": "d84653df5b2c824d4f7378035f8b02fe2e6052697ce25dc1bc8854b3ab01a6ae",
"sensor_model.py": "0f5a6bed0225f8dca425717758b58bfb937022a36abb0f8deca77d91257e14f9",
"settled_direction_audit.json": "353836450beb3a975fc3c2b23b4a1d2cdebcdc38456f8ad41447eaea105b5104",
"speed.cpp": "47cfdd14fbddc00a7be44b5ac31b0b469ec5e95cd245186975275a4f435c86aa",
"speed.dylib": "84afe2a9e5e058193c2e18945aa5289a69bd623b1d6cdc4f416fd628b250c553",
"stop_audit.py": "d6572f97105c0dd302cc2f57325a6f7bb79900ab3b58575290113f9993b7bf58",
"test_selected.json": "5c782d847079f55437bbf197f5a1b13b9f5687ae503ba427317bc458e2529df2",
"test_speed.py": "ede58694edd052edf2fbcc903f9e29efa2e7b9ea4167518158b95528776926af",
"train.py": "69eaf1e337d6c6f8e3e7f8dc343905ed3773c1bc727dd4c83cfc258511a2eecc",
"training_progress.jsonl": "dc3e3d657b6b02461fdef3f249ceedce26b6d103193cb138e0861af0af5b5c6a",
"validation_131076.json": "cf69cd324294f5e6fe632b4fb1a5e9e64670a3ffc941fcceb02aa951859abe2d",
"validation_196614.json": "cf3a7e36f03d3e8e61190f27e40ffbed2fe05a2e7b2e4dcb9593a330e2a9a94f",
"validation_262152.json": "1baffd988f41d4a8684241e2f1e1af3c3a426eabfa5589b8ded8b8769dfa1466",
"validation_65538.json": "530b229508614b73f875d82815898320f96eed487d5aa65cbcae9b61c0494f22",
"validation_initial.json": "b778095a3bc07d0d9320b3dec1659679fe4ab53833c981f4e9cd359d645eb9e4",
"validation_large_196614.json": "df6d46f3592d77d583abc72e1c342176a2d358f7a0f5843c76fec851ad089a7b",
"validation_stop.json": "25ea31ef4d78793aada30366897d10f7b4f3c6505b0479ffd65bec05504e0235",
"video_checks.json": "c8095f28d39e20422108538d23118de780f1a104973c9fe424e825783753be5b"
}
}

View File

@ -0,0 +1,51 @@
# SG90 360° 连续旋转舵机:速度控制仿真与训练
当前结果、14 段回放和原始统计在原仿真网站:<http://127.0.0.1:8766/continuous360/>。下载包解压后也可打开 `report/index.html` 离线查看。
## 结论与统计口径
在这份近似物理模型中,**不增加关节传感器也能学会倒地起身**。这不证明现有实物已能运行,也不代表持续摇摆平衡或机械零位自动复位。
冻结策略在 1600 个有效倒地初态上成功 **1581 次,98.8125%**,八个设置落下方向各 200 次。前/前左/左/后左/后/后右/右/前右分别成功 **198/197/198/196/196/196/200/200** 次。19 次失败中,13 次触发原 IMU 量程保护,6 次在 8 秒内未完成连续站稳;没有放宽保护或成功判据。
**不能把条件成功率当作任意启动的成功率。**另有 1090 个筛除初态:514 个已经站立,无需起身;576 个在原 4 秒等待内未完成 IMU 近静止初始化。若把这 576 次启动失败也计为失败,排除无需起身的初态,启动加起身为 **1581/2176 = 72.66%**。全部种子和筛除原因保留在 `test_selected.json`。
**不能把设置落下方向当成静置后实际朝向。**落下期间机构会自然滚动。`settled_direction_audit.json` 重新按初始化 IMU up 分类:实际前/前左/左/后左/后/后右/右/前右的回合数为 **223/12/533/2/238/29/549/14**,成功数为 **220/12/524/2/232/29/548/14**。因此当前结果证明存在可用的起身策略,但不能声称所有静置斜向均已获得充分样本验证。这项审计没有用于调整模型。
补充停转检查:按每个设定方向的 seed 取前20个成功例,共160个,成功后再请求零速度2秒。148/160仍满足完整站稳判据,所有底板均保持朝上,12例未全部满足接触/速度条件。见 `post_stop_audit.json`;这不是新增独立测试,也不改变1581/1600。
## 模型与训练身份
- 模型 SHA256:`7626bbef2099172970966085bcd8d08d85e304113c3f120b323834e348c74e0f`。
- 选定的是 `run1/step_196614`,权重在 `selected/actor.npz`,完整训练检查点在 `selected/policy.zip`。
- 先用读取模拟关节状态的参考控制器探测物理可行性,80 个原始启动中完成 5 次起身;它不是可部署策略,不计入新策略成功率。
- 从参考示例做行为模仿,再训练新 PPO 速度策略。Actor 只读取 196 维 IMU/历史命令;真实角度、速度和参数只供训练 critic 与仿真评分使用。`actor_input_audit.json` 验证改变 critic 输入不会改变 actor 输出。
- 原始验证种子从 60000000 开始,每方向 20 次。新一组扩大验证从 61000000 开始,每方向 100 次,选定策略为 791/800,最差方向 96%。达到所有设置方向验证至少 95% 后提前停止额外训练。
- `SELECTION.json` 在最终测试前冻结权重与规则。最终测试种子从 70000000 开始,每方向独立偏移 10000;不按测试结果回调模型。
- NumPy 导出与 Torch 推理最大误差约 `4.92e-7`。旧定位版权重和冻结模型文件未改动,见 `legacy_integrity.json`。
## 控制与物理模型
保留原 100×80×5 mm 底板、双 SG90、上部机构和固定在底板的 QMI8658C。X5、电池、外部电源及线缆负载不在活动刚体中。质量、惯量沿用 CAD 体积估算,未实测。
命令为两轴归一化速度/方向请求 `[-1,1]`,**不是目标角度,也不是旧 angle-to-PWM 接口**。驱动物理模型包含死区、停止偏差、响应滞后、带载速度-力矩关系;零速停止命令不产生位置保持。控制周期 56 ms,叠加原随机延迟;同一 224.2 Hz IMU 流、量程与量化保留。Actor 输入包含初始化 up、16 帧滤波 up/陀螺仪/上一速度命令/加速度/采样间隔,以及经过时间,不含真实关节角、关节速度、底座高度或位移。
[厂家连续旋转 SG90 规格](https://towerpro.com.tw/product/sg90-360-degree-continuous-rotation-servo/)在 4.8V 下标称 110 RPM、堵转 1.3 kg·cm。这里使用 0.08 N·m 的估算力矩上限,速度、力矩、死区、偏差和延迟随机化;死区与电压/速度/PWM关系并未在用户实物上测量。停止及转速语义可参考[连续旋转舵机控制说明](https://www.pololu.com/product/2820)。
去掉了旧的 ±60° 理想关节限位,未添加“自动回中”或读取真实角度的隐藏控制器。相邻刚体通过 560 对解析碰撞近似体检查自碰撞;超出假定 ±180° 线束半圈范围则失败,这只用于仿真评分,不会替 actor 限制动作。完整 CAD 全角度干涉、接插件和线束仍未验证。原先仅把越出 ±60° 记为失败的早期探测保留在 `probe/`,未拿它的结果冒充最终模型表现。
成功规则保留:**8 秒内完成至少连续 1 秒底板朝上、角速度/平移速度达标、地面接触仅来自底板支撑**。它不要求杆竖直、舵机回机械零位,也不表示永久站稳。
## 重放与运行
现有环境:
```sh
two_servo/.venv/bin/python training/continuous_sg90_20261112/run_demo.py --case right --seed 70060001 --out /tmp/continuous-episode.json
```
仅仿真运行需要 NumPy、MuJoCo、Gymnasium;依赖版本见 `dependencies.json`。原生加速库在 macOS/当前 MuJoCo 版本编译;其他平台可运行 `build_fast.py` 重编译,或给 `run_demo.py` 加 `--slow` 使用 Python 参考路径。`test_speed.py` 检查原生/参考轨迹一致、中位无位置锁定、无隐藏关节限位。
`replays/` 按每个设置方向的 seed 顺序取第一个成功和失败,共 14 段。右、前右各 200 次全部通过,因此没有本轮失败片段。每段视频经过不录制/录制两次轨迹逐值核对,与原评估终态及随机数状态一致。`manifest.json` 保存模型、评估文件和视频哈希,NPZ 保存帧状态与观测。
整个流程仅本机 MuJoCo 计算。没有 SSH、GPIO、PWM 或真机部署;没有改动用户的 IMU 校准、舵机接线或原部署模型。当前包不能直接交给旧板端角度驱动程序执行。

View File

@ -0,0 +1,11 @@
{
"reason": "Early stopping after a frozen candidate reached >=95% in every direction on 800 new validation starts; independent test not yet run",
"selection_validation_file": "validation_large_196614.json",
"selection_validation_sha256": "df6d46f3592d77d583abc72e1c342176a2d358f7a0f5843c76fec851ad089a7b",
"selected_step": 196614,
"actor_sha256": "7626bbef2099172970966085bcd8d08d85e304113c3f120b323834e348c74e0f",
"independent_test_seed_base": 70000000,
"test_count_per_direction": 200,
"test_used_for_selection": false,
"hardware_connected": false
}

View File

@ -0,0 +1,21 @@
{
"files": {
"environment.py": "b82a7d0c0124c7bc40f7cb4d01d85e753b3d4f0dfde70db0dbdcb79cb5ae7013",
"position_physical.py": "9c6c327192e4b24fd63b9a4ea28ec1550ea7737c8deb4204884e1e465d95cfa2",
"policy.py": "3cfbcd1135c260dd71cf6fd30b89e9b57e7f1c98366332a92d2e68010544df2b",
"physics.xml": "94214f8148db2b1f68f368fe1b6fcae3c1947b169c8d8699b5cddefdfd12e80a",
"model.xml": "4d271da65bd1dfad0ec5401398e9cad83e7d490a35d6f2628215bbc11c46f8d1",
"imu_orientation.py": "fc6048e029b7059832ddda3731d5151ab089b7396006ae31ba71a25edaa8772f",
"sensor_model.py": "0f5a6bed0225f8dca425717758b58bfb937022a36abb0f8deca77d91257e14f9",
"continuous_dynamics.json": "317aba7548857f4823e56a08618f5de51ef44534912517e0d199a81712ebeac1",
"speed.cpp": "47cfdd14fbddc00a7be44b5ac31b0b469ec5e95cd245186975275a4f435c86aa",
"speed.dylib": "84afe2a9e5e058193c2e18945aa5289a69bd623b1d6cdc4f416fd628b250c553",
"train.py": "69eaf1e337d6c6f8e3e7f8dc343905ed3773c1bc727dd4c83cfc258511a2eecc",
"add_contacts.py": "d6833f17f9ad7b0f65e214cf16a4ff8e6491c597acb450c8f6a83a531dd88f7d",
"self_collision.json": "3370662c1056b40a68de45ecce2f8a25809246117840914aa6b1ea0448aa6615"
},
"source_position_package": "delivery/sector2_test5m",
"actor_inputs": "196 IMU and commanded speed history only",
"privileged_data": "critic and feasibility oracle only; excluded from exported actor",
"hardware_connected": false
}

View File

@ -0,0 +1,9 @@
{
"actor_sha256": "7626bbef2099172970966085bcd8d08d85e304113c3f120b323834e348c74e0f",
"actor_input_dim": 196,
"critic_input_dim": 222,
"critic_perturbation_does_not_change_actor": true,
"numpy_torch_max_error": 4.917383193969727e-07,
"only_imu_and_command_history": true,
"hardware_connected": false
}

View File

@ -0,0 +1,23 @@
"""Audit settled IMU directions without changing the frozen policy or test outcomes."""
from concurrent.futures import ProcessPoolExecutor
import collections,json,math
import numpy as np
from environment import RecoveryEnv,HERE,CASES
def check(direction):
env=RecoveryEnv();rows=[]
for row in direction['episodes']:
obs,info=env.reset(seed=row['seed'],options={'case':row['case']})
assert not info['invalid_reset'];np.testing.assert_array_equal(info['initial_q'],row['initial']['initial_q'])
sector=int(np.rint((math.atan2(float(obs[1]),float(obs[0]))-math.pi)/(math.pi/4)))%8
rows.append(dict(seed=row['seed'],configured_case=row['case'],settled_case=CASES[sector],initial_up=obs[:3].tolist(),is_success=row['is_success']))
return rows
if __name__=='__main__':
test=json.loads((HERE/'test_selected.json').read_text())
with ProcessPoolExecutor(6) as p:rows=[r for group in p.map(check,test['directions']) for r in group]
summaries=[]
for case in CASES:
group=[r for r in rows if r['settled_case']==case];summaries.append(dict(case=case,total=len(group),successes=sum(r['is_success'] for r in group)))
out=dict(scope='Settled direction classified by initialized measurable IMU up; original table uses configured drop directions',same_configured_and_settled=sum(r['configured_case']==r['settled_case'] for r in rows),total=len(rows),by_settled_direction=summaries,episodes=rows)
(HERE/'settled_direction_audit.json').write_text(json.dumps(out,indent=2));print({k:v for k,v in out.items() if k!='episodes'})

View File

@ -0,0 +1,12 @@
from pathlib import Path
import subprocess
import sys
import mujoco
here=Path(__file__).resolve().parent
package=Path(mujoco.__file__).resolve().parent
library=next(package.glob('libmujoco*.dylib' if sys.platform=='darwin' else 'libmujoco.so*'))
output=here/('speed.dylib' if sys.platform=='darwin' else 'speed.so')
subprocess.run(['clang++','-O3','-ffp-contract=off','-std=c++17','-shared','-fPIC','-I'+str(package/'include'),
str(here/'speed.cpp'),str(library),'-Wl,-rpath,'+str(package),'-o',str(output)],check=True)
print(output)

View File

@ -0,0 +1,39 @@
{
"actuator": "continuous rotation: normalized signed drive / speed request, not angle",
"manufacturer_source": "https://towerpro.com.tw/product/sg90-360-degree-continuous-rotation-servo/",
"reference_rpm_4v8": 110,
"reference_stall_nm_4v8": 0.12748645,
"reference_voltage": 4.8,
"assumed_torque_cap_nm": 0.08,
"assumed_no_load_rad_s": 11.519173063162576,
"assumed_deadband": 0.06,
"assumed_response_lag_s": 0.04,
"assumed_joint_damping": 0.001,
"period_s": 0.056,
"randomization": {
"torque_multiplier": [
0.75,
1.35
],
"speed_multiplier": [
0.75,
1.15
],
"deadband": [
0.025,
0.1
],
"lag_s": [
0.02,
0.07
],
"neutral_offset": [
-0.015,
0.015
]
},
"joint_range_policy": "No physical hinge limits or angle feedback in actor. Explicit self-collision proxy pairs. Beyond +/-180deg fails as assumed cable half-turn envelope, not a physical servo limit. Geometry outside original +/-60deg CAD samples remains unvalidated.",
"neutral_model": "Within deadband: zero motor torque; passive joint damping only, no angle hold",
"measured_on_this_robot": false,
"hardware_connected": false
}

View File

@ -0,0 +1,9 @@
{
"numpy": "2.2.6",
"mujoco": "3.17.0",
"gymnasium": "1.4.0",
"torch": "2.9.1",
"stable-baselines3": "2.9.2",
"imageio": "2.37.0",
"Pillow": "12.3.0"
}

View File

@ -0,0 +1,24 @@
{
"servo": "SG90",
"source": "https://towerpro.com.tw/product/sg90-analog/",
"reference_voltage_v": 4.8,
"servo_mass_kg": 0.009,
"reference_stall_torque_nm": 0.1765197,
"reference_no_load_speed_rad_s": 8.72664626,
"sim_torque_cap_nm": 0.10,
"sim_torque_random_multiplier": [0.7, 1.2],
"sim_speed_random_multiplier": [0.75, 1.15],
"command_slew_rad_s": 6.0,
"position_kp": 0.45,
"position_kv": 0.018,
"joint_limit_deg": 60,
"period_s": 0.056,
"measured_on_this_robot": false,
"notes": [
"9g, 1.8kgf cm stall and 0.12s per 60deg are manufacturer specifications at 4.8V, not measurements of this unit.",
"The page also lists 0.1s per 60deg in its secondary table; the main specification 0.12s is used as nominal.",
"0.10Nm is an engineering simulation cap below stall, not a measured continuous torque rating.",
"PD gains, randomization, command slew and loaded speed response remain uncalibrated.",
"+/-60deg is the sampled CAD range, not a verified continuous mechanical/PWM limit."
]
}

View File

@ -0,0 +1,120 @@
"""Continuous-drive task. Actor sees only the existing IMU and command history."""
from collections import deque
from pathlib import Path
import ctypes
import json
import os
import sys
import gymnasium as gym
import mujoco
import numpy as np
os.environ['FAST_PHYSICS']='0'
from position_physical import PhysicalEnv, CASES, LIMIT, DT
HERE=Path(__file__).resolve().parent
CONFIG=json.loads((HERE/'continuous_dynamics.json').read_text())
OBS_DIM=196
class RecoveryEnv(PhysicalEnv):
def __init__(self,randomize=True,visual=False,fast=True,**ignored):
super().__init__(randomize=randomize,visual=visual)
self.observation_space=gym.spaces.Box(-np.inf,np.inf,(OBS_DIM,),dtype=np.float32)
self.motor_state=np.zeros(2,dtype=np.float64)
self.drive_bias=np.zeros(2);self.deadband=np.full(2,.06);self.lag=np.full(2,.04)
self._speed_native=None
if fast:
lib=ctypes.CDLL(str(HERE/('speed.dylib' if sys.platform=='darwin' else 'speed.so')))
self._speed_native=lib.speed_steps
ptr=ctypes.POINTER(ctypes.c_double)
self._speed_native.argtypes=[ctypes.c_void_p,ctypes.c_void_p,ctypes.c_int,ctypes.c_int,ctypes.c_int,
*([ptr]*7),ctypes.POINTER(ctypes.c_int),ctypes.c_int,ptr,ctypes.POINTER(ctypes.c_int)]
self._speed_native.restype=ctypes.c_int
def observation(self):
raw=super().observation()
now=float(self.data.time)
if self._obs_time is None:self._epoch=now;self._initial_up=raw[:3].copy();delta=0.
else:delta=now-self._obs_time
frame=np.r_[raw,self.accel_body_g,delta/.07].astype(np.float32)
if not self.history:self.history.extend([frame.copy() for _ in range(16)])
else:self.history.append(frame)
self._obs_time=now;self.raw=raw.copy()
return np.r_[self._initial_up,np.concatenate(self.history),(now-self._epoch)/8].astype(np.float32)
def reset(self,*,seed=None,options=None):
self.history=deque(maxlen=16);self._obs_time=None;self._settling=True
self.motor_state[:]=0;self.range_failed=False
obs,info=super().reset(seed=seed,options=options)
r=self.np_random
self.speed[:]=CONFIG['assumed_no_load_rad_s'];self.torque[:]=CONFIG['assumed_torque_cap_nm']
self.drive_bias[:]=0;self.deadband[:]=.06;self.lag[:]=.04
if self.randomize:
self.speed*=r.uniform(.75,1.15,2);self.torque*=r.uniform(.75,1.35,2)
self.drive_bias[:]=r.uniform(-.015,.015,2);self.deadband[:]=r.uniform(.025,.10,2);self.lag[:]=r.uniform(.02,.07,2)
self.model.actuator_gainprm[:,0]=1;self.model.actuator_biasprm[:]=0
self.model.actuator_forcerange[:]=np.c_[-self.torque,self.torque]
self.previous=np.zeros(2);self.servo_target=np.zeros(2);self.motor_state[:]=0
self._settling=False
if np.any(np.abs(self.data.qpos[self.qids])>np.pi):self.invalid='initial_exceeded_half_turn_cable_envelope'
self.history.clear();self._obs_time=None
if not self.invalid:self.last_obs=self.observation()
else:self.last_obs=np.zeros(OBS_DIM,dtype=np.float32)
self._last_up=float(self.data.xmat[self.torso,8]);self._stall_s=0.
info.update(invalid_reset=self.invalid,initial_q=self.data.qpos[self.qids].tolist(),actuator='continuous_speed',motor_command=[0.,0.])
return self.last_obs.copy(),info
def _physics_mechanics(self,target,n):
command=np.zeros(2) if self._settling else np.asarray(target,dtype=float)
if self._speed_native:
ptr=ctypes.POINTER(ctypes.c_double)
array=lambda a:np.ascontiguousarray(a,dtype=np.float64)
args=[array(a) for a in (command,self.drive_bias,self.deadband,self.lag,self.torque,self.speed)]
supports=(ctypes.c_int*len(self.supports))(*sorted(self.supports))
hold=ctypes.c_double(float(getattr(self,'hold',0.)));failed=ctypes.c_int(self.range_failed)
status=self._speed_native(self.model._address,self.data._address,n,int(self.active and not self._settling),self.torso,
*[a.ctypes.data_as(ptr) for a in args],self.motor_state.ctypes.data_as(ptr),supports,len(self.supports),ctypes.byref(hold),ctypes.byref(failed))
if status:raise RuntimeError('Speed physics ABI validation failed')
self.hold=hold.value;self.range_failed=bool(failed.value)
return
for _ in range(n):
effective=command+self.drive_bias
drive=np.sign(effective)*np.maximum(np.abs(effective)-self.deadband,0)/(1-self.deadband)
drive=np.clip(drive,-1,1)
self.motor_state+=(drive-self.motor_state)*(.002/(self.lag+.002))
force=self.torque*np.clip(self.motor_state-self.data.qvel[self.vids]/self.speed,-1,1)
force=np.where((drive==0)&(np.abs(self.motor_state)<.002),0.,force)
self.data.ctrl[:]=force;mujoco.mj_step(self.model,self.data)
if self.active and not self._settling:
self.range_failed|=bool(np.any(np.abs(self.data.qpos[self.qids])>np.pi))
_,stable=self.metrics();self.hold=self.hold+.002 if stable and not self.range_failed else 0.
def step(self,action):
action=np.asarray(action,dtype=float)
if action.shape!=(2,) or not np.isfinite(action).all():raise ValueError('Expected two finite signed speed commands')
if self.invalid:return self.last_obs.copy(),-25.,True,False,dict(case=self.case,is_success=False,failure=self.invalid,elapsed_s=0.,hold_s=0.,up_z=self._last_up,peak_up=self.peak_up)
delay=int(self.np_random.integers(16)) if self.randomize else 0
old_time=float(self.data.time);self.physics(self.servo_target,delay)
delta=DT+delay*.002
command=np.clip(np.clip(action,-1,1),self.previous-6*delta,self.previous+6*delta)
old=self.previous.copy();self.previous=command.copy();self.servo_target=command.copy()
self.physics(command,28)
up,stable=self.metrics();self.peak_up=max(up,self.peak_up)
elapsed=float(self.data.time)-self.started;fault=None
try:self.last_obs=self.observation()
except ValueError as e:fault='IMU: '+str(e)
if self.range_failed:fault='exceeded_half_turn_cable_envelope'
success=bool(self.hold>=1 and elapsed<=8 and not fault)
done=bool(success or elapsed>=8 or fault)
self._stall_s=self._stall_s+delta if up<.95 and abs(up-self._last_up)<.002 else 0.
reward=4*(up-self._last_up)+delta*(.25*up+4*float(stable)-2*float(self._stall_s>.7))
reward-=.003*float(action@action)+.003*float(np.sum((command-old)**2))
reward-=.25*delta*max(0,float(np.linalg.norm(self.raw[3:6]))-3)**2
if success:reward+=75
elif done:reward-=25
self._last_up=up
info=dict(case=self.case,is_success=success,up_z=up,peak_up=self.peak_up,hold_s=self.hold,elapsed_s=elapsed,
requested_action=action.tolist(),speed_command=command.tolist(),control_dt=float(self.data.time)-old_time)
if fault:info['failure']=fault
return self.last_obs.copy(),float(reward),done,False,info

View File

@ -0,0 +1,51 @@
"""Independent initial-state evaluation, explicit invalids and fixed direction slices."""
import argparse
import collections
import hashlib
import json
from pathlib import Path
import time
from concurrent.futures import ProcessPoolExecutor
import numpy as np
from environment import RecoveryEnv,CASES,HERE
from policy import Policy
def run_case(task):
actor_path,case,count,seed,randomize=task
actor=Policy(actor_path) if actor_path else lambda obs:np.zeros(2)
env=RecoveryEnv(randomize=randomize);episodes=[];invalid=[];attempt=0
while len(episodes)<count:
s=seed+attempt;attempt+=1;obs,initial=env.reset(seed=s,options={'case':case})
if initial['invalid_reset']:
invalid.append(dict(seed=s,case=case,reason=initial['invalid_reset']))
if attempt>count*10:raise RuntimeError('Too many invalid starts')
continue
for _ in range(150):
obs,_,done,_,info=env.step(actor(obs))
if done:break
else:raise AssertionError('Missing 8s termination')
episodes.append(dict(seed=s,initial=initial,**info))
return dict(case=case,successes=sum(r['is_success'] for r in episodes),total=count,episodes=episodes,invalid_resets=invalid)
def evaluate(path,count,seed,workers=6,randomize=True):
tasks=[(str(path) if path else None,c,count,seed+10000*i,randomize) for i,c in enumerate(CASES)]
start=time.monotonic()
with ProcessPoolExecutor(workers) as pool:rows=list(pool.map(run_case,tasks))
return dict(actor_sha256=hashlib.sha256(Path(path).read_bytes()).hexdigest() if path else None,
seed_base=seed,randomize=randomize,successes=sum(r['successes'] for r in rows),total=8*count,
success_rate=sum(r['successes'] for r in rows)/(8*count),wall_s=time.monotonic()-start,directions=rows,
invalid_reset_count=sum(len(r['invalid_resets']) for r in rows),
rule='<=8s and >=1s continuous stable hold, plate-only ground contacts; CAD proxy collisions and half-turn cable bound',
observations='IMU/command history196 only',hardware_tested=False)
def main():
p=argparse.ArgumentParser();p.add_argument('--actor',type=Path);p.add_argument('--count',type=int,default=20);p.add_argument('--seed',type=int,default=60000000);p.add_argument('--workers',type=int,default=6);p.add_argument('--out',type=Path,required=True);p.add_argument('--nominal',action='store_true');a=p.parse_args()
r=evaluate(a.actor,a.count,a.seed,a.workers,not a.nominal);a.out.parent.mkdir(parents=True,exist_ok=True);a.out.write_text(json.dumps(r,indent=2)+'\n')
print(json.dumps({k:v for k,v in r.items() if k!='directions'}),flush=True)
for d in r['directions']:print(d['case'],d['successes'],d['total'],flush=True)
if __name__=='__main__':main()

View File

@ -0,0 +1,127 @@
"""Offline attitude observation from QMI8658C samples; no I/O or motor control.
Accelerometer specific force points UP at rest. In sensor coordinates a fixed
world-up vector obeys d(up)/dt = -omega cross up. up_z is its dot product with
the board-up direction measured while the base was flat; absolute yaw is unused.
Reference samples must be physically stationary and fixed to the board, then pass
the checks below. A passing statistical check alone cannot prove that condition.
Gaps in this stationary averaging dataset are counted, not integrated. The runtime
interval limit still rejects every gap greater than 0.15 s without auto-resetting.
Runtime initialization also requires near-rest gyro and a reasonable accel norm.
Rejected samples raise ValueError without advancing the state; callers must not
reuse the last output as a fresh valid observation. reset() requires another quiet
initialization. This is not a validated board-side 50 Hz driver.
The 2 s complementary correction rejects accel norms outside 0.85..1.15 g, but
dynamic acceleration INSIDE that range can still corrupt tilt. Gyro-only periods
can drift. This estimator does not establish that a recovery controller is safe.
"""
import math
import numpy as np
MAX_DT_S = 0.15
GYRO_LIMIT_DPS = 1000.0 # Same fractional margin inside the configured +/-1024 dps range.
ACCEL_NORM_RANGE_G = (0.85, 1.15)
ACCEL_CORRECTION_TAU_S = 2.0
INITIAL_GYRO_LIMIT_DPS = 0.5
def _vector(value, name):
a = np.asarray(value)
if a.shape != (3,) or a.dtype.kind not in 'iuf' or not np.isfinite(a).all():
raise ValueError(name + ' must contain three finite numeric values')
return a.astype(float)
def _timestamp(value):
if isinstance(value, (bool, np.bool_)) or not isinstance(value, (int, float, np.integer, np.floating)):
raise ValueError('timestamp_s must be a finite monotonic timestamp')
value = float(value)
if not math.isfinite(value) or value < 0:
raise ValueError('timestamp_s must be a finite nonnegative monotonic timestamp')
return value
def _gyro(value):
gyro = _vector(value, 'gyro_dps')
if np.max(np.abs(gyro)) >= GYRO_LIMIT_DPS:
raise ValueError('Raw gyro is saturated or too close to its configured range limit')
return gyro
class ImuOrientation:
"""Build from stationary records containing accel_g, gyro_dps, elapsed_s."""
def __init__(self, reference_samples):
samples = list(reference_samples)
if len(samples) < 25:
raise ValueError('At least 25 stationary reference samples are required')
accel = np.stack([_vector(s['accel_g'], 'reference accel_g') for s in samples])
gyro = np.stack([_gyro(s['gyro_dps']) for s in samples])
times = np.asarray([_timestamp(s['elapsed_s']) for s in samples])
dt = np.diff(times)
if np.any(dt <= 0) or times[-1] - times[0] < 1:
raise ValueError('Reference timestamps must increase strictly and span >=1 s')
norms = np.linalg.norm(accel, axis=1)
if (np.any(norms < ACCEL_NORM_RANGE_G[0]) or np.any(norms > ACCEL_NORM_RANGE_G[1])
or np.max(np.std(accel, axis=0)) >= 0.02
or np.max(np.std(gyro, axis=0)) >= 0.5
or np.max(np.abs(np.mean(gyro, axis=0))) >= 10):
raise ValueError('Reference did not pass the stationary-data screening')
self.gyro_bias_dps = np.mean(gyro, axis=0)
self.reference_accel_g = np.mean(accel, axis=0)
self.reference_up_sensor = self.reference_accel_g / np.linalg.norm(self.reference_accel_g)
self.reference_sample_count = len(samples)
self.reference_gap_count = int(np.sum(dt > MAX_DT_S))
self.reference_max_gap_s = float(np.max(dt))
self.reset()
def reset(self):
"""Forget dynamic state; the next update must satisfy quiet initialization."""
self._up = None
self._last_timestamp = None
def update(self, accel_g, gyro_dps, timestamp_s):
"""Return a current observation, or raise; all input units are explicit.
Result: up_z [-1,1], up_sensor unit vector, timestamp_s, dt_s (None on
initialization), accel_correction_used and accel_norm_g. An out-of-range
accel norm is ignored AFTER initialization; gyro integration continues.
"""
accel = _vector(accel_g, 'accel_g')
gyro = _gyro(gyro_dps) - self.gyro_bias_dps
timestamp = _timestamp(timestamp_s)
norm = float(np.linalg.norm(accel))
if not math.isfinite(norm):
raise ValueError('Accelerometer magnitude is not finite')
accel_ok = ACCEL_NORM_RANGE_G[0] <= norm <= ACCEL_NORM_RANGE_G[1]
if self._last_timestamp is None:
if not accel_ok or np.max(np.abs(gyro)) > INITIAL_GYRO_LIMIT_DPS:
raise ValueError('Initialization requires reasonable gravity and near-rest corrected gyro')
up = accel / norm
dt = None
else:
dt = timestamp - self._last_timestamp
if not 0 < dt <= MAX_DT_S:
raise ValueError('Invalid or dropped-sample interval: require 0 < dt <= 0.15 s')
omega = np.deg2rad(gyro)
speed = float(np.linalg.norm(omega))
up = self._up.copy()
if speed > 0:
axis, angle = omega / speed, speed * dt
# Exact constant-rate rotation integrates the negative cross product.
up = (up * math.cos(angle) - np.cross(axis, up) * math.sin(angle)
+ axis * np.dot(axis, up) * (1 - math.cos(angle)))
if accel_ok:
weight = -math.expm1(-dt / ACCEL_CORRECTION_TAU_S)
up = (1 - weight) * up + weight * accel / norm
up /= np.linalg.norm(up)
if not np.isfinite(up).all():
raise ValueError('Orientation update is not finite')
self._up = up
self._last_timestamp = timestamp
return {'up_z': float(np.clip(np.dot(self.reference_up_sensor, up), -1, 1)),
'up_sensor': up.tolist(), 'timestamp_s': timestamp, 'dt_s': dt,
'accel_correction_used': accel_ok, 'accel_norm_g': norm}

View File

@ -0,0 +1,9 @@
{
"original_files_unchanged": {
"parent_actor.npz": "bef54767f3174a2496e1a59ff06a81533d791edd9f44eb71e2129bbe2b2a5982",
"expert_actor.npz": "2ea9c5d4aea0df6e3198f060cae4c2d0ae4245b90932d2cc51e23f208fe66d9a",
"runtime/physics.xml": "a922d11ec4e5ea3dd5521f4a711aac391b206f78fb5445885edae280d4bc6350",
"runtime/model.xml": "095d1ee392e4f7e65cf2d4b7afbc87c83766f97e40590377179e9dc62644361b",
"runtime/physical_env.py": "9c6c327192e4b24fd63b9a4ea28ec1550ea7737c8deb4204884e1e465d95cfa2"
}
}

File diff suppressed because one or more lines are too long

File diff suppressed because one or more lines are too long

View File

@ -0,0 +1,16 @@
"""Standalone NumPy speed-command policy; no simulated state or critic access."""
from pathlib import Path
import numpy as np
class Policy:
def __init__(self,path):
with np.load(Path(path),allow_pickle=False) as d:self.layers=[(d[f'w{i}'].copy(),d[f'b{i}'].copy()) for i in range(3)]
for (w,b),shape in zip(self.layers,[(64,196),(64,64),(2,64)]):
if w.shape!=shape or b.shape!=(shape[0],) or not np.isfinite(w).all() or not np.isfinite(b).all():raise ValueError('Invalid speed actor')
def reset(self):pass
def __call__(self,obs):
x=np.asarray(obs,dtype=np.float32)
if x.shape!=(196,) or not np.isfinite(x).all():raise ValueError('Expected measured IMU/command history196')
for w,b in self.layers:x=np.tanh(w@x+b)
return x

View File

@ -0,0 +1,279 @@
"""Eight measurable observations. Full state is used only for reward/evaluation."""
from pathlib import Path
import sys
import math
import json
import os
import gymnasium as gym
import numpy as np
import mujoco
HERE=Path(__file__).resolve().parent
import imu_orientation as _imu,sensor_model as _sensor
for _module in (_imu,_sensor):
if Path(_module.__file__).resolve().parent!=HERE:raise RuntimeError('Foreign IMU/sensor domain')
from imu_orientation import ImuOrientation
from sensor_model import quantize_accel_g,quantize_gyro_dps
CASES=('front','front_left','left','back_left','back','back_right','right','front_right')
LIMIT=math.radians(60)
DT=.056
DYNAMICS=json.loads((HERE/'dynamics.json').read_text())
IMU_RATE_HZ=1121/5
SENSOR_SEED_OFFSET=22419600
NOISE_DENSITY_FACTOR=math.sqrt(IMU_RATE_HZ/(1/DT))
ACCEL_NOISE_STD_G=.003*NOISE_DENSITY_FACTOR
GYRO_NOISE_STD_DPS=.03*NOISE_DENSITY_FACTOR
def imu_tick(sample_index):
"""Exact first 2ms tick at/after k/(1121/5) seconds, for k >= 1."""
if sample_index<1:raise ValueError('Sample index starts at one')
return (int(sample_index)*2500+1120)//1121
class PhysicalEnv(gym.Env):
def __init__(self, randomize=True, visual=False):
self.command_rate=6.
self._fast=None
if os.environ.get('FAST_PHYSICS')=='1':
import fast_core
if Path(fast_core.__file__).resolve().parent!=HERE:raise RuntimeError('Foreign C-physics wrapper')
from fast_core import FastPhysics
self._fast=FastPhysics()
self.model=mujoco.MjModel.from_xml_path(str(HERE/('model.xml' if visual else 'physics.xml')))
self.data=mujoco.MjData(self.model)
self.randomize=randomize
self.torso=self.model.body('torso').id
self.joints=self.model.actuator_trnid[:,0]
self.qids=self.model.jnt_qposadr[self.joints]
self.vids=self.model.jnt_dofadr[self.joints]
self.supports={i for i in range(self.model.ngeom) if self.model.geom(i).name.startswith('plate_support_')}
self.sensor_to_body=np.array([[0.,-1.,0.],[1.,0.,0.],[0.,0.,1.]])
self.nominal={n:getattr(self.model,n).copy() for n in
('body_mass','body_inertia','geom_friction','actuator_gainprm','actuator_biasprm','site_pos','site_quat')}
self.collisions=np.flatnonzero(self.model.geom_contype)[1:]
self.action_space=gym.spaces.Box(-1.,1.,(2,),dtype=np.float32)
self.observation_space=gym.spaces.Box(-np.inf,np.inf,(8,),dtype=np.float32)
self.reference=[dict(accel_g=[0,0,1],gyro_dps=[0,0,0],elapsed_s=i*.056) for i in range(25)]
self.sensor_rng=None
self._imu_stream_enabled=False
def _read_sensor_values(self):
"""Sensor-frame sample generation; independent of all physical randomization draws."""
a=self.data.sensor('imu_accelerometer').data.copy()/9.81
g=np.rad2deg(self.data.sensor('imu_gyro').data.copy())
if self.randomize:
a+=self.accel_bias+self.sensor_rng.normal(0,ACCEL_NOISE_STD_G,3)
g+=np.rad2deg(self.gyro_bias)+self.sensor_rng.normal(0,GYRO_NOISE_STD_DPS,3)
return a,g
def _start_sensor_stream(self,seed):
if seed is not None:
self.sensor_seed=int(seed)+SENSOR_SEED_OFFSET
self.sensor_rng=np.random.default_rng(self.sensor_seed)
elif self.sensor_rng is None:
self.sensor_seed=None
self.sensor_rng=np.random.default_rng()
self._imu_stream_enabled=True
self._imu_epoch_time=float(self.data.time)
self._imu_step_index=0;self._imu_next_tick=imu_tick(1)
self.sensor_sample_count=0;self.last_imu_sample_time=None
self.cached_accel_g=None;self.cached_gyro_dps=None
self.cached_accel_raw=None;self.cached_gyro_raw=None
self.imu_fault=None;self.imu_fault_time=None
self.sensor_gyro_guard_rejections=0;self.sensor_filter_rejections=0
self.sensor_accel_clipped_samples=0;self.sensor_gyro_clipped_samples=0
self.startup_sensor_faults=0;self._imu_quiet_duration=0.
self._imu_previous_attempt_time=self._imu_epoch_time
self.filter=ImuOrientation(self.reference)
def _latch_imu_fault(self,message,now):
if self.active:
if self.imu_fault is None:
self.imu_fault=str(message);self.imu_fault_time=now
else:
self.startup_sensor_faults+=1;self._imu_quiet_duration=0.;self.filter.reset()
def _sample_sensor_tick(self):
now=float(self.data.time)
dt=now-self._imu_previous_attempt_time
self._imu_previous_attempt_time=now
self.sensor_sample_count+=1
try:
a,g=self._read_sensor_values()
self.sensor_accel_clipped_samples+=int(np.any((a<-8)|(a>32767/4096)))
self.sensor_gyro_clipped_samples+=int(np.any((g<-1024)|(g>32767/32)))
raw_a,measured_a=quantize_accel_g(a);raw_g,measured_g=quantize_gyro_dps(g)
except ValueError as error:
self._latch_imu_fault('Invalid IMU sample: '+str(error),now);return
self.cached_accel_raw=raw_a.copy();self.cached_gyro_raw=raw_g.copy()
self.cached_accel_g=measured_a.copy();self.cached_gyro_dps=measured_g.copy()
self.last_imu_sample_time=now
if np.max(np.abs(measured_g))>=1000:
self.sensor_gyro_guard_rejections+=1
self._latch_imu_fault('Raw gyro is saturated or too close to its configured range limit',now)
return
if self.active and self.imu_fault is not None:return
if not self.active:
quiet=np.max(np.abs(measured_g))<.4 and .86<np.linalg.norm(measured_a)<1.14
self._imu_quiet_duration=self._imu_quiet_duration+dt if quiet else 0.
if not quiet:self.filter.reset()
if self._imu_quiet_duration<.3:return
try:self.filter.update(measured_a,measured_g,now)
except ValueError as error:
self.sensor_filter_rejections+=1;self._latch_imu_fault(str(error),now)
def sensors(self):
"""Read the same latest quantized cache; never sample or advance either RNG."""
if self.cached_accel_g is None or self.cached_gyro_dps is None:raise ValueError('No IMU stream sample available')
return self.cached_accel_g.copy(),self.cached_gyro_dps.copy()
def _predict_up_sensor(self,now):
if self.imu_fault is not None:raise ValueError('Latched fast IMU fault: '+self.imu_fault)
if self.filter._up is None or self.filter._last_timestamp is None:raise ValueError('IMU stream has no near-rest initialization')
dt=float(now)-self.filter._last_timestamp
if not 0<=dt<=.15:raise ValueError('IMU prediction age outside [0,.15] seconds')
up=self.filter._up.copy()
omega=np.deg2rad(self.cached_gyro_dps-self.filter.gyro_bias_dps);speed=float(np.linalg.norm(omega))
if speed>0 and dt>0:
axis=omega/speed;angle=speed*dt
up=up*math.cos(angle)-np.cross(axis,up)*math.sin(angle)+axis*np.dot(axis,up)*(1-math.cos(angle))
return up/np.linalg.norm(up)
def observation(self):
a,g=self.sensors()
self.accel_body_g=self.sensor_to_body@a
up_sensor=self._predict_up_sensor(float(self.data.time))
# No base height/velocity or measured joint angles enter the actor.
return np.r_[self.sensor_to_body@up_sensor, self.sensor_to_body@np.deg2rad(g-self.filter.gyro_bias_dps),
self.previous].astype(np.float32)
def metrics(self):
up=float(self.data.xmat[self.torso,8])
# Exact short circuit: tilted states cannot satisfy the existing stable predicate.
if up<=.98:return up,False
velocity=np.zeros(6)
mujoco.mj_objectVelocity(self.model,self.data,mujoco.mjtObj.mjOBJ_BODY,self.torso,velocity,1)
if np.linalg.norm(velocity[:3])>=.5 or np.linalg.norm(velocity[3:])>=.05:return up,False
contacts={int(c.geom2 if c.geom1==0 else c.geom1) for c in self.data.contact if 0 in (c.geom1,c.geom2)}
stable=(up>.98 and np.linalg.norm(velocity[:3])<.5 and np.linalg.norm(velocity[3:])<.05
and bool(contacts) and contacts.issubset(self.supports))
return up,stable
def reset(self,*,seed=None,options=None):
super().reset(seed=seed);r=self.np_random; options=options or {}
self.active=False
self.case=options.get('case',CASES[int(r.integers(len(CASES)))])
if self.case not in CASES:raise ValueError('Unknown fallen direction')
for n,v in self.nominal.items():getattr(self.model,n)[:]=v
self.speed=np.full(2,DYNAMICS['reference_no_load_speed_rad_s']);self.torque=np.full(2,DYNAMICS['sim_torque_cap_nm']);self.zero=np.zeros(2)
self.accel_bias=np.zeros(3);self.gyro_bias=np.zeros(3)
if self.randomize:
scales=r.uniform(.85,1.15,self.model.nbody);scales[0]=1
self.model.body_mass[:]*=scales;self.model.body_inertia[:]*=scales[:,None]
self.model.geom_friction[:,0]=r.uniform(.3,1.)
self.speed*=r.uniform(*DYNAMICS['sim_speed_random_multiplier'],size=2);self.torque*=r.uniform(*DYNAMICS['sim_torque_random_multiplier'],size=2)
self.zero=r.uniform(-math.radians(5),math.radians(5),2)
gain=r.uniform(.85,1.15,2)
self.model.actuator_gainprm[:,0]*=gain;self.model.actuator_biasprm[:,1]*=gain
sid=self.model.site('imu').id
self.model.site_pos[sid]+=r.uniform(-.002,.002,3)
perturb=np.zeros(4);angle=r.uniform(-math.radians(2),math.radians(2));axis=r.normal(size=3);axis/=np.linalg.norm(axis)
mujoco.mju_axisAngle2Quat(perturb,axis,angle)
q=self.model.site_quat[sid].copy();mujoco.mju_mulQuat(self.model.site_quat[sid],q,perturb)
self.accel_bias=r.uniform(-.006,.006,3);self.gyro_bias=np.deg2rad(r.uniform(-.12,.12,3))
mujoco.mj_setConst(self.model,self.data)
mujoco.mj_resetData(self.model,self.data)
angle=math.pi/2+r.uniform(-.15,.35)
azimuth=CASES.index(self.case)*math.pi/4+r.uniform(-.12,.12)
axis=np.array([-math.sin(azimuth),math.cos(azimuth),0.])
self.data.qpos[3:7]=np.r_[math.cos(angle/2),axis*math.sin(angle/2)]
self.data.qpos[self.qids]=r.uniform(-LIMIT,LIMIT,2)
self.previous=r.uniform(-1,1,2)
self.servo_target=self.previous*LIMIT
self.data.ctrl[:]=self.servo_target
mujoco.mj_forward(self.model,self.data)
minima=[]
for i in self.collisions:
rot=self.data.geom_xmat[i].reshape(3,3);size=self.model.geom_size[i]
if self.model.geom_type[i]==mujoco.mjtGeom.mjGEOM_BOX:extent=np.abs(rot[2])@size
elif self.model.geom_type[i]==mujoco.mjtGeom.mjGEOM_CYLINDER:
extent=abs(rot[2,2])*size[1]+size[0]*math.sqrt(max(0,1-rot[2,2]**2))
else:raise ValueError('Reset clearance needs an analytic collision shape')
minima.append(self.data.geom_xpos[i,2]-extent)
self.data.qpos[2]+=r.uniform(.01,.03)-min(minima)
mujoco.mj_forward(self.model,self.data)
self._start_sensor_stream(seed)
settle=0.
while settle<4. and (self._imu_quiet_duration<.3 or self.filter._up is None):
self.physics(self.servo_target,28);settle+=DT
self.invalid=None
try:self.last_obs=self.observation()
except ValueError as exc:
self.invalid='initial_sensor_rejected: '+str(exc);self.last_obs=np.zeros(8,dtype=np.float32)
up,_=self.metrics()
if up>.9:self.invalid='settled_already_upright'
self.started=float(self.data.time);self.hold=0.;self.peak_up=up;self.initial_up=up;self.active=True
return self.last_obs.copy(),dict(case=self.case,invalid_reset=self.invalid,initial_up=up,
initial_q=self.data.qpos[self.qids].tolist(),settle_s=settle)
def physics(self,target,n):
if not self._imu_stream_enabled:return self._physics_mechanics(target,n)
remaining=int(n)
while remaining>0:
chunk=min(remaining,self._imu_next_tick-self._imu_step_index)
if chunk<=0:raise RuntimeError('IMU tick scheduler did not advance')
self._physics_mechanics(target,chunk)
self._imu_step_index+=chunk;remaining-=chunk
if self._imu_step_index==self._imu_next_tick:
self._sample_sensor_tick()
self._imu_next_tick=imu_tick(self.sensor_sample_count+1)
def _physics_mechanics(self,target,n):
if self._fast is not None:
self._fast.step(self,target,n)
return
for _ in range(n):
self.data.ctrl[:]=target+self.zero
# Speed-torque envelope limits powered acceleration; external impacts can exceed
# no-load speed and are not clamped by teleporting joint velocity.
v=self.data.qvel[self.vids]
positive=self.torque*np.clip(1-np.maximum(v,0)/self.speed,0,1)
negative=self.torque*np.clip(1-np.maximum(-v,0)/self.speed,0,1)
self.model.actuator_forcerange[:,0]=-np.maximum(negative,1e-8)
self.model.actuator_forcerange[:,1]=np.maximum(positive,1e-8)
mujoco.mj_step(self.model,self.data)
if self.active:
_,stable=self.metrics()
self.hold=self.hold+.002 if stable else 0.
def step(self,action):
action=np.asarray(action,dtype=float)
if action.shape!=(2,) or not np.isfinite(action).all():raise ValueError('Expected two finite actions')
if self.invalid:
return self.last_obs.copy(),-10.,True,False,dict(is_success=False,failure=self.invalid,case=self.case)
action=np.clip(action,-1,1)
delay=int(self.np_random.integers(16)) if self.randomize else 0
self.physics(self.servo_target,delay)
actual_dt=DT+delay*.002
target=np.clip(action*LIMIT,self.servo_target-self.command_rate*actual_dt,self.servo_target+self.command_rate*actual_dt)
old=self.previous.copy();self.previous=target/LIMIT;self.servo_target=target
self.physics(target,28)
up,stable=self.metrics();self.peak_up=max(self.peak_up,up)
elapsed=float(self.data.time)-self.started
info=dict(case=self.case,is_success=False,up_z=up,peak_up=self.peak_up,hold_s=self.hold,elapsed_s=elapsed)
info.update(imu_sample_count=self.sensor_sample_count,imu_guard_rejections=self.sensor_gyro_guard_rejections,
imu_filter_rejections=self.sensor_filter_rejections,imu_fault_latched=self.imu_fault is not None,
imu_fault_time_s=self.imu_fault_time,last_imu_sample_time_s=self.last_imu_sample_time,
imu_sample_age_s=float(self.data.time)-self.last_imu_sample_time if self.last_imu_sample_time is not None else None)
try:self.last_obs=self.observation()
except ValueError as exc:
info['failure']='sensor_rejected: '+str(exc)
return self.last_obs.copy(),-10.,True,False,info
success=self.hold>=1.
info['is_success']=bool(success)
torque_cost=np.prod(np.abs(self.data.actuator_force)/self.torque)
reward=actual_dt*(2*up+4*float(stable)-.1*torque_cost)-.02*np.sum((old-self.previous)**2)
if success:reward+=25
return self.last_obs.copy(),float(reward),bool(success),elapsed>=8. and not success,info

File diff suppressed because it is too large Load Diff

File diff suppressed because it is too large Load Diff

View File

@ -0,0 +1,51 @@
"""Replay evaluated speed-policy seeds with the original CAD meshes and strict trajectory checks."""
import argparse
import hashlib
import importlib.util
import json
from pathlib import Path
import imageio.v2 as imageio
import numpy as np
from environment import RecoveryEnv,HERE,CASES
from policy import Policy
spec=importlib.util.spec_from_file_location('existing_replay',HERE/'replay_support.py')
existing=importlib.util.module_from_spec(spec);spec.loader.exec_module(existing)
def main():
p=argparse.ArgumentParser();p.add_argument('--actor',type=Path,required=True);p.add_argument('--evaluation',type=Path,required=True);p.add_argument('--out',type=Path,default=HERE/'replays');p.add_argument('--cases',nargs='+',default=list(CASES));args=p.parse_args()
evaluation=json.loads(args.evaluation.read_text())
digest=hashlib.sha256(args.actor.read_bytes()).hexdigest();assert evaluation['actor_sha256']==digest
args.out.mkdir(exist_ok=True);actor=Policy(args.actor);visual=existing.Visualizer(HERE)
clips=[];missing=[]
for direction in evaluation['directions']:
case=direction['case']
if case not in args.cases:continue
rows=sorted(direction['episodes'],key=lambda r:r['seed'])
for success in (True,False):
row=next((r for r in rows if r['is_success']==success),None)
if row is None:missing.append(dict(case=case,success=success));continue
seed=row['seed'];env=RecoveryEnv()
reference=existing.run_episode(env,actor,seed,case)
existing.assert_evaluation_match(row,reference)
name=f'{case}_{seed}_{"success" if success else "failure"}.mp4'
label=f'{case} | CONTINUOUS SPEED | {"SUCCESS" if success else "FAILURE"}'
with imageio.get_writer(args.out/name,fps=50,codec='libx264',macro_block_size=16) as writer:
captured=existing.run_episode(env,actor,seed,case,capture=True,
frame_sink=lambda e,t:writer.append_data(visual.frame(e,label,t)))
existing.assert_trajectory_equal(reference,captured)
np.savez_compressed(args.out/(name[:-4]+'.npz'),qpos=np.array([f['qpos'] for f in captured['frames']]),
times=np.array([f['elapsed_s'] for f in captured['frames']]),observations=captured['observations'],
final_qpos=captured['final_qpos'],final_qvel=captured['final_qvel'])
imageio.imwrite(args.out/(name[:-4]+'.png'),visual.frame(env,label,captured['result']['elapsed_s']))
clips.append(dict(case=case,seed=seed,success=success,file=name,frames=len(captured['frames']),
result=captured['result'],actor_sha256=digest,trajectory_bit_exact=True,
video_sha256=hashlib.sha256((args.out/name).read_bytes()).hexdigest()))
print('Rendered',name,flush=True);env.close()
(args.out/'manifest.json').write_text(json.dumps(dict(evaluation=str(args.evaluation),evaluation_sha256=hashlib.sha256(args.evaluation.read_bytes()).hexdigest(),clips=clips,missing=missing,hardware_connected=False),indent=2))
(args.out/'manifest.json').write_text(json.dumps(dict(evaluation=str(args.evaluation),evaluation_sha256=hashlib.sha256(args.evaluation.read_bytes()).hexdigest(),clips=clips,missing=missing,hardware_connected=False),indent=2))
visual.close()
if __name__=='__main__':main()

View File

@ -0,0 +1,233 @@
"""Replay saved evaluation seeds with the exact physical task and a 50 Hz video."""
import argparse
import hashlib
import importlib
import json
from pathlib import Path
import sys
import numpy as np
HERE=Path(__file__).resolve().parent
DOMAIN_MODULES=('environment','physical_env','policy','base_policy','imu_orientation','sensor_model','fast_core')
FPS=50
def load_domain(path):
"""The selected sensor domain must win over this script's directory/cache."""
path=Path(path).resolve()
for name in ('environment.py','physical_env.py','policy.py','physics.xml','model.xml'):
if not (path/name).is_file():raise ValueError('Missing domain file: '+str(path/name))
sys.path.insert(0,str(path))
for name in DOMAIN_MODULES:sys.modules.pop(name,None)
environment=importlib.import_module('environment');policy=importlib.import_module('policy')
for module in (environment,policy,sys.modules['physical_env']):
if Path(module.__file__).resolve().parent!=path:raise RuntimeError('Mixed sensor-domain imports')
if environment.OBS_DIM not in (40,196):raise ValueError('Replay requires an observable residual actor40 or actor196')
return environment,policy.Policy
def select_examples(evaluation,cases):
if evaluation.get('teacher_only'):raise ValueError('Privileged teacher is not a deployable replay candidate')
rows=sorted(evaluation['episodes'],key=lambda row:row['seed']);selected=[];missing=[]
for case in cases:
valid=[r for r in rows if r.get('case',r.get('initial',{}).get('case'))==case
and not r.get('initial',{}).get('invalid_reset')]
for success in (True,False):
found=next((r for r in valid if bool(r['is_success'])==success),None)
if found is None:missing.append(dict(case=case,is_success=success))
else:selected.append(found)
return selected,missing
def step_action(env,actor,observation,absolute=False):
if actor is None:return np.zeros(2)
action=np.asarray(actor(observation),dtype=float)
if action.shape!=(2,) or not np.isfinite(action).all() or np.any(np.abs(action)>1+1e-7):
raise ValueError('Expected a bounded two-axis actor action')
return (action-env.base(env.raw))/2 if absolute else action
def run_episode(env,actor,seed,case,absolute=False,capture=False,frame_sink=None):
"""Split original physics calls only at video ticks; never sample extra sensors."""
observation,initial=env.reset(seed=seed,options={'case':case})
if actor is not None and hasattr(actor,'reset'):actor.reset()
if initial['invalid_reset']:raise ValueError('Selected seed has an invalid reset: '+str(initial))
original=env.physics;frames=[];controls=[];observations=[];ticks=0
dt=float(env.model.opt.timestep)
stride=int(round(1/FPS/dt))
if stride!=10 or not np.isclose(stride*dt,1/FPS,rtol=0,atol=1e-12):
raise ValueError('Replay requires the unchanged 2ms physics timestep')
def sample():
elapsed=float(env.data.time)-env.started
frames.append(dict(elapsed_s=elapsed,qpos=env.data.qpos.tolist()))
if frame_sink is not None:frame_sink(env,elapsed)
def physics(target,n):
nonlocal ticks
remaining=int(n)
while remaining:
chunk=min(remaining,stride-ticks%stride)
original(target,chunk);ticks+=chunk;remaining-=chunk
if ticks%stride==0:sample()
if capture:
env.physics=physics;sample()
try:
for _ in range(150):
observations.append(observation.copy())
action=step_action(env,actor,observation,absolute)
observation,_,done,truncated,info=env.step(action)
controls.append(dict(action=action.tolist(),qpos=env.data.qpos.tolist(),
qvel=env.data.qvel.tolist(),**info))
if done or truncated:break
else:raise AssertionError('Episode did not terminate within the original 150-step evaluator limit')
finally:env.physics=original
if info['is_success']:
if info['elapsed_s']>8 or info['hold_s']<1:raise AssertionError('Relaxed success rule')
return dict(seed=int(seed),case=case,initial=initial,result=info,trajectory=controls,
frames=frames,observations=np.asarray(observations),
final_qpos=env.data.qpos.copy(),final_qvel=env.data.qvel.copy(),
rng_state=env.np_random.bit_generator.state,
sensor_rng_state=env.sensor_rng.bit_generator.state if hasattr(env,'sensor_rng') else None)
def assert_evaluation_match(expected,episode):
actual=episode['result']
if 'action_trace' in expected:
if len(expected['action_trace'])!=len(episode['trajectory']):raise AssertionError('Evaluation control count differs')
for saved,replayed in zip(expected['action_trace'],episode['trajectory']):
if 'residual' in saved:np.testing.assert_array_equal(saved['residual'],replayed['action'])
requested=saved.get('requested_action',saved.get('absolute_requested'))
if requested is not None:np.testing.assert_array_equal(requested,replayed['requested_action'])
for key in ('case','is_success','failure'):
if expected.get(key)!=actual.get(key):raise AssertionError(f'Evaluation mismatch {key}: {expected.get(key)!r} != {actual.get(key)!r}')
for key in ('elapsed_s','up_z','peak_up','hold_s','stall_s','requested_action','qpos'):
if key in expected:
value=episode['final_qpos'] if key=='qpos' else actual.get(key)
np.testing.assert_allclose(value,expected[key],rtol=0,atol=1e-10,err_msg='Evaluation mismatch '+key)
for key in ('initial_up','initial_q','settle_s'):
if key in expected.get('initial',{}):
np.testing.assert_allclose(episode['initial'][key],expected['initial'][key],rtol=0,atol=1e-10,err_msg='Initial mismatch '+key)
if 'estimated_up' in expected.get('initial',{}):
np.testing.assert_array_equal(episode['observations'][0,:3],expected['initial']['estimated_up'])
def assert_trajectory_equal(reference,recorded):
for key in ('final_qpos','final_qvel','observations'):
np.testing.assert_array_equal(reference[key],recorded[key],err_msg='Capture changed '+key)
if reference['result']!=recorded['result']:raise AssertionError('Capture changed terminal result')
if reference['trajectory']!=recorded['trajectory']:raise AssertionError('Capture changed control trajectory')
if reference['rng_state']!=recorded['rng_state']:raise AssertionError('Capture consumed simulator RNG')
if reference['sensor_rng_state']!=recorded['sensor_rng_state']:raise AssertionError('Capture consumed sensor RNG')
class Visualizer:
def __init__(self,domain):
import mujoco
from PIL import ImageFont
self.mujoco=mujoco
self.model=mujoco.MjModel.from_xml_path(str(domain/'model.xml'))
self.data=mujoco.MjData(self.model)
self.renderer=mujoco.Renderer(self.model,height=480,width=640)
self.camera=mujoco.MjvCamera();self.camera.distance=.39
self.camera.azimuth=135;self.camera.elevation=-23
self.torso=self.model.body('torso').id
try:self.font=ImageFont.load_default(size=16)
except TypeError:self.font=ImageFont.load_default()
def frame(self,env,label,elapsed):
from PIL import Image,ImageDraw
if self.model.nq!=env.model.nq or self.model.nv!=env.model.nv:
raise ValueError('Visual model joint state differs from physical model')
self.data.qpos[:]=env.data.qpos;self.data.qvel[:]=env.data.qvel
self.data.time=env.data.time;self.mujoco.mj_forward(self.model,self.data)
self.camera.lookat[:]=self.data.xpos[self.torso]+[0,0,.045]
self.renderer.update_scene(self.data,camera=self.camera)
image=Image.fromarray(self.renderer.render());draw=ImageDraw.Draw(image)
text=label+f'\nsimulation t={elapsed:.3f}s | 8s deadline, continuous 1s stable hold'
draw.rectangle((0,0,640,48),fill=(25,28,32))
draw.text((8,5),text,font=self.font,fill='white')
return np.asarray(image)
def close(self):self.renderer.close()
def sha(path):return hashlib.sha256(Path(path).read_bytes()).hexdigest()
def generate(domain,folder,evaluation_path,absolute=False,cases=None,actor_factory=None,controller_files=None):
import imageio.v2 as imageio
domain=Path(domain).resolve();folder=Path(folder).resolve();evaluation_path=Path(evaluation_path).resolve()
environment,Policy=load_domain(domain)
cases=list(cases or environment.CASES)
if len(set(cases))!=len(cases) or any(c not in environment.CASES for c in cases):raise ValueError('Unknown or duplicate cases')
evaluation=json.loads(evaluation_path.read_text())
if evaluation.get('baseline'):raise ValueError('Select the trained policy evaluation, not the baseline evaluation')
actor_path=folder/'actor.npz'
if actor_factory is not None and not controller_files:raise ValueError('Custom controller requires its source and weight files')
controller_sources={str(Path(p).resolve()):sha(p) for p in (controller_files or [])}
actor=Policy(actor_path) if actor_factory is None else actor_factory()
if not callable(actor):raise TypeError('Controller factory must return a callable actor')
if 'actor_sha256' in evaluation and evaluation['actor_sha256']!=sha(actor_path):raise ValueError('Actor SHA does not match evaluation')
if 'base_actor_sha256' in evaluation and evaluation['base_actor_sha256']!=sha(domain/'base_actor.npz'):raise ValueError('Baseline SHA does not match evaluation')
selected,missing=select_examples(evaluation,cases)
settings=folder/'controller.json';slew=json.loads(settings.read_text())['command_slew_rad_s'] if settings.exists() else 6.
out=folder/'replays';out.mkdir(exist_ok=True)
manifest=dict(status='INCOMPLETE',domain=str(domain),evaluation=str(evaluation_path),
evaluation_sha256=sha(evaluation_path),actor_sha256=sha(actor_path),absolute=absolute,
replay_source_sha256=sha(Path(__file__)),
fps=FPS,resolution=[640,480],command_slew_rad_s=slew,actor_features=environment.OBS_DIM,
rule='unchanged simulator: <=8s deadline and >=1s continuous physical stable hold',
source_hashes={name:sha(domain/name) for name in ('environment.py','physical_env.py','policy.py','physics.xml','model.xml','base_actor.npz','sensor_model.py','sensor_contract.json') if (domain/name).exists()},
controller_sources=controller_sources,missing_examples=missing,replays=[],hardware_connected=False)
manifest_path=folder/'replays.json'
manifest_path.write_text(json.dumps(manifest,indent=2)+'\n')
visual=Visualizer(domain)
try:
for chosen in selected:
case=chosen['case'];seed=chosen['seed'];want=bool(chosen['is_success'])
for baseline in ((False,True) if case in ('right','back_right') else (False,)):
env=environment.RecoveryEnv(focus=False,command_slew=slew)
try:
reference=run_episode(env,None if baseline else actor,seed,case,absolute and not baseline)
if not baseline:assert_evaluation_match(chosen,reference)
result=bool(reference['result']['is_success']);role='frozen_baseline' if baseline else 'trained_policy'
name=f'{case}_{seed}_{"success" if want else "failure"}_sample_{role}'
label=f'{case} | seed={seed} | {role}: {"SUCCESS" if result else "FAILURE"}'
with imageio.get_writer(out/(name+'.mp4'),fps=FPS,codec='libx264',macro_block_size=16) as writer:
recorded=run_episode(env,None if baseline else actor,seed,case,absolute and not baseline,
capture=True,frame_sink=lambda current,t:writer.append_data(visual.frame(current,label,t)))
assert_trajectory_equal(reference,recorded)
if not baseline:assert_evaluation_match(chosen,recorded)
imageio.imwrite(out/(name+'.png'),visual.frame(env,label,recorded['result']['elapsed_s']))
payload={key:value for key,value in recorded.items() if key not in ('observations','final_qpos','final_qvel')}
payload.update(final_qpos=recorded['final_qpos'].tolist(),final_qvel=recorded['final_qvel'].tolist(),
policy_role=role,absolute=absolute and not baseline,reference_physics_exact_match=True)
(out/(name+'.json')).write_text(json.dumps(payload,indent=2)+'\n')
manifest['replays'].append(dict(case=case,seed=seed,policy_role=role,is_success=result,
evaluation_sample_success=want,evaluation_asserted=not baseline,
same_seed_frozen_baseline=baseline,terminal=recorded['result'],
reference_physics_exact_match=True,video_frames=len(recorded['frames']),
video=str((out/(name+'.mp4')).relative_to(folder)),
image=str((out/(name+'.png')).relative_to(folder)),
trajectory=str((out/(name+'.json')).relative_to(folder))))
manifest_path.write_text(json.dumps(manifest,indent=2)+'\n')
print(label,flush=True)
finally:env.close()
finally:visual.close()
for path,expected in controller_sources.items():
if sha(path)!=expected:raise AssertionError('Controller changed during replay: '+path)
manifest['status']='PHYSICS_VERIFIED_REPLAYS_COMPLETE';manifest_path.write_text(json.dumps(manifest,indent=2)+'\n')
return manifest
def main():
parser=argparse.ArgumentParser();parser.add_argument('--domain',type=Path,default=HERE)
parser.add_argument('--folder',type=Path,required=True);parser.add_argument('--evaluation',type=Path,required=True)
parser.add_argument('--absolute',action='store_true');parser.add_argument('--cases',nargs='+')
args=parser.parse_args();generate(args.domain,args.folder,args.evaluation,args.absolute,args.cases)
if __name__=='__main__':main()

Some files were not shown because too many files have changed in this diff Show More