完善行程标定与视觉运动判定
This commit is contained in:
+5
-3
@@ -50,7 +50,8 @@ Thumbs.db
|
||||
|
||||
# Runtime and calibration scratch files
|
||||
/logs/
|
||||
/MvSdkLog/
|
||||
# The camera SDK writes logs relative to the launch working directory.
|
||||
MvSdkLog/
|
||||
*.tmp
|
||||
*.log
|
||||
*.bak
|
||||
@@ -61,8 +62,9 @@ Thumbs.db
|
||||
# Reproducible seed profiles remain under
|
||||
# src/linkerhand_retarget/resource/linkerforce_v2/profiles/.
|
||||
/profiles/
|
||||
/calibration_output/
|
||||
/range_calibration_output/
|
||||
# Calibration outputs may be created from the workspace root or a subdirectory.
|
||||
calibration_output/
|
||||
range_calibration_output/
|
||||
/config/*_three_camera_extrinsics.yaml
|
||||
# Superseded local O6 camera calibrations. Keep the active extrinsics and
|
||||
# intrinsics referenced by o6_right_product.yaml available for version control.
|
||||
|
||||
@@ -110,7 +110,7 @@ SDK 自身的初始化检查和设备保护保持原样。标定仍要求 SDK
|
||||
| 侧面 / 8 | index_mcp_pitch、index_pip、index_dip | 7、11、16 |
|
||||
| 顶部 / 9 | thumb_cmc_yaw | 1 |
|
||||
|
||||
任务顺序:正面拇指3项 → 四指侧摆同步1项 → 侧面弯曲12项 → 顶部拇指1项。
|
||||
任务顺序:正面拇指3项 → 四指侧摆分阶段1项 → 侧面弯曲12项 → 顶部拇指1项。
|
||||
|
||||
标定 `thumb_cmc_roll` 时,准备姿态中的 `index_mcp_roll`(SDK 下标2)固定为0。
|
||||
准备姿态稳定后开始拇指扫描,食指侧摆始终保持0;准备过程不计入边界采样。
|
||||
@@ -126,9 +126,34 @@ SDK 自身的初始化检查和设备保护保持原样。标定仍要求 SDK
|
||||
准备姿态稳定后才扫描 `thumb_mcp`,扫描期间拇指 yaw 始终保持80。
|
||||
该避让仅用于 `thumb_mcp`,进入后续 `thumb_ip` 时按照基础姿态及对应任务配置重新准备。
|
||||
|
||||
四指侧摆使用相同指令,同时更新SDK下标2、3、4、5,不应用避让配置;各Tag独立计算范围。
|
||||
一指先确认起动边界后保留该值,整组继续同步逐1移动;四指均确认该端边界后,共同切换到另一端。
|
||||
若某指扫描到对端仍不能确认运动,该指记为失败;其余手指继续测量,不为已失败手指重复寻找另一端。
|
||||
### 四指侧摆分阶段标定
|
||||
|
||||
四指侧摆仍作为一个任务,统一发送包含20项的目标数组;其余16个通道保持基础姿态,不应用弯曲避让。
|
||||
中指下限最后测量,顺序如下(“三指”指小指、无名指、食指):
|
||||
|
||||
| 阶段 | 三指目标 | 中指目标 | 测量内容 |
|
||||
|---|---|---|---|
|
||||
| 同步正向 | 0→255 | 80→255 | 三指各自的 min |
|
||||
| 同步反向 | 255→0 | 255→80 | 四指各自的 max |
|
||||
| 中指回零 | 保持0 | 80→0 | 仅定位,不计算边界 |
|
||||
| 中指单独正向 | 保持0 | 0→80 | 中指 min |
|
||||
| 必要时联动继续 | 0→255 | 80→255 | 继续测中指 min |
|
||||
|
||||
同步阶段按各自区间的相同比例推进:三指指令为 `c` 时,中指为 `80 + round(175*c/255)`。
|
||||
三指每次改变1,中指每次改变0或1,始终在同一条消息中更新。每个 Tag 独立判断;中指相同指令的重复观测不重复参与边界搜索。
|
||||
正向确认三指下限后,可直接沿限速轨迹到255再测上限;反向确认四指上限后,仍会先到达“三指0、中指80”并等待新反馈稳定,再只让中指回零。
|
||||
|
||||
中指单独扫描只要求正面 ID3 有效,其他三指保持0。若到80仍未检测到运动,就进入联动继续阶段;
|
||||
从0采集的参考和噪声全部保留,不在80重置,也不重复计算80。首次检测到运动即结束该轮,不再扫描剩余内部行程。
|
||||
同步及联动继续阶段要求四个 Tag 有效,任一丢失整组停止推进。
|
||||
|
||||
若中指反向扫到80仍无法确认上限,该指记录失败,不将80填作上限;其他关节独立保留结果。
|
||||
其他关节扫描到对端仍无法确认运动也记录失败,不为已失败关节重复测量。
|
||||
复测时重做上述完整顺序;暂停后继续会重做整个四指任务。
|
||||
|
||||
配置位于 `config/profiles/o30_right.yaml` 的 `four_finger_roll.deferred_lower`:
|
||||
`joint: middle_mcp_roll` 指定延后测下限的通道,`split: 80` 指定分段值。
|
||||
该流程不依赖 O30 的关节名称,可复用于其他型号的多通道任务。没有该配置的任务沿用普通两端搜索。
|
||||
|
||||
侧面按小指→无名指→中指→食指测量,每指依次测 mcp_pitch、pip、dip。
|
||||
测无名指时小指弯曲;测中指时小指和无名指弯曲;测食指时前三指弯曲。
|
||||
@@ -146,32 +171,65 @@ SDK 自身的初始化检查和设备保护保持原样。标定仍要求 SDK
|
||||
|
||||
## 5. 算法与暂停
|
||||
|
||||
默认只测两端,一轮包含以下步骤:
|
||||
默认只测两端;普通任务一轮包含以下步骤,四指侧摆采用上面的分阶段顺序:
|
||||
|
||||
1. 到达任务准备姿态并稳定。
|
||||
2. 沿限速轨迹到达0,采集稳定的低端参考角点和噪声。
|
||||
3. 按1、2、3……逐1递增,与低端参考比较;连续3个有效采样点确认离开参考姿态后,保留第一次触发运动的指令。
|
||||
3. 按1、2、3……逐1递增,与低端参考比较;记录指令发出后是否发生过有效位移,首次检测到运动的指令记为 `min=当前指令-1`,结束低端搜索。
|
||||
4. 沿限速轨迹直接移动到255,中间不停车采样;稳定后重新采集高端参考角点和噪声。
|
||||
5. 按254、253、252……逐1递减,与高端参考比较;确认离开参考姿态后记录第一次触发指令,结束测量。
|
||||
5. 按254、253、252……逐1递减,与高端参考比较;首次检测到运动的指令记为 `max=当前指令+1`,结束高端搜索。
|
||||
6. 保存结果;若任务配置了测后恢复关节,按配置顺序逐个恢复为基础姿态,每一步都等待新反馈稳定,再结束任务或进入下一任务。
|
||||
|
||||
确认期间若观测重新回到参考姿态附近,会清除候选值,继续寻找。
|
||||
只判断“是否发生位移”,不判断图像中的运动方向,不要求后续几个指令继续运动,也不按后续转向或停顿重新选择起动值。
|
||||
每端参考姿态固定为该端起始的稳定观测,不再执行平台重建或候选方向修正。Tag 原地旋转也会改变四个角点,因此同样可以检测。
|
||||
例如255→254时首次检测到运动,就在254结束该端搜索并记录 `max=255`,不再为了确认而采集253、252。
|
||||
检测仍使用每点的新帧稳定窗口和运动阈值,不能用旧帧、丢失 Tag 或单张异常图像作为起动证据。
|
||||
从该关节目标第一次实际发出时就开始记录逐帧角点,覆盖0.3秒等待期间和之后的运动过程。先保留证据,等本点稳定后再判断,避免两帧随机偏移一出现就被永久记成运动。
|
||||
稳定位置变化与瞬时运动使用不同门槛:稳定窗口的角点代表位置相对端点参考超过 `motion_floor_px`(默认0.5像素)即可判动;瞬时运动需要连续 `motion_frames` 张有效新图像(默认2张)超过「0.5像素 + 端点逐帧抖动范围 + 当前点逐帧抖动范围」。抖动范围由各自稳定窗口的角点到代表位置的距离99分位数计算,不假设逐帧噪声独立,也不判断位移方向。
|
||||
本点完成采样时,稳定位置变化或瞬时运动任一成立,便使用这个指令计算边界。超过抖动范围的短暂运动,即使随后回原位也保留;准备动作、到达初始端点的过程、目标发出前的旧图像,以及无关 Tag 都不参与过程位移判定。
|
||||
默认只执行一轮(`scan.endpoint_repetitions: 1`)。设为2或更大时,按相同顺序重新寻找两端,
|
||||
同一端复测差异最多2单位(`repeat_tolerance`),通过后取区间交集。旧工位文件的 `coarse_step`、`fine_radius`、`fine_repetitions` 仍可加载,但不影响新流程。
|
||||
同一端复测差异最多2单位(`repeat_tolerance`),通过后取区间交集。旧工位文件的 `coarse_step`、`fine_radius`、`fine_repetitions`、`confirmation_points` 仍可加载,但不影响新流程。
|
||||
|
||||
只比较同一个Tag的四个有序角点,结合稳定窗口、端点平台、边界附近多个观测点的确认和静止噪声。
|
||||
每点至少等待0.3秒,随后采集8张稳定新图像;扫描期间只检查本任务主动关节的反馈稳定性,不要求反馈等于目标指令。
|
||||
只比较同一个Tag的四个有序角点,使用过程中的有效观测及各指令点的稳定窗口,与固定端点参考计算均方根位移。
|
||||
每点到达轨迹目标后至少等待0.3秒,再采集至少8张、时间跨度至少0.5秒的稳定新图像(`stable_frames`、`stable_seconds` 同时满足);过程运动检测同时进行,不会丢弃等待期间“动一下又回位”的证据。
|
||||
**每端的起始参考额外复核**:目标保持不变,图像至少覆盖 `reference_seconds`(默认1.5秒)且至少 `3×stable_frames` 张;前、中、后三段的角点代表位置两两差异均不超过运动门槛的一半(默认0.25像素),同时仍需通过随机抖动和持续漂移检查。参考不稳定时继续保持当前目标等待,不能拿尚在回位的短窗口作为0或255的参考。该复核增加静止观察时间,不增加扫描轮次,也不发送额外往返动作。
|
||||
窗口代表位置按每个坐标去掉最高和最低各四分之一的值,再取剩余值的平均;减少随机抖动两侧帧数不均时,中位数随相位跳变的影响。运动过程仍检查原始帧,不会用平均位置抹掉短暂运动。
|
||||
稳定采样将随机抖动与持续漂移分开判断,默认配置如下:
|
||||
|
||||
| 参数 | 默认值 | 含义 |
|
||||
|---|---:|---|
|
||||
| `stability_noise_px` | 0.5 | 随机角点抖动上限,单位像素 |
|
||||
| `stability_drift_px` | 0.1 | 扣除抖动裕量后的累计漂移上限,单位像素 |
|
||||
| `stability_drift_sigma` | 3 | 估计时间趋势时使用的抖动裕量倍数 |
|
||||
|
||||
先按实际时间戳估计四角点的移动趋势,再用去除趋势后的 MAD 和相邻帧差分估计随机抖动。
|
||||
全窗口及前、后半窗口分别检查趋势,避免缓慢往返运动在整个窗口中互相抵消;每个半窗口至少4帧。
|
||||
持续漂移取各段 `max(0, 趋势累计位移 - 抖动裕量)` 的最大值,裕量由随机抖动、帧数和时间分布计算。
|
||||
两项门槛独立生效:随机抖动允许到0.5像素,低噪声条件下的持续漂移仍按0.1像素检查。任一项超限就保持当前指令继续等待,超过 `point_timeout` 仍不能稳定则暂停。
|
||||
`point_timeout` 默认6秒,给换向后的短时振动留出衰减时间;这是最长等待时间,达到稳定条件就立即采样推进。持续振动仍会暂停,不通过放宽噪声或漂移门槛获得结果。
|
||||
这是对有限图像的估计,并非已完全静止的保证。高噪声会降低细小运动的可辨别性,实机准确性仍需复测;不根据四指一起变化自动扣除整体位移,以免消除四指真实的同步运动。
|
||||
旧工位文件的 `stability_px` 仍可加载,仅作为随机抖动上限;新增配置优先,漂移上限默认0.1。建议自定义工位改用上述三个明确字段。
|
||||
|
||||
新日志的 `sampling_method` 为 `separate_noise_drift_v2`。样本分别记录 `jitter_px`、`trend_px`、`trend_allowance_px`、`drift_px` 及两项门槛。
|
||||
`sampling_unstable` 和 `sampling_timeout` 额外保存当时的逐帧时间戳、四角点(`window_samples`),便于离线检查。旧日志只有汇总值时无法补回随机抖动与持续漂移的区别。
|
||||
超时提示会列出当前受阻的机位、Tag ID、关节和原因,例如 `正面 Tag ID0(thumb_cmc_roll):随机抖动 0.620 > 0.500 像素` 或 `持续漂移 0.180 > 0.100 像素`。
|
||||
原因包括未检出、检测质量不足、观测过期、新图像数量或观察时长不足、随机抖动/持续漂移过大;反馈不足或未稳定会单独说明,不归因于 Tag。
|
||||
四指任务只列出当前受阻的观测;相机检测流断流时列出该机位当前任务所需的 Tag。
|
||||
同一机位至少3个 Tag 的角点在相同时间呈现高度一致的平移时,超时提示会补充“多个 Tag 出现共同平移”及幅度,日志记录 `shared_motion`。这只能提示共同变化,无法仅凭指尖 Tag 判定是相机、手掌还是支架在移动,也不会据此扣除图像位移或放行采样。
|
||||
`sampling_timeout` 日志在清空采样窗口前保存超时当刻的具体原因、测量值、目标及任务阶段,便于事后定位;这些提示不改变采样门槛或运动流程。
|
||||
扫描期间只检查本任务主动关节的反馈稳定性,不要求反馈等于目标指令。
|
||||
准备姿态、测后恢复和示教保存继续等待全手反馈稳定;SDK 连接及反馈时效检查保留。
|
||||
旧配置中的 `non_target_tolerance` 仍可加载,但不再用于扫描判定。
|
||||
每端独立计算阈值 `max(0.5 px, 5×该端静止噪声)`。比较对象始终是该端参考姿态,细小的累计运动也能被检测。
|
||||
稳定位置使用固定 `motion_floor_px`,不再把一次起点观测的随机抖动乘5作为运动门槛,避免相同真实位移在不同轮次被漏掉。`noise_multiplier` 仅保留给旧日志复算。细小运动仍相对于固定端点参考累计比较,瞬时帧单独使用上述抖动范围检查。
|
||||
JSON 沿用边界定义:`min = 低端首次运动指令 - 1`,`max = 高端反向首次运动指令 + 1`。
|
||||
例如从0递增到7首次运动、从255递减到242首次运动,结果为6~243;加减1对应 O30 的指令分辨率,其他型号使用自身分辨率。
|
||||
默认3点确认时,该例只采集0~9和255~240共26个指令点,两端之间只移动。
|
||||
界面显示端点定位、低端/高端搜索、当前指令和轮次;进度按已确认或已判失败的边界计算。
|
||||
该例只采集0~7和255~242共22个指令点;两端之间只移动。
|
||||
界面显示阶段、低端/高端搜索、各主动关节的当前指令和轮次;进度按已确认或已判失败的边界计算。
|
||||
|
||||
范围只描述两端的指令边界,中间是否持续运动不参与成功/失败判定。
|
||||
例如低端静止到6、高端从243开始静止,即使中途停了一段再恢复运动,仍可得到6~243。
|
||||
新日志的离线复算与在线标定共用端点搜索逻辑。全程没有可确认运动、端点观测不足、两端边界重叠或启用复测后边界不一致时,无法给出可靠范围。
|
||||
新日志记录每个阶段及各关节实际用于边界搜索的指令,离线复算与在线标定共用端点搜索逻辑,同时兼容旧版相同指令扫描日志。
|
||||
全程未检测到超过阈值的运动、端点观测不足、两端边界重叠或启用复测后边界不一致时,无法给出可靠范围。一次有效稳定位移已经算起动,后续是否停住不参与判定。
|
||||
|
||||
单步分辨率为1,不代表真实边界误差必然为1。过小运动、噪声、速度和力矩设置都会影响可检测边界。
|
||||
本方法测量低端正向起动和高端反向起动边界,回差或反向空行程可能使结果收窄。
|
||||
@@ -198,7 +256,9 @@ SDK 诊断只进入日志,保留标定关节名、SDK 原名和具体内容,
|
||||
每个任务结束、暂停或取消时原子保存。再次点击开始/重测创建新会话,不覆盖历史文件。
|
||||
|
||||
同目录 `samples.jsonl` 保存型号配置、示教、运动设置、命令、反馈、角点、时间戳和失败原因。
|
||||
新日志以 `scan_method: endpoint_search_v1` 标明流程,记录各端参考观测、首次运动指令、确认指令和最终边界;旧版粗扫/细扫日志仍按原采样结构复算。
|
||||
新日志以 `scan_method: endpoint_search_v10` 标明静止参考复核与分离运动门槛;记录阶段、各关节参与搜索的指令、参考观测、首次运动指令和最终边界。`confirmed_at` 与 `trigger` 均为首次检测到运动的指令。
|
||||
每个样本的 `window_samples` 保存稳定窗口的逐帧角点;`motion_trace` 保存目标首次发出时间戳及整个观测过程,缺失 Tag 显式记为空值,不跨缺失帧拼接证据。`boundary_found.evidence` 说明由稳定位置还是瞬时运动触发,以及当时的位移、抖动范围和证据帧。日志不保存原始视频,角点发生真实位移仍不能单独证明是手指、Tag 贴片或支架在动。
|
||||
`endpoint_search_v1` 至 `endpoint_search_v9` 和旧版粗扫/细扫日志仍按各自原算法复算。旧日志缺少新参考窗口及完整过程帧,不能用本次修改凭空修正旧结果,须重新测量;不会自动改写历史结果。
|
||||
离线复算读取观测,重新运行范围算法,不连接硬件、不直接复制既有结果:
|
||||
|
||||
```bash
|
||||
|
||||
@@ -48,7 +48,10 @@ tasks:
|
||||
- {name: thumb_cmc_roll, joints: [thumb_cmc_roll], clearances: [index_roll_for_thumb], restore_after: [thumb_cmc_roll, index_mcp_roll]}
|
||||
- {name: thumb_mcp, joints: [thumb_mcp], clearances: [thumb_yaw_for_mcp]}
|
||||
- {name: thumb_ip, joints: [thumb_ip]}
|
||||
- {name: four_finger_roll, joints: [index_mcp_roll, middle_mcp_roll, ring_mcp_roll, pinky_mcp_roll], allow_override: false}
|
||||
- name: four_finger_roll
|
||||
joints: [index_mcp_roll, middle_mcp_roll, ring_mcp_roll, pinky_mcp_roll]
|
||||
allow_override: false
|
||||
deferred_lower: {joint: middle_mcp_roll, split: 80}
|
||||
- {name: pinky_mcp_pitch, joints: [pinky_mcp_pitch]}
|
||||
- {name: pinky_pip, joints: [pinky_pip]}
|
||||
- {name: pinky_dip, joints: [pinky_dip]}
|
||||
|
||||
@@ -18,14 +18,18 @@ scan:
|
||||
rate: 20 # 仅用于自动标定的准备姿态、避让及扫描轨迹,不限制手动示教。
|
||||
settle_seconds: 0.3
|
||||
stable_frames: 8
|
||||
point_timeout: 3
|
||||
stable_seconds: 0.5 # 新图像必须覆盖连续静止时间,不能只凭几帧局部平稳。
|
||||
reference_seconds: 1.5 # 每端开始前额外核对前、中、后三段参考位置一致。
|
||||
motion_frames: 2 # 瞬时运动须超过参考和当前点的抖动包络;不看方向。
|
||||
point_timeout: 6 # 最长等待振动衰减时间;稳定后立即推进,不固定等满6秒。
|
||||
freshness: 0.5
|
||||
feedback_tolerance: 2
|
||||
repeat_tolerance: 2
|
||||
motion_floor_px: 0.5
|
||||
noise_multiplier: 5
|
||||
stability_px: 1
|
||||
confirmation_points: 3
|
||||
noise_multiplier: 5 # 仅用于旧日志复算;不再用单次原始噪声抬高稳定位置运动门槛。
|
||||
stability_noise_px: 0.5 # 去除时间趋势后,允许的随机角点抖动(像素)。
|
||||
stability_drift_px: 0.1 # 扣除抖动裕量后,允许的窗口累计漂移(像素)。
|
||||
stability_drift_sigma: 3 # 时间趋势估计的随机抖动裕量倍数。
|
||||
max_hamming: 0
|
||||
min_margin: 30
|
||||
min_edge_px: 30
|
||||
|
||||
@@ -0,0 +1,70 @@
|
||||
"""Readable, structured sampling diagnostics shared by the engine and ROS host."""
|
||||
import numpy as np
|
||||
|
||||
|
||||
def view_label(view):
|
||||
return {'front': '正面', 'side': '侧面', 'top': '顶部'}.get(view, view)
|
||||
|
||||
|
||||
def tag_label(view, tag_id, joint):
|
||||
return f'{view_label(view)} Tag ID{tag_id}({joint})'
|
||||
|
||||
|
||||
def feedback_issues(profile, samples, count, tolerance, indices=None):
|
||||
"""The same check supplies both the readiness decision and its explanation."""
|
||||
if len(samples) < count:
|
||||
return [{'source': 'feedback', 'reason': f'新反馈不足:{len(samples)}/{count} 帧'}]
|
||||
spread = np.ptp(np.asarray([sample.positions for sample in samples]), axis=0)
|
||||
return [{'source': 'feedback', 'joint': joint.name,
|
||||
'reason': f'反馈未稳定:变化 {spread[joint.index]:g} > 允许 {tolerance:g}',
|
||||
'spread': float(spread[joint.index]), 'limit': tolerance}
|
||||
for joint in profile.joints
|
||||
if (indices is None or joint.index in indices) and spread[joint.index] > tolerance]
|
||||
|
||||
|
||||
def describe_issue(issue):
|
||||
if issue['source'] == 'tag':
|
||||
label = tag_label(issue['view'], issue['tag_id'], issue['joint'])
|
||||
else:
|
||||
label = f'关节 {issue["joint"]}' if 'joint' in issue else 'SDK 反馈'
|
||||
return f'{label}:{issue["reason"]}'
|
||||
|
||||
|
||||
def shared_translation(issues):
|
||||
"""Describe aligned Tag motion for diagnosis only; never correct observations.
|
||||
|
||||
Similar magnitudes alone are not evidence of shared motion. Require at least
|
||||
three distinct IDs in one view and compare their corners at common times.
|
||||
The result cannot identify which part of the camera/hand setup is moving.
|
||||
"""
|
||||
views = {}
|
||||
for issue in issues:
|
||||
if issue['source'] == 'tag' and issue.get('window_samples'):
|
||||
views.setdefault(issue['view'], {})[issue['tag_id']] = {
|
||||
frame['stamp_ns']: frame['corners'] for frame in issue['window_samples']}
|
||||
reports = []
|
||||
for view, tags in views.items():
|
||||
if len(tags) < 3:
|
||||
continue
|
||||
stamps = sorted(set.intersection(*(set(frames) for frames in tags.values())))
|
||||
if len(stamps) < 4:
|
||||
continue
|
||||
ids = sorted(tags)
|
||||
points = np.asarray([[tags[tag][stamp] for stamp in stamps] for tag in ids], dtype=float)
|
||||
offsets = points - np.median(points, axis=1, keepdims=True)
|
||||
translation = np.median(np.mean(offsets, axis=2), axis=0)
|
||||
energy = float(np.sum(offsets ** 2))
|
||||
if energy <= 1e-12:
|
||||
continue
|
||||
residual = offsets - translation[None, :, None, :]
|
||||
fraction = 1 - float(np.sum(residual ** 2)) / energy
|
||||
if fraction < .95:
|
||||
continue
|
||||
span = np.ptp(translation, axis=0)
|
||||
reason = (f'{view_label(view)}:多个 Tag 出现共同平移(位移占比 {fraction:.1%},'
|
||||
f'水平往返 {span[0]:.2f}、垂直往返 {span[1]:.2f} 像素);'
|
||||
'请检查相机和手掌固定处是否振动。')
|
||||
reports.append({'view': view, 'tag_ids': ids, 'frames': len(stamps),
|
||||
'shared_fraction': fraction, 'horizontal_span_px': float(span[0]),
|
||||
'vertical_span_px': float(span[1]), 'reason': reason})
|
||||
return reports
|
||||
@@ -1,44 +1,53 @@
|
||||
"""Incremental searches from both command limits, shared by live scans and replay."""
|
||||
"""Measure each endpoint at the first witnessed visual displacement."""
|
||||
import math
|
||||
|
||||
import numpy as np
|
||||
|
||||
from .errors import Unmeasurable
|
||||
from ..vision.observations import rms_delta
|
||||
from ..vision.motion import transient_evidence, witnessed_motion
|
||||
from ..vision.reference import jitter_radius
|
||||
|
||||
|
||||
SCAN_METHOD = 'endpoint_search_v1'
|
||||
SCAN_METHOD = 'endpoint_search_v10'
|
||||
ENDPOINT_METHODS = (SCAN_METHOD, *(f'endpoint_search_v{version}' for version in range(9, 0, -1)))
|
||||
|
||||
|
||||
class EndpointSearch:
|
||||
"""Confirm the first departure from a fixed endpoint reference.
|
||||
"""Compare stable corners with the fixed reference at the command limit.
|
||||
|
||||
Observations are stable summaries at consecutive command values. Confirmation
|
||||
retains the first trigger; later points never move an already confirmed bound.
|
||||
The engine verifies a quiet reference and supplies stable positions plus
|
||||
all post-command frames. Settled position and transient motion have
|
||||
different noise envelopes; neither uses image-space direction.
|
||||
"""
|
||||
|
||||
def __init__(self, joint, direction, settings):
|
||||
def __init__(self, joint, direction, settings, scan_method=SCAN_METHOD):
|
||||
if direction not in ('up', 'down'):
|
||||
raise ValueError('端点搜索方向无效')
|
||||
self.joint, self.direction, self.settings = joint, direction, settings
|
||||
self.scan_method = scan_method
|
||||
self.sign = 1 if direction == 'up' else -1
|
||||
self.origin = joint.minimum if direction == 'up' else joint.maximum
|
||||
self.terminal = joint.maximum if direction == 'up' else joint.minimum
|
||||
self.steps = 0
|
||||
self.reference, self.threshold = None, None
|
||||
self.candidate, self.support = None, 0
|
||||
self.reference_radius = 0.0
|
||||
self.evidence = None
|
||||
self.boundary, self.error = None, None
|
||||
|
||||
@property
|
||||
def done(self):
|
||||
return self.boundary is not None or self.error is not None
|
||||
|
||||
@property
|
||||
def next_command(self):
|
||||
return self.origin + self.sign * self.steps * self.joint.resolution
|
||||
|
||||
def add(self, command, observation):
|
||||
if self.done:
|
||||
return
|
||||
command = self.joint.validate_value(command)
|
||||
expected = self.origin + self.sign * self.steps * self.joint.resolution
|
||||
if not math.isclose(command, expected, rel_tol=0, abs_tol=1e-6):
|
||||
if not math.isclose(command, self.next_command, rel_tol=0, abs_tol=1e-6):
|
||||
raise Unmeasurable('端点搜索采样必须从指令端点按分辨率连续推进,不能缺点或重复')
|
||||
points = np.asarray(observation['corners'], dtype=float)
|
||||
if points.shape != (4, 2) or not np.isfinite(points).all():
|
||||
@@ -48,27 +57,35 @@ class EndpointSearch:
|
||||
if not math.isfinite(noise) or noise < 0:
|
||||
raise Unmeasurable('端点噪声估计无效')
|
||||
self.reference = points.copy()
|
||||
self.threshold = max(self.settings.motion_floor_px, self.settings.noise_multiplier * noise)
|
||||
elif rms_delta(points, self.reference) > self.threshold:
|
||||
if self.candidate is None:
|
||||
self.candidate = command
|
||||
self.support += 1
|
||||
if self.support >= self.settings.confirmation_points:
|
||||
bound = self.candidate - self.sign * self.joint.resolution
|
||||
self.boundary = {
|
||||
'bound': self.joint.validate_value(bound), 'trigger': self.candidate,
|
||||
'confirmed_at': command, 'threshold': self.threshold,
|
||||
}
|
||||
self.threshold = self.settings.motion_floor_px
|
||||
if self.scan_method != SCAN_METHOD:
|
||||
self.threshold = max(self.threshold, self.settings.noise_multiplier * noise)
|
||||
if observation.get('window_samples'):
|
||||
self.reference_radius = jitter_radius([f['corners'] for f in observation['window_samples']])
|
||||
else:
|
||||
self.candidate, self.support = None, 0
|
||||
displacement = rms_delta(points, self.reference)
|
||||
if self.scan_method == SCAN_METHOD:
|
||||
transient = transient_evidence(observation.get('motion_trace'), self.reference,
|
||||
self.reference_radius, observation.get('window_samples'),
|
||||
self.settings)
|
||||
else:
|
||||
transient = witnessed_motion(observation.get('motion'), self.reference, self.threshold,
|
||||
self.settings.motion_frames, self.settings.freshness)
|
||||
if displacement > self.threshold or transient:
|
||||
self.evidence = {'source': 'settled' if displacement > self.threshold else 'transient',
|
||||
'settled_displacement_px': displacement, 'transient': transient}
|
||||
self.boundary = {
|
||||
'bound': self.joint.validate_value(command - self.sign * self.joint.resolution),
|
||||
'trigger': command, 'confirmed_at': command, 'threshold': self.threshold,
|
||||
}
|
||||
self.steps += 1
|
||||
if self.boundary is None and math.isclose(command, self.terminal, rel_tol=0, abs_tol=1e-6):
|
||||
end = '低端' if self.direction == 'up' else '高端'
|
||||
self.error = end + '已扫描至对端,未获得足够观测点确认起动边界'
|
||||
self.error = end + '已扫描至对端,未检测到超过阈值的运动'
|
||||
|
||||
def result(self):
|
||||
if self.boundary is None:
|
||||
raise Unmeasurable(self.error or '端点搜索数据不足,起动边界尚未确认')
|
||||
raise Unmeasurable(self.error or '端点搜索数据不足,尚未检测到起动')
|
||||
return dict(self.boundary)
|
||||
|
||||
|
||||
@@ -88,12 +105,50 @@ def aggregate_endpoints(joint, endpoints, settings):
|
||||
'endpoints': {f'{r}_{d}': value for (r, d), value in endpoints.items()}}
|
||||
|
||||
|
||||
def analyze_endpoint_samples(joint, sweeps, settings):
|
||||
def _replay_search(joint, direction, settings, scan_method):
|
||||
if scan_method in (SCAN_METHOD, 'endpoint_search_v9', 'endpoint_search_v8'):
|
||||
return EndpointSearch(joint, direction, settings, scan_method)
|
||||
if scan_method not in ENDPOINT_METHODS:
|
||||
raise ValueError(f'不支持的日志扫描方法: {scan_method}')
|
||||
# Historical confirmation rules are isolated from the live scan path.
|
||||
from .legacy_endpoints import EndpointSearch as LegacyEndpointSearch
|
||||
return LegacyEndpointSearch(joint, direction, settings, scan_method)
|
||||
|
||||
|
||||
def analyze_endpoint_samples(joint, sweeps, settings, scan_method=SCAN_METHOD):
|
||||
endpoints = {}
|
||||
for repeat in range(settings.endpoint_repetitions):
|
||||
for direction in ('up', 'down'):
|
||||
search = EndpointSearch(joint, direction, settings)
|
||||
search = _replay_search(joint, direction, settings, scan_method)
|
||||
for sample in sweeps.get((repeat, direction), []):
|
||||
search.add(sample['command'], sample['observations'][joint.name])
|
||||
if 'search_commands' in sample:
|
||||
if joint.name not in sample['search_commands']:
|
||||
continue
|
||||
command = sample['search_commands'][joint.name]
|
||||
else: # Historical logs used the same command for all channels.
|
||||
command = sample['command']
|
||||
observed = sample['observations'][joint.name]
|
||||
if scan_method == SCAN_METHOD and 'window_samples' in observed:
|
||||
observed = _recompute_window(observed, settings, search.reference is None)
|
||||
search.add(command, observed)
|
||||
endpoints[(repeat, direction)] = search.result()
|
||||
return aggregate_endpoints(joint, endpoints, settings)
|
||||
|
||||
|
||||
def _recompute_window(observed, settings, reference):
|
||||
"""Recheck the recorded window instead of trusting a saved stable flag."""
|
||||
from ..vision.observations import Observation, StableWindow
|
||||
from ..vision.reference import ReferenceWindow
|
||||
window = (ReferenceWindow(settings) if reference
|
||||
else StableWindow(settings.stable_frames, settings.stable_seconds))
|
||||
for frame in observed['window_samples']:
|
||||
points = np.asarray(frame['corners'], dtype=float)
|
||||
if points.shape != (4, 2) or not np.isfinite(points).all():
|
||||
raise Unmeasurable('日志包含无效的稳定窗口角点')
|
||||
if not window.add(Observation(frame['stamp_ns'], 0, frame['corners'])):
|
||||
raise Unmeasurable('日志稳定窗口时间戳重复或倒退')
|
||||
measured = window.summary(settings.stability_noise_px, settings.stability_drift_px,
|
||||
settings.stability_drift_sigma)
|
||||
if measured is None:
|
||||
raise Unmeasurable('日志的端点参考或采样窗口不满足稳定条件')
|
||||
return {**observed, **measured}
|
||||
|
||||
@@ -4,9 +4,13 @@ from collections import deque
|
||||
import numpy as np
|
||||
|
||||
from .endpoints import SCAN_METHOD, EndpointSearch, aggregate_endpoints
|
||||
from .diagnostics import describe_issue, feedback_issues, shared_translation, tag_label
|
||||
from .errors import Unmeasurable
|
||||
from .scan_plan import endpoint_plan
|
||||
from .trajectory import Trajectory
|
||||
from ..vision.observations import Observation, StableWindow
|
||||
from ..vision.motion import MotionTrace
|
||||
from ..vision.reference import ReferenceWindow
|
||||
|
||||
|
||||
class Engine:
|
||||
@@ -22,8 +26,10 @@ class Engine:
|
||||
self.task_index = 0
|
||||
self.results = {j.name: {'min': None, 'max': None, 'status': '未完成'} for j in profile.joints}
|
||||
self.latest, self.view_stamps = {}, {}
|
||||
self.rejected_observations = {}
|
||||
self.feedbacks = deque(maxlen=64)
|
||||
self.windows = {}
|
||||
self.motion_captures = {}
|
||||
self.trajectory = None
|
||||
self.restoration_steps, self.restoring_joint = deque(), ''
|
||||
self.manual_target, self.manual_uid = None, None
|
||||
@@ -31,6 +37,7 @@ class Engine:
|
||||
self.manual_finished = float('-inf')
|
||||
self.phase = ''
|
||||
self.stage, self.direction, self.repeat = '', '', 0
|
||||
self.segment = None
|
||||
self.progress = (0, 0)
|
||||
self.last_diagnostic_warning = None
|
||||
|
||||
@@ -73,12 +80,12 @@ class Engine:
|
||||
self.task = self.tasks[self.task_index]
|
||||
self.prepare_pose = self.teaching.prepare(self.task)
|
||||
self.specs = [self.profile.by_name[name] for name in self.task.joints]
|
||||
self.active_indices = {j.index for j in self.specs}
|
||||
self.keys = {j.name: (j.view, j.tag_id) for j in self.specs}
|
||||
self.windows = {name: StableWindow(self.settings.stable_frames) for name in self.task.joints}
|
||||
self._set_observation_channels(self.task.joints)
|
||||
self.plan = endpoint_plan(self.task, self.profile.by_name)
|
||||
self.segment, self.segment_index = None, -1
|
||||
self.samples, self.failures = {}, {}
|
||||
self.endpoint_results = {name: {} for name in self.task.joints}
|
||||
self.searches = {}
|
||||
self.searches, self.search_bank = {}, {}
|
||||
self.reason = ''
|
||||
self.stage, self.direction, self.repeat = 'prepare', '', 0
|
||||
self.progress = (0, 2 * len(self.specs) * self.settings.endpoint_repetitions)
|
||||
@@ -87,21 +94,43 @@ class Engine:
|
||||
self._move(self.prepare_pose, now)
|
||||
|
||||
def _move(self, target, now):
|
||||
self.trajectory = Trajectory(self.profile, self.command, target, self.settings.rate, now)
|
||||
self.motion_captures = {}
|
||||
self.trajectory = Trajectory(self.profile, self.command, target, self.settings.rate, now,
|
||||
coordinated=self.state == 'SCANNING')
|
||||
self.phase = 'moving'
|
||||
self.blocked_since = None
|
||||
for window in self.windows.values():
|
||||
window.clear()
|
||||
self.unstable_reported = set()
|
||||
self.windows = {
|
||||
name: (ReferenceWindow(self.settings) if self.state == 'SCANNING'
|
||||
and name in self.searches and self.searches[name].reference is None
|
||||
else StableWindow(self.settings.stable_frames, self.settings.stable_seconds))
|
||||
for name in self.keys}
|
||||
|
||||
def observe(self, view, stamp_ns, accepted, now):
|
||||
def _set_observation_channels(self, names):
|
||||
joints = [self.profile.by_name[name] for name in names]
|
||||
self.active_indices = {joint.index for joint in joints}
|
||||
self.keys = {joint.name: (joint.view, joint.tag_id) for joint in joints}
|
||||
self.windows = {name: StableWindow(self.settings.stable_frames, self.settings.stable_seconds)
|
||||
for name in names}
|
||||
|
||||
def observe(self, view, stamp_ns, accepted, now, rejected=None):
|
||||
"""One whole detector frame; missing IDs explicitly invalidate observations."""
|
||||
if stamp_ns <= self.view_stamps.get(view, -1):
|
||||
return
|
||||
self.view_stamps[view] = stamp_ns
|
||||
for key in [k for k in self.latest if k[0] == view]:
|
||||
self.latest.pop(key)
|
||||
for key in [k for k in self.rejected_observations if k[0] == view]:
|
||||
self.rejected_observations.pop(key)
|
||||
for tag_id, reason in (rejected or {}).items():
|
||||
self.rejected_observations[(view, tag_id)] = (now, reason)
|
||||
for tag_id, corners in accepted.items():
|
||||
self.latest[(view, tag_id)] = Observation(stamp_ns, now, tuple(tuple(p) for p in corners))
|
||||
if self.state == 'SCANNING':
|
||||
for name, capture in self.motion_captures.items():
|
||||
key = self.keys[name]
|
||||
if key[0] == view:
|
||||
capture.observe(stamp_ns, self.latest.get(key))
|
||||
if self.state not in ('PREPARING', 'SCANNING') or self.phase != 'waiting':
|
||||
return
|
||||
required = [(name, key) for name, key in self.keys.items() if key[0] == view]
|
||||
@@ -116,14 +145,12 @@ class Engine:
|
||||
|
||||
def feedback_stable(self, after_ns=0, indices=None):
|
||||
"""Check selected channels; preparation and teaching use the whole hand."""
|
||||
return not self._feedback_issues(after_ns, indices)
|
||||
|
||||
def _feedback_issues(self, after_ns=0, indices=None):
|
||||
count = self.settings.stable_frames
|
||||
selected = [f for f in self.feedbacks if f.stamp_ns >= after_ns][-count:]
|
||||
if len(selected) < count:
|
||||
return False
|
||||
points = np.asarray([f.positions for f in selected])
|
||||
if indices is not None:
|
||||
points = points[:, list(indices)]
|
||||
return bool((np.ptp(points, axis=0) <= self.settings.feedback_tolerance).all())
|
||||
return feedback_issues(self.profile, selected, count, self.settings.feedback_tolerance, indices)
|
||||
|
||||
def can_capture_teaching(self, now):
|
||||
return (not self.active and self.state != 'PAUSED' and self.feedback_stable(self.manual_gate_ns)
|
||||
@@ -185,6 +212,58 @@ class Engine:
|
||||
return all(key in self.latest and now-self.latest[key].received <= self.settings.freshness
|
||||
for key in self.keys.values())
|
||||
|
||||
def _visibility_issues(self, now):
|
||||
issues = []
|
||||
for name, (view, tag_id) in self.keys.items():
|
||||
key = (view, tag_id)
|
||||
observation = self.latest.get(key)
|
||||
rejected = self.rejected_observations.get(key)
|
||||
if rejected and now - rejected[0] <= self.settings.freshness:
|
||||
reason = rejected[1]
|
||||
elif observation is None:
|
||||
reason = '未检出有效 Tag'
|
||||
elif now - observation.received > self.settings.freshness:
|
||||
reason = f'有效观测已过期:{now - observation.received:.2f} 秒未更新'
|
||||
elif self.phase == 'waiting' and observation.stamp_ns < self.gate_ns:
|
||||
reason = '尚未收到目标到达并等待后的新图像'
|
||||
else:
|
||||
continue
|
||||
issues.append({'source': 'tag', 'joint': name, 'view': view, 'tag_id': tag_id,
|
||||
'reason': reason})
|
||||
return issues
|
||||
|
||||
def _pause_sampling_timeout(self, now, moving=False):
|
||||
restoring = self.state == 'RESTORING'
|
||||
issues = [] if restoring else self._visibility_issues(now)
|
||||
if not moving:
|
||||
indices = self.active_indices if self.state == 'SCANNING' else None
|
||||
issues.extend(self._feedback_issues(self.gate_ns, indices))
|
||||
# Missing observations or unstable feedback reset all image windows.
|
||||
# Report those primary blockers instead of blaming every cleared Tag.
|
||||
if not issues and not moving and not restoring and self.stage != 'segment_finish':
|
||||
for name, window in self.windows.items():
|
||||
problem = window.problem(self.settings.stability_noise_px, self.settings.stability_drift_px,
|
||||
self.settings.stability_drift_sigma)
|
||||
if problem:
|
||||
view, tag_id = self.keys[name]
|
||||
issues.append({'source': 'tag', 'joint': name, 'view': view,
|
||||
'tag_id': tag_id, 'window_samples': window.snapshot(), **problem})
|
||||
prefix = ('目标 Tag 持续不可见或观测过期' if moving else
|
||||
'测后回位稳定超时' if restoring else '单点稳定采样超时')
|
||||
details = '\n'.join(describe_issue(issue) for issue in issues)
|
||||
shared_motion = shared_translation(issues)
|
||||
if shared_motion:
|
||||
details += '\n' + '\n'.join(report['reason'] for report in shared_motion)
|
||||
if not details:
|
||||
# Conditions can recover on the tick that crosses the deadline.
|
||||
context = self.restoring_joint if restoring else '、'.join(
|
||||
tag_label(view, tag_id, name) for name, (view, tag_id) in self.keys.items())
|
||||
details = f'{context}:条件刚恢复,但已超过 {self.settings.point_timeout:g} 秒等待时限'
|
||||
self._record('sampling_timeout', time=now, task=self.task.name, stage=self.stage,
|
||||
direction=self.direction, repeat=self.repeat, target=list(self.command),
|
||||
issues=issues, shared_motion=shared_motion, reason=details)
|
||||
self.pause(prefix + '\n' + details)
|
||||
|
||||
def tick(self, now, stamp_ns):
|
||||
feedback = self.adapter.feedback
|
||||
if feedback and (not self.feedbacks or feedback.stamp_ns > self.feedbacks[-1].stamp_ns):
|
||||
@@ -215,7 +294,7 @@ class Engine:
|
||||
if self.blocked_since is None:
|
||||
self.blocked_since = now
|
||||
if now-self.blocked_since > self.settings.point_timeout:
|
||||
self.pause('目标 Tag 持续不可见或观测过期')
|
||||
self._pause_sampling_timeout(now, moving=True)
|
||||
return
|
||||
else:
|
||||
self.blocked_since = None
|
||||
@@ -230,8 +309,7 @@ class Engine:
|
||||
window.clear()
|
||||
return
|
||||
if now-self.arrived > self.settings.point_timeout:
|
||||
self.pause('测后回位稳定超时:检查反馈' if self.state == 'RESTORING'
|
||||
else '单点稳定采样超时:检查 Tag 可见性、图像质量和反馈')
|
||||
self._pause_sampling_timeout(now)
|
||||
return
|
||||
if self.state == 'RESTORING':
|
||||
if self.feedback_stable(self.gate_ns):
|
||||
@@ -245,24 +323,45 @@ class Engine:
|
||||
for window in self.windows.values():
|
||||
window.clear()
|
||||
return
|
||||
summaries = {name: window.summary(self.settings.stability_px)
|
||||
if self.stage == 'segment_finish':
|
||||
# The group must reach its configured endpoint before moving one finger alone.
|
||||
self._record('segment_positioned', task=self.task.name, segment=self.segment.name,
|
||||
target=list(self.command), feedback=list(feedback.positions), stamp_ns=stamp_ns)
|
||||
self._next_segment(now)
|
||||
return
|
||||
summaries = {name: window.summary(self.settings.stability_noise_px, self.settings.stability_drift_px,
|
||||
self.settings.stability_drift_sigma)
|
||||
for name, window in self.windows.items()}
|
||||
if any(value is None for value in summaries.values()):
|
||||
for name, summary in summaries.items():
|
||||
quality = self.windows[name].last_quality
|
||||
if summary is None and quality and name not in self.unstable_reported:
|
||||
view, tag_id = self.keys[name]
|
||||
self._record('sampling_unstable', task=self.task.name, joint=name,
|
||||
view=view, tag_id=tag_id, target=list(self.command),
|
||||
window_samples=self.windows[name].snapshot(), **quality)
|
||||
self.unstable_reported.add(name)
|
||||
return
|
||||
if self.state == 'PREPARING':
|
||||
self._record('prepared', task=self.task.name, target=list(self.command),
|
||||
feedback=list(feedback.positions))
|
||||
self.state = 'SCANNING'
|
||||
self._begin_endpoint('up', now)
|
||||
self._next_segment(now)
|
||||
else:
|
||||
sample = {'command': self.point_command, 'observations': summaries,
|
||||
search_commands = self._search_commands()
|
||||
for name, summary in summaries.items():
|
||||
summary['window_samples'] = self.windows[name].snapshot()
|
||||
for name, capture in self.motion_captures.items():
|
||||
summaries[name]['motion_trace'] = capture.snapshot()
|
||||
sample = {'command': self.point_command, 'search_commands': search_commands,
|
||||
'segment': self.segment.name, 'observations': summaries,
|
||||
'feedback': list(feedback.positions), 'target': list(self.command),
|
||||
'feedback_stamp_ns': feedback.stamp_ns}
|
||||
key = (self.repeat, self.direction)
|
||||
self.samples.setdefault(key, []).append(sample)
|
||||
self._record('sample', task=self.task.name, stage='endpoint', repeat=self.repeat,
|
||||
direction=self.direction, **sample)
|
||||
self._accept_endpoint_sample(summaries, now)
|
||||
self._accept_endpoint_sample(summaries, search_commands, now)
|
||||
|
||||
def _tick_manual(self, now, stamp_ns):
|
||||
if self.manual_pending:
|
||||
@@ -280,53 +379,104 @@ class Engine:
|
||||
self.command = tuple(values)
|
||||
self._record('command', target=list(values), stamp_ns=stamp_ns, time=now,
|
||||
state=self.state, task=self.task.name if self.task else None)
|
||||
if self.state == 'SCANNING':
|
||||
for name, target in self._search_commands().items():
|
||||
search = self.searches[name]
|
||||
# Start at the first actual publication of this channel's new
|
||||
# target, which may precede the trajectory's completion tick.
|
||||
if (name not in self.motion_captures and search.reference is not None and
|
||||
values[self.profile.by_name[name].index] == target):
|
||||
self.motion_captures[name] = MotionTrace(stamp_ns)
|
||||
|
||||
def _begin_endpoint(self, direction, now):
|
||||
self.direction, self.stage = direction, 'reference'
|
||||
self.searches = {joint.name: EndpointSearch(joint, direction, self.settings)
|
||||
for joint in self.specs if joint.name not in self.failures}
|
||||
def _next_segment(self, now):
|
||||
while True:
|
||||
self.segment_index += 1
|
||||
if self.segment_index == len(self.plan):
|
||||
if self.repeat + 1 == self.settings.endpoint_repetitions or len(self.failures) == len(self.specs):
|
||||
self._endpoints_finished(now)
|
||||
return
|
||||
self.repeat += 1
|
||||
self.segment_index = 0
|
||||
self.segment = self.plan[self.segment_index]
|
||||
self.direction, self.stage = self.segment.direction, 'reference'
|
||||
self.searches = {}
|
||||
for name in self.segment.measure:
|
||||
if name in self.failures:
|
||||
continue
|
||||
key = (self.repeat, self.direction, name)
|
||||
if not self.segment.continues:
|
||||
self.search_bank[key] = EndpointSearch(self.profile.by_name[name], self.direction, self.settings)
|
||||
self.searches[name] = self.search_bank[key]
|
||||
if any(not search.done for search in self.searches.values()):
|
||||
break
|
||||
names = tuple(dict.fromkeys((*self.segment.moving, *self.segment.measure)))
|
||||
self._set_observation_channels(names)
|
||||
self.point_index = 0
|
||||
self._record('segment_started', task=self.task.name, segment=self.segment.name,
|
||||
label=self.segment.label, direction=self.direction, repeat=self.repeat,
|
||||
start=self.segment.start, end=self.segment.end, measure=list(self.segment.measure),
|
||||
continues=self.segment.continues)
|
||||
self._move_to_endpoint_point(now)
|
||||
|
||||
def _move_to_endpoint_point(self, now):
|
||||
joint = self.specs[0]
|
||||
origin, sign = (joint.minimum, 1) if self.direction == 'up' else (joint.maximum, -1)
|
||||
self.point_command = joint.validate_value(origin + sign * self.point_index * joint.resolution)
|
||||
self.point_commands = self.segment.point(self.point_index, self.profile.by_name)
|
||||
self.point_command = self.point_commands[self.segment.measure[0]]
|
||||
goal = list(self.prepare_pose)
|
||||
for joint in self.specs:
|
||||
goal[joint.index] = self.point_command
|
||||
for name, value in self.point_commands.items():
|
||||
goal[self.profile.by_name[name].index] = value
|
||||
self._move(goal, now)
|
||||
|
||||
def _accept_endpoint_sample(self, summaries, now):
|
||||
def _search_commands(self):
|
||||
# Shorter channel intervals may hold the same command for two group points.
|
||||
# Such a point cannot confirm movement twice or reset a continued reference.
|
||||
commands = {}
|
||||
for name, search in self.searches.items():
|
||||
if search.done:
|
||||
continue
|
||||
search.add(self.point_command, summaries[name])
|
||||
command = self.point_commands[name]
|
||||
previous = search.next_command - search.sign * search.joint.resolution
|
||||
if search.steps and np.isclose(command, previous, rtol=0, atol=1e-6):
|
||||
continue
|
||||
commands[name] = command
|
||||
return commands
|
||||
|
||||
def _accept_endpoint_sample(self, summaries, search_commands, now):
|
||||
at_end = self.point_index == self.segment.steps(self.profile.by_name)
|
||||
for name, command in search_commands.items():
|
||||
search = self.searches[name]
|
||||
search.add(command, summaries[name])
|
||||
if search.boundary is not None:
|
||||
boundary = search.result()
|
||||
self.endpoint_results[name][(self.repeat, self.direction)] = boundary
|
||||
self._record('boundary_found', task=self.task.name, joint=name, repeat=self.repeat,
|
||||
direction=self.direction, **boundary)
|
||||
direction=self.direction, evidence=search.evidence, **boundary)
|
||||
elif search.error:
|
||||
self.failures[name] = search.error
|
||||
self._record('boundary_failed', task=self.task.name, joint=name, repeat=self.repeat,
|
||||
direction=self.direction, reason=search.error)
|
||||
self._boundary_failed(name, search.error)
|
||||
if at_end and not self.segment.has_continuation:
|
||||
for name, search in self.searches.items():
|
||||
if not search.done:
|
||||
search.error = (f'{"低端" if self.direction == "up" else "高端"}在阶段范围 '
|
||||
f'{self.segment.start[name]:g}→{self.segment.end[name]:g} '
|
||||
'内未检测到超过阈值的运动')
|
||||
self._boundary_failed(name, search.error)
|
||||
total_per_joint = 2 * self.settings.endpoint_repetitions
|
||||
self.progress = (sum(total_per_joint if name in self.failures else len(values)
|
||||
for name, values in self.endpoint_results.items()), self.progress[1])
|
||||
if not all(search.done for search in self.searches.values()):
|
||||
if not at_end and not all(search.done for search in self.searches.values()):
|
||||
self.stage = 'search'
|
||||
self.point_index += 1
|
||||
self._move_to_endpoint_point(now)
|
||||
elif len(self.failures) == len(self.specs):
|
||||
self._finish_task({}, now)
|
||||
elif self.direction == 'up':
|
||||
self._begin_endpoint('down', now)
|
||||
elif self.repeat + 1 < self.settings.endpoint_repetitions:
|
||||
self.repeat += 1
|
||||
self._begin_endpoint('up', now)
|
||||
elif self.segment.finish_at_end and not at_end:
|
||||
self.stage = 'segment_finish'
|
||||
self.point_index = self.segment.steps(self.profile.by_name)
|
||||
self._move_to_endpoint_point(now)
|
||||
else:
|
||||
self._endpoints_finished(now)
|
||||
self._next_segment(now)
|
||||
|
||||
def _boundary_failed(self, name, reason):
|
||||
self.failures[name] = reason
|
||||
self._record('boundary_failed', task=self.task.name, joint=name, repeat=self.repeat,
|
||||
direction=self.direction, reason=reason)
|
||||
|
||||
def _endpoints_finished(self, now):
|
||||
outcomes = {}
|
||||
|
||||
+218
@@ -0,0 +1,218 @@
|
||||
"""Frozen v1-v7 algorithms used only to reproduce historical journals."""
|
||||
from collections import deque
|
||||
import math
|
||||
|
||||
import numpy as np
|
||||
|
||||
from .errors import Unmeasurable
|
||||
from ..vision.observations import rms_delta
|
||||
|
||||
|
||||
SCAN_METHOD = 'endpoint_search_v7'
|
||||
ENDPOINT_METHODS = (SCAN_METHOD, 'endpoint_search_v6', 'endpoint_search_v5', 'endpoint_search_v4',
|
||||
'endpoint_search_v3', 'endpoint_search_v2', 'endpoint_search_v1')
|
||||
|
||||
|
||||
class _MotionContinuation:
|
||||
"""Confirm continued travel independently of the first departure direction.
|
||||
|
||||
The first displacement seeds a direction. A later significant turn replaces
|
||||
that direction, then needs further travel to confirm it. This neither moves
|
||||
the onset command nor accepts alternating offsets as continued motion.
|
||||
"""
|
||||
|
||||
def __init__(self, reference, departure, threshold):
|
||||
self.anchor = departure.copy()
|
||||
self.axis = departure - reference
|
||||
self.threshold = threshold
|
||||
|
||||
def observe(self, points):
|
||||
displacement = points - self.anchor
|
||||
distance = float(np.sqrt(np.mean(np.sum(self.axis ** 2, axis=1))))
|
||||
projection = float(np.mean(np.sum(displacement * self.axis, axis=1)) / distance)
|
||||
if projection > self.threshold:
|
||||
return True
|
||||
if rms_delta(points, self.anchor) > self.threshold:
|
||||
self.axis = displacement
|
||||
self.anchor = points.copy()
|
||||
return False
|
||||
|
||||
|
||||
class EndpointSearch:
|
||||
"""Confirm departure with continued travel from a stable endpoint platform.
|
||||
|
||||
Observations are stable summaries at consecutive command values. Confirmation
|
||||
retains the first trigger; later points never move an already confirmed bound.
|
||||
"""
|
||||
|
||||
def __init__(self, joint, direction, settings, scan_method=SCAN_METHOD):
|
||||
if direction not in ('up', 'down'):
|
||||
raise ValueError('端点搜索方向无效')
|
||||
if scan_method not in ENDPOINT_METHODS:
|
||||
raise ValueError('端点搜索方法无效')
|
||||
self.joint, self.direction, self.settings = joint, direction, settings
|
||||
self.scan_method = scan_method
|
||||
self.require_progress = scan_method not in ('endpoint_search_v1', 'endpoint_search_v2')
|
||||
self.recent_plateau = scan_method not in ('endpoint_search_v1', 'endpoint_search_v2', 'endpoint_search_v3')
|
||||
self.recover_subthreshold_offset = scan_method in ('endpoint_search_v5', 'endpoint_search_v6', 'endpoint_search_v7')
|
||||
self.reject_opposed_candidate = scan_method == 'endpoint_search_v6'
|
||||
self.follow_continuation = scan_method == 'endpoint_search_v7'
|
||||
self.sign = 1 if direction == 'up' else -1
|
||||
self.origin = joint.minimum if direction == 'up' else joint.maximum
|
||||
self.terminal = joint.maximum if direction == 'up' else joint.minimum
|
||||
self.steps = 0
|
||||
self.reference, self.threshold = None, None
|
||||
self.reference_noise = None
|
||||
self.candidate, self.support = None, 0
|
||||
self.candidate_points = None
|
||||
self.continuation = None
|
||||
self.history = deque(maxlen=max(3, settings.confirmation_points))
|
||||
self.rejections = []
|
||||
self.boundary, self.error = None, None
|
||||
|
||||
@property
|
||||
def done(self):
|
||||
return self.boundary is not None or self.error is not None
|
||||
|
||||
@property
|
||||
def next_command(self):
|
||||
return self.origin + self.sign * self.steps * self.joint.resolution
|
||||
|
||||
def add(self, command, observation):
|
||||
if self.done:
|
||||
return
|
||||
command = self.joint.validate_value(command)
|
||||
expected = self.next_command
|
||||
if not math.isclose(command, expected, rel_tol=0, abs_tol=1e-6):
|
||||
raise Unmeasurable('端点搜索采样必须从指令端点按分辨率连续推进,不能缺点或重复')
|
||||
points = np.asarray(observation['corners'], dtype=float)
|
||||
if points.shape != (4, 2) or not np.isfinite(points).all():
|
||||
raise Unmeasurable('端点角点观测无效')
|
||||
if self.reference is None:
|
||||
noise = float(observation['noise'])
|
||||
if not math.isfinite(noise) or noise < 0:
|
||||
raise Unmeasurable('端点噪声估计无效')
|
||||
self.reference = points.copy()
|
||||
self.reference_noise = noise
|
||||
self.threshold = max(self.settings.motion_floor_px, self.settings.noise_multiplier * noise)
|
||||
else:
|
||||
self._update_evidence(command, points)
|
||||
self.history.append((command, points.copy()))
|
||||
self.steps += 1
|
||||
if self.boundary is None and math.isclose(command, self.terminal, rel_tol=0, abs_tol=1e-6):
|
||||
end = '低端' if self.direction == 'up' else '高端'
|
||||
self.error = end + '已扫描至对端,未获得足够观测点确认起动边界'
|
||||
|
||||
def _progress_from_candidate(self, points):
|
||||
"""Signed additional travel along the candidate's 4-corner displacement.
|
||||
|
||||
Unlike center displacement, this also measures a tag rotating in place.
|
||||
Units are pixels, normalized in the same way as rms_delta.
|
||||
"""
|
||||
return self._projected_travel(points, self.candidate_points)
|
||||
|
||||
def _projected_travel(self, points, start, minimum_offset=None):
|
||||
axis = start - self.reference
|
||||
distance = rms_delta(start, self.reference)
|
||||
if distance <= (self.threshold if minimum_offset is None else max(1e-9, minimum_offset)):
|
||||
return 0.0
|
||||
return float(np.mean(np.sum((points - start) * axis, axis=1)) / distance)
|
||||
|
||||
def _discard_reversed_offset(self, command, points):
|
||||
"""A one-off offset followed by a plateau cannot establish scan direction.
|
||||
|
||||
If motion then leaves that stable plateau in the opposite direction,
|
||||
restart from the plateau. Never move a confirmed boundary or demand
|
||||
continuous motion through the finger's interior range.
|
||||
"""
|
||||
count = self.history.maxlen
|
||||
if not self.require_progress or len(self.history) < count:
|
||||
return
|
||||
if self.candidate is None:
|
||||
if not self.recover_subthreshold_offset:
|
||||
return
|
||||
elif self.support < count:
|
||||
return
|
||||
plateau = np.median([value for _, value in self.history], axis=0)
|
||||
if any(rms_delta(value, plateau) > self.threshold for _, value in self.history):
|
||||
return
|
||||
# The initial offset can creep before settling. A later departure must
|
||||
# be measured from that settled platform, not from the first offset.
|
||||
start = plateau if self.recent_plateau else self.candidate_points
|
||||
# A settled initial offset can stay below the departure threshold yet
|
||||
# cancel the first real movement in the opposite direction. V5 allows
|
||||
# that axis once it exceeds reference noise, even with no candidate.
|
||||
# Actual departure and subsequent confirmation still require the full
|
||||
# motion threshold; do not move the reference along slow forward travel.
|
||||
axis_floor = self.reference_noise if self.recover_subthreshold_offset else None
|
||||
if self._projected_travel(points, start, axis_floor) >= -self.threshold:
|
||||
return
|
||||
self.rejections.append({
|
||||
'trigger': self.candidate, 'rejected_at': command,
|
||||
'reason': '初始偏移形成稳定平台,随后反向离开;改用最近平台继续确认',
|
||||
'initial_offset_px': rms_delta(plateau, self.reference),
|
||||
'threshold': self.threshold,
|
||||
'reference_commands': [value for value, _ in self.history],
|
||||
'reference_corners': plateau.tolist(),
|
||||
})
|
||||
self.reference = plateau
|
||||
self._clear_candidate()
|
||||
|
||||
def _clear_candidate(self):
|
||||
self.candidate, self.candidate_points, self.support = None, None, 0
|
||||
self.continuation = None
|
||||
|
||||
def _discard_opposed_candidate(self, command, points):
|
||||
"""Crossing the reference invalidates the unconfirmed departure direction.
|
||||
|
||||
A discrete command step can move from one side of the reference to the
|
||||
other without a sample inside the threshold. Reset its evidence just as
|
||||
when returning to the reference; do not wait for another static plateau.
|
||||
Keep the original reference and noise threshold, and require fresh
|
||||
confirmation points plus additional travel in the new direction.
|
||||
"""
|
||||
if not self.reject_opposed_candidate or self.candidate is None:
|
||||
return
|
||||
axis = self.candidate_points - self.reference
|
||||
distance = rms_delta(self.candidate_points, self.reference)
|
||||
departure = float(np.mean(np.sum((points - self.reference) * axis, axis=1)) / distance)
|
||||
if departure >= -self.threshold:
|
||||
return
|
||||
self.rejections.append({
|
||||
'trigger': self.candidate, 'rejected_at': command,
|
||||
'reason': '观测跨过参考姿态,离开方向与未确认候选相反;重新确认起动',
|
||||
'projected_departure_px': departure, 'threshold': self.threshold,
|
||||
})
|
||||
self._clear_candidate()
|
||||
|
||||
def _update_evidence(self, command, points):
|
||||
self._discard_reversed_offset(command, points)
|
||||
if rms_delta(points, self.reference) <= self.threshold:
|
||||
self._clear_candidate()
|
||||
return
|
||||
self._discard_opposed_candidate(command, points)
|
||||
if self.candidate is None:
|
||||
self.candidate, self.candidate_points = command, points.copy()
|
||||
if self.follow_continuation:
|
||||
self.continuation = _MotionContinuation(self.reference, points, self.threshold)
|
||||
self.support += 1
|
||||
# Several still frames at an offset position are one displacement, not
|
||||
# repeated evidence of travel. Keep the first trigger while awaiting
|
||||
# additional motion, even if that motion takes a different direction.
|
||||
# Older journals retain their original fixed-direction confirmation.
|
||||
if self.continuation is not None:
|
||||
progressed = self.continuation.observe(points)
|
||||
else:
|
||||
progressed = not self.require_progress or self._progress_from_candidate(points) > self.threshold
|
||||
if self.support >= self.settings.confirmation_points and progressed:
|
||||
bound = self.candidate - self.sign * self.joint.resolution
|
||||
self.boundary = {
|
||||
'bound': self.joint.validate_value(bound), 'trigger': self.candidate,
|
||||
'confirmed_at': command, 'threshold': self.threshold,
|
||||
}
|
||||
|
||||
def result(self):
|
||||
if self.boundary is None:
|
||||
raise Unmeasurable(self.error or '端点搜索数据不足,起动边界尚未确认')
|
||||
return dict(self.boundary)
|
||||
|
||||
@@ -0,0 +1,57 @@
|
||||
"""Model-independent endpoint stages, including a deferred lower-bound search."""
|
||||
from dataclasses import dataclass
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class ScanSegment:
|
||||
name: str
|
||||
label: str
|
||||
direction: str
|
||||
start: dict[str, float]
|
||||
end: dict[str, float]
|
||||
measure: tuple[str, ...]
|
||||
# A split search preserves its original endpoint reference.
|
||||
continues: bool = False
|
||||
has_continuation: bool = False
|
||||
finish_at_end: bool = False
|
||||
|
||||
@property
|
||||
def moving(self):
|
||||
return tuple(name for name in self.start if self.start[name] != self.end[name])
|
||||
|
||||
def steps(self, by_name):
|
||||
return max(round(abs(self.end[name] - value) / by_name[name].resolution)
|
||||
for name, value in self.start.items())
|
||||
|
||||
def point(self, index, by_name):
|
||||
"""Shared progress, quantized per channel; no channel skips a command step."""
|
||||
steps = self.steps(by_name)
|
||||
values = {}
|
||||
for name, start in self.start.items():
|
||||
joint = by_name[name]
|
||||
distance = round((self.end[name] - start) / joint.resolution)
|
||||
value = start + round(distance * index / steps) * joint.resolution
|
||||
values[name] = joint.validate_value(value)
|
||||
return values
|
||||
|
||||
|
||||
def endpoint_plan(task, by_name):
|
||||
low = {name: by_name[name].minimum for name in task.joints}
|
||||
high = {name: by_name[name].maximum for name in task.joints}
|
||||
if task.deferred_lower is None:
|
||||
return (
|
||||
ScanSegment('lower', '测下限', 'up', low, high, task.joints),
|
||||
ScanSegment('upper', '测上限', 'down', high, low, task.joints),
|
||||
)
|
||||
deferred = task.deferred_lower
|
||||
split = {**low, deferred.joint: by_name[deferred.joint].validate_value(deferred.split)}
|
||||
others = tuple(name for name in task.joints if name != deferred.joint)
|
||||
return (
|
||||
ScanSegment('group_lower', '同步测其余关节下限', 'up', split, high, others),
|
||||
ScanSegment('group_upper', '同步测全部关节上限', 'down', high, split,
|
||||
task.joints, finish_at_end=True),
|
||||
ScanSegment('deferred_lower', f'{deferred.joint} 单独测下限', 'up', low, split,
|
||||
(deferred.joint,), has_continuation=True),
|
||||
ScanSegment('deferred_extension', f'{deferred.joint} 联动继续测下限', 'up', split, high,
|
||||
(deferred.joint,), continues=True),
|
||||
)
|
||||
@@ -3,17 +3,24 @@ import numpy as np
|
||||
|
||||
|
||||
class Trajectory:
|
||||
def __init__(self, profile, start, goal, rate, now):
|
||||
def __init__(self, profile, start, goal, rate, now, coordinated=False):
|
||||
self.profile = profile
|
||||
self.value = np.asarray(profile.vector(start), dtype=float)
|
||||
self.goal = np.asarray(profile.vector(goal), dtype=float)
|
||||
self.rate, self.previous = rate, now
|
||||
self.coordinated = coordinated
|
||||
|
||||
def advance(self, now, hold=False):
|
||||
dt = max(0.0, min(now-self.previous, 0.1))
|
||||
self.previous = now
|
||||
if not hold:
|
||||
self.value += np.clip(self.goal-self.value, -self.rate*dt, self.rate*dt)
|
||||
delta = self.goal-self.value
|
||||
if self.coordinated:
|
||||
distance = np.max(np.abs(delta))
|
||||
scale = min(1.0, self.rate * dt / distance) if distance else 0.0
|
||||
self.value += delta * scale
|
||||
else:
|
||||
self.value += np.clip(delta, -self.rate*dt, self.rate*dt)
|
||||
values = [j.minimum + round((self.value[j.index]-j.minimum)/j.resolution)*j.resolution
|
||||
for j in self.profile.joints]
|
||||
return self.profile.vector(values), bool(np.allclose(self.value, self.goal, atol=1e-8))
|
||||
|
||||
@@ -28,6 +28,13 @@ class Joint:
|
||||
return int(value) if value.is_integer() else value
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class DeferredLower:
|
||||
"""Keep this channel above a split during group scans; find its low end last."""
|
||||
joint: str
|
||||
split: float
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class Task:
|
||||
name: str
|
||||
@@ -36,6 +43,7 @@ class Task:
|
||||
allow_override: bool = True
|
||||
# Restore one joint at a time in this order, waiting for feedback at each step.
|
||||
restore_after: tuple[str, ...] = ()
|
||||
deferred_lower: DeferredLower | None = None
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
@@ -85,21 +93,34 @@ class Settings:
|
||||
rate: float = 20
|
||||
settle_seconds: float = 0.3
|
||||
stable_frames: int = 8
|
||||
point_timeout: float = 3
|
||||
stable_seconds: float = 0.5
|
||||
reference_seconds: float = 1.5 # Verify the endpoint across independent time blocks.
|
||||
motion_frames: int = 2 # Consecutive valid frames at this command, independent of direction.
|
||||
point_timeout: float = 6
|
||||
freshness: float = 0.5
|
||||
feedback_tolerance: float = 2
|
||||
non_target_tolerance: float = 3 # Legacy configuration only; unrelated motion does not gate scans.
|
||||
repeat_tolerance: float = 2
|
||||
motion_floor_px: float = 0.5
|
||||
noise_multiplier: float = 5
|
||||
stability_px: float = 1
|
||||
confirmation_points: int = 3
|
||||
noise_multiplier: float = 5 # Historical endpoint algorithms only.
|
||||
stability_noise_px: float = 0.5
|
||||
stability_drift_px: float = 0.1
|
||||
stability_drift_sigma: float = 3
|
||||
confirmation_points: int = 3 # Historical journals only; live scans stop at first motion.
|
||||
max_hamming: int = 0
|
||||
min_margin: float = 30
|
||||
min_edge_px: float = 30
|
||||
|
||||
@classmethod
|
||||
def from_dict(cls, data):
|
||||
data = dict(data)
|
||||
# Historical journals and custom stations used a shared tolerance.
|
||||
# Keep its noise limit; drift now has an independent, explicit default.
|
||||
if 'stability_px' in data:
|
||||
legacy = data.pop('stability_px')
|
||||
if not isinstance(legacy, (float, int)) or not math.isfinite(legacy) or legacy <= 0:
|
||||
raise ValueError('旧扫描参数 stability_px 必须为正有限数值')
|
||||
data.setdefault('stability_noise_px', legacy)
|
||||
unknown = set(data) - {f.name for f in fields(cls)}
|
||||
if unknown:
|
||||
raise ValueError(f'未知扫描参数: {sorted(unknown)}')
|
||||
@@ -109,13 +130,19 @@ class Settings:
|
||||
raise ValueError(f'扫描参数 {name} 必须为有限数值')
|
||||
if value < 0 or (name != 'max_hamming' and value == 0):
|
||||
raise ValueError(f'扫描参数 {name} 范围不合法')
|
||||
for name in ('endpoint_repetitions', 'fine_repetitions', 'stable_frames', 'confirmation_points', 'max_hamming'):
|
||||
for name in ('endpoint_repetitions', 'fine_repetitions', 'stable_frames', 'motion_frames', 'confirmation_points', 'max_hamming'):
|
||||
if not isinstance(getattr(result, name), int):
|
||||
raise ValueError(f'{name} 必须为整数')
|
||||
if result.stable_frames < 4 or result.fine_repetitions < 2:
|
||||
raise ValueError('稳定帧至少4张,细扫复测至少2轮')
|
||||
if result.point_timeout <= result.settle_seconds:
|
||||
raise ValueError('单点超时必须大于最短等待')
|
||||
if result.motion_frames < 2:
|
||||
raise ValueError('运动过程检测至少需要2张连续有效图像')
|
||||
if result.point_timeout <= result.settle_seconds + result.stable_seconds:
|
||||
raise ValueError('单点超时必须大于最短等待与稳定观察时长之和')
|
||||
if result.reference_seconds < result.stable_seconds:
|
||||
raise ValueError('端点参考观察时长不能短于普通稳定观察时长')
|
||||
if result.point_timeout <= result.settle_seconds + result.reference_seconds:
|
||||
raise ValueError('单点超时必须大于最短等待与端点参考观察时长之和')
|
||||
return result
|
||||
|
||||
|
||||
@@ -154,7 +181,8 @@ def profile_from_dict(data):
|
||||
raise ValueError(f'{j.name}: 指令范围或 Tag 配置无效')
|
||||
j.validate_value(j.maximum)
|
||||
tasks = tuple(Task(t['name'], tuple(t['joints']), tuple(t.get('clearances', [])),
|
||||
t.get('allow_override', True), tuple(t.get('restore_after', [])))
|
||||
t.get('allow_override', True), tuple(t.get('restore_after', [])),
|
||||
DeferredLower(**t['deferred_lower']) if 'deferred_lower' in t else None)
|
||||
for t in data['tasks'])
|
||||
used = [name for t in tasks for name in t.joints]
|
||||
if sorted(used) != sorted(names) or len({t.name for t in tasks}) != len(tasks):
|
||||
@@ -185,6 +213,14 @@ def profile_from_dict(data):
|
||||
observations = [(j.view, j.tag_id) for j in joints if j.name in t.joints]
|
||||
if len(set(observations)) != len(observations):
|
||||
raise ValueError(f'并行任务 {t.name} 必须有独立观测源')
|
||||
if t.deferred_lower is not None:
|
||||
deferred = t.deferred_lower
|
||||
if len(t.joints) < 2 or deferred.joint not in t.joints:
|
||||
raise ValueError(f'任务 {t.name} 的延后下限关节必须属于多关节任务')
|
||||
joint = by_name[deferred.joint]
|
||||
split = joint.validate_value(deferred.split)
|
||||
if not joint.minimum < split < joint.maximum:
|
||||
raise ValueError(f'任务 {t.name} 的下限搜索分段值必须位于指令范围内部')
|
||||
return Profile(data['model'], data['side'], data['command_unit'], data['adapter'],
|
||||
joints, tasks, clearances, data['sdk'], data)
|
||||
|
||||
|
||||
@@ -3,7 +3,7 @@ import json
|
||||
from pathlib import Path
|
||||
|
||||
from .core.analysis import aggregate, direction_range, Unmeasurable, RefineWindow
|
||||
from .core.endpoints import SCAN_METHOD, analyze_endpoint_samples
|
||||
from .core.endpoints import ENDPOINT_METHODS, analyze_endpoint_samples
|
||||
from .profiles import profile_from_dict, Settings
|
||||
from .storage import write_json
|
||||
|
||||
@@ -20,7 +20,7 @@ def replay(journal, output):
|
||||
profile = profile_from_dict(event['profile'])
|
||||
settings = Settings.from_dict(event['settings'])
|
||||
scan_method = event.get('scan_method', 'coarse_fine_v1')
|
||||
if scan_method not in (SCAN_METHOD, 'coarse_fine_v1'):
|
||||
if scan_method not in (*ENDPOINT_METHODS, 'coarse_fine_v1'):
|
||||
raise ValueError(f'不支持的日志扫描方法: {scan_method}')
|
||||
document = {'schema_version': 1, 'model': profile.model, 'side': profile.side,
|
||||
'device_uid': event['uid'], 'command_unit': profile.command_unit,
|
||||
@@ -45,9 +45,9 @@ def replay(journal, output):
|
||||
for name in task.joints:
|
||||
joint = profile.by_name[name]
|
||||
try:
|
||||
if scan_method == SCAN_METHOD:
|
||||
if scan_method in ENDPOINT_METHODS:
|
||||
sweeps = {(r, d): v for (s, r, d), v in data.items() if s == 'endpoint'}
|
||||
result = analyze_endpoint_samples(joint, sweeps, settings)
|
||||
result = analyze_endpoint_samples(joint, sweeps, settings, scan_method)
|
||||
else:
|
||||
threshold = thresholds[task.name][name]
|
||||
for direction in ('up', 'down'):
|
||||
|
||||
@@ -17,6 +17,7 @@ from sensor_msgs.msg import CameraInfo, Image
|
||||
|
||||
from .adapters.fake import FakeAdapter
|
||||
from .adapters.registry import adapter_type
|
||||
from .core.diagnostics import tag_label
|
||||
from .core.engine import Engine
|
||||
from .profiles import Settings
|
||||
from .simulation import simulated_teaching
|
||||
@@ -157,18 +158,24 @@ class CalibrationRuntime(Node):
|
||||
cache[stamp] = statuses
|
||||
while len(cache) > 40:
|
||||
cache.popitem(last=False)
|
||||
self.engine.observe(view, stamp, accepted, now)
|
||||
rejected = {tag: statuses[tag]['error'] for tag in seen if statuses[tag]['error']}
|
||||
self.engine.observe(view, stamp, accepted, now, rejected=rejected)
|
||||
|
||||
def _vision_error(self, views, now):
|
||||
def _vision_error(self, requirements, now):
|
||||
if self.demo:
|
||||
return None
|
||||
for view in views:
|
||||
for view in dict.fromkeys(view for view, tag in requirements.values()):
|
||||
if view in self.camera_error:
|
||||
return f'{view}: {self.camera_error[view]}'
|
||||
if now-self.camera_received.get(view, float('-inf')) > 1:
|
||||
return f'{view}: CameraInfo 未收到或断流'
|
||||
if now-self.image_times.get(view, float('-inf')) > self.settings.freshness:
|
||||
return f'{view}: 有效时间戳的检测流未收到或断流'
|
||||
reason = self.camera_error[view]
|
||||
elif now-self.camera_received.get(view, float('-inf')) > 1:
|
||||
reason = 'CameraInfo 未收到或断流'
|
||||
elif now-self.image_times.get(view, float('-inf')) > self.settings.freshness:
|
||||
reason = '有效时间戳的检测流未收到或断流'
|
||||
else:
|
||||
continue
|
||||
affected = '、'.join(tag_label(v, tag, name) for name, (v, tag) in requirements.items()
|
||||
if v == view)
|
||||
return f'{affected}:{reason}'
|
||||
return None
|
||||
|
||||
def _device(self):
|
||||
@@ -242,8 +249,9 @@ class CalibrationRuntime(Node):
|
||||
missing = self.teaching.missing(tasks)
|
||||
if missing:
|
||||
raise ValueError('缺少示教: ' + ', '.join(missing))
|
||||
views = {self.profile.by_name[n].view for t in tasks for n in t.joints}
|
||||
error = self._vision_error(views, now)
|
||||
requirements = {name: (self.profile.by_name[name].view, self.profile.by_name[name].tag_id)
|
||||
for task in tasks for name in task.joints}
|
||||
error = self._vision_error(requirements, now)
|
||||
if error:
|
||||
raise ValueError(error)
|
||||
session = Session(self.station['output_root'], self.profile, self.device_uid,
|
||||
@@ -290,8 +298,7 @@ class CalibrationRuntime(Node):
|
||||
self.message = str(error)
|
||||
self.applied_epoch = epoch
|
||||
if self.engine.state in ('PREPARING', 'SCANNING'):
|
||||
views = {self.profile.by_name[n].view for n in self.engine.task.joints}
|
||||
error = self._vision_error(views, now)
|
||||
error = self._vision_error(self.engine.keys, now)
|
||||
if error:
|
||||
self.engine.pause(error)
|
||||
self.engine.tick(now, stamp)
|
||||
@@ -329,8 +336,11 @@ class CalibrationRuntime(Node):
|
||||
'task_index': self.engine.task_index, 'task_count': len(self.engine.tasks),
|
||||
'stage': self.engine.stage, 'direction': self.engine.direction,
|
||||
'restoring_joint': self.engine.restoring_joint,
|
||||
'scan_command': (self.engine.command[self.engine.specs[0].index]
|
||||
if self.engine.state == 'SCANNING' else None),
|
||||
'scan_segment': (self.engine.segment.label
|
||||
if self.engine.state == 'SCANNING' and self.engine.segment else ''),
|
||||
'scan_commands': ({joint.name: self.engine.command[joint.index]
|
||||
for joint in self.engine.specs}
|
||||
if self.engine.state == 'SCANNING' else {}),
|
||||
'repeat': self.engine.repeat, 'progress': self.engine.progress,
|
||||
'active': self.engine.active,
|
||||
'can_save': self.engine.can_capture_teaching(now),
|
||||
|
||||
@@ -9,6 +9,7 @@ import tempfile
|
||||
import yaml
|
||||
|
||||
from .core.endpoints import SCAN_METHOD
|
||||
from .vision.observations import SAMPLING_METHOD
|
||||
|
||||
|
||||
def safe_uid(uid):
|
||||
@@ -135,7 +136,8 @@ class Session:
|
||||
self.journal = (self.directory / 'samples.jsonl').open('a', encoding='utf-8', buffering=1)
|
||||
self.results = {j.name: {'min': None, 'max': None} for j in profile.joints}
|
||||
self.emit({'kind': 'metadata', 'profile': profile.raw, 'settings': settings,
|
||||
'teaching': teaching.data, 'motion': motion, 'uid': uid, 'scan_method': SCAN_METHOD})
|
||||
'teaching': teaching.data, 'motion': motion, 'uid': uid, 'scan_method': SCAN_METHOD,
|
||||
'sampling_method': SAMPLING_METHOD})
|
||||
self.save()
|
||||
|
||||
def emit(self, event):
|
||||
|
||||
@@ -30,6 +30,7 @@ class CalibrationWindow(QMainWindow):
|
||||
layout = QVBoxLayout(root)
|
||||
self.identity, self.status, self.message, self.teaching_status = QLabel(), QLabel(), QLabel(), QLabel()
|
||||
self.identity.setWordWrap(True)
|
||||
self.status.setWordWrap(True)
|
||||
self.message.setWordWrap(True)
|
||||
self.message.setTextInteractionFlags(Qt.TextSelectableByMouse)
|
||||
for label in (self.identity, self.status, self.teaching_status):
|
||||
@@ -51,7 +52,7 @@ class CalibrationWindow(QMainWindow):
|
||||
controls = QHBoxLayout()
|
||||
self.task = QComboBox()
|
||||
for task in self.profile.tasks:
|
||||
self.task.addItem('四指侧摆(同步)' if task.name == 'four_finger_roll' else task.name, task.name)
|
||||
self.task.addItem('四指侧摆(分阶段)' if task.name == 'four_finger_roll' else task.name, task.name)
|
||||
controls.addWidget(self.task)
|
||||
self.start_one = self.button('试标定 / 重测所选任务', lambda: runtime.submit('start', tasks=[self.task.currentData()]), controls)
|
||||
self.start_all = self.button('全手标定', lambda: runtime.submit('start'), controls)
|
||||
@@ -133,10 +134,16 @@ class CalibrationWindow(QMainWindow):
|
||||
f'{control_error or "SDK 控制连接已就绪"}')
|
||||
search_label = f'逐{self.profile.joints[0].resolution:g}寻找边界'
|
||||
restore_label = f'{snap["restoring_joint"]} 恢复基础姿态' if snap['restoring_joint'] else ''
|
||||
short_names = {'index_mcp_roll': '食指', 'middle_mcp_roll': '中指',
|
||||
'ring_mcp_roll': '无名指', 'pinky_mcp_roll': '小指'}
|
||||
commands = snap['scan_commands']
|
||||
command_text = (' / '.join(f'{short_names.get(name, name)}={value:g}' for name, value in commands.items())
|
||||
if len(commands) > 1 else str(next(iter(commands.values()), '—')))
|
||||
self.status.setText(f'{STATES.get(snap["state"], snap["state"])} | {snap["task"]} | '
|
||||
f'{ {"prepare":"准备姿态", "reference":"端点定位 / 参考采样", "search":search_label, "restore":restore_label}.get(snap["stage"], "")} '
|
||||
f'{snap["scan_segment"]} '
|
||||
f'{ {"prepare":"准备姿态", "reference":"阶段定位 / 参考采样", "search":search_label, "segment_finish":"到达阶段终点", "restore":restore_label}.get(snap["stage"], "")} '
|
||||
f'{ {"up":"低端递增", "down":"高端递减"}.get(snap["direction"], "")} '
|
||||
f'指令 {snap["scan_command"] if snap["scan_command"] is not None else "—"} '
|
||||
f'指令 {command_text} '
|
||||
f'第{snap["repeat"]+1}轮 | '
|
||||
f'任务 {min(snap["task_index"]+1,snap["task_count"])}/{snap["task_count"]}')
|
||||
self.teaching_status.setText('待示教:' + '、'.join(snap['missing']) if snap['missing'] else '所需示教已完成')
|
||||
|
||||
@@ -0,0 +1,114 @@
|
||||
"""Remember motion during a command, including motion that returns to its start."""
|
||||
from collections import deque
|
||||
|
||||
import numpy as np
|
||||
|
||||
from .observations import rms_delta
|
||||
from .reference import jitter_radius
|
||||
|
||||
|
||||
class MotionTrace:
|
||||
"""Keep the command's observations until its settled noise can be evaluated.
|
||||
|
||||
Unlike a two-frame latch, an early noisy excursion is not irreversible.
|
||||
Missing detections are retained as gaps so replay cannot join unrelated frames.
|
||||
"""
|
||||
|
||||
def __init__(self, gate_ns):
|
||||
self.gate_ns = self.last_stamp = gate_ns
|
||||
self.frames = []
|
||||
|
||||
def observe(self, stamp_ns, observation):
|
||||
if stamp_ns <= self.last_stamp:
|
||||
return
|
||||
self.last_stamp = stamp_ns
|
||||
self.frames.append({'stamp_ns': stamp_ns,
|
||||
'corners': np.asarray(observation.corners).tolist() if observation else None})
|
||||
|
||||
def snapshot(self):
|
||||
return {'gate_stamp_ns': self.gate_ns, 'frames': list(self.frames)}
|
||||
|
||||
|
||||
def transient_evidence(trace, reference, reference_radius, settled_frames, settings):
|
||||
"""Require movement beyond both endpoint and current-point jitter envelopes.
|
||||
|
||||
The settled position and a raw frame have different uncertainty. This
|
||||
check uses full, timestamped evidence and never compares motion direction.
|
||||
"""
|
||||
if not trace or not settled_frames:
|
||||
return None
|
||||
radius = jitter_radius([f['corners'] for f in settled_frames])
|
||||
threshold = settings.motion_floor_px + reference_radius + radius
|
||||
pending = deque(maxlen=settings.motion_frames)
|
||||
previous = trace['gate_stamp_ns']
|
||||
for frame in trace['frames']:
|
||||
stamp = frame['stamp_ns']
|
||||
if stamp <= previous:
|
||||
continue
|
||||
if stamp - previous > round(settings.freshness * 1e9):
|
||||
pending.clear()
|
||||
previous = stamp
|
||||
points = np.asarray(frame['corners'], dtype=float)
|
||||
if (points.shape != (4, 2) or not np.isfinite(points).all()
|
||||
or rms_delta(points, reference) <= threshold):
|
||||
pending.clear()
|
||||
continue
|
||||
pending.append(frame)
|
||||
if len(pending) == settings.motion_frames:
|
||||
return {'threshold_px': threshold, 'reference_jitter_radius_px': reference_radius,
|
||||
'point_jitter_radius_px': radius, 'frames': list(pending)}
|
||||
return None
|
||||
|
||||
|
||||
class MotionCapture:
|
||||
"""Historical v9 two-frame latch; retained for reproducing old journals."""
|
||||
def __init__(self, reference, threshold, gate_ns, count, freshness):
|
||||
self.reference, self.threshold = reference, threshold
|
||||
self.gate_ns, self.count = gate_ns, count
|
||||
self.max_gap_ns = round(freshness * 1e9)
|
||||
self.pending = deque(maxlen=count)
|
||||
self.last_stamp = gate_ns
|
||||
self.evidence = None
|
||||
|
||||
def observe(self, observation):
|
||||
"""Return True once; a return to the reference never erases evidence."""
|
||||
if self.evidence is not None:
|
||||
return False
|
||||
if observation is None:
|
||||
self.pending.clear()
|
||||
return False
|
||||
if observation.stamp_ns <= self.last_stamp:
|
||||
return False
|
||||
if observation.stamp_ns - self.last_stamp > self.max_gap_ns:
|
||||
self.pending.clear()
|
||||
self.last_stamp = observation.stamp_ns
|
||||
points = np.asarray(observation.corners, dtype=float)
|
||||
if points.shape != (4, 2) or not np.isfinite(points).all():
|
||||
self.pending.clear()
|
||||
return False
|
||||
if rms_delta(observation.corners, self.reference) <= self.threshold:
|
||||
self.pending.clear()
|
||||
return False
|
||||
self.pending.append({'stamp_ns': observation.stamp_ns,
|
||||
'corners': np.asarray(observation.corners).tolist()})
|
||||
if len(self.pending) < self.count:
|
||||
return False
|
||||
self.evidence = {'gate_stamp_ns': self.gate_ns, 'frames': list(self.pending)}
|
||||
return True
|
||||
|
||||
|
||||
def witnessed_motion(evidence, reference, threshold, count, freshness):
|
||||
"""Replay recorded frame evidence without trusting a saved motion flag."""
|
||||
if not evidence or len(evidence.get('frames', [])) < count:
|
||||
return False
|
||||
previous = evidence['gate_stamp_ns']
|
||||
for index, frame in enumerate(evidence['frames']):
|
||||
stamp, points = frame['stamp_ns'], np.asarray(frame['corners'], dtype=float)
|
||||
if stamp <= previous or (index and stamp - previous > round(freshness * 1e9)):
|
||||
return False
|
||||
if points.shape != (4, 2) or not np.isfinite(points).all():
|
||||
return False
|
||||
if rms_delta(points, reference) <= threshold:
|
||||
return False
|
||||
previous = stamp
|
||||
return True
|
||||
+104
-10
@@ -5,6 +5,9 @@ from dataclasses import dataclass
|
||||
import numpy as np
|
||||
|
||||
|
||||
SAMPLING_METHOD = 'separate_noise_drift_v2'
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class Observation:
|
||||
stamp_ns: int
|
||||
@@ -24,6 +27,59 @@ def noise_sigma(points):
|
||||
return float(np.sqrt(np.mean(np.sum(mad * mad, axis=-1))))
|
||||
|
||||
|
||||
def robust_position(points):
|
||||
"""Average the middle half of each coordinate's observations.
|
||||
|
||||
A median can switch between vibration peaks when a window gains one frame.
|
||||
A trimmed average reduces this phase/count bias while removing isolated
|
||||
corner spikes. Instantaneous motion is checked separately on raw frames.
|
||||
"""
|
||||
values = np.sort(np.asarray(points, dtype=float), axis=0)
|
||||
trim = len(values) // 4
|
||||
return np.mean(values[trim:len(values) - trim], axis=0)
|
||||
|
||||
|
||||
def temporal_motion(points, stamps, drift_sigma):
|
||||
"""Separate a robust time trend from jitter, retaining all four corners.
|
||||
|
||||
Long-baseline pairwise slopes suppress individual bad detections. Residual
|
||||
and frame-difference MAD estimate jitter without counting smooth movement
|
||||
as random noise. Check the whole window and its halves so that slow motion
|
||||
reversing in the middle cannot cancel out. The allowance is heuristic,
|
||||
not a guarantee that camera noise is statistically independent.
|
||||
"""
|
||||
times = (np.asarray(stamps, dtype=np.int64) - stamps[0]) / 1e9
|
||||
centered = times - np.mean(times)
|
||||
velocity = _median_velocity(points, times)
|
||||
residual = points - centered[:, None, None] * velocity
|
||||
difference_mad = 1.4826 * np.median(np.abs(np.diff(residual, axis=0)), axis=0) / np.sqrt(2)
|
||||
difference_noise = float(np.sqrt(np.mean(np.sum(difference_mad ** 2, axis=-1))))
|
||||
jitter = min(noise_sigma(residual), difference_noise)
|
||||
sections = [(points, times)]
|
||||
if len(points) >= 8:
|
||||
middle = len(points) // 2
|
||||
sections.extend([(points[:middle], times[:middle]), (points[middle:], times[middle:])])
|
||||
trends = []
|
||||
for section, clock in sections:
|
||||
span = clock[-1] - clock[0]
|
||||
velocity = _median_velocity(section, clock)
|
||||
trend = float(np.sqrt(np.mean(np.sum((velocity * span) ** 2, axis=-1))))
|
||||
allowance = float(drift_sigma * jitter * span / np.sqrt(np.sum((clock - np.mean(clock)) ** 2)))
|
||||
trends.append((max(0.0, trend - allowance), trend, allowance, span))
|
||||
drift, trend, allowance, span = max(trends, key=lambda value: value[0])
|
||||
return {'jitter_px': jitter, 'trend_px': trend, 'trend_allowance_px': allowance,
|
||||
'trend_span_seconds': float(span), 'drift_px': drift}
|
||||
|
||||
|
||||
def _median_velocity(points, times):
|
||||
span = times[-1] - times[0]
|
||||
left, right = np.triu_indices(len(times), 1)
|
||||
dt = times[right] - times[left]
|
||||
keep = dt >= span / 2
|
||||
slopes = (points[right[keep]] - points[left[keep]]) / dt[keep, None, None]
|
||||
return np.median(slopes, axis=0)
|
||||
|
||||
|
||||
def quality_error(corners, width, height, hamming, margin, settings):
|
||||
p = np.asarray(corners, dtype=float)
|
||||
if p.shape != (4, 2) or not np.isfinite(p).all() or not np.isfinite(margin):
|
||||
@@ -44,30 +100,68 @@ def quality_error(corners, width, height, hamming, margin, settings):
|
||||
|
||||
|
||||
class StableWindow:
|
||||
def __init__(self, count):
|
||||
self.values = deque(maxlen=count)
|
||||
def __init__(self, count, minimum_seconds=0.0):
|
||||
self.values = deque()
|
||||
self.last_stamp = -1
|
||||
self.count = count
|
||||
self.duration_ns = round(minimum_seconds * 1e9)
|
||||
self.last_quality = None
|
||||
|
||||
def clear(self):
|
||||
self.values.clear()
|
||||
self.last_quality = None
|
||||
|
||||
def snapshot(self):
|
||||
"""Small timestamped corner arrays for diagnosing rejected windows offline."""
|
||||
return [{'stamp_ns': value.stamp_ns, 'corners': np.asarray(value.corners).tolist()}
|
||||
for value in self.values]
|
||||
|
||||
def add(self, observation):
|
||||
if observation.stamp_ns <= self.last_stamp:
|
||||
return False
|
||||
self.last_stamp = observation.stamp_ns
|
||||
self.values.append(observation)
|
||||
# Keep the shortest suffix satisfying both frame count and elapsed time.
|
||||
# This works at different camera rates without assuming a fixed FPS.
|
||||
while (len(self.values) > self.count and
|
||||
observation.stamp_ns - self.values[1].stamp_ns >= self.duration_ns):
|
||||
self.values.popleft()
|
||||
return True
|
||||
|
||||
def summary(self, tolerance):
|
||||
if len(self.values) < self.count:
|
||||
def summary(self, noise_limit, drift_limit, drift_sigma=3):
|
||||
if (len(self.values) < self.count or
|
||||
self.values[-1].stamp_ns - self.values[0].stamp_ns < self.duration_ns):
|
||||
return None
|
||||
points = np.asarray([v.corners for v in self.values])
|
||||
split = len(points)//2
|
||||
if rms_delta(np.median(points[:split], axis=0), np.median(points[split:], axis=0)) > tolerance:
|
||||
motion = temporal_motion(points, [v.stamp_ns for v in self.values], drift_sigma)
|
||||
# Preserve raw spread for diagnostics and historical comparisons. The
|
||||
# current endpoint detector evaluates position and raw-frame noise separately.
|
||||
self.last_quality = {'noise': noise_sigma(points), **motion,
|
||||
'noise_limit_px': noise_limit, 'drift_limit_px': drift_limit,
|
||||
'drift_sigma': drift_sigma, 'sampling_method': SAMPLING_METHOD,
|
||||
'first_stamp_ns': self.values[0].stamp_ns,
|
||||
'last_stamp_ns': self.values[-1].stamp_ns, 'frames': len(points)}
|
||||
if motion['drift_px'] > drift_limit or motion['jitter_px'] > noise_limit:
|
||||
return None
|
||||
if noise_sigma(points) > tolerance:
|
||||
return {'corners': robust_position(points).tolist(), **self.last_quality}
|
||||
|
||||
def problem(self, noise_limit, drift_limit, drift_sigma=3):
|
||||
"""Explain the current window without using a previous window's metrics."""
|
||||
if self.summary(noise_limit, drift_limit, drift_sigma) is not None:
|
||||
return None
|
||||
return {'corners': np.median(points, axis=0).tolist(), 'noise': noise_sigma(points),
|
||||
'first_stamp_ns': self.values[0].stamp_ns,
|
||||
'last_stamp_ns': self.values[-1].stamp_ns, 'frames': len(points)}
|
||||
frames = len(self.values)
|
||||
span_ns = self.values[-1].stamp_ns - self.values[0].stamp_ns if frames else 0
|
||||
if frames < self.count:
|
||||
return {'reason': f'有效新图像不足:{frames}/{self.count} 帧', 'frames': frames}
|
||||
if span_ns < self.duration_ns:
|
||||
span = span_ns / 1e9
|
||||
return {'reason': f'稳定观察时长不足:{span:.3f}/{self.duration_ns / 1e9:.3f} 秒',
|
||||
'frames': frames, 'span_seconds': span}
|
||||
quality = self.last_quality
|
||||
details = []
|
||||
if quality['jitter_px'] > noise_limit:
|
||||
details.append(f'随机抖动 {quality["jitter_px"]:.3f} > {noise_limit:.3f} 像素')
|
||||
if quality['drift_px'] > drift_limit:
|
||||
details.append(f'持续漂移 {quality["drift_px"]:.3f} > {drift_limit:.3f} 像素'
|
||||
f'(趋势 {quality["trend_px"]:.3f},抖动裕量 {quality["trend_allowance_px"]:.3f})')
|
||||
return {'reason': ';'.join(details), **quality}
|
||||
|
||||
@@ -0,0 +1,44 @@
|
||||
"""Establish a quiet endpoint before distinguishing command motion from jitter."""
|
||||
import itertools
|
||||
|
||||
import numpy as np
|
||||
|
||||
from .observations import StableWindow, rms_delta, robust_position
|
||||
|
||||
|
||||
def block_shift(points):
|
||||
"""Compare disjoint early/middle/late positions, without cancelling drift."""
|
||||
positions = [robust_position(block) for block in np.array_split(points, 3)]
|
||||
return max(rms_delta(a, b) for a, b in itertools.combinations(positions, 2))
|
||||
|
||||
|
||||
def jitter_radius(points):
|
||||
"""Observed frame excursions; no independent-noise assumption."""
|
||||
values = np.asarray(points, dtype=float)
|
||||
radii = np.sqrt(np.mean(np.sum((values - robust_position(values)) ** 2, axis=-1), axis=-1))
|
||||
return float(np.quantile(radii, .99))
|
||||
|
||||
|
||||
class ReferenceWindow(StableWindow):
|
||||
def __init__(self, settings):
|
||||
super().__init__(3 * settings.stable_frames, settings.reference_seconds)
|
||||
# Reserve half of the movement threshold for reference uncertainty.
|
||||
# This concerns block positions, not per-frame jitter or drift velocity.
|
||||
self.shift_limit = settings.motion_floor_px / 2
|
||||
|
||||
def summary(self, noise_limit, drift_limit, drift_sigma=3):
|
||||
summary = super().summary(noise_limit, drift_limit, drift_sigma)
|
||||
if summary is None:
|
||||
return None
|
||||
shift = block_shift(np.asarray([v.corners for v in self.values]))
|
||||
self.last_quality.update(reference_shift_px=shift, reference_shift_limit_px=self.shift_limit)
|
||||
if shift > self.shift_limit:
|
||||
return None
|
||||
return {**summary, **self.last_quality}
|
||||
|
||||
def problem(self, noise_limit, drift_limit, drift_sigma=3):
|
||||
problem = super().problem(noise_limit, drift_limit, drift_sigma)
|
||||
if problem and self.last_quality and self.last_quality.get('reference_shift_px', 0) > self.shift_limit:
|
||||
problem['reason'] = (f'端点参考尚未稳定:前、中、后三段位置差 '
|
||||
f'{self.last_quality["reference_shift_px"]:.3f} > {self.shift_limit:.3f} 像素')
|
||||
return problem
|
||||
+892
@@ -0,0 +1,892 @@
|
||||
[
|
||||
{
|
||||
"command": 0,
|
||||
"observations": {
|
||||
"index_mcp_roll": {
|
||||
"corners": [
|
||||
[
|
||||
942.1658325195312,
|
||||
614.3393249511716
|
||||
],
|
||||
[
|
||||
993.1404418945312,
|
||||
636.0686950683594
|
||||
],
|
||||
[
|
||||
1015.5987548828125,
|
||||
582.817596435547
|
||||
],
|
||||
[
|
||||
964.5825805664062,
|
||||
561.2039794921875
|
||||
]
|
||||
],
|
||||
"noise": 0.04076594501430361
|
||||
},
|
||||
"middle_mcp_roll": {
|
||||
"corners": [
|
||||
[
|
||||
838.1037902832031,
|
||||
602.2392883300781
|
||||
],
|
||||
[
|
||||
892.2730407714844,
|
||||
614.2301635742188
|
||||
],
|
||||
[
|
||||
904.810546875,
|
||||
558.0731811523438
|
||||
],
|
||||
[
|
||||
850.36962890625,
|
||||
546.08203125
|
||||
]
|
||||
],
|
||||
"noise": 0.04236790830887581
|
||||
},
|
||||
"ring_mcp_roll": {
|
||||
"corners": [
|
||||
[
|
||||
739.3731079101562,
|
||||
609.5610961914062
|
||||
],
|
||||
[
|
||||
793.4818420410156,
|
||||
622.5941467285156
|
||||
],
|
||||
[
|
||||
806.9288330078125,
|
||||
567.0270690917969
|
||||
],
|
||||
[
|
||||
753.462463378906,
|
||||
554.5124206542969
|
||||
]
|
||||
],
|
||||
"noise": 0.057182449144013825
|
||||
},
|
||||
"pinky_mcp_roll": {
|
||||
"corners": [
|
||||
[
|
||||
630.0345153808594,
|
||||
644.7550048828125
|
||||
],
|
||||
[
|
||||
685.34765625,
|
||||
649.0560607910156
|
||||
],
|
||||
[
|
||||
690.0279541015625,
|
||||
592.0843811035156
|
||||
],
|
||||
[
|
||||
635.2400512695314,
|
||||
588.0740661621094
|
||||
]
|
||||
],
|
||||
"noise": 0.03157646505636869
|
||||
}
|
||||
}
|
||||
},
|
||||
{
|
||||
"command": 1,
|
||||
"observations": {
|
||||
"index_mcp_roll": {
|
||||
"corners": [
|
||||
[
|
||||
942.9926147460938,
|
||||
614.5863037109374
|
||||
],
|
||||
[
|
||||
993.8966979980468,
|
||||
636.4872131347656
|
||||
],
|
||||
[
|
||||
1016.5432739257812,
|
||||
583.3279113769531
|
||||
],
|
||||
[
|
||||
965.5861511230469,
|
||||
561.5286865234374
|
||||
]
|
||||
],
|
||||
"noise": 0.051202073623555784
|
||||
},
|
||||
"middle_mcp_roll": {
|
||||
"corners": [
|
||||
[
|
||||
837.1094055175781,
|
||||
602.0164489746094
|
||||
],
|
||||
[
|
||||
891.3479309082031,
|
||||
613.7966918945312
|
||||
],
|
||||
[
|
||||
903.6159362792969,
|
||||
557.5814819335938
|
||||
],
|
||||
[
|
||||
849.1360778808594,
|
||||
545.8171997070312
|
||||
]
|
||||
],
|
||||
"noise": 0.03677257418691774
|
||||
},
|
||||
"ring_mcp_roll": {
|
||||
"corners": [
|
||||
[
|
||||
740.4667053222656,
|
||||
609.6826477050781
|
||||
],
|
||||
[
|
||||
794.5198974609375,
|
||||
622.9682312011719
|
||||
],
|
||||
[
|
||||
808.2039794921875,
|
||||
567.4523620605468
|
||||
],
|
||||
[
|
||||
754.7801513671874,
|
||||
554.6915893554688
|
||||
]
|
||||
],
|
||||
"noise": 0.020996480303045212
|
||||
},
|
||||
"pinky_mcp_roll": {
|
||||
"corners": [
|
||||
[
|
||||
630.036895751953,
|
||||
644.7475280761719
|
||||
],
|
||||
[
|
||||
685.3456420898438,
|
||||
649.0472717285156
|
||||
],
|
||||
[
|
||||
690.0276794433594,
|
||||
592.0641174316406
|
||||
],
|
||||
[
|
||||
635.2419738769529,
|
||||
588.0673828125
|
||||
]
|
||||
],
|
||||
"noise": 0.025270212770399245
|
||||
}
|
||||
}
|
||||
},
|
||||
{
|
||||
"command": 2,
|
||||
"observations": {
|
||||
"index_mcp_roll": {
|
||||
"corners": [
|
||||
[
|
||||
943.0314331054688,
|
||||
614.6201171875
|
||||
],
|
||||
[
|
||||
993.9412536621094,
|
||||
636.52099609375
|
||||
],
|
||||
[
|
||||
1016.6061096191406,
|
||||
583.3601379394531
|
||||
],
|
||||
[
|
||||
965.6469116210938,
|
||||
561.562744140625
|
||||
]
|
||||
],
|
||||
"noise": 0.022996185794191343
|
||||
},
|
||||
"middle_mcp_roll": {
|
||||
"corners": [
|
||||
[
|
||||
836.9605102539062,
|
||||
601.9946289062499
|
||||
],
|
||||
[
|
||||
891.2118530273438,
|
||||
613.739990234375
|
||||
],
|
||||
[
|
||||
903.4494323730469,
|
||||
557.51416015625
|
||||
],
|
||||
[
|
||||
848.96044921875,
|
||||
545.7806091308595
|
||||
]
|
||||
],
|
||||
"noise": 0.022502370051396207
|
||||
},
|
||||
"ring_mcp_roll": {
|
||||
"corners": [
|
||||
[
|
||||
740.4991760253906,
|
||||
609.6954040527346
|
||||
],
|
||||
[
|
||||
794.5404663085938,
|
||||
622.988037109375
|
||||
],
|
||||
[
|
||||
808.2434692382811,
|
||||
567.4777526855466
|
||||
],
|
||||
[
|
||||
754.8114013671873,
|
||||
554.7090148925784
|
||||
]
|
||||
],
|
||||
"noise": 0.026357821062818296
|
||||
},
|
||||
"pinky_mcp_roll": {
|
||||
"corners": [
|
||||
[
|
||||
630.0309448242188,
|
||||
644.7559814453125
|
||||
],
|
||||
[
|
||||
685.337890625,
|
||||
649.0545654296875
|
||||
],
|
||||
[
|
||||
690.0197448730469,
|
||||
592.0746459960935
|
||||
],
|
||||
[
|
||||
635.2414855957032,
|
||||
588.0703430175781
|
||||
]
|
||||
],
|
||||
"noise": 0.02037795019026747
|
||||
}
|
||||
}
|
||||
},
|
||||
{
|
||||
"command": 3,
|
||||
"observations": {
|
||||
"index_mcp_roll": {
|
||||
"corners": [
|
||||
[
|
||||
943.0643615722656,
|
||||
614.6192321777343
|
||||
],
|
||||
[
|
||||
993.971649169922,
|
||||
636.5349426269531
|
||||
],
|
||||
[
|
||||
1016.6253051757812,
|
||||
583.3798217773436
|
||||
],
|
||||
[
|
||||
965.6820068359373,
|
||||
561.5702514648438
|
||||
]
|
||||
],
|
||||
"noise": 0.018839247011240085
|
||||
},
|
||||
"middle_mcp_roll": {
|
||||
"corners": [
|
||||
[
|
||||
836.9627075195312,
|
||||
601.9944458007812
|
||||
],
|
||||
[
|
||||
891.2069396972656,
|
||||
613.7290649414062
|
||||
],
|
||||
[
|
||||
903.4407653808594,
|
||||
557.516052246094
|
||||
],
|
||||
[
|
||||
848.9676513671874,
|
||||
545.7850952148438
|
||||
]
|
||||
],
|
||||
"noise": 0.018843810334788297
|
||||
},
|
||||
"ring_mcp_roll": {
|
||||
"corners": [
|
||||
[
|
||||
740.5218200683594,
|
||||
609.6960754394531
|
||||
],
|
||||
[
|
||||
794.57666015625,
|
||||
622.9872436523438
|
||||
],
|
||||
[
|
||||
808.2703247070312,
|
||||
567.4963378906248
|
||||
],
|
||||
[
|
||||
754.8447265625,
|
||||
554.7146911621094
|
||||
]
|
||||
],
|
||||
"noise": 0.016067742020406905
|
||||
},
|
||||
"pinky_mcp_roll": {
|
||||
"corners": [
|
||||
[
|
||||
630.0536499023438,
|
||||
644.7599182128906
|
||||
],
|
||||
[
|
||||
685.3619995117188,
|
||||
649.0549621582031
|
||||
],
|
||||
[
|
||||
690.0481262207031,
|
||||
592.08154296875
|
||||
],
|
||||
[
|
||||
635.2618103027344,
|
||||
588.0802612304688
|
||||
]
|
||||
],
|
||||
"noise": 0.01684557475207796
|
||||
}
|
||||
}
|
||||
},
|
||||
{
|
||||
"command": 4,
|
||||
"observations": {
|
||||
"index_mcp_roll": {
|
||||
"corners": [
|
||||
[
|
||||
943.0610961914062,
|
||||
614.622772216797
|
||||
],
|
||||
[
|
||||
993.9665222167969,
|
||||
636.5389404296875
|
||||
],
|
||||
[
|
||||
1016.6296997070312,
|
||||
583.3784484863281
|
||||
],
|
||||
[
|
||||
965.6797485351562,
|
||||
561.5781555175781
|
||||
]
|
||||
],
|
||||
"noise": 0.018882960610405963
|
||||
},
|
||||
"middle_mcp_roll": {
|
||||
"corners": [
|
||||
[
|
||||
834.5687255859374,
|
||||
601.4897766113282
|
||||
],
|
||||
[
|
||||
888.9640502929689,
|
||||
612.6998291015625
|
||||
],
|
||||
[
|
||||
900.6335144042969,
|
||||
556.35888671875
|
||||
],
|
||||
[
|
||||
846.0301818847656,
|
||||
545.1744689941407
|
||||
]
|
||||
],
|
||||
"noise": 0.09660129145540024
|
||||
},
|
||||
"ring_mcp_roll": {
|
||||
"corners": [
|
||||
[
|
||||
740.5050659179688,
|
||||
609.695068359375
|
||||
],
|
||||
[
|
||||
794.5516662597655,
|
||||
622.9812927246094
|
||||
],
|
||||
[
|
||||
808.2597961425781,
|
||||
567.4856872558594
|
||||
],
|
||||
[
|
||||
754.8283081054688,
|
||||
554.7145385742186
|
||||
]
|
||||
],
|
||||
"noise": 0.019168407051991073
|
||||
},
|
||||
"pinky_mcp_roll": {
|
||||
"corners": [
|
||||
[
|
||||
630.0425415039062,
|
||||
644.7564697265623
|
||||
],
|
||||
[
|
||||
685.3498229980469,
|
||||
649.0524597167969
|
||||
],
|
||||
[
|
||||
690.0369567871094,
|
||||
592.0793151855469
|
||||
],
|
||||
[
|
||||
635.2521362304688,
|
||||
588.0719909667969
|
||||
]
|
||||
],
|
||||
"noise": 0.02016451292903033
|
||||
}
|
||||
}
|
||||
},
|
||||
{
|
||||
"command": 5,
|
||||
"observations": {
|
||||
"index_mcp_roll": {
|
||||
"corners": [
|
||||
[
|
||||
943.07763671875,
|
||||
614.6163635253906
|
||||
],
|
||||
[
|
||||
993.991943359375,
|
||||
636.5233764648438
|
||||
],
|
||||
[
|
||||
1016.6383361816406,
|
||||
583.3706665039061
|
||||
],
|
||||
[
|
||||
965.7053527832031,
|
||||
561.5636596679688
|
||||
]
|
||||
],
|
||||
"noise": 0.021249634596398045
|
||||
},
|
||||
"middle_mcp_roll": {
|
||||
"corners": [
|
||||
[
|
||||
834.40673828125,
|
||||
601.4405517578125
|
||||
],
|
||||
[
|
||||
888.8147888183595,
|
||||
612.6332397460938
|
||||
],
|
||||
[
|
||||
900.4404602050781,
|
||||
556.2811279296875
|
||||
],
|
||||
[
|
||||
845.8300476074219,
|
||||
545.1272583007812
|
||||
]
|
||||
],
|
||||
"noise": 0.11601886890355814
|
||||
},
|
||||
"ring_mcp_roll": {
|
||||
"corners": [
|
||||
[
|
||||
740.528564453125,
|
||||
609.6726074218751
|
||||
],
|
||||
[
|
||||
794.5759887695312,
|
||||
622.9730529785156
|
||||
],
|
||||
[
|
||||
808.2815246582032,
|
||||
567.47412109375
|
||||
],
|
||||
[
|
||||
754.8548583984377,
|
||||
554.7016296386719
|
||||
]
|
||||
],
|
||||
"noise": 0.025602718484113215
|
||||
},
|
||||
"pinky_mcp_roll": {
|
||||
"corners": [
|
||||
[
|
||||
630.0650024414062,
|
||||
644.7447509765623
|
||||
],
|
||||
[
|
||||
685.3726196289062,
|
||||
649.0375671386719
|
||||
],
|
||||
[
|
||||
690.0503234863281,
|
||||
592.0626831054688
|
||||
],
|
||||
[
|
||||
635.2684020996095,
|
||||
588.0572509765625
|
||||
]
|
||||
],
|
||||
"noise": 0.02628157734762308
|
||||
}
|
||||
}
|
||||
},
|
||||
{
|
||||
"command": 6,
|
||||
"observations": {
|
||||
"index_mcp_roll": {
|
||||
"corners": [
|
||||
[
|
||||
943.0676574707031,
|
||||
614.6292114257812
|
||||
],
|
||||
[
|
||||
993.9799499511719,
|
||||
636.5503540039062
|
||||
],
|
||||
[
|
||||
1016.6375427246094,
|
||||
583.3923034667969
|
||||
],
|
||||
[
|
||||
965.6950683593749,
|
||||
561.568603515625
|
||||
]
|
||||
],
|
||||
"noise": 0.026472016560625748
|
||||
},
|
||||
"middle_mcp_roll": {
|
||||
"corners": [
|
||||
[
|
||||
835.607421875,
|
||||
601.6996459960938
|
||||
],
|
||||
[
|
||||
889.9469299316407,
|
||||
613.1572570800781
|
||||
],
|
||||
[
|
||||
901.8699645996094,
|
||||
556.8709716796875
|
||||
],
|
||||
[
|
||||
847.313659667969,
|
||||
545.4301452636719
|
||||
]
|
||||
],
|
||||
"noise": 0.02651126262549541
|
||||
},
|
||||
"ring_mcp_roll": {
|
||||
"corners": [
|
||||
[
|
||||
738.5291442871093,
|
||||
609.4590148925781
|
||||
],
|
||||
[
|
||||
792.6817626953124,
|
||||
622.3050537109375
|
||||
],
|
||||
[
|
||||
805.9322509765625,
|
||||
566.692138671875
|
||||
],
|
||||
[
|
||||
752.4276123046875,
|
||||
554.3790588378905
|
||||
]
|
||||
],
|
||||
"noise": 0.03352888108341187
|
||||
},
|
||||
"pinky_mcp_roll": {
|
||||
"corners": [
|
||||
[
|
||||
630.0492858886719,
|
||||
644.7572021484375
|
||||
],
|
||||
[
|
||||
685.35595703125,
|
||||
649.0496215820312
|
||||
],
|
||||
[
|
||||
690.0468139648436,
|
||||
592.07568359375
|
||||
],
|
||||
[
|
||||
635.2585144042968,
|
||||
588.0628051757812
|
||||
]
|
||||
],
|
||||
"noise": 0.027536105360250716
|
||||
}
|
||||
}
|
||||
},
|
||||
{
|
||||
"command": 7,
|
||||
"observations": {
|
||||
"index_mcp_roll": {
|
||||
"corners": [
|
||||
[
|
||||
941.0931701660156,
|
||||
614.0591430664062
|
||||
],
|
||||
[
|
||||
992.1590270996094,
|
||||
635.5539245605469
|
||||
],
|
||||
[
|
||||
1014.3836975097656,
|
||||
582.2167663574218
|
||||
],
|
||||
[
|
||||
963.2599792480468,
|
||||
560.8146667480469
|
||||
]
|
||||
],
|
||||
"noise": 0.026702449953993755
|
||||
},
|
||||
"middle_mcp_roll": {
|
||||
"corners": [
|
||||
[
|
||||
834.7217712402344,
|
||||
601.5038146972656
|
||||
],
|
||||
[
|
||||
889.11083984375,
|
||||
612.7603454589844
|
||||
],
|
||||
[
|
||||
900.82275390625,
|
||||
556.4249572753905
|
||||
],
|
||||
[
|
||||
846.2205505371094,
|
||||
545.1946105957031
|
||||
]
|
||||
],
|
||||
"noise": 0.03570311290381906
|
||||
},
|
||||
"ring_mcp_roll": {
|
||||
"corners": [
|
||||
[
|
||||
738.4523315429688,
|
||||
609.4460144042971
|
||||
],
|
||||
[
|
||||
792.616973876953,
|
||||
622.2828063964844
|
||||
],
|
||||
[
|
||||
805.8515319824219,
|
||||
566.6593322753906
|
||||
],
|
||||
[
|
||||
752.3434143066406,
|
||||
554.3677673339846
|
||||
]
|
||||
],
|
||||
"noise": 0.025576588861750085
|
||||
},
|
||||
"pinky_mcp_roll": {
|
||||
"corners": [
|
||||
[
|
||||
629.1194152832031,
|
||||
644.8320922851562
|
||||
],
|
||||
[
|
||||
684.4285278320312,
|
||||
648.9013671875
|
||||
],
|
||||
[
|
||||
688.9304809570311,
|
||||
591.9113159179688
|
||||
],
|
||||
[
|
||||
634.1359558105469,
|
||||
588.1372375488281
|
||||
]
|
||||
],
|
||||
"noise": 0.19933226879479535
|
||||
}
|
||||
}
|
||||
},
|
||||
{
|
||||
"command": 8,
|
||||
"observations": {
|
||||
"index_mcp_roll": {
|
||||
"corners": [
|
||||
[
|
||||
941.041015625,
|
||||
614.0475769042969
|
||||
],
|
||||
[
|
||||
992.1016540527343,
|
||||
635.54638671875
|
||||
],
|
||||
[
|
||||
1014.3245544433594,
|
||||
582.2024230957032
|
||||
],
|
||||
[
|
||||
963.2027282714844,
|
||||
560.8007202148438
|
||||
]
|
||||
],
|
||||
"noise": 0.02587441347847323
|
||||
},
|
||||
"middle_mcp_roll": {
|
||||
"corners": [
|
||||
[
|
||||
834.3887023925781,
|
||||
601.4427795410154
|
||||
],
|
||||
[
|
||||
888.7993469238284,
|
||||
612.6258850097656
|
||||
],
|
||||
[
|
||||
900.4302673339844,
|
||||
556.2850036621094
|
||||
],
|
||||
[
|
||||
845.80908203125,
|
||||
545.1360473632812
|
||||
]
|
||||
],
|
||||
"noise": 0.030072401820227287
|
||||
},
|
||||
"ring_mcp_roll": {
|
||||
"corners": [
|
||||
[
|
||||
737.114990234375,
|
||||
609.3218994140623
|
||||
],
|
||||
[
|
||||
791.3253173828126,
|
||||
621.8379211425781
|
||||
],
|
||||
[
|
||||
804.2518005371094,
|
||||
566.1667175292969
|
||||
],
|
||||
[
|
||||
750.6727905273438,
|
||||
554.1644897460938
|
||||
]
|
||||
],
|
||||
"noise": 0.0739416356404054
|
||||
},
|
||||
"pinky_mcp_roll": {
|
||||
"corners": [
|
||||
[
|
||||
626.8981323242189,
|
||||
644.9990234375001
|
||||
],
|
||||
[
|
||||
682.238037109375,
|
||||
648.5708618164062
|
||||
],
|
||||
[
|
||||
686.2099304199219,
|
||||
591.5399169921874
|
||||
],
|
||||
[
|
||||
631.3916625976562,
|
||||
588.2774963378906
|
||||
]
|
||||
],
|
||||
"noise": 0.02295893372928721
|
||||
}
|
||||
}
|
||||
},
|
||||
{
|
||||
"command": 9,
|
||||
"observations": {
|
||||
"index_mcp_roll": {
|
||||
"corners": [
|
||||
[
|
||||
941.0336303710939,
|
||||
614.0338134765625
|
||||
],
|
||||
[
|
||||
992.1029357910156,
|
||||
635.5238647460938
|
||||
],
|
||||
[
|
||||
1014.31298828125,
|
||||
582.1758422851562
|
||||
],
|
||||
[
|
||||
963.1897583007812,
|
||||
560.7911682128906
|
||||
]
|
||||
],
|
||||
"noise": 0.010863787193749166
|
||||
},
|
||||
"middle_mcp_roll": {
|
||||
"corners": [
|
||||
[
|
||||
833.375,
|
||||
601.2227783203123
|
||||
],
|
||||
[
|
||||
887.8031311035156,
|
||||
612.1820983886719
|
||||
],
|
||||
[
|
||||
899.2230529785156,
|
||||
555.7844238281251
|
||||
],
|
||||
[
|
||||
844.5379638671874,
|
||||
544.8694763183593
|
||||
]
|
||||
],
|
||||
"noise": 0.011858385398811481
|
||||
},
|
||||
"ring_mcp_roll": {
|
||||
"corners": [
|
||||
[
|
||||
735.5470581054688,
|
||||
609.1393737792971
|
||||
],
|
||||
[
|
||||
789.824462890625,
|
||||
621.3017883300781
|
||||
],
|
||||
[
|
||||
802.4012451171875,
|
||||
565.5362243652344
|
||||
],
|
||||
[
|
||||
748.7284240722656,
|
||||
553.9152526855469
|
||||
]
|
||||
],
|
||||
"noise": 0.011113586153275012
|
||||
},
|
||||
"pinky_mcp_roll": {
|
||||
"corners": [
|
||||
[
|
||||
626.8903198242188,
|
||||
644.9892883300781
|
||||
],
|
||||
[
|
||||
682.2307128906251,
|
||||
648.5549926757812
|
||||
],
|
||||
[
|
||||
686.1928100585938,
|
||||
591.5229187011719
|
||||
],
|
||||
[
|
||||
631.381652832031,
|
||||
588.2687377929688
|
||||
]
|
||||
],
|
||||
"noise": 0.009929346848267706
|
||||
}
|
||||
}
|
||||
}
|
||||
]
|
||||
+524
@@ -0,0 +1,524 @@
|
||||
{
|
||||
"source": "20260917_141542_806957, front ID3, deferred_lower",
|
||||
"note": "Summaries of recorded images; command 5 changes little, command 6 has the first clear displacement.",
|
||||
"samples": [
|
||||
{
|
||||
"command": 0,
|
||||
"observation": {
|
||||
"corners": [
|
||||
[
|
||||
725.3046264648438,
|
||||
609.7582092285157
|
||||
],
|
||||
[
|
||||
775.8345336914061,
|
||||
633.587646484375
|
||||
],
|
||||
[
|
||||
800.3261108398438,
|
||||
580.3838806152341
|
||||
],
|
||||
[
|
||||
748.8726196289062,
|
||||
557.1367492675781
|
||||
]
|
||||
],
|
||||
"noise": 0.1633641227151424,
|
||||
"jitter_px": 0.1038583744127908,
|
||||
"trend_px": 0.1440312611475312,
|
||||
"trend_allowance_px": 0.28363668643248435,
|
||||
"trend_span_seconds": 0.533486258,
|
||||
"drift_px": 0.0,
|
||||
"noise_limit_px": 0.5,
|
||||
"drift_limit_px": 0.1,
|
||||
"drift_sigma": 3,
|
||||
"sampling_method": "separate_noise_drift_v1",
|
||||
"first_stamp_ns": 1789625819289343436,
|
||||
"last_stamp_ns": 1789625819822829694,
|
||||
"frames": 14
|
||||
},
|
||||
"feedback": 0.0
|
||||
},
|
||||
{
|
||||
"command": 1,
|
||||
"observation": {
|
||||
"corners": [
|
||||
[
|
||||
725.6734619140625,
|
||||
609.832275390625
|
||||
],
|
||||
[
|
||||
776.0892944335938,
|
||||
633.8901977539062
|
||||
],
|
||||
[
|
||||
800.7736206054688,
|
||||
580.7450866699219
|
||||
],
|
||||
[
|
||||
749.427947998047,
|
||||
557.2926025390625
|
||||
]
|
||||
],
|
||||
"noise": 0.19133932575164356,
|
||||
"jitter_px": 0.13234278885560932,
|
||||
"trend_px": 0.379247359736653,
|
||||
"trend_allowance_px": 0.33650056286964497,
|
||||
"trend_span_seconds": 0.50015173,
|
||||
"drift_px": 0.04274679686700805,
|
||||
"noise_limit_px": 0.5,
|
||||
"drift_limit_px": 0.1,
|
||||
"drift_sigma": 3,
|
||||
"sampling_method": "separate_noise_drift_v1",
|
||||
"first_stamp_ns": 1789625820556240175,
|
||||
"last_stamp_ns": 1789625821056391905,
|
||||
"frames": 14
|
||||
},
|
||||
"feedback": 0.0
|
||||
},
|
||||
{
|
||||
"command": 2,
|
||||
"observation": {
|
||||
"corners": [
|
||||
[
|
||||
725.8374328613281,
|
||||
609.8664245605466
|
||||
],
|
||||
[
|
||||
776.216552734375,
|
||||
633.9867858886719
|
||||
],
|
||||
[
|
||||
800.9550476074219,
|
||||
580.8789062500001
|
||||
],
|
||||
[
|
||||
749.6381835937499,
|
||||
557.3692932128906
|
||||
]
|
||||
],
|
||||
"noise": 0.13405867096158716,
|
||||
"jitter_px": 0.13066063409522238,
|
||||
"trend_px": 0.1523370779922021,
|
||||
"trend_allowance_px": 0.3397731139665314,
|
||||
"trend_span_seconds": 0.500027841,
|
||||
"drift_px": 0.0,
|
||||
"noise_limit_px": 0.5,
|
||||
"drift_limit_px": 0.1,
|
||||
"drift_sigma": 3,
|
||||
"sampling_method": "separate_noise_drift_v1",
|
||||
"first_stamp_ns": 1789625821689951644,
|
||||
"last_stamp_ns": 1789625822189979485,
|
||||
"frames": 14
|
||||
},
|
||||
"feedback": 0.0
|
||||
},
|
||||
{
|
||||
"command": 3,
|
||||
"observation": {
|
||||
"corners": [
|
||||
[
|
||||
725.9013366699218,
|
||||
609.8230590820312
|
||||
],
|
||||
[
|
||||
776.2781982421875,
|
||||
633.9623718261719
|
||||
],
|
||||
[
|
||||
801.0191955566406,
|
||||
580.8565979003906
|
||||
],
|
||||
[
|
||||
749.7086181640625,
|
||||
557.3333740234375
|
||||
]
|
||||
],
|
||||
"noise": 0.08407833629282355,
|
||||
"jitter_px": 0.06849176569048543,
|
||||
"trend_px": 0.04421722299693615,
|
||||
"trend_allowance_px": 0.1794691669794271,
|
||||
"trend_span_seconds": 0.500133075,
|
||||
"drift_px": 0.0,
|
||||
"noise_limit_px": 0.5,
|
||||
"drift_limit_px": 0.1,
|
||||
"drift_sigma": 3,
|
||||
"sampling_method": "separate_noise_drift_v1",
|
||||
"first_stamp_ns": 1789625822823475193,
|
||||
"last_stamp_ns": 1789625823323608268,
|
||||
"frames": 14
|
||||
},
|
||||
"feedback": 0.0
|
||||
},
|
||||
{
|
||||
"command": 4,
|
||||
"observation": {
|
||||
"corners": [
|
||||
[
|
||||
725.9137573242188,
|
||||
609.8248901367185
|
||||
],
|
||||
[
|
||||
776.2877807617188,
|
||||
633.9755249023439
|
||||
],
|
||||
[
|
||||
801.0433959960938,
|
||||
580.8682250976564
|
||||
],
|
||||
[
|
||||
749.7379150390625,
|
||||
557.3363647460936
|
||||
]
|
||||
],
|
||||
"noise": 0.07165695443486175,
|
||||
"jitter_px": 0.07180243264250147,
|
||||
"trend_px": 0.05872617135219982,
|
||||
"trend_allowance_px": 0.19075743801209966,
|
||||
"trend_span_seconds": 0.500048832,
|
||||
"drift_px": 0.0,
|
||||
"noise_limit_px": 0.5,
|
||||
"drift_limit_px": 0.1,
|
||||
"drift_sigma": 3,
|
||||
"sampling_method": "separate_noise_drift_v1",
|
||||
"first_stamp_ns": 1789625823957170707,
|
||||
"last_stamp_ns": 1789625824457219539,
|
||||
"frames": 13
|
||||
},
|
||||
"feedback": 0.0
|
||||
},
|
||||
{
|
||||
"command": 5,
|
||||
"observation": {
|
||||
"corners": [
|
||||
[
|
||||
725.9176330566406,
|
||||
609.8261108398438
|
||||
],
|
||||
[
|
||||
776.2955322265626,
|
||||
633.9828186035156
|
||||
],
|
||||
[
|
||||
801.0438537597655,
|
||||
580.8761901855469
|
||||
],
|
||||
[
|
||||
749.7462463378906,
|
||||
557.3454284667969
|
||||
]
|
||||
],
|
||||
"noise": 0.046125005191288204,
|
||||
"jitter_px": 0.03966469495253417,
|
||||
"trend_px": 0.015691223317140072,
|
||||
"trend_allowance_px": 0.10085507986642671,
|
||||
"trend_span_seconds": 0.50017508,
|
||||
"drift_px": 0.0,
|
||||
"noise_limit_px": 0.5,
|
||||
"drift_limit_px": 0.1,
|
||||
"drift_sigma": 3,
|
||||
"sampling_method": "separate_noise_drift_v1",
|
||||
"first_stamp_ns": 1789625825090890683,
|
||||
"last_stamp_ns": 1789625825591065763,
|
||||
"frames": 14
|
||||
},
|
||||
"feedback": 0.0
|
||||
},
|
||||
{
|
||||
"command": 6,
|
||||
"observation": {
|
||||
"corners": [
|
||||
[
|
||||
724.80810546875,
|
||||
609.4242858886719
|
||||
],
|
||||
[
|
||||
775.3557739257811,
|
||||
633.3601379394531
|
||||
],
|
||||
[
|
||||
799.8371276855469,
|
||||
580.1375732421875
|
||||
],
|
||||
[
|
||||
748.3967285156252,
|
||||
556.8480224609373
|
||||
]
|
||||
],
|
||||
"noise": 0.14805881526375836,
|
||||
"jitter_px": 0.08691260818921784,
|
||||
"trend_px": 0.1982129637846842,
|
||||
"trend_allowance_px": 0.2372810940977189,
|
||||
"trend_span_seconds": 0.533190049,
|
||||
"drift_px": 0.0,
|
||||
"noise_limit_px": 0.5,
|
||||
"drift_limit_px": 0.1,
|
||||
"drift_sigma": 3,
|
||||
"sampling_method": "separate_noise_drift_v1",
|
||||
"first_stamp_ns": 1789625826291301962,
|
||||
"last_stamp_ns": 1789625826824492011,
|
||||
"frames": 14
|
||||
},
|
||||
"feedback": 4.0
|
||||
},
|
||||
{
|
||||
"command": 7,
|
||||
"observation": {
|
||||
"corners": [
|
||||
[
|
||||
724.6632080078125,
|
||||
609.365966796875
|
||||
],
|
||||
[
|
||||
775.229736328125,
|
||||
633.2728881835938
|
||||
],
|
||||
[
|
||||
799.6818542480469,
|
||||
580.0370483398438
|
||||
],
|
||||
[
|
||||
748.237060546875,
|
||||
556.7812805175782
|
||||
]
|
||||
],
|
||||
"noise": 0.09413649452954809,
|
||||
"jitter_px": 0.06207585611532894,
|
||||
"trend_px": 0.09001067654022754,
|
||||
"trend_allowance_px": 0.1655087946680811,
|
||||
"trend_span_seconds": 0.500029394,
|
||||
"drift_px": 0.0,
|
||||
"noise_limit_px": 0.5,
|
||||
"drift_limit_px": 0.1,
|
||||
"drift_sigma": 3,
|
||||
"sampling_method": "separate_noise_drift_v1",
|
||||
"first_stamp_ns": 1789625827424758746,
|
||||
"last_stamp_ns": 1789625827924788140,
|
||||
"frames": 14
|
||||
},
|
||||
"feedback": 4.0
|
||||
},
|
||||
{
|
||||
"command": 8,
|
||||
"observation": {
|
||||
"corners": [
|
||||
[
|
||||
724.6258544921875,
|
||||
609.4119873046875
|
||||
],
|
||||
[
|
||||
775.1986389160155,
|
||||
633.2896728515625
|
||||
],
|
||||
[
|
||||
799.6358032226561,
|
||||
580.0672302246095
|
||||
],
|
||||
[
|
||||
748.1854248046875,
|
||||
556.8016662597656
|
||||
]
|
||||
],
|
||||
"noise": 0.23456026301632749,
|
||||
"jitter_px": 0.17069424687191923,
|
||||
"trend_px": 0.08347234871033143,
|
||||
"trend_allowance_px": 0.4481955362068039,
|
||||
"trend_span_seconds": 0.500136975,
|
||||
"drift_px": 0.0,
|
||||
"noise_limit_px": 0.5,
|
||||
"drift_limit_px": 0.1,
|
||||
"drift_sigma": 3,
|
||||
"sampling_method": "separate_noise_drift_v1",
|
||||
"first_stamp_ns": 1789625828558251841,
|
||||
"last_stamp_ns": 1789625829058388816,
|
||||
"frames": 14
|
||||
},
|
||||
"feedback": 4.0
|
||||
},
|
||||
{
|
||||
"command": 9,
|
||||
"observation": {
|
||||
"corners": [
|
||||
[
|
||||
722.7569885253905,
|
||||
608.575927734375
|
||||
],
|
||||
[
|
||||
773.571075439453,
|
||||
632.091064453125
|
||||
],
|
||||
[
|
||||
797.6090393066407,
|
||||
578.71826171875
|
||||
],
|
||||
[
|
||||
745.971405029297,
|
||||
555.848388671875
|
||||
]
|
||||
],
|
||||
"noise": 0.28939170791198104,
|
||||
"jitter_px": 0.18650108361326428,
|
||||
"trend_px": 0.41099993943257884,
|
||||
"trend_allowance_px": 0.4703926160593854,
|
||||
"trend_span_seconds": 0.50005576,
|
||||
"drift_px": 0.0,
|
||||
"noise_limit_px": 0.5,
|
||||
"drift_limit_px": 0.1,
|
||||
"drift_sigma": 3,
|
||||
"sampling_method": "separate_noise_drift_v1",
|
||||
"first_stamp_ns": 1789625829691989752,
|
||||
"last_stamp_ns": 1789625830192045512,
|
||||
"frames": 14
|
||||
},
|
||||
"feedback": 8.0
|
||||
},
|
||||
{
|
||||
"command": 10,
|
||||
"observation": {
|
||||
"corners": [
|
||||
[
|
||||
722.6316833496094,
|
||||
608.6056518554689
|
||||
],
|
||||
[
|
||||
773.4482116699219,
|
||||
632.0843200683594
|
||||
],
|
||||
[
|
||||
797.4689636230469,
|
||||
578.7062072753906
|
||||
],
|
||||
[
|
||||
745.824462890625,
|
||||
555.8463439941406
|
||||
]
|
||||
],
|
||||
"noise": 0.39986276411656946,
|
||||
"jitter_px": 0.27293816191364334,
|
||||
"trend_px": 0.08020979759981824,
|
||||
"trend_allowance_px": 0.715132310034276,
|
||||
"trend_span_seconds": 0.500219558,
|
||||
"drift_px": 0.0,
|
||||
"noise_limit_px": 0.5,
|
||||
"drift_limit_px": 0.1,
|
||||
"drift_sigma": 3,
|
||||
"sampling_method": "separate_noise_drift_v1",
|
||||
"first_stamp_ns": 1789625830858870767,
|
||||
"last_stamp_ns": 1789625831359090325,
|
||||
"frames": 14
|
||||
},
|
||||
"feedback": 8.0
|
||||
},
|
||||
{
|
||||
"command": 11,
|
||||
"observation": {
|
||||
"corners": [
|
||||
[
|
||||
722.5533752441406,
|
||||
608.5771484375001
|
||||
],
|
||||
[
|
||||
773.3728942871094,
|
||||
632.0658264160156
|
||||
],
|
||||
[
|
||||
797.3864440917969,
|
||||
578.6747131347656
|
||||
],
|
||||
[
|
||||
745.7501525878907,
|
||||
555.8247375488281
|
||||
]
|
||||
],
|
||||
"noise": 0.23539919240635074,
|
||||
"jitter_px": 0.1836222742672145,
|
||||
"trend_px": 0.18848684307304703,
|
||||
"trend_allowance_px": 0.47210784230400843,
|
||||
"trend_span_seconds": 0.500184729,
|
||||
"drift_px": 0.0,
|
||||
"noise_limit_px": 0.5,
|
||||
"drift_limit_px": 0.1,
|
||||
"drift_sigma": 3,
|
||||
"sampling_method": "separate_noise_drift_v1",
|
||||
"first_stamp_ns": 1789625832059199497,
|
||||
"last_stamp_ns": 1789625832559384226,
|
||||
"frames": 14
|
||||
},
|
||||
"feedback": 8.0
|
||||
},
|
||||
{
|
||||
"command": 12,
|
||||
"observation": {
|
||||
"corners": [
|
||||
[
|
||||
722.6437072753906,
|
||||
608.5297546386719
|
||||
],
|
||||
[
|
||||
773.471466064453,
|
||||
632.0102844238281
|
||||
],
|
||||
[
|
||||
797.4671020507812,
|
||||
578.6137084960938
|
||||
],
|
||||
[
|
||||
745.8285522460939,
|
||||
555.771240234375
|
||||
]
|
||||
],
|
||||
"noise": 0.31533275146493395,
|
||||
"jitter_px": 0.2761758683451024,
|
||||
"trend_px": 0.1556500145590418,
|
||||
"trend_allowance_px": 0.7088039489512584,
|
||||
"trend_span_seconds": 0.5001264,
|
||||
"drift_px": 0.0,
|
||||
"noise_limit_px": 0.5,
|
||||
"drift_limit_px": 0.1,
|
||||
"drift_sigma": 3,
|
||||
"sampling_method": "separate_noise_drift_v1",
|
||||
"first_stamp_ns": 1789625833192765939,
|
||||
"last_stamp_ns": 1789625833692892339,
|
||||
"frames": 14
|
||||
},
|
||||
"feedback": 8.0
|
||||
},
|
||||
{
|
||||
"command": 13,
|
||||
"observation": {
|
||||
"corners": [
|
||||
[
|
||||
721.2131958007812,
|
||||
609.1647338867188
|
||||
],
|
||||
[
|
||||
772.7929077148439,
|
||||
631.1820678710938
|
||||
],
|
||||
[
|
||||
795.1753540039062,
|
||||
577.0762329101561
|
||||
],
|
||||
[
|
||||
742.9220581054686,
|
||||
555.7918090820314
|
||||
]
|
||||
],
|
||||
"noise": 0.37441533524015225,
|
||||
"jitter_px": 0.26799377564636295,
|
||||
"trend_px": 0.5725672560194645,
|
||||
"trend_allowance_px": 0.7548675373675982,
|
||||
"trend_span_seconds": 0.500125003,
|
||||
"drift_px": 0.0,
|
||||
"noise_limit_px": 0.5,
|
||||
"drift_limit_px": 0.1,
|
||||
"drift_sigma": 3,
|
||||
"sampling_method": "separate_noise_drift_v1",
|
||||
"first_stamp_ns": 1789625834926541351,
|
||||
"last_stamp_ns": 1789625835426666354,
|
||||
"frames": 13
|
||||
},
|
||||
"feedback": 17.0
|
||||
}
|
||||
]
|
||||
}
|
||||
+294
@@ -0,0 +1,294 @@
|
||||
{
|
||||
"source": "20260917_144212_695120, side Tag 5, pinky_mcp_pitch descending",
|
||||
"note": "Recorded stable corner summaries. The 254 offset points opposite to continued travel from 253.",
|
||||
"samples": [
|
||||
{
|
||||
"command": 255,
|
||||
"observation": {
|
||||
"corners": [
|
||||
[
|
||||
1273.6978759765625,
|
||||
769.2497558593749
|
||||
],
|
||||
[
|
||||
1257.3016357421875,
|
||||
847.8428955078125
|
||||
],
|
||||
[
|
||||
1337.9423828125002,
|
||||
862.4230957031249
|
||||
],
|
||||
[
|
||||
1355.0609130859375,
|
||||
786.3067626953125
|
||||
]
|
||||
],
|
||||
"noise": 0.0638528437915139,
|
||||
"jitter_px": 0.03898050750100214,
|
||||
"trend_px": 0.11125048069665004,
|
||||
"trend_allowance_px": 0.10412129027730176,
|
||||
"trend_span_seconds": 0.533272293,
|
||||
"drift_px": 0.007129190419348286,
|
||||
"noise_limit_px": 0.5,
|
||||
"drift_limit_px": 0.1,
|
||||
"drift_sigma": 3,
|
||||
"sampling_method": "separate_noise_drift_v1",
|
||||
"first_stamp_ns": 1789627643382947185,
|
||||
"last_stamp_ns": 1789627643916219478,
|
||||
"frames": 13
|
||||
}
|
||||
},
|
||||
{
|
||||
"command": 254,
|
||||
"observation": {
|
||||
"corners": [
|
||||
[
|
||||
1273.6815185546875,
|
||||
769.8360290527343
|
||||
],
|
||||
[
|
||||
1257.2051391601562,
|
||||
848.3825073242188
|
||||
],
|
||||
[
|
||||
1337.817749023438,
|
||||
863.0668334960938
|
||||
],
|
||||
[
|
||||
1355.0137939453125,
|
||||
786.94873046875
|
||||
]
|
||||
],
|
||||
"noise": 0.05369982549450039,
|
||||
"jitter_px": 0.05403506716437094,
|
||||
"trend_px": 0.0463358706659947,
|
||||
"trend_allowance_px": 0.1559876888202236,
|
||||
"trend_span_seconds": 0.500105992,
|
||||
"drift_px": 0.0,
|
||||
"noise_limit_px": 0.5,
|
||||
"drift_limit_px": 0.1,
|
||||
"drift_sigma": 3,
|
||||
"sampling_method": "separate_noise_drift_v1",
|
||||
"first_stamp_ns": 1789627644549744182,
|
||||
"last_stamp_ns": 1789627645049850174,
|
||||
"frames": 12
|
||||
}
|
||||
},
|
||||
{
|
||||
"command": 253,
|
||||
"observation": {
|
||||
"corners": [
|
||||
[
|
||||
1273.7655029296875,
|
||||
767.4974365234374
|
||||
],
|
||||
[
|
||||
1257.66552734375,
|
||||
846.1763916015625
|
||||
],
|
||||
[
|
||||
1338.3572998046877,
|
||||
860.4445190429689
|
||||
],
|
||||
[
|
||||
1355.20263671875,
|
||||
784.2747802734376
|
||||
]
|
||||
],
|
||||
"noise": 0.04881191331715123,
|
||||
"jitter_px": 0.04836036611910405,
|
||||
"trend_px": 0.05408126314169758,
|
||||
"trend_allowance_px": 0.13961867019782592,
|
||||
"trend_span_seconds": 0.500004214,
|
||||
"drift_px": 0.0,
|
||||
"noise_limit_px": 0.5,
|
||||
"drift_limit_px": 0.1,
|
||||
"drift_sigma": 3,
|
||||
"sampling_method": "separate_noise_drift_v1",
|
||||
"first_stamp_ns": 1789627645683538490,
|
||||
"last_stamp_ns": 1789627646183542704,
|
||||
"frames": 12
|
||||
}
|
||||
},
|
||||
{
|
||||
"command": 252,
|
||||
"observation": {
|
||||
"corners": [
|
||||
[
|
||||
1273.8129882812498,
|
||||
765.5435791015626
|
||||
],
|
||||
[
|
||||
1258.0709228515625,
|
||||
844.3208007812502
|
||||
],
|
||||
[
|
||||
1338.8150634765625,
|
||||
858.2280883789062
|
||||
],
|
||||
[
|
||||
1355.3193359375,
|
||||
782.0257568359375
|
||||
]
|
||||
],
|
||||
"noise": 0.07184739712909484,
|
||||
"jitter_px": 0.05014399615128821,
|
||||
"trend_px": 0.07003170715135461,
|
||||
"trend_allowance_px": 0.1327168184227382,
|
||||
"trend_span_seconds": 0.500118122,
|
||||
"drift_px": 0.0,
|
||||
"noise_limit_px": 0.5,
|
||||
"drift_limit_px": 0.1,
|
||||
"drift_sigma": 3,
|
||||
"sampling_method": "separate_noise_drift_v1",
|
||||
"first_stamp_ns": 1789627646850438325,
|
||||
"last_stamp_ns": 1789627647350556447,
|
||||
"frames": 13
|
||||
}
|
||||
},
|
||||
{
|
||||
"command": 251,
|
||||
"observation": {
|
||||
"corners": [
|
||||
[
|
||||
1273.7503051757808,
|
||||
763.9506530761719
|
||||
],
|
||||
[
|
||||
1258.3120727539062,
|
||||
842.7455444335938
|
||||
],
|
||||
[
|
||||
1339.0409545898438,
|
||||
856.3628234863281
|
||||
],
|
||||
[
|
||||
1355.307861328125,
|
||||
780.06982421875
|
||||
]
|
||||
],
|
||||
"noise": 0.05175413750825869,
|
||||
"jitter_px": 0.04962061954897588,
|
||||
"trend_px": 0.04477872158303314,
|
||||
"trend_allowance_px": 0.14392986338813155,
|
||||
"trend_span_seconds": 0.500091532,
|
||||
"drift_px": 0.0,
|
||||
"noise_limit_px": 0.5,
|
||||
"drift_limit_px": 0.1,
|
||||
"drift_sigma": 3,
|
||||
"sampling_method": "separate_noise_drift_v1",
|
||||
"first_stamp_ns": 1789627648017379982,
|
||||
"last_stamp_ns": 1789627648517471514,
|
||||
"frames": 12
|
||||
}
|
||||
},
|
||||
{
|
||||
"command": 250,
|
||||
"observation": {
|
||||
"corners": [
|
||||
[
|
||||
1273.7471313476562,
|
||||
763.4657592773438
|
||||
],
|
||||
[
|
||||
1258.3818969726562,
|
||||
842.2876892089845
|
||||
],
|
||||
[
|
||||
1339.12255859375,
|
||||
855.7933959960938
|
||||
],
|
||||
[
|
||||
1355.3082885742188,
|
||||
779.4655761718749
|
||||
]
|
||||
],
|
||||
"noise": 0.03953247260396676,
|
||||
"jitter_px": 0.03832493601221934,
|
||||
"trend_px": 0.024577910888215284,
|
||||
"trend_allowance_px": 0.11063456061665009,
|
||||
"trend_span_seconds": 0.5001264,
|
||||
"drift_px": 0.0,
|
||||
"noise_limit_px": 0.5,
|
||||
"drift_limit_px": 0.1,
|
||||
"drift_sigma": 3,
|
||||
"sampling_method": "separate_noise_drift_v1",
|
||||
"first_stamp_ns": 1789627649150962220,
|
||||
"last_stamp_ns": 1789627649651088620,
|
||||
"frames": 12
|
||||
}
|
||||
},
|
||||
{
|
||||
"command": 249,
|
||||
"observation": {
|
||||
"corners": [
|
||||
[
|
||||
1273.7210083007815,
|
||||
763.47802734375
|
||||
],
|
||||
[
|
||||
1258.3460083007812,
|
||||
842.2982788085938
|
||||
],
|
||||
[
|
||||
1339.0960083007815,
|
||||
855.8121948242188
|
||||
],
|
||||
[
|
||||
1355.2714233398438,
|
||||
779.5064392089844
|
||||
]
|
||||
],
|
||||
"noise": 0.04961577833358761,
|
||||
"jitter_px": 0.033217208095547415,
|
||||
"trend_px": 0.06525336135301234,
|
||||
"trend_allowance_px": 0.0941170895703111,
|
||||
"trend_span_seconds": 0.500144418,
|
||||
"drift_px": 0.0,
|
||||
"noise_limit_px": 0.5,
|
||||
"drift_limit_px": 0.1,
|
||||
"drift_sigma": 3,
|
||||
"sampling_method": "separate_noise_drift_v1",
|
||||
"first_stamp_ns": 1789627650284594722,
|
||||
"last_stamp_ns": 1789627650784739140,
|
||||
"frames": 12
|
||||
}
|
||||
},
|
||||
{
|
||||
"command": 248,
|
||||
"observation": {
|
||||
"corners": [
|
||||
[
|
||||
1273.7547607421875,
|
||||
763.5081176757812
|
||||
],
|
||||
[
|
||||
1258.37548828125,
|
||||
842.3294677734375
|
||||
],
|
||||
[
|
||||
1339.1057739257812,
|
||||
855.85595703125
|
||||
],
|
||||
[
|
||||
1355.3087768554688,
|
||||
779.533447265625
|
||||
]
|
||||
],
|
||||
"noise": 0.06620387480061013,
|
||||
"jitter_px": 0.06684487550359008,
|
||||
"trend_px": 0.019933394501092185,
|
||||
"trend_allowance_px": 0.19296516199312497,
|
||||
"trend_span_seconds": 0.500106124,
|
||||
"drift_px": 0.0,
|
||||
"noise_limit_px": 0.5,
|
||||
"drift_limit_px": 0.1,
|
||||
"drift_sigma": 3,
|
||||
"sampling_method": "separate_noise_drift_v1",
|
||||
"first_stamp_ns": 1789627651418263846,
|
||||
"last_stamp_ns": 1789627651918369970,
|
||||
"frames": 12
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
+114
@@ -0,0 +1,114 @@
|
||||
{
|
||||
"source": "20260917_150302_309649, side Tag 5, pinky_mcp_pitch descending",
|
||||
"note": "Only final stable summaries were logged. No frames from before the 0.3 s sampling gate are available.",
|
||||
"samples": [
|
||||
{
|
||||
"command": 255,
|
||||
"observation": {
|
||||
"corners": [
|
||||
[
|
||||
1274.3587036132815,
|
||||
774.0721435546875
|
||||
],
|
||||
[
|
||||
1256.6531982421875,
|
||||
852.3977355957032
|
||||
],
|
||||
[
|
||||
1337.0504150390625,
|
||||
868.3337707519531
|
||||
],
|
||||
[
|
||||
1355.3723754882812,
|
||||
792.4201965332031
|
||||
]
|
||||
],
|
||||
"noise": 0.07677731544894326,
|
||||
"jitter_px": 0.06915598836612893,
|
||||
"trend_px": 0.07067661877158919,
|
||||
"trend_allowance_px": 0.1934994221139253,
|
||||
"trend_span_seconds": 0.500164993,
|
||||
"drift_px": 0.0,
|
||||
"noise_limit_px": 0.5,
|
||||
"drift_limit_px": 0.1,
|
||||
"drift_sigma": 3,
|
||||
"sampling_method": "separate_noise_drift_v1",
|
||||
"first_stamp_ns": 1789628600431201374,
|
||||
"last_stamp_ns": 1789628600931366367,
|
||||
"frames": 12
|
||||
}
|
||||
},
|
||||
{
|
||||
"command": 254,
|
||||
"observation": {
|
||||
"corners": [
|
||||
[
|
||||
1274.3126831054688,
|
||||
774.1547546386719
|
||||
],
|
||||
[
|
||||
1256.5889892578125,
|
||||
852.4772338867188
|
||||
],
|
||||
[
|
||||
1336.9767456054685,
|
||||
868.4264526367188
|
||||
],
|
||||
[
|
||||
1355.3369140625,
|
||||
792.5004882812499
|
||||
]
|
||||
],
|
||||
"noise": 0.05312886824277245,
|
||||
"jitter_px": 0.05662112711965161,
|
||||
"trend_px": 0.10070611585415146,
|
||||
"trend_allowance_px": 0.1604277605896978,
|
||||
"trend_span_seconds": 0.500107491,
|
||||
"drift_px": 0.0,
|
||||
"noise_limit_px": 0.5,
|
||||
"drift_limit_px": 0.1,
|
||||
"drift_sigma": 3,
|
||||
"sampling_method": "separate_noise_drift_v1",
|
||||
"first_stamp_ns": 1789628601564762253,
|
||||
"last_stamp_ns": 1789628602064869744,
|
||||
"frames": 12
|
||||
}
|
||||
},
|
||||
{
|
||||
"command": 253,
|
||||
"observation": {
|
||||
"corners": [
|
||||
[
|
||||
1274.3944091796875,
|
||||
772.2680358886719
|
||||
],
|
||||
[
|
||||
1256.9893188476562,
|
||||
850.7044982910157
|
||||
],
|
||||
[
|
||||
1337.468017578125,
|
||||
866.2935791015625
|
||||
],
|
||||
[
|
||||
1355.518798828125,
|
||||
790.3155212402344
|
||||
]
|
||||
],
|
||||
"noise": 0.04742025782261909,
|
||||
"jitter_px": 0.04368953813957662,
|
||||
"trend_px": 0.026077948707714425,
|
||||
"trend_allowance_px": 0.11966037522274116,
|
||||
"trend_span_seconds": 0.500001229,
|
||||
"drift_px": 0.0,
|
||||
"noise_limit_px": 0.5,
|
||||
"drift_limit_px": 0.1,
|
||||
"drift_sigma": 3,
|
||||
"sampling_method": "separate_noise_drift_v1",
|
||||
"first_stamp_ns": 1789628602731858019,
|
||||
"last_stamp_ns": 1789628603231859248,
|
||||
"frames": 12
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
+394
@@ -0,0 +1,394 @@
|
||||
[
|
||||
{
|
||||
"command": 0,
|
||||
"capture_after_first_step": 0,
|
||||
"observation": {
|
||||
"corners": [
|
||||
[
|
||||
742.5088806152344,
|
||||
611.0645446777344
|
||||
],
|
||||
[
|
||||
797.4117126464844,
|
||||
620.8008728027344
|
||||
],
|
||||
[
|
||||
807.3281860351562,
|
||||
564.4863891601561
|
||||
],
|
||||
[
|
||||
753.1266479492186,
|
||||
555.2648620605469
|
||||
]
|
||||
],
|
||||
"noise": 0.06209724336448246,
|
||||
"frames": 8,
|
||||
"first_stamp_ns": 1789622318425285240,
|
||||
"last_stamp_ns": 1789622318692111391
|
||||
}
|
||||
},
|
||||
{
|
||||
"command": 1,
|
||||
"capture_after_first_step": 0.481756416,
|
||||
"observation": {
|
||||
"corners": [
|
||||
[
|
||||
743.0127868652344,
|
||||
611.1296691894531
|
||||
],
|
||||
[
|
||||
797.9120483398438,
|
||||
620.9851379394531
|
||||
],
|
||||
[
|
||||
807.909423828125,
|
||||
564.6954956054688
|
||||
],
|
||||
[
|
||||
753.7304992675781,
|
||||
555.3496704101562
|
||||
]
|
||||
],
|
||||
"noise": 0.17849494174036784,
|
||||
"frames": 8,
|
||||
"first_stamp_ns": 1789622319325525252,
|
||||
"last_stamp_ns": 1789622319592305756
|
||||
}
|
||||
},
|
||||
{
|
||||
"command": 2,
|
||||
"capture_after_first_step": 1.3486976,
|
||||
"observation": {
|
||||
"corners": [
|
||||
[
|
||||
743.6234130859376,
|
||||
611.2144775390623
|
||||
],
|
||||
[
|
||||
798.4994812011719,
|
||||
621.2155456542969
|
||||
],
|
||||
[
|
||||
808.6659851074219,
|
||||
564.9505615234374
|
||||
],
|
||||
[
|
||||
754.5102233886718,
|
||||
555.4612426757812
|
||||
]
|
||||
],
|
||||
"noise": 0.049362087869316736,
|
||||
"frames": 8,
|
||||
"first_stamp_ns": 1789622320192489601,
|
||||
"last_stamp_ns": 1789622320459223681
|
||||
}
|
||||
},
|
||||
{
|
||||
"command": 3,
|
||||
"capture_after_first_step": 2.282204672,
|
||||
"observation": {
|
||||
"corners": [
|
||||
[
|
||||
743.8411254882814,
|
||||
611.260223388672
|
||||
],
|
||||
[
|
||||
798.6911315917969,
|
||||
621.2981567382812
|
||||
],
|
||||
[
|
||||
808.9057006835939,
|
||||
565.0499877929688
|
||||
],
|
||||
[
|
||||
754.7449645996096,
|
||||
555.5102844238281
|
||||
]
|
||||
],
|
||||
"noise": 0.026590851286742108,
|
||||
"frames": 8,
|
||||
"first_stamp_ns": 1789622321125924795,
|
||||
"last_stamp_ns": 1789622321392802797
|
||||
}
|
||||
},
|
||||
{
|
||||
"command": 4,
|
||||
"capture_after_first_step": 3.182333184,
|
||||
"observation": {
|
||||
"corners": [
|
||||
[
|
||||
743.8772277832031,
|
||||
611.2159729003906
|
||||
],
|
||||
[
|
||||
798.741180419922,
|
||||
621.2796020507812
|
||||
],
|
||||
[
|
||||
808.9512634277343,
|
||||
565.0045166015625
|
||||
],
|
||||
[
|
||||
754.7894592285155,
|
||||
555.4828186035159
|
||||
]
|
||||
],
|
||||
"noise": 0.03532359720127038,
|
||||
"frames": 8,
|
||||
"first_stamp_ns": 1789622322026134617,
|
||||
"last_stamp_ns": 1789622322292850022
|
||||
}
|
||||
},
|
||||
{
|
||||
"command": 5,
|
||||
"capture_after_first_step": 4.416085504,
|
||||
"observation": {
|
||||
"corners": [
|
||||
[
|
||||
743.8970642089846,
|
||||
611.229766845703
|
||||
],
|
||||
[
|
||||
798.7597961425781,
|
||||
621.2988891601562
|
||||
],
|
||||
[
|
||||
808.9626159667968,
|
||||
565.0206298828125
|
||||
],
|
||||
[
|
||||
754.7981567382812,
|
||||
555.5004272460935
|
||||
]
|
||||
],
|
||||
"noise": 0.05400584486468604,
|
||||
"frames": 8,
|
||||
"first_stamp_ns": 1789622323259951327,
|
||||
"last_stamp_ns": 1789622323526538114
|
||||
}
|
||||
},
|
||||
{
|
||||
"command": 6,
|
||||
"capture_after_first_step": 5.382996224,
|
||||
"observation": {
|
||||
"corners": [
|
||||
[
|
||||
743.4072265625,
|
||||
611.1723327636719
|
||||
],
|
||||
[
|
||||
798.2764892578125,
|
||||
621.1207275390625
|
||||
],
|
||||
[
|
||||
808.3975830078124,
|
||||
564.8411865234376
|
||||
],
|
||||
[
|
||||
754.2210083007812,
|
||||
555.4093322753906
|
||||
]
|
||||
],
|
||||
"noise": 0.04009534303293828,
|
||||
"frames": 8,
|
||||
"first_stamp_ns": 1789622324226828831,
|
||||
"last_stamp_ns": 1789622324493481883
|
||||
}
|
||||
},
|
||||
{
|
||||
"command": 7,
|
||||
"capture_after_first_step": 6.2831872,
|
||||
"observation": {
|
||||
"corners": [
|
||||
[
|
||||
741.6768493652344,
|
||||
610.9752807617188
|
||||
],
|
||||
[
|
||||
796.629364013672,
|
||||
620.5213928222656
|
||||
],
|
||||
[
|
||||
806.3320922851562,
|
||||
564.1734619140623
|
||||
],
|
||||
[
|
||||
752.1263732910155,
|
||||
555.147979736328
|
||||
]
|
||||
],
|
||||
"noise": 0.026984666916765182,
|
||||
"frames": 8,
|
||||
"first_stamp_ns": 1789622325126979213,
|
||||
"last_stamp_ns": 1789622325393713293
|
||||
}
|
||||
},
|
||||
{
|
||||
"command": 8,
|
||||
"capture_after_first_step": 7.183621888,
|
||||
"observation": {
|
||||
"corners": [
|
||||
[
|
||||
741.5669860839844,
|
||||
610.9591979980469
|
||||
],
|
||||
[
|
||||
796.5059814453125,
|
||||
620.4830627441406
|
||||
],
|
||||
[
|
||||
806.2083740234375,
|
||||
564.13134765625
|
||||
],
|
||||
[
|
||||
751.972412109375,
|
||||
555.1249389648438
|
||||
]
|
||||
],
|
||||
"noise": 0.05984294294719469,
|
||||
"frames": 8,
|
||||
"first_stamp_ns": 1789622326027413962,
|
||||
"last_stamp_ns": 1789622326294148042
|
||||
}
|
||||
},
|
||||
{
|
||||
"command": 9,
|
||||
"capture_after_first_step": 8.117092096,
|
||||
"observation": {
|
||||
"corners": [
|
||||
[
|
||||
740.7914428710938,
|
||||
610.8335571289062
|
||||
],
|
||||
[
|
||||
795.7557983398438,
|
||||
620.1954650878906
|
||||
],
|
||||
[
|
||||
805.2627868652344,
|
||||
563.8038940429686
|
||||
],
|
||||
[
|
||||
751.0033569335938,
|
||||
554.9827575683594
|
||||
]
|
||||
],
|
||||
"noise": 0.26736788159781666,
|
||||
"frames": 8,
|
||||
"first_stamp_ns": 1789622326960775070,
|
||||
"last_stamp_ns": 1789622327227727634
|
||||
}
|
||||
},
|
||||
{
|
||||
"command": 10,
|
||||
"capture_after_first_step": 9.0505536,
|
||||
"observation": {
|
||||
"corners": [
|
||||
[
|
||||
740.2551879882812,
|
||||
610.7673950195312
|
||||
],
|
||||
[
|
||||
795.2304077148439,
|
||||
619.9817504882812
|
||||
],
|
||||
[
|
||||
804.6241149902344,
|
||||
563.5697021484375
|
||||
],
|
||||
[
|
||||
750.3331909179685,
|
||||
554.8916015625
|
||||
]
|
||||
],
|
||||
"noise": 0.031879226278586,
|
||||
"frames": 8,
|
||||
"first_stamp_ns": 1789622327894336301,
|
||||
"last_stamp_ns": 1789622328161089237
|
||||
}
|
||||
},
|
||||
{
|
||||
"command": 11,
|
||||
"capture_after_first_step": 9.984171776,
|
||||
"observation": {
|
||||
"corners": [
|
||||
[
|
||||
737.6374206542968,
|
||||
610.4871826171877
|
||||
],
|
||||
[
|
||||
792.7057495117188,
|
||||
619.0874938964844
|
||||
],
|
||||
[
|
||||
801.4889831542969,
|
||||
562.5967712402343
|
||||
],
|
||||
[
|
||||
747.1112670898438,
|
||||
554.500732421875
|
||||
]
|
||||
],
|
||||
"noise": 0.09376922935599069,
|
||||
"frames": 8,
|
||||
"first_stamp_ns": 1789622328827963813,
|
||||
"last_stamp_ns": 1789622329094697893
|
||||
}
|
||||
},
|
||||
{
|
||||
"command": 12,
|
||||
"capture_after_first_step": 11.718063616,
|
||||
"observation": {
|
||||
"corners": [
|
||||
[
|
||||
737.5603332519531,
|
||||
610.4834594726562
|
||||
],
|
||||
[
|
||||
792.6178588867189,
|
||||
619.0565490722656
|
||||
],
|
||||
[
|
||||
801.383819580078,
|
||||
562.5599060058594
|
||||
],
|
||||
[
|
||||
746.9969177246093,
|
||||
554.4959106445312
|
||||
]
|
||||
],
|
||||
"noise": 0.1415795171765463,
|
||||
"frames": 8,
|
||||
"first_stamp_ns": 1789622330561883199,
|
||||
"last_stamp_ns": 1789622330828562166
|
||||
}
|
||||
},
|
||||
{
|
||||
"command": 13,
|
||||
"capture_after_first_step": 12.951605504,
|
||||
"observation": {
|
||||
"corners": [
|
||||
[
|
||||
737.5241394042969,
|
||||
610.4622192382812
|
||||
],
|
||||
[
|
||||
792.5900268554689,
|
||||
619.0160217285156
|
||||
],
|
||||
[
|
||||
801.3379211425781,
|
||||
562.5231018066406
|
||||
],
|
||||
[
|
||||
746.9424743652341,
|
||||
554.4635009765625
|
||||
]
|
||||
],
|
||||
"noise": 0.07121157415371036,
|
||||
"frames": 8,
|
||||
"first_stamp_ns": 1789622331795364624,
|
||||
"last_stamp_ns": 1789622332062164704
|
||||
}
|
||||
}
|
||||
]
|
||||
+1102
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,151 @@
|
||||
"""Only the first stable displacement matters, irrespective of motion direction."""
|
||||
from dataclasses import asdict
|
||||
import json
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from linkerhand_range_calibration.adapters.fake import FakeAdapter
|
||||
from linkerhand_range_calibration.core.endpoints import SCAN_METHOD, EndpointSearch
|
||||
from linkerhand_range_calibration.core.legacy_endpoints import EndpointSearch as LegacyEndpointSearch
|
||||
from linkerhand_range_calibration.core.engine import Engine
|
||||
from linkerhand_range_calibration.profiles import Settings, load_profile
|
||||
from linkerhand_range_calibration.replay import replay
|
||||
from linkerhand_range_calibration.simulation import simulated_teaching
|
||||
from linkerhand_range_calibration.storage import Session
|
||||
|
||||
|
||||
def recorded_samples():
|
||||
path = Path(__file__).parent / 'fixtures' / 'pinky_pitch_reversed_departure.json'
|
||||
return json.loads(path.read_text())['samples']
|
||||
|
||||
|
||||
def observation(displacement, rotation=False):
|
||||
points = np.array([[100, 100], [140, 100], [140, 140], [100, 140]], dtype=float)
|
||||
if rotation:
|
||||
angle = displacement / np.sqrt(800)
|
||||
matrix = np.array([[np.cos(angle), -np.sin(angle)], [np.sin(angle), np.cos(angle)]])
|
||||
points = (points - 120) @ matrix + 120
|
||||
else:
|
||||
points += [displacement, 0]
|
||||
return {'corners': points.tolist(), 'noise': .02}
|
||||
|
||||
|
||||
def test_recorded_reversal_confirms_the_first_departure_without_replacing_it():
|
||||
joint = load_profile().by_name['pinky_mcp_pitch']
|
||||
legacy = LegacyEndpointSearch(joint, 'down', Settings(), 'endpoint_search_v5')
|
||||
current = EndpointSearch(joint, 'down', Settings())
|
||||
for sample in recorded_samples():
|
||||
legacy.add(sample['command'], sample['observation'])
|
||||
current.add(sample['command'], sample['observation'])
|
||||
assert legacy.candidate == 254 and legacy.boundary is None
|
||||
assert current.result() == {'trigger': 254, 'bound': 255, 'confirmed_at': 254, 'threshold': .5}
|
||||
assert current.reference == pytest.approx(np.asarray(recorded_samples()[0]['observation']['corners']))
|
||||
|
||||
|
||||
@pytest.mark.parametrize('direction', ['up', 'down'])
|
||||
@pytest.mark.parametrize('rotation', [False, True])
|
||||
@pytest.mark.parametrize('shift', [-1.2, 1.2])
|
||||
def test_any_first_displacement_stops_without_followup_commands(direction, rotation, shift):
|
||||
search = EndpointSearch(load_profile().joints[0], direction, Settings(confirmation_points=99))
|
||||
origin, first, bound = (0, 1, 0) if direction == 'up' else (255, 254, 255)
|
||||
search.add(origin, observation(0, rotation))
|
||||
search.add(first, observation(shift, rotation))
|
||||
assert search.done
|
||||
assert search.result() == {'trigger': first, 'bound': bound, 'confirmed_at': first, 'threshold': .5}
|
||||
assert search.steps == 2
|
||||
|
||||
|
||||
@pytest.mark.parametrize('later', [0, .7, -1.2, 4])
|
||||
def test_later_stop_return_or_reversal_cannot_reject_a_recorded_movement(later):
|
||||
search = EndpointSearch(load_profile().joints[0], 'up', Settings())
|
||||
search.add(0, observation(0))
|
||||
search.add(1, observation(.7))
|
||||
saved = search.result()
|
||||
for command in range(2, 7):
|
||||
search.add(command, observation(later))
|
||||
assert search.result() == saved
|
||||
assert saved['bound'] == 0 and saved['confirmed_at'] == 1
|
||||
assert search.steps == 2
|
||||
|
||||
|
||||
def test_static_noise_below_threshold_is_not_motion():
|
||||
search = EndpointSearch(load_profile().joints[0], 'up', Settings())
|
||||
search.add(0, observation(0))
|
||||
for command in range(1, 256):
|
||||
search.add(command, observation(.2 * (-1) ** command))
|
||||
assert search.error and search.boundary is None
|
||||
|
||||
|
||||
def test_recorded_departure_stops_live_single_joint_scan_and_replays(tmp_path):
|
||||
trace = {sample['command']: sample['observation']['corners'] for sample in recorded_samples()}
|
||||
|
||||
class RecordedAdapter(FakeAdapter):
|
||||
def frames(self):
|
||||
frames = super().frames()
|
||||
if self.target[10] in trace:
|
||||
frames['side'][5] = trace[self.target[10]]
|
||||
return frames
|
||||
|
||||
profile = load_profile()
|
||||
name = 'pinky_mcp_pitch'
|
||||
adapter = RecordedAdapter(profile)
|
||||
adapter.ranges[name] = (6, 255)
|
||||
settings = Settings(rate=10000, stable_frames=4, settle_seconds=.01)
|
||||
teaching = simulated_teaching(profile, adapter.uid)
|
||||
session = Session(tmp_path, profile, adapter.uid, asdict(settings), teaching, {'simulated': True})
|
||||
engine = Engine(profile, settings, adapter, session.emit)
|
||||
adapter.advance(1., 1_000_000_000)
|
||||
engine.start(teaching, 1., 1_000_000_000, [name])
|
||||
try:
|
||||
for tick in range(1, 1000):
|
||||
now = 1 + tick / 10
|
||||
stamp = round(now * 1e9)
|
||||
adapter.advance(now, stamp)
|
||||
for view, frame in adapter.frames().items():
|
||||
engine.observe(view, stamp, frame, now)
|
||||
engine.tick(now, stamp)
|
||||
assert engine.state != 'PAUSED', engine.reason
|
||||
if engine.state == 'COMPLETED':
|
||||
break
|
||||
assert engine.state == 'COMPLETED'
|
||||
finally:
|
||||
session.close()
|
||||
journal = session.directory / 'samples.jsonl'
|
||||
events = [json.loads(line) for line in journal.read_text().splitlines()]
|
||||
assert events[0]['scan_method'] == SCAN_METHOD
|
||||
assert session.document()['joints'][name] == {'min': 6, 'max': 255}
|
||||
commands = [event['command'] for event in events
|
||||
if event['kind'] == 'sample' and event['direction'] == 'down']
|
||||
assert commands == [255, 254]
|
||||
assert not any(event['kind'] == 'boundary_candidate_rejected' for event in events)
|
||||
assert replay(journal, tmp_path / 'recomputed.json') == session.document()
|
||||
|
||||
|
||||
@pytest.mark.parametrize('method,expected_max', [
|
||||
('endpoint_search_v5', None), ('endpoint_search_v6', 254),
|
||||
('endpoint_search_v7', 255), ('endpoint_search_v8', 255), (SCAN_METHOD, 255),
|
||||
])
|
||||
def test_replay_preserves_the_logged_confirmation_method(tmp_path, method, expected_max):
|
||||
profile = load_profile()
|
||||
name = 'pinky_mcp_pitch'
|
||||
events = [
|
||||
{'kind': 'metadata', 'profile': profile.raw, 'settings': asdict(Settings()),
|
||||
'uid': 'RECORDED_PINKY', 'scan_method': method},
|
||||
{'kind': 'task_started', 'task': name, 'joints': [name]},
|
||||
]
|
||||
for command in range(4):
|
||||
events.append({'kind': 'sample', 'task': name, 'stage': 'endpoint', 'repeat': 0,
|
||||
'direction': 'up', 'command': command,
|
||||
'observations': {name: observation(command * 2)}})
|
||||
for sample in recorded_samples():
|
||||
events.append({'kind': 'sample', 'task': name, 'stage': 'endpoint', 'repeat': 0,
|
||||
'direction': 'down', 'command': sample['command'],
|
||||
'observations': {name: sample['observation']}})
|
||||
events.append({'kind': 'task_result', 'task': name})
|
||||
journal = tmp_path / 'recorded.jsonl'
|
||||
journal.write_text('\n'.join(json.dumps(event) for event in events))
|
||||
document = replay(journal, tmp_path / 'recomputed.json')
|
||||
assert document['joints'][name] == {'min': 0 if expected_max is not None else None,
|
||||
'max': expected_max}
|
||||
@@ -497,22 +497,22 @@ def test_quality_and_duplicate_frames():
|
||||
assert quality_error(corners,120,480,0,60,s)
|
||||
w=StableWindow(4)
|
||||
for _ in range(10):w.add(Observation(1,1,corners))
|
||||
assert w.summary(1) is None
|
||||
assert w.summary(1, .1) is None
|
||||
for stamp in range(2,5):w.add(Observation(stamp,stamp,corners))
|
||||
assert w.summary(1)['frames']==4
|
||||
assert w.summary(1, .1)['frames']==4
|
||||
|
||||
|
||||
def test_group_holds_and_restart_preserves_completed_results():
|
||||
p=load_profile();s=Settings(stable_frames=4,rate=1000,settle_seconds=.01)
|
||||
ranges={joint.name:(0,255) for joint in p.joints}
|
||||
ranges['middle_mcp_roll']=(7,255)
|
||||
ranges['ring_mcp_roll']=(7,255)
|
||||
a=FakeAdapter(p,ranges);e=Engine(p,s,a);teach=simulated_teaching(p,a.uid)
|
||||
now=1.;a.advance(now,int(now*1e9));e.start(teach,now,int(now*1e9),['thumb_cmc_roll','four_finger_roll'])
|
||||
for _ in range(400):
|
||||
now+=.1;stamp=round(now*1e9);a.advance(now,stamp)
|
||||
for view,frame in a.frames().items():e.observe(view,stamp,frame,now)
|
||||
e.tick(now,stamp)
|
||||
if (e.task.name=='four_finger_roll' and e.direction=='up' and e.phase=='moving'
|
||||
if (e.task.name=='four_finger_roll' and e.segment and e.segment.name=='group_lower' and e.phase=='moving'
|
||||
and any(e.endpoint_results.values())):break
|
||||
assert e.results['thumb_cmc_roll']['status']=='成功'
|
||||
assert e.endpoint_results['index_mcp_roll'] and not e.endpoint_results['middle_mcp_roll']
|
||||
|
||||
@@ -11,6 +11,7 @@ from linkerhand_range_calibration.core.endpoints import (
|
||||
)
|
||||
from linkerhand_range_calibration.core.engine import Engine
|
||||
from linkerhand_range_calibration.core.errors import Unmeasurable
|
||||
from linkerhand_range_calibration.core.trajectory import Trajectory
|
||||
from linkerhand_range_calibration.profiles import Settings, load_profile, profile_from_dict
|
||||
from linkerhand_range_calibration.replay import replay
|
||||
from linkerhand_range_calibration.simulation import simulated_teaching
|
||||
@@ -44,23 +45,25 @@ def test_search_confirms_first_trigger_and_stops_early(bounds):
|
||||
boundary = search.result()
|
||||
if direction == 'up':
|
||||
assert boundary == {'bound': bounds[0], 'trigger': bounds[0] + 1,
|
||||
'confirmed_at': bounds[0] + 3, 'threshold': .5}
|
||||
'confirmed_at': bounds[0] + 1, 'threshold': .5}
|
||||
else:
|
||||
assert boundary == {'bound': bounds[1], 'trigger': bounds[1] - 1,
|
||||
'confirmed_at': bounds[1] - 3, 'threshold': .5}
|
||||
'confirmed_at': bounds[1] - 1, 'threshold': .5}
|
||||
assert len(samples) < 35
|
||||
result = analyze_endpoint_samples(joint, sweeps, settings)
|
||||
assert (result['min'], result['max']) == bounds
|
||||
|
||||
|
||||
def test_transient_outlier_is_rejected_and_cumulative_motion_is_detected():
|
||||
def test_first_stable_displacement_is_final_and_small_motion_accumulates():
|
||||
joint, settings = load_profile().joints[0], Settings()
|
||||
search = EndpointSearch(joint, 'up', settings)
|
||||
for command in range(10):
|
||||
x = 50 if command == 2 else float(np.clip(command, 6, 243))
|
||||
search.add(command, observed(x))
|
||||
assert search.result()['trigger'] == 7
|
||||
assert search.result()['bound'] == 6
|
||||
# These inputs are already stable window summaries, not individual frames.
|
||||
# Their first displacement is the boundary, regardless of later behavior.
|
||||
assert search.result()['trigger'] == 2
|
||||
assert search.result()['bound'] == 1
|
||||
# Adjacent positions differ by less than the threshold, but the endpoint remains fixed.
|
||||
search, _ = endpoint_samples(joint, 'up', (6, 243), settings,
|
||||
observe=lambda x: observed(x, scale=.1))
|
||||
@@ -81,7 +84,10 @@ def test_rotation_and_independent_noise_reference_at_each_end():
|
||||
high = EndpointSearch(joint, 'down', settings)
|
||||
low.add(0, observed(0, noise=.01))
|
||||
high.add(255, observed(255, noise=.2))
|
||||
assert low.threshold == .5 and high.threshold == 1
|
||||
assert low.threshold == high.threshold == .5
|
||||
historical = EndpointSearch(joint, 'down', settings, 'endpoint_search_v9')
|
||||
historical.add(255, observed(255, noise=.2))
|
||||
assert historical.threshold == 1
|
||||
|
||||
|
||||
def test_search_rejects_missing_duplicate_and_unconfirmed_samples():
|
||||
@@ -94,11 +100,11 @@ def test_search_rejects_missing_duplicate_and_unconfirmed_samples():
|
||||
for direction in ('up', 'down'):
|
||||
search, samples = endpoint_samples(joint, direction, (0, 0), settings)
|
||||
assert len(samples) == 256 and search.done
|
||||
with pytest.raises(Unmeasurable, match='未获得足够'):
|
||||
with pytest.raises(Unmeasurable, match='未检测到'):
|
||||
search.result()
|
||||
search, _ = endpoint_samples(joint, 'up', (254, 255), settings)
|
||||
with pytest.raises(Unmeasurable, match='未获得足够'):
|
||||
search.result()
|
||||
assert search.result()['bound'] == 254
|
||||
assert search.result()['confirmed_at'] == 255
|
||||
|
||||
|
||||
def test_optional_repeat_and_disjoint_boundary_evidence():
|
||||
@@ -156,8 +162,8 @@ def test_single_task_samples_only_endpoint_neighborhoods_and_replays(tmp_path):
|
||||
engine, session, events = run_scan(tmp_path, 'thumb_cmc_roll', [(6, 243)])
|
||||
points = [event for event in events if event['kind'] == 'sample']
|
||||
assert [(p['direction'], p['command']) for p in points] == (
|
||||
[('up', c) for c in range(10)] + [('down', c) for c in range(255, 239, -1)])
|
||||
assert len(points) == 26
|
||||
[('up', c) for c in range(8)] + [('down', c) for c in range(255, 241, -1)])
|
||||
assert len(points) == 22
|
||||
assert {p['stage'] for p in points} == {'endpoint'}
|
||||
assert events[0]['scan_method'] == SCAN_METHOD
|
||||
assert engine.progress == (2, 2)
|
||||
@@ -174,16 +180,53 @@ def test_single_task_samples_only_endpoint_neighborhoods_and_replays(tmp_path):
|
||||
assert result['joints']['thumb_cmc_roll'] == {'min': 6, 'max': 243}
|
||||
|
||||
|
||||
def test_four_fingers_share_commands_and_stop_when_all_boundaries_confirmed(tmp_path):
|
||||
bounds = [(0, 230), (7, 255), (6, 243), (0, 255)]
|
||||
@pytest.mark.parametrize('middle_min', [7, 77, 79, 80, 100])
|
||||
def test_group_scan_defers_middle_lower_and_continues_across_split(tmp_path, middle_min):
|
||||
bounds = [(0, 230), (middle_min, 255), (6, 243), (0, 255)]
|
||||
engine, session, events = run_scan(tmp_path, 'four_finger_roll', bounds)
|
||||
for name, expected in zip(engine.task.joints, bounds):
|
||||
assert (engine.results[name]['min'], engine.results[name]['max']) == expected
|
||||
points = [event for event in events if event['kind'] == 'sample']
|
||||
assert [p['command'] for p in points if p['direction'] == 'up'] == list(range(11))
|
||||
assert [p['command'] for p in points if p['direction'] == 'down'] == list(range(255, 226, -1))
|
||||
assert all(len(set(event['target'][2:6])) == 1 for event in events
|
||||
if event['kind'] == 'command' and event['state'] == 'SCANNING')
|
||||
starts = [event for event in events if event['kind'] == 'segment_started']
|
||||
expected_segments = ['group_lower', 'group_upper', 'deferred_lower']
|
||||
if middle_min + 1 > 80:
|
||||
expected_segments.append('deferred_extension')
|
||||
assert [event['segment'] for event in starts] == expected_segments
|
||||
first_targets = {name: next(p['target'][2:6] for p in points if p['segment'] == name)
|
||||
for name in expected_segments}
|
||||
assert first_targets['group_lower'] == [0, 80, 0, 0]
|
||||
assert first_targets['group_upper'] == [255, 255, 255, 255]
|
||||
assert first_targets['deferred_lower'] == [0, 0, 0, 0]
|
||||
if 'deferred_extension' in first_targets:
|
||||
assert first_targets['deferred_extension'] == [0, 80, 0, 0]
|
||||
for point in points:
|
||||
index, middle, ring, pinky = point['target'][2:6]
|
||||
assert index == ring == pinky
|
||||
if point['segment'] == 'deferred_lower':
|
||||
assert index == 0
|
||||
assert set(point['observations']) == {'middle_mcp_roll'}
|
||||
else:
|
||||
assert 80 <= middle <= 255
|
||||
assert abs(middle - (80 + 175 * index / 255)) <= .5
|
||||
if point['segment'] == 'group_lower':
|
||||
assert 'middle_mcp_roll' not in point['search_commands']
|
||||
# The middle finger's fixed reference survives the split, with
|
||||
# exactly one observation for each command despite repeated group setpoints.
|
||||
for name, (lo, hi) in zip(engine.task.joints, bounds):
|
||||
for direction, expected in [('up', list(range(lo + 2))),
|
||||
('down', list(range(255, hi - 2, -1)))]:
|
||||
commands = [p['search_commands'][name] for p in points
|
||||
if p['direction'] == direction and name in p['search_commands']]
|
||||
assert commands == expected
|
||||
positioned = next(i for i, e in enumerate(events) if e['kind'] == 'segment_positioned')
|
||||
assert events[positioned]['target'][2:6] == [0, 80, 0, 0]
|
||||
solo_sample = next(i for i, e in enumerate(events)
|
||||
if e['kind'] == 'sample' and e['segment'] == 'deferred_lower')
|
||||
return_targets = [e['target'][2:6] for e in events[positioned:solo_sample] if e['kind'] == 'command']
|
||||
assert return_targets and return_targets[-1] == [0, 0, 0, 0]
|
||||
assert all(index == ring == pinky == 0 for index, middle, ring, pinky in return_targets)
|
||||
assert all(len(e['target']) == 20 and e['target'][:2] + e['target'][6:] == [64] * 16
|
||||
for e in events if e['kind'] == 'command' and e['state'] == 'SCANNING')
|
||||
assert engine.progress == (8, 8)
|
||||
assert replay(session.directory / 'samples.jsonl', tmp_path / 'recomputed.json') == session.document()
|
||||
|
||||
@@ -194,11 +237,87 @@ def test_stationary_finger_fails_without_repeating_its_search_at_other_end(tmp_p
|
||||
assert engine.results[name]['min'] is None and engine.results[name]['max'] is None
|
||||
assert all(engine.results[other]['status'] == '成功' for other in engine.task.joints[1:])
|
||||
points = [event for event in events if event['kind'] == 'sample']
|
||||
assert len([p for p in points if p['direction'] == 'up']) == 256
|
||||
assert len([p for p in points if p['direction'] == 'down']) == 16
|
||||
assert len([p for p in points if p['segment'] == 'group_lower']) == 256
|
||||
assert not any(name in p['search_commands'] for p in points if p['direction'] == 'down')
|
||||
assert replay(session.directory / 'samples.jsonl', tmp_path / 'recomputed.json') == session.document()
|
||||
|
||||
|
||||
def test_middle_upper_is_not_fabricated_when_motion_is_below_split(tmp_path):
|
||||
engine, session, events = run_scan(tmp_path, 'four_finger_roll', [(6, 243), (7, 70), (6, 243), (6, 243)])
|
||||
assert session.document()['joints']['middle_mcp_roll'] == {'min': None, 'max': None}
|
||||
assert '255→80' in engine.results['middle_mcp_roll']['reason']
|
||||
assert all(engine.results[name]['status'] == '成功' for name in engine.task.joints if name != 'middle_mcp_roll')
|
||||
assert not any(e['kind'] == 'segment_started' and e['segment'].startswith('deferred') for e in events)
|
||||
assert replay(session.directory / 'samples.jsonl', tmp_path / 'recomputed.json') == session.document()
|
||||
|
||||
|
||||
def test_middle_alone_ignores_other_tags_and_extension_requires_group_again():
|
||||
profile = load_profile()
|
||||
adapter = FakeAdapter(profile)
|
||||
adapter.ranges['middle_mcp_roll'] = (100, 243)
|
||||
settings = Settings(rate=10000, stable_frames=4, settle_seconds=.01)
|
||||
engine = Engine(profile, settings, adapter)
|
||||
teaching = simulated_teaching(profile, adapter.uid)
|
||||
now = 1.0
|
||||
adapter.advance(now, int(now * 1e9))
|
||||
engine.start(teaching, now, int(now * 1e9), ['four_finger_roll'])
|
||||
solo_seen = False
|
||||
for _ in range(2000):
|
||||
now += .1
|
||||
stamp = round(now * 1e9)
|
||||
adapter.advance(now, stamp)
|
||||
frame = adapter.frames()['front']
|
||||
if engine.segment and engine.segment.name == 'deferred_lower':
|
||||
solo_seen = True
|
||||
frame = {3: frame[3]}
|
||||
engine.observe('front', stamp, frame, now)
|
||||
engine.tick(now, stamp)
|
||||
assert engine.state != 'PAUSED', engine.reason
|
||||
if engine.segment and engine.segment.name == 'deferred_extension':
|
||||
break
|
||||
else:
|
||||
pytest.fail('未完成中指单独扫描')
|
||||
assert solo_seen and engine.searches['middle_mcp_roll'].steps == 81
|
||||
assert (0, 'up') not in engine.endpoint_results['middle_mcp_roll']
|
||||
before = len(adapter.sent)
|
||||
for _ in range(round((settings.point_timeout + 1) / .1)):
|
||||
now += .1
|
||||
stamp = round(now * 1e9)
|
||||
adapter.advance(now, stamp)
|
||||
frame = adapter.frames()['front']
|
||||
frame.pop(1)
|
||||
engine.observe('front', stamp, frame, now)
|
||||
engine.tick(now, stamp)
|
||||
assert engine.state == 'PAUSED' and len(adapter.sent) == before
|
||||
engine.resume(now, stamp)
|
||||
assert engine.state == 'PREPARING' and engine.progress == (0, 8)
|
||||
assert not engine.search_bank and not engine.samples and engine.segment is None
|
||||
assert set(engine.keys) == set(engine.task.joints)
|
||||
|
||||
|
||||
def test_group_positioning_preserves_progress_and_rate_after_a_hold():
|
||||
profile = load_profile()
|
||||
start, end = [64] * 20, [64] * 20
|
||||
start[2:6], end[2:6] = [0, 80, 0, 0], [255] * 4
|
||||
trajectory = Trajectory(profile, start, end, rate=20, now=0, coordinated=True)
|
||||
now, previous = 0.0, tuple(start)
|
||||
for step in range(200):
|
||||
now += .1
|
||||
values, done = trajectory.advance(now)
|
||||
assert all(abs(value - old) <= 2 for value, old in zip(values, previous))
|
||||
index, middle, ring, pinky = values[2:6]
|
||||
assert index == ring == pinky
|
||||
assert abs(middle - (80 + 175 * index / 255)) <= .5
|
||||
if step == 20:
|
||||
now += 5
|
||||
held, done = trajectory.advance(now, hold=True)
|
||||
assert held == values and not done
|
||||
previous = values
|
||||
if done:
|
||||
break
|
||||
assert done and values == tuple(end)
|
||||
|
||||
|
||||
def test_fully_stationary_task_finishes_with_null_range(tmp_path):
|
||||
engine, session, events = run_scan(tmp_path, 'thumb_cmc_roll', [(0, 0)])
|
||||
assert engine.results['thumb_cmc_roll']['status'] == '失败'
|
||||
@@ -223,6 +342,61 @@ def test_live_optional_repetitions_and_fractional_non_o30_domain(tmp_path):
|
||||
assert replay(session.directory / 'samples.jsonl', tmp_path / 'recomputed.json') == session.document()
|
||||
|
||||
|
||||
def test_deferred_plan_is_generic_and_repeats_complete_sequence(tmp_path):
|
||||
data = json.loads(json.dumps(load_profile().raw))
|
||||
data.update(model='VIRTUAL', command_unit='native', command={'minimum': -2, 'maximum': 8, 'resolution': .5})
|
||||
data['joints'] = data['joints'][:2]
|
||||
for index, name in enumerate(('alpha', 'beta')):
|
||||
data['joints'][index].update(name=name, sdk_name=name, index=index)
|
||||
data['tasks'] = [{'name': 'virtual_pair', 'joints': ['alpha', 'beta'],
|
||||
'deferred_lower': {'joint': 'beta', 'split': 2}}]
|
||||
data['clearances'] = {}
|
||||
profile = profile_from_dict(data)
|
||||
settings = Settings(rate=10000, stable_frames=4, settle_seconds=.01,
|
||||
endpoint_repetitions=2, motion_floor_px=.2)
|
||||
engine, session, events = run_scan(tmp_path, 'virtual_pair', [(-1, 7), (3, 7)], settings, profile)
|
||||
assert session.document()['joints'] == {'alpha': {'min': -1, 'max': 7}, 'beta': {'min': 3, 'max': 7}}
|
||||
assert engine.progress == (8, 8)
|
||||
stages = ['group_lower', 'group_upper', 'deferred_lower', 'deferred_extension']
|
||||
assert [(e['repeat'], e['segment']) for e in events if e['kind'] == 'segment_started'] == [
|
||||
(r, stage) for r in range(2) for stage in stages]
|
||||
assert replay(session.directory / 'samples.jsonl', tmp_path / 'recomputed.json') == session.document()
|
||||
|
||||
|
||||
@pytest.mark.parametrize('joint, split', [('thumb_cmc_roll', 80), ('middle_mcp_roll', -1),
|
||||
('middle_mcp_roll', 0), ('middle_mcp_roll', 255),
|
||||
('middle_mcp_roll', 80.5)])
|
||||
def test_deferred_configuration_rejects_invalid_channel_and_split(joint, split):
|
||||
data = json.loads(json.dumps(load_profile().raw))
|
||||
task = next(t for t in data['tasks'] if t['name'] == 'four_finger_roll')
|
||||
task['deferred_lower'] = {'joint': joint, 'split': split}
|
||||
with pytest.raises(ValueError):
|
||||
profile_from_dict(data)
|
||||
|
||||
|
||||
def test_old_same_command_endpoint_journal_still_replays(tmp_path):
|
||||
profile, settings = load_profile(), Settings()
|
||||
task = next(t for t in profile.tasks if t.name == 'four_finger_roll')
|
||||
raw = json.loads(json.dumps(profile.raw))
|
||||
next(t for t in raw['tasks'] if t['name'] == task.name).pop('deferred_lower')
|
||||
events = [
|
||||
{'kind': 'metadata', 'profile': raw, 'settings': asdict(settings), 'uid': 'OLD_ENDPOINTS',
|
||||
'scan_method': 'endpoint_search_v1'},
|
||||
{'kind': 'task_started', 'task': task.name, 'joints': list(task.joints)},
|
||||
]
|
||||
bounds = [(6, 243), (7, 255), (0, 230), (0, 255)]
|
||||
for direction, commands in [('up', range(11)), ('down', range(255, 226, -1))]:
|
||||
for command in commands:
|
||||
observations = {name: observed(float(np.clip(command, *pair))) for name, pair in zip(task.joints, bounds)}
|
||||
events.append({'kind': 'sample', 'task': task.name, 'stage': 'endpoint', 'repeat': 0,
|
||||
'direction': direction, 'command': command, 'observations': observations})
|
||||
events.append({'kind': 'task_result', 'task': task.name})
|
||||
journal = tmp_path / 'old_endpoints.jsonl'
|
||||
journal.write_text('\n'.join(json.dumps(event) for event in events))
|
||||
document = replay(journal, tmp_path / 'recomputed.json')
|
||||
assert all(document['joints'][name] == {'min': lo, 'max': hi} for name, (lo, hi) in zip(task.joints, bounds))
|
||||
|
||||
|
||||
def test_legacy_full_sweep_journal_still_replays(tmp_path):
|
||||
profile, settings = load_profile(), Settings()
|
||||
joint, task = profile.joints[0], profile.tasks[0]
|
||||
|
||||
@@ -21,7 +21,7 @@ from linkerhand_range_calibration.ui.window import CalibrationWindow
|
||||
rclpy.init();app=QApplication([])
|
||||
station=load_station();station['output_root']=str(Path(sys.argv[1])/'output')
|
||||
station['teaching_root']=str(Path(sys.argv[1])/'teaching')
|
||||
station['scan'].update(rate=1000,stable_frames=4,settle_seconds=.01)
|
||||
station['scan'].update(rate=1000,stable_frames=4,stable_seconds=.01,settle_seconds=.01)
|
||||
node=CalibrationRuntime(load_profile(),station,demo=True)
|
||||
window=CalibrationWindow(node);window.show()
|
||||
def until(condition,timeout=6):
|
||||
@@ -201,6 +201,43 @@ until(lambda:node.engine.state=='COMPLETED')
|
||||
assert first.slider.value()==node.teaching.data['baseline']['target'][0]
|
||||
assert index_control.slider.value()==node.teaching.data['baseline']['target'][2]
|
||||
assert all(control.isEnabled() for control in window.joint_controls)
|
||||
|
||||
# The phased group shows all four actual targets, including the raised middle start.
|
||||
window.task.setCurrentIndex(window.task.findData('four_finger_roll'))
|
||||
assert '分阶段' in window.task.currentText()
|
||||
window.start_one.click()
|
||||
until(lambda:node.engine.state=='SCANNING' and node.engine.phase=='waiting')
|
||||
assert node.engine.command[2:6]==(0,80,0,0)
|
||||
assert node.snapshot()['scan_commands']['middle_mcp_roll']==80
|
||||
assert '同步测其余关节下限' in window.status.text()
|
||||
assert all(text in window.status.text() for text in ['食指=0','中指=80','无名指=0','小指=0'])
|
||||
assert not any(control.isEnabled() for control in window.joint_controls)
|
||||
|
||||
# The ROS detection callback preserves the rejected Tag's reason through UI and journal.
|
||||
from types import SimpleNamespace as NS
|
||||
node.camera_info['front']=(1624,1240)
|
||||
message=NS(header=NS(stamp=node.get_clock().now().to_msg()),detections=[])
|
||||
for tag,points in node.adapter.frames()['front'].items():
|
||||
message.detections.append(NS(id=tag,family='36h11',hamming=0,
|
||||
decision_margin=0 if tag==2 else 100,corners=[NS(x=x,y=y) for x,y in points]))
|
||||
node._detections('front',message)
|
||||
node.engine._pause_sampling_timeout(time.monotonic())
|
||||
node._tick();window.refresh()
|
||||
assert '正面 Tag ID2(ring_mcp_roll):Tag 解码质量不足' in window.message.text()
|
||||
events=[json.loads(line) for line in (node.session.directory/'samples.jsonl').read_text().splitlines()]
|
||||
failure=next(event for event in reversed(events) if event['kind']=='sampling_timeout')
|
||||
assert any(issue.get('tag_id')==2 and issue['reason']=='Tag 解码质量不足'
|
||||
for issue in failure['issues'])
|
||||
|
||||
# Stream-level failures likewise identify affected tags from the current requirements.
|
||||
node.demo=False
|
||||
now=time.monotonic()
|
||||
node.camera_received['front']=now
|
||||
node.image_times['front']=now-2
|
||||
error=node._vision_error({'ring_mcp_roll':('front',2)},now)
|
||||
assert '正面 Tag ID2(ring_mcp_roll)' in error and '检测流未收到或断流' in error
|
||||
node.demo=True
|
||||
window.cancel.click();until(lambda:node.engine.state=='CANCELLED')
|
||||
window.close();node.close_session();node.destroy_node();rclpy.shutdown()
|
||||
print('GUI workflow passed')
|
||||
'''
|
||||
|
||||
@@ -0,0 +1,213 @@
|
||||
"""Quiet references and noise-aware transient evidence without direction rules."""
|
||||
from dataclasses import asdict
|
||||
import json
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from linkerhand_range_calibration.adapters.fake import FakeAdapter
|
||||
from linkerhand_range_calibration.core.endpoints import EndpointSearch
|
||||
from linkerhand_range_calibration.core.engine import Engine
|
||||
from linkerhand_range_calibration.profiles import Settings, load_profile
|
||||
from linkerhand_range_calibration.replay import replay
|
||||
from linkerhand_range_calibration.simulation import simulated_teaching
|
||||
from linkerhand_range_calibration.storage import Session
|
||||
from linkerhand_range_calibration.vision.motion import MotionTrace, transient_evidence
|
||||
from linkerhand_range_calibration.vision.observations import Observation, StableWindow, rms_delta
|
||||
from linkerhand_range_calibration.vision.reference import ReferenceWindow
|
||||
|
||||
|
||||
BASE = np.array([[100, 100], [140, 100], [140, 140], [100, 140]], dtype=float)
|
||||
|
||||
|
||||
def window_samples(noise=.2, offset=0, start=0):
|
||||
return [{'stamp_ns': start + i * 40_000_000,
|
||||
'corners': (BASE + [offset + noise * (-1) ** i, 0]).tolist()}
|
||||
for i in range(24)]
|
||||
|
||||
|
||||
def summary(noise=.2, offset=0):
|
||||
return {'corners': (BASE + [offset, 0]).tolist(), 'noise': noise,
|
||||
'window_samples': window_samples(noise, offset)}
|
||||
|
||||
|
||||
def trace(shifts):
|
||||
capture = MotionTrace(1_000_000_000)
|
||||
for i, shift in enumerate(shifts):
|
||||
stamp = 1_030_000_000 + i * 30_000_000
|
||||
capture.observe(stamp, None if shift is None else Observation(stamp, 0, BASE + [shift, 0]))
|
||||
return capture.snapshot()
|
||||
|
||||
|
||||
def test_two_frame_jitter_is_not_latched_as_motion_and_true_pulse_still_counts():
|
||||
settings, joint = Settings(), load_profile().joints[0]
|
||||
search = EndpointSearch(joint, 'up', settings)
|
||||
search.add(0, summary())
|
||||
search.add(1, {**summary(), 'motion_trace': trace([.55, .6, 0])})
|
||||
assert search.boundary is None
|
||||
# A new observation, not additional commanded travel, proves a real pulse.
|
||||
search.add(2, {**summary(), 'motion_trace': trace([2, -2, 0])})
|
||||
assert search.result()['bound'] == 1
|
||||
assert search.evidence['source'] == 'transient'
|
||||
assert search.evidence['transient']['threshold_px'] == pytest.approx(.9)
|
||||
|
||||
|
||||
def test_noise_at_current_point_is_checked_even_if_endpoint_was_quiet():
|
||||
search = EndpointSearch(load_profile().joints[0], 'down', Settings())
|
||||
search.add(255, summary(.02))
|
||||
search.add(254, {**summary(.4), 'motion_trace': trace([.65, .7, 0])})
|
||||
assert search.boundary is None
|
||||
|
||||
|
||||
@pytest.mark.parametrize('noise', [.02, .1, .3])
|
||||
def test_reference_frame_noise_does_not_raise_settled_motion_threshold(noise):
|
||||
search = EndpointSearch(load_profile().joints[0], 'up', Settings())
|
||||
search.add(0, summary(noise))
|
||||
search.add(1, summary(noise, .7))
|
||||
assert search.result()['bound'] == 0
|
||||
assert search.result()['threshold'] == .5
|
||||
assert search.evidence['source'] == 'settled'
|
||||
|
||||
|
||||
@pytest.mark.parametrize('frames', [[2, None, 2], [2, 0, 2]])
|
||||
def test_missing_or_stationary_frames_break_a_transient_sequence(frames):
|
||||
assert transient_evidence(trace(frames), BASE, .1, window_samples(), Settings()) is None
|
||||
|
||||
|
||||
def test_old_frames_and_duplicate_timestamps_cannot_prove_motion():
|
||||
capture = MotionTrace(1_000_000_000)
|
||||
for stamp in (900_000_000, 1_030_000_000, 1_030_000_000):
|
||||
capture.observe(stamp, Observation(stamp, 0, BASE + 2))
|
||||
assert len(capture.frames) == 1
|
||||
assert transient_evidence(capture.snapshot(), BASE, 0, window_samples(0), Settings()) is None
|
||||
|
||||
|
||||
def test_endpoint_reference_waits_through_late_settling_before_advancing():
|
||||
old = StableWindow(8, .5)
|
||||
current = ReferenceWindow(Settings())
|
||||
first_old = first_new = None
|
||||
for i in range(100):
|
||||
now = i / 30
|
||||
offset = np.clip((now - .8) / .4, 0, 1) * .95
|
||||
observation = Observation(round(now * 1e9), now, BASE + [offset, 0])
|
||||
old.add(observation)
|
||||
current.add(observation)
|
||||
if first_old is None and old.summary(.5, .1):
|
||||
first_old = now
|
||||
measured = current.summary(.5, .1)
|
||||
if first_new is None and measured:
|
||||
first_new = now
|
||||
assert np.asarray(measured['corners']) == pytest.approx(BASE + [.95, 0])
|
||||
assert first_old < .8 and first_new > 1.5
|
||||
|
||||
|
||||
def test_reference_timeout_explains_position_change_despite_local_quietness():
|
||||
window = ReferenceWindow(Settings(stability_drift_sigma=100))
|
||||
for i in range(49):
|
||||
offset = (i // 17) * .4 + .1 * (-1) ** i
|
||||
stamp = round(i / 30 * 1e9)
|
||||
window.add(Observation(stamp, 0, BASE + [offset, 0]))
|
||||
problem = window.problem(.5, .1, 100)
|
||||
assert problem and '参考尚未稳定' in problem['reason']
|
||||
|
||||
|
||||
def test_stationary_alternating_jitter_does_not_move_when_frame_parity_changes():
|
||||
reference = ReferenceWindow(Settings())
|
||||
point = StableWindow(8, .5)
|
||||
arrays = []
|
||||
for window, count, duration, phase in [(reference, 45, 1.5, 1), (point, 15, .5, -1)]:
|
||||
points = []
|
||||
for i in range(count):
|
||||
stamp = round(i / (count - 1) * duration * 1e9)
|
||||
corners = BASE + [.3 * phase * (-1) ** i, 0]
|
||||
window.add(Observation(stamp, 0, corners))
|
||||
points.append(corners)
|
||||
arrays.append(points)
|
||||
# Previously two accepted windows could have medians .6 px apart, despite
|
||||
# identical stationary jitter and no actual commanded displacement.
|
||||
assert rms_delta(np.median(arrays[0], axis=0), np.median(arrays[1], axis=0)) > .5
|
||||
baseline, current = reference.summary(.5, .1), point.summary(.5, .1)
|
||||
assert baseline is not None and current is not None
|
||||
assert rms_delta(baseline['corners'], current['corners']) < .1
|
||||
search = EndpointSearch(load_profile().joints[0], 'up', Settings())
|
||||
search.add(0, {**baseline, 'window_samples': reference.snapshot()})
|
||||
search.add(1, {**current, 'window_samples': point.snapshot()})
|
||||
assert search.boundary is None
|
||||
|
||||
|
||||
@pytest.mark.parametrize('seed', [2, 6, 19])
|
||||
def test_one_pass_four_fingers_with_late_settling_noise_and_replay(tmp_path, seed):
|
||||
profile = load_profile()
|
||||
names = ['index_mcp_roll', 'middle_mcp_roll', 'ring_mcp_roll', 'pinky_mcp_roll']
|
||||
bounds = dict(zip(names, [(6, 255), (4, 255), (5, 255), (6, 255)]))
|
||||
|
||||
class SettlingHand(FakeAdapter):
|
||||
"""Known time-driven relaxation, not reconstructed unrecorded video."""
|
||||
def __init__(self):
|
||||
super().__init__(profile, {j.name: bounds.get(j.name, (0, 255)) for j in profile.joints})
|
||||
self.now, self.endpoint_at = 0, {}
|
||||
self.rng = np.random.default_rng(seed)
|
||||
|
||||
def advance(self, now, stamp_ns):
|
||||
self.now = now
|
||||
super().advance(now, stamp_ns)
|
||||
|
||||
def send_positions(self, target):
|
||||
for name in names:
|
||||
index = profile.by_name[name].index
|
||||
if target[index] in (0, 255) and target[index] != self.target[index]:
|
||||
self.endpoint_at[name] = self.now
|
||||
super().send_positions(target)
|
||||
|
||||
def frames(self):
|
||||
frames = super().frames(noise=.1, rng=self.rng)
|
||||
for name, started in self.endpoint_at.items():
|
||||
joint = profile.by_name[name]
|
||||
offset = .95 * np.clip((self.now - started - .8) / .4, 0, 1)
|
||||
frames[joint.view][joint.tag_id] = (np.asarray(frames[joint.view][joint.tag_id]) + [offset, 0]).tolist()
|
||||
return frames
|
||||
|
||||
settings = Settings(rate=10000)
|
||||
adapter = SettlingHand()
|
||||
teaching = simulated_teaching(profile, adapter.uid)
|
||||
session = Session(tmp_path, profile, adapter.uid, asdict(settings), teaching, {})
|
||||
engine = Engine(profile, settings, adapter, session.emit)
|
||||
engine.start(teaching, 1, 1_000_000_000, ['four_finger_roll'])
|
||||
try:
|
||||
for tick in range(1, 8000):
|
||||
now = 1 + tick / 30
|
||||
stamp = round(now * 1e9)
|
||||
adapter.advance(now, stamp)
|
||||
for view, frame in adapter.frames().items():
|
||||
engine.observe(view, stamp, frame, now)
|
||||
engine.tick(now, stamp)
|
||||
assert engine.state != 'PAUSED', engine.reason
|
||||
if engine.state == 'COMPLETED':
|
||||
break
|
||||
assert engine.state == 'COMPLETED'
|
||||
finally:
|
||||
session.close()
|
||||
assert settings.endpoint_repetitions == 1
|
||||
for name, (lo, hi) in bounds.items():
|
||||
assert session.document()['joints'][name] == {'min': lo, 'max': hi}
|
||||
assert replay(session.directory / 'samples.jsonl', tmp_path / 'replayed.json') == session.document()
|
||||
events = [json.loads(line) for line in (session.directory / 'samples.jsonl').read_text().splitlines()]
|
||||
references = [e for e in events if e['kind'] == 'sample' and e['command'] in (0, 255)]
|
||||
for event in references:
|
||||
for name in event['search_commands']:
|
||||
observed = event['observations'][name]
|
||||
assert observed['last_stamp_ns'] - observed['first_stamp_ns'] >= 1_500_000_000
|
||||
# Replay uses the saved raw window, not a stale/tampered summary coordinate.
|
||||
for event in events:
|
||||
if event['kind'] == 'sample':
|
||||
for observed in event['observations'].values():
|
||||
observed['corners'] = BASE.tolist()
|
||||
altered = tmp_path / 'altered.jsonl'
|
||||
altered.write_text('\n'.join(json.dumps(e) for e in events))
|
||||
assert replay(altered, tmp_path / 'replayed_altered.json') == session.document()
|
||||
|
||||
|
||||
def test_reference_duration_and_timeout_configuration_are_validated():
|
||||
for change in ({'reference_seconds': .1}, {'point_timeout': 1}):
|
||||
with pytest.raises(ValueError, match='参考观察时长'):
|
||||
Settings.from_dict(change)
|
||||
@@ -0,0 +1,160 @@
|
||||
"""Historical platform recovery stays reproducible; live references stay fixed."""
|
||||
from dataclasses import asdict
|
||||
import json
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from linkerhand_range_calibration.core.legacy_endpoints import SCAN_METHOD, EndpointSearch
|
||||
from linkerhand_range_calibration.core.engine import Engine
|
||||
from linkerhand_range_calibration.adapters.fake import FakeAdapter
|
||||
from linkerhand_range_calibration.profiles import Settings, load_profile
|
||||
from linkerhand_range_calibration.replay import replay
|
||||
from linkerhand_range_calibration.simulation import simulated_teaching
|
||||
from linkerhand_range_calibration.storage import Session
|
||||
|
||||
|
||||
def recorded_samples():
|
||||
path = Path(__file__).parent / 'fixtures' / 'middle_subthreshold_startup_shift.json'
|
||||
return json.loads(path.read_text())['samples']
|
||||
|
||||
|
||||
def test_recorded_middle_departure_was_cancelled_by_its_initial_reference():
|
||||
samples = recorded_samples()
|
||||
joint = load_profile().by_name['middle_mcp_roll']
|
||||
searches = {method: EndpointSearch(joint, 'up', Settings(), method)
|
||||
for method in ('endpoint_search_v4', SCAN_METHOD)}
|
||||
for sample in samples:
|
||||
for search in searches.values():
|
||||
search.add(sample['command'], sample['observation'])
|
||||
assert searches['endpoint_search_v4'].result()['bound'] == 8
|
||||
current = searches[SCAN_METHOD]
|
||||
assert current.result()['trigger'] == 6
|
||||
assert current.result()['bound'] == 5
|
||||
assert current.result()['confirmed_at'] == 9
|
||||
rebased, = current.rejections
|
||||
assert rebased['trigger'] is None
|
||||
assert rebased['initial_offset_px'] < rebased['threshold']
|
||||
assert rebased['reference_commands'] == [3, 4, 5]
|
||||
# The manual report of command 5 is not substituted for recorded evidence.
|
||||
assert samples[5]['feedback'] == 0 and samples[6]['feedback'] == 4
|
||||
|
||||
|
||||
def observation(x, rotation=False):
|
||||
square = np.array([[100, 100], [140, 100], [140, 140], [100, 140]], dtype=float)
|
||||
if rotation:
|
||||
angle = x / np.sqrt(800)
|
||||
matrix = np.array([[np.cos(angle), -np.sin(angle)], [np.sin(angle), np.cos(angle)]])
|
||||
points = (square - 120) @ matrix + 120
|
||||
else:
|
||||
points = square + [x, 0]
|
||||
return {'corners': points.tolist(), 'noise': .16}
|
||||
|
||||
|
||||
@pytest.mark.parametrize('direction', ['up', 'down'])
|
||||
@pytest.mark.parametrize('rotation', [False, True])
|
||||
def test_first_departure_at_step_five_is_kept_after_later_confirmation(direction, rotation):
|
||||
search = EndpointSearch(load_profile().joints[0], direction, Settings())
|
||||
for step in range(15):
|
||||
# The .65 offset is below the .8 threshold; the first real 1.15 step
|
||||
# returns towards the original reference before moving further later.
|
||||
x = 0 if step == 0 else .65 if step < 5 else -.5 - 1.15 * ((step - 5) // 4)
|
||||
command = step if direction == 'up' else 255 - step
|
||||
search.add(command, observation(x, rotation))
|
||||
if search.done:
|
||||
break
|
||||
assert search.result()['trigger'] == (5 if direction == 'up' else 250)
|
||||
assert search.result()['bound'] == (4 if direction == 'up' else 251)
|
||||
assert search.result()['confirmed_at'] == (9 if direction == 'up' else 246)
|
||||
assert search.rejections[0]['trigger'] is None
|
||||
|
||||
|
||||
def test_reference_does_not_follow_small_same_direction_motion():
|
||||
search = EndpointSearch(load_profile().joints[0], 'up', Settings())
|
||||
for command in range(30):
|
||||
search.add(command, observation(max(0, command - 4) * .1))
|
||||
if search.done:
|
||||
break
|
||||
assert search.boundary is not None
|
||||
assert not search.rejections
|
||||
assert search.reference == pytest.approx(np.asarray(observation(0)['corners']))
|
||||
|
||||
|
||||
def test_one_reverse_displacement_without_additional_motion_is_not_confirmed():
|
||||
search = EndpointSearch(load_profile().joints[0], 'up', Settings())
|
||||
for command in range(256):
|
||||
x = 0 if command == 0 else .65 if command < 5 else -.5
|
||||
search.add(command, observation(x))
|
||||
assert search.boundary is None and search.error
|
||||
assert len(search.rejections) == 1
|
||||
|
||||
|
||||
def test_live_group_uses_its_original_middle_reference(tmp_path):
|
||||
class MiddleOffsetAdapter(FakeAdapter):
|
||||
def advance(self, now, stamp_ns):
|
||||
super().advance(now, stamp_ns)
|
||||
self.tick = round(now * 10)
|
||||
|
||||
def frames(self):
|
||||
frames = super().frames()
|
||||
command = self.target[3]
|
||||
x = 0 if command == 0 else .65 if command < 5 else -.5 - 1.15 * ((command - 5) // 4)
|
||||
frames['front'][3] = observation(x + .1 * (-1) ** self.tick)['corners']
|
||||
return frames
|
||||
|
||||
profile = load_profile()
|
||||
settings = Settings(rate=10000, stable_frames=4, settle_seconds=.01, motion_floor_px=.8)
|
||||
adapter = MiddleOffsetAdapter(profile)
|
||||
teaching = simulated_teaching(profile, adapter.uid)
|
||||
session = Session(tmp_path, profile, adapter.uid, asdict(settings), teaching, {'simulated': True})
|
||||
engine = Engine(profile, settings, adapter, session.emit)
|
||||
engine.start(teaching, 1.0, 1_000_000_000, ['four_finger_roll'])
|
||||
try:
|
||||
for tick in range(1, 3000):
|
||||
now = 1 + tick / 10
|
||||
stamp = round(now * 1e9)
|
||||
adapter.advance(now, stamp)
|
||||
for view, frame in adapter.frames().items():
|
||||
engine.observe(view, stamp, frame, now)
|
||||
engine.tick(now, stamp)
|
||||
assert engine.state != 'PAUSED', engine.reason
|
||||
if engine.state == 'COMPLETED':
|
||||
break
|
||||
assert engine.state == 'COMPLETED'
|
||||
finally:
|
||||
session.close()
|
||||
# Set the .8 threshold explicitly: new live scans no longer derive it from
|
||||
# one noisy reference. This test concerns fixed-reference semantics only.
|
||||
# The .65 initial offset and -.5 position remain inside the .8 threshold.
|
||||
# With a fixed reference, command 9 first exceeds it; no rebase is inferred.
|
||||
assert engine.results['middle_mcp_roll']['min'] == 8
|
||||
journal = session.directory / 'samples.jsonl'
|
||||
events = [json.loads(line) for line in journal.read_text().splitlines()]
|
||||
assert not any(event['kind'] == 'endpoint_reference_rebased' for event in events)
|
||||
assert replay(journal, tmp_path / 'recomputed.json') == session.document()
|
||||
|
||||
|
||||
@pytest.mark.parametrize('method,expected_min', [('endpoint_search_v4', 8), (SCAN_METHOD, 5)])
|
||||
def test_offline_replay_respects_recorded_method(tmp_path, method, expected_min):
|
||||
profile = load_profile()
|
||||
task = next(task for task in profile.tasks if task.name == 'four_finger_roll')
|
||||
name = 'middle_mcp_roll'
|
||||
events = [
|
||||
{'kind': 'metadata', 'profile': profile.raw, 'settings': asdict(Settings()),
|
||||
'uid': 'RECORDED_MIDDLE', 'scan_method': method},
|
||||
{'kind': 'task_started', 'task': task.name, 'joints': list(task.joints)},
|
||||
]
|
||||
for sample in recorded_samples():
|
||||
events.append({'kind': 'sample', 'task': task.name, 'stage': 'endpoint', 'repeat': 0,
|
||||
'direction': 'up', 'search_commands': {name: sample['command']},
|
||||
'observations': {name: sample['observation']}})
|
||||
for command in range(255, 250, -1):
|
||||
events.append({'kind': 'sample', 'task': task.name, 'stage': 'endpoint', 'repeat': 0,
|
||||
'direction': 'down', 'search_commands': {name: command},
|
||||
'observations': {name: observation((255 - command) * 2)}})
|
||||
events.append({'kind': 'task_result', 'task': task.name})
|
||||
journal = tmp_path / 'recorded.jsonl'
|
||||
journal.write_text('\n'.join(json.dumps(event) for event in events))
|
||||
document = replay(journal, tmp_path / 'recomputed.json')
|
||||
assert document['joints'][name] == {'min': expected_min, 'max': 255}
|
||||
@@ -0,0 +1,209 @@
|
||||
"""Timeout explanations must identify actual blockers before windows are cleared."""
|
||||
from dataclasses import replace
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from linkerhand_range_calibration.adapters.base import Feedback
|
||||
from linkerhand_range_calibration.adapters.fake import FakeAdapter
|
||||
from linkerhand_range_calibration.core.engine import Engine
|
||||
from linkerhand_range_calibration.profiles import Settings, load_profile
|
||||
from linkerhand_range_calibration.simulation import simulated_teaching
|
||||
from linkerhand_range_calibration.vision.observations import Observation, StableWindow
|
||||
from linkerhand_range_calibration.vision.reference import ReferenceWindow
|
||||
|
||||
|
||||
class SamplingRun:
|
||||
def __init__(self, task='thumb_cmc_roll', phase='waiting'):
|
||||
profile = load_profile()
|
||||
settings = Settings(rate=1000, stable_frames=4, stable_seconds=.15, reference_seconds=.45,
|
||||
settle_seconds=.05, point_timeout=.8)
|
||||
self.adapter = FakeAdapter(profile)
|
||||
self.events, self.now = [], 1.0
|
||||
self.engine = Engine(profile, settings, self.adapter, self.events.append)
|
||||
self.engine.start(simulated_teaching(profile, self.adapter.uid), self.now,
|
||||
round(self.now * 1e9), [task])
|
||||
for _ in range(100):
|
||||
self.step()
|
||||
if self.engine.state == 'SCANNING' and self.engine.phase == phase:
|
||||
break
|
||||
assert self.engine.state == 'SCANNING' and self.engine.phase == phase
|
||||
|
||||
def step(self, change=None):
|
||||
self.now = round(self.now + .05, 6)
|
||||
stamp = round(self.now * 1e9)
|
||||
self.adapter.advance(self.now, stamp)
|
||||
frames, rejected = self.adapter.frames(), {}
|
||||
if change:
|
||||
change(frames, rejected)
|
||||
for view, frame in frames.items():
|
||||
self.engine.observe(view, stamp, frame, self.now, rejected.get(view))
|
||||
self.engine.tick(self.now, stamp)
|
||||
|
||||
def timeout(self, change):
|
||||
before = len(self.adapter.sent)
|
||||
for _ in range(40):
|
||||
self.step(change)
|
||||
if self.engine.state == 'PAUSED':
|
||||
break
|
||||
assert self.engine.state == 'PAUSED', self.engine.reason
|
||||
assert len(self.adapter.sent) == before # A diagnostic must not move the hand.
|
||||
assert all(not window.values for window in self.engine.windows.values())
|
||||
return next(event for event in reversed(self.events) if event['kind'] == 'sampling_timeout')
|
||||
|
||||
|
||||
@pytest.mark.parametrize('task,noisy,expected', [
|
||||
('thumb_cmc_roll', [0], {'thumb_cmc_roll'}),
|
||||
('four_finger_roll', [2], {'ring_mcp_roll'}),
|
||||
('four_finger_roll', [2, 4], {'ring_mcp_roll', 'index_mcp_roll'}),
|
||||
])
|
||||
def test_current_unstable_metrics_identify_only_affected_tags(task, noisy, expected):
|
||||
run = SamplingRun(task)
|
||||
|
||||
def creep(frames, rejected):
|
||||
for tag in noisy:
|
||||
frames['front'][tag] = (np.asarray(frames['front'][tag]) + [run.now * 8, 0]).tolist()
|
||||
|
||||
event = run.timeout(creep)
|
||||
assert {issue['joint'] for issue in event['issues']} == expected
|
||||
for issue in event['issues']:
|
||||
assert f'正面 Tag ID{issue["tag_id"]}({issue["joint"]})' in run.engine.reason
|
||||
assert issue['drift_px'] > issue['drift_limit_px']
|
||||
assert issue['jitter_px'] < issue['noise_limit_px']
|
||||
assert '持续漂移' in issue['reason'] and '随机抖动' not in issue['reason']
|
||||
assert issue['last_stamp_ns'] == round(run.now * 1e9)
|
||||
assert len(issue['window_samples']) == issue['frames']
|
||||
# These failures occur while establishing the endpoint reference, whose
|
||||
# window is longer than a normal command-point window.
|
||||
recovered = (ReferenceWindow(run.engine.settings) if issue['joint'] in run.engine.searches
|
||||
else StableWindow(run.engine.settings.stable_frames, run.engine.settings.stable_seconds))
|
||||
for frame in issue['window_samples']:
|
||||
recovered.add(Observation(frame['stamp_ns'], 0, frame['corners']))
|
||||
assert recovered.problem(.5, .1)['drift_px'] == pytest.approx(issue['drift_px'])
|
||||
assert event['target'] == list(run.engine.command)
|
||||
|
||||
|
||||
def test_four_finger_sampling_advances_with_random_jitter_above_old_limit():
|
||||
run = SamplingRun('four_finger_roll')
|
||||
|
||||
def jitter(frames, rejected):
|
||||
offset = .25 * (-1) ** round(run.now * 20)
|
||||
for tag in (1, 2, 3, 4):
|
||||
frames['front'][tag] = (np.asarray(frames['front'][tag]) + [offset, 0]).tolist()
|
||||
|
||||
for _ in range(15):
|
||||
run.step(jitter)
|
||||
assert run.engine.state != 'PAUSED', run.engine.reason
|
||||
samples = [event for event in run.events if event['kind'] == 'sample']
|
||||
if samples:
|
||||
break
|
||||
assert samples
|
||||
for observation in samples[0]['observations'].values():
|
||||
assert .1 < observation['jitter_px'] <= .5
|
||||
assert observation['drift_px'] <= .1
|
||||
|
||||
|
||||
@pytest.mark.parametrize('deadline,settles_at', [(3, 4), (6, 4), (6, 0), (6, None)])
|
||||
def test_waits_for_transient_vibration_without_advancing_or_relaxing_limits(deadline, settles_at):
|
||||
run = SamplingRun('four_finger_roll')
|
||||
run.engine.settings = replace(run.engine.settings, point_timeout=deadline)
|
||||
started, sent = run.now, len(run.adapter.sent)
|
||||
|
||||
def shake(frames, rejected):
|
||||
# Synthetic transient: unlike the recorded fixture, this deliberately
|
||||
# supplies a known recovery after four seconds to exercise the deadline.
|
||||
if settles_at is None or run.now - started < settles_at:
|
||||
offset = .85 * (-1) ** round(run.now * 20)
|
||||
for tag in (1, 2, 3, 4):
|
||||
frames['front'][tag] = (np.asarray(frames['front'][tag]) + [offset, 0]).tolist()
|
||||
|
||||
for _ in range(140):
|
||||
run.step(shake)
|
||||
samples = [event for event in run.events if event['kind'] == 'sample']
|
||||
if samples or run.engine.state == 'PAUSED':
|
||||
break
|
||||
assert len(run.adapter.sent) == sent
|
||||
if settles_at is None or settles_at >= deadline:
|
||||
assert run.engine.state == 'PAUSED' and not samples
|
||||
assert deadline < run.now - started <= deadline + .1
|
||||
timeout = next(event for event in reversed(run.events) if event['kind'] == 'sampling_timeout')
|
||||
assert '共同平移' in run.engine.reason
|
||||
assert timeout['shared_motion'][0]['tag_ids'] == [1, 2, 3, 4]
|
||||
else:
|
||||
assert samples and run.engine.state == 'SCANNING'
|
||||
assert settles_at <= run.now - started < settles_at + 1
|
||||
for observation in samples[0]['observations'].values():
|
||||
assert observation['jitter_px'] <= .5 and observation['drift_px'] <= .1
|
||||
|
||||
|
||||
@pytest.mark.parametrize('phase', ['moving', 'waiting'])
|
||||
@pytest.mark.parametrize('reason', [None, 'Tag 像素尺寸不足'])
|
||||
def test_missing_or_rejected_tag_does_not_blame_other_group_tags(phase, reason):
|
||||
run = SamplingRun('four_finger_roll', phase)
|
||||
|
||||
def lose_ring(frames, rejected):
|
||||
frames['front'].pop(2)
|
||||
if reason:
|
||||
rejected['front'] = {2: reason}
|
||||
|
||||
event = run.timeout(lose_ring)
|
||||
assert len(event['issues']) == 1
|
||||
assert event['issues'][0]['joint'] == 'ring_mcp_roll'
|
||||
assert event['issues'][0]['reason'] == (reason or '未检出有效 Tag')
|
||||
assert '正面 Tag ID2(ring_mcp_roll)' in run.engine.reason
|
||||
assert 'ID3' not in run.engine.reason and 'ID4' not in run.engine.reason
|
||||
|
||||
|
||||
def test_feedback_instability_names_the_joint_without_accusing_tags():
|
||||
run = SamplingRun('pinky_pip')
|
||||
joint = run.engine.profile.by_name['pinky_pip']
|
||||
|
||||
def feedback_moves(frames, rejected):
|
||||
values = list(run.adapter.feedback.positions)
|
||||
values[joint.index] = round(run.now * 20) % 2 * 5
|
||||
run.adapter.feedback = Feedback(round(run.now * 1e9), run.now, tuple(values))
|
||||
|
||||
event = run.timeout(feedback_moves)
|
||||
assert len(event['issues']) == 1
|
||||
assert event['issues'][0]['source'] == 'feedback'
|
||||
assert event['issues'][0]['joint'] == 'pinky_pip'
|
||||
assert '反馈未稳定' in run.engine.reason and 'Tag' not in run.engine.reason
|
||||
|
||||
|
||||
def test_stale_tag_reports_correct_camera_and_id():
|
||||
run = SamplingRun('pinky_pip')
|
||||
event = run.timeout(lambda frames, rejected: frames.pop('side'))
|
||||
assert len(event['issues']) == 1
|
||||
assert '侧面 Tag ID5(pinky_pip)' in run.engine.reason
|
||||
assert '有效观测已过期' in run.engine.reason
|
||||
|
||||
|
||||
def test_rejection_is_replaced_only_by_a_new_detector_frame():
|
||||
run = SamplingRun()
|
||||
stamp = round((run.now + .01) * 1e9)
|
||||
run.engine.observe('front', stamp, {}, run.now, {0: 'Tag 解码质量不足'})
|
||||
run.engine.observe('front', stamp - 1, run.adapter.frames()['front'], run.now)
|
||||
assert run.engine._visibility_issues(run.now)[0]['reason'] == 'Tag 解码质量不足'
|
||||
run.engine.observe('front', stamp + 1, run.adapter.frames()['front'], run.now)
|
||||
assert not run.engine.rejected_observations
|
||||
# Recovery before the gate must still explain why the image cannot be sampled.
|
||||
assert '新图像' in run.engine._visibility_issues(run.now)[0]['reason']
|
||||
run.engine.observe('front', run.engine.gate_ns, run.adapter.frames()['front'], run.now)
|
||||
assert not run.engine._visibility_issues(run.now)
|
||||
|
||||
|
||||
def test_window_explains_count_duration_and_recovery_without_stale_metrics():
|
||||
window = StableWindow(4, .5)
|
||||
points = np.array([[100, 100], [140, 100], [140, 140], [100, 140]])
|
||||
for i in range(4):
|
||||
window.add(Observation(i * 100_000_000, i / 10, points + i * .2))
|
||||
assert '时长不足' in window.problem(.5, .1)['reason']
|
||||
window.add(Observation(500_000_000, .5, points + 1))
|
||||
assert '持续漂移' in window.problem(.5, .1)['reason']
|
||||
window.clear()
|
||||
window.add(Observation(600_000_000, .6, points))
|
||||
problem = window.problem(.5, .1)
|
||||
assert problem == {'reason': '有效新图像不足:1/4 帧', 'frames': 1}
|
||||
for i in range(7, 13):
|
||||
window.add(Observation(i * 100_000_000, i / 10, points))
|
||||
assert window.problem(.5, .1) is None
|
||||
@@ -0,0 +1,200 @@
|
||||
"""Sampling must finish settling before pixels are used as motion evidence."""
|
||||
from dataclasses import replace
|
||||
import json
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from linkerhand_range_calibration.adapters.fake import FakeAdapter
|
||||
from linkerhand_range_calibration.core.legacy_endpoints import EndpointSearch
|
||||
from linkerhand_range_calibration.core.engine import Engine
|
||||
from linkerhand_range_calibration.profiles import Settings, load_profile, load_station
|
||||
from linkerhand_range_calibration.simulation import simulated_teaching
|
||||
from linkerhand_range_calibration.vision.observations import Observation, StableWindow, noise_sigma, rms_delta
|
||||
|
||||
|
||||
def ring_samples():
|
||||
return json.loads((Path(__file__).parent / 'fixtures' / 'ring_gradual_startup_shift.json').read_text())
|
||||
|
||||
|
||||
def test_latest_trace_exposes_gradual_relaxation_missed_by_previous_confirmation():
|
||||
profile, settings = load_profile(), Settings()
|
||||
search = EndpointSearch(profile.by_name['ring_mcp_roll'], 'up', settings, 'endpoint_search_v3')
|
||||
samples = ring_samples()
|
||||
for sample in samples:
|
||||
search.add(sample['command'], sample['observation'])
|
||||
assert search.result()['bound'] == 0
|
||||
assert search.result()['confirmed_at'] == 3
|
||||
# That first trigger did not meet the new stationary-window requirement.
|
||||
first = samples[1]['observation']
|
||||
assert first['noise'] > .1
|
||||
assert (first['last_stamp_ns'] - first['first_stamp_ns']) / 1e9 < settings.stable_seconds
|
||||
|
||||
|
||||
def test_stable_window_requires_time_and_rejects_subpixel_creep():
|
||||
corners = np.array([[100, 100], [140, 100], [140, 140], [100, 140]])
|
||||
window = StableWindow(8, .5)
|
||||
for i in range(8):
|
||||
window.add(Observation(i * 10_000_000, i / 100, corners))
|
||||
assert window.summary(.5, .1) is None # Plenty of frames, too little elapsed time.
|
||||
window.clear()
|
||||
for i in range(30):
|
||||
stamp = 1_000_000_000 + i * 33_333_333
|
||||
window.add(Observation(stamp, stamp / 1e9, corners + [i * .04, 0]))
|
||||
assert window.summary(.5, .1) is None
|
||||
assert window.last_quality['jitter_px'] < .001
|
||||
assert window.last_quality['drift_px'] > .1
|
||||
for i in range(30, 55):
|
||||
stamp = 1_000_000_000 + i * 33_333_333
|
||||
window.add(Observation(stamp, stamp / 1e9, corners + [1.16, 0]))
|
||||
result = window.summary(.5, .1)
|
||||
assert result is not None and result['drift_px'] == 0
|
||||
assert result['last_stamp_ns'] - result['first_stamp_ns'] >= 500_000_000
|
||||
assert not window.add(Observation(stamp, stamp / 1e9, corners))
|
||||
|
||||
|
||||
def test_departure_is_measured_from_recent_plateau_not_first_partial_offset():
|
||||
profile = load_profile()
|
||||
search = EndpointSearch(profile.by_name['ring_mcp_roll'], 'up', Settings())
|
||||
corners = np.array([[100, 100], [140, 100], [140, 140], [100, 140]])
|
||||
for command, x in enumerate([0, 1.4, 1.5, 1.52, 1.55, 1.56, 1.0, -1.0, -1.1]):
|
||||
search.add(command, {'corners': (corners + [x, 0]).tolist(), 'noise': .02})
|
||||
assert search.result()['trigger'] == 6 and search.result()['bound'] == 5
|
||||
|
||||
|
||||
class RingRelaxation(FakeAdapter):
|
||||
"""Time-driven settling fitted to the recorded startup drift, then useful travel.
|
||||
|
||||
The recorded summaries are not raw video, so this is a response model used
|
||||
to test holding a command; it is not a reconstruction of unrecorded frames.
|
||||
"""
|
||||
|
||||
def __init__(self, profile):
|
||||
super().__init__(profile)
|
||||
self.trace = ring_samples()
|
||||
self.clock, self.first_step_at = 0.0, None
|
||||
|
||||
def advance(self, now, stamp_ns):
|
||||
super().advance(now, stamp_ns)
|
||||
self.clock = now
|
||||
command = self.target[4]
|
||||
if command == 0:
|
||||
self.first_step_at = None
|
||||
elif 0 < command < 6 and self.first_step_at is None:
|
||||
self.first_step_at = now
|
||||
|
||||
def frames(self):
|
||||
frames = super().frames()
|
||||
command = self.target[4]
|
||||
if 0 < command < 6:
|
||||
times = [s['capture_after_first_step'] for s in self.trace[:6]]
|
||||
points = np.asarray([s['observation']['corners'] for s in self.trace[:6]])
|
||||
elapsed = self.clock - self.first_step_at
|
||||
corners = np.array([np.interp(elapsed, times, points[:, i, j])
|
||||
for i in range(4) for j in range(2)]).reshape(4, 2)
|
||||
elif command <= 13:
|
||||
corners = np.asarray(self.trace[command]['observation']['corners'])
|
||||
else:
|
||||
last = np.asarray(self.trace[-1]['observation']['corners'])
|
||||
corners = last + [-(command - 13) * .8, 0]
|
||||
frames['front'][2] = corners.tolist()
|
||||
return frames
|
||||
|
||||
|
||||
@pytest.mark.parametrize('shared_limit', [None, .1, .5])
|
||||
def test_stability_gates_remain_required_before_recording_first_displacement(monkeypatch, shared_limit):
|
||||
if shared_limit is not None:
|
||||
# Reproduce the previous shared-limit algorithm for comparison only.
|
||||
class SharedLimitWindow(StableWindow):
|
||||
def summary(self, noise_limit, drift_limit, drift_sigma=3):
|
||||
super().summary(noise_limit, drift_limit, drift_sigma)
|
||||
if self.last_quality is None:
|
||||
return None
|
||||
points = np.asarray([value.corners for value in self.values])
|
||||
split = len(points) // 2
|
||||
drift = rms_delta(np.median(points[:split], axis=0), np.median(points[split:], axis=0))
|
||||
if noise_sigma(points) > shared_limit or drift > shared_limit:
|
||||
return None
|
||||
return {'corners': np.median(points, axis=0).tolist(), **self.last_quality}
|
||||
monkeypatch.setattr('linkerhand_range_calibration.core.engine.StableWindow', SharedLimitWindow)
|
||||
profile = load_profile()
|
||||
settings = Settings(rate=10000)
|
||||
adapter = RingRelaxation(profile)
|
||||
events = []
|
||||
engine = Engine(profile, settings, adapter, events.append)
|
||||
now = 1.0
|
||||
adapter.advance(now, round(now * 1e9))
|
||||
engine.start(simulated_teaching(profile, adapter.uid), now, round(now * 1e9), ['four_finger_roll'])
|
||||
for _ in range(5000):
|
||||
now += 1 / 30
|
||||
stamp = round(now * 1e9)
|
||||
adapter.advance(now, stamp)
|
||||
for view, frame in adapter.frames().items():
|
||||
engine.observe(view, stamp, frame, now)
|
||||
engine.tick(now, stamp)
|
||||
assert engine.state != 'PAUSED', engine.reason
|
||||
if engine.state == 'COMPLETED':
|
||||
break
|
||||
assert engine.state == 'COMPLETED'
|
||||
# After settling, even a one-off displacement at command 1 counts as motion.
|
||||
assert engine.results['ring_mcp_roll']['min'] == 0
|
||||
if shared_limit is None:
|
||||
waits = [e for e in events if e['kind'] == 'sampling_unstable' and e['joint'] == 'ring_mcp_roll']
|
||||
assert waits and any(e['target'][4] == 1 for e in waits)
|
||||
for event in events:
|
||||
if event['kind'] == 'sample':
|
||||
for observed in event['observations'].values():
|
||||
assert observed['jitter_px'] <= settings.stability_noise_px
|
||||
assert observed['drift_px'] <= settings.stability_drift_px
|
||||
assert observed['last_stamp_ns'] - observed['first_stamp_ns'] >= 500_000_000
|
||||
|
||||
|
||||
def test_independent_limits_and_legacy_settings():
|
||||
settings = Settings.from_dict(load_station()['scan'])
|
||||
assert settings.stability_noise_px == .5 and settings.stability_drift_px == .1
|
||||
assert Settings.from_dict({'stability_px': 1}).stability_noise_px == 1
|
||||
assert Settings.from_dict({'stability_px': 1}).stability_drift_px == .1
|
||||
assert Settings.from_dict({'stability_px': 1, 'stability_noise_px': .5}).stability_noise_px == .5
|
||||
assert replace(Settings(), motion_floor_px=.25).stability_drift_px == .1
|
||||
for key in ('stability_noise_px', 'stability_drift_px', 'stability_drift_sigma', 'stability_px'):
|
||||
with pytest.raises(ValueError):
|
||||
Settings.from_dict({key: 0})
|
||||
with pytest.raises(ValueError, match='稳定观察时长'):
|
||||
Settings.from_dict({'point_timeout': .7})
|
||||
|
||||
|
||||
def test_half_pixel_tolerance_accepts_jitter_rejected_by_point_one():
|
||||
window = StableWindow(8, .5)
|
||||
corners = np.array([[100, 100], [140, 100], [140, 140], [100, 140]])
|
||||
for i in range(16):
|
||||
window.add(Observation(round(i / 30 * 1e9), i / 30, corners + [.25 * (-1) ** i, 0]))
|
||||
assert window.summary(.1, .1) is None
|
||||
summary = window.summary(.5, .1)
|
||||
assert summary is not None
|
||||
assert .1 < summary['noise'] < .5
|
||||
assert summary['noise_limit_px'] == .5 and summary['drift_limit_px'] == .1
|
||||
|
||||
|
||||
def test_stationary_noisy_tag_does_not_produce_a_successful_range():
|
||||
profile = load_profile()
|
||||
settings = Settings(rate=10000, settle_seconds=.01)
|
||||
adapter = FakeAdapter(profile, {'thumb_cmc_roll': (0, 0),
|
||||
**{j.name: (0, 255) for j in profile.joints if j.name != 'thumb_cmc_roll'}})
|
||||
engine = Engine(profile, settings, adapter)
|
||||
rng = np.random.default_rng(20)
|
||||
engine.start(simulated_teaching(profile, adapter.uid), 1.0, 1_000_000_000, ['thumb_cmc_roll'])
|
||||
for tick in range(1, 8000):
|
||||
now = 1 + tick / 20
|
||||
stamp = round(now * 1e9)
|
||||
adapter.advance(now, stamp)
|
||||
for view, frame in adapter.frames(noise=.2, rng=rng).items():
|
||||
engine.observe(view, stamp, frame, now)
|
||||
engine.tick(now, stamp)
|
||||
assert engine.state != 'PAUSED', engine.reason
|
||||
if engine.state == 'COMPLETED':
|
||||
break
|
||||
assert engine.state == 'COMPLETED'
|
||||
result = engine.results['thumb_cmc_roll']
|
||||
assert result['min'] is None and result['max'] is None
|
||||
assert result['status'] != '成功'
|
||||
@@ -0,0 +1,65 @@
|
||||
"""Correlated Tag motion is a diagnosis, never a motion correction."""
|
||||
import copy
|
||||
import json
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
|
||||
from linkerhand_range_calibration.core.diagnostics import shared_translation
|
||||
from linkerhand_range_calibration.profiles import Settings, load_station
|
||||
from linkerhand_range_calibration.vision.observations import Observation, StableWindow
|
||||
|
||||
|
||||
def recorded_issues():
|
||||
path = Path(__file__).parent / 'fixtures' / 'shared_translation_timeout.json'
|
||||
return json.loads(path.read_text())['issues']
|
||||
|
||||
|
||||
def test_recorded_shared_vibration_is_explained_but_still_rejected():
|
||||
issues = recorded_issues()
|
||||
original = copy.deepcopy(issues)
|
||||
report, = shared_translation(issues)
|
||||
assert report['view'] == 'front' and report['tag_ids'] == [1, 2, 3, 4]
|
||||
assert report['shared_fraction'] > .99
|
||||
assert 1.9 < report['horizontal_span_px'] < 2.1
|
||||
assert '共同平移' in report['reason']
|
||||
settings = Settings.from_dict(load_station()['scan'])
|
||||
assert settings.point_timeout == 6
|
||||
assert settings.stability_noise_px == .5 and settings.stability_drift_px == .1
|
||||
for issue in issues:
|
||||
window = StableWindow(settings.stable_frames, settings.stable_seconds)
|
||||
for sample in issue['window_samples']:
|
||||
window.add(Observation(sample['stamp_ns'], 0, sample['corners']))
|
||||
assert window.summary(settings.stability_noise_px, settings.stability_drift_px) is None
|
||||
assert .75 < window.last_quality['jitter_px'] < .8
|
||||
assert issues == original # No compensated corners replace the measured data.
|
||||
|
||||
|
||||
def test_similar_magnitudes_with_different_time_paths_are_not_shared_motion():
|
||||
issues = recorded_issues()
|
||||
for index, issue in enumerate(issues):
|
||||
base = np.asarray(issue['window_samples'][0]['corners'])
|
||||
count = len(issue['window_samples'])
|
||||
for frame, sample in enumerate(issue['window_samples']):
|
||||
shift = np.sin(frame * 2 * np.pi / count + index * np.pi / 2)
|
||||
sample['corners'] = (base + [shift, 0]).tolist()
|
||||
assert shared_translation(issues) == []
|
||||
|
||||
|
||||
def test_tags_require_matching_capture_times_and_distinct_ids():
|
||||
issues = recorded_issues()
|
||||
assert shared_translation(issues[:2]) == []
|
||||
assert shared_translation([issues[0], issues[0], issues[0]]) == []
|
||||
for index, issue in enumerate(issues):
|
||||
for sample in issue['window_samples']:
|
||||
sample['stamp_ns'] += index
|
||||
assert shared_translation(issues) == []
|
||||
|
||||
|
||||
def test_stationary_frames_do_not_report_a_shared_vibration():
|
||||
issues = recorded_issues()
|
||||
for issue in issues:
|
||||
corners = issue['window_samples'][0]['corners']
|
||||
for sample in issue['window_samples']:
|
||||
sample['corners'] = corners
|
||||
assert shared_translation(issues) == []
|
||||
@@ -0,0 +1,141 @@
|
||||
"""Historical offset rules replay unchanged; live scans use the first movement."""
|
||||
from dataclasses import asdict
|
||||
import json
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from linkerhand_range_calibration.core.legacy_endpoints import EndpointSearch
|
||||
from linkerhand_range_calibration.core.engine import Engine
|
||||
from linkerhand_range_calibration.adapters.fake import FakeAdapter
|
||||
from linkerhand_range_calibration.profiles import Settings, load_profile
|
||||
from linkerhand_range_calibration.replay import replay
|
||||
from linkerhand_range_calibration.simulation import simulated_teaching
|
||||
from linkerhand_range_calibration.storage import Session
|
||||
|
||||
|
||||
def observed(displacement):
|
||||
points = np.array([[100, 100], [140, 100], [140, 140], [100, 140]], dtype=float)
|
||||
return {'corners': (points + [displacement, 0]).tolist(), 'noise': .02}
|
||||
|
||||
|
||||
def startup_samples():
|
||||
return json.loads((Path(__file__).parent / 'fixtures' / 'four_finger_startup_shift.json').read_text())
|
||||
|
||||
|
||||
def test_real_corner_trace_reproduces_old_false_zero_and_rejects_it():
|
||||
profile, settings = load_profile(), Settings()
|
||||
searches = {}
|
||||
for name, expected in [('index_mcp_roll', None), ('ring_mcp_roll', 5), ('pinky_mcp_roll', 6)]:
|
||||
current = EndpointSearch(profile.by_name[name], 'up', settings)
|
||||
legacy = EndpointSearch(profile.by_name[name], 'up', settings, 'endpoint_search_v2')
|
||||
for sample in startup_samples():
|
||||
observation = sample['observations'][name]
|
||||
current.add(sample['command'], observation)
|
||||
legacy.add(sample['command'], observation)
|
||||
assert legacy.result()['bound'] == (6 if name == 'pinky_mcp_roll' else 0)
|
||||
assert (current.boundary['bound'] if current.boundary else None) == expected
|
||||
searches[name] = current
|
||||
# The recorded scan ended at 9, before the index finger moved further.
|
||||
# Retain its first real trigger at 7 without inventing a confirmed result.
|
||||
assert searches['index_mcp_roll'].candidate == 7
|
||||
for name, trigger in [('index_mcp_roll', 7), ('ring_mcp_roll', 6)]:
|
||||
rejected = searches[name].rejections
|
||||
assert len(rejected) == 1
|
||||
assert rejected[0]['trigger'] == 1 and rejected[0]['rejected_at'] == trigger
|
||||
|
||||
|
||||
@pytest.mark.parametrize('direction, trigger, bound', [('up', 7, 6), ('down', 242, 243)])
|
||||
def test_quantized_motion_confirms_first_trigger_after_initial_opposite_offset(direction, trigger, bound):
|
||||
joint = load_profile().joints[0]
|
||||
search = EndpointSearch(joint, direction, Settings())
|
||||
commands = range(256) if direction == 'up' else range(255, -1, -1)
|
||||
onset = abs(trigger - commands[0])
|
||||
for step, command in enumerate(commands):
|
||||
# A one-off displacement settles before real motion. The actuator then
|
||||
# advances only every five command steps; pauses do not move the trigger.
|
||||
x = 0 if step == 0 else 1 if step < onset else 1 - 2 * (1 + (step - onset) // 5)
|
||||
search.add(command, observed(x))
|
||||
if step < onset + 5:
|
||||
assert search.boundary is None
|
||||
if search.done:
|
||||
break
|
||||
assert search.result()['trigger'] == trigger
|
||||
assert search.result()['bound'] == bound
|
||||
assert len(search.rejections) == 1
|
||||
|
||||
|
||||
def test_persistent_one_off_offset_without_further_motion_is_not_success():
|
||||
search = EndpointSearch(load_profile().joints[0], 'up', Settings())
|
||||
for command in range(256):
|
||||
search.add(command, observed(0 if command == 0 else 1.2))
|
||||
assert search.boundary is None and search.error
|
||||
|
||||
|
||||
def test_group_live_scan_records_first_stable_offset_and_replays(tmp_path):
|
||||
class StartupAdapter(FakeAdapter):
|
||||
def frames(self):
|
||||
frames = super().frames()
|
||||
# Simulate the observed initial offset and quantized useful travel,
|
||||
# with different onset commands for each finger.
|
||||
for name, onset in [('index_mcp_roll', 7), ('ring_mcp_roll', 6)]:
|
||||
joint = self.profile.by_name[name]
|
||||
command = self.target[joint.index]
|
||||
x = 0 if command == 0 else 1 if command < onset else 1 - 2 * (1 + (command - onset) // 5)
|
||||
frames[joint.view][joint.tag_id] = observed(x)['corners']
|
||||
return frames
|
||||
|
||||
profile = load_profile()
|
||||
settings = Settings(rate=10000, stable_frames=4, settle_seconds=.01)
|
||||
adapter = StartupAdapter(profile)
|
||||
teaching = simulated_teaching(profile, adapter.uid)
|
||||
session = Session(tmp_path, profile, adapter.uid, asdict(settings), teaching, {'simulated': True})
|
||||
engine = Engine(profile, settings, adapter, session.emit)
|
||||
now = 1.0
|
||||
adapter.advance(now, int(now * 1e9))
|
||||
engine.start(teaching, now, int(now * 1e9), ['four_finger_roll'])
|
||||
try:
|
||||
for _ in range(3000):
|
||||
now += .1
|
||||
stamp = round(now * 1e9)
|
||||
adapter.advance(now, stamp)
|
||||
for view, frame in adapter.frames().items():
|
||||
engine.observe(view, stamp, frame, now)
|
||||
engine.tick(now, stamp)
|
||||
assert engine.state != 'PAUSED', engine.reason
|
||||
if engine.state == 'COMPLETED':
|
||||
break
|
||||
assert engine.state == 'COMPLETED'
|
||||
finally:
|
||||
session.close()
|
||||
assert engine.results['index_mcp_roll']['min'] == 0
|
||||
assert engine.results['ring_mcp_roll']['min'] == 0
|
||||
journal = session.directory / 'samples.jsonl'
|
||||
events = [json.loads(line) for line in journal.read_text().splitlines()]
|
||||
assert not any(e['kind'] == 'boundary_candidate_rejected' for e in events)
|
||||
assert replay(journal, tmp_path / 'recomputed.json') == session.document()
|
||||
|
||||
|
||||
@pytest.mark.parametrize('method', ['endpoint_search_v1', 'endpoint_search_v2'])
|
||||
def test_legacy_replay_preserves_original_confirmation_semantics(tmp_path, method):
|
||||
profile, settings = load_profile(), Settings()
|
||||
task = next(t for t in profile.tasks if t.name == 'four_finger_roll')
|
||||
events = [
|
||||
{'kind': 'metadata', 'profile': profile.raw, 'settings': asdict(settings),
|
||||
'uid': 'LEGACY_FIXTURE', 'scan_method': method},
|
||||
{'kind': 'task_started', 'task': task.name, 'joints': list(task.joints)},
|
||||
]
|
||||
for sample in startup_samples():
|
||||
events.append({'kind': 'sample', 'task': task.name, 'stage': 'endpoint', 'repeat': 0,
|
||||
'direction': 'up', **sample})
|
||||
for command in range(255, 251, -1):
|
||||
events.append({'kind': 'sample', 'task': task.name, 'stage': 'endpoint', 'repeat': 0,
|
||||
'direction': 'down', 'command': command,
|
||||
'observations': {name: observed(command) for name in task.joints}})
|
||||
events.append({'kind': 'task_result', 'task': task.name})
|
||||
journal = tmp_path / 'legacy.jsonl'
|
||||
journal.write_text('\n'.join(json.dumps(e) for e in events))
|
||||
document = replay(journal, tmp_path / 'legacy_ranges.json')
|
||||
assert document['joints']['index_mcp_roll'] == {'min': 0, 'max': 255}
|
||||
assert document['joints']['ring_mcp_roll'] == {'min': 0, 'max': 255}
|
||||
@@ -0,0 +1,90 @@
|
||||
"""Time order, not just pixel spread, separates random jitter from movement."""
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from linkerhand_range_calibration.vision.observations import Observation, StableWindow, noise_sigma
|
||||
|
||||
|
||||
SQUARE = np.array([[100, 100], [140, 100], [140, 140], [100, 140]], dtype=float)
|
||||
|
||||
|
||||
def make_window(points, times=None):
|
||||
times = np.linspace(0, .5, len(points)) if times is None else times
|
||||
window = StableWindow(8, .5)
|
||||
for time, corners in zip(times, points):
|
||||
stamp = 1_700_000_000_000_000_000 + round(time * 1e9)
|
||||
window.add(Observation(stamp, time, corners))
|
||||
return window
|
||||
|
||||
|
||||
def test_identical_spread_has_different_outcome_when_time_order_changes():
|
||||
points = SQUARE + np.linspace(-.35, .35, 16)[:, None, None] * [1, 0]
|
||||
drifting = make_window(points)
|
||||
shuffled = make_window(np.random.default_rng(4).permutation(points))
|
||||
assert drifting.summary(.5, .1) is None
|
||||
assert drifting.last_quality['jitter_px'] < .001
|
||||
assert drifting.last_quality['drift_px'] == pytest.approx(.7)
|
||||
assert shuffled.summary(.5, .1) is not None
|
||||
assert drifting.last_quality['noise'] == pytest.approx(shuffled.last_quality['noise'])
|
||||
|
||||
|
||||
@pytest.mark.parametrize('count', [9, 16, 31])
|
||||
@pytest.mark.parametrize('seed', [0, 1, 2])
|
||||
def test_stationary_random_jitter_is_accepted_at_different_frame_rates(count, seed):
|
||||
points = SQUARE + np.random.default_rng(seed).normal(0, .2, (count, 4, 2))
|
||||
window = make_window(points)
|
||||
summary = window.summary(.5, .1)
|
||||
assert summary is not None, window.last_quality
|
||||
assert .1 < summary['jitter_px'] <= .5
|
||||
assert summary['drift_px'] <= .1
|
||||
# Endpoint thresholds keep the raw spread, not artificially small detrended noise.
|
||||
assert summary['noise'] == pytest.approx(noise_sigma(points))
|
||||
|
||||
|
||||
@pytest.mark.parametrize('motion', ['translation', 'rotation', 'reversal', 'step'])
|
||||
def test_drift_rotation_and_slow_reversal_are_not_random_jitter(motion):
|
||||
rng = np.random.default_rng(6)
|
||||
frames = []
|
||||
for t in np.linspace(0, 1, 16):
|
||||
if motion == 'rotation':
|
||||
angle = t * .04
|
||||
rotation = np.array([[np.cos(angle), -np.sin(angle)], [np.sin(angle), np.cos(angle)]])
|
||||
points = (SQUARE - 120) @ rotation + 120
|
||||
assert np.mean(points, axis=0) == pytest.approx([120, 120])
|
||||
else:
|
||||
offset = {'translation': .8 * t, 'reversal': .8 * np.sin(t * np.pi),
|
||||
'step': .8 * (t >= .5)}[motion]
|
||||
points = SQUARE + [offset, 0]
|
||||
frames.append(points + rng.normal(0, .02, (4, 2)))
|
||||
window = make_window(frames)
|
||||
assert window.summary(.5, .1) is None
|
||||
assert window.last_quality['jitter_px'] < .5
|
||||
assert window.last_quality['drift_px'] > .1
|
||||
assert '持续漂移' in window.problem(.5, .1)['reason']
|
||||
|
||||
|
||||
def test_excessive_noise_reports_its_own_limit():
|
||||
points = SQUARE + np.random.default_rng(10).normal(0, 1, (16, 4, 2))
|
||||
window = make_window(points)
|
||||
problem = window.problem(.5, .1)
|
||||
assert problem['jitter_px'] > .5
|
||||
assert '随机抖动' in problem['reason'] and '0.500' in problem['reason']
|
||||
|
||||
|
||||
def test_isolated_bad_frame_does_not_create_a_motion_candidate():
|
||||
points = np.repeat(SQUARE[None], 16, axis=0)
|
||||
points[6] += 3
|
||||
summary = make_window(points).summary(.5, .1)
|
||||
assert summary is not None
|
||||
assert summary['corners'] == SQUARE.tolist()
|
||||
|
||||
|
||||
def test_trend_uses_actual_irregular_timestamps_and_independent_limits():
|
||||
times = np.array([0, .015, .03, .07, .09, .12, .3, .35, .4, .5])
|
||||
points = SQUARE + times[:, None, None] * [1, 0]
|
||||
window = make_window(points, times)
|
||||
assert window.summary(.5, .1) is None
|
||||
assert window.last_quality['jitter_px'] < .001
|
||||
assert window.last_quality['trend_px'] == pytest.approx(.5)
|
||||
assert window.summary(.5, .6) is not None
|
||||
|
||||
@@ -0,0 +1,169 @@
|
||||
"""A command can cause motion even when the finger returns before settling."""
|
||||
from dataclasses import asdict
|
||||
import copy
|
||||
import json
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from linkerhand_range_calibration.adapters.fake import FakeAdapter
|
||||
from linkerhand_range_calibration.core.endpoints import EndpointSearch
|
||||
from linkerhand_range_calibration.core.engine import Engine
|
||||
from linkerhand_range_calibration.profiles import Settings, load_profile
|
||||
from linkerhand_range_calibration.replay import replay
|
||||
from linkerhand_range_calibration.simulation import simulated_teaching
|
||||
from linkerhand_range_calibration.storage import Session
|
||||
from linkerhand_range_calibration.vision.motion import MotionCapture, MotionTrace, witnessed_motion
|
||||
from linkerhand_range_calibration.vision.observations import Observation, rms_delta
|
||||
|
||||
|
||||
BASE = np.array([[100, 100], [140, 100], [140, 140], [100, 140]], dtype=float)
|
||||
|
||||
|
||||
def frame(stamp, shift):
|
||||
return Observation(stamp, stamp / 1e9, BASE + [shift, 0])
|
||||
|
||||
|
||||
def test_process_motion_survives_return_to_reference_without_using_direction():
|
||||
capture = MotionCapture(BASE, .5, 1_000_000_000, 2, .5)
|
||||
assert not capture.observe(frame(1_030_000_000, 2))
|
||||
assert capture.observe(frame(1_060_000_000, -2))
|
||||
saved = copy.deepcopy(capture.evidence)
|
||||
assert not capture.observe(frame(1_300_000_000, 0))
|
||||
assert capture.evidence == saved
|
||||
assert witnessed_motion(saved, BASE, .5, 2, .5)
|
||||
|
||||
|
||||
@pytest.mark.parametrize('gap', ['old', 'duplicate', 'missing', 'invalid', 'too_long', 'returned'])
|
||||
def test_two_valid_fresh_motion_frames_are_required(gap):
|
||||
capture = MotionCapture(BASE, .5, 1_000_000_000, 2, .5)
|
||||
if gap == 'old':
|
||||
assert not capture.observe(frame(990_000_000, 2))
|
||||
assert not capture.observe(frame(1_030_000_000, 2))
|
||||
else:
|
||||
assert not capture.observe(frame(1_030_000_000, 2))
|
||||
if gap == 'duplicate':
|
||||
assert not capture.observe(frame(1_030_000_000, 2))
|
||||
elif gap == 'too_long':
|
||||
assert not capture.observe(frame(1_600_000_000, 2))
|
||||
else:
|
||||
middle = (None if gap == 'missing' else frame(1_040_000_000, float('nan'))
|
||||
if gap == 'invalid' else frame(1_040_000_000, 0))
|
||||
assert not capture.observe(middle)
|
||||
assert not capture.observe(frame(1_060_000_000, 2))
|
||||
assert capture.evidence is None
|
||||
|
||||
|
||||
@pytest.mark.parametrize('direction', ['up', 'down'])
|
||||
def test_endpoint_uses_recorded_transient_instead_of_only_the_final_pose(direction):
|
||||
search = EndpointSearch(load_profile().joints[0], direction, Settings())
|
||||
capture = MotionTrace(1_000_000_000)
|
||||
for stamp in (1_030_000_000, 1_060_000_000):
|
||||
capture.observe(stamp, frame(stamp, 2))
|
||||
sample = {'corners': BASE.tolist(), 'noise': .02,
|
||||
'window_samples': [{'stamp_ns': 1_300_000_000, 'corners': BASE.tolist()}]}
|
||||
search.add(0 if direction == 'up' else 255, sample)
|
||||
search.add(1 if direction == 'up' else 254, {**sample, 'motion_trace': capture.snapshot()})
|
||||
assert search.result()['bound'] == (0 if direction == 'up' else 255)
|
||||
assert search.result()['confirmed_at'] == (1 if direction == 'up' else 254)
|
||||
|
||||
|
||||
@pytest.mark.parametrize('tamper', ['before_command', 'duplicate', 'no_motion', 'one_frame'])
|
||||
def test_replay_recalculates_motion_from_valid_frame_evidence(tamper):
|
||||
capture = MotionCapture(BASE, .5, 1_000_000_000, 2, .5)
|
||||
capture.observe(frame(1_030_000_000, 2))
|
||||
capture.observe(frame(1_060_000_000, 2))
|
||||
evidence = capture.evidence
|
||||
if tamper == 'before_command':
|
||||
evidence['frames'][0]['stamp_ns'] = 999_000_000
|
||||
elif tamper == 'duplicate':
|
||||
evidence['frames'][1]['stamp_ns'] = evidence['frames'][0]['stamp_ns']
|
||||
elif tamper == 'no_motion':
|
||||
evidence['frames'][1]['corners'] = BASE.tolist()
|
||||
else:
|
||||
evidence['frames'].pop()
|
||||
assert not witnessed_motion(evidence, BASE, .5, 2, .5)
|
||||
|
||||
|
||||
@pytest.mark.parametrize('rate', [20, 10000])
|
||||
@pytest.mark.parametrize('pulse_frames,expected_max', [(0, 254), (1, 254), (2, 255)])
|
||||
def test_live_scan_remembers_motion_before_settling_and_replays(tmp_path, pulse_frames, expected_max, rate):
|
||||
class TransientAdapter(FakeAdapter):
|
||||
def __init__(self, profile):
|
||||
super().__init__(profile)
|
||||
self.clock = self.pulse_start = 0
|
||||
self.ranges['pinky_mcp_pitch'] = (1, 255)
|
||||
|
||||
def advance(self, now, stamp_ns):
|
||||
self.clock = now
|
||||
super().advance(now, stamp_ns)
|
||||
|
||||
def send_positions(self, target):
|
||||
if target[10] == 254 and self.target[10] == 255:
|
||||
self.pulse_start = self.clock
|
||||
super().send_positions(target)
|
||||
|
||||
def frames(self):
|
||||
frames = super().frames()
|
||||
if self.target[10] in (253, 254, 255):
|
||||
elapsed_frames = round((self.clock - self.pulse_start) * 30)
|
||||
moved = (self.target[10] == 253 or
|
||||
(self.target[10] == 254 and 1 <= elapsed_frames <= pulse_frames))
|
||||
frames['side'][5] = (BASE + [2 if moved else 0, 0]).tolist()
|
||||
return frames
|
||||
|
||||
profile, settings = load_profile(), Settings(rate=rate)
|
||||
adapter = TransientAdapter(profile)
|
||||
teaching = simulated_teaching(profile, adapter.uid)
|
||||
session = Session(tmp_path, profile, adapter.uid, asdict(settings), teaching, {'simulated': True})
|
||||
engine = Engine(profile, settings, adapter, session.emit)
|
||||
adapter.advance(1, 1_000_000_000)
|
||||
engine.start(teaching, 1, 1_000_000_000, ['pinky_mcp_pitch'])
|
||||
try:
|
||||
for tick in range(1, 1500):
|
||||
now = 1 + tick / 30
|
||||
stamp = round(now * 1e9)
|
||||
adapter.advance(now, stamp)
|
||||
for view, detections in adapter.frames().items():
|
||||
engine.observe(view, stamp, detections, now)
|
||||
engine.tick(now, stamp)
|
||||
assert engine.state != 'PAUSED', engine.reason
|
||||
if engine.state == 'COMPLETED':
|
||||
break
|
||||
assert engine.state == 'COMPLETED'
|
||||
finally:
|
||||
session.close()
|
||||
journal = session.directory / 'samples.jsonl'
|
||||
events = [json.loads(line) for line in journal.read_text().splitlines()]
|
||||
samples = [e for e in events if e['kind'] == 'sample' and e['direction'] == 'down']
|
||||
assert [e['command'] for e in samples] == ([255, 254] if pulse_frames >= 2 else [255, 254, 253])
|
||||
at_254 = samples[1]['observations']['pinky_mcp_pitch']
|
||||
assert rms_delta(at_254['corners'], BASE) == 0 # The pulse is over before settling.
|
||||
if pulse_frames >= 2:
|
||||
boundary = next(e for e in events if e['kind'] == 'boundary_found' and e['direction'] == 'down')
|
||||
motion = boundary['evidence']['transient']
|
||||
assert motion['frames'][-1]['stamp_ns'] < at_254['first_stamp_ns']
|
||||
assert motion['frames'][-1]['stamp_ns'] - at_254['motion_trace']['gate_stamp_ns'] < 300_000_000
|
||||
assert boundary['evidence']['source'] == 'transient'
|
||||
else:
|
||||
assert not any(e['kind'] == 'boundary_found' and e['direction'] == 'down' and e['trigger'] == 254
|
||||
for e in events)
|
||||
assert session.document()['joints']['pinky_mcp_pitch'] == {'min': 1, 'max': expected_max}
|
||||
assert replay(journal, tmp_path / 'recomputed.json') == session.document()
|
||||
|
||||
|
||||
@pytest.mark.parametrize('count', [0, 1, 2.5])
|
||||
def test_motion_frame_count_rejects_invalid_settings(count):
|
||||
with pytest.raises(ValueError):
|
||||
Settings.from_dict({'motion_frames': count})
|
||||
|
||||
|
||||
def test_latest_old_recording_cannot_recover_motion_before_the_sampling_gate():
|
||||
data = json.loads((Path(__file__).parent / 'fixtures' / 'pinky_pitch_small_final_offset.json').read_text())
|
||||
search = EndpointSearch(load_profile().by_name['pinky_mcp_pitch'], 'down', Settings())
|
||||
for sample in data['samples']:
|
||||
search.add(sample['command'], sample['observation'])
|
||||
assert search.result()['bound'] == 254 # No transient frames were saved in this recording.
|
||||
assert rms_delta(data['samples'][0]['observation']['corners'],
|
||||
data['samples'][1]['observation']['corners']) == pytest.approx(.101374, abs=1e-6)
|
||||
Reference in New Issue
Block a user