完善行程标定与视觉运动判定

This commit is contained in:
lxp
2026-09-17 18:25:36 +08:00
parent af2dc9c38f
commit 32b6af62e2
36 changed files with 6023 additions and 161 deletions
+5 -3
View File
@@ -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.
+76 -16
View File
@@ -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 ID0thumb_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点确认时,该例只采集09和255240共26个指令点两端之间只移动。
界面显示端点定位、低端/高端搜索、当前指令和轮次;进度按已确认或已判失败的边界计算。
该例只采集07和255242共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 = {}
@@ -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
@@ -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
@@ -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
}
}
}
]
@@ -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
}
]
}
@@ -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
}
}
]
}
@@ -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
}
}
]
}
@@ -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
}
}
]
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 ID2ring_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 ID2ring_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 ID2ring_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 ID5pinky_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)