标定稳定性
This commit is contained in:
@@ -15,18 +15,34 @@ ros2 run g20_thumb_apriltag_calibration calibrate_g20_right
|
||||
当场剔除并从其在扫掠顺序中的原始位置重采，避免全部任务采完后才在最终验收
|
||||
失败、把会话拉回靠前的关节。运行中的多视角任务按正面主测量和侧面校验测量
|
||||
独立保留；单轮转轴异常且其余三轮形成一致簇时只补扫异常轮的两个方向。侧面
|
||||
校验视角的任务级有效率只记录为诊断，完整端点、分箱覆盖和模型质量门限仍保持
|
||||
不变。需要强制从第一个关节
|
||||
轴线位置若也能明确定位为单轮异常，同样只补扫该轮；补扫会保留任务预检和前次
|
||||
采集确定的PnP分支参考，不会因重新初始化切换到另一组平面Tag镜像解。侧面
|
||||
校验视角的任务级有效率只记录为诊断；G20右手预检若逐帧识别率低于标称值，
|
||||
但同步有效位姿已经完整覆盖端点、中点、最小分箱数和最大分箱空洞，也按完整
|
||||
轨迹通过。正式扫描仍逐方向执行相同的硬分箱覆盖检查，轴线、曲线和模型质量
|
||||
门限保持不变。每个任务的低速往返预检、首轮交接和四轮双向正式扫描属于同一
|
||||
采集事务：相邻方向共享已验证端点和任务级PnP参考。G20右手正式扫描固定使用
|
||||
产品审定速度，不再根据单次识别密度自动提速，确保不同会话测量的是同一动态
|
||||
过程。需要强制从第一个关节
|
||||
重新采集时使用 `--no-resume`。升级前已经分别完成的正面/侧面roll也会合并为
|
||||
一个完整同步任务断点；只有两边数据都完整时才复用。
|
||||
|
||||
命令自动完成产品哈希预检、运动、当前任务补扫、前三轮训练、第四轮隔离留出、
|
||||
15个会话数据求解主动关节URDF零位修正（`thumb_mcp`固定CAD参考）、
|
||||
16个会话数据求解主动关节URDF零位修正、
|
||||
21条视觉实测命令曲线发布；四指PIP/DIP的动态曲线均实测，四指DIP静态零位保留CAD。
|
||||
终端只显示中文进度和问题；失败时复制“请复制以下内容给开发者”块即可。
|
||||
|
||||
四指末端的16 mm Tag允许使用刚性延长杆避挡；软件不假设末端Tag平面与中节Tag
|
||||
平面平行。延长杆和Tag在一次标定期间必须完全刚性，不能晃动、扭转或重新调整。
|
||||
侧面掌部基准Tag（ID 4）与各活动指节Tag也不要求安装面平行：首次联合PnP使用
|
||||
静态多帧刚性、重投影误差和跨轮任务参考选择分支，不再用固定15°安装角门限阻断扫描。
|
||||
单Tag独立位姿仍保留75°倾角保护；对包含锁定掌部基准和完整父子链的任务，倾角保护只
|
||||
限制独立选择，不会在联合选择前删除正深度、低重投影的IPPE候选。联合跟踪继续用相邻帧
|
||||
绝对/相对位姿连续性约束这些斜视候选，最终轴线残差、四轮重复性和隔离留出门限不放宽。
|
||||
终端中的Tag计数表示“可见”；等待扫描起点时会另列PnP初始化进度和累计拒绝原因。
|
||||
若联合候选仍然失败，`raw_samples.jsonl` 会按8个反馈计数的区间保存
|
||||
`group_pnp_candidate_event`，其中包含缺失角色、逐Tag候选数/倾角/重投影、角点和相机内参
|
||||
哈希，可直接定位运动到哪个机械位置后开始失效，而不需要再次盲扫整条流程。
|
||||
|
||||
正式结果位于 `calibration_output/G20_RIGHT_001/latest_passed`。该指针只在
|
||||
JSON、URDF数值等价、mesh完整性、21条曲线CAD限位、被动关节保护和隔离留出验证
|
||||
@@ -64,19 +80,22 @@ ros2 launch g20_thumb_apriltag_calibration three_camera_calibration.launch.py \
|
||||
固定掌部位姿。小指和无名指弯曲避让会遮住正面ID 0，因此四指正面+侧面同步roll中
|
||||
允许ID 0暂时不可见，并使用本会话基准锁定值；运动连杆Tag仍必须实时可见，门限不
|
||||
放宽。终端用`锁`表示该固定参考有效，例如`正面[0锁,12✓]`，`✗`才表示需要处理的
|
||||
实时Tag。锁定后不得移动相机、手掌底座或整只手，否则缓存参考失效，必须重新启动
|
||||
标定。任务级Tag有效率门限按"当前任务所需角色生效期间的采集帧"统计；
|
||||
实时Tag。锁定后本次标定运行中不得再移动相机、手掌底座或整只手，否则缓存参考
|
||||
失效，必须重新启动标定；两次独立会话之间轻微调整整只手的位置不会改变机械端点
|
||||
零位基准。任务级Tag有效率门限按"当前任务所需角色生效期间的采集帧"统计；
|
||||
任务结束后角色要求会切回预检全套标签，静止期帧不参与该门限，避免把
|
||||
采集质量良好的任务误判为可见性失败。
|
||||
|
||||
恢复末端Tag后，程序能够独立实测四指PIP和DIP的转轴及动态命令曲线。但同一次
|
||||
相机外参和同一套Tag安装下的重复扫描无法排除固定安装相位偏差；实体手在反馈0端
|
||||
能够触掌是独立的机械端点约束。因此四指MCP pitch、PIP的URDF静态零位由本次
|
||||
实测全屈曲行程与CAD触掌角之差求出，不再把同相机的平行轴PnP相位直接当作绝对
|
||||
零位，也不写死为0。生成URDF时同步修正这些关节的坐标上限和DIP mimic坐标偏置，
|
||||
能够触掌是独立的机械端点约束。三个thumb CMC主动轴、`thumb_mcp`、四指MCP pitch
|
||||
和PIP的URDF静态零位均由本次实测全行程与CAD机械端点之差求出，不再把跨相机的
|
||||
平面PnP绝对相位直接当作编码器零位，也不写死为0。生成URDF时同步修正这些关节的
|
||||
坐标上限以及相关被动关节的mimic坐标偏置，
|
||||
保证非零零偏不会缩短最大闭合量。末节Tag继续用于DIP动态曲线、轴线质量、遮挡和
|
||||
第四轮留出检查。正式数据求解静态修正范围为拇指CMC三个主动关节、四指MCP roll、
|
||||
MCP pitch及PIP，共15个；只有`thumb_mcp`固定为既有CAD参考。
|
||||
第四轮留出检查。正式数据求解静态修正范围为拇指CMC三个主动关节、`thumb_mcp`、
|
||||
四指MCP roll、MCP pitch及PIP，共16个。`thumb_mcp`与四指屈伸关节一样使用
|
||||
实测全屈曲行程和CAD机械端点联合求解，不直接采用MCP/IP耦合运动的单目PnP相位。
|
||||
侧面累计避障按“PIP→MCP pitch→roll”的安全顺序分阶段进入，并按逆序分阶段退出；
|
||||
同类辅助电机（全部邻指滚转、全部PIP、全部MCP pitch）合并为同一个并行航点同时
|
||||
运动，被测通道最后单独进入。“滚转全部回中前不展开弯曲手指”“每指pitch先于PIP”
|
||||
@@ -87,7 +106,8 @@ MCP pitch及PIP，共15个；只有`thumb_mcp`固定为既有CAD参考。
|
||||
当前已经在位（含反馈容差）的辅助电机保持原位，只有下一组不再使用的避障电机
|
||||
退回基准，避免"先展开回基准、马上又折回"的多余动作；已评审的
|
||||
"滚转先回中再展开""先滚开再弯曲"顺序保持不变。预检正反方向若都保留至少64个电机分箱且最大空缺
|
||||
不超过8，会把非roll任务四轮正式速度最多提高到预检速度的1.5倍；否则保持原保守速度。
|
||||
不超过8，只作为采集能力诊断。G20右手四轮正式速度始终使用产品配置的固定值，
|
||||
不会因本次预检帧率或识别密度而改变；旧11-Tag布局仍保留自适应速度兼容逻辑。
|
||||
四指roll不再把同一反馈127误当成方向无关的唯一机械姿态：以`255→127`为标准物理
|
||||
零位，反向到达127的实测偏差保留在`increasing_rad`中。方向分支间隙上限1.5°、
|
||||
四轮间隙极差上限0.3°；其他关节仍使用严格的0.5°baseline回差门限。
|
||||
@@ -96,7 +116,8 @@ MCP pitch及PIP，共15个；只有`thumb_mcp`固定为既有CAD参考。
|
||||
方向分支检查和动态曲线的127相位都使用这两组双向静止数据，运动中经过127的帧不再
|
||||
替代静态保持姿态。
|
||||
前三轮只用于训练，第四轮完全留出；留出轮不参与显著性、Student-t置信区间或最终重拟合。
|
||||
每轮PnP都清空帧间跟踪状态并重新执行8帧静态初始化，但同一任务第1轮
|
||||
每个任务只在低速递减预检起点执行一次8帧PnP静态初始化；预检往返和四轮正式
|
||||
扫描连续复用同一帧间分支与任务参考，不再让每一轮独立选择平面Tag解。同一任务第1轮
|
||||
已确立的端点相对姿态作为后3轮的分支锚点，防止独立初始化选到相反的
|
||||
IPPE镜像解。baseline标准接近和全部质量门限保持不变。
|
||||
电机15任务会利用源URDF中已确认的`thumb_ip mimic=1.03`，只在逐帧IPPE双解中
|
||||
@@ -105,26 +126,31 @@ IPPE镜像解。baseline标准接近和全部质量门限保持不变。
|
||||
|
||||
当前19-Tag产品流程发布精简schema v4：21条运行时曲线全部来自当前会话的视觉实测。
|
||||
URDF零位字段覆盖拇指4个主动关节和四指各自的`mcp_roll/mcp_pitch/pip`，共16个，
|
||||
其中四指`mcp_pitch/pip`共8个字段由视觉行程+机械端点联合求解，只有`thumb_mcp`
|
||||
字段固定为0；
|
||||
其中三个thumb CMC轴、`thumb_mcp`及四指`mcp_pitch/pip`共12个字段由实测旋转行程
|
||||
与机械端点联合求解；
|
||||
写出非零`thumb_mcp`零偏时同步平移其关节坐标上限，并更新被动`thumb_ip`的
|
||||
`mimic offset`，因此不会改变CAD定义的最大屈曲实体姿态；
|
||||
`thumb_ip`及四指DIP静态零位保留源CAD。旧schema v5文件仅作历史回放兼容，
|
||||
当前一键流程不再生成它。正面/侧面roll在同一次运动中独立拟合；方向、
|
||||
轴线和动态曲线均通过时做不确定度加权轴融合。侧面PIP连杆标签在滚转扫掠中
|
||||
相对侧相机视线倾斜约13°~20°，平面标签的单目IPPE姿态二义性会给侧视姿态引入
|
||||
数度的系统性"绕视线"偏差（亚像素重投影无法发现，会话20260820_105535实测
|
||||
前后轴向稳定相差11.4°），因此跨视角方向差超过0.75°融合门限时不再判定任务
|
||||
失败：程序记录`cross_view_roll_axis_diagnostic`诊断、跳过融合并采用可信的
|
||||
前视轴向；仅当差值超过粗错误兜底上限（`cross_view_roll_maximum_axis_difference_deg`
|
||||
默认15°或线距超过`cross_view_roll_maximum_axis_line_difference_mm`默认30mm，
|
||||
对应标签贴错连杆或标签松动）时才失败。前视侧摆连杆与侧视PIP连杆的轴线
|
||||
本身存在约21mm的系统性位置差（丝杠平移连杆），属预期现象。侧视校验通道
|
||||
前后轴向稳定相差11.4°），因此侧视PIP连杆姿态不再参与MCP轴向融合或角曲线验收，
|
||||
正侧姿态差只写入`cross_view_roll_axis_diagnostic`。四根MCP侧摆轴在产品URDF中
|
||||
严格平行：小指作为先采集的参考轴，其余三指复用该公共方向并各自独立拟合轴线位置，
|
||||
避免平面PnP分支在不同会话中改变轴向。前视侧摆连杆受丝杆平移影响，其纯旋转拟合得到的是
|
||||
随手指结构变化的伪轴线，不能与侧视PIP连杆的物理轴线使用统一距离门限；
|
||||
两者线距仅记录在诊断中。侧视校验通道
|
||||
（`*_mcp_roll_side`）的
|
||||
分支间隙跨轮极差上限放宽为
|
||||
`cross_view_roll_alias_maximum_branch_gap_range_deg`默认0.5°（绝对间隙1.5°上限
|
||||
不变），其`axis_pose_line_rms`降为诊断，不再作为准入门限；径向、平面、
|
||||
圆一致性等其余数据质量门限全部保留。侧面端视roll的圆轨迹方向已经受
|
||||
姿态分支间隙跨轮极差和独立姿态轴方向极差只作诊断，不触发重复采集；这两个量来自
|
||||
近掠射平面Tag的非发布姿态分量。绝对分支间隙1.5°上限保持不变，真正发布的正面主轴
|
||||
仍使用原跨轮严格门限。侧视逐帧`axis_pose_line_rms`同样只作诊断，组合轴线改用四轮
|
||||
位置RMS验收；径向、平面、圆一致性及可见性门限全部保留。
|
||||
侧面端视roll的圆轨迹方向已经受
|
||||
姿态轴约束，因此自由三维圆平面与姿态轴的夹角只保留诊断，不再被重复作为硬门限；
|
||||
径向残差、SE(3)轴线残差和正侧面轴/曲线一致性仍是硬门限。任一静态目标、第四轮留出、
|
||||
径向残差和四轮轴线位置RMS仍是硬门限。正式MCP动态曲线统一使用正面Tag中心的
|
||||
二维投影圆角度，侧面姿态曲线仅保留为诊断；任一正式视角自身四轮不重复或第四轮
|
||||
留出失败仍会拒绝发布。任一静态目标、第四轮留出、
|
||||
遮挡、PnP或跨机位检查失败时，只保留原始轨迹和`passed:false`诊断，不发布正式URDF。
|
||||
8个组合姿态仅保留为开发诊断，正式产品默认不执行。轴线零位求解不提供适合绝对笛卡尔
|
||||
位置验收的手基座变换，因此不能用该诊断推翻已经通过的单关节隔离留出结果。三个CMC轴恢复使用
|
||||
@@ -335,13 +361,14 @@ Tag位置，接近轴向观察时改用相机图像平面相位并丢弃单目Pn
|
||||
约4°的整指倾斜；重复扫描与同源留出不能排除这种系统偏差，因此不得写入URDF。
|
||||
|
||||
四指MCP屈伸和PIP采用同一静态策略：参考指轨迹仍参与动态曲线、轴质量和机构诊断，
|
||||
但拟合出的绝对相位不写入任何一根四指的 `origin.rpy`。拇指CMC roll/yaw/pitch的非零
|
||||
修正只能来自当前会话的三轮轨迹求解并通过第三轮留出验证；代码和配置中不保存任何
|
||||
按左右手或序列号写死的拇指零位角。电机5的256点动态曲线也使用本机三轮实测结果。
|
||||
但拟合出的共同掌坐标相位不写入四指 `origin.rpy`；只发布各指相对四指中值的实测
|
||||
装配偏差。拇指CMC roll/yaw/pitch的非零修正来自当前会话的完整相对旋转行程与机械
|
||||
端点，视觉轴链继续用于轴线、PnP和留出诊断；代码和配置中不保存任何按左右手或
|
||||
序列号写死的拇指零位角。电机5的256点动态曲线同样使用本机四轮实测结果。
|
||||
|
||||
7个直接零位依赖链为：yaw轴约束拇指roll、pitch轴约束拇指yaw、MCP轴线相位约束
|
||||
拇指pitch；IP轴线相位仅作诊断，不能覆盖拇指MCP的原始CAD零位。参考指MCP pitch轴
|
||||
约束roll，PIP/DIP轴线相位只用于参考指机构诊断，不再覆盖四指CAD静态零位。
|
||||
视觉依赖链仍为：yaw轴检查拇指roll、pitch轴检查拇指yaw、MCP轴线相位检查拇指
|
||||
pitch、IP轴线相位检查拇指MCP。该链用于几何和PnP诊断，不再决定四个具有机械端点
|
||||
的拇指主动关节绝对零位；四指PIP/DIP轴线相位也继续用于机构诊断。
|
||||
原始URDF的 `origin.xyz`、`axis.xyz`、连杆长度、mesh和被动结构固定。yaw扫描时电机5
|
||||
保持145，求解器使用实测 `angle_rad[145]` 还原该条件，不会把145误当成baseline。
|
||||
偏移超过各关节专用上限时整次失败。数值求解会在更宽的诊断范围内继续估计，因此状态和原始JSONL
|
||||
@@ -431,9 +458,9 @@ calibration_output/G20_RIGHT_001/<时间戳>/
|
||||
`zero_angles.urdf_zero_offset_rad`、5个被动标记、模板来源和总体质量。新URDF每次从
|
||||
指定原始CAD文件生成，采用 `T_original × Rot(axis, offset)`，绝不叠加旧校准文件或
|
||||
覆盖原文件；只有通过独立求解验证或明确机械装配基准授权的主动关节 `origin.rpy` 可能
|
||||
改变；当前19-Tag右手保留16个主动静态零位字段，其中15个由本会话数据求解，只有
|
||||
`thumb_mcp`固定为原始CAD零位0；四指DIP为被动关节，发布实测动态曲线并保留CAD
|
||||
静态零位，其mimic坐标偏置只随上游PIP坐标系变换作等价调整。
|
||||
改变；当前19-Tag右手的16个主动静态零位字段全部由本会话数据求解；四指DIP和
|
||||
`thumb_ip`为被动关节，发布实测动态曲线并保留CAD静态零位，其mimic坐标偏置只随
|
||||
上游主动关节坐标系变换作等价调整。
|
||||
未观测关节和其他URDF文本保持不变。源URDF中的相对mesh资源会按原相对路径复制到
|
||||
同一会话，保证会话内URDF可独立加载，并在正式发布时逐文件记录SHA256。每帧Tag SE(3)、
|
||||
图像时间戳、20通道状态、同步误差和PnP误差只进入 `raw_samples.jsonl`。
|
||||
|
||||
@@ -1,6 +1,7 @@
|
||||
schema_version: 1
|
||||
model: G20
|
||||
side: right
|
||||
tag_layout: g20_right_19
|
||||
serial_number: G20_RIGHT_001
|
||||
can_interface: can0
|
||||
output_root: calibration_output
|
||||
@@ -23,9 +24,9 @@ artifacts:
|
||||
source_urdf: src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/linkerhand_g20_right.urdf
|
||||
source_urdf_sha256: eeb6ffb0e95d2a6acd4c26331ae68062e0d74160de4b552b4f6d395cce5ca4e8
|
||||
camera_extrinsics: config/g20_three_camera_extrinsics.yaml
|
||||
camera_extrinsics_sha256: aa0a1498a210ef36f20d59bd4fdc612a01fd09eb5a2bc1c0b8d84c05a44d5c5a
|
||||
camera_extrinsics_sha256: 59ab7510ad0a2912ca636876039427c4c25906cbbfa84e96300d46b86e929334
|
||||
calibration_config: src/g20_thumb_apriltag_calibration/config/three_camera_calibration.yaml
|
||||
calibration_config_sha256: d1a8be37c9ae21cb4f975ddd66c3cf93fed25dae98ddcf971259b395fa69b521
|
||||
calibration_config_sha256: 8fa16814d7a3462a00aae4eadc273e66bc167bedd5908b0fed40ce6d505b2aeb
|
||||
tag_config: src/g20_thumb_apriltag_calibration/config/three_camera_tags_g20_right_19.yaml
|
||||
tag_config_sha256: b1ab45e97ae42d57b0a3a63c725107b5aa3828c06b2ced16f8222d6e9ebadc41
|
||||
|
||||
|
||||
@@ -84,10 +84,12 @@ g20_calibration:
|
||||
zero_maximum_axis_cone_mismatch_deg: 5.0
|
||||
zero_maximum_observability_condition_number: 10000000000.0
|
||||
zero_maximum_offset_deg: 20.0
|
||||
# 四指MCP roll保留严格的装配保护范围。MCP pitch/PIP静态零位由
|
||||
# 实测全屈曲行程与反馈0触掌的CAD机械端点联合求解，不允许写死为0。
|
||||
# 四指MCP roll保留严格的装配保护范围。thumb CMC三轴由多轴视觉几何
|
||||
# 求解且不假定电气端点等于CAD上限；thumb_mcp及四指MCP pitch/PIP
|
||||
# 静态零位由实测全行程与CAD机械端点联合求解，不写死为0。
|
||||
zero_finger_maximum_offset_deg: 3.0
|
||||
flexion_endpoint_maximum_offset_deg: 5.0
|
||||
# 只对实物已确认等同CAD端点的关节使用该限制；CMC电气端点不作此假设。
|
||||
mechanical_endpoint_maximum_offset_deg: 5.0
|
||||
|
||||
endpoint_tolerance_u8: 2.0
|
||||
# 请求命令与固件反馈是两个标定域。稳态检查点允许小幅死区，但反馈
|
||||
@@ -155,6 +157,11 @@ g20_calibration:
|
||||
directional_zero_maximum_branch_gap_deg: 2.0
|
||||
directional_zero_maximum_branch_gap_range_deg: 0.3
|
||||
cross_view_roll_maximum_branch_gap_difference_deg: 0.3
|
||||
# 正面roll是Tag中心的二维投影角，侧面roll是三维姿态角。允许一个有界的
|
||||
# 固定比例吸收Tag安装倾角/偏置带来的投影缩放，再严格比较两条曲线形状；
|
||||
# 比例过大、方向相反、形状RMS及两视角各自的四轮重复性仍会失败。
|
||||
cross_view_roll_maximum_shape_rms_deg: 1.25
|
||||
cross_view_roll_maximum_projection_scale_ratio: 1.5
|
||||
passive_maximum_monotonic_correction_deg: 3.0
|
||||
passive_maximum_hysteresis_deg: 2.0
|
||||
# 九点姿态先按实测反馈重新对齐，再用此门限检查固有机械回差；请求命令域
|
||||
|
||||
@@ -17,6 +17,7 @@ import xml.etree.ElementTree as ET
|
||||
|
||||
import numpy as np
|
||||
|
||||
from .core import COMMAND_NAMES
|
||||
from .trajectory import (
|
||||
_angle_for_circle,
|
||||
_fit_circle_with_axis,
|
||||
@@ -72,6 +73,24 @@ class SweepSpec:
|
||||
return f"{self.view}:motor{self.motor_index}:{','.join(self.joints)}"
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class PalmAxisObserver:
|
||||
"""Non-blocking direction observation attached to an existing task.
|
||||
|
||||
This is deliberately not a ``JointSpec``: it has no curve, endpoint or
|
||||
retry semantics. The camera callback records it only while both Tags are
|
||||
visible during the named sweep.
|
||||
"""
|
||||
|
||||
source_name: str
|
||||
task_name: str
|
||||
view: str
|
||||
parent_role: str
|
||||
child_role: str
|
||||
model_joint: str
|
||||
motor_index: int
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class JointCurveFit:
|
||||
angle_rad: tuple[float, ...]
|
||||
@@ -98,6 +117,23 @@ class HandCalibrationProfile:
|
||||
layout_id: str = "legacy_11"
|
||||
measurement_specs: Mapping[str, JointSpec] | None = None
|
||||
axis_validation_sources: Mapping[str, str] | None = None
|
||||
palm_axis_observers: tuple[PalmAxisObserver, ...] = ()
|
||||
minimum_palm_orientation_sources: int = 0
|
||||
# Hardware and algorithm differences belong to the product profile, not
|
||||
# to the acquisition state machine. Keeping these fields here lets a new
|
||||
# model (or the mirrored hand) reuse the same recorder/fitter by supplying
|
||||
# a different declarative profile.
|
||||
model: str = "G20"
|
||||
command_names: tuple[str, ...] = COMMAND_NAMES
|
||||
baseline_command: tuple[int, ...] = THREE_CAMERA_BASELINE_COMMAND
|
||||
capabilities: frozenset[str] = frozenset()
|
||||
|
||||
@property
|
||||
def command_count(self) -> int:
|
||||
return len(self.command_names)
|
||||
|
||||
def supports(self, capability: str) -> bool:
|
||||
return str(capability) in self.capabilities
|
||||
|
||||
@property
|
||||
def measured_joints(self) -> tuple[str, ...]:
|
||||
@@ -115,6 +151,20 @@ class HandCalibrationProfile:
|
||||
name for name, spec in self.record_specs.items() if spec.measured
|
||||
)
|
||||
|
||||
@property
|
||||
def palm_orientation_sources(self) -> Mapping[str, str]:
|
||||
return {
|
||||
observer.source_name: observer.model_joint
|
||||
for observer in self.palm_axis_observers
|
||||
}
|
||||
|
||||
@property
|
||||
def palm_axis_motor_by_source(self) -> Mapping[str, int]:
|
||||
return {
|
||||
observer.source_name: int(observer.motor_index)
|
||||
for observer in self.palm_axis_observers
|
||||
}
|
||||
|
||||
@property
|
||||
def active_joints(self) -> tuple[str, ...]:
|
||||
return tuple(
|
||||
@@ -612,6 +662,20 @@ def _build_right_19_profile() -> HandCalibrationProfile:
|
||||
layout_id=G20_RIGHT_19_LAYOUT,
|
||||
measurement_specs=measurement_specs,
|
||||
axis_validation_sources=axis_validation_sources,
|
||||
palm_axis_observers=(),
|
||||
minimum_palm_orientation_sources=0,
|
||||
capabilities=frozenset(
|
||||
{
|
||||
"precheck_sweeps",
|
||||
"steady_command_checkpoints",
|
||||
"directional_zero",
|
||||
"isolated_holdout",
|
||||
"cross_view_roll_curve",
|
||||
"stable_cross_view_cone_bias",
|
||||
"measured_passive_dips",
|
||||
"urdf_zero_publication",
|
||||
}
|
||||
),
|
||||
)
|
||||
|
||||
|
||||
@@ -1435,18 +1499,86 @@ def compare_cross_view_roll_curves(
|
||||
*,
|
||||
maximum_rms_difference_rad: float = math.radians(1.0),
|
||||
maximum_branch_gap_difference_rad: float = math.radians(0.3),
|
||||
allow_projection_scale: bool = False,
|
||||
maximum_projection_scale_ratio: float = 1.5,
|
||||
) -> dict[str, float]:
|
||||
"""Validate two independently fitted roll curves before axis fusion."""
|
||||
"""Validate two independently fitted roll curves before axis fusion.
|
||||
|
||||
The G20 right roll primary is an image-plane centre trajectory while its
|
||||
side validation is a 3-D rotation trajectory. A fixed Tag mounting tilt
|
||||
changes the image-plane angular scale without changing the mechanism. In
|
||||
projection-scale mode, remove that one constant scale before comparing
|
||||
curve shape. Direction, scale bounds, direction-dependent branch gap and
|
||||
every single-view quality/holdout gate remain independent hard checks.
|
||||
"""
|
||||
if maximum_rms_difference_rad <= 0.0:
|
||||
raise ValueError("cross-view RMS limit must be positive")
|
||||
if maximum_branch_gap_difference_rad <= 0.0:
|
||||
raise ValueError("cross-view branch-gap limit must be positive")
|
||||
metrics: dict[str, float] = {}
|
||||
if maximum_projection_scale_ratio <= 1.0:
|
||||
raise ValueError("cross-view projection-scale ratio must exceed one")
|
||||
|
||||
fields: dict[str, tuple[np.ndarray, np.ndarray]] = {}
|
||||
for field_name in ("angle_rad", "decreasing_rad", "increasing_rad"):
|
||||
left = np.asarray(getattr(primary, field_name), dtype=float)
|
||||
right = np.asarray(getattr(secondary, field_name), dtype=float)
|
||||
if left.shape != (256,) or right.shape != (256,):
|
||||
raise ValueError("cross-view curves must contain 256 values")
|
||||
if not np.all(np.isfinite(left)) or not np.all(np.isfinite(right)):
|
||||
raise ValueError("cross-view curves must be finite")
|
||||
fields[field_name] = (left, right)
|
||||
|
||||
primary_travel = float(
|
||||
fields["angle_rad"][0][0] - fields["angle_rad"][0][255]
|
||||
)
|
||||
secondary_travel = float(
|
||||
fields["angle_rad"][1][0] - fields["angle_rad"][1][255]
|
||||
)
|
||||
if primary_travel * secondary_travel <= 0.0:
|
||||
raise ValueError("cross_view_roll_curve_direction_disagrees")
|
||||
|
||||
primary_zero = int(primary.circle.get("zero_command_u8", 127))
|
||||
secondary_zero = int(secondary.circle.get("zero_command_u8", 127))
|
||||
if not 0 <= primary_zero <= 255 or not 0 <= secondary_zero <= 255:
|
||||
raise ValueError("cross-view zero command must be in [0, 255]")
|
||||
primary_anchor = float(primary.decreasing_rad[primary_zero])
|
||||
secondary_anchor = float(secondary.decreasing_rad[secondary_zero])
|
||||
projection_scale = 1.0
|
||||
if allow_projection_scale:
|
||||
primary_values = np.concatenate(
|
||||
[left - primary_anchor for left, _ in fields.values()]
|
||||
)
|
||||
secondary_values = np.concatenate(
|
||||
[right - secondary_anchor for _, right in fields.values()]
|
||||
)
|
||||
denominator = float(secondary_values @ secondary_values)
|
||||
if denominator <= np.finfo(float).eps:
|
||||
raise ValueError("cross_view_roll_projection_scale_unobservable")
|
||||
projection_scale = float(
|
||||
(primary_values @ secondary_values) / denominator
|
||||
)
|
||||
if projection_scale <= 0.0 or not math.isfinite(projection_scale):
|
||||
raise ValueError("cross_view_roll_curve_direction_disagrees")
|
||||
scale_ratio = max(projection_scale, 1.0 / projection_scale)
|
||||
if scale_ratio > float(maximum_projection_scale_ratio):
|
||||
raise ValueError(
|
||||
"cross_view_roll_projection_scale_ratio_too_large:"
|
||||
f"{scale_ratio:.6f}"
|
||||
)
|
||||
|
||||
metrics: dict[str, float] = {
|
||||
"projection_scale": projection_scale,
|
||||
"projection_scale_ratio": max(
|
||||
projection_scale, 1.0 / projection_scale
|
||||
),
|
||||
}
|
||||
for field_name, (left, right) in fields.items():
|
||||
metrics[f"raw_{field_name}_rms_difference_rad"] = float(
|
||||
np.sqrt(np.mean(np.square(left - right)))
|
||||
)
|
||||
if allow_projection_scale:
|
||||
left = left - primary_anchor
|
||||
right = projection_scale * (right - secondary_anchor)
|
||||
rms = float(np.sqrt(np.mean(np.square(left - right))))
|
||||
metrics[f"{field_name}_rms_difference_rad"] = rms
|
||||
if rms > maximum_rms_difference_rad:
|
||||
@@ -1454,13 +1586,7 @@ def compare_cross_view_roll_curves(
|
||||
"cross_view_roll_curve_difference_too_large:"
|
||||
f"{field_name}:{math.degrees(rms):.6f}deg"
|
||||
)
|
||||
primary_travel = float(primary.angle_rad[0] - primary.angle_rad[255])
|
||||
secondary_travel = float(secondary.angle_rad[0] - secondary.angle_rad[255])
|
||||
if primary_travel * secondary_travel <= 0.0:
|
||||
raise ValueError("cross_view_roll_curve_direction_disagrees")
|
||||
metrics["travel_difference_rad"] = abs(primary_travel - secondary_travel)
|
||||
primary_zero = int(primary.circle.get("zero_command_u8", 127))
|
||||
secondary_zero = int(secondary.circle.get("zero_command_u8", 127))
|
||||
primary_gap = float(
|
||||
primary.increasing_rad[primary_zero]
|
||||
- primary.decreasing_rad[primary_zero]
|
||||
@@ -1469,9 +1595,27 @@ def compare_cross_view_roll_curves(
|
||||
secondary.increasing_rad[secondary_zero]
|
||||
- secondary.decreasing_rad[secondary_zero]
|
||||
)
|
||||
branch_gap_difference = abs(primary_gap - secondary_gap)
|
||||
metrics["raw_baseline_branch_gap_difference_rad"] = abs(
|
||||
primary_gap - secondary_gap
|
||||
)
|
||||
branch_gap_difference = abs(
|
||||
primary_gap - projection_scale * secondary_gap
|
||||
)
|
||||
metrics["baseline_branch_gap_difference_rad"] = branch_gap_difference
|
||||
if branch_gap_difference > maximum_branch_gap_difference_rad:
|
||||
# In projection-scale mode the primary is an image-circle phase while the
|
||||
# secondary is a 3-D rotation angle. A fixed Tag tilt/offset changes the
|
||||
# image projection of the two motion branches independently, so their
|
||||
# absolute gap is not a cross-view invariant. Both views retain their
|
||||
# own hard absolute-gap and four-cycle gap-range checks in the acquisition
|
||||
# quality gate. Keep this value as a diagnostic, but apply the cross-view
|
||||
# absolute-gap gate only when both curves live in comparable coordinates.
|
||||
metrics["baseline_branch_gap_gate_applied"] = float(
|
||||
not allow_projection_scale
|
||||
)
|
||||
if (
|
||||
not allow_projection_scale
|
||||
and branch_gap_difference > maximum_branch_gap_difference_rad
|
||||
):
|
||||
raise ValueError(
|
||||
"cross_view_roll_branch_gap_difference_too_large:"
|
||||
f"{math.degrees(branch_gap_difference):.6f}deg"
|
||||
@@ -1479,6 +1623,35 @@ def compare_cross_view_roll_curves(
|
||||
return metrics
|
||||
|
||||
|
||||
def cross_view_roll_diagnostic_metrics(
|
||||
primary: JointCurveFit,
|
||||
secondary: JointCurveFit,
|
||||
) -> dict[str, float]:
|
||||
"""Compare a validation-only roll view without creating a retry gate.
|
||||
|
||||
G20-right publishes the front image-plane roll curve. The side PIP-link
|
||||
observation supplies an independently quality-gated physical axis-line
|
||||
position, but its angle includes projection and linkage effects and is
|
||||
therefore diagnostic rather than a second measurement of the published
|
||||
curve. Live fitting and offline replay must use the same non-blocking
|
||||
comparison; otherwise a stable view bias can recall an already complete
|
||||
task even though reacquiring the same motion cannot change the result.
|
||||
"""
|
||||
try:
|
||||
return compare_cross_view_roll_curves(
|
||||
primary,
|
||||
secondary,
|
||||
maximum_rms_difference_rad=float("inf"),
|
||||
maximum_branch_gap_difference_rad=float("inf"),
|
||||
allow_projection_scale=True,
|
||||
maximum_projection_scale_ratio=float("inf"),
|
||||
)
|
||||
except ValueError as error:
|
||||
if str(error) == "cross_view_roll_curve_direction_disagrees":
|
||||
return {"direction_disagrees": 1.0}
|
||||
raise
|
||||
|
||||
|
||||
def fit_projected_zero(
|
||||
records: Sequence[Mapping[str, Any]],
|
||||
*,
|
||||
|
||||
+310
-61
@@ -30,25 +30,34 @@ from .full_hand import (
|
||||
build_compact_payload,
|
||||
canonical_zero_direction,
|
||||
clamp_runtime_fits_to_urdf_limits,
|
||||
compare_cross_view_roll_curves,
|
||||
cross_view_roll_diagnostic_metrics,
|
||||
derive_mimic_passive_fits,
|
||||
fit_joint_image_curve,
|
||||
get_hand_calibration_profile,
|
||||
validate_compact_payload,
|
||||
)
|
||||
from .product import get_product_calibration_contract
|
||||
from .storage import atomic_write_json
|
||||
from .urdf_zero import (
|
||||
JointAxisMeasurement,
|
||||
UrdfKinematicModel,
|
||||
_angles_from_state,
|
||||
anchor_right_19_mechanical_endpoint_curves,
|
||||
axis_line_uses_depth_free_interpretation_plane,
|
||||
axis_line_cycle_rms_m,
|
||||
baseline_hysteresis_by_cycle_rad,
|
||||
circle_direction_is_constrained,
|
||||
cross_view_side_line_source,
|
||||
fit_joint_axis_measurement,
|
||||
fit_partial_palm_orientation_measurements,
|
||||
fit_rotation_joint_curve,
|
||||
get_zero_calibration_profile,
|
||||
derive_right_19_flexion_endpoint_offsets,
|
||||
derive_right_19_mechanical_endpoint_offsets,
|
||||
joint_curve_holdout_errors,
|
||||
refit_axis_line_group_with_shared_radius,
|
||||
select_cross_view_roll_direction_source,
|
||||
solve_urdf_zero_offsets,
|
||||
with_depth_free_axis_projection,
|
||||
write_zero_corrected_urdf,
|
||||
)
|
||||
|
||||
@@ -139,6 +148,35 @@ def _latest_attempt_records(
|
||||
return dict(records)
|
||||
|
||||
|
||||
def _latest_palm_axis_records(
|
||||
rows: Sequence[Mapping[str, Any]],
|
||||
) -> dict[str, list[dict[str, Any]]]:
|
||||
"""Select the latest accepted append-only palm side-channel attempts."""
|
||||
samples = [
|
||||
dict(row) for row in rows if row.get("kind") == "palm_axis_sample"
|
||||
]
|
||||
latest_attempt: dict[tuple[str, int, str], int] = {}
|
||||
for row in samples:
|
||||
key = (
|
||||
str(row["source_joint"]),
|
||||
int(row["cycle"]),
|
||||
str(row["direction"]),
|
||||
)
|
||||
latest_attempt[key] = max(
|
||||
latest_attempt.get(key, 0), int(row.get("attempt", 1))
|
||||
)
|
||||
records: dict[str, list[dict[str, Any]]] = defaultdict(list)
|
||||
for row in samples:
|
||||
key = (
|
||||
str(row["source_joint"]),
|
||||
int(row["cycle"]),
|
||||
str(row["direction"]),
|
||||
)
|
||||
if int(row.get("attempt", 1)) == latest_attempt[key]:
|
||||
records[key[0]].append(row)
|
||||
return dict(records)
|
||||
|
||||
|
||||
def _load_raw_session(
|
||||
session_dir: Path,
|
||||
) -> tuple[
|
||||
@@ -146,6 +184,7 @@ def _load_raw_session(
|
||||
dict[str, list[dict[str, Any]]],
|
||||
dict[str, list[dict[str, Any]]],
|
||||
dict[str, list[dict[str, Any]]],
|
||||
dict[str, list[dict[str, Any]]],
|
||||
Path,
|
||||
]:
|
||||
raw_path = session_dir / "raw_samples.jsonl"
|
||||
@@ -163,7 +202,9 @@ def _load_raw_session(
|
||||
for row in rows:
|
||||
if "command_u8" in row:
|
||||
continue
|
||||
if row.get("kind") in {"sample", "baseline_hold_sample"}:
|
||||
if row.get("kind") in {
|
||||
"sample", "baseline_hold_sample", "palm_axis_sample"
|
||||
}:
|
||||
row["command_u8"] = int(round(float(row["feedback_u8"])))
|
||||
elif row.get("kind") == "steady_command_sample":
|
||||
row["command_u8"] = int(row["requested_command_u8"])
|
||||
@@ -175,6 +216,7 @@ def _load_raw_session(
|
||||
_latest_attempt_records(rows),
|
||||
_latest_attempt_records(rows, kind="baseline_hold_sample"),
|
||||
_latest_attempt_records(rows, kind="steady_command_sample"),
|
||||
_latest_palm_axis_records(rows),
|
||||
raw_path,
|
||||
)
|
||||
|
||||
@@ -189,7 +231,13 @@ def _fit_curve(
|
||||
motor = profile.record_specs[name].motor_index
|
||||
if (
|
||||
profile.layout_id == G20_RIGHT_19_LAYOUT
|
||||
and name in RIGHT_19_END_ON_IMAGE_CURVE_JOINTS
|
||||
and (
|
||||
name in RIGHT_19_END_ON_IMAGE_CURVE_JOINTS
|
||||
or (
|
||||
name.endswith("_mcp_roll")
|
||||
and not name.startswith("thumb_")
|
||||
)
|
||||
)
|
||||
):
|
||||
return fit_joint_image_curve(records)
|
||||
return fit_rotation_joint_curve(
|
||||
@@ -234,6 +282,7 @@ def _fit_axes(
|
||||
)
|
||||
extrinsics = load_three_camera_extrinsics(extrinsics_file)
|
||||
cache: dict[tuple[str, int], JointAxisMeasurement] = {}
|
||||
cross_view_direction_source_cache: dict[str, str] = {}
|
||||
upstream_by_joint = {
|
||||
"thumb_mcp": "thumb_cmc_pitch",
|
||||
"thumb_ip": "thumb_mcp",
|
||||
@@ -246,6 +295,15 @@ def _fit_axes(
|
||||
for finger in ("index", "middle", "ring", "pinky")
|
||||
},
|
||||
}
|
||||
if profile.layout_id == G20_RIGHT_19_LAYOUT:
|
||||
reference_roll = f"{profile.reference_finger}_mcp_roll"
|
||||
upstream_by_joint.update(
|
||||
{
|
||||
f"{finger}_mcp_roll": reference_roll
|
||||
for finger in ("index", "middle", "ring", "pinky")
|
||||
if f"{finger}_mcp_roll" != reference_roll
|
||||
}
|
||||
)
|
||||
|
||||
def fit_raw(name: str, cycle: int) -> JointAxisMeasurement:
|
||||
key = (name, cycle)
|
||||
@@ -288,6 +346,9 @@ def _fit_axes(
|
||||
),
|
||||
view_normal_common_xyz=tuple(float(value) for value in view_normal),
|
||||
)
|
||||
result = with_depth_free_axis_projection(
|
||||
result, extrinsics.transform(spec.view)[:3, 3]
|
||||
)
|
||||
cache[key] = result
|
||||
return result
|
||||
|
||||
@@ -311,11 +372,52 @@ def _fit_axes(
|
||||
np.cross(secondary_point - primary_point, primary_axis)
|
||||
)
|
||||
)
|
||||
if (
|
||||
profile.record_specs[validation_name].zero_kind
|
||||
== "axis_cross_view_validation"
|
||||
):
|
||||
point_view = profile.record_specs[name].view
|
||||
if point_view is None:
|
||||
raise ValueError(
|
||||
f"{name}: primary point source has no view"
|
||||
)
|
||||
point_camera_center = extrinsics.transform(point_view)[:3, 3]
|
||||
point_ray = primary_point - point_camera_center
|
||||
interpretation_plane_normal = np.cross(
|
||||
point_ray, primary_axis
|
||||
)
|
||||
interpretation_plane_normal /= np.linalg.norm(
|
||||
interpretation_plane_normal
|
||||
)
|
||||
return replace(
|
||||
primary,
|
||||
point_common_xyz_m=tuple(
|
||||
float(value)
|
||||
for value in primary.point_common_xyz_m
|
||||
),
|
||||
pose_axis_line_rms_m=primary.pose_axis_line_rms_m,
|
||||
pose_axis_line_source_joints=(name,),
|
||||
axis_point_source=(
|
||||
"front_interpretation_plane_cross_view_validated"
|
||||
),
|
||||
axis_point_camera_center_common_xyz_m=tuple(
|
||||
float(value)
|
||||
for value in point_camera_center
|
||||
),
|
||||
axis_point_interpretation_plane_normal_common_xyz=tuple(
|
||||
float(value)
|
||||
for value in interpretation_plane_normal
|
||||
),
|
||||
)
|
||||
# Mirror the live node's tiered policy: the side-view planar-tag
|
||||
# IPPE bias makes sub-degree cross-view agreement unreachable, so
|
||||
# disagreement above the fusion gate keeps the front-only axis while
|
||||
# only the gross bound (wrong link / loose tag) still fails.
|
||||
if difference > math.radians(15.0) or line_distance > 0.030:
|
||||
# only a gross direction error (wrong link / loose tag) still fails.
|
||||
# The front screw-driven roll trajectory yields a displaced
|
||||
# pseudo-line, so its distance to the side physical line is retained
|
||||
# only as a diagnostic. The live node likewise keeps only the gross
|
||||
# direction gate here and validates side-line quality independently.
|
||||
if difference > math.radians(15.0):
|
||||
raise ValueError(f"{name}: cross-view axis gross disagreement")
|
||||
if difference > math.radians(0.75) or line_distance > 0.001:
|
||||
selected_direction = primary_axis
|
||||
@@ -329,7 +431,6 @@ def _fit_axes(
|
||||
None,
|
||||
)
|
||||
if observer_name is not None:
|
||||
observer = fit_raw(observer_name, cycle)
|
||||
model = UrdfKinematicModel(source_urdf)
|
||||
parent_axis, _ = model.axis_line(
|
||||
name, zero_offsets={}, joint_angles={}
|
||||
@@ -340,11 +441,9 @@ def _fit_axes(
|
||||
expected_cone = math.acos(
|
||||
abs(float(np.clip(parent_axis @ observer_axis, -1.0, 1.0)))
|
||||
)
|
||||
measured_observer = np.asarray(
|
||||
observer.axis_common_xyz, dtype=float
|
||||
)
|
||||
|
||||
def cone_residual(candidate: np.ndarray) -> float:
|
||||
def cone_residual(
|
||||
candidate: np.ndarray, measured_observer: np.ndarray
|
||||
) -> float:
|
||||
measured_cone = math.acos(
|
||||
abs(
|
||||
float(
|
||||
@@ -356,10 +455,46 @@ def _fit_axes(
|
||||
)
|
||||
return abs(measured_cone - expected_cone)
|
||||
|
||||
if (
|
||||
cone_residual(primary_axis) > math.radians(5.0)
|
||||
and cone_residual(secondary_axis) <= math.radians(5.0)
|
||||
):
|
||||
if name not in cross_view_direction_source_cache:
|
||||
primary_residuals: list[float] = []
|
||||
secondary_residuals: list[float] = []
|
||||
for group_cycle in range(repetitions):
|
||||
group_observer = np.asarray(
|
||||
fit_raw(
|
||||
observer_name, group_cycle
|
||||
).axis_common_xyz,
|
||||
dtype=float,
|
||||
)
|
||||
primary_residuals.append(
|
||||
cone_residual(
|
||||
np.asarray(
|
||||
fit_raw(
|
||||
name, group_cycle
|
||||
).axis_common_xyz,
|
||||
dtype=float,
|
||||
),
|
||||
group_observer,
|
||||
)
|
||||
)
|
||||
secondary_residuals.append(
|
||||
cone_residual(
|
||||
np.asarray(
|
||||
fit_raw(
|
||||
validation_name, group_cycle
|
||||
).axis_common_xyz,
|
||||
dtype=float,
|
||||
),
|
||||
group_observer,
|
||||
)
|
||||
)
|
||||
cross_view_direction_source_cache[name] = (
|
||||
select_cross_view_roll_direction_source(
|
||||
primary_residuals,
|
||||
secondary_residuals,
|
||||
math.radians(5.0),
|
||||
)
|
||||
)
|
||||
if cross_view_direction_source_cache[name] == "secondary":
|
||||
selected_direction = secondary_axis
|
||||
selected_source = "cross_view_cone_selected_secondary"
|
||||
# Mirror the live node: use the view whose direction satisfies
|
||||
@@ -375,6 +510,7 @@ def _fit_axes(
|
||||
for value in secondary.point_common_xyz_m
|
||||
),
|
||||
pose_axis_line_rms_m=secondary.pose_axis_line_rms_m,
|
||||
pose_axis_line_source_joints=(validation_name,),
|
||||
axis_direction_source=selected_source,
|
||||
axis_point_source="side_circle_cross_view",
|
||||
)
|
||||
@@ -406,14 +542,45 @@ def _fit_axes(
|
||||
secondary.pose_axis_line_rms_m,
|
||||
line_distance,
|
||||
),
|
||||
pose_axis_line_source_joints=(name, validation_name),
|
||||
axis_direction_source="cross_view_weighted_fusion",
|
||||
)
|
||||
|
||||
return [
|
||||
fit_one(name, cycle)
|
||||
for name in zero_profile.axis_joints
|
||||
for cycle in range(repetitions)
|
||||
]
|
||||
result: list[JointAxisMeasurement] = []
|
||||
for name in zero_profile.axis_joints:
|
||||
group = [fit_one(name, cycle) for cycle in range(repetitions)]
|
||||
side_sources = {
|
||||
source
|
||||
for measurement in group
|
||||
if (source := cross_view_side_line_source(measurement)) is not None
|
||||
}
|
||||
if (
|
||||
len(side_sources) == 1
|
||||
and all(
|
||||
cross_view_side_line_source(measurement) is not None
|
||||
for measurement in group
|
||||
)
|
||||
and not any(
|
||||
axis_line_uses_depth_free_interpretation_plane(measurement)
|
||||
for measurement in group
|
||||
)
|
||||
):
|
||||
source = next(iter(side_sources))
|
||||
source_spec = profile.record_specs[source]
|
||||
group = list(
|
||||
refit_axis_line_group_with_shared_radius(
|
||||
group,
|
||||
records_by_joint[source],
|
||||
zero_command_u8=int(
|
||||
baseline[source_spec.motor_index]
|
||||
),
|
||||
canonical_zero_direction=canonical_zero_direction(
|
||||
profile, source
|
||||
),
|
||||
)
|
||||
)
|
||||
result.extend(group)
|
||||
return result
|
||||
|
||||
|
||||
def _maximum_undirected_axis_difference(axes: Sequence[Sequence[float]]) -> float:
|
||||
@@ -601,6 +768,7 @@ def _quality_failures(
|
||||
if (
|
||||
max(baseline_hysteresis) - min(baseline_hysteresis)
|
||||
> gap_range_limit
|
||||
and spec.zero_kind != "axis_cross_view_validation"
|
||||
):
|
||||
failures.append(
|
||||
f"{name}: baseline directional gap repeatability"
|
||||
@@ -613,6 +781,7 @@ def _quality_failures(
|
||||
cycle_travels: list[float] = []
|
||||
cycle_axes: list[Sequence[float]] = []
|
||||
cycle_axis_sources: list[str] = []
|
||||
cycle_axis_measurements: list[JointAxisMeasurement] = []
|
||||
for cycle in range(repetitions):
|
||||
cycle_fit = _fit_curve(
|
||||
name,
|
||||
@@ -626,11 +795,13 @@ def _quality_failures(
|
||||
axis = axis_by_key[(name, cycle)]
|
||||
cycle_axes.append(axis.axis_common_xyz)
|
||||
cycle_axis_sources.append(axis.axis_direction_source)
|
||||
cycle_axis_measurements.append(axis)
|
||||
if axis.radial_rms_m > float(parameters["axis_maximum_radial_rms_m"]):
|
||||
failures.append(f"{name} cycle {cycle + 1}: radial RMS")
|
||||
if (
|
||||
profile.record_specs[name].zero_kind
|
||||
!= "axis_cross_view_validation"
|
||||
and cross_view_side_line_source(axis) is None
|
||||
and axis.pose_axis_line_rms_m
|
||||
> float(parameters["axis_maximum_pose_line_rms_m"])
|
||||
):
|
||||
@@ -655,6 +826,29 @@ def _quality_failures(
|
||||
)
|
||||
):
|
||||
failures.append(f"{name} cycle {cycle + 1}: axis disagreement")
|
||||
side_line_sources = {
|
||||
source
|
||||
for measurement in cycle_axis_measurements
|
||||
if (
|
||||
source := cross_view_side_line_source(measurement)
|
||||
) is not None
|
||||
}
|
||||
if (
|
||||
len(cycle_axis_measurements) == repetitions
|
||||
and len(side_line_sources) == 1
|
||||
and all(
|
||||
cross_view_side_line_source(measurement) is not None
|
||||
for measurement in cycle_axis_measurements
|
||||
)
|
||||
and not any(
|
||||
axis_line_uses_depth_free_interpretation_plane(measurement)
|
||||
for measurement in cycle_axis_measurements
|
||||
)
|
||||
and axis_line_cycle_rms_m(cycle_axis_measurements)
|
||||
> float(parameters["axis_maximum_pose_line_rms_m"])
|
||||
):
|
||||
source = next(iter(side_line_sources))
|
||||
failures.append(f"{source}: axis-line cycle RMS")
|
||||
travel_limit = math.radians(
|
||||
float(
|
||||
parameters[
|
||||
@@ -671,6 +865,7 @@ def _quality_failures(
|
||||
source == "upstream_constraint"
|
||||
for source in cycle_axis_sources
|
||||
)
|
||||
and spec.zero_kind != "axis_cross_view_validation"
|
||||
and _maximum_undirected_axis_difference(cycle_axes) > math.radians(
|
||||
float(parameters["zero_maximum_axis_cycle_difference_deg"])
|
||||
)
|
||||
@@ -832,21 +1027,28 @@ def replay_session(
|
||||
records_by_joint,
|
||||
baseline_records_by_joint,
|
||||
command_records_by_joint,
|
||||
palm_axis_records_by_source,
|
||||
raw_path,
|
||||
) = _load_raw_session(session)
|
||||
model = str(start.get("model", "G20")).strip().upper()
|
||||
side = str(start["hand_type"]).lower()
|
||||
requested_layout_id = str(
|
||||
start.get("tag_layout", "legacy_11")
|
||||
).lower()
|
||||
profile = get_hand_calibration_profile(side, requested_layout_id)
|
||||
contract = get_product_calibration_contract(
|
||||
model, side, requested_layout_id
|
||||
)
|
||||
profile = contract.profile
|
||||
# Normalise the former g20_right_15 compatibility alias immediately so
|
||||
# every downstream product guard uses the actual physical 19-Tag profile
|
||||
# instead of silently falling through legacy-11 logic.
|
||||
layout_id = profile.layout_id
|
||||
zero_profile = get_zero_calibration_profile(side, layout_id)
|
||||
zero_profile = contract.zero_profile
|
||||
baseline = tuple(int(value) for value in start["baseline_command_u8"])
|
||||
if len(baseline) != 20:
|
||||
raise ValueError("session baseline must contain exactly 20 commands")
|
||||
if len(baseline) != profile.command_count:
|
||||
raise ValueError(
|
||||
"session baseline length differs from the registered product schema"
|
||||
)
|
||||
if (
|
||||
layout_id == G20_RIGHT_19_LAYOUT
|
||||
and write_outputs
|
||||
@@ -878,8 +1080,14 @@ def replay_session(
|
||||
for row in combinations
|
||||
):
|
||||
raise ValueError("combination pose validation exceeds product limits")
|
||||
if set(records_by_joint) != set(profile.record_joints):
|
||||
raise ValueError("raw session does not contain exactly the task record set")
|
||||
required_record_joints = set(profile.record_joints)
|
||||
if (
|
||||
not required_record_joints.issubset(records_by_joint)
|
||||
or not set(records_by_joint).issubset(set(profile.record_joints))
|
||||
):
|
||||
raise ValueError(
|
||||
"raw session does not contain the required task record set"
|
||||
)
|
||||
|
||||
def hysteresis_records(
|
||||
name: str,
|
||||
@@ -904,7 +1112,9 @@ def replay_session(
|
||||
name: str, *, require_full_pose: bool = False
|
||||
) -> list[dict[str, Any]]:
|
||||
"""Merge settled baseline holds into directional model records."""
|
||||
records = [dict(record) for record in records_by_joint[name]]
|
||||
records = [
|
||||
dict(record) for record in records_by_joint.get(name, ())
|
||||
]
|
||||
if canonical_zero_direction(profile, name) is None:
|
||||
return records
|
||||
required = {
|
||||
@@ -971,7 +1181,7 @@ def replay_session(
|
||||
}
|
||||
command_fits = dict(training_fits)
|
||||
if layout_id == G20_RIGHT_19_LAYOUT:
|
||||
if set(command_records_by_joint) != set(profile.record_joints):
|
||||
if set(command_records_by_joint) != required_record_joints:
|
||||
raise ValueError(
|
||||
"g20_right_19 session is incomplete: missing steady command checkpoints"
|
||||
)
|
||||
@@ -1035,6 +1245,11 @@ def replay_session(
|
||||
command_fits,
|
||||
profile=profile,
|
||||
)
|
||||
command_fits = anchor_right_19_mechanical_endpoint_curves(
|
||||
command_fits,
|
||||
command_records_by_joint,
|
||||
maximum_direction_difference_rad=command_gap_limit,
|
||||
)
|
||||
cross_view_roll_metrics: dict[str, dict[str, float]] = {}
|
||||
for name, validation_name in (
|
||||
profile.axis_validation_sources or {}
|
||||
@@ -1055,35 +1270,11 @@ def replay_session(
|
||||
profile=profile,
|
||||
baseline=baseline,
|
||||
)
|
||||
training_metrics = compare_cross_view_roll_curves(
|
||||
training_fits[name],
|
||||
validation_training,
|
||||
maximum_rms_difference_rad=math.radians(
|
||||
float(parameters["maximum_validation_mae_deg"])
|
||||
),
|
||||
maximum_branch_gap_difference_rad=math.radians(
|
||||
float(
|
||||
parameters.get(
|
||||
"cross_view_roll_maximum_branch_gap_difference_deg",
|
||||
0.3,
|
||||
)
|
||||
)
|
||||
),
|
||||
training_metrics = cross_view_roll_diagnostic_metrics(
|
||||
training_fits[name], validation_training
|
||||
)
|
||||
final_metrics = compare_cross_view_roll_curves(
|
||||
measured_fits[name],
|
||||
validation_all,
|
||||
maximum_rms_difference_rad=math.radians(
|
||||
float(parameters["maximum_validation_mae_deg"])
|
||||
),
|
||||
maximum_branch_gap_difference_rad=math.radians(
|
||||
float(
|
||||
parameters.get(
|
||||
"cross_view_roll_maximum_branch_gap_difference_deg",
|
||||
0.3,
|
||||
)
|
||||
)
|
||||
),
|
||||
final_metrics = cross_view_roll_diagnostic_metrics(
|
||||
measured_fits[name], validation_all
|
||||
)
|
||||
cross_view_roll_metrics[name] = {
|
||||
**{
|
||||
@@ -1103,6 +1294,28 @@ def replay_session(
|
||||
source_urdf=source_urdf,
|
||||
repetitions=repetitions,
|
||||
)
|
||||
palm_orientation_measurements, palm_orientation_rejections = (
|
||||
fit_partial_palm_orientation_measurements(
|
||||
sources=profile.palm_orientation_sources,
|
||||
records_by_joint=palm_axis_records_by_source,
|
||||
motor_by_source=profile.palm_axis_motor_by_source,
|
||||
baseline_command_u8=baseline,
|
||||
cycles=range(repetitions),
|
||||
minimum_sources=int(
|
||||
profile.minimum_palm_orientation_sources
|
||||
),
|
||||
minimum_arc_rad=math.radians(
|
||||
float(parameters["trajectory_minimum_arc_deg"])
|
||||
),
|
||||
maximum_rotation_orthogonal_rms_rad=math.radians(
|
||||
float(
|
||||
parameters[
|
||||
"active_maximum_rotation_orthogonal_rms_deg"
|
||||
]
|
||||
)
|
||||
),
|
||||
)
|
||||
)
|
||||
failures = _quality_failures(
|
||||
records_by_joint=records_by_joint,
|
||||
baseline_records_by_joint=(
|
||||
@@ -1223,11 +1436,15 @@ def replay_session(
|
||||
name: int(spec.motor_index) for name, spec in profile.joint_specs.items()
|
||||
}
|
||||
endpoint_zero_offsets = (
|
||||
derive_right_19_flexion_endpoint_offsets(
|
||||
derive_right_19_mechanical_endpoint_offsets(
|
||||
source_urdf,
|
||||
command_fits,
|
||||
maximum_offset_rad=math.radians(
|
||||
float(parameters.get("flexion_endpoint_maximum_offset_deg", 5.0))
|
||||
float(
|
||||
parameters.get(
|
||||
"mechanical_endpoint_maximum_offset_deg", 5.0
|
||||
)
|
||||
)
|
||||
),
|
||||
)
|
||||
if layout_id == G20_RIGHT_19_LAYOUT
|
||||
@@ -1242,6 +1459,7 @@ def replay_session(
|
||||
solve_arguments = {
|
||||
"source_urdf": source_urdf,
|
||||
"measurements": axes,
|
||||
"palm_orientation_measurements": palm_orientation_measurements,
|
||||
"motor_by_joint": motor_by_joint,
|
||||
"maximum_offset_rad": math.radians(float(parameters["zero_maximum_offset_deg"])),
|
||||
"finger_maximum_offset_rad": math.radians(
|
||||
@@ -1254,6 +1472,13 @@ def replay_session(
|
||||
"maximum_axis_cone_mismatch_rad": math.radians(
|
||||
float(parameters["zero_maximum_axis_cone_mismatch_deg"])
|
||||
),
|
||||
"maximum_systematic_axis_cone_bias_rad": math.radians(
|
||||
float(
|
||||
parameters.get(
|
||||
"cross_view_roll_maximum_axis_difference_deg", 15.0
|
||||
)
|
||||
)
|
||||
),
|
||||
"maximum_observability_condition_number": float(
|
||||
parameters.get(
|
||||
"zero_maximum_observability_condition_number", 1.0e10
|
||||
@@ -1469,6 +1694,21 @@ def replay_session(
|
||||
"offset_covariance_rad2": (
|
||||
final_zero.offset_covariance_rad2
|
||||
),
|
||||
"palm_orientation_sources": [
|
||||
{
|
||||
"source_joint": item.source_joint,
|
||||
"model_joint": item.model_joint,
|
||||
"cycle": item.cycle,
|
||||
"observed_arc_rad": item.observed_arc_rad,
|
||||
"rotation_orthogonal_rms_rad": (
|
||||
item.rotation_orthogonal_rms_rad
|
||||
),
|
||||
}
|
||||
for item in palm_orientation_measurements
|
||||
],
|
||||
"palm_orientation_rejections": dict(
|
||||
palm_orientation_rejections
|
||||
),
|
||||
}
|
||||
if layout_id == G20_RIGHT_19_LAYOUT
|
||||
else None
|
||||
@@ -1508,7 +1748,7 @@ def replay_session(
|
||||
"thumb_default": float(parameters["zero_maximum_offset_deg"]),
|
||||
},
|
||||
"static_zero_policy": (
|
||||
"visual_axes_plus_measured_contact_endpoint"
|
||||
"visual_axes_plus_measured_mechanical_endpoint"
|
||||
if layout_id == G20_RIGHT_19_LAYOUT
|
||||
else "direct_measurements_only"
|
||||
),
|
||||
@@ -1538,6 +1778,15 @@ def replay_session(
|
||||
name: [math.degrees(value) for value in values]
|
||||
for name, values in holdout_zero.cycle_offsets_rad.items()
|
||||
},
|
||||
"axis_cone_mismatch_by_joint_deg": {
|
||||
name: math.degrees(value)
|
||||
for name, value in (
|
||||
holdout_zero.axis_cone_mismatch_by_joint_rad.items()
|
||||
)
|
||||
},
|
||||
"axis_cone_bias_classification_by_joint": dict(
|
||||
holdout_zero.axis_cone_bias_classification_by_joint
|
||||
),
|
||||
"offset_uncertainty_deg": {
|
||||
name: math.degrees(value)
|
||||
for name, value in holdout_zero.offset_uncertainty_rad.items()
|
||||
@@ -1577,7 +1826,7 @@ def replay_session(
|
||||
|
||||
def main() -> None:
|
||||
parser = argparse.ArgumentParser(
|
||||
description="Replay and independently validate a complete G20 calibration session."
|
||||
description="Replay and validate a registered hand-calibration session."
|
||||
)
|
||||
parser.add_argument("session_dir")
|
||||
parser.add_argument("--serial-number", default=None)
|
||||
|
||||
@@ -1,4 +1,4 @@
|
||||
"""One-command product runner for G20_RIGHT_001 calibration."""
|
||||
"""One-command runner for a registered hand-calibration product."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
@@ -155,8 +155,9 @@ def _launch_command(
|
||||
resume_from: Path | None = None,
|
||||
) -> list[str]:
|
||||
values = {
|
||||
"hand_type": "right",
|
||||
"tag_layout": "g20_right_19",
|
||||
"model": config.model,
|
||||
"hand_type": config.side,
|
||||
"tag_layout": config.tag_layout,
|
||||
"serial_number": config.serial_number,
|
||||
"can_interface": config.can_interface,
|
||||
"session_dir": str(session),
|
||||
@@ -462,8 +463,8 @@ def _automatic_resume_candidate(config: ProductConfig) -> Path | None:
|
||||
continue
|
||||
if (
|
||||
start is None
|
||||
or start.get("hand_type") != "right"
|
||||
or start.get("tag_layout") != "g20_right_19"
|
||||
or start.get("hand_type") != config.side
|
||||
or start.get("tag_layout") != config.tag_layout
|
||||
or start.get("source_urdf_sha256")
|
||||
!= config.source_urdf_sha256
|
||||
):
|
||||
@@ -550,9 +551,9 @@ def run(
|
||||
print(
|
||||
"\n".join(
|
||||
[
|
||||
"PASS：G20右手标定、URDF修正和独立复验全部通过。",
|
||||
f"PASS：{config.model} {config.side} 标定、URDF修正和复验全部通过。",
|
||||
f"正式结果：{config.session_root / 'latest_passed'}",
|
||||
f"JSON：{session / f'g20_right_{config.serial_number}_calibration.json'}",
|
||||
f"JSON：{session / summary['artifacts']['json']}",
|
||||
f"URDF：{summary['artifacts']['urdf']}",
|
||||
]
|
||||
),
|
||||
@@ -564,7 +565,7 @@ def run(
|
||||
|
||||
|
||||
def main(args: list[str] | None = None) -> None:
|
||||
parser = argparse.ArgumentParser(description="G20右手一键精密标定")
|
||||
parser = argparse.ArgumentParser(description="配置驱动的机械手精密标定")
|
||||
parser.add_argument("--config", default=str(_default_product_config()))
|
||||
parser.add_argument("--workspace", default=None)
|
||||
parser.add_argument("--preflight-only", action="store_true")
|
||||
|
||||
+157
-6
@@ -55,6 +55,43 @@ def _task_tag_id_status(views: Mapping[str, Any]) -> str:
|
||||
)
|
||||
|
||||
|
||||
def _pnp_wait_status(views: Mapping[str, Any]) -> str:
|
||||
"""Explain why visible Tags have not produced a usable 3-D pose."""
|
||||
labels = {"front": "正面", "side": "侧面", "top": "顶部"}
|
||||
details: list[str] = []
|
||||
for name in ("front", "side", "top"):
|
||||
item = views.get(name, {})
|
||||
if not isinstance(item, Mapping) or item.get("pnp_pose_valid"):
|
||||
continue
|
||||
parts: list[str] = []
|
||||
progress = item.get("pnp_initialization_progress")
|
||||
if isinstance(progress, Mapping):
|
||||
parts.append(
|
||||
"初始化"
|
||||
f"{int(progress.get('accepted', 0))}/"
|
||||
f"{int(progress.get('required', 0))}"
|
||||
)
|
||||
counts: dict[str, int] = {}
|
||||
for field in ("pnp_rejection_counts", "group_pnp_rejection_counts"):
|
||||
values = item.get(field, {})
|
||||
if not isinstance(values, Mapping):
|
||||
continue
|
||||
for reason, count in values.items():
|
||||
counts[str(reason)] = counts.get(str(reason), 0) + int(count)
|
||||
if counts:
|
||||
common = sorted(
|
||||
counts.items(), key=lambda pair: (-pair[1], pair[0])
|
||||
)[:2]
|
||||
parts.append(
|
||||
"拒绝=" + ",".join(f"{reason}×{count}" for reason, count in common)
|
||||
)
|
||||
if parts:
|
||||
details.append(f"{labels[name]} " + "；".join(parts))
|
||||
if not details:
|
||||
return ""
|
||||
return "PnP：" + " | ".join(details) + "（Tag可见不等于三维位姿有效）"
|
||||
|
||||
|
||||
@dataclass
|
||||
class ProgressEstimator:
|
||||
started_at: float
|
||||
@@ -202,7 +239,7 @@ def render_progress_zh(
|
||||
)
|
||||
else:
|
||||
tag_status = (
|
||||
f"{visible_tags}/{required_tags} 有效"
|
||||
f"{visible_tags}/{required_tags} 可见"
|
||||
+ (f"（含锁定 {locked_tag_count}）" if locked_tag_count else "")
|
||||
)
|
||||
task_tag_ids = (
|
||||
@@ -233,10 +270,23 @@ def render_progress_zh(
|
||||
f"Tag：{tag_status}{task_tag_ids} 相机：{ready_cameras}/3 正常 反馈：{feedback_hz:.1f} Hz",
|
||||
f"质量：有效帧 {active.get('valid_frames', 0)} 已自动重扫 {retry} 次",
|
||||
]
|
||||
if fit_attempt_limit > 1:
|
||||
if str(status.get("reason", "")) == "waiting_for_task_tags_at_sweep_start":
|
||||
pnp_wait = _pnp_wait_status(views)
|
||||
if pnp_wait:
|
||||
lines.append(pnp_wait)
|
||||
fit_retry_cycles = [
|
||||
int(cycle) for cycle in active.get("fit_retry_cycles", [])
|
||||
]
|
||||
if fit_attempt > 1 and fit_retry_cycles:
|
||||
cycle_text = "/".join(str(cycle) for cycle in fit_retry_cycles)
|
||||
lines.append(
|
||||
f"拟合：整关节第 {fit_attempt}/{fit_attempt_limit} 次尝试；"
|
||||
"每次包含完整四轮双向扫描"
|
||||
f"拟合：首次拟合定位第 {cycle_text} 轮异常，"
|
||||
"仅补采该轮双向；其余轮次数据保留"
|
||||
)
|
||||
elif fit_attempt_limit > 1:
|
||||
lines.append(
|
||||
f"拟合：任务级硬门限检查（最多允许 "
|
||||
f"{fit_attempt_limit - 1} 次异常轮补采）"
|
||||
)
|
||||
resume = status.get("resume", {})
|
||||
if isinstance(resume, Mapping) and resume.get("used"):
|
||||
@@ -266,6 +316,25 @@ def classify_error(reason: str, status: Mapping[str, Any]) -> tuple[str, str, st
|
||||
for item in views.values()
|
||||
if isinstance(item, Mapping) and item.get("group_pnp_reason")
|
||||
] if isinstance(views, Mapping) else []
|
||||
pnp_diagnostics_present = bool(
|
||||
group_pnp_reasons
|
||||
or (
|
||||
isinstance(views, Mapping)
|
||||
and any(
|
||||
isinstance(item, Mapping)
|
||||
and any(
|
||||
item.get(field)
|
||||
for field in (
|
||||
"pnp_rejection_counts",
|
||||
"group_pnp_rejection_counts",
|
||||
"pnp_initialization_progress",
|
||||
"group_missing_candidate_roles",
|
||||
)
|
||||
)
|
||||
for item in views.values()
|
||||
)
|
||||
)
|
||||
)
|
||||
if value.startswith("CFG-"):
|
||||
return value, "产品配置或文件预检失败", "不要移动相机；复制本诊断块给开发者。"
|
||||
if value.startswith("CAM-STATUS-202"):
|
||||
@@ -280,6 +349,13 @@ def classify_error(reason: str, status: Mapping[str, Any]) -> tuple[str, str, st
|
||||
"标定节点状态心跳停止更新",
|
||||
"已采集的完整任务会保留；检查calibration.log后从断点继续，不要重新采集。",
|
||||
)
|
||||
if "palm_orientation_quality_failed" in value:
|
||||
return (
|
||||
"VAL-QUALITY-501",
|
||||
"掌部方向校正的可观测轨迹不足",
|
||||
"保持Tag安装不变；调整标定前手位，使正面Tag 10–13至少三枚"
|
||||
"在对应运动起始段可见。",
|
||||
)
|
||||
if "motor_state_stalled" in value:
|
||||
return "MOTION-STALL-301", "电机反馈停止向目标推进，程序已保持当前位置", "先检查机械卡阻，未排除前不要重复强推。"
|
||||
if any(
|
||||
@@ -295,9 +371,19 @@ def classify_error(reason: str, status: Mapping[str, Any]) -> tuple[str, str, st
|
||||
if (
|
||||
("synchronised" in value or "sweep_start_tag_timeout" in value)
|
||||
and not missing
|
||||
and group_pnp_reasons
|
||||
and pnp_diagnostics_present
|
||||
):
|
||||
return "CAM-GEOMETRY-201", "Tag可见，但整组PnP候选持续被几何检查拒绝", "不要调整Tag；复制本诊断块给开发者检查PnP候选选择。"
|
||||
problem = (
|
||||
"Tag可见，但三维PnP轨迹在运动中失效"
|
||||
if value.startswith("synchronised")
|
||||
and int(active.get("valid_frames", 0) or 0) > 0
|
||||
else "Tag可见，但三维PnP位姿初始化未完成"
|
||||
)
|
||||
return (
|
||||
"CAM-GEOMETRY-201",
|
||||
problem,
|
||||
"不要根据二维码可见性调整Tag；复制累计PnP拒绝原因给开发者。",
|
||||
)
|
||||
if missing or "tag" in value or "detection" in value or "synchronised" in value:
|
||||
return "OBS-TAG-103", f"所需Tag不可用或持续丢失：{missing}", "检查Tag是否脱落、翘起、反光或被遮挡后重新运行。"
|
||||
if "state" in value and ("timeout" in value or "lost" in value):
|
||||
@@ -308,6 +394,15 @@ def classify_error(reason: str, status: Mapping[str, Any]) -> tuple[str, str, st
|
||||
return "FIT-CHECKPOINT-402", "稳态检查点采集流程未完整结束", "完整样本已保留；复制本诊断块给开发者检查采集状态机。"
|
||||
if "fit" in value or "trajectory" in value or "axis" in value:
|
||||
return "FIT-MODEL-401", "关节轴或动态曲线拟合未达到精度门限", "不要放宽门限；复制本诊断块给开发者分析原始样本。"
|
||||
if (
|
||||
"endpoint_zero_offsets' is not defined" in value
|
||||
or "validated_endpoint_zero_state" in value
|
||||
):
|
||||
return (
|
||||
"PUB-ARTIFACT-601",
|
||||
"已验证零位状态未能完整传递到URDF发布阶段",
|
||||
"采集与验证数据仍可保留；不要移动相机或Tag，复制诊断块给开发者。",
|
||||
)
|
||||
if "validation" in value or "combination" in value or "quality" in value or "zero" in value:
|
||||
return "VAL-QUALITY-501", "留出验证或URDF零位验证未通过", "结果不会发布；复制本诊断块给开发者。"
|
||||
if "publish" in value or "artifact" in value or "URDF" in value:
|
||||
@@ -338,6 +433,20 @@ def build_failure_report(
|
||||
}
|
||||
if group_pnp_reasons:
|
||||
active["group_pnp_reasons"] = group_pnp_reasons
|
||||
for field in (
|
||||
"pnp_rejection_counts",
|
||||
"group_pnp_rejection_counts",
|
||||
"pnp_initialization_progress",
|
||||
"group_missing_candidate_roles",
|
||||
"pnp_candidate_diagnostics",
|
||||
):
|
||||
values = {
|
||||
str(view): item.get(field)
|
||||
for view, item in views.items()
|
||||
if isinstance(item, Mapping) and item.get(field)
|
||||
}
|
||||
if values:
|
||||
active[field] = values
|
||||
try:
|
||||
explanation, automatic_action = three_camera_reason_zh(
|
||||
str(status.get("state", "")), reason, active
|
||||
@@ -359,8 +468,43 @@ def build_failure_report(
|
||||
metrics = {
|
||||
"valid_frames": active.get("valid_frames"),
|
||||
"sample": active.get("sample", {}),
|
||||
"detection_frames": active.get("detection_frames"),
|
||||
"detection_valid_frames": active.get("detection_valid_frames"),
|
||||
"detection_rate": active.get("detection_rate"),
|
||||
"detection_rate_by_view": active.get("detection_rate_by_view", {}),
|
||||
"automatic_retry_count": active.get("automatic_retry_count", 0),
|
||||
}
|
||||
pnp_diagnostics = {
|
||||
str(view): {
|
||||
field: item.get(field)
|
||||
for field in (
|
||||
"group_pnp_reason",
|
||||
"group_missing_candidate_roles",
|
||||
"pnp_rejections",
|
||||
"pnp_rejection_counts",
|
||||
"group_pnp_rejection_counts",
|
||||
"pnp_initialization_progress",
|
||||
"pnp_candidate_diagnostics",
|
||||
)
|
||||
if item.get(field)
|
||||
}
|
||||
for view, item in (
|
||||
views.items() if isinstance(views, Mapping) else []
|
||||
)
|
||||
if isinstance(item, Mapping)
|
||||
and any(
|
||||
item.get(field)
|
||||
for field in (
|
||||
"group_pnp_reason",
|
||||
"group_missing_candidate_roles",
|
||||
"pnp_rejections",
|
||||
"pnp_rejection_counts",
|
||||
"group_pnp_rejection_counts",
|
||||
"pnp_initialization_progress",
|
||||
"pnp_candidate_diagnostics",
|
||||
)
|
||||
)
|
||||
}
|
||||
payload: dict[str, Any] = {
|
||||
"schema_version": 1,
|
||||
"serial_number": config.serial_number,
|
||||
@@ -374,6 +518,7 @@ def build_failure_report(
|
||||
"automatic_action_zh": automatic_action,
|
||||
"suggestion_zh": suggestion,
|
||||
"metrics": metrics,
|
||||
"pnp_diagnostics": pnp_diagnostics,
|
||||
"camera_state": camera_state,
|
||||
"feedback_hz": status.get("feedback_hz", 0.0),
|
||||
"hashes": {
|
||||
@@ -397,6 +542,12 @@ def build_failure_report(
|
||||
f"详细说明：{explanation}",
|
||||
f"自动处理：{automatic_action}",
|
||||
f"关键指标：{json.dumps(metrics, ensure_ascii=False, separators=(',', ':'))}",
|
||||
"PnP诊断："
|
||||
+ json.dumps(
|
||||
pnp_diagnostics,
|
||||
ensure_ascii=False,
|
||||
separators=(",", ":"),
|
||||
),
|
||||
f"相机状态：{json.dumps(camera_state, ensure_ascii=False, separators=(',', ':'))}",
|
||||
f"反馈状态：{float(payload['feedback_hz'] or 0.0):.1f} Hz",
|
||||
f"配置哈希：{payload['hashes']['product_config_sha256']}",
|
||||
|
||||
@@ -913,11 +913,15 @@ class SquareTagPoseTracker:
|
||||
self.last_candidates_by_role: dict[
|
||||
str, tuple[SquareTagPose, ...]
|
||||
] = {}
|
||||
self.last_candidate_diagnostics_by_role: dict[
|
||||
str, dict[str, float | int]
|
||||
] = {}
|
||||
self.branch_correction_counts: dict[str, int] = {}
|
||||
|
||||
def reset(self) -> None:
|
||||
self._previous.clear()
|
||||
self.last_candidates_by_role.clear()
|
||||
self.last_candidate_diagnostics_by_role.clear()
|
||||
self.branch_correction_counts.clear()
|
||||
|
||||
def estimate(
|
||||
@@ -938,28 +942,91 @@ class SquareTagPoseTracker:
|
||||
)
|
||||
except (ValueError, cv2.error):
|
||||
self.last_candidates_by_role[str(role)] = ()
|
||||
self.last_candidate_diagnostics_by_role[str(role)] = {
|
||||
"solved_candidate_count": 0,
|
||||
"reprojection_candidate_count": 0,
|
||||
"independent_tilt_candidate_count": 0,
|
||||
"maximum_reprojection_error_px": float(
|
||||
self.maximum_reprojection_error_px
|
||||
),
|
||||
"maximum_independent_tilt_deg": math.degrees(
|
||||
self.maximum_tag_tilt_rad
|
||||
),
|
||||
}
|
||||
return None, "pnp_solve_failed"
|
||||
if not candidates:
|
||||
self.last_candidates_by_role[str(role)] = ()
|
||||
self.last_candidate_diagnostics_by_role[str(role)] = {
|
||||
"solved_candidate_count": 0,
|
||||
"reprojection_candidate_count": 0,
|
||||
"independent_tilt_candidate_count": 0,
|
||||
"maximum_reprojection_error_px": float(
|
||||
self.maximum_reprojection_error_px
|
||||
),
|
||||
"maximum_independent_tilt_deg": math.degrees(
|
||||
self.maximum_tag_tilt_rad
|
||||
),
|
||||
}
|
||||
return None, "pnp_solve_failed"
|
||||
usable_candidates: list[SquareTagPose] = []
|
||||
for candidate in candidates:
|
||||
reprojection_candidates = [
|
||||
candidate
|
||||
for candidate in candidates
|
||||
if candidate.reprojection_error_px
|
||||
<= self.maximum_reprojection_error_px
|
||||
]
|
||||
candidate_tilts_rad: list[float] = []
|
||||
independent_candidates: list[SquareTagPose] = []
|
||||
for candidate in reprojection_candidates:
|
||||
normal = Rotation.from_quat(
|
||||
candidate.quaternion_xyzw
|
||||
).as_matrix()[:, 2]
|
||||
tilt = math.acos(
|
||||
float(np.clip(abs(normal[2]), 0.0, 1.0))
|
||||
)
|
||||
if (
|
||||
candidate.reprojection_error_px
|
||||
<= self.maximum_reprojection_error_px
|
||||
and tilt <= self.maximum_tag_tilt_rad
|
||||
):
|
||||
usable_candidates.append(candidate)
|
||||
candidate_tilts_rad.append(float(tilt))
|
||||
if tilt <= self.maximum_tag_tilt_rad:
|
||||
independent_candidates.append(candidate)
|
||||
|
||||
# Candidate generation and candidate selection have different
|
||||
# contracts. The per-Tag tilt limit protects a pose used without any
|
||||
# other geometry, but it must not erase a finite, low-reprojection
|
||||
# IPPE solution before SquareTagGroupPoseTracker can evaluate it
|
||||
# against the fixed palm reference, the articulated chain and the
|
||||
# preceding group pose. At a strongly oblique view the planar
|
||||
# ambiguity is usually smaller, and rejecting both branches at a
|
||||
# fixed angle caused deterministic mid-sweep holes despite continuous
|
||||
# image detections. Group tracking therefore receives every
|
||||
# reprojection-valid candidate; independent tracking below retains the
|
||||
# original tilt safety gate.
|
||||
self.last_candidates_by_role[str(role)] = tuple(
|
||||
usable_candidates
|
||||
reprojection_candidates
|
||||
)
|
||||
if not usable_candidates:
|
||||
diagnostics: dict[str, float | int] = {
|
||||
"solved_candidate_count": len(candidates),
|
||||
"reprojection_candidate_count": len(reprojection_candidates),
|
||||
"independent_tilt_candidate_count": len(independent_candidates),
|
||||
"minimum_reprojection_error_px": float(
|
||||
min(
|
||||
candidate.reprojection_error_px
|
||||
for candidate in candidates
|
||||
)
|
||||
),
|
||||
"maximum_reprojection_error_px": float(
|
||||
self.maximum_reprojection_error_px
|
||||
),
|
||||
"maximum_independent_tilt_deg": math.degrees(
|
||||
self.maximum_tag_tilt_rad
|
||||
),
|
||||
}
|
||||
if candidate_tilts_rad:
|
||||
diagnostics["minimum_candidate_tilt_deg"] = math.degrees(
|
||||
min(candidate_tilts_rad)
|
||||
)
|
||||
diagnostics["maximum_candidate_tilt_deg"] = math.degrees(
|
||||
max(candidate_tilts_rad)
|
||||
)
|
||||
self.last_candidate_diagnostics_by_role[str(role)] = diagnostics
|
||||
if not independent_candidates:
|
||||
return None, "no_pose_within_reprojection_or_tilt_limit"
|
||||
|
||||
previous_record = self._previous.get(str(role))
|
||||
@@ -971,7 +1038,7 @@ class SquareTagPoseTracker:
|
||||
previous = previous_pose
|
||||
|
||||
selected, reason = select_continuous_pose(
|
||||
usable_candidates,
|
||||
independent_candidates,
|
||||
previous=previous,
|
||||
maximum_reprojection_error_px=(
|
||||
self.maximum_reprojection_error_px
|
||||
@@ -1180,6 +1247,7 @@ class SquareTagGroupPoseTracker:
|
||||
self._task_reference_relative_poses: dict[
|
||||
tuple[str, str], tuple[Rotation, np.ndarray]
|
||||
] = {}
|
||||
self.last_missing_roles: tuple[str, ...] = ()
|
||||
|
||||
def reset(self, *, preserve_task_reference: bool = False) -> None:
|
||||
self._previous.clear()
|
||||
@@ -1190,6 +1258,7 @@ class SquareTagGroupPoseTracker:
|
||||
self.branch_correction_counts.clear()
|
||||
self._decreasing_relative_rotations.clear()
|
||||
self._coupled_reference_rotations.clear()
|
||||
self.last_missing_roles = ()
|
||||
if not preserve_task_reference:
|
||||
self._task_reference_relative_poses.clear()
|
||||
|
||||
@@ -1369,8 +1438,14 @@ class SquareTagGroupPoseTracker:
|
||||
tuple(candidates_by_role.get(role, ()))
|
||||
for role in self.roles
|
||||
]
|
||||
if any(not candidates for candidates in candidate_lists):
|
||||
self.last_missing_roles = tuple(
|
||||
role
|
||||
for role, candidates in zip(self.roles, candidate_lists)
|
||||
if not candidates
|
||||
)
|
||||
if self.last_missing_roles:
|
||||
return None, "group_missing_pose_candidates"
|
||||
self.last_missing_roles = ()
|
||||
|
||||
combinations = [
|
||||
dict(zip(self.roles, combination))
|
||||
|
||||
@@ -1,4 +1,10 @@
|
||||
"""Immutable G20 right-hand product identity and one-command preflight."""
|
||||
"""Immutable product identity and shared calibration preflight.
|
||||
|
||||
Model-specific kinematics live in registered calibration contracts. The
|
||||
loader itself only verifies that the selected contract, Tag configuration,
|
||||
cameras and immutable artifacts agree, so adding another hand does not
|
||||
require another copy of the one-command runner.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
@@ -11,11 +17,113 @@ from typing import Any, Mapping
|
||||
import yaml
|
||||
|
||||
from .extrinsics import camera_info_fingerprint, load_three_camera_extrinsics
|
||||
from .full_hand import (
|
||||
G20_RIGHT_19_LAYOUT,
|
||||
HandCalibrationProfile,
|
||||
get_hand_calibration_profile,
|
||||
)
|
||||
from .urdf_zero import ZeroCalibrationProfile, get_zero_calibration_profile
|
||||
|
||||
|
||||
VIEWS = ("front", "side", "top")
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class ProductCalibrationContract:
|
||||
"""Declarative boundary between one hand model and the shared engine."""
|
||||
|
||||
model: str
|
||||
side: str
|
||||
layout_id: str
|
||||
profile: HandCalibrationProfile
|
||||
zero_profile: ZeroCalibrationProfile
|
||||
|
||||
@property
|
||||
def required_tag_ids(self) -> frozenset[int]:
|
||||
return frozenset(
|
||||
int(tag_id)
|
||||
for tags in self.profile.view_tags.values()
|
||||
for tag_id in tags.values()
|
||||
)
|
||||
|
||||
@property
|
||||
def views(self) -> tuple[str, ...]:
|
||||
return tuple(self.profile.view_tags)
|
||||
|
||||
|
||||
_PRODUCT_CONTRACTS: dict[
|
||||
tuple[str, str, str], ProductCalibrationContract
|
||||
] = {}
|
||||
|
||||
|
||||
def register_product_calibration_contract(
|
||||
contract: ProductCalibrationContract,
|
||||
) -> None:
|
||||
"""Register one model/side/layout without modifying shared workflow code."""
|
||||
model = str(contract.model).strip().upper()
|
||||
side = str(contract.side).strip().lower()
|
||||
layout = str(contract.layout_id).strip().lower()
|
||||
if not model or side not in {"left", "right"} or not layout:
|
||||
raise ValueError("product calibration contract identity is invalid")
|
||||
if contract.profile.side != side:
|
||||
raise ValueError("product contract side differs from hand profile")
|
||||
if contract.profile.layout_id.lower() != layout:
|
||||
raise ValueError("product contract layout differs from hand profile")
|
||||
if contract.zero_profile.hand != contract.profile:
|
||||
raise ValueError("zero-calibration profile differs from hand profile")
|
||||
if len(contract.profile.baseline_command) != contract.profile.command_count:
|
||||
raise ValueError("profile baseline and command names differ in length")
|
||||
if not contract.required_tag_ids:
|
||||
raise ValueError("product contract must declare at least one Tag")
|
||||
key = (model, side, layout)
|
||||
existing = _PRODUCT_CONTRACTS.get(key)
|
||||
if existing is not None and existing != contract:
|
||||
raise ValueError(f"product calibration contract already registered: {key}")
|
||||
_PRODUCT_CONTRACTS[key] = contract
|
||||
|
||||
|
||||
def get_product_calibration_contract(
|
||||
model: str, side: str, layout_id: str
|
||||
) -> ProductCalibrationContract:
|
||||
key = (
|
||||
str(model).strip().upper(),
|
||||
str(side).strip().lower(),
|
||||
str(layout_id).strip().lower(),
|
||||
)
|
||||
try:
|
||||
return _PRODUCT_CONTRACTS[key]
|
||||
except KeyError as error:
|
||||
supported = ", ".join("/".join(item) for item in sorted(_PRODUCT_CONTRACTS))
|
||||
raise ValueError(
|
||||
f"unsupported calibration product {key}; registered={supported}"
|
||||
) from error
|
||||
|
||||
|
||||
register_product_calibration_contract(
|
||||
ProductCalibrationContract(
|
||||
model="G20",
|
||||
side="right",
|
||||
layout_id=G20_RIGHT_19_LAYOUT,
|
||||
profile=get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT),
|
||||
zero_profile=get_zero_calibration_profile(
|
||||
"right", G20_RIGHT_19_LAYOUT
|
||||
),
|
||||
)
|
||||
)
|
||||
for _legacy_side in ("left", "right"):
|
||||
register_product_calibration_contract(
|
||||
ProductCalibrationContract(
|
||||
model="G20",
|
||||
side=_legacy_side,
|
||||
layout_id="legacy_11",
|
||||
profile=get_hand_calibration_profile(_legacy_side, "legacy_11"),
|
||||
zero_profile=get_zero_calibration_profile(
|
||||
_legacy_side, "legacy_11"
|
||||
),
|
||||
)
|
||||
)
|
||||
|
||||
|
||||
def sha256_file(path: str | Path) -> str:
|
||||
digest = hashlib.sha256()
|
||||
with Path(path).open("rb") as stream:
|
||||
@@ -104,6 +212,10 @@ def _custom_pnp_tag_sizes_m_by_id(
|
||||
class ProductConfig:
|
||||
path: Path
|
||||
workspace: Path
|
||||
model: str
|
||||
side: str
|
||||
tag_layout: str
|
||||
calibration_contract: ProductCalibrationContract
|
||||
serial_number: str
|
||||
can_interface: str
|
||||
source_urdf: Path
|
||||
@@ -130,7 +242,7 @@ def load_product_config(
|
||||
workspace: str | Path | None = None,
|
||||
check_can: bool = True,
|
||||
) -> ProductConfig:
|
||||
"""Load and fully verify the fixed G20_RIGHT_001 product configuration."""
|
||||
"""Load and verify a product against its registered calibration contract."""
|
||||
source = Path(path).expanduser().resolve()
|
||||
if not source.is_file():
|
||||
raise ValueError(f"product config does not exist: {source}")
|
||||
@@ -142,8 +254,17 @@ def load_product_config(
|
||||
serial = str(raw.get("serial_number", ""))
|
||||
if re.fullmatch(r"[A-Za-z0-9_.-]+", serial) is None:
|
||||
raise ValueError("serial_number is invalid")
|
||||
if raw.get("model") != "G20" or raw.get("side") != "right":
|
||||
raise ValueError("product config must describe the G20 right hand")
|
||||
model = str(raw.get("model", "")).strip().upper()
|
||||
side = str(raw.get("side", "")).strip().lower()
|
||||
# Keep the deployed G20 schema compatible while making the layout an
|
||||
# explicit product choice for all new configurations.
|
||||
layout = str(
|
||||
raw.get(
|
||||
"tag_layout",
|
||||
G20_RIGHT_19_LAYOUT if (model, side) == ("G20", "right") else "",
|
||||
)
|
||||
).strip().lower()
|
||||
contract = get_product_calibration_contract(model, side, layout)
|
||||
can_interface = str(raw.get("can_interface", "")).strip()
|
||||
if not can_interface:
|
||||
raise ValueError("can_interface is required")
|
||||
@@ -197,16 +318,13 @@ def load_product_config(
|
||||
if actual != expected:
|
||||
raise ValueError(f"{name} SHA-256 mismatch: expected={expected} actual={actual}")
|
||||
|
||||
expected_tag_ids = set(range(19))
|
||||
expected_tag_ids = set(contract.required_tag_ids)
|
||||
tag_sizes = _tag_sizes_m_by_id(tag_config)
|
||||
if set(tag_sizes) != expected_tag_ids:
|
||||
raise ValueError(
|
||||
"g20_right_19 Tag config must contain exactly IDs 0 through 18"
|
||||
)
|
||||
expected_tag_sizes = {tag_id: 0.016 for tag_id in expected_tag_ids}
|
||||
if tag_sizes != expected_tag_sizes:
|
||||
raise ValueError(
|
||||
"g20_right_19 black-code Tag sizes must be 16 mm for every ID"
|
||||
f"{model}/{side}/{layout} Tag IDs differ from the registered "
|
||||
f"profile: expected={sorted(expected_tag_ids)} "
|
||||
f"actual={sorted(tag_sizes)}"
|
||||
)
|
||||
if _custom_pnp_tag_sizes_m_by_id(
|
||||
calibration_config, expected_tag_ids
|
||||
@@ -216,12 +334,17 @@ def load_product_config(
|
||||
)
|
||||
|
||||
camera_raw = _mapping(raw.get("cameras"), "cameras")
|
||||
if set(camera_raw) != set(VIEWS):
|
||||
raise ValueError("cameras must contain front/side/top")
|
||||
required_views = tuple(contract.views)
|
||||
if set(required_views) != set(VIEWS):
|
||||
raise ValueError(
|
||||
"the current shared engine requires front/side/top views"
|
||||
)
|
||||
if set(camera_raw) != set(required_views):
|
||||
raise ValueError(f"cameras must contain {'/'.join(required_views)}")
|
||||
loaded_extrinsics = load_three_camera_extrinsics(extrinsics)
|
||||
cameras: dict[str, dict[str, str]] = {}
|
||||
serials: set[str] = set()
|
||||
for view in VIEWS:
|
||||
for view in required_views:
|
||||
item = _mapping(camera_raw[view], f"cameras.{view}")
|
||||
camera_serial = str(item.get("serial_number", ""))
|
||||
info_path = _resolve_path(
|
||||
@@ -259,6 +382,10 @@ def load_product_config(
|
||||
return ProductConfig(
|
||||
path=source,
|
||||
workspace=root,
|
||||
model=model,
|
||||
side=side,
|
||||
tag_layout=layout,
|
||||
calibration_contract=contract,
|
||||
serial_number=serial,
|
||||
can_interface=can_interface,
|
||||
source_urdf=source_urdf,
|
||||
|
||||
@@ -24,7 +24,7 @@ from .full_hand import (
|
||||
from .product import ProductConfig, sha256_file
|
||||
from .storage import atomic_write_json
|
||||
from .urdf_zero import (
|
||||
RIGHT_19_FLEXION_ENDPOINT_JOINTS,
|
||||
RIGHT_19_MECHANICAL_ENDPOINT_JOINTS,
|
||||
get_zero_calibration_profile,
|
||||
)
|
||||
|
||||
@@ -41,6 +41,7 @@ ACTIVE_ZERO_JOINTS = frozenset(
|
||||
RETAINED_ACTIVE_ZERO_JOINTS = frozenset(
|
||||
get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT).active_joints
|
||||
) - ACTIVE_ZERO_JOINTS
|
||||
SCHEMA_V4_OFFSET_QUANTIZATION_TOLERANCE_RAD = 5.1e-9
|
||||
|
||||
|
||||
def _load_json(path: Path) -> dict[str, Any]:
|
||||
@@ -89,7 +90,7 @@ def _mask_active_origin_rpy_fields(text: str) -> str:
|
||||
|
||||
|
||||
def _mask_endpoint_coordinate_fields(text: str) -> str:
|
||||
"""Mask only limit/mimic fields induced by flexion zero coordinates."""
|
||||
"""Mask only limit/mimic fields induced by endpoint zero coordinates."""
|
||||
pattern = re.compile(
|
||||
r"<joint\b[^>]*\bname\s*=\s*([\"'])(?P<name>[^\"']+)\1[^>]*>.*?</joint>",
|
||||
re.DOTALL,
|
||||
@@ -98,7 +99,7 @@ def _mask_endpoint_coordinate_fields(text: str) -> str:
|
||||
def replace(match: re.Match[str]) -> str:
|
||||
block = match.group(0)
|
||||
name = match.group("name")
|
||||
if name in RIGHT_19_FLEXION_ENDPOINT_JOINTS:
|
||||
if name in RIGHT_19_MECHANICAL_ENDPOINT_JOINTS:
|
||||
return re.sub(
|
||||
r"(<limit\b[^>]*\bupper\s*=\s*)([\"'])[^\"']*\2",
|
||||
r"\1\2__CALIBRATED_UPPER__\2",
|
||||
@@ -110,7 +111,10 @@ def _mask_endpoint_coordinate_fields(text: str) -> str:
|
||||
block,
|
||||
re.DOTALL,
|
||||
)
|
||||
if mimic is not None and mimic.group("source") in RIGHT_19_FLEXION_ENDPOINT_JOINTS:
|
||||
if (
|
||||
mimic is not None
|
||||
and mimic.group("source") in RIGHT_19_MECHANICAL_ENDPOINT_JOINTS
|
||||
):
|
||||
return re.sub(
|
||||
r"(<mimic\b[^>]*\boffset\s*=\s*)([\"'])[^\"']*\2",
|
||||
r"\1\2__CALIBRATED_MIMIC_OFFSET__\2",
|
||||
@@ -145,7 +149,7 @@ def _verify_expected_origin_offsets(
|
||||
if set(offsets) != ACTIVE_ZERO_JOINTS or any(
|
||||
not math.isfinite(value) for value in offsets.values()
|
||||
):
|
||||
raise ValueError("expected offsets must contain 12 finite static-zero values")
|
||||
raise ValueError("expected offsets must contain all finite active static-zero values")
|
||||
before = _joint_elements(source)
|
||||
after = _joint_elements(corrected)
|
||||
maximum_rotation_error = 0.0
|
||||
@@ -186,7 +190,7 @@ def _verify_expected_origin_offsets(
|
||||
# so comparing that file with the published JSON necessarily permits half
|
||||
# of one last-place unit. This is about 2.9e-7 degrees and is far below
|
||||
# any calibration or URDF numerical significance.
|
||||
if maximum_rotation_error > 5.1e-9:
|
||||
if maximum_rotation_error > SCHEMA_V4_OFFSET_QUANTIZATION_TOLERANCE_RAD:
|
||||
raise ValueError(
|
||||
"corrected URDF origin.rpy does not match the published zero offsets"
|
||||
)
|
||||
@@ -245,7 +249,13 @@ def verify_corrected_urdf(
|
||||
original_limit = source_joints[name].find("limit")
|
||||
corrected_limit = corrected_joints[name].find("limit")
|
||||
expected_upper = float(original_limit.get("upper")) - offset
|
||||
if abs(float(corrected_limit.get("upper")) - expected_upper) > 1.0e-10:
|
||||
# Schema v4 stores the corresponding zero at eight decimal
|
||||
# places, while the URDF is written from the full-precision solve.
|
||||
# Match the half-last-place tolerance used for origin rotations.
|
||||
if (
|
||||
abs(float(corrected_limit.get("upper")) - expected_upper)
|
||||
> SCHEMA_V4_OFFSET_QUANTIZATION_TOLERANCE_RAD
|
||||
):
|
||||
raise ValueError(f"corrected URDF has invalid {name} upper limit")
|
||||
for name in PASSIVE_JOINTS:
|
||||
original_mimic = source_joints[name].find("mimic")
|
||||
@@ -253,9 +263,20 @@ def verify_corrected_urdf(
|
||||
if original_mimic is None or corrected_mimic is None:
|
||||
continue
|
||||
source_name = str(original_mimic.get("joint"))
|
||||
multiplier = float(original_mimic.get("multiplier", "1"))
|
||||
expected = float(original_mimic.get("offset", "0"))
|
||||
expected += float(original_mimic.get("multiplier", "1")) * endpoint_offsets.get(source_name, 0.0)
|
||||
if abs(float(corrected_mimic.get("offset", "0")) - expected) > 1.0e-10:
|
||||
expected += multiplier * endpoint_offsets.get(source_name, 0.0)
|
||||
# The active endpoint offset comes from schema-v4 JSON rounded to
|
||||
# eight decimal places, while the URDF mimic was written from the
|
||||
# full-precision solve. Propagate exactly the same accepted
|
||||
# quantization through the mimic multiplier; retain a much
|
||||
# smaller allowance for XML decimal formatting itself.
|
||||
tolerance = (
|
||||
abs(multiplier)
|
||||
* SCHEMA_V4_OFFSET_QUANTIZATION_TOLERANCE_RAD
|
||||
+ 1.0e-12
|
||||
)
|
||||
if abs(float(corrected_mimic.get("offset", "0")) - expected) > tolerance:
|
||||
raise ValueError(f"corrected URDF has invalid {name} mimic offset")
|
||||
return tuple(sorted(changed))
|
||||
|
||||
@@ -284,55 +305,62 @@ def verify_urdf_mesh_resources(urdf: str | Path) -> dict[str, Path]:
|
||||
|
||||
|
||||
def validate_runtime_curves_against_urdf_limits(
|
||||
payload: Mapping[str, Any], source_urdf: str | Path
|
||||
payload: Mapping[str, Any], runtime_urdf: str | Path
|
||||
) -> None:
|
||||
"""Reject a runtime curve whose commanded q leaves a CAD safety limit."""
|
||||
"""Reject a curve whose q leaves its runtime URDF coordinate limits.
|
||||
|
||||
Curves in schema v4 are expressed in the corrected URDF joint coordinate,
|
||||
not in the source-CAD coordinate. Endpoint zero calibration can therefore
|
||||
move a corrected coordinate limit while preserving the same physical CAD
|
||||
endpoint; callers publishing a calibrated pair must pass that corrected
|
||||
URDF here.
|
||||
"""
|
||||
validate_compact_payload(payload)
|
||||
source_joints = _joint_elements(source_urdf)
|
||||
runtime_joints = _joint_elements(runtime_urdf)
|
||||
for name, calibration in payload["joints"].items():
|
||||
joint = source_joints.get(str(name))
|
||||
joint = runtime_joints.get(str(name))
|
||||
if joint is None:
|
||||
raise ValueError(f"source URDF is missing runtime joint {name}")
|
||||
raise ValueError(f"runtime URDF is missing joint {name}")
|
||||
limit = joint.find("limit")
|
||||
if limit is None or limit.get("lower") is None or limit.get("upper") is None:
|
||||
raise ValueError(f"source URDF joint {name} has no finite position limit")
|
||||
raise ValueError(f"runtime URDF joint {name} has no finite position limit")
|
||||
lower = float(limit.get("lower"))
|
||||
upper = float(limit.get("upper"))
|
||||
if not math.isfinite(lower) or not math.isfinite(upper) or lower > upper:
|
||||
raise ValueError(f"source URDF joint {name} has invalid position limits")
|
||||
raise ValueError(f"runtime URDF joint {name} has invalid position limits")
|
||||
curve = np.asarray(calibration["angle_rad"], dtype=float)
|
||||
minimum = float(np.min(curve))
|
||||
maximum = float(np.max(curve))
|
||||
tolerance = 1.0e-7
|
||||
if minimum < lower - tolerance or maximum > upper + tolerance:
|
||||
raise ValueError(
|
||||
f"runtime curve exceeds source URDF limit for {name}: "
|
||||
f"runtime curve exceeds runtime URDF limit for {name}: "
|
||||
f"[{minimum:.9g}, {maximum:.9g}] not within "
|
||||
f"[{lower:.9g}, {upper:.9g}]"
|
||||
)
|
||||
|
||||
|
||||
def clamp_compact_payload_to_urdf_limits(
|
||||
payload: Mapping[str, Any], source_urdf: str | Path
|
||||
payload: Mapping[str, Any], runtime_urdf: str | Path
|
||||
) -> tuple[dict[str, Any], dict[str, int]]:
|
||||
"""Return a schema-preserving runtime payload bounded by CAD limits."""
|
||||
"""Return a schema-preserving payload bounded in its runtime coordinates."""
|
||||
result = copy.deepcopy(dict(payload))
|
||||
validate_compact_payload(result)
|
||||
source_joints = _joint_elements(source_urdf)
|
||||
runtime_joints = _joint_elements(runtime_urdf)
|
||||
clipped_by_joint: dict[str, int] = {}
|
||||
for name, calibration in result["joints"].items():
|
||||
joint = source_joints.get(str(name))
|
||||
joint = runtime_joints.get(str(name))
|
||||
limit = None if joint is None else joint.find("limit")
|
||||
if (
|
||||
limit is None
|
||||
or limit.get("lower") is None
|
||||
or limit.get("upper") is None
|
||||
):
|
||||
raise ValueError(f"source URDF joint {name} has no finite position limit")
|
||||
raise ValueError(f"runtime URDF joint {name} has no finite position limit")
|
||||
lower = float(limit.get("lower"))
|
||||
upper = float(limit.get("upper"))
|
||||
if not math.isfinite(lower) or not math.isfinite(upper) or lower > upper:
|
||||
raise ValueError(f"source URDF joint {name} has invalid position limits")
|
||||
raise ValueError(f"runtime URDF joint {name} has invalid position limits")
|
||||
source = np.asarray(calibration["angle_rad"], dtype=float)
|
||||
bounded = np.clip(source, lower, upper)
|
||||
count = int(np.count_nonzero(bounded != source))
|
||||
@@ -429,7 +457,10 @@ def compare_session_offsets(
|
||||
) -> dict[str, float]:
|
||||
left = active_offsets(first)
|
||||
right = active_offsets(second)
|
||||
differences = {name: abs(left[name] - right[name]) for name in sorted(left)}
|
||||
differences = {
|
||||
name: abs(left[name] - right[name])
|
||||
for name in sorted(left)
|
||||
}
|
||||
failed = {name: value for name, value in differences.items() if value > maximum_difference_rad}
|
||||
if failed:
|
||||
details = ", ".join(
|
||||
@@ -549,14 +580,15 @@ def finalize_session_artifacts(
|
||||
raise ValueError("node status is missing combination validation")
|
||||
_verify_combination_validation(combination)
|
||||
offsets = active_offsets(payload)
|
||||
endpoint_offsets = {
|
||||
name: offsets[name]
|
||||
for name in RIGHT_19_MECHANICAL_ENDPOINT_JOINTS
|
||||
}
|
||||
changed_joints = verify_corrected_urdf(
|
||||
config.source_urdf,
|
||||
paths["urdf"],
|
||||
expected_offsets_rad=offsets,
|
||||
endpoint_anchored_offsets_rad={
|
||||
name: offsets[name]
|
||||
for name in RIGHT_19_FLEXION_ENDPOINT_JOINTS
|
||||
},
|
||||
endpoint_anchored_offsets_rad=endpoint_offsets,
|
||||
)
|
||||
payload, clipped_runtime_joints = clamp_compact_payload_to_urdf_limits(
|
||||
payload, paths["urdf"]
|
||||
@@ -628,8 +660,13 @@ def finalize_session_artifacts(
|
||||
config.source_urdf,
|
||||
paths["urdf"],
|
||||
expected_offsets_rad=offsets,
|
||||
endpoint_anchored_offsets_rad=endpoint_offsets,
|
||||
)
|
||||
validate_runtime_curves_against_urdf_limits(payload, config.source_urdf)
|
||||
# Revalidate the same coordinate contract immediately before publication:
|
||||
# verify_corrected_urdf proves that corrected endpoint limits map back to
|
||||
# the original physical CAD endpoints, while the runtime curves must stay
|
||||
# inside those corrected-coordinate limits.
|
||||
validate_runtime_curves_against_urdf_limits(payload, paths["urdf"])
|
||||
final_mesh_hashes = {
|
||||
name: sha256_file(path)
|
||||
for name, path in verify_urdf_mesh_resources(paths["urdf"]).items()
|
||||
|
||||
+200
-18
@@ -3,7 +3,7 @@
|
||||
from __future__ import annotations
|
||||
|
||||
import re
|
||||
from typing import Any, Mapping
|
||||
from typing import Any, Mapping, Sequence
|
||||
|
||||
|
||||
STATE_NAMES_ZH = {
|
||||
@@ -115,10 +115,13 @@ def _task_text(active: Mapping[str, Any]) -> str:
|
||||
)
|
||||
fit_attempt = int(active.get("fit_attempt", 1))
|
||||
if fit_attempt > 1:
|
||||
task += (
|
||||
f"（整关节自动重采第{fit_attempt}/"
|
||||
f"{active.get('fit_attempt_limit', '?')}次）"
|
||||
)
|
||||
retry_cycles = active.get("fit_retry_cycles", [])
|
||||
if retry_cycles:
|
||||
task += "（补采异常轮" + "/".join(
|
||||
str(cycle) for cycle in retry_cycles
|
||||
) + "）"
|
||||
else:
|
||||
task += f"（拟合补采第{fit_attempt}次）"
|
||||
return task
|
||||
|
||||
|
||||
@@ -240,14 +243,87 @@ def three_camera_reason_zh(
|
||||
if reason == "synchronised_tag_state_timeout":
|
||||
group_reasons = active.get("group_pnp_reasons", {})
|
||||
if isinstance(group_reasons, Mapping) and group_reasons:
|
||||
reason_text = "、".join(
|
||||
f"{VIEW_NAMES_ZH.get(str(view), str(view))}={value}"
|
||||
for view, value in group_reasons.items()
|
||||
tag_rejections = active.get("pnp_rejection_counts", {})
|
||||
group_rejections = active.get(
|
||||
"group_pnp_rejection_counts", {}
|
||||
)
|
||||
missing_roles = active.get(
|
||||
"group_missing_candidate_roles", {}
|
||||
)
|
||||
candidate_diagnostics = active.get(
|
||||
"pnp_candidate_diagnostics", {}
|
||||
)
|
||||
view_details: list[str] = []
|
||||
for view, value in group_reasons.items():
|
||||
view_name = str(view)
|
||||
parts = [str(value)]
|
||||
missing = (
|
||||
missing_roles.get(view_name, ())
|
||||
if isinstance(missing_roles, Mapping)
|
||||
else ()
|
||||
)
|
||||
if isinstance(missing, Sequence) and not isinstance(
|
||||
missing, (str, bytes)
|
||||
) and missing:
|
||||
parts.append(
|
||||
"缺候选=" + ",".join(str(role) for role in missing)
|
||||
)
|
||||
counts: dict[str, int] = {}
|
||||
for source in (tag_rejections, group_rejections):
|
||||
values = (
|
||||
source.get(view_name)
|
||||
if isinstance(source, Mapping)
|
||||
else None
|
||||
)
|
||||
if isinstance(values, Mapping):
|
||||
for name, count in values.items():
|
||||
counts[str(name)] = counts.get(str(name), 0) + int(
|
||||
count
|
||||
)
|
||||
if counts:
|
||||
common = sorted(
|
||||
counts.items(), key=lambda pair: (-pair[1], pair[0])
|
||||
)[:3]
|
||||
parts.append(
|
||||
"累计拒绝="
|
||||
+ ",".join(
|
||||
f"{name}×{count}" for name, count in common
|
||||
)
|
||||
)
|
||||
view_candidates = (
|
||||
candidate_diagnostics.get(view_name, {})
|
||||
if isinstance(candidate_diagnostics, Mapping)
|
||||
else {}
|
||||
)
|
||||
if isinstance(view_candidates, Mapping) and missing:
|
||||
summaries: list[str] = []
|
||||
for role in missing:
|
||||
diagnostic = view_candidates.get(str(role), {})
|
||||
if not isinstance(diagnostic, Mapping):
|
||||
continue
|
||||
summaries.append(
|
||||
f"{role}(solve="
|
||||
f"{int(diagnostic.get('solved_candidate_count', 0))},"
|
||||
"reproj="
|
||||
f"{int(diagnostic.get('reprojection_candidate_count', 0))},"
|
||||
"tilt="
|
||||
f"{int(diagnostic.get('independent_tilt_candidate_count', 0))})"
|
||||
)
|
||||
if summaries:
|
||||
parts.append("候选统计=" + ",".join(summaries))
|
||||
view_details.append(
|
||||
f"{VIEW_NAMES_ZH.get(view_name, view_name)}="
|
||||
+ "；".join(parts)
|
||||
)
|
||||
reason_text = "、".join(view_details)
|
||||
return (
|
||||
f"{detail_prefix}Tag仍可见且反馈正常，但连续图像帧被整组PnP几何检查拒绝"
|
||||
f"{detail_prefix}已经取得部分有效轨迹，但Tag仍可见且反馈正常时，"
|
||||
"后续连续图像帧"
|
||||
"被整组PnP几何检查拒绝"
|
||||
f"（{reason_text}），因此无法与电机状态形成有效轨迹帧。",
|
||||
"不要调整或反复粘贴Tag；保留当前会话并复制诊断块给开发者检查PnP分支逻辑。",
|
||||
"不要调整或反复粘贴Tag；保留当前会话中的"
|
||||
"group_pnp_candidate_event，"
|
||||
"按缺失角色的候选统计检查PnP分支逻辑。",
|
||||
)
|
||||
return (
|
||||
f"{detail_prefix}运动过程中连续超过允许时间没有取得“所需Tag全部有效且能与电机状态"
|
||||
@@ -273,15 +349,81 @@ def three_camera_reason_zh(
|
||||
)
|
||||
if reason == "sweep_start_tag_timeout":
|
||||
group_reasons = active.get("group_pnp_reasons", {})
|
||||
if isinstance(group_reasons, Mapping) and group_reasons:
|
||||
reason_text = "、".join(
|
||||
f"{VIEW_NAMES_ZH.get(str(view), str(view))}={value}"
|
||||
for view, value in group_reasons.items()
|
||||
progress_by_view = active.get("pnp_initialization_progress", {})
|
||||
tag_rejections = active.get("pnp_rejection_counts", {})
|
||||
group_rejections = active.get("group_pnp_rejection_counts", {})
|
||||
if any(
|
||||
isinstance(value, Mapping) and bool(value)
|
||||
for value in (
|
||||
group_reasons,
|
||||
progress_by_view,
|
||||
tag_rejections,
|
||||
group_rejections,
|
||||
)
|
||||
):
|
||||
details: list[str] = []
|
||||
views = set()
|
||||
for value in (
|
||||
group_reasons,
|
||||
progress_by_view,
|
||||
tag_rejections,
|
||||
group_rejections,
|
||||
):
|
||||
if isinstance(value, Mapping):
|
||||
views.update(str(view) for view in value)
|
||||
for view in sorted(views):
|
||||
parts: list[str] = []
|
||||
progress = (
|
||||
progress_by_view.get(view)
|
||||
if isinstance(progress_by_view, Mapping)
|
||||
else None
|
||||
)
|
||||
if isinstance(progress, Mapping):
|
||||
parts.append(
|
||||
"初始化"
|
||||
f"{int(progress.get('accepted', 0))}/"
|
||||
f"{int(progress.get('required', 0))}"
|
||||
)
|
||||
counts: dict[str, int] = {}
|
||||
for source in (tag_rejections, group_rejections):
|
||||
values = (
|
||||
source.get(view)
|
||||
if isinstance(source, Mapping)
|
||||
else None
|
||||
)
|
||||
if isinstance(values, Mapping):
|
||||
for name, count in values.items():
|
||||
counts[str(name)] = (
|
||||
counts.get(str(name), 0) + int(count)
|
||||
)
|
||||
if counts:
|
||||
common = sorted(
|
||||
counts.items(), key=lambda pair: (-pair[1], pair[0])
|
||||
)[:3]
|
||||
parts.append(
|
||||
"累计拒绝="
|
||||
+ ",".join(
|
||||
f"{name}×{count}" for name, count in common
|
||||
)
|
||||
)
|
||||
latest = (
|
||||
group_reasons.get(view)
|
||||
if isinstance(group_reasons, Mapping)
|
||||
else None
|
||||
)
|
||||
if latest and not str(latest).startswith(
|
||||
"group_initializing:"
|
||||
):
|
||||
parts.append(f"最后状态={latest}")
|
||||
if parts:
|
||||
details.append(
|
||||
f"{VIEW_NAMES_ZH.get(view, view)}=" + "；".join(parts)
|
||||
)
|
||||
reason_text = "、".join(details) or "未形成完整初始化窗口"
|
||||
return (
|
||||
"被测电机已经到达扫描起点，所需Tag也可见，但整组PnP初始化持续"
|
||||
f"拒绝候选（{reason_text}），因此没有生成同步端点帧。",
|
||||
"不要重新粘贴可见Tag；保留当前会话并复制诊断块给开发者检查PnP安装先验。",
|
||||
"被测电机已经到达扫描起点，所需Tag也可见，但三维PnP位姿初始化"
|
||||
f"没有完成（{reason_text}），因此没有生成同步端点帧。",
|
||||
"不要根据可见性重复粘贴Tag；保留累计拒绝原因并检查PnP候选选择。",
|
||||
)
|
||||
return (
|
||||
"被测电机已经到达扫描起点，但当前任务所需的实时运动Tag没有形成足够的"
|
||||
@@ -308,6 +450,19 @@ def three_camera_reason_zh(
|
||||
"随机复测位置没有采集到足够的同步有效Tag帧。",
|
||||
"检查当前机位Tag可见性后调用resume。",
|
||||
)
|
||||
if reason == "palm_orientation_quality_failed":
|
||||
failures = active.get("failures", [])
|
||||
detail = (
|
||||
str(failures[0].get("reason", "方向观测不足"))
|
||||
if failures
|
||||
else "方向观测不足"
|
||||
)
|
||||
return (
|
||||
"掌部公共方向无法由至少三根手指的短时正面轨迹稳定确定："
|
||||
+ detail,
|
||||
"保持Tag安装不变；让正面Tag 10–13在对应MCP-pitch起始段"
|
||||
"至少可见15°行程后重新标定。",
|
||||
)
|
||||
if reason in {"joint_fit_check_failed", "joint_fit_systematic_failure"}:
|
||||
metric_names = {
|
||||
"plane_rms_mm": "平面拟合RMS",
|
||||
@@ -327,8 +482,10 @@ def three_camera_reason_zh(
|
||||
"axis_plane_rms_mm": "三维圆轴向RMS",
|
||||
"axis_radial_rms_mm": "三维圆半径RMS",
|
||||
"axis_pose_line_rms_mm": "姿态轨迹轴线RMS",
|
||||
"axis_line_cycle_rms_mm": "四轮轴线位置RMS",
|
||||
"rotation_circle_axis_difference_deg": "姿态轴与圆轨迹轴夹角",
|
||||
"axis_cycle_difference_deg": "各轮转轴方向极差",
|
||||
"cross_view_roll_curve": "正面/侧面关节角曲线差异RMS",
|
||||
"third_cycle_axis_holdout_deg": "第三轮留出零位可观测轴向误差",
|
||||
"third_cycle_axis_line_rms_mm": "第三轮留出轴线RMS",
|
||||
"third_cycle_trajectory_p95_deg": "第三轮留出轨迹误差P95",
|
||||
@@ -353,6 +510,7 @@ def three_camera_reason_zh(
|
||||
"axis_plane_rms_mm": "mm",
|
||||
"axis_radial_rms_mm": "mm",
|
||||
"axis_pose_line_rms_mm": "mm",
|
||||
"axis_line_cycle_rms_mm": "mm",
|
||||
"rotation_circle_axis_difference_deg": "°",
|
||||
"axis_cycle_difference_deg": "°",
|
||||
"third_cycle_axis_holdout_deg": "°",
|
||||
@@ -360,6 +518,7 @@ def three_camera_reason_zh(
|
||||
"third_cycle_trajectory_p95_deg": "°",
|
||||
"state_image_sync_p95_ms": "ms",
|
||||
"tag_valid_rate_percent": "%",
|
||||
"cross_view_roll_curve": "°",
|
||||
}
|
||||
details: list[str] = []
|
||||
for failure in active.get("failures", []):
|
||||
@@ -402,8 +561,19 @@ def three_camera_reason_zh(
|
||||
for failure in active.get("failures", [])
|
||||
)
|
||||
if reason == "joint_fit_systematic_failure":
|
||||
cross_view_systematic = any(
|
||||
failure.get("classification")
|
||||
in {
|
||||
"stable_cross_view_installation_or_model_bias",
|
||||
"stable_cross_view_direction_conflict",
|
||||
}
|
||||
for failure in active.get("failures", [])
|
||||
)
|
||||
suggestion = (
|
||||
"各轮重复出现同一模型冲突，继续运动不会改善；程序已禁止自动重扫。"
|
||||
"四轮都出现稳定的正面/侧面差异，属于Tag安装外参或跨视角模型偏差，"
|
||||
"继续重扫不会改善；检查Tag刚性安装与跨视角安装变换，不要放宽门限。"
|
||||
if cross_view_systematic
|
||||
else "各轮重复出现同一模型冲突，继续运动不会改善；程序已禁止自动重扫。"
|
||||
"请直接复制诊断块给开发者，不要放宽门限。"
|
||||
)
|
||||
else:
|
||||
@@ -543,6 +713,18 @@ def three_camera_reason_zh(
|
||||
return "三维轨迹、关节轴零位和第三轮留出验证已经完成。", "检查JSON、修正URDF路径和quality.passed。"
|
||||
if reason == "quality_failed":
|
||||
return "标定流程完成，但拟合或随机复测质量没有达到验收阈值。", "检查最终JSON的quality以及启动终端中的拟合日志。"
|
||||
if reason.startswith("validated_endpoint_zero_state"):
|
||||
return (
|
||||
"轨迹和URDF零位验证已经通过，但发布前检测到端点零位状态缺失或与"
|
||||
"已验证模型不一致；这是程序内部状态生命周期错误，结果未发布。",
|
||||
"不要移动相机、Tag或机械手底座；保留当前会话并把原因码交给开发者。",
|
||||
)
|
||||
if reason.startswith("PUB-ARTIFACT-601:"):
|
||||
return (
|
||||
"标定节点已经生成通过质量门限的JSON和候选URDF，但一键程序在正式发布前"
|
||||
"发现这对产物的坐标、限位、哈希或资源一致性检查失败；原始URDF未被覆盖。",
|
||||
"不要重新标定相机或调整Tag；保留本会话产物和启动日志供开发者检查发布契约。",
|
||||
)
|
||||
if reason == "combination_pose_prediction_failed":
|
||||
return (
|
||||
"单关节、零位和URDF几何验证已通过，但当前多关节组合姿态的Tag实测位姿与模型预测超过门限。",
|
||||
|
||||
+2374
-270
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
@@ -39,16 +39,21 @@ def _default_source_urdf(hand_type: str) -> Path:
|
||||
|
||||
|
||||
def _launch_stack(context):
|
||||
from g20_thumb_apriltag_calibration.product import (
|
||||
get_product_calibration_contract,
|
||||
)
|
||||
|
||||
model = LaunchConfiguration("model").perform(context).strip().upper()
|
||||
hand_type = LaunchConfiguration("hand_type").perform(context).lower()
|
||||
if hand_type not in {"left", "right"}:
|
||||
raise RuntimeError("hand_type must be left or right")
|
||||
tag_layout = LaunchConfiguration("tag_layout").perform(context).lower()
|
||||
if tag_layout not in {"legacy_11", "g20_right_15", "g20_right_19"}:
|
||||
raise RuntimeError(
|
||||
"tag_layout must be legacy_11, g20_right_15 or g20_right_19"
|
||||
try:
|
||||
contract = get_product_calibration_contract(
|
||||
model, hand_type, tag_layout
|
||||
)
|
||||
if tag_layout in {"g20_right_15", "g20_right_19"} and hand_type != "right":
|
||||
raise RuntimeError("G20 right product layout requires hand_type:=right")
|
||||
except ValueError as error:
|
||||
raise RuntimeError(str(error)) from error
|
||||
requested_tag_config = LaunchConfiguration("tag_config").perform(context)
|
||||
package_share = Path(
|
||||
get_package_share_directory("g20_thumb_apriltag_calibration")
|
||||
@@ -68,9 +73,10 @@ def _launch_stack(context):
|
||||
)
|
||||
if not tag_config.is_file():
|
||||
raise RuntimeError(f"tag config does not exist: {tag_config}")
|
||||
command_topic = f"/g20/cb_{hand_type}_hand_control_cmd"
|
||||
state_topic = f"/g20/cb_{hand_type}_hand_state"
|
||||
info_topic = f"/g20/cb_{hand_type}_hand_info"
|
||||
topic_prefix = f"/{model.lower()}"
|
||||
command_topic = f"{topic_prefix}/cb_{hand_type}_hand_control_cmd"
|
||||
state_topic = f"{topic_prefix}/cb_{hand_type}_hand_state"
|
||||
info_topic = f"{topic_prefix}/cb_{hand_type}_hand_info"
|
||||
requested_source = LaunchConfiguration("source_urdf_path").perform(context)
|
||||
source_urdf = (
|
||||
Path(requested_source).expanduser().resolve()
|
||||
@@ -82,7 +88,7 @@ def _launch_stack(context):
|
||||
expected_source_hash = LaunchConfiguration(
|
||||
"source_urdf_expected_sha256"
|
||||
).perform(context).strip().lower()
|
||||
if tag_layout in {"g20_right_15", "g20_right_19"}:
|
||||
if contract.profile.supports("urdf_zero_publication"):
|
||||
if re.fullmatch(r"[0-9a-f]{64}", expected_source_hash) is None:
|
||||
raise RuntimeError(
|
||||
"G20 right product layout requires source_urdf_expected_sha256 confirmed "
|
||||
@@ -239,10 +245,10 @@ def _launch_stack(context):
|
||||
parameters=[
|
||||
{
|
||||
"hand_type": hand_type,
|
||||
"hand_joint": "G20",
|
||||
"hand_joint": model,
|
||||
"can": LaunchConfiguration("can_interface"),
|
||||
"modbus": "None",
|
||||
"topic_prefix": "/g20",
|
||||
"topic_prefix": topic_prefix,
|
||||
"move_on_startup": False,
|
||||
"startup_speed": ParameterValue(
|
||||
LaunchConfiguration("calibration_speed"), value_type=int
|
||||
@@ -274,6 +280,7 @@ def _launch_stack(context):
|
||||
LaunchConfiguration("calibration_config"),
|
||||
{
|
||||
"serial_number": hand_serial,
|
||||
"model": model,
|
||||
"hand_type": hand_type,
|
||||
"tag_layout": tag_layout,
|
||||
"session_dir": str(session_dir),
|
||||
@@ -396,6 +403,7 @@ def generate_launch_description() -> LaunchDescription:
|
||||
name="FASTRTPS_DEFAULT_PROFILES_FILE",
|
||||
value=str(package_share / "config" / "fastdds_large_images.xml"),
|
||||
),
|
||||
DeclareLaunchArgument("model", default_value="G20"),
|
||||
DeclareLaunchArgument("hand_type", default_value="left"),
|
||||
DeclareLaunchArgument("tag_layout", default_value="legacy_11"),
|
||||
DeclareLaunchArgument("serial_number", default_value="UNSET"),
|
||||
|
||||
@@ -26,7 +26,7 @@ setup(
|
||||
zip_safe=True,
|
||||
maintainer="lxp",
|
||||
maintainer_email="support@linker-robotics.com",
|
||||
description="One-command three-camera AprilTag calibration for the G20 right hand",
|
||||
description="Profile-driven three-camera AprilTag hand calibration",
|
||||
license="MIT",
|
||||
entry_points={
|
||||
"console_scripts": [
|
||||
@@ -68,6 +68,10 @@ setup(
|
||||
"calibrate_g20_right = "
|
||||
"g20_thumb_apriltag_calibration.one_command:main"
|
||||
),
|
||||
(
|
||||
"calibrate_hand = "
|
||||
"g20_thumb_apriltag_calibration.one_command:main"
|
||||
),
|
||||
],
|
||||
},
|
||||
)
|
||||
|
||||
@@ -206,7 +206,7 @@ def test_three_camera_calibration_has_hard_preflight_and_three_rounds() -> None:
|
||||
assert parameters["zero_maximum_observability_condition_number"] >= 1.0
|
||||
assert parameters["zero_maximum_offset_deg"] <= 20.0
|
||||
assert parameters["zero_finger_maximum_offset_deg"] <= 3.0
|
||||
assert parameters["flexion_endpoint_maximum_offset_deg"] == 5.0
|
||||
assert parameters["mechanical_endpoint_maximum_offset_deg"] == 5.0
|
||||
assert parameters["image_trajectory_maximum_radial_rms_px"] <= 2.0
|
||||
assert parameters["image_trajectory_maximum_radial_p95_px"] <= 3.5
|
||||
assert parameters["image_trajectory_minimum_radius_px"] >= 20.0
|
||||
|
||||
@@ -28,6 +28,7 @@ from g20_thumb_apriltag_calibration.full_hand import (
|
||||
clamp_runtime_fits_to_urdf_limits,
|
||||
center_splay_curve,
|
||||
compare_cross_view_roll_curves,
|
||||
cross_view_roll_diagnostic_metrics,
|
||||
derive_mimic_passive_fits,
|
||||
fit_joint_center_curve,
|
||||
fit_joint_image_curve,
|
||||
@@ -98,6 +99,8 @@ def test_right_19_profile_has_exact_layout_tasks_and_measured_dips() -> None:
|
||||
"pinky_mcp_roll",
|
||||
"pinky_mcp_roll_side",
|
||||
)
|
||||
assert profile.palm_axis_observers == ()
|
||||
assert profile.minimum_palm_orientation_sources == 0
|
||||
assert profile.sweep_specs[6].joints == ("pinky_pip", "pinky_dip")
|
||||
assert {
|
||||
name: spec.source_joint
|
||||
@@ -411,6 +414,98 @@ def test_cross_view_roll_curve_must_agree_before_fusion() -> None:
|
||||
compare_cross_view_roll_curves(fit, reverse)
|
||||
|
||||
|
||||
def test_cross_view_roll_curve_allows_bounded_projection_scale() -> None:
|
||||
values = tuple(0.5 * (127 - command) / 127 for command in range(256))
|
||||
primary = JointCurveFit(values, values, values, {}, 0.0, 0.0, {})
|
||||
projected = tuple(0.8 * value for value in values)
|
||||
secondary = replace(
|
||||
primary,
|
||||
angle_rad=projected,
|
||||
decreasing_rad=projected,
|
||||
increasing_rad=projected,
|
||||
)
|
||||
|
||||
with pytest.raises(ValueError, match="cross_view_roll_curve"):
|
||||
compare_cross_view_roll_curves(primary, secondary)
|
||||
|
||||
metrics = compare_cross_view_roll_curves(
|
||||
primary,
|
||||
secondary,
|
||||
allow_projection_scale=True,
|
||||
)
|
||||
|
||||
assert metrics["projection_scale"] == pytest.approx(1.25)
|
||||
assert metrics["angle_rad_rms_difference_rad"] == pytest.approx(0.0)
|
||||
assert metrics["raw_angle_rad_rms_difference_rad"] > 0.0
|
||||
|
||||
|
||||
def test_projected_cross_view_keeps_branch_gap_as_diagnostic() -> None:
|
||||
values = tuple(0.5 * (127 - command) / 127 for command in range(256))
|
||||
primary = JointCurveFit(values, values, values, {}, 0.0, 0.0, {})
|
||||
shifted_increasing = tuple(
|
||||
value + math.radians(0.5) for value in values
|
||||
)
|
||||
secondary = replace(primary, increasing_rad=shifted_increasing)
|
||||
|
||||
metrics = compare_cross_view_roll_curves(
|
||||
primary,
|
||||
secondary,
|
||||
allow_projection_scale=True,
|
||||
)
|
||||
|
||||
assert metrics["baseline_branch_gap_difference_rad"] > math.radians(0.3)
|
||||
assert metrics["baseline_branch_gap_gate_applied"] == 0.0
|
||||
|
||||
|
||||
def test_cross_view_roll_curve_rejects_implausible_projection_scale() -> None:
|
||||
values = tuple(0.5 * (127 - command) / 127 for command in range(256))
|
||||
primary = JointCurveFit(values, values, values, {}, 0.0, 0.0, {})
|
||||
collapsed = tuple(0.2 * value for value in values)
|
||||
secondary = replace(
|
||||
primary,
|
||||
angle_rad=collapsed,
|
||||
decreasing_rad=collapsed,
|
||||
increasing_rad=collapsed,
|
||||
)
|
||||
|
||||
with pytest.raises(ValueError, match="projection_scale_ratio"):
|
||||
compare_cross_view_roll_curves(
|
||||
primary,
|
||||
secondary,
|
||||
allow_projection_scale=True,
|
||||
)
|
||||
|
||||
|
||||
def test_validation_only_cross_view_metrics_never_create_a_retry_gate() -> None:
|
||||
values = tuple(0.5 * (127 - command) / 127 for command in range(256))
|
||||
primary = JointCurveFit(values, values, values, {}, 0.0, 0.0, {})
|
||||
biased = tuple(
|
||||
value
|
||||
+ math.radians(2.5) * math.sin(math.pi * command / 255.0)
|
||||
for command, value in enumerate(values)
|
||||
)
|
||||
secondary = replace(
|
||||
primary,
|
||||
angle_rad=biased,
|
||||
decreasing_rad=biased,
|
||||
increasing_rad=biased,
|
||||
)
|
||||
|
||||
metrics = cross_view_roll_diagnostic_metrics(primary, secondary)
|
||||
|
||||
assert metrics["angle_rad_rms_difference_rad"] > math.radians(1.0)
|
||||
|
||||
reversed_secondary = replace(
|
||||
secondary,
|
||||
angle_rad=tuple(-value for value in secondary.angle_rad),
|
||||
decreasing_rad=tuple(-value for value in secondary.decreasing_rad),
|
||||
increasing_rad=tuple(-value for value in secondary.increasing_rad),
|
||||
)
|
||||
assert cross_view_roll_diagnostic_metrics(
|
||||
primary, reversed_secondary
|
||||
) == {"direction_disagrees": 1.0}
|
||||
|
||||
|
||||
def _records() -> list[dict[str, object]]:
|
||||
commands = list(range(0, 256, 16))
|
||||
if commands[-1] != 255:
|
||||
|
||||
@@ -4,6 +4,7 @@ import json
|
||||
import math
|
||||
from pathlib import Path
|
||||
from types import SimpleNamespace
|
||||
from xml.etree import ElementTree as ET
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
@@ -39,6 +40,7 @@ from g20_thumb_apriltag_calibration.publication import (
|
||||
atomic_session_pointer,
|
||||
build_mujoco_validation_commands,
|
||||
finalize_session_artifacts,
|
||||
session_artifact_paths,
|
||||
validate_runtime_curves_against_urdf_limits,
|
||||
verify_corrected_urdf,
|
||||
verify_urdf_mesh_resources,
|
||||
@@ -52,6 +54,8 @@ from g20_thumb_apriltag_calibration.three_camera_node import (
|
||||
_combination_motor_directions,
|
||||
_combination_validation_items,
|
||||
_model_link_in_observer_base,
|
||||
_palm_axis_observer_schema,
|
||||
_palm_axis_resume_policy,
|
||||
_steady_checkpoint_commands,
|
||||
_unresolved_fit_failure_tasks,
|
||||
combination_target_coverage,
|
||||
@@ -59,7 +63,7 @@ from g20_thumb_apriltag_calibration.three_camera_node import (
|
||||
resumable_completed_task_prefix,
|
||||
)
|
||||
from g20_thumb_apriltag_calibration.urdf_zero import (
|
||||
RIGHT_19_FLEXION_ENDPOINT_JOINTS,
|
||||
RIGHT_19_MECHANICAL_ENDPOINT_JOINTS,
|
||||
UrdfKinematicModel,
|
||||
get_zero_calibration_profile,
|
||||
write_zero_corrected_urdf,
|
||||
@@ -79,8 +83,15 @@ def _config(tmp_path: Path, *, passes: int = 2):
|
||||
)
|
||||
|
||||
|
||||
def _payload(serial: str = "G20_RIGHT_001") -> dict:
|
||||
def _payload(
|
||||
serial: str = "G20_RIGHT_001",
|
||||
*,
|
||||
zero_offsets: dict[str, float] | None = None,
|
||||
) -> dict:
|
||||
profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT)
|
||||
offsets = zero_offsets or {
|
||||
name: 0.01 for name in profile.active_joints
|
||||
}
|
||||
baseline = [255] * 20
|
||||
baseline[6:10] = [127] * 4
|
||||
fits = {}
|
||||
@@ -100,7 +111,7 @@ def _payload(serial: str = "G20_RIGHT_001") -> dict:
|
||||
return build_compact_payload(
|
||||
serial_number=serial,
|
||||
measured_fits=fits,
|
||||
urdf_zero_offsets_rad={name: 0.01 for name in profile.active_joints},
|
||||
urdf_zero_offsets_rad=offsets,
|
||||
validation_errors_rad=[0.0, math.radians(0.5)],
|
||||
passed=True,
|
||||
baseline=baseline,
|
||||
@@ -109,10 +120,18 @@ def _payload(serial: str = "G20_RIGHT_001") -> dict:
|
||||
)
|
||||
|
||||
|
||||
def _make_passed_session(config, stamp: str) -> Path:
|
||||
def _make_passed_session(
|
||||
config,
|
||||
stamp: str,
|
||||
*,
|
||||
zero_offsets: dict[str, float] | None = None,
|
||||
) -> Path:
|
||||
session = config.session_root / stamp
|
||||
session.mkdir(parents=True)
|
||||
payload = _payload(config.serial_number)
|
||||
offsets = zero_offsets or {
|
||||
name: 0.01 for name in ACTIVE_ZERO_JOINTS
|
||||
}
|
||||
payload = _payload(config.serial_number, zero_offsets=offsets)
|
||||
atomic_write_json(
|
||||
session / f"g20_right_{config.serial_number}_calibration.json", payload
|
||||
)
|
||||
@@ -120,9 +139,10 @@ def _make_passed_session(config, stamp: str) -> Path:
|
||||
source_urdf=config.source_urdf,
|
||||
output_directory=session,
|
||||
serial_number=config.serial_number,
|
||||
offsets_rad={name: 0.01 for name in ACTIVE_ZERO_JOINTS},
|
||||
offsets_rad=offsets,
|
||||
endpoint_anchored_offsets_rad={
|
||||
name: 0.01 for name in RIGHT_19_FLEXION_ENDPOINT_JOINTS
|
||||
name: offsets[name]
|
||||
for name in RIGHT_19_MECHANICAL_ENDPOINT_JOINTS
|
||||
},
|
||||
timestamp=stamp,
|
||||
)
|
||||
@@ -156,6 +176,18 @@ def _passed_node_status() -> dict:
|
||||
|
||||
def test_product_config_locks_three_cameras_tags_and_artifact_hashes() -> None:
|
||||
config = load_product_config(PRODUCT, workspace=REPO, check_can=False)
|
||||
assert (config.model, config.side, config.tag_layout) == (
|
||||
"G20",
|
||||
"right",
|
||||
G20_RIGHT_19_LAYOUT,
|
||||
)
|
||||
assert config.calibration_contract.profile.command_count == 20
|
||||
assert config.calibration_contract.profile.supports(
|
||||
"urdf_zero_publication"
|
||||
)
|
||||
assert config.calibration_contract.profile.supports(
|
||||
"stable_cross_view_cone_bias"
|
||||
)
|
||||
assert config.serial_number == "G20_RIGHT_001"
|
||||
assert config.required_independent_passes == 1
|
||||
assert set(config.cameras) == {"front", "side", "top"}
|
||||
@@ -266,6 +298,71 @@ def test_resume_reuses_only_a_fully_committed_task_prefix() -> None:
|
||||
assert {row["task_name"] for row in reusable} == {first.key}
|
||||
|
||||
|
||||
def test_resume_preserves_palm_axis_side_channel_without_requiring_it() -> None:
|
||||
profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT)
|
||||
spec = next(
|
||||
item for item in profile.sweep_specs if item.key == "pinky_pitch_side"
|
||||
)
|
||||
rows = _complete_resume_task_rows(profile, spec)
|
||||
rows.append(
|
||||
{
|
||||
"kind": "palm_axis_sample",
|
||||
"attempt": 1,
|
||||
"task_name": spec.key,
|
||||
"source_joint": "pinky_mcp_pitch_front_axis",
|
||||
"cycle": 0,
|
||||
"direction": "decreasing",
|
||||
"feedback_u8": 240,
|
||||
}
|
||||
)
|
||||
|
||||
completed, reusable = resumable_completed_task_prefix(
|
||||
profile,
|
||||
4,
|
||||
[255] * 20,
|
||||
rows,
|
||||
allow_sparse=True,
|
||||
)
|
||||
|
||||
assert completed == (spec.key,)
|
||||
assert sum(
|
||||
row.get("kind") == "palm_axis_sample" for row in reusable
|
||||
) == 1
|
||||
|
||||
completed_without_side_channel, _ = resumable_completed_task_prefix(
|
||||
profile,
|
||||
4,
|
||||
[255] * 20,
|
||||
_complete_resume_task_rows(profile, spec),
|
||||
allow_sparse=True,
|
||||
)
|
||||
assert completed_without_side_channel == (spec.key,)
|
||||
|
||||
|
||||
def test_checkpoint_needs_no_palm_axis_reacquisition_when_disabled() -> None:
|
||||
profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT)
|
||||
old_capabilities = set(profile.capabilities) - {
|
||||
"palm_axis_side_channel_v1"
|
||||
}
|
||||
|
||||
compatible, invalidated = _palm_axis_resume_policy(
|
||||
profile, {"capabilities": sorted(old_capabilities)}
|
||||
)
|
||||
|
||||
assert compatible is True
|
||||
assert invalidated == ()
|
||||
|
||||
compatible, invalidated = _palm_axis_resume_policy(
|
||||
profile,
|
||||
{
|
||||
"capabilities": sorted(profile.capabilities),
|
||||
"palm_axis_observers": _palm_axis_observer_schema(profile),
|
||||
},
|
||||
)
|
||||
assert compatible is True
|
||||
assert invalidated == ()
|
||||
|
||||
|
||||
def test_sparse_resume_keeps_complete_tasks_after_failed_task() -> None:
|
||||
profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT)
|
||||
first, failed, later = profile.sweep_specs[:3]
|
||||
@@ -590,6 +687,41 @@ def test_resume_revalidates_retired_passive_dip_position_metrics() -> None:
|
||||
assert _unresolved_fit_failure_tasks(profile, rows) == set()
|
||||
|
||||
|
||||
def test_resume_retires_validation_only_cross_view_curve_failure() -> None:
|
||||
profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT)
|
||||
spec = next(
|
||||
item for item in profile.sweep_specs
|
||||
if item.key == "ring_roll_multiview"
|
||||
)
|
||||
rows = [
|
||||
{
|
||||
"kind": "sample",
|
||||
"task_name": spec.key,
|
||||
"attempt": 3,
|
||||
},
|
||||
{
|
||||
"kind": "fit_failure",
|
||||
"task_name": spec.key,
|
||||
"view": spec.view,
|
||||
"motor_index": spec.motor_index,
|
||||
"joints": list(spec.joints),
|
||||
"attempt": 3,
|
||||
"failures": [
|
||||
{
|
||||
"joint": "ring_mcp_roll",
|
||||
"metric": "cross_view_roll_curve",
|
||||
"reason": (
|
||||
"cross_view_roll_curve_difference_too_large:"
|
||||
"angle_rad:1.297957deg"
|
||||
),
|
||||
}
|
||||
],
|
||||
},
|
||||
]
|
||||
|
||||
assert _unresolved_fit_failure_tasks(profile, rows) == set()
|
||||
|
||||
|
||||
def test_automatic_resume_requires_failed_matching_geometry(tmp_path: Path) -> None:
|
||||
config = _config(tmp_path)
|
||||
session = config.session_root / "20260819_150000"
|
||||
@@ -718,6 +850,9 @@ def test_node_restores_complete_prefix_into_new_self_contained_raw(
|
||||
},
|
||||
*_complete_resume_task_rows(profile, profile.sweep_specs[0]),
|
||||
]
|
||||
for row in rows:
|
||||
if row.get("task_name") == profile.sweep_specs[0].key:
|
||||
row["attempt"] = 2
|
||||
source_raw.write_text(
|
||||
"".join(json.dumps(row) + "\n" for row in rows)
|
||||
)
|
||||
@@ -760,10 +895,14 @@ def test_node_restores_complete_prefix_into_new_self_contained_raw(
|
||||
assert count == 1
|
||||
assert node.sweep_index == 10
|
||||
assert node.resumed_task_keys == (profile.sweep_specs[0].key,)
|
||||
assert node.sweep_attempts[profile.sweep_specs[0].key] == 2
|
||||
restored = [
|
||||
json.loads(line) for line in current_raw.read_text().splitlines()
|
||||
]
|
||||
assert restored[0]["kind"] == "resume_checkpoint_import"
|
||||
assert restored[0]["imported_attempt_floor_by_task"] == {
|
||||
profile.sweep_specs[0].key: 2
|
||||
}
|
||||
assert any(row["kind"] == "sample" for row in restored)
|
||||
|
||||
|
||||
@@ -839,10 +978,89 @@ def test_publication_numerically_binds_json_offsets_and_urdf_limits(
|
||||
payload = _payload(config.serial_number)
|
||||
validate_runtime_curves_against_urdf_limits(payload, config.source_urdf)
|
||||
payload["joints"]["thumb_mcp"]["angle_rad"][0] = 2.0
|
||||
with pytest.raises(ValueError, match="exceeds source URDF limit"):
|
||||
with pytest.raises(ValueError, match="exceeds runtime URDF limit"):
|
||||
validate_runtime_curves_against_urdf_limits(payload, config.source_urdf)
|
||||
|
||||
|
||||
def test_publication_uses_corrected_coordinates_for_negative_endpoint_zero(
|
||||
tmp_path: Path,
|
||||
) -> None:
|
||||
config = _config(tmp_path, passes=1)
|
||||
offsets = {name: 0.01 for name in ACTIVE_ZERO_JOINTS}
|
||||
endpoint_offset = -0.00801549
|
||||
offsets["index_mcp_pitch"] = endpoint_offset
|
||||
session = _make_passed_session(
|
||||
config,
|
||||
"20260826_102518",
|
||||
zero_offsets=offsets,
|
||||
)
|
||||
paths = session_artifact_paths(session, config.serial_number)
|
||||
payload = json.loads(paths["json"].read_text(encoding="utf-8"))
|
||||
curve_maximum = 1.22801310
|
||||
payload["joints"]["index_mcp_pitch"]["angle_rad"][0] = curve_maximum
|
||||
atomic_write_json(paths["json"], payload)
|
||||
|
||||
summary, release_ready = finalize_session_artifacts(
|
||||
config,
|
||||
session,
|
||||
node_status=_passed_node_status(),
|
||||
)
|
||||
|
||||
def upper_limit(path: Path) -> float:
|
||||
joint = next(
|
||||
node
|
||||
for node in ET.parse(path).getroot().findall("joint")
|
||||
if node.get("name") == "index_mcp_pitch"
|
||||
)
|
||||
return float(joint.find("limit").get("upper"))
|
||||
|
||||
source_upper = upper_limit(config.source_urdf)
|
||||
corrected_upper = upper_limit(paths["urdf"])
|
||||
assert release_ready is True
|
||||
assert summary["result"] == "PASS"
|
||||
assert corrected_upper == pytest.approx(source_upper - endpoint_offset)
|
||||
assert curve_maximum <= corrected_upper
|
||||
assert curve_maximum + endpoint_offset <= source_upper
|
||||
|
||||
|
||||
def test_publication_propagates_schema_v4_rounding_to_mimic_offset(
|
||||
tmp_path: Path,
|
||||
) -> None:
|
||||
config = _config(tmp_path, passes=1)
|
||||
offsets = {name: 0.01 for name in ACTIVE_ZERO_JOINTS}
|
||||
full_precision_offset = -0.06200179363832
|
||||
offsets["middle_pip"] = full_precision_offset
|
||||
session = _make_passed_session(
|
||||
config,
|
||||
"20260827_152432",
|
||||
zero_offsets=offsets,
|
||||
)
|
||||
paths = session_artifact_paths(session, config.serial_number)
|
||||
payload = json.loads(paths["json"].read_text(encoding="utf-8"))
|
||||
assert payload["joints"]["middle_pip"]["zero_angles"][
|
||||
"urdf_zero_offset_rad"
|
||||
] == -0.06200179
|
||||
|
||||
summary, release_ready = finalize_session_artifacts(
|
||||
config,
|
||||
session,
|
||||
node_status=_passed_node_status(),
|
||||
)
|
||||
|
||||
corrected = ET.parse(paths["urdf"]).getroot()
|
||||
middle_dip = next(
|
||||
node
|
||||
for node in corrected.findall("joint")
|
||||
if node.get("name") == "middle_dip"
|
||||
)
|
||||
assert float(middle_dip.find("mimic").get("offset")) == pytest.approx(
|
||||
0.89 * full_precision_offset,
|
||||
abs=1.0e-14,
|
||||
)
|
||||
assert release_ready is True
|
||||
assert summary["result"] == "PASS"
|
||||
|
||||
|
||||
def test_publication_rejects_non_joint_urdf_changes(tmp_path: Path) -> None:
|
||||
config = _config(tmp_path, passes=1)
|
||||
session = _make_passed_session(config, "20260819_112000")
|
||||
@@ -907,7 +1125,7 @@ def test_publication_accepts_half_lsb_schema_zero_quantisation(
|
||||
assert release_ready is True
|
||||
|
||||
|
||||
def test_publication_saturates_legacy_runtime_curve_before_release(
|
||||
def test_publication_keeps_cmc_runtime_limit_independent_of_certified_zero(
|
||||
tmp_path: Path,
|
||||
) -> None:
|
||||
config = _config(tmp_path, passes=1)
|
||||
@@ -923,6 +1141,9 @@ def test_publication_saturates_legacy_runtime_curve_before_release(
|
||||
published = json.loads(json_path.read_text(encoding="utf-8"))
|
||||
|
||||
assert release_ready is True
|
||||
# CMC has a certified origin, not an assertion that its electrical
|
||||
# endpoint equals the source-CAD upper coordinate. Its runtime coordinate
|
||||
# limit therefore remains the protected source value.
|
||||
assert published["joints"]["thumb_cmc_yaw"]["angle_rad"][0] == 1.57
|
||||
assert summary["runtime_limit_clipped_bins"]["thumb_cmc_yaw"] == 1
|
||||
|
||||
@@ -977,7 +1198,19 @@ def test_synchronised_timeout_with_visible_tags_reports_pnp_geometry(tmp_path: P
|
||||
"front": {
|
||||
"ready": True,
|
||||
"missing_tag_ids": [],
|
||||
"group_pnp_reason": "group_coupled_rotation",
|
||||
"group_pnp_reason": "group_missing_pose_candidates",
|
||||
"group_missing_candidate_roles": ["thumb_ip"],
|
||||
"pnp_rejection_counts": {
|
||||
"thumb_ip:no_pose_within_reprojection_or_tilt_limit": 12,
|
||||
},
|
||||
"pnp_candidate_diagnostics": {
|
||||
"thumb_ip": {
|
||||
"tag_id": 3,
|
||||
"solved_candidate_count": 2,
|
||||
"reprojection_candidate_count": 0,
|
||||
"independent_tilt_candidate_count": 0,
|
||||
}
|
||||
},
|
||||
},
|
||||
"side": {"ready": True, "missing_tag_ids": []},
|
||||
"top": {"ready": True, "missing_tag_ids": []},
|
||||
@@ -989,7 +1222,12 @@ def test_synchronised_timeout_with_visible_tags_reports_pnp_geometry(tmp_path: P
|
||||
)
|
||||
|
||||
assert payload["error_code"] == "CAM-GEOMETRY-201"
|
||||
assert "轨迹在运动中失效" in payload["problem_zh"]
|
||||
assert "整组PnP几何检查拒绝" in payload["explanation_zh"]
|
||||
assert "缺候选=thumb_ip" in payload["explanation_zh"]
|
||||
assert payload["pnp_diagnostics"]["front"][
|
||||
"group_missing_candidate_roles"
|
||||
] == ["thumb_ip"]
|
||||
assert "不要调整" in payload["automatic_action_zh"]
|
||||
|
||||
|
||||
@@ -1007,7 +1245,14 @@ def test_sweep_start_timeout_with_visible_tags_reports_pnp_geometry(tmp_path: Pa
|
||||
"side": {
|
||||
"ready": True,
|
||||
"missing_tag_ids": [],
|
||||
"group_pnp_reason": "group_normal_alignment",
|
||||
"group_pnp_reason": "group_initializing:7/8",
|
||||
"pnp_initialization_progress": {
|
||||
"accepted": 7,
|
||||
"required": 8,
|
||||
},
|
||||
"group_pnp_rejection_counts": {
|
||||
"group_normal_alignment": 5,
|
||||
},
|
||||
},
|
||||
"top": {"ready": True, "missing_tag_ids": []},
|
||||
},
|
||||
@@ -1019,6 +1264,8 @@ def test_sweep_start_timeout_with_visible_tags_reports_pnp_geometry(tmp_path: Pa
|
||||
|
||||
assert payload["error_code"] == "CAM-GEOMETRY-201"
|
||||
assert "PnP" in payload["problem_zh"]
|
||||
assert "初始化7/8" in payload["explanation_zh"]
|
||||
assert "group_normal_alignment×5" in payload["explanation_zh"]
|
||||
|
||||
|
||||
def test_multiview_sample_failure_has_stable_observation_code() -> None:
|
||||
@@ -1030,6 +1277,33 @@ def test_multiview_sample_failure_has_stable_observation_code() -> None:
|
||||
assert "轨迹不完整" in problem
|
||||
|
||||
|
||||
def test_palm_orientation_coverage_has_validation_error_code() -> None:
|
||||
code, problem, suggestion = classify_error(
|
||||
"palm_orientation_quality_failed", {}
|
||||
)
|
||||
|
||||
assert code == "VAL-QUALITY-501"
|
||||
assert "方向校正" in problem
|
||||
assert "至少三枚" in suggestion
|
||||
|
||||
|
||||
@pytest.mark.parametrize(
|
||||
"reason",
|
||||
(
|
||||
"name 'endpoint_zero_offsets' is not defined",
|
||||
"validated_endpoint_zero_state_incomplete:missing=index_pip;extra=-",
|
||||
),
|
||||
)
|
||||
def test_endpoint_zero_lifecycle_failure_is_a_publication_error(
|
||||
reason: str,
|
||||
) -> None:
|
||||
code, problem, suggestion = classify_error(reason, {})
|
||||
|
||||
assert code == "PUB-ARTIFACT-601"
|
||||
assert "发布阶段" in problem
|
||||
assert "不要移动相机或Tag" in suggestion
|
||||
|
||||
|
||||
def test_fitting_uses_long_watchdog_without_weakening_motion_timeout() -> None:
|
||||
assert _status_timeout_seconds({"state": "FITTING"}) == 600.0
|
||||
assert _status_timeout_seconds({"state": "SWEEP"}) == 90.0
|
||||
@@ -1114,8 +1388,9 @@ def test_progress_contains_stage_eta_tags_cameras_and_feedback() -> None:
|
||||
"actual_u8": 116.5,
|
||||
"valid_frames": 80,
|
||||
"automatic_retry_count": 1,
|
||||
"fit_attempt": 3,
|
||||
"fit_attempt_limit": 3,
|
||||
"fit_attempt": 3,
|
||||
"fit_attempt_limit": 3,
|
||||
"fit_retry_cycles": [2],
|
||||
},
|
||||
"views": {
|
||||
view: {
|
||||
@@ -1136,7 +1411,7 @@ def test_progress_contains_stage_eta_tags_cameras_and_feedback() -> None:
|
||||
assert "反馈：98.0 Hz" in text
|
||||
assert "当前任务ID：正面[0✓] 侧面[0✓] 顶部[0✓]" in text
|
||||
assert "断点：已恢复 3/16 个完整任务" in text
|
||||
assert "拟合：整关节第 3/3 次尝试" in text
|
||||
assert "首次拟合定位第 2 轮异常，仅补采该轮双向" in text
|
||||
|
||||
|
||||
def test_preflight_progress_marks_non_base_tag_occlusion_as_allowed() -> None:
|
||||
@@ -1218,7 +1493,7 @@ def test_sweep_progress_lists_visible_and_missing_task_tag_ids() -> None:
|
||||
"G20_RIGHT_001", status, ProgressEstimator(started_at=0.0)
|
||||
)
|
||||
|
||||
assert "Tag：4/5 有效" in text
|
||||
assert "Tag：4/5 可见" in text
|
||||
assert "正面[0✓]" in text
|
||||
assert "侧面[4✓,5✗,14✓]" in text
|
||||
assert "顶部[8✓]" in text
|
||||
@@ -1254,6 +1529,14 @@ def test_multiview_progress_marks_occluded_front_base_as_locked() -> None:
|
||||
"required_tag_ids": [4, 15],
|
||||
"locked_reference_tag_ids": [],
|
||||
"detected_tag_ids": [4, 15],
|
||||
"pnp_pose_valid": False,
|
||||
"pnp_initialization_progress": {
|
||||
"accepted": 5,
|
||||
"required": 8,
|
||||
},
|
||||
"pnp_rejection_counts": {
|
||||
"middle_pip:no_pose_within_reprojection_or_tilt_limit": 7,
|
||||
},
|
||||
},
|
||||
"top": {
|
||||
"camera_info_valid": True,
|
||||
@@ -1271,9 +1554,11 @@ def test_multiview_progress_marks_occluded_front_base_as_locked() -> None:
|
||||
|
||||
assert "已到扫描起点，等待任务Tag" in text
|
||||
assert "命令/反馈：255/254.0" in text
|
||||
assert "Tag：5/5 有效（含锁定 1）" in text
|
||||
assert "Tag：5/5 可见（含锁定 1）" in text
|
||||
assert "正面[0锁,12✓]" in text
|
||||
assert "预计剩余 等待Tag" in text
|
||||
assert "PnP：侧面 初始化5/8" in text
|
||||
assert "Tag可见不等于三维位姿有效" in text
|
||||
|
||||
|
||||
def test_device_preflight_defers_tag_gate_until_after_baseline_recovery() -> None:
|
||||
|
||||
@@ -2,6 +2,7 @@ import pytest
|
||||
|
||||
from g20_thumb_apriltag_calibration.offline_replay import (
|
||||
_latest_attempt_records,
|
||||
_latest_palm_axis_records,
|
||||
_output_suffix,
|
||||
)
|
||||
|
||||
@@ -36,6 +37,30 @@ def test_latest_attempt_is_selected_per_joint_cycle_and_direction() -> None:
|
||||
] == [1]
|
||||
|
||||
|
||||
def test_latest_palm_axis_attempt_is_selected_per_cycle_and_direction() -> None:
|
||||
def sample(cycle: int, direction: str, attempt: int) -> dict:
|
||||
return {
|
||||
"kind": "palm_axis_sample",
|
||||
"source_joint": "pinky_mcp_pitch_front_axis",
|
||||
"cycle": cycle,
|
||||
"direction": direction,
|
||||
"attempt": attempt,
|
||||
}
|
||||
|
||||
selected = _latest_palm_axis_records(
|
||||
[
|
||||
sample(0, "decreasing", 1),
|
||||
sample(0, "decreasing", 2),
|
||||
sample(0, "increasing", 1),
|
||||
]
|
||||
)
|
||||
|
||||
assert [
|
||||
record["attempt"]
|
||||
for record in selected["pinky_mcp_pitch_front_axis"]
|
||||
] == [2, 1]
|
||||
|
||||
|
||||
def test_output_suffix_is_safe_and_explicit() -> None:
|
||||
assert _output_suffix(None) == ""
|
||||
assert _output_suffix("MEASURED_ZERO_V2") == "_MEASURED_ZERO_V2"
|
||||
|
||||
@@ -234,6 +234,83 @@ def _pose(
|
||||
)
|
||||
|
||||
|
||||
def test_group_tracker_receives_oblique_reprojection_valid_candidates() -> None:
|
||||
per_tag = SquareTagPoseTracker(
|
||||
maximum_reprojection_error_px=1.5,
|
||||
reprojection_tie_px=1.5,
|
||||
maximum_pose_jump_rad=np.deg2rad(35.0),
|
||||
maximum_translation_jump_m=0.04,
|
||||
maximum_tag_tilt_rad=np.deg2rad(75.0),
|
||||
reset_after_seconds=5.0,
|
||||
)
|
||||
oblique_rotation = Rotation.from_euler("y", 89.0, degrees=True)
|
||||
|
||||
independent, reason = per_tag.estimate(
|
||||
"child",
|
||||
_project(oblique_rotation, np.asarray([0.03, 0.0, 0.25])),
|
||||
tag_size_m=0.01,
|
||||
camera_matrix=_camera_matrix(),
|
||||
stamp_ns=1_000_000_000,
|
||||
)
|
||||
|
||||
assert independent is None
|
||||
assert reason == "no_pose_within_reprojection_or_tilt_limit"
|
||||
assert per_tag.last_candidates_by_role["child"]
|
||||
diagnostics = per_tag.last_candidate_diagnostics_by_role["child"]
|
||||
assert diagnostics["reprojection_candidate_count"] == 2
|
||||
assert diagnostics["independent_tilt_candidate_count"] == 0
|
||||
assert diagnostics["minimum_candidate_tilt_deg"] > 75.0
|
||||
|
||||
group = SquareTagGroupPoseTracker(
|
||||
roles=("base", "child"),
|
||||
adjacent_pairs=(("base", "child"),),
|
||||
maximum_pose_jump_rad=np.deg2rad(35.0),
|
||||
maximum_translation_jump_m=0.04,
|
||||
relative_rotation_scale_rad=np.deg2rad(5.0),
|
||||
relative_translation_scale_m=0.01,
|
||||
reprojection_scale_px=0.1,
|
||||
reprojection_weight=0.05,
|
||||
reset_after_seconds=5.0,
|
||||
)
|
||||
selected, group_reason = group.select(
|
||||
{
|
||||
"base": (_pose(0.0, 0.0, 0.05),),
|
||||
"child": per_tag.last_candidates_by_role["child"],
|
||||
},
|
||||
stamp_ns=1_000_000_000,
|
||||
)
|
||||
|
||||
assert group_reason == ""
|
||||
assert selected is not None
|
||||
assert "child" in selected
|
||||
|
||||
|
||||
def test_group_tracker_exposes_roles_without_candidates() -> None:
|
||||
tracker = SquareTagGroupPoseTracker(
|
||||
roles=("base", "parent", "child"),
|
||||
adjacent_pairs=(("base", "parent"), ("parent", "child")),
|
||||
maximum_pose_jump_rad=np.deg2rad(35.0),
|
||||
maximum_translation_jump_m=0.04,
|
||||
relative_rotation_scale_rad=np.deg2rad(5.0),
|
||||
relative_translation_scale_m=0.01,
|
||||
reprojection_scale_px=0.1,
|
||||
reprojection_weight=0.05,
|
||||
reset_after_seconds=5.0,
|
||||
)
|
||||
|
||||
selected, reason = tracker.select(
|
||||
{
|
||||
"base": (_pose(0.0, 0.0, 0.05),),
|
||||
"parent": (),
|
||||
},
|
||||
stamp_ns=1_000_000_000,
|
||||
)
|
||||
|
||||
assert selected is None
|
||||
assert reason == "group_missing_pose_candidates"
|
||||
assert tracker.last_missing_roles == ("parent", "child")
|
||||
|
||||
|
||||
def test_group_tracker_uses_coupling_to_choose_branch_but_never_rejects_measurement() -> None:
|
||||
tracker = SquareTagGroupPoseTracker(
|
||||
roles=("base", "mcp", "ip"),
|
||||
|
||||
@@ -74,6 +74,47 @@ def test_missing_endpoint_has_specific_chinese_reason_and_recovery() -> None:
|
||||
assert "/g20_calibration/resume" in text
|
||||
|
||||
|
||||
def test_endpoint_zero_lifecycle_failure_is_not_reported_as_quality() -> None:
|
||||
payload = {
|
||||
"state": "PAUSED",
|
||||
"reason": (
|
||||
"validated_endpoint_zero_state_incomplete:"
|
||||
"missing=index_pip;extra=-"
|
||||
),
|
||||
"active": {},
|
||||
"progress": 1.0,
|
||||
"completed_sweeps": 16,
|
||||
"total_sweeps": 16,
|
||||
"views": {},
|
||||
}
|
||||
|
||||
text = render_three_camera_status_text_zh(payload)
|
||||
|
||||
assert "程序内部状态生命周期错误" in text
|
||||
assert "不要移动相机、Tag或机械手底座" in text
|
||||
|
||||
|
||||
def test_artifact_failure_explains_that_collection_need_not_restart() -> None:
|
||||
payload = {
|
||||
"state": "PAUSED",
|
||||
"reason": (
|
||||
"PUB-ARTIFACT-601:runtime curve exceeds runtime URDF limit "
|
||||
"for index_mcp_pitch"
|
||||
),
|
||||
"active": {},
|
||||
"progress": 1.0,
|
||||
"completed_sweeps": 16,
|
||||
"total_sweeps": 16,
|
||||
"views": {},
|
||||
}
|
||||
|
||||
text = render_three_camera_status_text_zh(payload)
|
||||
|
||||
assert "正式发布前" in text
|
||||
assert "原始URDF未被覆盖" in text
|
||||
assert "不要重新标定相机或调整Tag" in text
|
||||
|
||||
|
||||
def test_combination_prediction_failure_is_not_reported_as_unclassified() -> None:
|
||||
explanation, action = three_camera_reason_zh(
|
||||
"PAUSED", "combination_pose_prediction_failed", {}
|
||||
@@ -91,14 +132,58 @@ def test_sweep_start_visible_tags_reports_group_pnp_rejection() -> None:
|
||||
"sweep_start_tag_timeout",
|
||||
{
|
||||
"group_pnp_reasons": {
|
||||
"side": "group_normal_alignment",
|
||||
}
|
||||
"side": "group_initializing:5/8",
|
||||
},
|
||||
"pnp_initialization_progress": {
|
||||
"side": {"accepted": 5, "required": 8},
|
||||
},
|
||||
"pnp_rejection_counts": {
|
||||
"side": {
|
||||
"ring_pip:no_pose_within_reprojection_or_tilt_limit": 12,
|
||||
},
|
||||
},
|
||||
},
|
||||
)
|
||||
|
||||
assert "所需Tag也可见" in explanation
|
||||
assert "group_normal_alignment" in explanation
|
||||
assert "不要重新粘贴" in action
|
||||
assert "初始化5/8" in explanation
|
||||
assert "no_pose_within_reprojection_or_tilt_limit×12" in explanation
|
||||
assert "不要根据可见性重复粘贴" in action
|
||||
|
||||
|
||||
def test_mid_sweep_pnp_failure_names_missing_candidate_role() -> None:
|
||||
explanation, action = three_camera_reason_zh(
|
||||
"PAUSED",
|
||||
"synchronised_tag_state_timeout:side",
|
||||
{
|
||||
"valid_frames": 150,
|
||||
"group_pnp_reasons": {
|
||||
"side": "group_missing_pose_candidates",
|
||||
},
|
||||
"group_missing_candidate_roles": {
|
||||
"side": ["index_dip"],
|
||||
},
|
||||
"pnp_rejection_counts": {
|
||||
"side": {
|
||||
"index_dip:no_pose_within_reprojection_or_tilt_limit": 90,
|
||||
},
|
||||
},
|
||||
"pnp_candidate_diagnostics": {
|
||||
"side": {
|
||||
"index_dip": {
|
||||
"solved_candidate_count": 2,
|
||||
"reprojection_candidate_count": 0,
|
||||
"independent_tilt_candidate_count": 0,
|
||||
},
|
||||
},
|
||||
},
|
||||
},
|
||||
)
|
||||
|
||||
assert "已经取得部分有效轨迹" in explanation
|
||||
assert "缺候选=index_dip" in explanation
|
||||
assert "solve=2,reproj=0,tilt=0" in explanation
|
||||
assert "group_pnp_candidate_event" in action
|
||||
|
||||
|
||||
def test_multiview_failure_names_the_camera_specific_joint() -> None:
|
||||
@@ -183,6 +268,60 @@ def test_joint_fit_failure_names_metric_and_selective_retry() -> None:
|
||||
assert "运动采样：" not in text
|
||||
|
||||
|
||||
def test_palm_orientation_failure_explains_direction_coverage() -> None:
|
||||
explanation, suggestion = three_camera_reason_zh(
|
||||
"PAUSED",
|
||||
"palm_orientation_quality_failed",
|
||||
{
|
||||
"failures": [
|
||||
{
|
||||
"reason": "palm orientation cycle 2 has 2/3 usable sources"
|
||||
}
|
||||
]
|
||||
},
|
||||
)
|
||||
|
||||
assert "至少三根手指" in explanation
|
||||
assert "2/3 usable sources" in explanation
|
||||
assert "Tag 10–13" in suggestion
|
||||
|
||||
|
||||
def test_cross_view_side_line_rms_names_side_source() -> None:
|
||||
payload = {
|
||||
"state": "PAUSED",
|
||||
"reason": "joint_fit_check_failed",
|
||||
"progress": 0.1,
|
||||
"completed_sweeps": 8,
|
||||
"total_sweeps": 80,
|
||||
"active": {
|
||||
"kind": "fit_failure",
|
||||
"view": "front",
|
||||
"motor_index": 9,
|
||||
"joints": ["pinky_mcp_roll", "pinky_mcp_roll_side"],
|
||||
"attempt": 1,
|
||||
"directions_to_rescan": 8,
|
||||
"failures": [
|
||||
{
|
||||
"joint": "pinky_mcp_roll_side",
|
||||
"model_joint": "pinky_mcp_roll",
|
||||
"quality_source_joints": ["pinky_mcp_roll_side"],
|
||||
"metric": "axis_line_cycle_rms_mm",
|
||||
"actual": 1.2,
|
||||
"limit": 1.0,
|
||||
"comparison": "maximum",
|
||||
}
|
||||
],
|
||||
},
|
||||
"views": {},
|
||||
"result_path": "",
|
||||
}
|
||||
|
||||
text = render_three_camera_status_text_zh(payload)
|
||||
|
||||
assert "小指MCP侧摆（侧面校验）的四轮轴线位置RMS为1.20mm" in text
|
||||
assert "要求不超过1.00mm" in text
|
||||
|
||||
|
||||
def test_baseline_hysteresis_failure_shows_values_instead_of_unknown() -> None:
|
||||
payload = {
|
||||
"state": "PAUSED",
|
||||
@@ -297,7 +436,7 @@ def test_zero_model_failure_explains_that_rescan_will_not_help() -> None:
|
||||
assert "运动采样：" not in text
|
||||
|
||||
|
||||
def test_sweep_status_shows_full_joint_fit_retry_attempt() -> None:
|
||||
def test_sweep_status_shows_localized_fit_retry_cycle() -> None:
|
||||
active = {
|
||||
"kind": "sweep",
|
||||
"view": "front",
|
||||
@@ -309,9 +448,10 @@ def test_sweep_status_shows_full_joint_fit_retry_attempt() -> None:
|
||||
"target_u8": 0,
|
||||
"fit_attempt": 2,
|
||||
"fit_attempt_limit": 3,
|
||||
"fit_retry_cycles": [2],
|
||||
}
|
||||
|
||||
assert "整关节自动重采第2/3次" in _task_text(active)
|
||||
assert "补采异常轮2" in _task_text(active)
|
||||
|
||||
|
||||
def test_motor_stall_reason_is_explained_in_chinese() -> None:
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -16,13 +16,18 @@ from g20_thumb_apriltag_calibration.urdf_zero import (
|
||||
DIRECT_ZERO_JOINTS,
|
||||
INHERITED_ZERO_JOINTS,
|
||||
JointAxisMeasurement,
|
||||
PalmOrientationMeasurement,
|
||||
UrdfKinematicModel,
|
||||
_angles_from_state,
|
||||
_zero_sensitive_axis_error_rad,
|
||||
anchor_right_19_mechanical_endpoint_curves,
|
||||
baseline_hysteresis_by_cycle_rad,
|
||||
derive_right_19_flexion_endpoint_offsets,
|
||||
derive_right_19_mechanical_endpoint_offsets,
|
||||
fit_joint_axis_measurement,
|
||||
fit_partial_palm_orientation_measurement,
|
||||
fit_partial_palm_orientation_measurements,
|
||||
fit_rotation_joint_curve,
|
||||
select_cross_view_roll_direction_source,
|
||||
solve_urdf_zero_offsets,
|
||||
get_zero_calibration_profile,
|
||||
write_zero_corrected_urdf,
|
||||
@@ -49,6 +54,30 @@ RIGHT_SOURCE_URDF = REPOSITORY / (
|
||||
)
|
||||
|
||||
|
||||
def test_cross_view_roll_direction_is_selected_once_by_group_consensus() -> None:
|
||||
# Session 20260824_171302: the side residual in cycle 3 was 5.037 deg,
|
||||
# just outside the 5 deg cone gate, while the other three side cycles
|
||||
# passed and every front cycle was near 8 deg. Per-cycle selection mixed
|
||||
# three side axes with one front axis and fabricated a 3.24 deg spread.
|
||||
source = select_cross_view_roll_direction_source(
|
||||
[math.radians(value) for value in (8.0078, 8.1710, 8.0712, 8.0236)],
|
||||
[math.radians(value) for value in (4.9043, 4.9136, 5.0374, 4.9389)],
|
||||
math.radians(5.0),
|
||||
)
|
||||
|
||||
assert source == "secondary"
|
||||
|
||||
|
||||
def test_cross_view_roll_direction_keeps_primary_without_consensus() -> None:
|
||||
source = select_cross_view_roll_direction_source(
|
||||
[math.radians(value) for value in (5.2, 4.8, 5.1, 4.9)],
|
||||
[math.radians(value) for value in (4.7, 5.3, 4.8, 5.2)],
|
||||
math.radians(5.0),
|
||||
)
|
||||
|
||||
assert source == "primary"
|
||||
|
||||
|
||||
def test_zero_sensitive_axis_error_ignores_fixed_cone_angle_mismatch():
|
||||
parent = np.asarray([0.0, 0.0, 1.0])
|
||||
predicted = np.asarray([1.0, 0.0, 0.0])
|
||||
@@ -588,9 +617,12 @@ def _solve_synthetic_offsets(
|
||||
inject_secondary_root_axis_bias_degrees: float = 0.0,
|
||||
inject_secondary_root_point_bias_m: float = 0.0,
|
||||
inject_observer_cone_bias_degrees: float = 0.0,
|
||||
maximum_systematic_axis_cone_bias_degrees: float | None = None,
|
||||
pose_axis_line_rms_by_joint_m: dict[str, float] | None = None,
|
||||
joint_maximum_offset_degrees: dict[str, float] | None = None,
|
||||
validation_offset_bias_degrees: dict[str, float] | None = None,
|
||||
base_euler_xyz_rad: tuple[float, float, float] = (0.5, -0.4, 0.8),
|
||||
base_translation_xyz_m: tuple[float, float, float] = (0.31, -0.19, 0.72),
|
||||
):
|
||||
hand = get_hand_calibration_profile(side, layout_id)
|
||||
zero = get_zero_calibration_profile(side, layout_id)
|
||||
@@ -620,12 +652,23 @@ def _solve_synthetic_offsets(
|
||||
for name, value in zip(zero.direct_zero_joints, offset_degrees)
|
||||
}
|
||||
model = UrdfKinematicModel(source)
|
||||
base_rotation = Rotation.from_euler("xyz", [0.5, -0.4, 0.8])
|
||||
base_translation = np.asarray([0.31, -0.19, 0.72])
|
||||
base_rotation = Rotation.from_euler("xyz", base_euler_xyz_rad)
|
||||
base_translation = np.asarray(base_translation_xyz_m)
|
||||
measurements: list[JointAxisMeasurement] = []
|
||||
palm_orientation_measurements: list[PalmOrientationMeasurement] = []
|
||||
validation_cycle = 3 if hand.layout_id == "g20_right_19" else 2
|
||||
training_cycles = tuple(range(validation_cycle))
|
||||
for cycle in range(validation_cycle + 1):
|
||||
cycle_offsets = dict(offsets)
|
||||
if cycle == validation_cycle:
|
||||
cycle_offsets.update(
|
||||
{
|
||||
name: cycle_offsets[name] + math.radians(value)
|
||||
for name, value in (
|
||||
validation_offset_bias_degrees or {}
|
||||
).items()
|
||||
}
|
||||
)
|
||||
for joint in zero.axis_joints:
|
||||
state = list(baseline)
|
||||
if joint == "thumb_cmc_yaw":
|
||||
@@ -636,16 +679,6 @@ def _solve_synthetic_offsets(
|
||||
motor_by_joint=motor_by_joint,
|
||||
inherited_zero_joints=zero.inherited_zero_joints,
|
||||
)
|
||||
cycle_offsets = dict(offsets)
|
||||
if cycle == validation_cycle:
|
||||
cycle_offsets.update(
|
||||
{
|
||||
name: cycle_offsets[name] + math.radians(value)
|
||||
for name, value in (
|
||||
validation_offset_bias_degrees or {}
|
||||
).items()
|
||||
}
|
||||
)
|
||||
axis, point = model.axis_line(
|
||||
joint, zero_offsets=cycle_offsets, joint_angles=angles
|
||||
)
|
||||
@@ -734,9 +767,36 @@ def _solve_synthetic_offsets(
|
||||
).get(joint, 0.0),
|
||||
)
|
||||
)
|
||||
for source_joint, model_joint in (
|
||||
hand.palm_orientation_sources or {}
|
||||
).items():
|
||||
state = list(baseline)
|
||||
angles = _angles_from_state(
|
||||
state,
|
||||
curves=curves,
|
||||
motor_by_joint=motor_by_joint,
|
||||
inherited_zero_joints=zero.inherited_zero_joints,
|
||||
)
|
||||
axis, _ = model.axis_line(
|
||||
model_joint,
|
||||
zero_offsets=cycle_offsets,
|
||||
joint_angles=angles,
|
||||
)
|
||||
palm_orientation_measurements.append(
|
||||
PalmOrientationMeasurement(
|
||||
source_joint=source_joint,
|
||||
model_joint=model_joint,
|
||||
cycle=cycle,
|
||||
axis_common_xyz=tuple(base_rotation.apply(axis)),
|
||||
condition_state_u8=tuple(state),
|
||||
observed_arc_rad=math.radians(45.0),
|
||||
rotation_orthogonal_rms_rad=math.radians(0.1),
|
||||
)
|
||||
)
|
||||
result = solve_urdf_zero_offsets(
|
||||
source_urdf=source,
|
||||
measurements=measurements,
|
||||
palm_orientation_measurements=palm_orientation_measurements,
|
||||
curves=curves,
|
||||
motor_by_joint=motor_by_joint,
|
||||
hand_type=side,
|
||||
@@ -747,6 +807,13 @@ def _solve_synthetic_offsets(
|
||||
},
|
||||
training_cycles=training_cycles,
|
||||
validation_cycle=validation_cycle,
|
||||
maximum_systematic_axis_cone_bias_rad=(
|
||||
None
|
||||
if maximum_systematic_axis_cone_bias_degrees is None
|
||||
else math.radians(
|
||||
maximum_systematic_axis_cone_bias_degrees
|
||||
)
|
||||
),
|
||||
)
|
||||
return zero, result
|
||||
|
||||
@@ -788,9 +855,9 @@ def test_right_19_solver_recovers_visual_targets_when_no_endpoint_anchor_is_supp
|
||||
expected, abs=0.05
|
||||
)
|
||||
assert math.degrees(result.all_active_offsets_rad["thumb_mcp"]) == pytest.approx(
|
||||
0.0, abs=0.05
|
||||
-1.5, abs=0.05
|
||||
)
|
||||
assert zero.fixed_direct_zero_offsets_rad == {"thumb_mcp": 0.0}
|
||||
assert zero.fixed_direct_zero_offsets_rad == {}
|
||||
assert result.training_cycles == (0, 1, 2)
|
||||
assert result.validation_cycle == 3
|
||||
assert all(
|
||||
@@ -803,7 +870,203 @@ def test_right_19_solver_recovers_visual_targets_when_no_endpoint_anchor_is_supp
|
||||
assert set(result.offset_covariance_rad2) == set(zero.direct_zero_joints)
|
||||
|
||||
|
||||
def test_right_19_flexion_zero_is_derived_from_measured_contact_endpoint() -> None:
|
||||
def _partial_orientation_records(
|
||||
*,
|
||||
tag_mount: Rotation,
|
||||
common_rotation: Rotation,
|
||||
maximum_angle_deg: float = 30.0,
|
||||
tag_offset_xyz_m: tuple[float, float, float] = (0.01, 0.02, -0.015),
|
||||
) -> list[dict[str, object]]:
|
||||
axis_parent = np.asarray([0.0, 1.0, 0.0])
|
||||
parent_pose = {
|
||||
"translation_xyz_m": [0.2, -0.1, 0.7],
|
||||
"quaternion_xyzw": list(common_rotation.as_quat()),
|
||||
}
|
||||
records: list[dict[str, object]] = []
|
||||
commands = tuple(range(255, 174, -4))
|
||||
for direction, ordered in (
|
||||
("decreasing", commands),
|
||||
("increasing", tuple(reversed(commands))),
|
||||
):
|
||||
for command in ordered:
|
||||
fraction = (255 - command) / (255 - commands[-1])
|
||||
angle = math.radians(maximum_angle_deg) * fraction
|
||||
relative = (
|
||||
Rotation.from_rotvec(axis_parent * angle) * tag_mount
|
||||
)
|
||||
relative_translation = Rotation.from_rotvec(
|
||||
axis_parent * angle
|
||||
).apply(tag_offset_xyz_m)
|
||||
state = [255.0] * 20
|
||||
state[1] = float(command)
|
||||
records.append(
|
||||
{
|
||||
"cycle": 0,
|
||||
"direction": direction,
|
||||
"command_u8": command,
|
||||
"relative_quaternion_xyzw": list(relative.as_quat()),
|
||||
"relative_translation_xyz_m": list(
|
||||
relative_translation
|
||||
),
|
||||
"parent_pose_common": parent_pose,
|
||||
"state_u8": state,
|
||||
}
|
||||
)
|
||||
return records
|
||||
|
||||
|
||||
def test_partial_palm_direction_ignores_fixed_tag_mount_pose() -> None:
|
||||
common_rotation = Rotation.from_euler("xyz", [0.35, -0.2, 0.6])
|
||||
first = fit_partial_palm_orientation_measurement(
|
||||
"index_mcp_pitch_front_axis",
|
||||
"index_mcp_pitch",
|
||||
_partial_orientation_records(
|
||||
tag_mount=Rotation.from_euler("xyz", [0.1, 0.2, -0.4]),
|
||||
common_rotation=common_rotation,
|
||||
),
|
||||
cycle=0,
|
||||
zero_command_u8=255,
|
||||
)
|
||||
second = fit_partial_palm_orientation_measurement(
|
||||
"index_mcp_pitch_front_axis",
|
||||
"index_mcp_pitch",
|
||||
_partial_orientation_records(
|
||||
tag_mount=Rotation.from_euler("xyz", [-0.7, 0.45, 0.9]),
|
||||
common_rotation=common_rotation,
|
||||
tag_offset_xyz_m=(-0.035, 0.008, 0.041),
|
||||
),
|
||||
cycle=0,
|
||||
zero_command_u8=255,
|
||||
)
|
||||
expected = common_rotation.apply([0.0, 1.0, 0.0])
|
||||
|
||||
assert abs(np.dot(first.axis_common_xyz, expected)) == pytest.approx(
|
||||
1.0, abs=1.0e-8
|
||||
)
|
||||
assert abs(
|
||||
np.dot(first.axis_common_xyz, second.axis_common_xyz)
|
||||
) == pytest.approx(1.0, abs=1.0e-8)
|
||||
|
||||
|
||||
def test_partial_palm_direction_is_not_weighted_by_dwell_frame_count() -> None:
|
||||
records = _partial_orientation_records(
|
||||
tag_mount=Rotation.from_euler("xyz", [0.2, -0.1, 0.3]),
|
||||
common_rotation=Rotation.identity(),
|
||||
)
|
||||
baseline = fit_partial_palm_orientation_measurement(
|
||||
"index_mcp_pitch_front_axis",
|
||||
"index_mcp_pitch",
|
||||
records,
|
||||
cycle=0,
|
||||
zero_command_u8=255,
|
||||
)
|
||||
source = next(
|
||||
record
|
||||
for record in records
|
||||
if record["direction"] == "decreasing"
|
||||
and record["command_u8"] == 223
|
||||
)
|
||||
biased = dict(source)
|
||||
biased["relative_quaternion_xyzw"] = list(
|
||||
(
|
||||
Rotation.from_rotvec([math.radians(2.0), 0.0, 0.0])
|
||||
* Rotation.from_quat(source["relative_quaternion_xyzw"])
|
||||
).as_quat()
|
||||
)
|
||||
fitted = fit_partial_palm_orientation_measurement(
|
||||
"index_mcp_pitch_front_axis",
|
||||
"index_mcp_pitch",
|
||||
[*records, *([biased] * 200)],
|
||||
cycle=0,
|
||||
zero_command_u8=255,
|
||||
)
|
||||
|
||||
difference = math.acos(
|
||||
abs(
|
||||
float(
|
||||
np.clip(
|
||||
np.dot(baseline.axis_common_xyz, fitted.axis_common_xyz),
|
||||
-1.0,
|
||||
1.0,
|
||||
)
|
||||
)
|
||||
)
|
||||
)
|
||||
assert difference < math.radians(0.25)
|
||||
|
||||
|
||||
def test_partial_palm_direction_rejects_too_short_visible_arc() -> None:
|
||||
records = _partial_orientation_records(
|
||||
tag_mount=Rotation.identity(),
|
||||
common_rotation=Rotation.identity(),
|
||||
maximum_angle_deg=5.0,
|
||||
)
|
||||
|
||||
with pytest.raises(ValueError, match="visible rotation arc"):
|
||||
fit_partial_palm_orientation_measurement(
|
||||
"index_mcp_pitch_front_axis",
|
||||
"index_mcp_pitch",
|
||||
records,
|
||||
cycle=0,
|
||||
zero_command_u8=255,
|
||||
)
|
||||
|
||||
|
||||
def test_partial_palm_direction_uses_three_of_four_visible_sources() -> None:
|
||||
sources = {
|
||||
f"{finger}_mcp_pitch_front_axis": f"{finger}_mcp_pitch"
|
||||
for finger in ("index", "middle", "ring", "pinky")
|
||||
}
|
||||
records = {
|
||||
source: _partial_orientation_records(
|
||||
tag_mount=Rotation.from_euler(
|
||||
"xyz", [0.1 * index, -0.2, 0.3]
|
||||
),
|
||||
common_rotation=Rotation.identity(),
|
||||
maximum_angle_deg=(5.0 if index == 3 else 30.0),
|
||||
)
|
||||
for index, source in enumerate(sources)
|
||||
}
|
||||
|
||||
fitted, rejected = fit_partial_palm_orientation_measurements(
|
||||
sources=sources,
|
||||
records_by_joint=records,
|
||||
motor_by_source={source: index + 1 for index, source in enumerate(sources)},
|
||||
baseline_command_u8=(255,) * 20,
|
||||
cycles=(0,),
|
||||
minimum_sources=3,
|
||||
)
|
||||
|
||||
assert len(fitted) == 3
|
||||
assert len(rejected) == 1
|
||||
assert next(iter(rejected)).startswith("pinky_mcp_pitch_front_axis")
|
||||
|
||||
|
||||
def test_right_19_offsets_are_invariant_to_rigid_hand_repositioning() -> None:
|
||||
injected = [
|
||||
2.0, -3.0, 4.0, -1.5,
|
||||
1.0, -1.0, 0.7,
|
||||
0.8, -0.7, -0.8,
|
||||
-0.5, 0.6, 0.9,
|
||||
1.1, -1.0, -0.6,
|
||||
]
|
||||
_, first = _solve_synthetic_offsets(
|
||||
"right", injected, layout_id="g20_right_19"
|
||||
)
|
||||
_, moved = _solve_synthetic_offsets(
|
||||
"right",
|
||||
injected,
|
||||
layout_id="g20_right_19",
|
||||
base_euler_xyz_rad=(-0.25, 0.55, -0.35),
|
||||
base_translation_xyz_m=(-0.12, 0.28, 0.91),
|
||||
)
|
||||
|
||||
assert moved.direct_offsets_rad == pytest.approx(
|
||||
first.direct_offsets_rad, abs=1.0e-8
|
||||
)
|
||||
|
||||
|
||||
def test_right_19_only_verified_contact_zeros_use_mechanical_endpoints() -> None:
|
||||
hand = get_hand_calibration_profile("right", "g20_right_19")
|
||||
curves = {
|
||||
name: _synthetic_curve(255, math.radians(50.0))
|
||||
@@ -816,16 +1079,35 @@ def test_right_19_flexion_zero_is_derived_from_measured_contact_endpoint() -> No
|
||||
curves[f"{finger}_pip"] = _synthetic_curve(
|
||||
255, math.radians(103.0)
|
||||
)
|
||||
curves["thumb_mcp"] = _synthetic_curve(255, math.radians(70.0))
|
||||
curves["thumb_cmc_roll"] = _synthetic_curve(
|
||||
255, math.radians(76.0)
|
||||
)
|
||||
curves["thumb_cmc_yaw"] = _synthetic_curve(
|
||||
255, math.radians(91.0)
|
||||
)
|
||||
curves["thumb_cmc_pitch"] = _synthetic_curve(
|
||||
255, math.radians(47.0)
|
||||
)
|
||||
|
||||
offsets = derive_right_19_flexion_endpoint_offsets(
|
||||
offsets = derive_right_19_mechanical_endpoint_offsets(
|
||||
RIGHT_SOURCE_URDF, curves
|
||||
)
|
||||
|
||||
assert set(offsets) == {
|
||||
f"{finger}_{suffix}"
|
||||
for finger in ("index", "middle", "ring", "pinky")
|
||||
for suffix in ("mcp_pitch", "pip")
|
||||
"thumb_mcp",
|
||||
*(
|
||||
f"{finger}_{suffix}"
|
||||
for finger in ("index", "middle", "ring", "pinky")
|
||||
for suffix in ("mcp_pitch", "pip")
|
||||
),
|
||||
}
|
||||
assert math.degrees(offsets["thumb_mcp"]) == pytest.approx(
|
||||
math.degrees(1.25) - 70.0
|
||||
)
|
||||
assert not {
|
||||
"thumb_cmc_roll", "thumb_cmc_yaw", "thumb_cmc_pitch"
|
||||
}.intersection(offsets)
|
||||
assert math.degrees(offsets["index_mcp_pitch"]) == pytest.approx(
|
||||
math.degrees(1.22) - 71.0
|
||||
)
|
||||
@@ -834,6 +1116,100 @@ def test_right_19_flexion_zero_is_derived_from_measured_contact_endpoint() -> No
|
||||
)
|
||||
|
||||
|
||||
def _settled_endpoint_records(
|
||||
travel_rad: float,
|
||||
*,
|
||||
increasing_travel_rad: float | None = None,
|
||||
) -> list[dict[str, object]]:
|
||||
fixed_mounting = Rotation.from_euler("xyz", [0.7, -0.4, 1.1])
|
||||
mounted_axis = Rotation.from_euler("xyz", [-0.3, 0.8, 0.2]).apply(
|
||||
[0.0, 0.0, 1.0]
|
||||
)
|
||||
result: list[dict[str, object]] = []
|
||||
for direction, branch_travel in (
|
||||
("decreasing", travel_rad),
|
||||
(
|
||||
"increasing",
|
||||
travel_rad
|
||||
if increasing_travel_rad is None
|
||||
else increasing_travel_rad,
|
||||
),
|
||||
):
|
||||
endpoint = fixed_mounting * Rotation.from_rotvec(
|
||||
mounted_axis * branch_travel
|
||||
)
|
||||
for command, rotation in ((0, endpoint), (255, fixed_mounting)):
|
||||
result.append(
|
||||
{
|
||||
"direction": direction,
|
||||
"requested_command_u8": command,
|
||||
"relative_quaternion_xyzw": rotation.as_quat().tolist(),
|
||||
}
|
||||
)
|
||||
return result
|
||||
|
||||
|
||||
def test_right_19_endpoint_curve_scale_uses_direct_rigid_rotation() -> None:
|
||||
hand = get_hand_calibration_profile("right", "g20_right_19")
|
||||
names = {
|
||||
"thumb_mcp",
|
||||
*(
|
||||
f"{finger}_{suffix}"
|
||||
for finger in ("index", "middle", "ring", "pinky")
|
||||
for suffix in ("mcp_pitch", "pip")
|
||||
),
|
||||
}
|
||||
projected_travel = math.radians(105.832)
|
||||
direct_travel = math.radians(103.820)
|
||||
curves = {
|
||||
name: _synthetic_curve(255, projected_travel)
|
||||
for name in hand.measured_joints
|
||||
}
|
||||
records = {
|
||||
name: _settled_endpoint_records(direct_travel) for name in names
|
||||
}
|
||||
|
||||
anchored = anchor_right_19_mechanical_endpoint_curves(curves, records)
|
||||
|
||||
assert curves["middle_pip"].angle_rad[0] == pytest.approx(
|
||||
projected_travel
|
||||
)
|
||||
for name in names:
|
||||
assert anchored[name].angle_rad[0] == pytest.approx(direct_travel)
|
||||
assert anchored[name].angle_rad[255] == pytest.approx(0.0)
|
||||
assert anchored[name].circle[
|
||||
"mechanical_endpoint_direct_travel_rad"
|
||||
] == pytest.approx(direct_travel)
|
||||
assert anchored["thumb_cmc_pitch"] is curves["thumb_cmc_pitch"]
|
||||
|
||||
|
||||
def test_right_19_endpoint_curve_scale_rejects_direction_disagreement() -> None:
|
||||
hand = get_hand_calibration_profile("right", "g20_right_19")
|
||||
names = {
|
||||
"thumb_mcp",
|
||||
*(
|
||||
f"{finger}_{suffix}"
|
||||
for finger in ("index", "middle", "ring", "pinky")
|
||||
for suffix in ("mcp_pitch", "pip")
|
||||
),
|
||||
}
|
||||
curves = {
|
||||
name: _synthetic_curve(255, math.radians(103.0))
|
||||
for name in hand.measured_joints
|
||||
}
|
||||
records = {
|
||||
name: _settled_endpoint_records(math.radians(103.0))
|
||||
for name in names
|
||||
}
|
||||
records["middle_pip"] = _settled_endpoint_records(
|
||||
math.radians(103.0),
|
||||
increasing_travel_rad=math.radians(104.5),
|
||||
)
|
||||
|
||||
with pytest.raises(ValueError, match="endpoint directions disagree"):
|
||||
anchor_right_19_mechanical_endpoint_curves(curves, records)
|
||||
|
||||
|
||||
def test_right_15_finger_roll_limit_applies_to_independent_deviation() -> None:
|
||||
# A shared electrical centre belongs to the four-motor common datum; the
|
||||
# strict 3 deg assembly guard applies to finger-to-finger deviations.
|
||||
@@ -919,6 +1295,19 @@ def test_right_19_holdout_never_changes_frozen_training_offsets() -> None:
|
||||
validation_offset_bias_degrees={"thumb_cmc_roll": 2.0},
|
||||
)
|
||||
|
||||
finger_rolls = tuple(
|
||||
name
|
||||
for name in zero.direct_zero_joints
|
||||
if name.endswith("_mcp_roll") and not name.startswith("thumb_")
|
||||
)
|
||||
roll_common = float(
|
||||
np.median(
|
||||
[
|
||||
injected[zero.direct_zero_joints.index(name)]
|
||||
for name in finger_rolls
|
||||
]
|
||||
)
|
||||
)
|
||||
assert math.degrees(
|
||||
result.direct_offsets_rad["thumb_cmc_roll"]
|
||||
) == pytest.approx(injected[0], abs=0.05)
|
||||
@@ -1032,12 +1421,14 @@ def test_small_stable_offsets_are_validated_without_rewriting_urdf_zero() -> Non
|
||||
|
||||
def test_profiles_do_not_contain_hard_coded_thumb_zero_offsets() -> None:
|
||||
right = get_zero_calibration_profile("right")
|
||||
right_19 = get_zero_calibration_profile("right", "g20_right_19")
|
||||
left = get_zero_calibration_profile("left")
|
||||
|
||||
assert "thumb_cmc_roll" not in right.fixed_direct_zero_offsets_rad
|
||||
assert "thumb_cmc_roll" not in right.static_output_zero_offsets_rad
|
||||
assert "thumb_cmc_roll" not in left.fixed_direct_zero_offsets_rad
|
||||
assert "thumb_cmc_roll" not in left.static_output_zero_offsets_rad
|
||||
assert "thumb_mcp" not in right_19.fixed_direct_zero_offsets_rad
|
||||
|
||||
|
||||
def test_reference_finger_roll_static_zero_is_fixed_to_upright_cad() -> None:
|
||||
@@ -1091,18 +1482,24 @@ def test_root_line_depth_bias_does_not_change_thumb_roll_zero() -> None:
|
||||
assert result.direct_offsets_rad[f"{zero.reference_finger}_mcp_roll"] == 0.0
|
||||
|
||||
|
||||
def test_thumb_mcp_static_phase_bias_cannot_override_original_cad_zero() -> None:
|
||||
offsets = [2.0, -3.0, 4.0, -40.0, 1.0, -1.0, 2.0]
|
||||
def test_thumb_mcp_is_not_hard_coded_to_original_cad_zero() -> None:
|
||||
offsets = [2.0, -3.0, 4.0, -40.0, 1.0, -1.0, 2.0, *([0.0] * 9)]
|
||||
_, result = _solve_synthetic_offsets(
|
||||
"right",
|
||||
offsets,
|
||||
layout_id="g20_right_19",
|
||||
joint_maximum_offset_degrees={"thumb_mcp": 45.0},
|
||||
)
|
||||
|
||||
assert result.passed is True
|
||||
assert result.direct_offsets_rad["thumb_mcp"] == pytest.approx(0.0)
|
||||
assert result.cycle_offsets_rad["thumb_mcp"] == pytest.approx((0.0, 0.0))
|
||||
assert "thumb_ip" not in result.validation_error_by_joint_rad
|
||||
assert math.degrees(result.direct_offsets_rad["thumb_mcp"]) == pytest.approx(
|
||||
-40.0, abs=0.05
|
||||
)
|
||||
assert all(
|
||||
math.degrees(value) == pytest.approx(-40.0, abs=0.05)
|
||||
for value in result.cycle_offsets_rad["thumb_mcp"]
|
||||
)
|
||||
assert result.validation_error_by_joint_rad["thumb_ip"] < math.radians(0.05)
|
||||
for name in ("thumb_cmc_roll", "thumb_cmc_yaw", "thumb_cmc_pitch"):
|
||||
assert abs(math.degrees(result.direct_offsets_rad[name])) <= 20.0
|
||||
for name in ("pinky_mcp_pitch", "pinky_pip"):
|
||||
@@ -1149,6 +1546,66 @@ def test_zero_solver_rejects_axis_cone_geometry_that_a_zero_cannot_fix() -> None
|
||||
)
|
||||
|
||||
|
||||
def test_right_19_audits_stable_cross_view_cone_bias_without_retry() -> None:
|
||||
injected = [0.0] * 16
|
||||
_, result = _solve_synthetic_offsets(
|
||||
"right",
|
||||
injected,
|
||||
layout_id="g20_right_19",
|
||||
inject_observer_cone_bias_degrees=8.0,
|
||||
maximum_systematic_axis_cone_bias_degrees=15.0,
|
||||
)
|
||||
|
||||
assert result.passed is True
|
||||
assert math.degrees(
|
||||
result.axis_cone_mismatch_by_joint_rad["thumb_cmc_yaw"]
|
||||
) == pytest.approx(8.0)
|
||||
assert result.axis_cone_bias_classification_by_joint[
|
||||
"thumb_cmc_yaw"
|
||||
] == "stable_cross_view_or_planar_pnp_bias"
|
||||
|
||||
|
||||
def test_right_19_still_rejects_gross_cross_view_cone_mismatch() -> None:
|
||||
injected = [0.0] * 16
|
||||
_, result = _solve_synthetic_offsets(
|
||||
"right",
|
||||
injected,
|
||||
layout_id="g20_right_19",
|
||||
inject_observer_cone_bias_degrees=16.0,
|
||||
maximum_systematic_axis_cone_bias_degrees=15.0,
|
||||
)
|
||||
|
||||
assert result.passed is False
|
||||
assert result.failure_reasons["thumb_cmc_yaw"] == (
|
||||
"zero_axis_cone_mismatch_too_large"
|
||||
)
|
||||
|
||||
|
||||
def test_right_19_finger_roll_common_gauge_is_removed_before_limits() -> None:
|
||||
zero = get_zero_calibration_profile("right", "g20_right_19")
|
||||
injected = [0.0] * len(zero.direct_zero_joints)
|
||||
deviations = {
|
||||
"index_mcp_roll": -0.4,
|
||||
"middle_mcp_roll": 0.7,
|
||||
"ring_mcp_roll": 0.1,
|
||||
"pinky_mcp_roll": -0.1,
|
||||
}
|
||||
for name, deviation in deviations.items():
|
||||
injected[zero.direct_zero_joints.index(name)] = 18.0 + deviation
|
||||
|
||||
_, result = _solve_synthetic_offsets(
|
||||
"right", injected, layout_id="g20_right_19"
|
||||
)
|
||||
|
||||
assert result.passed is True
|
||||
common = float(np.median(list(deviations.values())))
|
||||
for name, deviation in deviations.items():
|
||||
expected = deviation - common
|
||||
assert math.degrees(result.direct_offsets_rad[name]) == pytest.approx(
|
||||
expected, abs=0.05
|
||||
)
|
||||
|
||||
|
||||
def test_zero_solver_rejects_unreliable_parallel_axis_line_phase() -> None:
|
||||
_, result = _solve_synthetic_offsets(
|
||||
"right",
|
||||
|
||||
Reference in New Issue
Block a user