标定稳定性
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也会合并为
|
重新采集时使用 `--no-resume`。升级前已经分别完成的正面/侧面roll也会合并为
|
||||||
一个完整同步任务断点;只有两边数据都完整时才复用。
|
一个完整同步任务断点;只有两边数据都完整时才复用。
|
||||||
|
|
||||||
命令自动完成产品哈希预检、运动、当前任务补扫、前三轮训练、第四轮隔离留出、
|
命令自动完成产品哈希预检、运动、当前任务补扫、前三轮训练、第四轮隔离留出、
|
||||||
15个会话数据求解主动关节URDF零位修正(`thumb_mcp`固定CAD参考)、
|
16个会话数据求解主动关节URDF零位修正、
|
||||||
21条视觉实测命令曲线发布;四指PIP/DIP的动态曲线均实测,四指DIP静态零位保留CAD。
|
21条视觉实测命令曲线发布;四指PIP/DIP的动态曲线均实测,四指DIP静态零位保留CAD。
|
||||||
终端只显示中文进度和问题;失败时复制“请复制以下内容给开发者”块即可。
|
终端只显示中文进度和问题;失败时复制“请复制以下内容给开发者”块即可。
|
||||||
|
|
||||||
四指末端的16 mm Tag允许使用刚性延长杆避挡;软件不假设末端Tag平面与中节Tag
|
四指末端的16 mm Tag允许使用刚性延长杆避挡;软件不假设末端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`。该指针只在
|
正式结果位于 `calibration_output/G20_RIGHT_001/latest_passed`。该指针只在
|
||||||
JSON、URDF数值等价、mesh完整性、21条曲线CAD限位、被动关节保护和隔离留出验证
|
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,因此四指正面+侧面同步roll中
|
||||||
允许ID 0暂时不可见,并使用本会话基准锁定值;运动连杆Tag仍必须实时可见,门限不
|
允许ID 0暂时不可见,并使用本会话基准锁定值;运动连杆Tag仍必须实时可见,门限不
|
||||||
放宽。终端用`锁`表示该固定参考有效,例如`正面[0锁,12✓]`,`✗`才表示需要处理的
|
放宽。终端用`锁`表示该固定参考有效,例如`正面[0锁,12✓]`,`✗`才表示需要处理的
|
||||||
实时Tag。锁定后不得移动相机、手掌底座或整只手,否则缓存参考失效,必须重新启动
|
实时Tag。锁定后本次标定运行中不得再移动相机、手掌底座或整只手,否则缓存参考
|
||||||
标定。任务级Tag有效率门限按"当前任务所需角色生效期间的采集帧"统计;
|
失效,必须重新启动标定;两次独立会话之间轻微调整整只手的位置不会改变机械端点
|
||||||
|
零位基准。任务级Tag有效率门限按"当前任务所需角色生效期间的采集帧"统计;
|
||||||
任务结束后角色要求会切回预检全套标签,静止期帧不参与该门限,避免把
|
任务结束后角色要求会切回预检全套标签,静止期帧不参与该门限,避免把
|
||||||
采集质量良好的任务误判为可见性失败。
|
采集质量良好的任务误判为可见性失败。
|
||||||
|
|
||||||
恢复末端Tag后,程序能够独立实测四指PIP和DIP的转轴及动态命令曲线。但同一次
|
恢复末端Tag后,程序能够独立实测四指PIP和DIP的转轴及动态命令曲线。但同一次
|
||||||
相机外参和同一套Tag安装下的重复扫描无法排除固定安装相位偏差;实体手在反馈0端
|
相机外参和同一套Tag安装下的重复扫描无法排除固定安装相位偏差;实体手在反馈0端
|
||||||
能够触掌是独立的机械端点约束。因此四指MCP pitch、PIP的URDF静态零位由本次
|
能够触掌是独立的机械端点约束。三个thumb CMC主动轴、`thumb_mcp`、四指MCP pitch
|
||||||
实测全屈曲行程与CAD触掌角之差求出,不再把同相机的平行轴PnP相位直接当作绝对
|
和PIP的URDF静态零位均由本次实测全行程与CAD机械端点之差求出,不再把跨相机的
|
||||||
零位,也不写死为0。生成URDF时同步修正这些关节的坐标上限和DIP mimic坐标偏置,
|
平面PnP绝对相位直接当作编码器零位,也不写死为0。生成URDF时同步修正这些关节的
|
||||||
|
坐标上限以及相关被动关节的mimic坐标偏置,
|
||||||
保证非零零偏不会缩短最大闭合量。末节Tag继续用于DIP动态曲线、轴线质量、遮挡和
|
保证非零零偏不会缩短最大闭合量。末节Tag继续用于DIP动态曲线、轴线质量、遮挡和
|
||||||
第四轮留出检查。正式数据求解静态修正范围为拇指CMC三个主动关节、四指MCP roll、
|
第四轮留出检查。正式数据求解静态修正范围为拇指CMC三个主动关节、`thumb_mcp`、
|
||||||
MCP pitch及PIP,共15个;只有`thumb_mcp`固定为既有CAD参考。
|
四指MCP roll、MCP pitch及PIP,共16个。`thumb_mcp`与四指屈伸关节一样使用
|
||||||
|
实测全屈曲行程和CAD机械端点联合求解,不直接采用MCP/IP耦合运动的单目PnP相位。
|
||||||
侧面累计避障按“PIP→MCP pitch→roll”的安全顺序分阶段进入,并按逆序分阶段退出;
|
侧面累计避障按“PIP→MCP pitch→roll”的安全顺序分阶段进入,并按逆序分阶段退出;
|
||||||
同类辅助电机(全部邻指滚转、全部PIP、全部MCP pitch)合并为同一个并行航点同时
|
同类辅助电机(全部邻指滚转、全部PIP、全部MCP pitch)合并为同一个并行航点同时
|
||||||
运动,被测通道最后单独进入。“滚转全部回中前不展开弯曲手指”“每指pitch先于PIP”
|
运动,被测通道最后单独进入。“滚转全部回中前不展开弯曲手指”“每指pitch先于PIP”
|
||||||
@@ -87,7 +106,8 @@ MCP pitch及PIP,共15个;只有`thumb_mcp`固定为既有CAD参考。
|
|||||||
当前已经在位(含反馈容差)的辅助电机保持原位,只有下一组不再使用的避障电机
|
当前已经在位(含反馈容差)的辅助电机保持原位,只有下一组不再使用的避障电机
|
||||||
退回基准,避免"先展开回基准、马上又折回"的多余动作;已评审的
|
退回基准,避免"先展开回基准、马上又折回"的多余动作;已评审的
|
||||||
"滚转先回中再展开""先滚开再弯曲"顺序保持不变。预检正反方向若都保留至少64个电机分箱且最大空缺
|
"滚转先回中再展开""先滚开再弯曲"顺序保持不变。预检正反方向若都保留至少64个电机分箱且最大空缺
|
||||||
不超过8,会把非roll任务四轮正式速度最多提高到预检速度的1.5倍;否则保持原保守速度。
|
不超过8,只作为采集能力诊断。G20右手四轮正式速度始终使用产品配置的固定值,
|
||||||
|
不会因本次预检帧率或识别密度而改变;旧11-Tag布局仍保留自适应速度兼容逻辑。
|
||||||
四指roll不再把同一反馈127误当成方向无关的唯一机械姿态:以`255→127`为标准物理
|
四指roll不再把同一反馈127误当成方向无关的唯一机械姿态:以`255→127`为标准物理
|
||||||
零位,反向到达127的实测偏差保留在`increasing_rad`中。方向分支间隙上限1.5°、
|
零位,反向到达127的实测偏差保留在`increasing_rad`中。方向分支间隙上限1.5°、
|
||||||
四轮间隙极差上限0.3°;其他关节仍使用严格的0.5°baseline回差门限。
|
四轮间隙极差上限0.3°;其他关节仍使用严格的0.5°baseline回差门限。
|
||||||
@@ -96,7 +116,8 @@ MCP pitch及PIP,共15个;只有`thumb_mcp`固定为既有CAD参考。
|
|||||||
方向分支检查和动态曲线的127相位都使用这两组双向静止数据,运动中经过127的帧不再
|
方向分支检查和动态曲线的127相位都使用这两组双向静止数据,运动中经过127的帧不再
|
||||||
替代静态保持姿态。
|
替代静态保持姿态。
|
||||||
前三轮只用于训练,第四轮完全留出;留出轮不参与显著性、Student-t置信区间或最终重拟合。
|
前三轮只用于训练,第四轮完全留出;留出轮不参与显著性、Student-t置信区间或最终重拟合。
|
||||||
每轮PnP都清空帧间跟踪状态并重新执行8帧静态初始化,但同一任务第1轮
|
每个任务只在低速递减预检起点执行一次8帧PnP静态初始化;预检往返和四轮正式
|
||||||
|
扫描连续复用同一帧间分支与任务参考,不再让每一轮独立选择平面Tag解。同一任务第1轮
|
||||||
已确立的端点相对姿态作为后3轮的分支锚点,防止独立初始化选到相反的
|
已确立的端点相对姿态作为后3轮的分支锚点,防止独立初始化选到相反的
|
||||||
IPPE镜像解。baseline标准接近和全部质量门限保持不变。
|
IPPE镜像解。baseline标准接近和全部质量门限保持不变。
|
||||||
电机15任务会利用源URDF中已确认的`thumb_ip mimic=1.03`,只在逐帧IPPE双解中
|
电机15任务会利用源URDF中已确认的`thumb_ip mimic=1.03`,只在逐帧IPPE双解中
|
||||||
@@ -105,26 +126,31 @@ IPPE镜像解。baseline标准接近和全部质量门限保持不变。
|
|||||||
|
|
||||||
当前19-Tag产品流程发布精简schema v4:21条运行时曲线全部来自当前会话的视觉实测。
|
当前19-Tag产品流程发布精简schema v4:21条运行时曲线全部来自当前会话的视觉实测。
|
||||||
URDF零位字段覆盖拇指4个主动关节和四指各自的`mcp_roll/mcp_pitch/pip`,共16个,
|
URDF零位字段覆盖拇指4个主动关节和四指各自的`mcp_roll/mcp_pitch/pip`,共16个,
|
||||||
其中四指`mcp_pitch/pip`共8个字段由视觉行程+机械端点联合求解,只有`thumb_mcp`
|
其中三个thumb CMC轴、`thumb_mcp`及四指`mcp_pitch/pip`共12个字段由实测旋转行程
|
||||||
字段固定为0;
|
与机械端点联合求解;
|
||||||
|
写出非零`thumb_mcp`零偏时同步平移其关节坐标上限,并更新被动`thumb_ip`的
|
||||||
|
`mimic offset`,因此不会改变CAD定义的最大屈曲实体姿态;
|
||||||
`thumb_ip`及四指DIP静态零位保留源CAD。旧schema v5文件仅作历史回放兼容,
|
`thumb_ip`及四指DIP静态零位保留源CAD。旧schema v5文件仅作历史回放兼容,
|
||||||
当前一键流程不再生成它。正面/侧面roll在同一次运动中独立拟合;方向、
|
当前一键流程不再生成它。正面/侧面roll在同一次运动中独立拟合;方向、
|
||||||
轴线和动态曲线均通过时做不确定度加权轴融合。侧面PIP连杆标签在滚转扫掠中
|
轴线和动态曲线均通过时做不确定度加权轴融合。侧面PIP连杆标签在滚转扫掠中
|
||||||
相对侧相机视线倾斜约13°~20°,平面标签的单目IPPE姿态二义性会给侧视姿态引入
|
相对侧相机视线倾斜约13°~20°,平面标签的单目IPPE姿态二义性会给侧视姿态引入
|
||||||
数度的系统性"绕视线"偏差(亚像素重投影无法发现,会话20260820_105535实测
|
数度的系统性"绕视线"偏差(亚像素重投影无法发现,会话20260820_105535实测
|
||||||
前后轴向稳定相差11.4°),因此跨视角方向差超过0.75°融合门限时不再判定任务
|
前后轴向稳定相差11.4°),因此侧视PIP连杆姿态不再参与MCP轴向融合或角曲线验收,
|
||||||
失败:程序记录`cross_view_roll_axis_diagnostic`诊断、跳过融合并采用可信的
|
正侧姿态差只写入`cross_view_roll_axis_diagnostic`。四根MCP侧摆轴在产品URDF中
|
||||||
前视轴向;仅当差值超过粗错误兜底上限(`cross_view_roll_maximum_axis_difference_deg`
|
严格平行:小指作为先采集的参考轴,其余三指复用该公共方向并各自独立拟合轴线位置,
|
||||||
默认15°或线距超过`cross_view_roll_maximum_axis_line_difference_mm`默认30mm,
|
避免平面PnP分支在不同会话中改变轴向。前视侧摆连杆受丝杆平移影响,其纯旋转拟合得到的是
|
||||||
对应标签贴错连杆或标签松动)时才失败。前视侧摆连杆与侧视PIP连杆的轴线
|
随手指结构变化的伪轴线,不能与侧视PIP连杆的物理轴线使用统一距离门限;
|
||||||
本身存在约21mm的系统性位置差(丝杠平移连杆),属预期现象。侧视校验通道
|
两者线距仅记录在诊断中。侧视校验通道
|
||||||
(`*_mcp_roll_side`)的
|
(`*_mcp_roll_side`)的
|
||||||
分支间隙跨轮极差上限放宽为
|
姿态分支间隙跨轮极差和独立姿态轴方向极差只作诊断,不触发重复采集;这两个量来自
|
||||||
`cross_view_roll_alias_maximum_branch_gap_range_deg`默认0.5°(绝对间隙1.5°上限
|
近掠射平面Tag的非发布姿态分量。绝对分支间隙1.5°上限保持不变,真正发布的正面主轴
|
||||||
不变),其`axis_pose_line_rms`降为诊断,不再作为准入门限;径向、平面、
|
仍使用原跨轮严格门限。侧视逐帧`axis_pose_line_rms`同样只作诊断,组合轴线改用四轮
|
||||||
圆一致性等其余数据质量门限全部保留。侧面端视roll的圆轨迹方向已经受
|
位置RMS验收;径向、平面、圆一致性及可见性门限全部保留。
|
||||||
|
侧面端视roll的圆轨迹方向已经受
|
||||||
姿态轴约束,因此自由三维圆平面与姿态轴的夹角只保留诊断,不再被重复作为硬门限;
|
姿态轴约束,因此自由三维圆平面与姿态轴的夹角只保留诊断,不再被重复作为硬门限;
|
||||||
径向残差、SE(3)轴线残差和正侧面轴/曲线一致性仍是硬门限。任一静态目标、第四轮留出、
|
径向残差和四轮轴线位置RMS仍是硬门限。正式MCP动态曲线统一使用正面Tag中心的
|
||||||
|
二维投影圆角度,侧面姿态曲线仅保留为诊断;任一正式视角自身四轮不重复或第四轮
|
||||||
|
留出失败仍会拒绝发布。任一静态目标、第四轮留出、
|
||||||
遮挡、PnP或跨机位检查失败时,只保留原始轨迹和`passed:false`诊断,不发布正式URDF。
|
遮挡、PnP或跨机位检查失败时,只保留原始轨迹和`passed:false`诊断,不发布正式URDF。
|
||||||
8个组合姿态仅保留为开发诊断,正式产品默认不执行。轴线零位求解不提供适合绝对笛卡尔
|
8个组合姿态仅保留为开发诊断,正式产品默认不执行。轴线零位求解不提供适合绝对笛卡尔
|
||||||
位置验收的手基座变换,因此不能用该诊断推翻已经通过的单关节隔离留出结果。三个CMC轴恢复使用
|
位置验收的手基座变换,因此不能用该诊断推翻已经通过的单关节隔离留出结果。三个CMC轴恢复使用
|
||||||
@@ -335,13 +361,14 @@ Tag位置,接近轴向观察时改用相机图像平面相位并丢弃单目Pn
|
|||||||
约4°的整指倾斜;重复扫描与同源留出不能排除这种系统偏差,因此不得写入URDF。
|
约4°的整指倾斜;重复扫描与同源留出不能排除这种系统偏差,因此不得写入URDF。
|
||||||
|
|
||||||
四指MCP屈伸和PIP采用同一静态策略:参考指轨迹仍参与动态曲线、轴质量和机构诊断,
|
四指MCP屈伸和PIP采用同一静态策略:参考指轨迹仍参与动态曲线、轴质量和机构诊断,
|
||||||
但拟合出的绝对相位不写入任何一根四指的 `origin.rpy`。拇指CMC roll/yaw/pitch的非零
|
但拟合出的共同掌坐标相位不写入四指 `origin.rpy`;只发布各指相对四指中值的实测
|
||||||
修正只能来自当前会话的三轮轨迹求解并通过第三轮留出验证;代码和配置中不保存任何
|
装配偏差。拇指CMC roll/yaw/pitch的非零修正来自当前会话的完整相对旋转行程与机械
|
||||||
按左右手或序列号写死的拇指零位角。电机5的256点动态曲线也使用本机三轮实测结果。
|
端点,视觉轴链继续用于轴线、PnP和留出诊断;代码和配置中不保存任何按左右手或
|
||||||
|
序列号写死的拇指零位角。电机5的256点动态曲线同样使用本机四轮实测结果。
|
||||||
|
|
||||||
7个直接零位依赖链为:yaw轴约束拇指roll、pitch轴约束拇指yaw、MCP轴线相位约束
|
视觉依赖链仍为:yaw轴检查拇指roll、pitch轴检查拇指yaw、MCP轴线相位检查拇指
|
||||||
拇指pitch;IP轴线相位仅作诊断,不能覆盖拇指MCP的原始CAD零位。参考指MCP pitch轴
|
pitch、IP轴线相位检查拇指MCP。该链用于几何和PnP诊断,不再决定四个具有机械端点
|
||||||
约束roll,PIP/DIP轴线相位只用于参考指机构诊断,不再覆盖四指CAD静态零位。
|
的拇指主动关节绝对零位;四指PIP/DIP轴线相位也继续用于机构诊断。
|
||||||
原始URDF的 `origin.xyz`、`axis.xyz`、连杆长度、mesh和被动结构固定。yaw扫描时电机5
|
原始URDF的 `origin.xyz`、`axis.xyz`、连杆长度、mesh和被动结构固定。yaw扫描时电机5
|
||||||
保持145,求解器使用实测 `angle_rad[145]` 还原该条件,不会把145误当成baseline。
|
保持145,求解器使用实测 `angle_rad[145]` 还原该条件,不会把145误当成baseline。
|
||||||
偏移超过各关节专用上限时整次失败。数值求解会在更宽的诊断范围内继续估计,因此状态和原始JSONL
|
偏移超过各关节专用上限时整次失败。数值求解会在更宽的诊断范围内继续估计,因此状态和原始JSONL
|
||||||
@@ -431,9 +458,9 @@ calibration_output/G20_RIGHT_001/<时间戳>/
|
|||||||
`zero_angles.urdf_zero_offset_rad`、5个被动标记、模板来源和总体质量。新URDF每次从
|
`zero_angles.urdf_zero_offset_rad`、5个被动标记、模板来源和总体质量。新URDF每次从
|
||||||
指定原始CAD文件生成,采用 `T_original × Rot(axis, offset)`,绝不叠加旧校准文件或
|
指定原始CAD文件生成,采用 `T_original × Rot(axis, offset)`,绝不叠加旧校准文件或
|
||||||
覆盖原文件;只有通过独立求解验证或明确机械装配基准授权的主动关节 `origin.rpy` 可能
|
覆盖原文件;只有通过独立求解验证或明确机械装配基准授权的主动关节 `origin.rpy` 可能
|
||||||
改变;当前19-Tag右手保留16个主动静态零位字段,其中15个由本会话数据求解,只有
|
改变;当前19-Tag右手的16个主动静态零位字段全部由本会话数据求解;四指DIP和
|
||||||
`thumb_mcp`固定为原始CAD零位0;四指DIP为被动关节,发布实测动态曲线并保留CAD
|
`thumb_ip`为被动关节,发布实测动态曲线并保留CAD静态零位,其mimic坐标偏置只随
|
||||||
静态零位,其mimic坐标偏置只随上游PIP坐标系变换作等价调整。
|
上游主动关节坐标系变换作等价调整。
|
||||||
未观测关节和其他URDF文本保持不变。源URDF中的相对mesh资源会按原相对路径复制到
|
未观测关节和其他URDF文本保持不变。源URDF中的相对mesh资源会按原相对路径复制到
|
||||||
同一会话,保证会话内URDF可独立加载,并在正式发布时逐文件记录SHA256。每帧Tag SE(3)、
|
同一会话,保证会话内URDF可独立加载,并在正式发布时逐文件记录SHA256。每帧Tag SE(3)、
|
||||||
图像时间戳、20通道状态、同步误差和PnP误差只进入 `raw_samples.jsonl`。
|
图像时间戳、20通道状态、同步误差和PnP误差只进入 `raw_samples.jsonl`。
|
||||||
|
|||||||
@@ -1,6 +1,7 @@
|
|||||||
schema_version: 1
|
schema_version: 1
|
||||||
model: G20
|
model: G20
|
||||||
side: right
|
side: right
|
||||||
|
tag_layout: g20_right_19
|
||||||
serial_number: G20_RIGHT_001
|
serial_number: G20_RIGHT_001
|
||||||
can_interface: can0
|
can_interface: can0
|
||||||
output_root: calibration_output
|
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: src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/linkerhand_g20_right.urdf
|
||||||
source_urdf_sha256: eeb6ffb0e95d2a6acd4c26331ae68062e0d74160de4b552b4f6d395cce5ca4e8
|
source_urdf_sha256: eeb6ffb0e95d2a6acd4c26331ae68062e0d74160de4b552b4f6d395cce5ca4e8
|
||||||
camera_extrinsics: config/g20_three_camera_extrinsics.yaml
|
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: 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: src/g20_thumb_apriltag_calibration/config/three_camera_tags_g20_right_19.yaml
|
||||||
tag_config_sha256: b1ab45e97ae42d57b0a3a63c725107b5aa3828c06b2ced16f8222d6e9ebadc41
|
tag_config_sha256: b1ab45e97ae42d57b0a3a63c725107b5aa3828c06b2ced16f8222d6e9ebadc41
|
||||||
|
|
||||||
|
|||||||
@@ -84,10 +84,12 @@ g20_calibration:
|
|||||||
zero_maximum_axis_cone_mismatch_deg: 5.0
|
zero_maximum_axis_cone_mismatch_deg: 5.0
|
||||||
zero_maximum_observability_condition_number: 10000000000.0
|
zero_maximum_observability_condition_number: 10000000000.0
|
||||||
zero_maximum_offset_deg: 20.0
|
zero_maximum_offset_deg: 20.0
|
||||||
# 四指MCP roll保留严格的装配保护范围。MCP pitch/PIP静态零位由
|
# 四指MCP roll保留严格的装配保护范围。thumb CMC三轴由多轴视觉几何
|
||||||
# 实测全屈曲行程与反馈0触掌的CAD机械端点联合求解,不允许写死为0。
|
# 求解且不假定电气端点等于CAD上限;thumb_mcp及四指MCP pitch/PIP
|
||||||
|
# 静态零位由实测全行程与CAD机械端点联合求解,不写死为0。
|
||||||
zero_finger_maximum_offset_deg: 3.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
|
endpoint_tolerance_u8: 2.0
|
||||||
# 请求命令与固件反馈是两个标定域。稳态检查点允许小幅死区,但反馈
|
# 请求命令与固件反馈是两个标定域。稳态检查点允许小幅死区,但反馈
|
||||||
@@ -155,6 +157,11 @@ g20_calibration:
|
|||||||
directional_zero_maximum_branch_gap_deg: 2.0
|
directional_zero_maximum_branch_gap_deg: 2.0
|
||||||
directional_zero_maximum_branch_gap_range_deg: 0.3
|
directional_zero_maximum_branch_gap_range_deg: 0.3
|
||||||
cross_view_roll_maximum_branch_gap_difference_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_monotonic_correction_deg: 3.0
|
||||||
passive_maximum_hysteresis_deg: 2.0
|
passive_maximum_hysteresis_deg: 2.0
|
||||||
# 九点姿态先按实测反馈重新对齐,再用此门限检查固有机械回差;请求命令域
|
# 九点姿态先按实测反馈重新对齐,再用此门限检查固有机械回差;请求命令域
|
||||||
|
|||||||
@@ -17,6 +17,7 @@ import xml.etree.ElementTree as ET
|
|||||||
|
|
||||||
import numpy as np
|
import numpy as np
|
||||||
|
|
||||||
|
from .core import COMMAND_NAMES
|
||||||
from .trajectory import (
|
from .trajectory import (
|
||||||
_angle_for_circle,
|
_angle_for_circle,
|
||||||
_fit_circle_with_axis,
|
_fit_circle_with_axis,
|
||||||
@@ -72,6 +73,24 @@ class SweepSpec:
|
|||||||
return f"{self.view}:motor{self.motor_index}:{','.join(self.joints)}"
|
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)
|
@dataclass(frozen=True)
|
||||||
class JointCurveFit:
|
class JointCurveFit:
|
||||||
angle_rad: tuple[float, ...]
|
angle_rad: tuple[float, ...]
|
||||||
@@ -98,6 +117,23 @@ class HandCalibrationProfile:
|
|||||||
layout_id: str = "legacy_11"
|
layout_id: str = "legacy_11"
|
||||||
measurement_specs: Mapping[str, JointSpec] | None = None
|
measurement_specs: Mapping[str, JointSpec] | None = None
|
||||||
axis_validation_sources: Mapping[str, str] | 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
|
@property
|
||||||
def measured_joints(self) -> tuple[str, ...]:
|
def measured_joints(self) -> tuple[str, ...]:
|
||||||
@@ -115,6 +151,20 @@ class HandCalibrationProfile:
|
|||||||
name for name, spec in self.record_specs.items() if spec.measured
|
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
|
@property
|
||||||
def active_joints(self) -> tuple[str, ...]:
|
def active_joints(self) -> tuple[str, ...]:
|
||||||
return tuple(
|
return tuple(
|
||||||
@@ -612,6 +662,20 @@ def _build_right_19_profile() -> HandCalibrationProfile:
|
|||||||
layout_id=G20_RIGHT_19_LAYOUT,
|
layout_id=G20_RIGHT_19_LAYOUT,
|
||||||
measurement_specs=measurement_specs,
|
measurement_specs=measurement_specs,
|
||||||
axis_validation_sources=axis_validation_sources,
|
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_rms_difference_rad: float = math.radians(1.0),
|
||||||
maximum_branch_gap_difference_rad: float = math.radians(0.3),
|
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]:
|
) -> 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:
|
if maximum_rms_difference_rad <= 0.0:
|
||||||
raise ValueError("cross-view RMS limit must be positive")
|
raise ValueError("cross-view RMS limit must be positive")
|
||||||
if maximum_branch_gap_difference_rad <= 0.0:
|
if maximum_branch_gap_difference_rad <= 0.0:
|
||||||
raise ValueError("cross-view branch-gap limit must be positive")
|
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"):
|
for field_name in ("angle_rad", "decreasing_rad", "increasing_rad"):
|
||||||
left = np.asarray(getattr(primary, field_name), dtype=float)
|
left = np.asarray(getattr(primary, field_name), dtype=float)
|
||||||
right = np.asarray(getattr(secondary, field_name), dtype=float)
|
right = np.asarray(getattr(secondary, field_name), dtype=float)
|
||||||
if left.shape != (256,) or right.shape != (256,):
|
if left.shape != (256,) or right.shape != (256,):
|
||||||
raise ValueError("cross-view curves must contain 256 values")
|
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))))
|
rms = float(np.sqrt(np.mean(np.square(left - right))))
|
||||||
metrics[f"{field_name}_rms_difference_rad"] = rms
|
metrics[f"{field_name}_rms_difference_rad"] = rms
|
||||||
if rms > maximum_rms_difference_rad:
|
if rms > maximum_rms_difference_rad:
|
||||||
@@ -1454,13 +1586,7 @@ def compare_cross_view_roll_curves(
|
|||||||
"cross_view_roll_curve_difference_too_large:"
|
"cross_view_roll_curve_difference_too_large:"
|
||||||
f"{field_name}:{math.degrees(rms):.6f}deg"
|
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)
|
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_gap = float(
|
||||||
primary.increasing_rad[primary_zero]
|
primary.increasing_rad[primary_zero]
|
||||||
- primary.decreasing_rad[primary_zero]
|
- primary.decreasing_rad[primary_zero]
|
||||||
@@ -1469,9 +1595,27 @@ def compare_cross_view_roll_curves(
|
|||||||
secondary.increasing_rad[secondary_zero]
|
secondary.increasing_rad[secondary_zero]
|
||||||
- secondary.decreasing_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
|
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(
|
raise ValueError(
|
||||||
"cross_view_roll_branch_gap_difference_too_large:"
|
"cross_view_roll_branch_gap_difference_too_large:"
|
||||||
f"{math.degrees(branch_gap_difference):.6f}deg"
|
f"{math.degrees(branch_gap_difference):.6f}deg"
|
||||||
@@ -1479,6 +1623,35 @@ def compare_cross_view_roll_curves(
|
|||||||
return metrics
|
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(
|
def fit_projected_zero(
|
||||||
records: Sequence[Mapping[str, Any]],
|
records: Sequence[Mapping[str, Any]],
|
||||||
*,
|
*,
|
||||||
|
|||||||
+310
-61
@@ -30,25 +30,34 @@ from .full_hand import (
|
|||||||
build_compact_payload,
|
build_compact_payload,
|
||||||
canonical_zero_direction,
|
canonical_zero_direction,
|
||||||
clamp_runtime_fits_to_urdf_limits,
|
clamp_runtime_fits_to_urdf_limits,
|
||||||
compare_cross_view_roll_curves,
|
cross_view_roll_diagnostic_metrics,
|
||||||
derive_mimic_passive_fits,
|
derive_mimic_passive_fits,
|
||||||
fit_joint_image_curve,
|
fit_joint_image_curve,
|
||||||
get_hand_calibration_profile,
|
get_hand_calibration_profile,
|
||||||
validate_compact_payload,
|
validate_compact_payload,
|
||||||
)
|
)
|
||||||
|
from .product import get_product_calibration_contract
|
||||||
from .storage import atomic_write_json
|
from .storage import atomic_write_json
|
||||||
from .urdf_zero import (
|
from .urdf_zero import (
|
||||||
JointAxisMeasurement,
|
JointAxisMeasurement,
|
||||||
UrdfKinematicModel,
|
UrdfKinematicModel,
|
||||||
_angles_from_state,
|
_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,
|
baseline_hysteresis_by_cycle_rad,
|
||||||
circle_direction_is_constrained,
|
circle_direction_is_constrained,
|
||||||
|
cross_view_side_line_source,
|
||||||
fit_joint_axis_measurement,
|
fit_joint_axis_measurement,
|
||||||
|
fit_partial_palm_orientation_measurements,
|
||||||
fit_rotation_joint_curve,
|
fit_rotation_joint_curve,
|
||||||
get_zero_calibration_profile,
|
get_zero_calibration_profile,
|
||||||
derive_right_19_flexion_endpoint_offsets,
|
derive_right_19_mechanical_endpoint_offsets,
|
||||||
joint_curve_holdout_errors,
|
joint_curve_holdout_errors,
|
||||||
|
refit_axis_line_group_with_shared_radius,
|
||||||
|
select_cross_view_roll_direction_source,
|
||||||
solve_urdf_zero_offsets,
|
solve_urdf_zero_offsets,
|
||||||
|
with_depth_free_axis_projection,
|
||||||
write_zero_corrected_urdf,
|
write_zero_corrected_urdf,
|
||||||
)
|
)
|
||||||
|
|
||||||
@@ -139,6 +148,35 @@ def _latest_attempt_records(
|
|||||||
return dict(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(
|
def _load_raw_session(
|
||||||
session_dir: Path,
|
session_dir: Path,
|
||||||
) -> tuple[
|
) -> 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]]],
|
||||||
dict[str, list[dict[str, Any]]],
|
dict[str, list[dict[str, Any]]],
|
||||||
|
dict[str, list[dict[str, Any]]],
|
||||||
Path,
|
Path,
|
||||||
]:
|
]:
|
||||||
raw_path = session_dir / "raw_samples.jsonl"
|
raw_path = session_dir / "raw_samples.jsonl"
|
||||||
@@ -163,7 +202,9 @@ def _load_raw_session(
|
|||||||
for row in rows:
|
for row in rows:
|
||||||
if "command_u8" in row:
|
if "command_u8" in row:
|
||||||
continue
|
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"])))
|
row["command_u8"] = int(round(float(row["feedback_u8"])))
|
||||||
elif row.get("kind") == "steady_command_sample":
|
elif row.get("kind") == "steady_command_sample":
|
||||||
row["command_u8"] = int(row["requested_command_u8"])
|
row["command_u8"] = int(row["requested_command_u8"])
|
||||||
@@ -175,6 +216,7 @@ def _load_raw_session(
|
|||||||
_latest_attempt_records(rows),
|
_latest_attempt_records(rows),
|
||||||
_latest_attempt_records(rows, kind="baseline_hold_sample"),
|
_latest_attempt_records(rows, kind="baseline_hold_sample"),
|
||||||
_latest_attempt_records(rows, kind="steady_command_sample"),
|
_latest_attempt_records(rows, kind="steady_command_sample"),
|
||||||
|
_latest_palm_axis_records(rows),
|
||||||
raw_path,
|
raw_path,
|
||||||
)
|
)
|
||||||
|
|
||||||
@@ -189,7 +231,13 @@ def _fit_curve(
|
|||||||
motor = profile.record_specs[name].motor_index
|
motor = profile.record_specs[name].motor_index
|
||||||
if (
|
if (
|
||||||
profile.layout_id == G20_RIGHT_19_LAYOUT
|
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_joint_image_curve(records)
|
||||||
return fit_rotation_joint_curve(
|
return fit_rotation_joint_curve(
|
||||||
@@ -234,6 +282,7 @@ def _fit_axes(
|
|||||||
)
|
)
|
||||||
extrinsics = load_three_camera_extrinsics(extrinsics_file)
|
extrinsics = load_three_camera_extrinsics(extrinsics_file)
|
||||||
cache: dict[tuple[str, int], JointAxisMeasurement] = {}
|
cache: dict[tuple[str, int], JointAxisMeasurement] = {}
|
||||||
|
cross_view_direction_source_cache: dict[str, str] = {}
|
||||||
upstream_by_joint = {
|
upstream_by_joint = {
|
||||||
"thumb_mcp": "thumb_cmc_pitch",
|
"thumb_mcp": "thumb_cmc_pitch",
|
||||||
"thumb_ip": "thumb_mcp",
|
"thumb_ip": "thumb_mcp",
|
||||||
@@ -246,6 +295,15 @@ def _fit_axes(
|
|||||||
for finger in ("index", "middle", "ring", "pinky")
|
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:
|
def fit_raw(name: str, cycle: int) -> JointAxisMeasurement:
|
||||||
key = (name, cycle)
|
key = (name, cycle)
|
||||||
@@ -288,6 +346,9 @@ def _fit_axes(
|
|||||||
),
|
),
|
||||||
view_normal_common_xyz=tuple(float(value) for value in view_normal),
|
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
|
cache[key] = result
|
||||||
return result
|
return result
|
||||||
|
|
||||||
@@ -311,11 +372,52 @@ def _fit_axes(
|
|||||||
np.cross(secondary_point - primary_point, primary_axis)
|
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
|
# Mirror the live node's tiered policy: the side-view planar-tag
|
||||||
# IPPE bias makes sub-degree cross-view agreement unreachable, so
|
# IPPE bias makes sub-degree cross-view agreement unreachable, so
|
||||||
# disagreement above the fusion gate keeps the front-only axis while
|
# disagreement above the fusion gate keeps the front-only axis while
|
||||||
# only the gross bound (wrong link / loose tag) still fails.
|
# only a gross direction error (wrong link / loose tag) still fails.
|
||||||
if difference > math.radians(15.0) or line_distance > 0.030:
|
# 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")
|
raise ValueError(f"{name}: cross-view axis gross disagreement")
|
||||||
if difference > math.radians(0.75) or line_distance > 0.001:
|
if difference > math.radians(0.75) or line_distance > 0.001:
|
||||||
selected_direction = primary_axis
|
selected_direction = primary_axis
|
||||||
@@ -329,7 +431,6 @@ def _fit_axes(
|
|||||||
None,
|
None,
|
||||||
)
|
)
|
||||||
if observer_name is not None:
|
if observer_name is not None:
|
||||||
observer = fit_raw(observer_name, cycle)
|
|
||||||
model = UrdfKinematicModel(source_urdf)
|
model = UrdfKinematicModel(source_urdf)
|
||||||
parent_axis, _ = model.axis_line(
|
parent_axis, _ = model.axis_line(
|
||||||
name, zero_offsets={}, joint_angles={}
|
name, zero_offsets={}, joint_angles={}
|
||||||
@@ -340,11 +441,9 @@ def _fit_axes(
|
|||||||
expected_cone = math.acos(
|
expected_cone = math.acos(
|
||||||
abs(float(np.clip(parent_axis @ observer_axis, -1.0, 1.0)))
|
abs(float(np.clip(parent_axis @ observer_axis, -1.0, 1.0)))
|
||||||
)
|
)
|
||||||
measured_observer = np.asarray(
|
def cone_residual(
|
||||||
observer.axis_common_xyz, dtype=float
|
candidate: np.ndarray, measured_observer: np.ndarray
|
||||||
)
|
) -> float:
|
||||||
|
|
||||||
def cone_residual(candidate: np.ndarray) -> float:
|
|
||||||
measured_cone = math.acos(
|
measured_cone = math.acos(
|
||||||
abs(
|
abs(
|
||||||
float(
|
float(
|
||||||
@@ -356,10 +455,46 @@ def _fit_axes(
|
|||||||
)
|
)
|
||||||
return abs(measured_cone - expected_cone)
|
return abs(measured_cone - expected_cone)
|
||||||
|
|
||||||
if (
|
if name not in cross_view_direction_source_cache:
|
||||||
cone_residual(primary_axis) > math.radians(5.0)
|
primary_residuals: list[float] = []
|
||||||
and cone_residual(secondary_axis) <= math.radians(5.0)
|
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_direction = secondary_axis
|
||||||
selected_source = "cross_view_cone_selected_secondary"
|
selected_source = "cross_view_cone_selected_secondary"
|
||||||
# Mirror the live node: use the view whose direction satisfies
|
# 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
|
for value in secondary.point_common_xyz_m
|
||||||
),
|
),
|
||||||
pose_axis_line_rms_m=secondary.pose_axis_line_rms_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_direction_source=selected_source,
|
||||||
axis_point_source="side_circle_cross_view",
|
axis_point_source="side_circle_cross_view",
|
||||||
)
|
)
|
||||||
@@ -406,14 +542,45 @@ def _fit_axes(
|
|||||||
secondary.pose_axis_line_rms_m,
|
secondary.pose_axis_line_rms_m,
|
||||||
line_distance,
|
line_distance,
|
||||||
),
|
),
|
||||||
|
pose_axis_line_source_joints=(name, validation_name),
|
||||||
axis_direction_source="cross_view_weighted_fusion",
|
axis_direction_source="cross_view_weighted_fusion",
|
||||||
)
|
)
|
||||||
|
|
||||||
return [
|
result: list[JointAxisMeasurement] = []
|
||||||
fit_one(name, cycle)
|
for name in zero_profile.axis_joints:
|
||||||
for name in zero_profile.axis_joints
|
group = [fit_one(name, cycle) for cycle in range(repetitions)]
|
||||||
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:
|
def _maximum_undirected_axis_difference(axes: Sequence[Sequence[float]]) -> float:
|
||||||
@@ -601,6 +768,7 @@ def _quality_failures(
|
|||||||
if (
|
if (
|
||||||
max(baseline_hysteresis) - min(baseline_hysteresis)
|
max(baseline_hysteresis) - min(baseline_hysteresis)
|
||||||
> gap_range_limit
|
> gap_range_limit
|
||||||
|
and spec.zero_kind != "axis_cross_view_validation"
|
||||||
):
|
):
|
||||||
failures.append(
|
failures.append(
|
||||||
f"{name}: baseline directional gap repeatability"
|
f"{name}: baseline directional gap repeatability"
|
||||||
@@ -613,6 +781,7 @@ def _quality_failures(
|
|||||||
cycle_travels: list[float] = []
|
cycle_travels: list[float] = []
|
||||||
cycle_axes: list[Sequence[float]] = []
|
cycle_axes: list[Sequence[float]] = []
|
||||||
cycle_axis_sources: list[str] = []
|
cycle_axis_sources: list[str] = []
|
||||||
|
cycle_axis_measurements: list[JointAxisMeasurement] = []
|
||||||
for cycle in range(repetitions):
|
for cycle in range(repetitions):
|
||||||
cycle_fit = _fit_curve(
|
cycle_fit = _fit_curve(
|
||||||
name,
|
name,
|
||||||
@@ -626,11 +795,13 @@ def _quality_failures(
|
|||||||
axis = axis_by_key[(name, cycle)]
|
axis = axis_by_key[(name, cycle)]
|
||||||
cycle_axes.append(axis.axis_common_xyz)
|
cycle_axes.append(axis.axis_common_xyz)
|
||||||
cycle_axis_sources.append(axis.axis_direction_source)
|
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"]):
|
if axis.radial_rms_m > float(parameters["axis_maximum_radial_rms_m"]):
|
||||||
failures.append(f"{name} cycle {cycle + 1}: radial RMS")
|
failures.append(f"{name} cycle {cycle + 1}: radial RMS")
|
||||||
if (
|
if (
|
||||||
profile.record_specs[name].zero_kind
|
profile.record_specs[name].zero_kind
|
||||||
!= "axis_cross_view_validation"
|
!= "axis_cross_view_validation"
|
||||||
|
and cross_view_side_line_source(axis) is None
|
||||||
and axis.pose_axis_line_rms_m
|
and axis.pose_axis_line_rms_m
|
||||||
> float(parameters["axis_maximum_pose_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")
|
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(
|
travel_limit = math.radians(
|
||||||
float(
|
float(
|
||||||
parameters[
|
parameters[
|
||||||
@@ -671,6 +865,7 @@ def _quality_failures(
|
|||||||
source == "upstream_constraint"
|
source == "upstream_constraint"
|
||||||
for source in cycle_axis_sources
|
for source in cycle_axis_sources
|
||||||
)
|
)
|
||||||
|
and spec.zero_kind != "axis_cross_view_validation"
|
||||||
and _maximum_undirected_axis_difference(cycle_axes) > math.radians(
|
and _maximum_undirected_axis_difference(cycle_axes) > math.radians(
|
||||||
float(parameters["zero_maximum_axis_cycle_difference_deg"])
|
float(parameters["zero_maximum_axis_cycle_difference_deg"])
|
||||||
)
|
)
|
||||||
@@ -832,21 +1027,28 @@ def replay_session(
|
|||||||
records_by_joint,
|
records_by_joint,
|
||||||
baseline_records_by_joint,
|
baseline_records_by_joint,
|
||||||
command_records_by_joint,
|
command_records_by_joint,
|
||||||
|
palm_axis_records_by_source,
|
||||||
raw_path,
|
raw_path,
|
||||||
) = _load_raw_session(session)
|
) = _load_raw_session(session)
|
||||||
|
model = str(start.get("model", "G20")).strip().upper()
|
||||||
side = str(start["hand_type"]).lower()
|
side = str(start["hand_type"]).lower()
|
||||||
requested_layout_id = str(
|
requested_layout_id = str(
|
||||||
start.get("tag_layout", "legacy_11")
|
start.get("tag_layout", "legacy_11")
|
||||||
).lower()
|
).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
|
# Normalise the former g20_right_15 compatibility alias immediately so
|
||||||
# every downstream product guard uses the actual physical 19-Tag profile
|
# every downstream product guard uses the actual physical 19-Tag profile
|
||||||
# instead of silently falling through legacy-11 logic.
|
# instead of silently falling through legacy-11 logic.
|
||||||
layout_id = profile.layout_id
|
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"])
|
baseline = tuple(int(value) for value in start["baseline_command_u8"])
|
||||||
if len(baseline) != 20:
|
if len(baseline) != profile.command_count:
|
||||||
raise ValueError("session baseline must contain exactly 20 commands")
|
raise ValueError(
|
||||||
|
"session baseline length differs from the registered product schema"
|
||||||
|
)
|
||||||
if (
|
if (
|
||||||
layout_id == G20_RIGHT_19_LAYOUT
|
layout_id == G20_RIGHT_19_LAYOUT
|
||||||
and write_outputs
|
and write_outputs
|
||||||
@@ -878,8 +1080,14 @@ def replay_session(
|
|||||||
for row in combinations
|
for row in combinations
|
||||||
):
|
):
|
||||||
raise ValueError("combination pose validation exceeds product limits")
|
raise ValueError("combination pose validation exceeds product limits")
|
||||||
if set(records_by_joint) != set(profile.record_joints):
|
required_record_joints = set(profile.record_joints)
|
||||||
raise ValueError("raw session does not contain exactly the task record set")
|
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(
|
def hysteresis_records(
|
||||||
name: str,
|
name: str,
|
||||||
@@ -904,7 +1112,9 @@ def replay_session(
|
|||||||
name: str, *, require_full_pose: bool = False
|
name: str, *, require_full_pose: bool = False
|
||||||
) -> list[dict[str, Any]]:
|
) -> list[dict[str, Any]]:
|
||||||
"""Merge settled baseline holds into directional model records."""
|
"""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:
|
if canonical_zero_direction(profile, name) is None:
|
||||||
return records
|
return records
|
||||||
required = {
|
required = {
|
||||||
@@ -971,7 +1181,7 @@ def replay_session(
|
|||||||
}
|
}
|
||||||
command_fits = dict(training_fits)
|
command_fits = dict(training_fits)
|
||||||
if layout_id == G20_RIGHT_19_LAYOUT:
|
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(
|
raise ValueError(
|
||||||
"g20_right_19 session is incomplete: missing steady command checkpoints"
|
"g20_right_19 session is incomplete: missing steady command checkpoints"
|
||||||
)
|
)
|
||||||
@@ -1035,6 +1245,11 @@ def replay_session(
|
|||||||
command_fits,
|
command_fits,
|
||||||
profile=profile,
|
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]] = {}
|
cross_view_roll_metrics: dict[str, dict[str, float]] = {}
|
||||||
for name, validation_name in (
|
for name, validation_name in (
|
||||||
profile.axis_validation_sources or {}
|
profile.axis_validation_sources or {}
|
||||||
@@ -1055,35 +1270,11 @@ def replay_session(
|
|||||||
profile=profile,
|
profile=profile,
|
||||||
baseline=baseline,
|
baseline=baseline,
|
||||||
)
|
)
|
||||||
training_metrics = compare_cross_view_roll_curves(
|
training_metrics = cross_view_roll_diagnostic_metrics(
|
||||||
training_fits[name],
|
training_fits[name], validation_training
|
||||||
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,
|
|
||||||
)
|
|
||||||
)
|
|
||||||
),
|
|
||||||
)
|
)
|
||||||
final_metrics = compare_cross_view_roll_curves(
|
final_metrics = cross_view_roll_diagnostic_metrics(
|
||||||
measured_fits[name],
|
measured_fits[name], validation_all
|
||||||
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,
|
|
||||||
)
|
|
||||||
)
|
|
||||||
),
|
|
||||||
)
|
)
|
||||||
cross_view_roll_metrics[name] = {
|
cross_view_roll_metrics[name] = {
|
||||||
**{
|
**{
|
||||||
@@ -1103,6 +1294,28 @@ def replay_session(
|
|||||||
source_urdf=source_urdf,
|
source_urdf=source_urdf,
|
||||||
repetitions=repetitions,
|
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(
|
failures = _quality_failures(
|
||||||
records_by_joint=records_by_joint,
|
records_by_joint=records_by_joint,
|
||||||
baseline_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()
|
name: int(spec.motor_index) for name, spec in profile.joint_specs.items()
|
||||||
}
|
}
|
||||||
endpoint_zero_offsets = (
|
endpoint_zero_offsets = (
|
||||||
derive_right_19_flexion_endpoint_offsets(
|
derive_right_19_mechanical_endpoint_offsets(
|
||||||
source_urdf,
|
source_urdf,
|
||||||
command_fits,
|
command_fits,
|
||||||
maximum_offset_rad=math.radians(
|
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
|
if layout_id == G20_RIGHT_19_LAYOUT
|
||||||
@@ -1242,6 +1459,7 @@ def replay_session(
|
|||||||
solve_arguments = {
|
solve_arguments = {
|
||||||
"source_urdf": source_urdf,
|
"source_urdf": source_urdf,
|
||||||
"measurements": axes,
|
"measurements": axes,
|
||||||
|
"palm_orientation_measurements": palm_orientation_measurements,
|
||||||
"motor_by_joint": motor_by_joint,
|
"motor_by_joint": motor_by_joint,
|
||||||
"maximum_offset_rad": math.radians(float(parameters["zero_maximum_offset_deg"])),
|
"maximum_offset_rad": math.radians(float(parameters["zero_maximum_offset_deg"])),
|
||||||
"finger_maximum_offset_rad": math.radians(
|
"finger_maximum_offset_rad": math.radians(
|
||||||
@@ -1254,6 +1472,13 @@ def replay_session(
|
|||||||
"maximum_axis_cone_mismatch_rad": math.radians(
|
"maximum_axis_cone_mismatch_rad": math.radians(
|
||||||
float(parameters["zero_maximum_axis_cone_mismatch_deg"])
|
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(
|
"maximum_observability_condition_number": float(
|
||||||
parameters.get(
|
parameters.get(
|
||||||
"zero_maximum_observability_condition_number", 1.0e10
|
"zero_maximum_observability_condition_number", 1.0e10
|
||||||
@@ -1469,6 +1694,21 @@ def replay_session(
|
|||||||
"offset_covariance_rad2": (
|
"offset_covariance_rad2": (
|
||||||
final_zero.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
|
if layout_id == G20_RIGHT_19_LAYOUT
|
||||||
else None
|
else None
|
||||||
@@ -1508,7 +1748,7 @@ def replay_session(
|
|||||||
"thumb_default": float(parameters["zero_maximum_offset_deg"]),
|
"thumb_default": float(parameters["zero_maximum_offset_deg"]),
|
||||||
},
|
},
|
||||||
"static_zero_policy": (
|
"static_zero_policy": (
|
||||||
"visual_axes_plus_measured_contact_endpoint"
|
"visual_axes_plus_measured_mechanical_endpoint"
|
||||||
if layout_id == G20_RIGHT_19_LAYOUT
|
if layout_id == G20_RIGHT_19_LAYOUT
|
||||||
else "direct_measurements_only"
|
else "direct_measurements_only"
|
||||||
),
|
),
|
||||||
@@ -1538,6 +1778,15 @@ def replay_session(
|
|||||||
name: [math.degrees(value) for value in values]
|
name: [math.degrees(value) for value in values]
|
||||||
for name, values in holdout_zero.cycle_offsets_rad.items()
|
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": {
|
"offset_uncertainty_deg": {
|
||||||
name: math.degrees(value)
|
name: math.degrees(value)
|
||||||
for name, value in holdout_zero.offset_uncertainty_rad.items()
|
for name, value in holdout_zero.offset_uncertainty_rad.items()
|
||||||
@@ -1577,7 +1826,7 @@ def replay_session(
|
|||||||
|
|
||||||
def main() -> None:
|
def main() -> None:
|
||||||
parser = argparse.ArgumentParser(
|
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("session_dir")
|
||||||
parser.add_argument("--serial-number", default=None)
|
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
|
from __future__ import annotations
|
||||||
|
|
||||||
@@ -155,8 +155,9 @@ def _launch_command(
|
|||||||
resume_from: Path | None = None,
|
resume_from: Path | None = None,
|
||||||
) -> list[str]:
|
) -> list[str]:
|
||||||
values = {
|
values = {
|
||||||
"hand_type": "right",
|
"model": config.model,
|
||||||
"tag_layout": "g20_right_19",
|
"hand_type": config.side,
|
||||||
|
"tag_layout": config.tag_layout,
|
||||||
"serial_number": config.serial_number,
|
"serial_number": config.serial_number,
|
||||||
"can_interface": config.can_interface,
|
"can_interface": config.can_interface,
|
||||||
"session_dir": str(session),
|
"session_dir": str(session),
|
||||||
@@ -462,8 +463,8 @@ def _automatic_resume_candidate(config: ProductConfig) -> Path | None:
|
|||||||
continue
|
continue
|
||||||
if (
|
if (
|
||||||
start is None
|
start is None
|
||||||
or start.get("hand_type") != "right"
|
or start.get("hand_type") != config.side
|
||||||
or start.get("tag_layout") != "g20_right_19"
|
or start.get("tag_layout") != config.tag_layout
|
||||||
or start.get("source_urdf_sha256")
|
or start.get("source_urdf_sha256")
|
||||||
!= config.source_urdf_sha256
|
!= config.source_urdf_sha256
|
||||||
):
|
):
|
||||||
@@ -550,9 +551,9 @@ def run(
|
|||||||
print(
|
print(
|
||||||
"\n".join(
|
"\n".join(
|
||||||
[
|
[
|
||||||
"PASS:G20右手标定、URDF修正和独立复验全部通过。",
|
f"PASS:{config.model} {config.side} 标定、URDF修正和复验全部通过。",
|
||||||
f"正式结果:{config.session_root / 'latest_passed'}",
|
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']}",
|
f"URDF:{summary['artifacts']['urdf']}",
|
||||||
]
|
]
|
||||||
),
|
),
|
||||||
@@ -564,7 +565,7 @@ def run(
|
|||||||
|
|
||||||
|
|
||||||
def main(args: list[str] | None = None) -> None:
|
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("--config", default=str(_default_product_config()))
|
||||||
parser.add_argument("--workspace", default=None)
|
parser.add_argument("--workspace", default=None)
|
||||||
parser.add_argument("--preflight-only", action="store_true")
|
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
|
@dataclass
|
||||||
class ProgressEstimator:
|
class ProgressEstimator:
|
||||||
started_at: float
|
started_at: float
|
||||||
@@ -202,7 +239,7 @@ def render_progress_zh(
|
|||||||
)
|
)
|
||||||
else:
|
else:
|
||||||
tag_status = (
|
tag_status = (
|
||||||
f"{visible_tags}/{required_tags} 有效"
|
f"{visible_tags}/{required_tags} 可见"
|
||||||
+ (f"(含锁定 {locked_tag_count})" if locked_tag_count else "")
|
+ (f"(含锁定 {locked_tag_count})" if locked_tag_count else "")
|
||||||
)
|
)
|
||||||
task_tag_ids = (
|
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"Tag:{tag_status}{task_tag_ids} 相机:{ready_cameras}/3 正常 反馈:{feedback_hz:.1f} Hz",
|
||||||
f"质量:有效帧 {active.get('valid_frames', 0)} 已自动重扫 {retry} 次",
|
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(
|
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", {})
|
resume = status.get("resume", {})
|
||||||
if isinstance(resume, Mapping) and resume.get("used"):
|
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()
|
for item in views.values()
|
||||||
if isinstance(item, Mapping) and item.get("group_pnp_reason")
|
if isinstance(item, Mapping) and item.get("group_pnp_reason")
|
||||||
] if isinstance(views, Mapping) else []
|
] 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-"):
|
if value.startswith("CFG-"):
|
||||||
return value, "产品配置或文件预检失败", "不要移动相机;复制本诊断块给开发者。"
|
return value, "产品配置或文件预检失败", "不要移动相机;复制本诊断块给开发者。"
|
||||||
if value.startswith("CAM-STATUS-202"):
|
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后从断点继续,不要重新采集。",
|
"已采集的完整任务会保留;检查calibration.log后从断点继续,不要重新采集。",
|
||||||
)
|
)
|
||||||
|
if "palm_orientation_quality_failed" in value:
|
||||||
|
return (
|
||||||
|
"VAL-QUALITY-501",
|
||||||
|
"掌部方向校正的可观测轨迹不足",
|
||||||
|
"保持Tag安装不变;调整标定前手位,使正面Tag 10–13至少三枚"
|
||||||
|
"在对应运动起始段可见。",
|
||||||
|
)
|
||||||
if "motor_state_stalled" in value:
|
if "motor_state_stalled" in value:
|
||||||
return "MOTION-STALL-301", "电机反馈停止向目标推进,程序已保持当前位置", "先检查机械卡阻,未排除前不要重复强推。"
|
return "MOTION-STALL-301", "电机反馈停止向目标推进,程序已保持当前位置", "先检查机械卡阻,未排除前不要重复强推。"
|
||||||
if any(
|
if any(
|
||||||
@@ -295,9 +371,19 @@ def classify_error(reason: str, status: Mapping[str, Any]) -> tuple[str, str, st
|
|||||||
if (
|
if (
|
||||||
("synchronised" in value or "sweep_start_tag_timeout" in value)
|
("synchronised" in value or "sweep_start_tag_timeout" in value)
|
||||||
and not missing
|
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:
|
if missing or "tag" in value or "detection" in value or "synchronised" in value:
|
||||||
return "OBS-TAG-103", f"所需Tag不可用或持续丢失:{missing}", "检查Tag是否脱落、翘起、反光或被遮挡后重新运行。"
|
return "OBS-TAG-103", f"所需Tag不可用或持续丢失:{missing}", "检查Tag是否脱落、翘起、反光或被遮挡后重新运行。"
|
||||||
if "state" in value and ("timeout" in value or "lost" in value):
|
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", "稳态检查点采集流程未完整结束", "完整样本已保留;复制本诊断块给开发者检查采集状态机。"
|
return "FIT-CHECKPOINT-402", "稳态检查点采集流程未完整结束", "完整样本已保留;复制本诊断块给开发者检查采集状态机。"
|
||||||
if "fit" in value or "trajectory" in value or "axis" in value:
|
if "fit" in value or "trajectory" in value or "axis" in value:
|
||||||
return "FIT-MODEL-401", "关节轴或动态曲线拟合未达到精度门限", "不要放宽门限;复制本诊断块给开发者分析原始样本。"
|
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:
|
if "validation" in value or "combination" in value or "quality" in value or "zero" in value:
|
||||||
return "VAL-QUALITY-501", "留出验证或URDF零位验证未通过", "结果不会发布;复制本诊断块给开发者。"
|
return "VAL-QUALITY-501", "留出验证或URDF零位验证未通过", "结果不会发布;复制本诊断块给开发者。"
|
||||||
if "publish" in value or "artifact" in value or "URDF" in value:
|
if "publish" in value or "artifact" in value or "URDF" in value:
|
||||||
@@ -338,6 +433,20 @@ def build_failure_report(
|
|||||||
}
|
}
|
||||||
if group_pnp_reasons:
|
if group_pnp_reasons:
|
||||||
active["group_pnp_reasons"] = 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:
|
try:
|
||||||
explanation, automatic_action = three_camera_reason_zh(
|
explanation, automatic_action = three_camera_reason_zh(
|
||||||
str(status.get("state", "")), reason, active
|
str(status.get("state", "")), reason, active
|
||||||
@@ -359,8 +468,43 @@ def build_failure_report(
|
|||||||
metrics = {
|
metrics = {
|
||||||
"valid_frames": active.get("valid_frames"),
|
"valid_frames": active.get("valid_frames"),
|
||||||
"sample": active.get("sample", {}),
|
"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),
|
"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] = {
|
payload: dict[str, Any] = {
|
||||||
"schema_version": 1,
|
"schema_version": 1,
|
||||||
"serial_number": config.serial_number,
|
"serial_number": config.serial_number,
|
||||||
@@ -374,6 +518,7 @@ def build_failure_report(
|
|||||||
"automatic_action_zh": automatic_action,
|
"automatic_action_zh": automatic_action,
|
||||||
"suggestion_zh": suggestion,
|
"suggestion_zh": suggestion,
|
||||||
"metrics": metrics,
|
"metrics": metrics,
|
||||||
|
"pnp_diagnostics": pnp_diagnostics,
|
||||||
"camera_state": camera_state,
|
"camera_state": camera_state,
|
||||||
"feedback_hz": status.get("feedback_hz", 0.0),
|
"feedback_hz": status.get("feedback_hz", 0.0),
|
||||||
"hashes": {
|
"hashes": {
|
||||||
@@ -397,6 +542,12 @@ def build_failure_report(
|
|||||||
f"详细说明:{explanation}",
|
f"详细说明:{explanation}",
|
||||||
f"自动处理:{automatic_action}",
|
f"自动处理:{automatic_action}",
|
||||||
f"关键指标:{json.dumps(metrics, ensure_ascii=False, separators=(',', ':'))}",
|
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"相机状态:{json.dumps(camera_state, ensure_ascii=False, separators=(',', ':'))}",
|
||||||
f"反馈状态:{float(payload['feedback_hz'] or 0.0):.1f} Hz",
|
f"反馈状态:{float(payload['feedback_hz'] or 0.0):.1f} Hz",
|
||||||
f"配置哈希:{payload['hashes']['product_config_sha256']}",
|
f"配置哈希:{payload['hashes']['product_config_sha256']}",
|
||||||
|
|||||||
@@ -913,11 +913,15 @@ class SquareTagPoseTracker:
|
|||||||
self.last_candidates_by_role: dict[
|
self.last_candidates_by_role: dict[
|
||||||
str, tuple[SquareTagPose, ...]
|
str, tuple[SquareTagPose, ...]
|
||||||
] = {}
|
] = {}
|
||||||
|
self.last_candidate_diagnostics_by_role: dict[
|
||||||
|
str, dict[str, float | int]
|
||||||
|
] = {}
|
||||||
self.branch_correction_counts: dict[str, int] = {}
|
self.branch_correction_counts: dict[str, int] = {}
|
||||||
|
|
||||||
def reset(self) -> None:
|
def reset(self) -> None:
|
||||||
self._previous.clear()
|
self._previous.clear()
|
||||||
self.last_candidates_by_role.clear()
|
self.last_candidates_by_role.clear()
|
||||||
|
self.last_candidate_diagnostics_by_role.clear()
|
||||||
self.branch_correction_counts.clear()
|
self.branch_correction_counts.clear()
|
||||||
|
|
||||||
def estimate(
|
def estimate(
|
||||||
@@ -938,28 +942,91 @@ class SquareTagPoseTracker:
|
|||||||
)
|
)
|
||||||
except (ValueError, cv2.error):
|
except (ValueError, cv2.error):
|
||||||
self.last_candidates_by_role[str(role)] = ()
|
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"
|
return None, "pnp_solve_failed"
|
||||||
if not candidates:
|
if not candidates:
|
||||||
self.last_candidates_by_role[str(role)] = ()
|
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"
|
return None, "pnp_solve_failed"
|
||||||
usable_candidates: list[SquareTagPose] = []
|
reprojection_candidates = [
|
||||||
for candidate in 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(
|
normal = Rotation.from_quat(
|
||||||
candidate.quaternion_xyzw
|
candidate.quaternion_xyzw
|
||||||
).as_matrix()[:, 2]
|
).as_matrix()[:, 2]
|
||||||
tilt = math.acos(
|
tilt = math.acos(
|
||||||
float(np.clip(abs(normal[2]), 0.0, 1.0))
|
float(np.clip(abs(normal[2]), 0.0, 1.0))
|
||||||
)
|
)
|
||||||
if (
|
candidate_tilts_rad.append(float(tilt))
|
||||||
candidate.reprojection_error_px
|
if tilt <= self.maximum_tag_tilt_rad:
|
||||||
<= self.maximum_reprojection_error_px
|
independent_candidates.append(candidate)
|
||||||
and tilt <= self.maximum_tag_tilt_rad
|
|
||||||
):
|
# Candidate generation and candidate selection have different
|
||||||
usable_candidates.append(candidate)
|
# 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(
|
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"
|
return None, "no_pose_within_reprojection_or_tilt_limit"
|
||||||
|
|
||||||
previous_record = self._previous.get(str(role))
|
previous_record = self._previous.get(str(role))
|
||||||
@@ -971,7 +1038,7 @@ class SquareTagPoseTracker:
|
|||||||
previous = previous_pose
|
previous = previous_pose
|
||||||
|
|
||||||
selected, reason = select_continuous_pose(
|
selected, reason = select_continuous_pose(
|
||||||
usable_candidates,
|
independent_candidates,
|
||||||
previous=previous,
|
previous=previous,
|
||||||
maximum_reprojection_error_px=(
|
maximum_reprojection_error_px=(
|
||||||
self.maximum_reprojection_error_px
|
self.maximum_reprojection_error_px
|
||||||
@@ -1180,6 +1247,7 @@ class SquareTagGroupPoseTracker:
|
|||||||
self._task_reference_relative_poses: dict[
|
self._task_reference_relative_poses: dict[
|
||||||
tuple[str, str], tuple[Rotation, np.ndarray]
|
tuple[str, str], tuple[Rotation, np.ndarray]
|
||||||
] = {}
|
] = {}
|
||||||
|
self.last_missing_roles: tuple[str, ...] = ()
|
||||||
|
|
||||||
def reset(self, *, preserve_task_reference: bool = False) -> None:
|
def reset(self, *, preserve_task_reference: bool = False) -> None:
|
||||||
self._previous.clear()
|
self._previous.clear()
|
||||||
@@ -1190,6 +1258,7 @@ class SquareTagGroupPoseTracker:
|
|||||||
self.branch_correction_counts.clear()
|
self.branch_correction_counts.clear()
|
||||||
self._decreasing_relative_rotations.clear()
|
self._decreasing_relative_rotations.clear()
|
||||||
self._coupled_reference_rotations.clear()
|
self._coupled_reference_rotations.clear()
|
||||||
|
self.last_missing_roles = ()
|
||||||
if not preserve_task_reference:
|
if not preserve_task_reference:
|
||||||
self._task_reference_relative_poses.clear()
|
self._task_reference_relative_poses.clear()
|
||||||
|
|
||||||
@@ -1369,8 +1438,14 @@ class SquareTagGroupPoseTracker:
|
|||||||
tuple(candidates_by_role.get(role, ()))
|
tuple(candidates_by_role.get(role, ()))
|
||||||
for role in self.roles
|
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"
|
return None, "group_missing_pose_candidates"
|
||||||
|
self.last_missing_roles = ()
|
||||||
|
|
||||||
combinations = [
|
combinations = [
|
||||||
dict(zip(self.roles, combination))
|
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
|
from __future__ import annotations
|
||||||
|
|
||||||
@@ -11,11 +17,113 @@ from typing import Any, Mapping
|
|||||||
import yaml
|
import yaml
|
||||||
|
|
||||||
from .extrinsics import camera_info_fingerprint, load_three_camera_extrinsics
|
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")
|
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:
|
def sha256_file(path: str | Path) -> str:
|
||||||
digest = hashlib.sha256()
|
digest = hashlib.sha256()
|
||||||
with Path(path).open("rb") as stream:
|
with Path(path).open("rb") as stream:
|
||||||
@@ -104,6 +212,10 @@ def _custom_pnp_tag_sizes_m_by_id(
|
|||||||
class ProductConfig:
|
class ProductConfig:
|
||||||
path: Path
|
path: Path
|
||||||
workspace: Path
|
workspace: Path
|
||||||
|
model: str
|
||||||
|
side: str
|
||||||
|
tag_layout: str
|
||||||
|
calibration_contract: ProductCalibrationContract
|
||||||
serial_number: str
|
serial_number: str
|
||||||
can_interface: str
|
can_interface: str
|
||||||
source_urdf: Path
|
source_urdf: Path
|
||||||
@@ -130,7 +242,7 @@ def load_product_config(
|
|||||||
workspace: str | Path | None = None,
|
workspace: str | Path | None = None,
|
||||||
check_can: bool = True,
|
check_can: bool = True,
|
||||||
) -> ProductConfig:
|
) -> 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()
|
source = Path(path).expanduser().resolve()
|
||||||
if not source.is_file():
|
if not source.is_file():
|
||||||
raise ValueError(f"product config does not exist: {source}")
|
raise ValueError(f"product config does not exist: {source}")
|
||||||
@@ -142,8 +254,17 @@ def load_product_config(
|
|||||||
serial = str(raw.get("serial_number", ""))
|
serial = str(raw.get("serial_number", ""))
|
||||||
if re.fullmatch(r"[A-Za-z0-9_.-]+", serial) is None:
|
if re.fullmatch(r"[A-Za-z0-9_.-]+", serial) is None:
|
||||||
raise ValueError("serial_number is invalid")
|
raise ValueError("serial_number is invalid")
|
||||||
if raw.get("model") != "G20" or raw.get("side") != "right":
|
model = str(raw.get("model", "")).strip().upper()
|
||||||
raise ValueError("product config must describe the G20 right hand")
|
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()
|
can_interface = str(raw.get("can_interface", "")).strip()
|
||||||
if not can_interface:
|
if not can_interface:
|
||||||
raise ValueError("can_interface is required")
|
raise ValueError("can_interface is required")
|
||||||
@@ -197,16 +318,13 @@ def load_product_config(
|
|||||||
if actual != expected:
|
if actual != expected:
|
||||||
raise ValueError(f"{name} SHA-256 mismatch: expected={expected} actual={actual}")
|
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)
|
tag_sizes = _tag_sizes_m_by_id(tag_config)
|
||||||
if set(tag_sizes) != expected_tag_ids:
|
if set(tag_sizes) != expected_tag_ids:
|
||||||
raise ValueError(
|
raise ValueError(
|
||||||
"g20_right_19 Tag config must contain exactly IDs 0 through 18"
|
f"{model}/{side}/{layout} Tag IDs differ from the registered "
|
||||||
)
|
f"profile: expected={sorted(expected_tag_ids)} "
|
||||||
expected_tag_sizes = {tag_id: 0.016 for tag_id in expected_tag_ids}
|
f"actual={sorted(tag_sizes)}"
|
||||||
if tag_sizes != expected_tag_sizes:
|
|
||||||
raise ValueError(
|
|
||||||
"g20_right_19 black-code Tag sizes must be 16 mm for every ID"
|
|
||||||
)
|
)
|
||||||
if _custom_pnp_tag_sizes_m_by_id(
|
if _custom_pnp_tag_sizes_m_by_id(
|
||||||
calibration_config, expected_tag_ids
|
calibration_config, expected_tag_ids
|
||||||
@@ -216,12 +334,17 @@ def load_product_config(
|
|||||||
)
|
)
|
||||||
|
|
||||||
camera_raw = _mapping(raw.get("cameras"), "cameras")
|
camera_raw = _mapping(raw.get("cameras"), "cameras")
|
||||||
if set(camera_raw) != set(VIEWS):
|
required_views = tuple(contract.views)
|
||||||
raise ValueError("cameras must contain front/side/top")
|
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)
|
loaded_extrinsics = load_three_camera_extrinsics(extrinsics)
|
||||||
cameras: dict[str, dict[str, str]] = {}
|
cameras: dict[str, dict[str, str]] = {}
|
||||||
serials: set[str] = set()
|
serials: set[str] = set()
|
||||||
for view in VIEWS:
|
for view in required_views:
|
||||||
item = _mapping(camera_raw[view], f"cameras.{view}")
|
item = _mapping(camera_raw[view], f"cameras.{view}")
|
||||||
camera_serial = str(item.get("serial_number", ""))
|
camera_serial = str(item.get("serial_number", ""))
|
||||||
info_path = _resolve_path(
|
info_path = _resolve_path(
|
||||||
@@ -259,6 +382,10 @@ def load_product_config(
|
|||||||
return ProductConfig(
|
return ProductConfig(
|
||||||
path=source,
|
path=source,
|
||||||
workspace=root,
|
workspace=root,
|
||||||
|
model=model,
|
||||||
|
side=side,
|
||||||
|
tag_layout=layout,
|
||||||
|
calibration_contract=contract,
|
||||||
serial_number=serial,
|
serial_number=serial,
|
||||||
can_interface=can_interface,
|
can_interface=can_interface,
|
||||||
source_urdf=source_urdf,
|
source_urdf=source_urdf,
|
||||||
|
|||||||
@@ -24,7 +24,7 @@ from .full_hand import (
|
|||||||
from .product import ProductConfig, sha256_file
|
from .product import ProductConfig, sha256_file
|
||||||
from .storage import atomic_write_json
|
from .storage import atomic_write_json
|
||||||
from .urdf_zero import (
|
from .urdf_zero import (
|
||||||
RIGHT_19_FLEXION_ENDPOINT_JOINTS,
|
RIGHT_19_MECHANICAL_ENDPOINT_JOINTS,
|
||||||
get_zero_calibration_profile,
|
get_zero_calibration_profile,
|
||||||
)
|
)
|
||||||
|
|
||||||
@@ -41,6 +41,7 @@ ACTIVE_ZERO_JOINTS = frozenset(
|
|||||||
RETAINED_ACTIVE_ZERO_JOINTS = frozenset(
|
RETAINED_ACTIVE_ZERO_JOINTS = frozenset(
|
||||||
get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT).active_joints
|
get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT).active_joints
|
||||||
) - ACTIVE_ZERO_JOINTS
|
) - ACTIVE_ZERO_JOINTS
|
||||||
|
SCHEMA_V4_OFFSET_QUANTIZATION_TOLERANCE_RAD = 5.1e-9
|
||||||
|
|
||||||
|
|
||||||
def _load_json(path: Path) -> dict[str, Any]:
|
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:
|
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(
|
pattern = re.compile(
|
||||||
r"<joint\b[^>]*\bname\s*=\s*([\"'])(?P<name>[^\"']+)\1[^>]*>.*?</joint>",
|
r"<joint\b[^>]*\bname\s*=\s*([\"'])(?P<name>[^\"']+)\1[^>]*>.*?</joint>",
|
||||||
re.DOTALL,
|
re.DOTALL,
|
||||||
@@ -98,7 +99,7 @@ def _mask_endpoint_coordinate_fields(text: str) -> str:
|
|||||||
def replace(match: re.Match[str]) -> str:
|
def replace(match: re.Match[str]) -> str:
|
||||||
block = match.group(0)
|
block = match.group(0)
|
||||||
name = match.group("name")
|
name = match.group("name")
|
||||||
if name in RIGHT_19_FLEXION_ENDPOINT_JOINTS:
|
if name in RIGHT_19_MECHANICAL_ENDPOINT_JOINTS:
|
||||||
return re.sub(
|
return re.sub(
|
||||||
r"(<limit\b[^>]*\bupper\s*=\s*)([\"'])[^\"']*\2",
|
r"(<limit\b[^>]*\bupper\s*=\s*)([\"'])[^\"']*\2",
|
||||||
r"\1\2__CALIBRATED_UPPER__\2",
|
r"\1\2__CALIBRATED_UPPER__\2",
|
||||||
@@ -110,7 +111,10 @@ def _mask_endpoint_coordinate_fields(text: str) -> str:
|
|||||||
block,
|
block,
|
||||||
re.DOTALL,
|
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(
|
return re.sub(
|
||||||
r"(<mimic\b[^>]*\boffset\s*=\s*)([\"'])[^\"']*\2",
|
r"(<mimic\b[^>]*\boffset\s*=\s*)([\"'])[^\"']*\2",
|
||||||
r"\1\2__CALIBRATED_MIMIC_OFFSET__\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(
|
if set(offsets) != ACTIVE_ZERO_JOINTS or any(
|
||||||
not math.isfinite(value) for value in offsets.values()
|
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)
|
before = _joint_elements(source)
|
||||||
after = _joint_elements(corrected)
|
after = _joint_elements(corrected)
|
||||||
maximum_rotation_error = 0.0
|
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
|
# 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
|
# of one last-place unit. This is about 2.9e-7 degrees and is far below
|
||||||
# any calibration or URDF numerical significance.
|
# 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(
|
raise ValueError(
|
||||||
"corrected URDF origin.rpy does not match the published zero offsets"
|
"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")
|
original_limit = source_joints[name].find("limit")
|
||||||
corrected_limit = corrected_joints[name].find("limit")
|
corrected_limit = corrected_joints[name].find("limit")
|
||||||
expected_upper = float(original_limit.get("upper")) - offset
|
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")
|
raise ValueError(f"corrected URDF has invalid {name} upper limit")
|
||||||
for name in PASSIVE_JOINTS:
|
for name in PASSIVE_JOINTS:
|
||||||
original_mimic = source_joints[name].find("mimic")
|
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:
|
if original_mimic is None or corrected_mimic is None:
|
||||||
continue
|
continue
|
||||||
source_name = str(original_mimic.get("joint"))
|
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("offset", "0"))
|
||||||
expected += float(original_mimic.get("multiplier", "1")) * endpoint_offsets.get(source_name, 0.0)
|
expected += multiplier * endpoint_offsets.get(source_name, 0.0)
|
||||||
if abs(float(corrected_mimic.get("offset", "0")) - expected) > 1.0e-10:
|
# 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")
|
raise ValueError(f"corrected URDF has invalid {name} mimic offset")
|
||||||
return tuple(sorted(changed))
|
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(
|
def validate_runtime_curves_against_urdf_limits(
|
||||||
payload: Mapping[str, Any], source_urdf: str | Path
|
payload: Mapping[str, Any], runtime_urdf: str | Path
|
||||||
) -> None:
|
) -> 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)
|
validate_compact_payload(payload)
|
||||||
source_joints = _joint_elements(source_urdf)
|
runtime_joints = _joint_elements(runtime_urdf)
|
||||||
for name, calibration in payload["joints"].items():
|
for name, calibration in payload["joints"].items():
|
||||||
joint = source_joints.get(str(name))
|
joint = runtime_joints.get(str(name))
|
||||||
if joint is None:
|
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")
|
limit = joint.find("limit")
|
||||||
if limit is None or limit.get("lower") is None or limit.get("upper") is None:
|
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"))
|
lower = float(limit.get("lower"))
|
||||||
upper = float(limit.get("upper"))
|
upper = float(limit.get("upper"))
|
||||||
if not math.isfinite(lower) or not math.isfinite(upper) or lower > 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)
|
curve = np.asarray(calibration["angle_rad"], dtype=float)
|
||||||
minimum = float(np.min(curve))
|
minimum = float(np.min(curve))
|
||||||
maximum = float(np.max(curve))
|
maximum = float(np.max(curve))
|
||||||
tolerance = 1.0e-7
|
tolerance = 1.0e-7
|
||||||
if minimum < lower - tolerance or maximum > upper + tolerance:
|
if minimum < lower - tolerance or maximum > upper + tolerance:
|
||||||
raise ValueError(
|
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"[{minimum:.9g}, {maximum:.9g}] not within "
|
||||||
f"[{lower:.9g}, {upper:.9g}]"
|
f"[{lower:.9g}, {upper:.9g}]"
|
||||||
)
|
)
|
||||||
|
|
||||||
|
|
||||||
def clamp_compact_payload_to_urdf_limits(
|
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]]:
|
) -> 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))
|
result = copy.deepcopy(dict(payload))
|
||||||
validate_compact_payload(result)
|
validate_compact_payload(result)
|
||||||
source_joints = _joint_elements(source_urdf)
|
runtime_joints = _joint_elements(runtime_urdf)
|
||||||
clipped_by_joint: dict[str, int] = {}
|
clipped_by_joint: dict[str, int] = {}
|
||||||
for name, calibration in result["joints"].items():
|
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")
|
limit = None if joint is None else joint.find("limit")
|
||||||
if (
|
if (
|
||||||
limit is None
|
limit is None
|
||||||
or limit.get("lower") is None
|
or limit.get("lower") is None
|
||||||
or limit.get("upper") 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"))
|
lower = float(limit.get("lower"))
|
||||||
upper = float(limit.get("upper"))
|
upper = float(limit.get("upper"))
|
||||||
if not math.isfinite(lower) or not math.isfinite(upper) or lower > 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)
|
source = np.asarray(calibration["angle_rad"], dtype=float)
|
||||||
bounded = np.clip(source, lower, upper)
|
bounded = np.clip(source, lower, upper)
|
||||||
count = int(np.count_nonzero(bounded != source))
|
count = int(np.count_nonzero(bounded != source))
|
||||||
@@ -429,7 +457,10 @@ def compare_session_offsets(
|
|||||||
) -> dict[str, float]:
|
) -> dict[str, float]:
|
||||||
left = active_offsets(first)
|
left = active_offsets(first)
|
||||||
right = active_offsets(second)
|
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}
|
failed = {name: value for name, value in differences.items() if value > maximum_difference_rad}
|
||||||
if failed:
|
if failed:
|
||||||
details = ", ".join(
|
details = ", ".join(
|
||||||
@@ -549,14 +580,15 @@ def finalize_session_artifacts(
|
|||||||
raise ValueError("node status is missing combination validation")
|
raise ValueError("node status is missing combination validation")
|
||||||
_verify_combination_validation(combination)
|
_verify_combination_validation(combination)
|
||||||
offsets = active_offsets(payload)
|
offsets = active_offsets(payload)
|
||||||
|
endpoint_offsets = {
|
||||||
|
name: offsets[name]
|
||||||
|
for name in RIGHT_19_MECHANICAL_ENDPOINT_JOINTS
|
||||||
|
}
|
||||||
changed_joints = verify_corrected_urdf(
|
changed_joints = verify_corrected_urdf(
|
||||||
config.source_urdf,
|
config.source_urdf,
|
||||||
paths["urdf"],
|
paths["urdf"],
|
||||||
expected_offsets_rad=offsets,
|
expected_offsets_rad=offsets,
|
||||||
endpoint_anchored_offsets_rad={
|
endpoint_anchored_offsets_rad=endpoint_offsets,
|
||||||
name: offsets[name]
|
|
||||||
for name in RIGHT_19_FLEXION_ENDPOINT_JOINTS
|
|
||||||
},
|
|
||||||
)
|
)
|
||||||
payload, clipped_runtime_joints = clamp_compact_payload_to_urdf_limits(
|
payload, clipped_runtime_joints = clamp_compact_payload_to_urdf_limits(
|
||||||
payload, paths["urdf"]
|
payload, paths["urdf"]
|
||||||
@@ -628,8 +660,13 @@ def finalize_session_artifacts(
|
|||||||
config.source_urdf,
|
config.source_urdf,
|
||||||
paths["urdf"],
|
paths["urdf"],
|
||||||
expected_offsets_rad=offsets,
|
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 = {
|
final_mesh_hashes = {
|
||||||
name: sha256_file(path)
|
name: sha256_file(path)
|
||||||
for name, path in verify_urdf_mesh_resources(paths["urdf"]).items()
|
for name, path in verify_urdf_mesh_resources(paths["urdf"]).items()
|
||||||
|
|||||||
+200
-18
@@ -3,7 +3,7 @@
|
|||||||
from __future__ import annotations
|
from __future__ import annotations
|
||||||
|
|
||||||
import re
|
import re
|
||||||
from typing import Any, Mapping
|
from typing import Any, Mapping, Sequence
|
||||||
|
|
||||||
|
|
||||||
STATE_NAMES_ZH = {
|
STATE_NAMES_ZH = {
|
||||||
@@ -115,10 +115,13 @@ def _task_text(active: Mapping[str, Any]) -> str:
|
|||||||
)
|
)
|
||||||
fit_attempt = int(active.get("fit_attempt", 1))
|
fit_attempt = int(active.get("fit_attempt", 1))
|
||||||
if fit_attempt > 1:
|
if fit_attempt > 1:
|
||||||
task += (
|
retry_cycles = active.get("fit_retry_cycles", [])
|
||||||
f"(整关节自动重采第{fit_attempt}/"
|
if retry_cycles:
|
||||||
f"{active.get('fit_attempt_limit', '?')}次)"
|
task += "(补采异常轮" + "/".join(
|
||||||
)
|
str(cycle) for cycle in retry_cycles
|
||||||
|
) + ")"
|
||||||
|
else:
|
||||||
|
task += f"(拟合补采第{fit_attempt}次)"
|
||||||
return task
|
return task
|
||||||
|
|
||||||
|
|
||||||
@@ -240,14 +243,87 @@ def three_camera_reason_zh(
|
|||||||
if reason == "synchronised_tag_state_timeout":
|
if reason == "synchronised_tag_state_timeout":
|
||||||
group_reasons = active.get("group_pnp_reasons", {})
|
group_reasons = active.get("group_pnp_reasons", {})
|
||||||
if isinstance(group_reasons, Mapping) and group_reasons:
|
if isinstance(group_reasons, Mapping) and group_reasons:
|
||||||
reason_text = "、".join(
|
tag_rejections = active.get("pnp_rejection_counts", {})
|
||||||
f"{VIEW_NAMES_ZH.get(str(view), str(view))}={value}"
|
group_rejections = active.get(
|
||||||
for view, value in group_reasons.items()
|
"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 (
|
return (
|
||||||
f"{detail_prefix}Tag仍可见且反馈正常,但连续图像帧被整组PnP几何检查拒绝"
|
f"{detail_prefix}已经取得部分有效轨迹,但Tag仍可见且反馈正常时,"
|
||||||
|
"后续连续图像帧"
|
||||||
|
"被整组PnP几何检查拒绝"
|
||||||
f"({reason_text}),因此无法与电机状态形成有效轨迹帧。",
|
f"({reason_text}),因此无法与电机状态形成有效轨迹帧。",
|
||||||
"不要调整或反复粘贴Tag;保留当前会话并复制诊断块给开发者检查PnP分支逻辑。",
|
"不要调整或反复粘贴Tag;保留当前会话中的"
|
||||||
|
"group_pnp_candidate_event,"
|
||||||
|
"按缺失角色的候选统计检查PnP分支逻辑。",
|
||||||
)
|
)
|
||||||
return (
|
return (
|
||||||
f"{detail_prefix}运动过程中连续超过允许时间没有取得“所需Tag全部有效且能与电机状态"
|
f"{detail_prefix}运动过程中连续超过允许时间没有取得“所需Tag全部有效且能与电机状态"
|
||||||
@@ -273,15 +349,81 @@ def three_camera_reason_zh(
|
|||||||
)
|
)
|
||||||
if reason == "sweep_start_tag_timeout":
|
if reason == "sweep_start_tag_timeout":
|
||||||
group_reasons = active.get("group_pnp_reasons", {})
|
group_reasons = active.get("group_pnp_reasons", {})
|
||||||
if isinstance(group_reasons, Mapping) and group_reasons:
|
progress_by_view = active.get("pnp_initialization_progress", {})
|
||||||
reason_text = "、".join(
|
tag_rejections = active.get("pnp_rejection_counts", {})
|
||||||
f"{VIEW_NAMES_ZH.get(str(view), str(view))}={value}"
|
group_rejections = active.get("group_pnp_rejection_counts", {})
|
||||||
for view, value in group_reasons.items()
|
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 (
|
return (
|
||||||
"被测电机已经到达扫描起点,所需Tag也可见,但整组PnP初始化持续"
|
"被测电机已经到达扫描起点,所需Tag也可见,但三维PnP位姿初始化"
|
||||||
f"拒绝候选({reason_text}),因此没有生成同步端点帧。",
|
f"没有完成({reason_text}),因此没有生成同步端点帧。",
|
||||||
"不要重新粘贴可见Tag;保留当前会话并复制诊断块给开发者检查PnP安装先验。",
|
"不要根据可见性重复粘贴Tag;保留累计拒绝原因并检查PnP候选选择。",
|
||||||
)
|
)
|
||||||
return (
|
return (
|
||||||
"被测电机已经到达扫描起点,但当前任务所需的实时运动Tag没有形成足够的"
|
"被测电机已经到达扫描起点,但当前任务所需的实时运动Tag没有形成足够的"
|
||||||
@@ -308,6 +450,19 @@ def three_camera_reason_zh(
|
|||||||
"随机复测位置没有采集到足够的同步有效Tag帧。",
|
"随机复测位置没有采集到足够的同步有效Tag帧。",
|
||||||
"检查当前机位Tag可见性后调用resume。",
|
"检查当前机位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"}:
|
if reason in {"joint_fit_check_failed", "joint_fit_systematic_failure"}:
|
||||||
metric_names = {
|
metric_names = {
|
||||||
"plane_rms_mm": "平面拟合RMS",
|
"plane_rms_mm": "平面拟合RMS",
|
||||||
@@ -327,8 +482,10 @@ def three_camera_reason_zh(
|
|||||||
"axis_plane_rms_mm": "三维圆轴向RMS",
|
"axis_plane_rms_mm": "三维圆轴向RMS",
|
||||||
"axis_radial_rms_mm": "三维圆半径RMS",
|
"axis_radial_rms_mm": "三维圆半径RMS",
|
||||||
"axis_pose_line_rms_mm": "姿态轨迹轴线RMS",
|
"axis_pose_line_rms_mm": "姿态轨迹轴线RMS",
|
||||||
|
"axis_line_cycle_rms_mm": "四轮轴线位置RMS",
|
||||||
"rotation_circle_axis_difference_deg": "姿态轴与圆轨迹轴夹角",
|
"rotation_circle_axis_difference_deg": "姿态轴与圆轨迹轴夹角",
|
||||||
"axis_cycle_difference_deg": "各轮转轴方向极差",
|
"axis_cycle_difference_deg": "各轮转轴方向极差",
|
||||||
|
"cross_view_roll_curve": "正面/侧面关节角曲线差异RMS",
|
||||||
"third_cycle_axis_holdout_deg": "第三轮留出零位可观测轴向误差",
|
"third_cycle_axis_holdout_deg": "第三轮留出零位可观测轴向误差",
|
||||||
"third_cycle_axis_line_rms_mm": "第三轮留出轴线RMS",
|
"third_cycle_axis_line_rms_mm": "第三轮留出轴线RMS",
|
||||||
"third_cycle_trajectory_p95_deg": "第三轮留出轨迹误差P95",
|
"third_cycle_trajectory_p95_deg": "第三轮留出轨迹误差P95",
|
||||||
@@ -353,6 +510,7 @@ def three_camera_reason_zh(
|
|||||||
"axis_plane_rms_mm": "mm",
|
"axis_plane_rms_mm": "mm",
|
||||||
"axis_radial_rms_mm": "mm",
|
"axis_radial_rms_mm": "mm",
|
||||||
"axis_pose_line_rms_mm": "mm",
|
"axis_pose_line_rms_mm": "mm",
|
||||||
|
"axis_line_cycle_rms_mm": "mm",
|
||||||
"rotation_circle_axis_difference_deg": "°",
|
"rotation_circle_axis_difference_deg": "°",
|
||||||
"axis_cycle_difference_deg": "°",
|
"axis_cycle_difference_deg": "°",
|
||||||
"third_cycle_axis_holdout_deg": "°",
|
"third_cycle_axis_holdout_deg": "°",
|
||||||
@@ -360,6 +518,7 @@ def three_camera_reason_zh(
|
|||||||
"third_cycle_trajectory_p95_deg": "°",
|
"third_cycle_trajectory_p95_deg": "°",
|
||||||
"state_image_sync_p95_ms": "ms",
|
"state_image_sync_p95_ms": "ms",
|
||||||
"tag_valid_rate_percent": "%",
|
"tag_valid_rate_percent": "%",
|
||||||
|
"cross_view_roll_curve": "°",
|
||||||
}
|
}
|
||||||
details: list[str] = []
|
details: list[str] = []
|
||||||
for failure in active.get("failures", []):
|
for failure in active.get("failures", []):
|
||||||
@@ -402,8 +561,19 @@ def three_camera_reason_zh(
|
|||||||
for failure in active.get("failures", [])
|
for failure in active.get("failures", [])
|
||||||
)
|
)
|
||||||
if reason == "joint_fit_systematic_failure":
|
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 = (
|
suggestion = (
|
||||||
"各轮重复出现同一模型冲突,继续运动不会改善;程序已禁止自动重扫。"
|
"四轮都出现稳定的正面/侧面差异,属于Tag安装外参或跨视角模型偏差,"
|
||||||
|
"继续重扫不会改善;检查Tag刚性安装与跨视角安装变换,不要放宽门限。"
|
||||||
|
if cross_view_systematic
|
||||||
|
else "各轮重复出现同一模型冲突,继续运动不会改善;程序已禁止自动重扫。"
|
||||||
"请直接复制诊断块给开发者,不要放宽门限。"
|
"请直接复制诊断块给开发者,不要放宽门限。"
|
||||||
)
|
)
|
||||||
else:
|
else:
|
||||||
@@ -543,6 +713,18 @@ def three_camera_reason_zh(
|
|||||||
return "三维轨迹、关节轴零位和第三轮留出验证已经完成。", "检查JSON、修正URDF路径和quality.passed。"
|
return "三维轨迹、关节轴零位和第三轮留出验证已经完成。", "检查JSON、修正URDF路径和quality.passed。"
|
||||||
if reason == "quality_failed":
|
if reason == "quality_failed":
|
||||||
return "标定流程完成,但拟合或随机复测质量没有达到验收阈值。", "检查最终JSON的quality以及启动终端中的拟合日志。"
|
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":
|
if reason == "combination_pose_prediction_failed":
|
||||||
return (
|
return (
|
||||||
"单关节、零位和URDF几何验证已通过,但当前多关节组合姿态的Tag实测位姿与模型预测超过门限。",
|
"单关节、零位和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):
|
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()
|
hand_type = LaunchConfiguration("hand_type").perform(context).lower()
|
||||||
if hand_type not in {"left", "right"}:
|
if hand_type not in {"left", "right"}:
|
||||||
raise RuntimeError("hand_type must be left or right")
|
raise RuntimeError("hand_type must be left or right")
|
||||||
tag_layout = LaunchConfiguration("tag_layout").perform(context).lower()
|
tag_layout = LaunchConfiguration("tag_layout").perform(context).lower()
|
||||||
if tag_layout not in {"legacy_11", "g20_right_15", "g20_right_19"}:
|
try:
|
||||||
raise RuntimeError(
|
contract = get_product_calibration_contract(
|
||||||
"tag_layout must be legacy_11, g20_right_15 or g20_right_19"
|
model, hand_type, tag_layout
|
||||||
)
|
)
|
||||||
if tag_layout in {"g20_right_15", "g20_right_19"} and hand_type != "right":
|
except ValueError as error:
|
||||||
raise RuntimeError("G20 right product layout requires hand_type:=right")
|
raise RuntimeError(str(error)) from error
|
||||||
requested_tag_config = LaunchConfiguration("tag_config").perform(context)
|
requested_tag_config = LaunchConfiguration("tag_config").perform(context)
|
||||||
package_share = Path(
|
package_share = Path(
|
||||||
get_package_share_directory("g20_thumb_apriltag_calibration")
|
get_package_share_directory("g20_thumb_apriltag_calibration")
|
||||||
@@ -68,9 +73,10 @@ def _launch_stack(context):
|
|||||||
)
|
)
|
||||||
if not tag_config.is_file():
|
if not tag_config.is_file():
|
||||||
raise RuntimeError(f"tag config does not exist: {tag_config}")
|
raise RuntimeError(f"tag config does not exist: {tag_config}")
|
||||||
command_topic = f"/g20/cb_{hand_type}_hand_control_cmd"
|
topic_prefix = f"/{model.lower()}"
|
||||||
state_topic = f"/g20/cb_{hand_type}_hand_state"
|
command_topic = f"{topic_prefix}/cb_{hand_type}_hand_control_cmd"
|
||||||
info_topic = f"/g20/cb_{hand_type}_hand_info"
|
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)
|
requested_source = LaunchConfiguration("source_urdf_path").perform(context)
|
||||||
source_urdf = (
|
source_urdf = (
|
||||||
Path(requested_source).expanduser().resolve()
|
Path(requested_source).expanduser().resolve()
|
||||||
@@ -82,7 +88,7 @@ def _launch_stack(context):
|
|||||||
expected_source_hash = LaunchConfiguration(
|
expected_source_hash = LaunchConfiguration(
|
||||||
"source_urdf_expected_sha256"
|
"source_urdf_expected_sha256"
|
||||||
).perform(context).strip().lower()
|
).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:
|
if re.fullmatch(r"[0-9a-f]{64}", expected_source_hash) is None:
|
||||||
raise RuntimeError(
|
raise RuntimeError(
|
||||||
"G20 right product layout requires source_urdf_expected_sha256 confirmed "
|
"G20 right product layout requires source_urdf_expected_sha256 confirmed "
|
||||||
@@ -239,10 +245,10 @@ def _launch_stack(context):
|
|||||||
parameters=[
|
parameters=[
|
||||||
{
|
{
|
||||||
"hand_type": hand_type,
|
"hand_type": hand_type,
|
||||||
"hand_joint": "G20",
|
"hand_joint": model,
|
||||||
"can": LaunchConfiguration("can_interface"),
|
"can": LaunchConfiguration("can_interface"),
|
||||||
"modbus": "None",
|
"modbus": "None",
|
||||||
"topic_prefix": "/g20",
|
"topic_prefix": topic_prefix,
|
||||||
"move_on_startup": False,
|
"move_on_startup": False,
|
||||||
"startup_speed": ParameterValue(
|
"startup_speed": ParameterValue(
|
||||||
LaunchConfiguration("calibration_speed"), value_type=int
|
LaunchConfiguration("calibration_speed"), value_type=int
|
||||||
@@ -274,6 +280,7 @@ def _launch_stack(context):
|
|||||||
LaunchConfiguration("calibration_config"),
|
LaunchConfiguration("calibration_config"),
|
||||||
{
|
{
|
||||||
"serial_number": hand_serial,
|
"serial_number": hand_serial,
|
||||||
|
"model": model,
|
||||||
"hand_type": hand_type,
|
"hand_type": hand_type,
|
||||||
"tag_layout": tag_layout,
|
"tag_layout": tag_layout,
|
||||||
"session_dir": str(session_dir),
|
"session_dir": str(session_dir),
|
||||||
@@ -396,6 +403,7 @@ def generate_launch_description() -> LaunchDescription:
|
|||||||
name="FASTRTPS_DEFAULT_PROFILES_FILE",
|
name="FASTRTPS_DEFAULT_PROFILES_FILE",
|
||||||
value=str(package_share / "config" / "fastdds_large_images.xml"),
|
value=str(package_share / "config" / "fastdds_large_images.xml"),
|
||||||
),
|
),
|
||||||
|
DeclareLaunchArgument("model", default_value="G20"),
|
||||||
DeclareLaunchArgument("hand_type", default_value="left"),
|
DeclareLaunchArgument("hand_type", default_value="left"),
|
||||||
DeclareLaunchArgument("tag_layout", default_value="legacy_11"),
|
DeclareLaunchArgument("tag_layout", default_value="legacy_11"),
|
||||||
DeclareLaunchArgument("serial_number", default_value="UNSET"),
|
DeclareLaunchArgument("serial_number", default_value="UNSET"),
|
||||||
|
|||||||
@@ -26,7 +26,7 @@ setup(
|
|||||||
zip_safe=True,
|
zip_safe=True,
|
||||||
maintainer="lxp",
|
maintainer="lxp",
|
||||||
maintainer_email="support@linker-robotics.com",
|
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",
|
license="MIT",
|
||||||
entry_points={
|
entry_points={
|
||||||
"console_scripts": [
|
"console_scripts": [
|
||||||
@@ -68,6 +68,10 @@ setup(
|
|||||||
"calibrate_g20_right = "
|
"calibrate_g20_right = "
|
||||||
"g20_thumb_apriltag_calibration.one_command:main"
|
"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_observability_condition_number"] >= 1.0
|
||||||
assert parameters["zero_maximum_offset_deg"] <= 20.0
|
assert parameters["zero_maximum_offset_deg"] <= 20.0
|
||||||
assert parameters["zero_finger_maximum_offset_deg"] <= 3.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_rms_px"] <= 2.0
|
||||||
assert parameters["image_trajectory_maximum_radial_p95_px"] <= 3.5
|
assert parameters["image_trajectory_maximum_radial_p95_px"] <= 3.5
|
||||||
assert parameters["image_trajectory_minimum_radius_px"] >= 20.0
|
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,
|
clamp_runtime_fits_to_urdf_limits,
|
||||||
center_splay_curve,
|
center_splay_curve,
|
||||||
compare_cross_view_roll_curves,
|
compare_cross_view_roll_curves,
|
||||||
|
cross_view_roll_diagnostic_metrics,
|
||||||
derive_mimic_passive_fits,
|
derive_mimic_passive_fits,
|
||||||
fit_joint_center_curve,
|
fit_joint_center_curve,
|
||||||
fit_joint_image_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",
|
||||||
"pinky_mcp_roll_side",
|
"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 profile.sweep_specs[6].joints == ("pinky_pip", "pinky_dip")
|
||||||
assert {
|
assert {
|
||||||
name: spec.source_joint
|
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)
|
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]]:
|
def _records() -> list[dict[str, object]]:
|
||||||
commands = list(range(0, 256, 16))
|
commands = list(range(0, 256, 16))
|
||||||
if commands[-1] != 255:
|
if commands[-1] != 255:
|
||||||
|
|||||||
@@ -4,6 +4,7 @@ import json
|
|||||||
import math
|
import math
|
||||||
from pathlib import Path
|
from pathlib import Path
|
||||||
from types import SimpleNamespace
|
from types import SimpleNamespace
|
||||||
|
from xml.etree import ElementTree as ET
|
||||||
|
|
||||||
import numpy as np
|
import numpy as np
|
||||||
import pytest
|
import pytest
|
||||||
@@ -39,6 +40,7 @@ from g20_thumb_apriltag_calibration.publication import (
|
|||||||
atomic_session_pointer,
|
atomic_session_pointer,
|
||||||
build_mujoco_validation_commands,
|
build_mujoco_validation_commands,
|
||||||
finalize_session_artifacts,
|
finalize_session_artifacts,
|
||||||
|
session_artifact_paths,
|
||||||
validate_runtime_curves_against_urdf_limits,
|
validate_runtime_curves_against_urdf_limits,
|
||||||
verify_corrected_urdf,
|
verify_corrected_urdf,
|
||||||
verify_urdf_mesh_resources,
|
verify_urdf_mesh_resources,
|
||||||
@@ -52,6 +54,8 @@ from g20_thumb_apriltag_calibration.three_camera_node import (
|
|||||||
_combination_motor_directions,
|
_combination_motor_directions,
|
||||||
_combination_validation_items,
|
_combination_validation_items,
|
||||||
_model_link_in_observer_base,
|
_model_link_in_observer_base,
|
||||||
|
_palm_axis_observer_schema,
|
||||||
|
_palm_axis_resume_policy,
|
||||||
_steady_checkpoint_commands,
|
_steady_checkpoint_commands,
|
||||||
_unresolved_fit_failure_tasks,
|
_unresolved_fit_failure_tasks,
|
||||||
combination_target_coverage,
|
combination_target_coverage,
|
||||||
@@ -59,7 +63,7 @@ from g20_thumb_apriltag_calibration.three_camera_node import (
|
|||||||
resumable_completed_task_prefix,
|
resumable_completed_task_prefix,
|
||||||
)
|
)
|
||||||
from g20_thumb_apriltag_calibration.urdf_zero import (
|
from g20_thumb_apriltag_calibration.urdf_zero import (
|
||||||
RIGHT_19_FLEXION_ENDPOINT_JOINTS,
|
RIGHT_19_MECHANICAL_ENDPOINT_JOINTS,
|
||||||
UrdfKinematicModel,
|
UrdfKinematicModel,
|
||||||
get_zero_calibration_profile,
|
get_zero_calibration_profile,
|
||||||
write_zero_corrected_urdf,
|
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)
|
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 = [255] * 20
|
||||||
baseline[6:10] = [127] * 4
|
baseline[6:10] = [127] * 4
|
||||||
fits = {}
|
fits = {}
|
||||||
@@ -100,7 +111,7 @@ def _payload(serial: str = "G20_RIGHT_001") -> dict:
|
|||||||
return build_compact_payload(
|
return build_compact_payload(
|
||||||
serial_number=serial,
|
serial_number=serial,
|
||||||
measured_fits=fits,
|
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)],
|
validation_errors_rad=[0.0, math.radians(0.5)],
|
||||||
passed=True,
|
passed=True,
|
||||||
baseline=baseline,
|
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 = config.session_root / stamp
|
||||||
session.mkdir(parents=True)
|
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(
|
atomic_write_json(
|
||||||
session / f"g20_right_{config.serial_number}_calibration.json", payload
|
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,
|
source_urdf=config.source_urdf,
|
||||||
output_directory=session,
|
output_directory=session,
|
||||||
serial_number=config.serial_number,
|
serial_number=config.serial_number,
|
||||||
offsets_rad={name: 0.01 for name in ACTIVE_ZERO_JOINTS},
|
offsets_rad=offsets,
|
||||||
endpoint_anchored_offsets_rad={
|
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,
|
timestamp=stamp,
|
||||||
)
|
)
|
||||||
@@ -156,6 +176,18 @@ def _passed_node_status() -> dict:
|
|||||||
|
|
||||||
def test_product_config_locks_three_cameras_tags_and_artifact_hashes() -> None:
|
def test_product_config_locks_three_cameras_tags_and_artifact_hashes() -> None:
|
||||||
config = load_product_config(PRODUCT, workspace=REPO, check_can=False)
|
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.serial_number == "G20_RIGHT_001"
|
||||||
assert config.required_independent_passes == 1
|
assert config.required_independent_passes == 1
|
||||||
assert set(config.cameras) == {"front", "side", "top"}
|
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}
|
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:
|
def test_sparse_resume_keeps_complete_tasks_after_failed_task() -> None:
|
||||||
profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT)
|
profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT)
|
||||||
first, failed, later = profile.sweep_specs[:3]
|
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()
|
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:
|
def test_automatic_resume_requires_failed_matching_geometry(tmp_path: Path) -> None:
|
||||||
config = _config(tmp_path)
|
config = _config(tmp_path)
|
||||||
session = config.session_root / "20260819_150000"
|
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]),
|
*_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(
|
source_raw.write_text(
|
||||||
"".join(json.dumps(row) + "\n" for row in rows)
|
"".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 count == 1
|
||||||
assert node.sweep_index == 10
|
assert node.sweep_index == 10
|
||||||
assert node.resumed_task_keys == (profile.sweep_specs[0].key,)
|
assert node.resumed_task_keys == (profile.sweep_specs[0].key,)
|
||||||
|
assert node.sweep_attempts[profile.sweep_specs[0].key] == 2
|
||||||
restored = [
|
restored = [
|
||||||
json.loads(line) for line in current_raw.read_text().splitlines()
|
json.loads(line) for line in current_raw.read_text().splitlines()
|
||||||
]
|
]
|
||||||
assert restored[0]["kind"] == "resume_checkpoint_import"
|
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)
|
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)
|
payload = _payload(config.serial_number)
|
||||||
validate_runtime_curves_against_urdf_limits(payload, config.source_urdf)
|
validate_runtime_curves_against_urdf_limits(payload, config.source_urdf)
|
||||||
payload["joints"]["thumb_mcp"]["angle_rad"][0] = 2.0
|
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)
|
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:
|
def test_publication_rejects_non_joint_urdf_changes(tmp_path: Path) -> None:
|
||||||
config = _config(tmp_path, passes=1)
|
config = _config(tmp_path, passes=1)
|
||||||
session = _make_passed_session(config, "20260819_112000")
|
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
|
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,
|
tmp_path: Path,
|
||||||
) -> None:
|
) -> None:
|
||||||
config = _config(tmp_path, passes=1)
|
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"))
|
published = json.loads(json_path.read_text(encoding="utf-8"))
|
||||||
|
|
||||||
assert release_ready is True
|
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 published["joints"]["thumb_cmc_yaw"]["angle_rad"][0] == 1.57
|
||||||
assert summary["runtime_limit_clipped_bins"]["thumb_cmc_yaw"] == 1
|
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": {
|
"front": {
|
||||||
"ready": True,
|
"ready": True,
|
||||||
"missing_tag_ids": [],
|
"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": []},
|
"side": {"ready": True, "missing_tag_ids": []},
|
||||||
"top": {"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 payload["error_code"] == "CAM-GEOMETRY-201"
|
||||||
|
assert "轨迹在运动中失效" in payload["problem_zh"]
|
||||||
assert "整组PnP几何检查拒绝" in payload["explanation_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"]
|
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": {
|
"side": {
|
||||||
"ready": True,
|
"ready": True,
|
||||||
"missing_tag_ids": [],
|
"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": []},
|
"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 payload["error_code"] == "CAM-GEOMETRY-201"
|
||||||
assert "PnP" in payload["problem_zh"]
|
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:
|
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
|
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:
|
def test_fitting_uses_long_watchdog_without_weakening_motion_timeout() -> None:
|
||||||
assert _status_timeout_seconds({"state": "FITTING"}) == 600.0
|
assert _status_timeout_seconds({"state": "FITTING"}) == 600.0
|
||||||
assert _status_timeout_seconds({"state": "SWEEP"}) == 90.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,
|
"actual_u8": 116.5,
|
||||||
"valid_frames": 80,
|
"valid_frames": 80,
|
||||||
"automatic_retry_count": 1,
|
"automatic_retry_count": 1,
|
||||||
"fit_attempt": 3,
|
"fit_attempt": 3,
|
||||||
"fit_attempt_limit": 3,
|
"fit_attempt_limit": 3,
|
||||||
|
"fit_retry_cycles": [2],
|
||||||
},
|
},
|
||||||
"views": {
|
"views": {
|
||||||
view: {
|
view: {
|
||||||
@@ -1136,7 +1411,7 @@ def test_progress_contains_stage_eta_tags_cameras_and_feedback() -> None:
|
|||||||
assert "反馈:98.0 Hz" in text
|
assert "反馈:98.0 Hz" in text
|
||||||
assert "当前任务ID:正面[0✓] 侧面[0✓] 顶部[0✓]" in text
|
assert "当前任务ID:正面[0✓] 侧面[0✓] 顶部[0✓]" in text
|
||||||
assert "断点:已恢复 3/16 个完整任务" 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:
|
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)
|
"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 "正面[0✓]" in text
|
||||||
assert "侧面[4✓,5✗,14✓]" in text
|
assert "侧面[4✓,5✗,14✓]" in text
|
||||||
assert "顶部[8✓]" 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],
|
"required_tag_ids": [4, 15],
|
||||||
"locked_reference_tag_ids": [],
|
"locked_reference_tag_ids": [],
|
||||||
"detected_tag_ids": [4, 15],
|
"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": {
|
"top": {
|
||||||
"camera_info_valid": True,
|
"camera_info_valid": True,
|
||||||
@@ -1271,9 +1554,11 @@ def test_multiview_progress_marks_occluded_front_base_as_locked() -> None:
|
|||||||
|
|
||||||
assert "已到扫描起点,等待任务Tag" in text
|
assert "已到扫描起点,等待任务Tag" in text
|
||||||
assert "命令/反馈:255/254.0" 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 "正面[0锁,12✓]" in text
|
||||||
assert "预计剩余 等待Tag" 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:
|
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 (
|
from g20_thumb_apriltag_calibration.offline_replay import (
|
||||||
_latest_attempt_records,
|
_latest_attempt_records,
|
||||||
|
_latest_palm_axis_records,
|
||||||
_output_suffix,
|
_output_suffix,
|
||||||
)
|
)
|
||||||
|
|
||||||
@@ -36,6 +37,30 @@ def test_latest_attempt_is_selected_per_joint_cycle_and_direction() -> None:
|
|||||||
] == [1]
|
] == [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:
|
def test_output_suffix_is_safe_and_explicit() -> None:
|
||||||
assert _output_suffix(None) == ""
|
assert _output_suffix(None) == ""
|
||||||
assert _output_suffix("MEASURED_ZERO_V2") == "_MEASURED_ZERO_V2"
|
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:
|
def test_group_tracker_uses_coupling_to_choose_branch_but_never_rejects_measurement() -> None:
|
||||||
tracker = SquareTagGroupPoseTracker(
|
tracker = SquareTagGroupPoseTracker(
|
||||||
roles=("base", "mcp", "ip"),
|
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
|
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:
|
def test_combination_prediction_failure_is_not_reported_as_unclassified() -> None:
|
||||||
explanation, action = three_camera_reason_zh(
|
explanation, action = three_camera_reason_zh(
|
||||||
"PAUSED", "combination_pose_prediction_failed", {}
|
"PAUSED", "combination_pose_prediction_failed", {}
|
||||||
@@ -91,14 +132,58 @@ def test_sweep_start_visible_tags_reports_group_pnp_rejection() -> None:
|
|||||||
"sweep_start_tag_timeout",
|
"sweep_start_tag_timeout",
|
||||||
{
|
{
|
||||||
"group_pnp_reasons": {
|
"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 "所需Tag也可见" in explanation
|
||||||
assert "group_normal_alignment" in explanation
|
assert "初始化5/8" in explanation
|
||||||
assert "不要重新粘贴" in action
|
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:
|
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
|
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:
|
def test_baseline_hysteresis_failure_shows_values_instead_of_unknown() -> None:
|
||||||
payload = {
|
payload = {
|
||||||
"state": "PAUSED",
|
"state": "PAUSED",
|
||||||
@@ -297,7 +436,7 @@ def test_zero_model_failure_explains_that_rescan_will_not_help() -> None:
|
|||||||
assert "运动采样:" not in text
|
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 = {
|
active = {
|
||||||
"kind": "sweep",
|
"kind": "sweep",
|
||||||
"view": "front",
|
"view": "front",
|
||||||
@@ -309,9 +448,10 @@ def test_sweep_status_shows_full_joint_fit_retry_attempt() -> None:
|
|||||||
"target_u8": 0,
|
"target_u8": 0,
|
||||||
"fit_attempt": 2,
|
"fit_attempt": 2,
|
||||||
"fit_attempt_limit": 3,
|
"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:
|
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,
|
DIRECT_ZERO_JOINTS,
|
||||||
INHERITED_ZERO_JOINTS,
|
INHERITED_ZERO_JOINTS,
|
||||||
JointAxisMeasurement,
|
JointAxisMeasurement,
|
||||||
|
PalmOrientationMeasurement,
|
||||||
UrdfKinematicModel,
|
UrdfKinematicModel,
|
||||||
_angles_from_state,
|
_angles_from_state,
|
||||||
_zero_sensitive_axis_error_rad,
|
_zero_sensitive_axis_error_rad,
|
||||||
|
anchor_right_19_mechanical_endpoint_curves,
|
||||||
baseline_hysteresis_by_cycle_rad,
|
baseline_hysteresis_by_cycle_rad,
|
||||||
derive_right_19_flexion_endpoint_offsets,
|
derive_right_19_mechanical_endpoint_offsets,
|
||||||
fit_joint_axis_measurement,
|
fit_joint_axis_measurement,
|
||||||
|
fit_partial_palm_orientation_measurement,
|
||||||
|
fit_partial_palm_orientation_measurements,
|
||||||
fit_rotation_joint_curve,
|
fit_rotation_joint_curve,
|
||||||
|
select_cross_view_roll_direction_source,
|
||||||
solve_urdf_zero_offsets,
|
solve_urdf_zero_offsets,
|
||||||
get_zero_calibration_profile,
|
get_zero_calibration_profile,
|
||||||
write_zero_corrected_urdf,
|
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():
|
def test_zero_sensitive_axis_error_ignores_fixed_cone_angle_mismatch():
|
||||||
parent = np.asarray([0.0, 0.0, 1.0])
|
parent = np.asarray([0.0, 0.0, 1.0])
|
||||||
predicted = np.asarray([1.0, 0.0, 0.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_axis_bias_degrees: float = 0.0,
|
||||||
inject_secondary_root_point_bias_m: float = 0.0,
|
inject_secondary_root_point_bias_m: float = 0.0,
|
||||||
inject_observer_cone_bias_degrees: 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,
|
pose_axis_line_rms_by_joint_m: dict[str, float] | None = None,
|
||||||
joint_maximum_offset_degrees: dict[str, float] | None = None,
|
joint_maximum_offset_degrees: dict[str, float] | None = None,
|
||||||
validation_offset_bias_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)
|
hand = get_hand_calibration_profile(side, layout_id)
|
||||||
zero = get_zero_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)
|
for name, value in zip(zero.direct_zero_joints, offset_degrees)
|
||||||
}
|
}
|
||||||
model = UrdfKinematicModel(source)
|
model = UrdfKinematicModel(source)
|
||||||
base_rotation = Rotation.from_euler("xyz", [0.5, -0.4, 0.8])
|
base_rotation = Rotation.from_euler("xyz", base_euler_xyz_rad)
|
||||||
base_translation = np.asarray([0.31, -0.19, 0.72])
|
base_translation = np.asarray(base_translation_xyz_m)
|
||||||
measurements: list[JointAxisMeasurement] = []
|
measurements: list[JointAxisMeasurement] = []
|
||||||
|
palm_orientation_measurements: list[PalmOrientationMeasurement] = []
|
||||||
validation_cycle = 3 if hand.layout_id == "g20_right_19" else 2
|
validation_cycle = 3 if hand.layout_id == "g20_right_19" else 2
|
||||||
training_cycles = tuple(range(validation_cycle))
|
training_cycles = tuple(range(validation_cycle))
|
||||||
for cycle in range(validation_cycle + 1):
|
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:
|
for joint in zero.axis_joints:
|
||||||
state = list(baseline)
|
state = list(baseline)
|
||||||
if joint == "thumb_cmc_yaw":
|
if joint == "thumb_cmc_yaw":
|
||||||
@@ -636,16 +679,6 @@ def _solve_synthetic_offsets(
|
|||||||
motor_by_joint=motor_by_joint,
|
motor_by_joint=motor_by_joint,
|
||||||
inherited_zero_joints=zero.inherited_zero_joints,
|
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(
|
axis, point = model.axis_line(
|
||||||
joint, zero_offsets=cycle_offsets, joint_angles=angles
|
joint, zero_offsets=cycle_offsets, joint_angles=angles
|
||||||
)
|
)
|
||||||
@@ -734,9 +767,36 @@ def _solve_synthetic_offsets(
|
|||||||
).get(joint, 0.0),
|
).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(
|
result = solve_urdf_zero_offsets(
|
||||||
source_urdf=source,
|
source_urdf=source,
|
||||||
measurements=measurements,
|
measurements=measurements,
|
||||||
|
palm_orientation_measurements=palm_orientation_measurements,
|
||||||
curves=curves,
|
curves=curves,
|
||||||
motor_by_joint=motor_by_joint,
|
motor_by_joint=motor_by_joint,
|
||||||
hand_type=side,
|
hand_type=side,
|
||||||
@@ -747,6 +807,13 @@ def _solve_synthetic_offsets(
|
|||||||
},
|
},
|
||||||
training_cycles=training_cycles,
|
training_cycles=training_cycles,
|
||||||
validation_cycle=validation_cycle,
|
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
|
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
|
expected, abs=0.05
|
||||||
)
|
)
|
||||||
assert math.degrees(result.all_active_offsets_rad["thumb_mcp"]) == pytest.approx(
|
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.training_cycles == (0, 1, 2)
|
||||||
assert result.validation_cycle == 3
|
assert result.validation_cycle == 3
|
||||||
assert all(
|
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)
|
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")
|
hand = get_hand_calibration_profile("right", "g20_right_19")
|
||||||
curves = {
|
curves = {
|
||||||
name: _synthetic_curve(255, math.radians(50.0))
|
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(
|
curves[f"{finger}_pip"] = _synthetic_curve(
|
||||||
255, math.radians(103.0)
|
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
|
RIGHT_SOURCE_URDF, curves
|
||||||
)
|
)
|
||||||
|
|
||||||
assert set(offsets) == {
|
assert set(offsets) == {
|
||||||
f"{finger}_{suffix}"
|
"thumb_mcp",
|
||||||
for finger in ("index", "middle", "ring", "pinky")
|
*(
|
||||||
for suffix in ("mcp_pitch", "pip")
|
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(
|
assert math.degrees(offsets["index_mcp_pitch"]) == pytest.approx(
|
||||||
math.degrees(1.22) - 71.0
|
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:
|
def test_right_15_finger_roll_limit_applies_to_independent_deviation() -> None:
|
||||||
# A shared electrical centre belongs to the four-motor common datum; the
|
# A shared electrical centre belongs to the four-motor common datum; the
|
||||||
# strict 3 deg assembly guard applies to finger-to-finger deviations.
|
# 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},
|
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(
|
assert math.degrees(
|
||||||
result.direct_offsets_rad["thumb_cmc_roll"]
|
result.direct_offsets_rad["thumb_cmc_roll"]
|
||||||
) == pytest.approx(injected[0], abs=0.05)
|
) == 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:
|
def test_profiles_do_not_contain_hard_coded_thumb_zero_offsets() -> None:
|
||||||
right = get_zero_calibration_profile("right")
|
right = get_zero_calibration_profile("right")
|
||||||
|
right_19 = get_zero_calibration_profile("right", "g20_right_19")
|
||||||
left = get_zero_calibration_profile("left")
|
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.fixed_direct_zero_offsets_rad
|
||||||
assert "thumb_cmc_roll" not in right.static_output_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.fixed_direct_zero_offsets_rad
|
||||||
assert "thumb_cmc_roll" not in left.static_output_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:
|
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
|
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:
|
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]
|
offsets = [2.0, -3.0, 4.0, -40.0, 1.0, -1.0, 2.0, *([0.0] * 9)]
|
||||||
_, result = _solve_synthetic_offsets(
|
_, result = _solve_synthetic_offsets(
|
||||||
"right",
|
"right",
|
||||||
offsets,
|
offsets,
|
||||||
|
layout_id="g20_right_19",
|
||||||
joint_maximum_offset_degrees={"thumb_mcp": 45.0},
|
joint_maximum_offset_degrees={"thumb_mcp": 45.0},
|
||||||
)
|
)
|
||||||
|
|
||||||
assert result.passed is True
|
assert result.passed is True
|
||||||
assert result.direct_offsets_rad["thumb_mcp"] == pytest.approx(0.0)
|
assert math.degrees(result.direct_offsets_rad["thumb_mcp"]) == pytest.approx(
|
||||||
assert result.cycle_offsets_rad["thumb_mcp"] == pytest.approx((0.0, 0.0))
|
-40.0, abs=0.05
|
||||||
assert "thumb_ip" not in result.validation_error_by_joint_rad
|
)
|
||||||
|
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"):
|
for name in ("thumb_cmc_roll", "thumb_cmc_yaw", "thumb_cmc_pitch"):
|
||||||
assert abs(math.degrees(result.direct_offsets_rad[name])) <= 20.0
|
assert abs(math.degrees(result.direct_offsets_rad[name])) <= 20.0
|
||||||
for name in ("pinky_mcp_pitch", "pinky_pip"):
|
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:
|
def test_zero_solver_rejects_unreliable_parallel_axis_line_phase() -> None:
|
||||||
_, result = _solve_synthetic_offsets(
|
_, result = _solve_synthetic_offsets(
|
||||||
"right",
|
"right",
|
||||||
|
|||||||
Reference in New Issue
Block a user