diff --git a/.gitignore b/.gitignore index 87d3cc1..91eca50 100644 --- a/.gitignore +++ b/.gitignore @@ -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. diff --git a/src/linkerhand_range_calibration/README.md b/src/linkerhand_range_calibration/README.md index 523763f..e61a07b 100644 --- a/src/linkerhand_range_calibration/README.md +++ b/src/linkerhand_range_calibration/README.md @@ -110,7 +110,7 @@ SDK 自身的初始化检查和设备保护保持原样。标定仍要求 SDK | 侧面 / 8 | index_mcp_pitch、index_pip、index_dip | 7、11、16 | | 顶部 / 9 | thumb_cmc_yaw | 1 | -任务顺序：正面拇指3项 → 四指侧摆同步1项 → 侧面弯曲12项 → 顶部拇指1项。 +任务顺序：正面拇指3项 → 四指侧摆分阶段1项 → 侧面弯曲12项 → 顶部拇指1项。 标定 `thumb_cmc_roll` 时，准备姿态中的 `index_mcp_roll`（SDK 下标2）固定为0。 准备姿态稳定后开始拇指扫描，食指侧摆始终保持0；准备过程不计入边界采样。 @@ -126,9 +126,34 @@ SDK 自身的初始化检查和设备保护保持原样。标定仍要求 SDK 准备姿态稳定后才扫描 `thumb_mcp`，扫描期间拇指 yaw 始终保持80。 该避让仅用于 `thumb_mcp`，进入后续 `thumb_ip` 时按照基础姿态及对应任务配置重新准备。 -四指侧摆使用相同指令，同时更新SDK下标2、3、4、5，不应用避让配置；各Tag独立计算范围。 -一指先确认起动边界后保留该值，整组继续同步逐1移动；四指均确认该端边界后，共同切换到另一端。 -若某指扫描到对端仍不能确认运动，该指记为失败；其余手指继续测量，不为已失败手指重复寻找另一端。 +### 四指侧摆分阶段标定 + +四指侧摆仍作为一个任务，统一发送包含20项的目标数组；其余16个通道保持基础姿态，不应用弯曲避让。 +中指下限最后测量，顺序如下（“三指”指小指、无名指、食指）： + +| 阶段 | 三指目标 | 中指目标 | 测量内容 | +|---|---|---|---| +| 同步正向 | 0→255 | 80→255 | 三指各自的 min | +| 同步反向 | 255→0 | 255→80 | 四指各自的 max | +| 中指回零 | 保持0 | 80→0 | 仅定位，不计算边界 | +| 中指单独正向 | 保持0 | 0→80 | 中指 min | +| 必要时联动继续 | 0→255 | 80→255 | 继续测中指 min | + +同步阶段按各自区间的相同比例推进：三指指令为 `c` 时，中指为 `80 + round(175*c/255)`。 +三指每次改变1，中指每次改变0或1，始终在同一条消息中更新。每个 Tag 独立判断；中指相同指令的重复观测不重复参与边界搜索。 +正向确认三指下限后，可直接沿限速轨迹到255再测上限；反向确认四指上限后，仍会先到达“三指0、中指80”并等待新反馈稳定，再只让中指回零。 + +中指单独扫描只要求正面 ID3 有效，其他三指保持0。若到80仍未检测到运动，就进入联动继续阶段； +从0采集的参考和噪声全部保留，不在80重置，也不重复计算80。首次检测到运动即结束该轮，不再扫描剩余内部行程。 +同步及联动继续阶段要求四个 Tag 有效，任一丢失整组停止推进。 + +若中指反向扫到80仍无法确认上限，该指记录失败，不将80填作上限；其他关节独立保留结果。 +其他关节扫描到对端仍无法确认运动也记录失败，不为已失败关节重复测量。 +复测时重做上述完整顺序；暂停后继续会重做整个四指任务。 + +配置位于 `config/profiles/o30_right.yaml` 的 `four_finger_roll.deferred_lower`： +`joint: middle_mcp_roll` 指定延后测下限的通道，`split: 80` 指定分段值。 +该流程不依赖 O30 的关节名称，可复用于其他型号的多通道任务。没有该配置的任务沿用普通两端搜索。 侧面按小指→无名指→中指→食指测量，每指依次测 mcp_pitch、pip、dip。 测无名指时小指弯曲；测中指时小指和无名指弯曲；测食指时前三指弯曲。 @@ -146,32 +171,65 @@ SDK 自身的初始化检查和设备保护保持原样。标定仍要求 SDK ## 5. 算法与暂停 -默认只测两端，一轮包含以下步骤： +默认只测两端；普通任务一轮包含以下步骤，四指侧摆采用上面的分阶段顺序： 1. 到达任务准备姿态并稳定。 2. 沿限速轨迹到达0，采集稳定的低端参考角点和噪声。 -3. 按1、2、3……逐1递增，与低端参考比较；连续3个有效采样点确认离开参考姿态后，保留第一次触发运动的指令。 +3. 按1、2、3……逐1递增，与低端参考比较；记录指令发出后是否发生过有效位移，首次检测到运动的指令记为 `min=当前指令-1`，结束低端搜索。 4. 沿限速轨迹直接移动到255，中间不停车采样；稳定后重新采集高端参考角点和噪声。 -5. 按254、253、252……逐1递减，与高端参考比较；确认离开参考姿态后记录第一次触发指令，结束测量。 +5. 按254、253、252……逐1递减，与高端参考比较；首次检测到运动的指令记为 `max=当前指令+1`，结束高端搜索。 6. 保存结果；若任务配置了测后恢复关节，按配置顺序逐个恢复为基础姿态，每一步都等待新反馈稳定，再结束任务或进入下一任务。 -确认期间若观测重新回到参考姿态附近，会清除候选值，继续寻找。 +只判断“是否发生位移”，不判断图像中的运动方向，不要求后续几个指令继续运动，也不按后续转向或停顿重新选择起动值。 +每端参考姿态固定为该端起始的稳定观测，不再执行平台重建或候选方向修正。Tag 原地旋转也会改变四个角点，因此同样可以检测。 +例如255→254时首次检测到运动，就在254结束该端搜索并记录 `max=255`，不再为了确认而采集253、252。 +检测仍使用每点的新帧稳定窗口和运动阈值，不能用旧帧、丢失 Tag 或单张异常图像作为起动证据。 +从该关节目标第一次实际发出时就开始记录逐帧角点，覆盖0.3秒等待期间和之后的运动过程。先保留证据，等本点稳定后再判断，避免两帧随机偏移一出现就被永久记成运动。 +稳定位置变化与瞬时运动使用不同门槛：稳定窗口的角点代表位置相对端点参考超过 `motion_floor_px`（默认0.5像素）即可判动；瞬时运动需要连续 `motion_frames` 张有效新图像（默认2张）超过「0.5像素 + 端点逐帧抖动范围 + 当前点逐帧抖动范围」。抖动范围由各自稳定窗口的角点到代表位置的距离99分位数计算，不假设逐帧噪声独立，也不判断位移方向。 +本点完成采样时，稳定位置变化或瞬时运动任一成立，便使用这个指令计算边界。超过抖动范围的短暂运动，即使随后回原位也保留；准备动作、到达初始端点的过程、目标发出前的旧图像，以及无关 Tag 都不参与过程位移判定。 默认只执行一轮（`scan.endpoint_repetitions: 1`）。设为2或更大时，按相同顺序重新寻找两端， -同一端复测差异最多2单位（`repeat_tolerance`），通过后取区间交集。旧工位文件的 `coarse_step`、`fine_radius`、`fine_repetitions` 仍可加载，但不影响新流程。 +同一端复测差异最多2单位（`repeat_tolerance`），通过后取区间交集。旧工位文件的 `coarse_step`、`fine_radius`、`fine_repetitions`、`confirmation_points` 仍可加载，但不影响新流程。 -只比较同一个Tag的四个有序角点，结合稳定窗口、端点平台、边界附近多个观测点的确认和静止噪声。 -每点至少等待0.3秒，随后采集8张稳定新图像；扫描期间只检查本任务主动关节的反馈稳定性，不要求反馈等于目标指令。 +只比较同一个Tag的四个有序角点，使用过程中的有效观测及各指令点的稳定窗口，与固定端点参考计算均方根位移。 +每点到达轨迹目标后至少等待0.3秒，再采集至少8张、时间跨度至少0.5秒的稳定新图像（`stable_frames`、`stable_seconds` 同时满足）；过程运动检测同时进行，不会丢弃等待期间“动一下又回位”的证据。 +**每端的起始参考额外复核**：目标保持不变，图像至少覆盖 `reference_seconds`（默认1.5秒）且至少 `3×stable_frames` 张；前、中、后三段的角点代表位置两两差异均不超过运动门槛的一半（默认0.25像素），同时仍需通过随机抖动和持续漂移检查。参考不稳定时继续保持当前目标等待，不能拿尚在回位的短窗口作为0或255的参考。该复核增加静止观察时间，不增加扫描轮次，也不发送额外往返动作。 +窗口代表位置按每个坐标去掉最高和最低各四分之一的值，再取剩余值的平均；减少随机抖动两侧帧数不均时，中位数随相位跳变的影响。运动过程仍检查原始帧，不会用平均位置抹掉短暂运动。 +稳定采样将随机抖动与持续漂移分开判断，默认配置如下： + +| 参数 | 默认值 | 含义 | +|---|---:|---| +| `stability_noise_px` | 0.5 | 随机角点抖动上限，单位像素 | +| `stability_drift_px` | 0.1 | 扣除抖动裕量后的累计漂移上限，单位像素 | +| `stability_drift_sigma` | 3 | 估计时间趋势时使用的抖动裕量倍数 | + +先按实际时间戳估计四角点的移动趋势，再用去除趋势后的 MAD 和相邻帧差分估计随机抖动。 +全窗口及前、后半窗口分别检查趋势，避免缓慢往返运动在整个窗口中互相抵消；每个半窗口至少4帧。 +持续漂移取各段 `max(0, 趋势累计位移 - 抖动裕量)` 的最大值，裕量由随机抖动、帧数和时间分布计算。 +两项门槛独立生效：随机抖动允许到0.5像素，低噪声条件下的持续漂移仍按0.1像素检查。任一项超限就保持当前指令继续等待，超过 `point_timeout` 仍不能稳定则暂停。 +`point_timeout` 默认6秒，给换向后的短时振动留出衰减时间；这是最长等待时间，达到稳定条件就立即采样推进。持续振动仍会暂停，不通过放宽噪声或漂移门槛获得结果。 +这是对有限图像的估计，并非已完全静止的保证。高噪声会降低细小运动的可辨别性，实机准确性仍需复测；不根据四指一起变化自动扣除整体位移，以免消除四指真实的同步运动。 +旧工位文件的 `stability_px` 仍可加载，仅作为随机抖动上限；新增配置优先，漂移上限默认0.1。建议自定义工位改用上述三个明确字段。 + +新日志的 `sampling_method` 为 `separate_noise_drift_v2`。样本分别记录 `jitter_px`、`trend_px`、`trend_allowance_px`、`drift_px` 及两项门槛。 +`sampling_unstable` 和 `sampling_timeout` 额外保存当时的逐帧时间戳、四角点（`window_samples`），便于离线检查。旧日志只有汇总值时无法补回随机抖动与持续漂移的区别。 +超时提示会列出当前受阻的机位、Tag ID、关节和原因，例如 `正面 Tag ID0（thumb_cmc_roll）：随机抖动 0.620 > 0.500 像素` 或 `持续漂移 0.180 > 0.100 像素`。 +原因包括未检出、检测质量不足、观测过期、新图像数量或观察时长不足、随机抖动/持续漂移过大；反馈不足或未稳定会单独说明，不归因于 Tag。 +四指任务只列出当前受阻的观测；相机检测流断流时列出该机位当前任务所需的 Tag。 +同一机位至少3个 Tag 的角点在相同时间呈现高度一致的平移时，超时提示会补充“多个 Tag 出现共同平移”及幅度，日志记录 `shared_motion`。这只能提示共同变化，无法仅凭指尖 Tag 判定是相机、手掌还是支架在移动，也不会据此扣除图像位移或放行采样。 +`sampling_timeout` 日志在清空采样窗口前保存超时当刻的具体原因、测量值、目标及任务阶段，便于事后定位；这些提示不改变采样门槛或运动流程。 +扫描期间只检查本任务主动关节的反馈稳定性，不要求反馈等于目标指令。 准备姿态、测后恢复和示教保存继续等待全手反馈稳定；SDK 连接及反馈时效检查保留。 旧配置中的 `non_target_tolerance` 仍可加载，但不再用于扫描判定。 -每端独立计算阈值 `max(0.5 px, 5×该端静止噪声)`。比较对象始终是该端参考姿态，细小的累计运动也能被检测。 +稳定位置使用固定 `motion_floor_px`，不再把一次起点观测的随机抖动乘5作为运动门槛，避免相同真实位移在不同轮次被漏掉。`noise_multiplier` 仅保留给旧日志复算。细小运动仍相对于固定端点参考累计比较，瞬时帧单独使用上述抖动范围检查。 JSON 沿用边界定义：`min = 低端首次运动指令 - 1`，`max = 高端反向首次运动指令 + 1`。 例如从0递增到7首次运动、从255递减到242首次运动，结果为6～243；加减1对应 O30 的指令分辨率，其他型号使用自身分辨率。 -默认3点确认时，该例只采集0～9和255～240共26个指令点，两端之间只移动。 -界面显示端点定位、低端/高端搜索、当前指令和轮次；进度按已确认或已判失败的边界计算。 +该例只采集0～7和255～242共22个指令点；两端之间只移动。 +界面显示阶段、低端/高端搜索、各主动关节的当前指令和轮次；进度按已确认或已判失败的边界计算。 范围只描述两端的指令边界，中间是否持续运动不参与成功/失败判定。 例如低端静止到6、高端从243开始静止，即使中途停了一段再恢复运动，仍可得到6～243。 -新日志的离线复算与在线标定共用端点搜索逻辑。全程没有可确认运动、端点观测不足、两端边界重叠或启用复测后边界不一致时，无法给出可靠范围。 +新日志记录每个阶段及各关节实际用于边界搜索的指令，离线复算与在线标定共用端点搜索逻辑，同时兼容旧版相同指令扫描日志。 +全程未检测到超过阈值的运动、端点观测不足、两端边界重叠或启用复测后边界不一致时，无法给出可靠范围。一次有效稳定位移已经算起动，后续是否停住不参与判定。 单步分辨率为1，不代表真实边界误差必然为1。过小运动、噪声、速度和力矩设置都会影响可检测边界。 本方法测量低端正向起动和高端反向起动边界，回差或反向空行程可能使结果收窄。 @@ -198,7 +256,9 @@ SDK 诊断只进入日志，保留标定关节名、SDK 原名和具体内容， 每个任务结束、暂停或取消时原子保存。再次点击开始/重测创建新会话，不覆盖历史文件。 同目录 `samples.jsonl` 保存型号配置、示教、运动设置、命令、反馈、角点、时间戳和失败原因。 -新日志以 `scan_method: endpoint_search_v1` 标明流程，记录各端参考观测、首次运动指令、确认指令和最终边界；旧版粗扫/细扫日志仍按原采样结构复算。 +新日志以 `scan_method: endpoint_search_v10` 标明静止参考复核与分离运动门槛；记录阶段、各关节参与搜索的指令、参考观测、首次运动指令和最终边界。`confirmed_at` 与 `trigger` 均为首次检测到运动的指令。 +每个样本的 `window_samples` 保存稳定窗口的逐帧角点；`motion_trace` 保存目标首次发出时间戳及整个观测过程，缺失 Tag 显式记为空值，不跨缺失帧拼接证据。`boundary_found.evidence` 说明由稳定位置还是瞬时运动触发，以及当时的位移、抖动范围和证据帧。日志不保存原始视频，角点发生真实位移仍不能单独证明是手指、Tag 贴片或支架在动。 +`endpoint_search_v1` 至 `endpoint_search_v9` 和旧版粗扫/细扫日志仍按各自原算法复算。旧日志缺少新参考窗口及完整过程帧，不能用本次修改凭空修正旧结果，须重新测量；不会自动改写历史结果。 离线复算读取观测，重新运行范围算法，不连接硬件、不直接复制既有结果： ```bash diff --git a/src/linkerhand_range_calibration/config/profiles/o30_right.yaml b/src/linkerhand_range_calibration/config/profiles/o30_right.yaml index 2a839a9..a136723 100644 --- a/src/linkerhand_range_calibration/config/profiles/o30_right.yaml +++ b/src/linkerhand_range_calibration/config/profiles/o30_right.yaml @@ -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]} diff --git a/src/linkerhand_range_calibration/config/station.yaml b/src/linkerhand_range_calibration/config/station.yaml index 1e488a8..11d379e 100644 --- a/src/linkerhand_range_calibration/config/station.yaml +++ b/src/linkerhand_range_calibration/config/station.yaml @@ -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 diff --git a/src/linkerhand_range_calibration/linkerhand_range_calibration/core/diagnostics.py b/src/linkerhand_range_calibration/linkerhand_range_calibration/core/diagnostics.py new file mode 100644 index 0000000..927b669 --- /dev/null +++ b/src/linkerhand_range_calibration/linkerhand_range_calibration/core/diagnostics.py @@ -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 diff --git a/src/linkerhand_range_calibration/linkerhand_range_calibration/core/endpoints.py b/src/linkerhand_range_calibration/linkerhand_range_calibration/core/endpoints.py index 380dc71..39d8446 100644 --- a/src/linkerhand_range_calibration/linkerhand_range_calibration/core/endpoints.py +++ b/src/linkerhand_range_calibration/linkerhand_range_calibration/core/endpoints.py @@ -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} diff --git a/src/linkerhand_range_calibration/linkerhand_range_calibration/core/engine.py b/src/linkerhand_range_calibration/linkerhand_range_calibration/core/engine.py index 8b8c727..7d2e820 100644 --- a/src/linkerhand_range_calibration/linkerhand_range_calibration/core/engine.py +++ b/src/linkerhand_range_calibration/linkerhand_range_calibration/core/engine.py @@ -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 = {} diff --git a/src/linkerhand_range_calibration/linkerhand_range_calibration/core/legacy_endpoints.py b/src/linkerhand_range_calibration/linkerhand_range_calibration/core/legacy_endpoints.py new file mode 100644 index 0000000..16d3d49 --- /dev/null +++ b/src/linkerhand_range_calibration/linkerhand_range_calibration/core/legacy_endpoints.py @@ -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) + diff --git a/src/linkerhand_range_calibration/linkerhand_range_calibration/core/scan_plan.py b/src/linkerhand_range_calibration/linkerhand_range_calibration/core/scan_plan.py new file mode 100644 index 0000000..1a414ea --- /dev/null +++ b/src/linkerhand_range_calibration/linkerhand_range_calibration/core/scan_plan.py @@ -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), + ) diff --git a/src/linkerhand_range_calibration/linkerhand_range_calibration/core/trajectory.py b/src/linkerhand_range_calibration/linkerhand_range_calibration/core/trajectory.py index f23b843..b42135c 100644 --- a/src/linkerhand_range_calibration/linkerhand_range_calibration/core/trajectory.py +++ b/src/linkerhand_range_calibration/linkerhand_range_calibration/core/trajectory.py @@ -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)) diff --git a/src/linkerhand_range_calibration/linkerhand_range_calibration/profiles.py b/src/linkerhand_range_calibration/linkerhand_range_calibration/profiles.py index b99ab99..2a8efd8 100644 --- a/src/linkerhand_range_calibration/linkerhand_range_calibration/profiles.py +++ b/src/linkerhand_range_calibration/linkerhand_range_calibration/profiles.py @@ -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) diff --git a/src/linkerhand_range_calibration/linkerhand_range_calibration/replay.py b/src/linkerhand_range_calibration/linkerhand_range_calibration/replay.py index 6074472..3423dde 100644 --- a/src/linkerhand_range_calibration/linkerhand_range_calibration/replay.py +++ b/src/linkerhand_range_calibration/linkerhand_range_calibration/replay.py @@ -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'): diff --git a/src/linkerhand_range_calibration/linkerhand_range_calibration/runtime.py b/src/linkerhand_range_calibration/linkerhand_range_calibration/runtime.py index fd097ca..7f2f415 100644 --- a/src/linkerhand_range_calibration/linkerhand_range_calibration/runtime.py +++ b/src/linkerhand_range_calibration/linkerhand_range_calibration/runtime.py @@ -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), diff --git a/src/linkerhand_range_calibration/linkerhand_range_calibration/storage.py b/src/linkerhand_range_calibration/linkerhand_range_calibration/storage.py index bc274fe..427f34c 100644 --- a/src/linkerhand_range_calibration/linkerhand_range_calibration/storage.py +++ b/src/linkerhand_range_calibration/linkerhand_range_calibration/storage.py @@ -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): diff --git a/src/linkerhand_range_calibration/linkerhand_range_calibration/ui/window.py b/src/linkerhand_range_calibration/linkerhand_range_calibration/ui/window.py index 531f7d5..0bd24ae 100644 --- a/src/linkerhand_range_calibration/linkerhand_range_calibration/ui/window.py +++ b/src/linkerhand_range_calibration/linkerhand_range_calibration/ui/window.py @@ -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 '所需示教已完成') diff --git a/src/linkerhand_range_calibration/linkerhand_range_calibration/vision/motion.py b/src/linkerhand_range_calibration/linkerhand_range_calibration/vision/motion.py new file mode 100644 index 0000000..0f64ef0 --- /dev/null +++ b/src/linkerhand_range_calibration/linkerhand_range_calibration/vision/motion.py @@ -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 diff --git a/src/linkerhand_range_calibration/linkerhand_range_calibration/vision/observations.py b/src/linkerhand_range_calibration/linkerhand_range_calibration/vision/observations.py index 5faceae..7eaab6b 100644 --- a/src/linkerhand_range_calibration/linkerhand_range_calibration/vision/observations.py +++ b/src/linkerhand_range_calibration/linkerhand_range_calibration/vision/observations.py @@ -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} diff --git a/src/linkerhand_range_calibration/linkerhand_range_calibration/vision/reference.py b/src/linkerhand_range_calibration/linkerhand_range_calibration/vision/reference.py new file mode 100644 index 0000000..d916db3 --- /dev/null +++ b/src/linkerhand_range_calibration/linkerhand_range_calibration/vision/reference.py @@ -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 diff --git a/src/linkerhand_range_calibration/test/fixtures/four_finger_startup_shift.json b/src/linkerhand_range_calibration/test/fixtures/four_finger_startup_shift.json new file mode 100644 index 0000000..43535cf --- /dev/null +++ b/src/linkerhand_range_calibration/test/fixtures/four_finger_startup_shift.json @@ -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 + } + } + } +] diff --git a/src/linkerhand_range_calibration/test/fixtures/middle_subthreshold_startup_shift.json b/src/linkerhand_range_calibration/test/fixtures/middle_subthreshold_startup_shift.json new file mode 100644 index 0000000..3556025 --- /dev/null +++ b/src/linkerhand_range_calibration/test/fixtures/middle_subthreshold_startup_shift.json @@ -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 + } + ] +} diff --git a/src/linkerhand_range_calibration/test/fixtures/pinky_pitch_reversed_departure.json b/src/linkerhand_range_calibration/test/fixtures/pinky_pitch_reversed_departure.json new file mode 100644 index 0000000..b619f6c --- /dev/null +++ b/src/linkerhand_range_calibration/test/fixtures/pinky_pitch_reversed_departure.json @@ -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 + } + } + ] +} diff --git a/src/linkerhand_range_calibration/test/fixtures/pinky_pitch_small_final_offset.json b/src/linkerhand_range_calibration/test/fixtures/pinky_pitch_small_final_offset.json new file mode 100644 index 0000000..39ad60e --- /dev/null +++ b/src/linkerhand_range_calibration/test/fixtures/pinky_pitch_small_final_offset.json @@ -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 + } + } + ] +} diff --git a/src/linkerhand_range_calibration/test/fixtures/ring_gradual_startup_shift.json b/src/linkerhand_range_calibration/test/fixtures/ring_gradual_startup_shift.json new file mode 100644 index 0000000..1a12fc1 --- /dev/null +++ b/src/linkerhand_range_calibration/test/fixtures/ring_gradual_startup_shift.json @@ -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 + } + } +] diff --git a/src/linkerhand_range_calibration/test/fixtures/shared_translation_timeout.json b/src/linkerhand_range_calibration/test/fixtures/shared_translation_timeout.json new file mode 100644 index 0000000..599d696 --- /dev/null +++ b/src/linkerhand_range_calibration/test/fixtures/shared_translation_timeout.json @@ -0,0 +1,1102 @@ +{ + "source": "20260917_142749_918619, front tags 1-4, descending command 254", + "note": "Recorded timeout window only. No frames after this timeout are available.", + "issues": [ + { + "source": "tag", + "joint": "index_mcp_roll", + "view": "front", + "tag_id": 4, + "window_samples": [ + { + "stamp_ns": 1789626527738607441, + "corners": [ + [ + 682.3624877929686, + 599.2410888671873 + ], + [ + 737.2529907226561, + 598.8819580078124 + ], + [ + 736.5359497070312, + 540.5708618164064 + ], + [ + 681.651611328125, + 542.058349609375 + ] + ] + }, + { + "stamp_ns": 1789626527771949201, + "corners": [ + [ + 681.7247314453126, + 599.4792480468751 + ], + [ + 736.5308227539062, + 599.2413940429688 + ], + [ + 735.8697509765624, + 540.8587646484375 + ], + [ + 681.0892944335938, + 542.4291381835939 + ] + ] + }, + { + "stamp_ns": 1789626527838632721, + "corners": [ + [ + 680.4438476562501, + 599.3033447265627 + ], + [ + 735.3139038085936, + 598.9663696289062 + ], + [ + 734.6130981445314, + 540.6474609375 + ], + [ + 679.7249755859376, + 542.1278076171876 + ] + ] + }, + { + "stamp_ns": 1789626527871974481, + "corners": [ + [ + 680.9169311523438, + 598.8841552734376 + ], + [ + 736.0034179687501, + 598.5314941406249 + ], + [ + 735.2151489257812, + 540.2103271484374 + ], + [ + 680.558166503906, + 541.7347412109375 + ] + ] + }, + { + "stamp_ns": 1789626527905316241, + "corners": [ + [ + 681.7993164062498, + 598.6973876953124 + ], + [ + 736.86083984375, + 598.380859375 + ], + [ + 736.1094970703125, + 540.0558471679689 + ], + [ + 681.5477905273436, + 541.6141357421875 + ] + ] + }, + { + "stamp_ns": 1789626527938794268, + "corners": [ + [ + 682.1471557617188, + 599.0689086914064 + ], + [ + 737.1350097656249, + 598.6802368164064 + ], + [ + 736.4172973632812, + 540.3768920898438 + ], + [ + 681.6004638671876, + 541.8842163085938 + ] + ] + }, + { + "stamp_ns": 1789626528005477788, + "corners": [ + [ + 680.7386474609376, + 599.6327514648439 + ], + [ + 735.6223754882812, + 599.374267578125 + ], + [ + 734.9259643554688, + 541.0021972656249 + ], + [ + 680.246826171875, + 542.6068725585936 + ] + ] + }, + { + "stamp_ns": 1789626528038819548, + "corners": [ + [ + 680.6419677734376, + 599.4438476562501 + ], + [ + 735.432373046875, + 599.1944580078125 + ], + [ + 734.7528076171874, + 540.8372192382814 + ], + [ + 679.9002075195311, + 542.3644409179688 + ] + ] + }, + { + "stamp_ns": 1789626528105503068, + "corners": [ + [ + 681.7799072265625, + 598.6069946289061 + ], + [ + 736.8170776367189, + 598.3029174804689 + ], + [ + 736.0895385742186, + 539.9737548828126 + ], + [ + 681.5067138671875, + 541.505615234375 + ] + ] + }, + { + "stamp_ns": 1789626528138844828, + "corners": [ + [ + 681.9623413085938, + 598.8264160156251 + ], + [ + 737.0192871093751, + 598.4537963867186 + ], + [ + 736.2423706054688, + 540.1475219726564 + ], + [ + 681.5518188476561, + 541.6940917968752 + ] + ] + }, + { + "stamp_ns": 1789626528172186588, + "corners": [ + [ + 681.5726928710938, + 599.3531494140625 + ], + [ + 736.3883056640625, + 599.041259765625 + ], + [ + 735.6925048828125, + 540.7124633789062 + ], + [ + 680.8089599609375, + 542.2089233398438 + ] + ] + }, + { + "stamp_ns": 1789626528238870108, + "corners": [ + [ + 680.71337890625, + 599.5658569335938 + ], + [ + 735.5339965820312, + 599.3169555664061 + ], + [ + 734.8479614257812, + 540.9598999023438 + ], + [ + 680.0589599609375, + 542.5355834960938 + ] + ] + } + ], + "reason": "随机抖动 0.767 > 0.500 像素", + "noise": 1.0224897131685566, + "jitter_px": 0.7673631037667865, + "trend_px": 0.488607429807668, + "trend_allowance_px": 2.1403156173985924, + "trend_span_seconds": 0.500262667, + "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": 1789626527738607441, + "last_stamp_ns": 1789626528238870108, + "frames": 12 + }, + { + "source": "tag", + "joint": "middle_mcp_roll", + "view": "front", + "tag_id": 3, + "window_samples": [ + { + "stamp_ns": 1789626527738607441, + "corners": [ + [ + 581.7907104492189, + 600.7695922851562 + ], + [ + 635.0430908203125, + 586.7032470703125 + ], + [ + 619.7980346679688, + 530.7158813476562 + ], + [ + 566.4989624023438, + 545.9884643554688 + ] + ] + }, + { + "stamp_ns": 1789626527771949201, + "corners": [ + [ + 581.0524291992186, + 601.0728149414061 + ], + [ + 634.3506469726561, + 587.0545043945312 + ], + [ + 619.14404296875, + 531.02734375 + ], + [ + 565.8507080078125, + 546.3393554687498 + ] + ] + }, + { + "stamp_ns": 1789626527838632721, + "corners": [ + [ + 579.8452758789062, + 600.8051147460936 + ], + [ + 633.1129760742188, + 586.7771606445312 + ], + [ + 617.8923339843749, + 530.76708984375 + ], + [ + 564.57470703125, + 546.0017700195312 + ] + ] + }, + { + "stamp_ns": 1789626527871974481, + "corners": [ + [ + 580.4884033203124, + 600.4485473632814 + ], + [ + 633.7608032226561, + 586.3711547851564 + ], + [ + 618.5180053710938, + 530.3212280273436 + ], + [ + 565.2775268554688, + 545.6945190429686 + ] + ] + }, + { + "stamp_ns": 1789626527905316241, + "corners": [ + [ + 581.3423461914062, + 600.3016967773438 + ], + [ + 634.6672973632812, + 586.2359619140625 + ], + [ + 619.4064331054688, + 530.2001953125001 + ], + [ + 566.1976318359375, + 545.5598754882812 + ] + ] + }, + { + "stamp_ns": 1789626527938794268, + "corners": [ + [ + 581.670471191406, + 600.5828247070312 + ], + [ + 634.9181518554686, + 586.510498046875 + ], + [ + 619.7036132812501, + 530.4909057617186 + ], + [ + 566.4222412109375, + 545.8206787109374 + ] + ] + }, + { + "stamp_ns": 1789626528005477788, + "corners": [ + [ + 580.1260375976565, + 601.2293090820312 + ], + [ + 633.4365844726564, + 587.1987915039062 + ], + [ + 618.2078857421874, + 531.1688232421875 + ], + [ + 564.94775390625, + 546.5260009765625 + ] + ] + }, + { + "stamp_ns": 1789626528038819548, + "corners": [ + [ + 579.9505004882812, + 600.99951171875 + ], + [ + 633.2316284179686, + 586.9962158203126 + ], + [ + 618.0351562499999, + 530.9805297851562 + ], + [ + 564.7150268554688, + 546.2390747070311 + ] + ] + }, + { + "stamp_ns": 1789626528105503068, + "corners": [ + [ + 581.294921875, + 600.2120971679686 + ], + [ + 634.6081542968751, + 586.161865234375 + ], + [ + 619.3566284179688, + 530.1398315429689 + ], + [ + 566.138671875, + 545.4998168945311 + ] + ] + }, + { + "stamp_ns": 1789626528138844828, + "corners": [ + [ + 581.52685546875, + 600.3834228515625 + ], + [ + 634.7704467773438, + 586.3007202148436 + ], + [ + 619.55126953125, + 530.29541015625 + ], + [ + 566.3175659179688, + 545.6709594726564 + ] + ] + }, + { + "stamp_ns": 1789626528172186588, + "corners": [ + [ + 580.9052124023438, + 600.8876342773439 + ], + [ + 634.1644287109374, + 586.8487548828126 + ], + [ + 618.9612426757812, + 530.8597412109375 + ], + [ + 565.6531372070314, + 546.1040039062499 + ] + ] + }, + { + "stamp_ns": 1789626528238870108, + "corners": [ + [ + 580.0405883789061, + 601.1442260742189 + ], + [ + 633.3430175781249, + 587.126953125 + ], + [ + 618.1201782226562, + 531.1093749999999 + ], + [ + 564.8483886718751, + 546.4318237304688 + ] + ] + } + ], + "reason": "随机抖动 0.792 > 0.500 像素", + "noise": 1.0165292034532847, + "jitter_px": 0.791659542466096, + "trend_px": 0.5693921870236993, + "trend_allowance_px": 2.2080828151437473, + "trend_span_seconds": 0.500262667, + "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": 1789626527738607441, + "last_stamp_ns": 1789626528238870108, + "frames": 12 + }, + { + "source": "tag", + "joint": "ring_mcp_roll", + "view": "front", + "tag_id": 2, + "window_samples": [ + { + "stamp_ns": 1789626527738607441, + "corners": [ + [ + 468.01531982421875, + 624.8393554687498 + ], + [ + 520.2650146484374, + 606.0643310546874 + ], + [ + 501.66458129882807, + 552.4183349609376 + ], + [ + 450.4098815917969, + 572.1900024414062 + ] + ] + }, + { + "stamp_ns": 1789626527771949201, + "corners": [ + [ + 467.31802368164057, + 625.1397094726561 + ], + [ + 519.5863037109376, + 606.3606567382812 + ], + [ + 500.9960021972656, + 552.7167358398436 + ], + [ + 449.79458618164057, + 572.4486694335936 + ] + ] + }, + { + "stamp_ns": 1789626527838632721, + "corners": [ + [ + 466.0500183105468, + 624.8206176757814 + ], + [ + 518.306884765625, + 606.0779418945312 + ], + [ + 499.7321472167969, + 552.4192504882811 + ], + [ + 448.46490478515614, + 572.1686401367188 + ] + ] + }, + { + "stamp_ns": 1789626527871974481, + "corners": [ + [ + 466.62759399414057, + 624.4926147460939 + ], + [ + 518.9618530273438, + 605.7046508789062 + ], + [ + 500.38378906250006, + 552.0360717773438 + ], + [ + 449.2227478027344, + 571.7548828125001 + ] + ] + }, + { + "stamp_ns": 1789626527905316241, + "corners": [ + [ + 467.52758789062494, + 624.3761596679689 + ], + [ + 519.8609619140625, + 605.5826416015626 + ], + [ + 501.28851318359375, + 551.9244384765625 + ], + [ + 450.13732910156256, + 571.627197265625 + ] + ] + }, + { + "stamp_ns": 1789626527938794268, + "corners": [ + [ + 467.8620910644531, + 624.6155395507812 + ], + [ + 520.1478881835938, + 605.8532104492189 + ], + [ + 501.56494140625006, + 552.2025756835938 + ], + [ + 450.3174743652344, + 571.9486083984374 + ] + ] + }, + { + "stamp_ns": 1789626528005477788, + "corners": [ + [ + 466.35546875, + 625.2737426757811 + ], + [ + 518.6796875000001, + 606.5249633789064 + ], + [ + 500.0678100585937, + 552.8643798828125 + ], + [ + 448.92089843749983, + 572.5685424804685 + ] + ] + }, + { + "stamp_ns": 1789626528038819548, + "corners": [ + [ + 466.1922607421874, + 625.0068359375002 + ], + [ + 518.4630737304688, + 606.270751953125 + ], + [ + 499.87789916992193, + 552.635070800781 + ], + [ + 448.63208007812494, + 572.3427124023439 + ] + ] + }, + { + "stamp_ns": 1789626528105503068, + "corners": [ + [ + 467.478759765625, + 624.2969360351564 + ], + [ + 519.819091796875, + 605.5011596679688 + ], + [ + 501.2350158691406, + 551.8475341796875 + ], + [ + 450.08453369140625, + 571.5547485351562 + ] + ] + }, + { + "stamp_ns": 1789626528138844828, + "corners": [ + [ + 467.6828918457031, + 624.4551391601561 + ], + [ + 520.0101318359374, + 605.6632690429688 + ], + [ + 501.4256896972656, + 552.005126953125 + ], + [ + 450.25283813476585, + 571.7313232421875 + ] + ] + }, + { + "stamp_ns": 1789626528172186588, + "corners": [ + [ + 467.14520263671864, + 624.9090576171876 + ], + [ + 519.3858642578125, + 606.1693115234375 + ], + [ + 500.81048583984375, + 552.5579833984373 + ], + [ + 449.55072021484375, + 572.2560424804688 + ] + ] + }, + { + "stamp_ns": 1789626528238870108, + "corners": [ + [ + 466.2990722656249, + 625.1921997070315 + ], + [ + 518.5703735351562, + 606.4306030273439 + ], + [ + 499.997314453125, + 552.7822265624999 + ], + [ + 448.77737426757807, + 572.4921264648438 + ] + ] + } + ], + "reason": "随机抖动 0.785 > 0.500 像素", + "noise": 1.0116595951719103, + "jitter_px": 0.7847182284136107, + "trend_px": 0.5032291405592245, + "trend_allowance_px": 2.188722224572119, + "trend_span_seconds": 0.500262667, + "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": 1789626527738607441, + "last_stamp_ns": 1789626528238870108, + "frames": 12 + }, + { + "source": "tag", + "joint": "pinky_mcp_roll", + "view": "front", + "tag_id": 1, + "window_samples": [ + { + "stamp_ns": 1789626527738607441, + "corners": [ + [ + 375.4541931152343, + 673.229064941406 + ], + [ + 426.661376953125, + 653.2106323242186 + ], + [ + 407.7411804199218, + 600.1191406250001 + ], + [ + 357.469482421875, + 620.6364135742188 + ] + ] + }, + { + "stamp_ns": 1789626527771949201, + "corners": [ + [ + 374.7640686035157, + 673.4904174804686 + ], + [ + 425.94750976562506, + 653.4886474609374 + ], + [ + 407.0673828125, + 600.3782958984376 + ], + [ + 356.76272583007807, + 620.9140014648438 + ] + ] + }, + { + "stamp_ns": 1789626527838632721, + "corners": [ + [ + 373.4679260253906, + 673.1905517578125 + ], + [ + 424.68173217773443, + 653.170166015625 + ], + [ + 405.77893066406256, + 600.0721435546875 + ], + [ + 355.490478515625, + 620.5787353515625 + ] + ] + }, + { + "stamp_ns": 1789626527871974481, + "corners": [ + [ + 374.1032104492188, + 672.8207397460936 + ], + [ + 425.3378601074219, + 652.840087890625 + ], + [ + 406.4444885253906, + 599.7269287109377 + ], + [ + 356.1601867675782, + 620.2142944335936 + ] + ] + }, + { + "stamp_ns": 1789626527905316241, + "corners": [ + [ + 375.0165710449219, + 672.7108764648438 + ], + [ + 426.23220825195307, + 652.7081298828125 + ], + [ + 407.35723876953125, + 599.6029663085938 + ], + [ + 357.06552124023443, + 620.1124877929688 + ] + ] + }, + { + "stamp_ns": 1789626527938794268, + "corners": [ + [ + 375.29638671875006, + 672.9644165039061 + ], + [ + 426.54052734375006, + 652.9717407226562 + ], + [ + 407.62255859374994, + 599.88818359375 + ], + [ + 357.36199951171864, + 620.3686523437499 + ] + ] + }, + { + "stamp_ns": 1789626528005477788, + "corners": [ + [ + 373.82482910156244, + 673.6127929687499 + ], + [ + 425.0213623046875, + 653.6180419921875 + ], + [ + 406.14837646484375, + 600.505065917969 + ], + [ + 355.8135681152344, + 621.0410156250001 + ] + ] + }, + { + "stamp_ns": 1789626528038819548, + "corners": [ + [ + 373.63989257812494, + 673.3721923828125 + ], + [ + 424.82354736328125, + 653.3515625 + ], + [ + 405.9380493164062, + 600.2551269531249 + ], + [ + 355.61746215820307, + 620.7793579101561 + ] + ] + }, + { + "stamp_ns": 1789626528105503068, + "corners": [ + [ + 374.9942321777344, + 672.6427001953126 + ], + [ + 426.1871032714843, + 652.6387939453125 + ], + [ + 407.31622314453125, + 599.5458374023436 + ], + [ + 357.0220336914063, + 620.0499877929689 + ] + ] + }, + { + "stamp_ns": 1789626528138844828, + "corners": [ + [ + 375.17333984375, + 672.7915039062499 + ], + [ + 426.4070739746094, + 652.8006591796875 + ], + [ + 407.5098571777344, + 599.7074584960938 + ], + [ + 357.2473449707032, + 620.1854248046874 + ] + ] + }, + { + "stamp_ns": 1789626528172186588, + "corners": [ + [ + 374.5724182128906, + 673.311340332031 + ], + [ + 425.7458496093749, + 653.2886962890625 + ], + [ + 406.85546875000006, + 600.1936035156251 + ], + [ + 356.5634460449221, + 620.7167358398438 + ] + ] + }, + { + "stamp_ns": 1789626528238870108, + "corners": [ + [ + 373.74105834960943, + 673.5431518554686 + ], + [ + 424.9248046875001, + 653.5291748046875 + ], + [ + 406.0404052734375, + 600.4226074218751 + ], + [ + 355.70809936523426, + 620.9590454101562 + ] + ] + } + ], + "reason": "随机抖动 0.780 > 0.500 像素", + "noise": 1.0280826500857396, + "jitter_px": 0.7803970463285479, + "trend_px": 0.5634509296238571, + "trend_allowance_px": 2.176669659812511, + "trend_span_seconds": 0.500262667, + "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": 1789626527738607441, + "last_stamp_ns": 1789626528238870108, + "frames": 12 + } + ] +} diff --git a/src/linkerhand_range_calibration/test/test_candidate_direction.py b/src/linkerhand_range_calibration/test/test_candidate_direction.py new file mode 100644 index 0000000..0ae15b1 --- /dev/null +++ b/src/linkerhand_range_calibration/test/test_candidate_direction.py @@ -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} diff --git a/src/linkerhand_range_calibration/test/test_core.py b/src/linkerhand_range_calibration/test/test_core.py index b94ba2e..8d05f8d 100644 --- a/src/linkerhand_range_calibration/test/test_core.py +++ b/src/linkerhand_range_calibration/test/test_core.py @@ -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'] diff --git a/src/linkerhand_range_calibration/test/test_endpoints.py b/src/linkerhand_range_calibration/test/test_endpoints.py index f2a6b3e..fd2ea92 100644 --- a/src/linkerhand_range_calibration/test/test_endpoints.py +++ b/src/linkerhand_range_calibration/test/test_endpoints.py @@ -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] diff --git a/src/linkerhand_range_calibration/test/test_gui.py b/src/linkerhand_range_calibration/test/test_gui.py index 1957f6a..3bb830a 100644 --- a/src/linkerhand_range_calibration/test/test_gui.py +++ b/src/linkerhand_range_calibration/test/test_gui.py @@ -21,7 +21,7 @@ from linkerhand_range_calibration.ui.window import CalibrationWindow rclpy.init();app=QApplication([]) station=load_station();station['output_root']=str(Path(sys.argv[1])/'output') station['teaching_root']=str(Path(sys.argv[1])/'teaching') -station['scan'].update(rate=1000,stable_frames=4,settle_seconds=.01) +station['scan'].update(rate=1000,stable_frames=4,stable_seconds=.01,settle_seconds=.01) node=CalibrationRuntime(load_profile(),station,demo=True) window=CalibrationWindow(node);window.show() def until(condition,timeout=6): @@ -201,6 +201,43 @@ until(lambda:node.engine.state=='COMPLETED') assert first.slider.value()==node.teaching.data['baseline']['target'][0] assert index_control.slider.value()==node.teaching.data['baseline']['target'][2] assert all(control.isEnabled() for control in window.joint_controls) + +# The phased group shows all four actual targets, including the raised middle start. +window.task.setCurrentIndex(window.task.findData('four_finger_roll')) +assert '分阶段' in window.task.currentText() +window.start_one.click() +until(lambda:node.engine.state=='SCANNING' and node.engine.phase=='waiting') +assert node.engine.command[2:6]==(0,80,0,0) +assert node.snapshot()['scan_commands']['middle_mcp_roll']==80 +assert '同步测其余关节下限' in window.status.text() +assert all(text in window.status.text() for text in ['食指=0','中指=80','无名指=0','小指=0']) +assert not any(control.isEnabled() for control in window.joint_controls) + +# The ROS detection callback preserves the rejected Tag's reason through UI and journal. +from types import SimpleNamespace as NS +node.camera_info['front']=(1624,1240) +message=NS(header=NS(stamp=node.get_clock().now().to_msg()),detections=[]) +for tag,points in node.adapter.frames()['front'].items(): + message.detections.append(NS(id=tag,family='36h11',hamming=0, + decision_margin=0 if tag==2 else 100,corners=[NS(x=x,y=y) for x,y in points])) +node._detections('front',message) +node.engine._pause_sampling_timeout(time.monotonic()) +node._tick();window.refresh() +assert '正面 Tag ID2（ring_mcp_roll）：Tag 解码质量不足' in window.message.text() +events=[json.loads(line) for line in (node.session.directory/'samples.jsonl').read_text().splitlines()] +failure=next(event for event in reversed(events) if event['kind']=='sampling_timeout') +assert any(issue.get('tag_id')==2 and issue['reason']=='Tag 解码质量不足' + for issue in failure['issues']) + +# Stream-level failures likewise identify affected tags from the current requirements. +node.demo=False +now=time.monotonic() +node.camera_received['front']=now +node.image_times['front']=now-2 +error=node._vision_error({'ring_mcp_roll':('front',2)},now) +assert '正面 Tag ID2（ring_mcp_roll）' in error and '检测流未收到或断流' in error +node.demo=True +window.cancel.click();until(lambda:node.engine.state=='CANCELLED') window.close();node.close_session();node.destroy_node();rclpy.shutdown() print('GUI workflow passed') ''' diff --git a/src/linkerhand_range_calibration/test/test_observation_evidence.py b/src/linkerhand_range_calibration/test/test_observation_evidence.py new file mode 100644 index 0000000..a182922 --- /dev/null +++ b/src/linkerhand_range_calibration/test/test_observation_evidence.py @@ -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) diff --git a/src/linkerhand_range_calibration/test/test_reference_platform.py b/src/linkerhand_range_calibration/test/test_reference_platform.py new file mode 100644 index 0000000..1efbf80 --- /dev/null +++ b/src/linkerhand_range_calibration/test/test_reference_platform.py @@ -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} diff --git a/src/linkerhand_range_calibration/test/test_sampling_diagnostics.py b/src/linkerhand_range_calibration/test/test_sampling_diagnostics.py new file mode 100644 index 0000000..4b5e637 --- /dev/null +++ b/src/linkerhand_range_calibration/test/test_sampling_diagnostics.py @@ -0,0 +1,209 @@ +"""Timeout explanations must identify actual blockers before windows are cleared.""" +from dataclasses import replace + +import numpy as np +import pytest + +from linkerhand_range_calibration.adapters.base import Feedback +from linkerhand_range_calibration.adapters.fake import FakeAdapter +from linkerhand_range_calibration.core.engine import Engine +from linkerhand_range_calibration.profiles import Settings, load_profile +from linkerhand_range_calibration.simulation import simulated_teaching +from linkerhand_range_calibration.vision.observations import Observation, StableWindow +from linkerhand_range_calibration.vision.reference import ReferenceWindow + + +class SamplingRun: + def __init__(self, task='thumb_cmc_roll', phase='waiting'): + profile = load_profile() + settings = Settings(rate=1000, stable_frames=4, stable_seconds=.15, reference_seconds=.45, + settle_seconds=.05, point_timeout=.8) + self.adapter = FakeAdapter(profile) + self.events, self.now = [], 1.0 + self.engine = Engine(profile, settings, self.adapter, self.events.append) + self.engine.start(simulated_teaching(profile, self.adapter.uid), self.now, + round(self.now * 1e9), [task]) + for _ in range(100): + self.step() + if self.engine.state == 'SCANNING' and self.engine.phase == phase: + break + assert self.engine.state == 'SCANNING' and self.engine.phase == phase + + def step(self, change=None): + self.now = round(self.now + .05, 6) + stamp = round(self.now * 1e9) + self.adapter.advance(self.now, stamp) + frames, rejected = self.adapter.frames(), {} + if change: + change(frames, rejected) + for view, frame in frames.items(): + self.engine.observe(view, stamp, frame, self.now, rejected.get(view)) + self.engine.tick(self.now, stamp) + + def timeout(self, change): + before = len(self.adapter.sent) + for _ in range(40): + self.step(change) + if self.engine.state == 'PAUSED': + break + assert self.engine.state == 'PAUSED', self.engine.reason + assert len(self.adapter.sent) == before # A diagnostic must not move the hand. + assert all(not window.values for window in self.engine.windows.values()) + return next(event for event in reversed(self.events) if event['kind'] == 'sampling_timeout') + + +@pytest.mark.parametrize('task,noisy,expected', [ + ('thumb_cmc_roll', [0], {'thumb_cmc_roll'}), + ('four_finger_roll', [2], {'ring_mcp_roll'}), + ('four_finger_roll', [2, 4], {'ring_mcp_roll', 'index_mcp_roll'}), +]) +def test_current_unstable_metrics_identify_only_affected_tags(task, noisy, expected): + run = SamplingRun(task) + + def creep(frames, rejected): + for tag in noisy: + frames['front'][tag] = (np.asarray(frames['front'][tag]) + [run.now * 8, 0]).tolist() + + event = run.timeout(creep) + assert {issue['joint'] for issue in event['issues']} == expected + for issue in event['issues']: + assert f'正面 Tag ID{issue["tag_id"]}（{issue["joint"]}）' in run.engine.reason + assert issue['drift_px'] > issue['drift_limit_px'] + assert issue['jitter_px'] < issue['noise_limit_px'] + assert '持续漂移' in issue['reason'] and '随机抖动' not in issue['reason'] + assert issue['last_stamp_ns'] == round(run.now * 1e9) + assert len(issue['window_samples']) == issue['frames'] + # These failures occur while establishing the endpoint reference, whose + # window is longer than a normal command-point window. + recovered = (ReferenceWindow(run.engine.settings) if issue['joint'] in run.engine.searches + else StableWindow(run.engine.settings.stable_frames, run.engine.settings.stable_seconds)) + for frame in issue['window_samples']: + recovered.add(Observation(frame['stamp_ns'], 0, frame['corners'])) + assert recovered.problem(.5, .1)['drift_px'] == pytest.approx(issue['drift_px']) + assert event['target'] == list(run.engine.command) + + +def test_four_finger_sampling_advances_with_random_jitter_above_old_limit(): + run = SamplingRun('four_finger_roll') + + def jitter(frames, rejected): + offset = .25 * (-1) ** round(run.now * 20) + for tag in (1, 2, 3, 4): + frames['front'][tag] = (np.asarray(frames['front'][tag]) + [offset, 0]).tolist() + + for _ in range(15): + run.step(jitter) + assert run.engine.state != 'PAUSED', run.engine.reason + samples = [event for event in run.events if event['kind'] == 'sample'] + if samples: + break + assert samples + for observation in samples[0]['observations'].values(): + assert .1 < observation['jitter_px'] <= .5 + assert observation['drift_px'] <= .1 + + +@pytest.mark.parametrize('deadline,settles_at', [(3, 4), (6, 4), (6, 0), (6, None)]) +def test_waits_for_transient_vibration_without_advancing_or_relaxing_limits(deadline, settles_at): + run = SamplingRun('four_finger_roll') + run.engine.settings = replace(run.engine.settings, point_timeout=deadline) + started, sent = run.now, len(run.adapter.sent) + + def shake(frames, rejected): + # Synthetic transient: unlike the recorded fixture, this deliberately + # supplies a known recovery after four seconds to exercise the deadline. + if settles_at is None or run.now - started < settles_at: + offset = .85 * (-1) ** round(run.now * 20) + for tag in (1, 2, 3, 4): + frames['front'][tag] = (np.asarray(frames['front'][tag]) + [offset, 0]).tolist() + + for _ in range(140): + run.step(shake) + samples = [event for event in run.events if event['kind'] == 'sample'] + if samples or run.engine.state == 'PAUSED': + break + assert len(run.adapter.sent) == sent + if settles_at is None or settles_at >= deadline: + assert run.engine.state == 'PAUSED' and not samples + assert deadline < run.now - started <= deadline + .1 + timeout = next(event for event in reversed(run.events) if event['kind'] == 'sampling_timeout') + assert '共同平移' in run.engine.reason + assert timeout['shared_motion'][0]['tag_ids'] == [1, 2, 3, 4] + else: + assert samples and run.engine.state == 'SCANNING' + assert settles_at <= run.now - started < settles_at + 1 + for observation in samples[0]['observations'].values(): + assert observation['jitter_px'] <= .5 and observation['drift_px'] <= .1 + + +@pytest.mark.parametrize('phase', ['moving', 'waiting']) +@pytest.mark.parametrize('reason', [None, 'Tag 像素尺寸不足']) +def test_missing_or_rejected_tag_does_not_blame_other_group_tags(phase, reason): + run = SamplingRun('four_finger_roll', phase) + + def lose_ring(frames, rejected): + frames['front'].pop(2) + if reason: + rejected['front'] = {2: reason} + + event = run.timeout(lose_ring) + assert len(event['issues']) == 1 + assert event['issues'][0]['joint'] == 'ring_mcp_roll' + assert event['issues'][0]['reason'] == (reason or '未检出有效 Tag') + assert '正面 Tag ID2（ring_mcp_roll）' in run.engine.reason + assert 'ID3' not in run.engine.reason and 'ID4' not in run.engine.reason + + +def test_feedback_instability_names_the_joint_without_accusing_tags(): + run = SamplingRun('pinky_pip') + joint = run.engine.profile.by_name['pinky_pip'] + + def feedback_moves(frames, rejected): + values = list(run.adapter.feedback.positions) + values[joint.index] = round(run.now * 20) % 2 * 5 + run.adapter.feedback = Feedback(round(run.now * 1e9), run.now, tuple(values)) + + event = run.timeout(feedback_moves) + assert len(event['issues']) == 1 + assert event['issues'][0]['source'] == 'feedback' + assert event['issues'][0]['joint'] == 'pinky_pip' + assert '反馈未稳定' in run.engine.reason and 'Tag' not in run.engine.reason + + +def test_stale_tag_reports_correct_camera_and_id(): + run = SamplingRun('pinky_pip') + event = run.timeout(lambda frames, rejected: frames.pop('side')) + assert len(event['issues']) == 1 + assert '侧面 Tag ID5（pinky_pip）' in run.engine.reason + assert '有效观测已过期' in run.engine.reason + + +def test_rejection_is_replaced_only_by_a_new_detector_frame(): + run = SamplingRun() + stamp = round((run.now + .01) * 1e9) + run.engine.observe('front', stamp, {}, run.now, {0: 'Tag 解码质量不足'}) + run.engine.observe('front', stamp - 1, run.adapter.frames()['front'], run.now) + assert run.engine._visibility_issues(run.now)[0]['reason'] == 'Tag 解码质量不足' + run.engine.observe('front', stamp + 1, run.adapter.frames()['front'], run.now) + assert not run.engine.rejected_observations + # Recovery before the gate must still explain why the image cannot be sampled. + assert '新图像' in run.engine._visibility_issues(run.now)[0]['reason'] + run.engine.observe('front', run.engine.gate_ns, run.adapter.frames()['front'], run.now) + assert not run.engine._visibility_issues(run.now) + + +def test_window_explains_count_duration_and_recovery_without_stale_metrics(): + window = StableWindow(4, .5) + points = np.array([[100, 100], [140, 100], [140, 140], [100, 140]]) + for i in range(4): + window.add(Observation(i * 100_000_000, i / 10, points + i * .2)) + assert '时长不足' in window.problem(.5, .1)['reason'] + window.add(Observation(500_000_000, .5, points + 1)) + assert '持续漂移' in window.problem(.5, .1)['reason'] + window.clear() + window.add(Observation(600_000_000, .6, points)) + problem = window.problem(.5, .1) + assert problem == {'reason': '有效新图像不足：1/4 帧', 'frames': 1} + for i in range(7, 13): + window.add(Observation(i * 100_000_000, i / 10, points)) + assert window.problem(.5, .1) is None diff --git a/src/linkerhand_range_calibration/test/test_settling.py b/src/linkerhand_range_calibration/test/test_settling.py new file mode 100644 index 0000000..cd7d562 --- /dev/null +++ b/src/linkerhand_range_calibration/test/test_settling.py @@ -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'] != '成功' diff --git a/src/linkerhand_range_calibration/test/test_shared_motion.py b/src/linkerhand_range_calibration/test/test_shared_motion.py new file mode 100644 index 0000000..3d7cca2 --- /dev/null +++ b/src/linkerhand_range_calibration/test/test_shared_motion.py @@ -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) == [] diff --git a/src/linkerhand_range_calibration/test/test_startup_evidence.py b/src/linkerhand_range_calibration/test/test_startup_evidence.py new file mode 100644 index 0000000..754cfcd --- /dev/null +++ b/src/linkerhand_range_calibration/test/test_startup_evidence.py @@ -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} diff --git a/src/linkerhand_range_calibration/test/test_temporal_motion.py b/src/linkerhand_range_calibration/test/test_temporal_motion.py new file mode 100644 index 0000000..f3bb236 --- /dev/null +++ b/src/linkerhand_range_calibration/test/test_temporal_motion.py @@ -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 + diff --git a/src/linkerhand_range_calibration/test/test_transient_motion.py b/src/linkerhand_range_calibration/test/test_transient_motion.py new file mode 100644 index 0000000..1a02ad3 --- /dev/null +++ b/src/linkerhand_range_calibration/test/test_transient_motion.py @@ -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)