G20四指单独标定(少末端tag)

This commit is contained in:
lxp
2026-08-21 12:21:39 +08:00
parent a609d521a0
commit ef65681230
29 changed files with 16589 additions and 609 deletions
+3 -1
View File
@@ -62,7 +62,7 @@ Thumbs.db
# src/linkerhand_retarget/resource/linkerforce_v2/profiles/.
/profiles/
/calibration_output/
/config/g20_three_camera_extrinsics.yaml
/config/*_three_camera_extrinsics.yaml
*.wear_check.json
*.checkpoint.json
*.verification.json
@@ -92,3 +92,5 @@ candump-*
# Local Codex/agent workspace metadata
/.agents/
/.codex/
/.codebuddy/
/.zcode/
+148 -9
View File
@@ -1,5 +1,136 @@
# G20 左右手 AprilTag 标定
## G20右手正式一键标定
固定三相机和15张Tag安装完成后,用户只运行:
```bash
ros2 run g20_thumb_apriltag_calibration calibrate_g20_right
```
开发阶段若上一次会话失败,同一命令会自动校验硬件/几何哈希,并恢复已经
完整提交的关节任务;失败中的当前任务始终丢弃重做,位于它后面但已经完整通过的
独立任务仍会复用,不再因“连续前缀”限制整段重采。导入的任务会立即用与
最终验收相同的硬门限复检(不含视口实时有效率):只以预警带余量通过的旧数据
当场剔除并从其在扫掠顺序中的原始位置重采,避免全部任务采完后才在最终验收
失败、把会话拉回靠前的关节。需要强制从第一个关节
重新采集时使用 `--no-resume`。升级前已经分别完成的正面/侧面roll也会合并为
一个完整同步任务断点;只有两边数据都完整时才复用。
命令自动完成产品哈希预检、运动、当前任务补扫、前三轮训练、第四轮隔离留出、
11个可求解主动关节URDF零位修正(`thumb_mcp`固定CAD零位)、
17条实测命令曲线以及4条
源URDF mimic派生DIP曲线发布。
终端只显示中文进度和问题;失败时复制“请复制以下内容给开发者”块即可。
正式结果位于 `calibration_output/G20_RIGHT_001/latest_passed`。该指针只在
JSON、URDF数值等价、mesh完整性、21条曲线CAD限位、被动关节保护和隔离留出验证
全部通过后更新。
## G20右手15-Tag底层调试入口
以下内容仅保留给旧会话回放和开发调试;正式一键命令只发布上面的精简
schema v4 JSON,不再生成schema v5运行文件。
新布局用独立参数启用,原有左右手11-Tag流程仍默认使用
`tag_layout:=legacy_11`,两套配置和结果schema互不覆盖:
```bash
ros2 launch g20_thumb_apriltag_calibration three_camera_calibration.launch.py \
hand_type:=right \
tag_layout:=g20_right_15 \
serial_number:=G20_RIGHT_001 \
camera_extrinsics_file:=/home/lxp/projects/linkerhand_retarget_ros2/config/g20_three_camera_extrinsics.yaml \
source_urdf_expected_sha256:=<CAD负责人确认的G20右手源URDF_SHA256>
```
15张Tag均为16 mm `tag36h11`。正面为
`0,1,2,3,10,11,12,13`,侧面为
`4,5,6,15,17`,上面为`8,9`ID `7,14,16,18`不再粘贴。详细角色以
`config/three_camera_tags_g20_right_15.yaml`为唯一软件配置源。程序执行4项拇指任务,
以及小指、无名指、中指、食指各自的正面+侧面同步roll、侧面pitch、
侧面PIP,共16个物理运动任务。同步roll只驱动电机一次,但两台相机仍分别拟合并通过
各自的观测质量门限。四个DIP动态曲线由同次PIP实测曲线乘源URDF中受保护的mimic
multiplier生成,不参与视觉拟合或视觉质量门限。每项正式四轮之前自动低速往返预检
0/127/255可见性;
基准形态恢复完成后,程序先用至少30帧稳健锁定正面ID 0、侧面ID 4和顶部ID 8的
固定掌部位姿。小指和无名指弯曲避让会遮住正面ID 0,因此四指正面+侧面同步roll中
允许ID 0暂时不可见,并使用本会话基准锁定值;运动连杆Tag仍必须实时可见,门限不
放宽。终端用`锁`表示该固定参考有效,例如`正面[0锁,12✓]``✗`才表示需要处理的
实时Tag。锁定后不得移动相机、手掌底座或整只手,否则缓存参考失效,必须重新启动
标定。任务级Tag有效率门限按"当前任务所需角色生效期间的采集帧"统计;
任务结束后角色要求会切回预检全套标签,静止期帧不参与该门限,避免把
采集质量良好的任务误判为可见性失败。
取消末端Tag后,四个PIP缺少下游DIP轴作为任意粘贴Tag条件下的绝对零位观察基准;
因此PIP动态曲线仍为视觉实测,但PIP的URDF静态零位保留源CAD,不猜测写入。
正式修正范围为拇指4个主动关节,以及四指各自的MCP roll/MCP pitch,共12个。
侧面累计避障按“PIP→MCP pitch→roll”的安全顺序分阶段进入,并按逆序分阶段退出;
同类辅助电机(全部邻指滚转、全部PIP、全部MCP pitch)合并为同一个并行航点同时
运动,被测通道最后单独进入。“滚转全部回中前不展开弯曲手指”“每指pitch先于PIP”
等已评审不变量保持不变,过渡仍受类别限速、逐航点到位确认、停滞检测和超时保护。
`parallel_pose_transitions`(默认true)置false可回退旧的逐电机顺序。
同一任务的预检和四轮正式扫描会保持完整避障姿态连续执行,只在任务切换时退出,
不再每轮重复展开/弯曲辅助手指。跨手指组切换时,下一组避障姿态仍然需要、且
当前已经在位(含反馈容差)的辅助电机保持原位,只有下一组不再使用的避障电机
退回基准,避免"先展开回基准、马上又折回"的多余动作;已评审的
"滚转先回中再展开""先滚开再弯曲"顺序保持不变。预检正反方向若都保留至少64个电机分箱且最大空缺
不超过8,会把非roll任务四轮正式速度最多提高到预检速度的1.5倍;否则保持原保守速度。
四指roll不再把同一反馈127误当成方向无关的唯一机械姿态:以`255→127`为标准物理
零位,反向到达127的实测偏差保留在`increasing_rad`中。方向分支间隙上限1.5°、
四轮间隙极差上限0.3°;其他关节仍使用严格的0.5°baseline回差门限。
预检、正式四轮和拟合重扫始终使用速度5。
正式roll的每个方向会在经过127时先到位保持0.5秒,再独立保存至少10帧静止Tag/反馈;
方向分支检查和动态曲线的127相位都使用这两组双向静止数据,运动中经过127的帧不再
替代静态保持姿态。
前三轮只用于训练,第四轮完全留出;留出轮不参与显著性、Student-t置信区间或最终重拟合。
每轮PnP都清空帧间跟踪状态并重新执行8帧静态初始化,但同一任务第1轮
已确立的端点相对姿态作为后3轮的分支锚点,防止独立初始化选到相反的
IPPE镜像解。baseline标准接近和全部质量门限保持不变。
电机15任务会利用源URDF中已确认的`thumb_ip mimic=1.03`,只在逐帧IPPE双解中
排除与MCP同步运动明显矛盾(残差超过7.5°)的ID3镜像候选。该先验不生成或缩放
`thumb_ip`曲线;通过分支选择后的`ID2→ID3`姿态仍独立拟合并接受完整留出验证。
当前15-Tag产品流程发布精简schema v4:21条运行时曲线中,17条来自当前会话的
视觉实测,四指DIP按源URDF受保护的mimic multiplier由同指PIP曲线派生。
URDF零位字段只覆盖拇指4个主动关节和四指各自的`mcp_roll/mcp_pitch`,共12个,
其中`thumb_mcp`字段固定为0而不会修改源URDF
四指PIP、`thumb_ip`及四指DIP静态零位保留源CAD。旧schema v5文件仅作历史回放兼容,
当前一键流程不再生成它。正面/侧面roll在同一次运动中独立拟合;方向、
轴线和动态曲线均通过时做不确定度加权轴融合。侧面PIP连杆标签在滚转扫掠中
相对侧相机视线倾斜约13°~20°,平面标签的单目IPPE姿态二义性会给侧视姿态引入
数度的系统性"绕视线"偏差(亚像素重投影无法发现,会话20260820_105535实测
前后轴向稳定相差11.4°),因此跨视角方向差超过0.75°融合门限时不再判定任务
失败:程序记录`cross_view_roll_axis_diagnostic`诊断、跳过融合并采用可信的
前视轴向;仅当差值超过粗错误兜底上限(`cross_view_roll_maximum_axis_difference_deg`
默认15°或线距超过`cross_view_roll_maximum_axis_line_difference_mm`默认30mm
对应标签贴错连杆或标签松动)时才失败。前视侧摆连杆与侧视PIP连杆的轴线
本身存在约21mm的系统性位置差(丝杠平移连杆),属预期现象。侧视校验通道
`*_mcp_roll_side`)的
分支间隙跨轮极差上限放宽为
`cross_view_roll_alias_maximum_branch_gap_range_deg`默认0.5°(绝对间隙1.5°上限
不变),其`axis_pose_line_rms`降为诊断,不再作为准入门限;径向、平面、
圆一致性等其余数据质量门限全部保留。侧面端视roll的圆轨迹方向已经受
姿态轴约束,因此自由三维圆平面与姿态轴的夹角只保留诊断,不再被重复作为硬门限;
径向残差、SE(3)轴线残差和正侧面轴/曲线一致性仍是硬门限。任一静态目标、第四轮留出、
遮挡、PnP或跨机位检查失败时,只保留原始轨迹和`passed:false`诊断,不发布正式URDF。
8个组合姿态仅保留为开发诊断,正式产品默认不执行。15-Tag布局的侧面每根手指只有一张
PIP Tag,并不存在可独立验证的DIP Tag;同时轴线零位求解不提供适合绝对笛卡尔位置验收的
手基座变换,因此不能用该诊断推翻已经通过的单关节隔离留出结果。三个CMC轴恢复使用
`a609d521`验证过的完整四轮相对旋转曲线;全部实测关节均由隔离第四轮逐关节验收。
现场需要区分某根手指的roll机构回差与单机位误差时,可设置
`cross_view_roll_diagnostic_finger:=pinky|ring|middle|index`。该会话只执行目标手指的一次
正面+侧面同步roll,共10个预检/正式方向;任一机位数据不足会重扫同一物理任务,
双机位数据齐全后即使存在轴质量失败也不再自动重采,而是把失败项随双机位结果
一起写入`cross_view_roll_diagnostic`并立即暂停。诊断会话永久
锁定URDF发布,不能用`resume`转换成正式标定。
schema v5明确声明曲线输入域为真实反馈u8;运行桥默认订阅
`/g20/cb_right_hand_state`,并按反馈增减方向选择正程/反程曲线,停止时锁存最后运动
方向。尚未观察到运动方向时使用`255→127`标准分支,不使用两个机械分支的平均值。
schema v4继续兼容旧
命令域。两者都只发布动态角度,不重复叠加已写入URDF的静态偏移。
## 三机位三维关节轴零位标定(schema v4)
正式入口同时使用三台海康 `MV-CS020-10U/10UM` 黑白全局快门相机,只有
@@ -177,7 +308,7 @@ Tag位置,接近轴向观察时改用相机图像平面相位并丢弃单目Pn
前两轮拟合,第三轮强制留出验证;轨迹与零位角度MAE必须≤1°、P95≤2°,三轮轴/零位
差≤0.75°、径向RMS≤3 mm、轴线SE(3)残差≤1 mm。非零修正必须在第三轮优于原始URDF,并通过按三轮分组的
95% bootstrap改善置信检查。最终门限不会因自动重试而放宽。
训练周期Student-t 95%改善下界检查。最终门限不会因自动重试而放宽。
单轮姿态相对理想固定轴的轴外RMS与跨轮重复性分别判定:主动关节上限2.5°,被动
耦合关节上限7.5°。较宽的被动模型门限只容纳可重复的机构耦合和双Tag PnP系统误差,
@@ -209,18 +340,22 @@ Tag位置,接近轴向观察时改用相机图像平面相位并丢弃单目Pn
生成修正URDF时只修改通过验收的主动关节 `origin.rpy`,不会修改任何关节的
`origin.xyz`、转轴、mimic关系或原始CAD/机械安全限位。256项实测轨迹只保存在最终
JSON实测曲线即使略微越过CAD限位,也不能自动扩大URDF限位。
JSON任一曲线点越过CAD限位都会阻止正式发布,程序不会自动扩大URDF限位。
坏帧只丢弃。短时Tag丢失、同步帧中断、扫描超时、端点/分箱不足会自动保持当前位置、
重置当前机位PnP、返回基准后重扫当前方向,最多3次;速度依次降为80%/60%/50%
端点保持延长到0.75/1.0/1.25秒,扫描超时按降速比例同步延长。若反馈在远离目标时
连续8秒没有至少1个u8的进展,则按机械碰撞/摩擦或硬件故障立即保持当前反馈位置并
暂停,不消耗三次采样重试预算。单轮拟合失败只重扫该轮两个方向,全局不一致才重扫
完整关节,每关节最多自动重采2轮。过程指标在最终门限的1.25倍内只发黄色预警,最终
拟合仍按原硬门限验收。零位触边、稳定留出误差或URDF几何无法解释属于模型失败,程序
完整关节,每关节最多自动重采2轮。过程指标在最终门限的1.25倍内时会标记为黄色
预警,但只要仍超过硬门限,就在当前关节立即使用剩余重试预算
`provisional_fit_warning_rescan`);第三次仍超限则当场暂停,不允许预警数据继续到
后续关节。最终拟合仍按原硬门限验收,因此不会在全部任务采完后才回头重采靠前
关节。零位触边、稳定留出误差或URDF几何无法解释属于模型失败,程序
只暂停一次且不再自动重扫,防止重复运动;此时也拒绝`resume`形成死循环。其他可恢复
失败在预算耗尽后才暂停,`resume`从最小失败单元继续,已通过数据保留。所有失败尝试
仍保存在 `raw_samples.jsonl`
仍保存在 `raw_samples.jsonl`若连续两次完整重扫出现轮次和数值都重复的
PnP双簇行程,程序将它判为系统性分支失败并当场停止,不再浪费第3次全关节重扫。
每个新机位/Tag组合开始运动前,不使用单个端点帧直接决定平面Tag的IPPE姿态分支。
程序在静止端点联合8帧候选,按相邻Tag相对姿态的跨帧稳定性和重投影误差选择整组
@@ -273,20 +408,23 @@ ID 9的可见性和PnP稳定性。当前方向自动重试、失败轮次重试
```text
calibration_output/G20_LEFT_001/<时间戳>/
g20_left_G20_LEFT_001_calibration.json
src/.../g20_left/
linkerhand_g20_left_zero_calibrated_G20_LEFT_001_<时间戳>.urdf
meshes/*.STL
calibration_output/G20_RIGHT_001/<时间戳>/
g20_right_G20_RIGHT_001_calibration.json
src/.../g20_right/
linkerhand_g20_right_zero_calibrated_G20_RIGHT_001_<时间戳>.urdf
meshes/*.STL
```
文件包含21个关节的256项 `angle_rad`、16个主动关节的 `zero_command_u8`
`zero_angles.urdf_zero_offset_rad`、5个被动标记、模板来源和总体质量。新URDF每次从
指定原始CAD文件生成,采用 `T_original × Rot(axis, offset)`,绝不叠加旧校准文件或
覆盖原文件;只有通过独立求解验证或明确机械装配基准授权的主动关节 `origin.rpy` 可能
改变,未观测关节和其他URDF文本保持不变。每帧Tag SE(3)、
改变;当前15-Tag右手保留12个主动静态零位字段,其中11个由数据求解,`thumb_mcp`
固定为原始CAD零位0;四指PIP虽保留字段但数值固定为0。
未观测关节和其他URDF文本保持不变。源URDF中的相对mesh资源会按原相对路径复制到
同一会话,保证会话内URDF可独立加载,并在正式发布时逐文件记录SHA256。每帧Tag SE(3)、
图像时间戳、20通道状态、同步误差和PnP误差只进入 `raw_samples.jsonl`
完整 `raw_samples.jsonl` 已存在时,可以按当前算法离线重放,不连接相机、不发送电机
@@ -315,7 +453,8 @@ ros2 launch g20_thumb_apriltag_calibration calibrated_joint_state_bridge.launch.
calibration_file:=$PWD/calibration_output/G20_RIGHT_001/20260811_120146/g20_right_G20_RIGHT_001_calibration.json
```
默认订阅 `/cb_right_hand_control_cmd`,发布
schema v5默认订阅 `/g20/cb_right_hand_state`schema v4默认订阅
`/g20/cb_right_hand_control_cmd`。两者均发布
`/sim/mujoco/g20/right/joint_state`。启动前必须停止任何旧的同名话题桥,避免两个
发布者同时驱动仿真。节点会拒绝左右手不匹配、质量未通过、字段不完整或非有限命令,
因此不会静默退回旧标定。
@@ -0,0 +1,36 @@
schema_version: 1
model: G20
side: right
serial_number: G20_RIGHT_001
can_interface: can0
output_root: calibration_output
cameras:
front:
serial_number: DB2163742
camera_name: hikrobot_front_DB2163742
camera_info: ~/.ros/camera_info/hikrobot_DB2163742.yaml
side:
serial_number: DB2163749
camera_name: hikrobot_side_DB2163749
camera_info: ~/.ros/camera_info/hikrobot_DB2163749.yaml
top:
serial_number: DB2163739
camera_name: hikrobot_top_DB2163739
camera_info: ~/.ros/camera_info/hikrobot_DB2163739.yaml
artifacts:
source_urdf: src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/linkerhand_g20_right.urdf
source_urdf_sha256: eeb6ffb0e95d2a6acd4c26331ae68062e0d74160de4b552b4f6d395cce5ca4e8
camera_extrinsics: config/g20_three_camera_extrinsics.yaml
camera_extrinsics_sha256: aa0a1498a210ef36f20d59bd4fdc612a01fd09eb5a2bc1c0b8d84c05a44d5c5a
calibration_config: src/g20_thumb_apriltag_calibration/config/three_camera_calibration.yaml
calibration_config_sha256: 80758d0a240a7dea1426d4fad688b09eac8360560a1d5870d709b7bb103d3280
tag_config: src/g20_thumb_apriltag_calibration/config/three_camera_tags_g20_right_15.yaml
tag_config_sha256: 7ce50be80dfccb3529c34d5bd601090b9d378e2bf6abcef2d5b93dc6a4da99fd
release:
# Each task already contains three training cycles plus an isolated fourth
# holdout, so a second complete hardware session duplicates hours of motion.
required_independent_passes: 1
static_repeatability_deg: 1.0
@@ -16,14 +16,24 @@ g20_calibration:
normal_calibration_speed: 15
index_roll_calibration_speed: 5
index_flex_calibration_speed: 10
# 15-Tag产品预检仍使用上面保守速度;只有正反预检都留出至少双倍正式分箱余量,
# 才把非roll任务正式扫描最多提速1.5倍。四指roll受0.5°回差门限约束,
# 始终保持速度5;任一方向采样余量不足也保持原速度。
adaptive_formal_speed_enabled: true
adaptive_formal_speed_max_scale: 1.5
adaptive_formal_speed_minimum_bins: 64
adaptive_formal_speed_maximum_bin_gap: 8
speed_setting_settle_seconds: 0.25
# tag36h11检测角点围成的黑色正方形实测为16 mm;不包含外围白边。
tag_size_m: 0.016
repetitions: 3
# 15-Tag产品正式零位使用前三轮训练、最后一轮完全留出;旧11-Tag仍读取repetitions=3。
g20_right_19_repetitions: 4
preflight_frames: 60
minimum_detection_rate: 0.95
minimum_detection_hz: 15.0
minimum_feedback_hz: 25.0
maximum_hamming: 0
minimum_decision_margin: 30.0
minimum_edge_pixels: 30.0
@@ -36,9 +46,15 @@ g20_calibration:
pnp_tracker_reset_seconds: 5.0
# 标定任务不再用第一帧决定平面Tag的IPPE分支;静止端点联合8帧选择整组最稳定解。
pnp_group_initialization_frames: 8
# 侧面Tag 4/5/6/7在初始化端点的贴面法向应一致;用此先验消除静态IPPE镜像双解。
# 侧面当前任务所需Tag在初始化端点的贴面法向应一致;用此先验消除静态IPPE镜像双解。
pnp_group_normal_alignment_scale_deg: 5.0
pnp_group_maximum_normal_alignment_deg: 15.0
# 仅在拇指MCP/IP同步运动且至少一个候选落入可信区间时,用源URDF mimic
# 关系辅助选择IPPE分支;若全部候选超限则退回纯视觉,绝不丢帧,也不生成、
# 缩放或替代被动IP的自身Tag实测曲线。
thumb_ip_pnp_coupling_multiplier: 1.03
thumb_ip_pnp_coupling_scale_deg: 3.0
thumb_ip_pnp_maximum_coupling_residual_deg: 7.5
top_pnp_invalid_reset_seconds: 1.0
# 三维位姿必须与实测20通道状态严格按时间戳配对。
maximum_state_image_skew_ms: 50.0
@@ -54,17 +70,22 @@ g20_calibration:
# 约束三维圆,不让单目平面Tag的深度噪声自由决定转轴方向。
axis_maximum_rotation_circle_difference_deg: 1.0
# 单轴模型残差与跨轮重复误差分开判定:主动刚性关节要求更严;被动耦合
# 关节允许可重复的非理想单轴分量,但仍须通过0.75°跨轮轴差及第三轮留出。
# 关节允许可重复的非理想单轴分量,但仍须通过0.75°跨轮轴差及最终轮留出。
active_maximum_rotation_orthogonal_rms_deg: 2.5
passive_maximum_rotation_orthogonal_rms_deg: 7.5
zero_maximum_axis_cycle_difference_deg: 0.75
# 零位无法改变父子轴夹角;超过该值属于CAD/PnP几何错误,不能吸收到零位。
zero_maximum_axis_cone_mismatch_deg: 5.0
zero_maximum_observability_condition_number: 10000000000.0
zero_maximum_offset_deg: 20.0
# 四指绝对静态零偏默认保护范围。MCP侧摆只保留实测动态曲线,静态零位固定为CAD 0
# 四指MCP roll/pitch绝对静态零偏保护范围PIP仍保留CAD静态零位
zero_finger_maximum_offset_deg: 3.0
endpoint_tolerance_u8: 2.0
# 请求命令与固件反馈是两个标定域。稳态检查点允许小幅死区,但反馈
# 必须已经稳定;大残差仍由机械卡滞保护处理。
steady_checkpoint_command_feedback_tolerance_u8: 8.0
steady_checkpoint_maximum_feedback_range_u8: 2.0
# 电机10在命令0时实测会稳定反馈为4;该0端使用±4。
thumb_yaw_zero_endpoint_tolerance_u8: 4.0
# 右手电机10在命令255时多次实测稳定反馈为250;仅右手该端点使用±5。
@@ -72,12 +93,18 @@ g20_calibration:
# 右手小指PIP电机19在命令0时固件反馈稳定饱和为5;仅其0端使用±5。
pinky_pip_zero_endpoint_tolerance_u8: 5.0
endpoint_hold_seconds: 0.5
# roll零位127必须从两个方向到位并静止采集,禁止用运动中经过127的帧判回差。
baseline_hold_seconds: 0.5
minimum_baseline_hold_frames: 10
# 15-Tag产品每项正式四轮前先做一次低速往返,端点Tag稳定至少2秒。
task_precheck_hold_seconds: 2.0
position_timeout_seconds: 30.0
sweep_timeout_seconds: 90.0
# 反馈在远离目标时连续8秒没有至少1个u8的进展,按机械卡滞立即暂停;
# 启动宽限1秒后,反馈连续2秒没有至少1个u8的进展,按机械卡滞立即暂停;
# 这类故障不进入遮挡/超时的三次自动重扫。
motor_stall_timeout_seconds: 8.0
# 低速5也应持续产生反馈进展;5秒无进展即停,减少机构持续顶死时间。
motor_stall_timeout_seconds: 2.0
motor_stall_startup_grace_seconds: 1.0
motor_stall_minimum_progress_u8: 1.0
invalid_timeout_seconds: 3.0
minimum_sweep_frames: 40
@@ -85,15 +112,18 @@ g20_calibration:
minimum_sweep_bins: 32
maximum_bin_gap: 16
# 可恢复的采样失败自动重扫当前方向;超过次数才暂停等待人工处理。
automatic_sweep_retry_limit: 3
automatic_sweep_retry_limit: 2
# 轨迹拟合失败优先只重扫失败轮次;零位/URDF模型失败不重复运动。
automatic_fit_retry_limit: 2
automatic_motion_retry_limit: 2
# 留空为正式标定;设为pinky/ring/middle/index时只采该指正面+侧面roll
# 即使正面baseline回差失败也继续完成侧面对照,并永久锁定本会话URDF发布。
cross_view_roll_diagnostic_finger: ""
# 过程检查允许25%的黄色预警带,最终验收仍使用下面的严格门限。
provisional_warning_ratio: 1.25
retry_minimum_speed: 3
retry_speed_scales: [0.8, 0.6, 0.5]
retry_endpoint_hold_seconds: [0.75, 1.0, 1.25]
retry_speed_scales: [0.8, 0.6]
retry_endpoint_hold_seconds: [0.75, 1.0]
trajectory_maximum_plane_rms_m: 0.004
trajectory_maximum_radial_rms_m: 0.004
@@ -106,15 +136,40 @@ g20_calibration:
trajectory_maximum_cycle_travel_difference_deg: 3.0
passive_maximum_cycle_travel_difference_deg: 10.0
maximum_monotonic_correction_deg: 2.0
maximum_hysteresis_deg: 5.0
# 旧布局仍用连续扫描正反程差门限;15-Tag产品的连续运动包含速度相关滞后,
# 由方向曲线和最终留出验证建模,不再重复硬判。其绝对正反程门禁使用下面
# 的九点稳态command_maximum_direction_gap_deg。
maximum_hysteresis_deg: 2.0
# 15-Tag产品模式额外要求每轮正反方向在各自baseline处绕实测关节轴的角度差
# 不超过0.5°;四指roll例外:127以255→127为唯一物理零位,反向分支
# 保留实测偏差,并改为检查分支间隙上限及跨轮稳定性。
baseline_maximum_hysteresis_deg: 0.5
directional_zero_maximum_branch_gap_deg: 2.0
directional_zero_maximum_branch_gap_range_deg: 0.3
cross_view_roll_maximum_branch_gap_difference_deg: 0.3
passive_maximum_monotonic_correction_deg: 3.0
passive_maximum_hysteresis_deg: 7.5
passive_maximum_hysteresis_deg: 2.0
# 九点姿态先按实测反馈重新对齐,再用此门限检查固有机械回差;请求命令域
# 的两条运行曲线仍原样保留固件方向死区,不能把command/feedback差算成回差。
command_maximum_direction_gap_deg: 2.0
# 默认无额外随机动作;第三轮扫描始终作为不可关闭的留出验证。
# 默认无额外随机动作;15-Tag产品最终一轮始终作为不可关闭的留出验证。
validation_enabled: false
# 第四轮留出求解后必须再走8个固定安全组合姿态;三机位规定Tag全部可见
# 且实测20通道到位才允许发布。只保存Tag位姿,不保存原始图像。
# Developer diagnostic only. The 15-Tag product has one side PIP Tag per
# finger and no independent DIP Tag, while the formal fourth sweep cycle
# already provides an isolated per-joint holdout.
combination_validation_enabled: false
combination_validation_frames: 10
combination_maximum_position_p95_m: 0.003
combination_maximum_orientation_p95_deg: 2.0
validation_command_count: 3
validation_frames: 10
validation_seed: 20260804
validation_timeout_seconds: 20.0
maximum_validation_mae_deg: 1.0
maximum_validation_p95_deg: 2.0
# 15-Tag产品模式使用更严格的任一点及静态零偏95%置信区间门限。
maximum_validation_error_deg: 3.0
zero_maximum_confidence_half_width_deg: 1.5
@@ -0,0 +1,62 @@
/g20_calibration/front/apriltag/apriltag:
ros__parameters:
image_transport: raw
qos_profile: sensor_data
family: 36h11
size: 0.016
profile: false
max_hamming: 0
detector:
threads: 4
decimate: 1.5
blur: 0.0
refine: true
sharpening: 0.25
debug: false
pose_estimation_method: pnp
tag:
ids: [0, 1, 2, 3, 10, 11, 12, 13]
frames: [front_base, thumb_cmc, thumb_mcp, thumb_ip, pinky_roll, ring_roll, middle_roll, index_roll]
sizes: [0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016]
/g20_calibration/side/apriltag/apriltag:
ros__parameters:
image_transport: raw
qos_profile: sensor_data
family: 36h11
size: 0.016
profile: false
max_hamming: 0
detector:
threads: 4
decimate: 1.5
blur: 0.0
refine: true
sharpening: 0.25
debug: false
pose_estimation_method: pnp
tag:
ids: [4, 5, 6, 15, 17]
frames: [side_base, ring_pip, pinky_pip, middle_pip, index_pip]
sizes: [0.016, 0.016, 0.016, 0.016, 0.016]
/g20_calibration/top/apriltag/apriltag:
ros__parameters:
image_transport: raw
qos_profile: sensor_data
family: 36h11
size: 0.016
profile: false
max_hamming: 0
detector:
threads: 4
decimate: 1.5
blur: 0.0
refine: true
sharpening: 0.25
debug: false
pose_estimation_method: pnp
tag:
ids: [8, 9]
frames: [top_base, thumb_yaw]
sizes: [0.016, 0.016]
@@ -1,8 +1,13 @@
"""Map G20 u8 commands to URDF joint angles using one calibration JSON.
"""Map G20 u8 feedback to URDF joint angles using one calibration JSON.
The static encoder-zero corrections in ``zero_angles`` are already baked into
the corrected URDF joint origins. This bridge therefore publishes only the
dynamic ``angle_rad`` values and never adds the static offsets a second time.
Schema-v5 trajectories are fitted against timestamp-synchronised hardware
feedback, not controller set-points. They must therefore be queried with the
SDK ``hand_state`` topic. The retained schema-v4 path is command-indexed for
backwards compatibility only.
"""
from __future__ import annotations
@@ -16,7 +21,11 @@ import rclpy
from rclpy.node import Node
from sensor_msgs.msg import JointState
from .full_hand import get_hand_calibration_profile, validate_compact_payload
from .full_hand import (
get_hand_calibration_profile,
infer_compact_payload_layout,
validate_compact_payload,
)
G20_COMMAND_NAMES: tuple[str, ...] = (
@@ -71,7 +80,7 @@ G20_URDF_JOINT_NAMES: tuple[str, ...] = (
class CalibratedCommandMapper:
"""Validated, side-specific lookup from a G20 command to URDF radians."""
"""Validated, side-specific lookup from G20 u8 values to URDF radians."""
def __init__(
self, payload: Mapping[str, Any], *, expected_side: str | None = None
@@ -86,9 +95,20 @@ class CalibratedCommandMapper:
quality = payload["quality"]
if quality.get("passed") is not True:
raise ValueError("calibration quality.passed must be true")
profile = get_hand_calibration_profile(side)
layout_id = infer_compact_payload_layout(payload)
profile = get_hand_calibration_profile(side, layout_id)
self.side = side
self.layout_id = layout_id
self.serial_number = str(payload["serial_number"])
schema_version = int(payload["schema_version"])
self.input_domain = str(
payload.get(
"curve_input_domain",
"command_u8" if schema_version == 4 else "",
)
)
if self.input_domain not in {"command_u8", "feedback_u8"}:
raise ValueError("calibration curve_input_domain is invalid")
self._motor_by_joint = {
name: int(profile.joint_specs[name].motor_index)
for name in G20_URDF_JOINT_NAMES
@@ -100,6 +120,27 @@ class CalibratedCommandMapper:
)
for name in G20_URDF_JOINT_NAMES
}
self._decreasing_curves = {
name: tuple(
float(value)
for value in payload["joints"][name].get(
"decreasing_rad", payload["joints"][name]["angle_rad"]
)
)
for name in G20_URDF_JOINT_NAMES
}
self._increasing_curves = {
name: tuple(
float(value)
for value in payload["joints"][name].get(
"increasing_rad", payload["joints"][name]["angle_rad"]
)
)
for name in G20_URDF_JOINT_NAMES
}
self._previous_by_motor: dict[int, float] = {}
self._direction_by_motor: dict[int, str] = {}
self.direction_deadband_u8 = 0.5
@staticmethod
def _command_index(value: float) -> int:
@@ -133,10 +174,34 @@ class CalibratedCommandMapper:
)
command = values
indices = tuple(self._command_index(value) for value in command)
return tuple(
self._curves[name][indices[self._motor_by_joint[name]]]
for name in G20_URDF_JOINT_NAMES
)
direction_by_motor: dict[int, str | None] = {}
for motor, value in enumerate(command):
previous = self._previous_by_motor.get(motor)
direction = self._direction_by_motor.get(motor)
if previous is not None:
if value > previous + self.direction_deadband_u8:
direction = "increasing"
elif value < previous - self.direction_deadband_u8:
direction = "decreasing"
direction_by_motor[motor] = direction
result: list[float] = []
for name in G20_URDF_JOINT_NAMES:
motor = self._motor_by_joint[name]
direction = direction_by_motor[motor]
curves = (
self._increasing_curves
if direction == "increasing"
else self._decreasing_curves
if direction == "decreasing"
else self._curves
)
result.append(curves[name][indices[motor]])
for motor, value in enumerate(command):
self._previous_by_motor[motor] = value
direction = direction_by_motor[motor]
if direction is not None:
self._direction_by_motor[motor] = direction
return tuple(result)
def load_calibrated_command_mapper(
@@ -149,6 +214,17 @@ def load_calibrated_command_mapper(
return CalibratedCommandMapper(payload, expected_side=expected_side)
def default_input_topic(hand_type: str, input_domain: str) -> str:
side = str(hand_type).lower()
if side not in {"left", "right"}:
raise ValueError("hand_type must be left or right")
if input_domain == "feedback_u8":
return f"/g20/cb_{side}_hand_state"
if input_domain == "command_u8":
return f"/g20/cb_{side}_hand_control_cmd"
raise ValueError("calibration curve_input_domain is invalid")
class CalibratedJointStateBridge(Node):
def __init__(self) -> None:
super().__init__("g20_calibrated_joint_state_bridge")
@@ -168,7 +244,9 @@ class CalibratedJointStateBridge(Node):
)
input_topic = str(self.get_parameter("input_topic").value).strip()
output_topic = str(self.get_parameter("output_topic").value).strip()
self.input_topic = input_topic or f"/cb_{hand_type}_hand_control_cmd"
self.input_topic = input_topic or default_input_topic(
hand_type, self.mapper.input_domain
)
self.output_topic = (
output_topic or f"/sim/mujoco/g20/{hand_type}/joint_state"
)
@@ -179,7 +257,8 @@ class CalibratedJointStateBridge(Node):
self._last_error = ""
self.get_logger().info(
f"loaded {hand_type} G20 calibration for {self.mapper.serial_number}: "
f"{self.input_topic} -> {self.output_topic}"
f"{self.input_topic} ({self.mapper.input_domain}) -> "
f"{self.output_topic}"
)
def _command_callback(self, command: JointState) -> None:
File diff suppressed because it is too large Load Diff
File diff suppressed because it is too large Load Diff
@@ -0,0 +1,553 @@
"""One-command product runner for G20_RIGHT_001 calibration."""
from __future__ import annotations
import argparse
from datetime import datetime
import json
import os
from pathlib import Path
import signal
import subprocess
import sys
import time
import traceback
from typing import Any, Mapping
from ament_index_python.packages import get_package_share_directory
import rclpy
from rclpy.node import Node
from std_msgs.msg import String
from std_srvs.srv import Trigger
from .hikrobot_camera import configure_fastdds_large_image_transport
from .operator_report import ProgressEstimator, build_failure_report, render_progress_zh
from .product import ProductConfig, load_product_config, sha256_file
from .publication import atomic_session_pointer, finalize_session_artifacts
EXIT_PASS = 0
EXIT_QUALITY = 2
EXIT_SAFETY = 3
STATUS_TIMEOUT_SECONDS = 90.0
class CalibrationMonitor(Node):
def __init__(self) -> None:
super().__init__("g20_calibration_product_runner")
self.latest_status: dict[str, Any] = {}
self.last_status_at = time.monotonic()
self.start_requested = False
self.start_future: Any = None
self.abort_future: Any = None
self.create_subscription(String, "/g20_calibration/status", self._status, 10)
self.start_client = self.create_client(Trigger, "/g20_calibration/start")
self.abort_client = self.create_client(Trigger, "/g20_calibration/abort")
def _status(self, message: String) -> None:
try:
payload = json.loads(message.data)
except (TypeError, json.JSONDecodeError):
return
if isinstance(payload, dict):
self.latest_status = payload
self.last_status_at = time.monotonic()
def maybe_start(self) -> None:
if self.start_requested or self.latest_status.get("state") != "WAIT_START":
return
if not self.start_client.service_is_ready():
self.start_client.wait_for_service(timeout_sec=0.05)
return
self.start_requested = True
self.start_future = self.start_client.call_async(Trigger.Request())
def abort(self) -> None:
if not self.abort_client.service_is_ready():
self.abort_client.wait_for_service(timeout_sec=1.0)
if self.abort_client.service_is_ready():
self.abort_future = self.abort_client.call_async(Trigger.Request())
class ProgressConsole:
def __init__(self, serial_number: str) -> None:
self.serial_number = serial_number
self.estimator = ProgressEstimator.start()
self.last_text = ""
self.last_issue = ""
def update(self, status: Mapping[str, Any]) -> None:
text = render_progress_zh(self.serial_number, status, self.estimator)
if text == self.last_text:
return
self.last_text = text
if sys.stdout.isatty():
sys.stdout.write("\x1b[2J\x1b[H" + text + "\n")
sys.stdout.flush()
else:
print(text, flush=True)
reason = str(status.get("reason", ""))
if reason.startswith("automatic_retry_") and reason != self.last_issue:
self.last_issue = reason
active = status.get("active", {})
print(
"\n".join(
[
f"⚠ 当前任务出现问题:{reason.removeprefix('automatic_retry_')}",
f"系统处理:只重扫当前任务(第 {active.get('automatic_retry_count', 1)}/2 次)",
]
),
flush=True,
)
def _default_product_config() -> Path:
try:
installed = Path(
get_package_share_directory("g20_thumb_apriltag_calibration")
) / "config" / "g20_right_product.yaml"
if installed.is_file():
return installed
except Exception:
pass
return (
Path.cwd()
/ "src/g20_thumb_apriltag_calibration/config/g20_right_product.yaml"
).resolve()
def _launch_command(
config: ProductConfig,
session: Path,
*,
resume_from: Path | None = None,
) -> list[str]:
values = {
"hand_type": "right",
"tag_layout": "g20_right_15",
"serial_number": config.serial_number,
"can_interface": config.can_interface,
"session_dir": str(session),
"output_root": str(config.output_root),
"camera_extrinsics_file": str(config.camera_extrinsics),
"source_urdf_path": str(config.source_urdf),
"source_urdf_expected_sha256": config.source_urdf_sha256,
"corrected_urdf_output_dir": str(session),
"calibration_config": str(config.calibration_config),
"tag_config": str(config.tag_config),
"commands_enabled": "true",
"start_cameras": "true",
"start_sdk": "true",
"record_bag": "false",
"validation_enabled": "false",
}
if resume_from is not None:
values["resume_raw_samples_path"] = str(
resume_from / "raw_samples.jsonl"
)
for view, camera in config.cameras.items():
values[f"{view}_camera_serial"] = camera["serial_number"]
values[f"{view}_camera_name"] = camera["camera_name"]
values[f"{view}_camera_info_url"] = camera["camera_info"]
return [
"ros2",
"launch",
"g20_thumb_apriltag_calibration",
"three_camera_calibration.launch.py",
*(f"{name}:={value}" for name, value in values.items()),
]
def _stop_stack(process: subprocess.Popen[Any]) -> None:
if process.poll() is not None:
return
try:
os.killpg(process.pid, signal.SIGINT)
except ProcessLookupError:
return
try:
process.wait(timeout=15.0)
except subprocess.TimeoutExpired:
try:
os.killpg(process.pid, signal.SIGTERM)
except ProcessLookupError:
return
try:
process.wait(timeout=5.0)
except subprocess.TimeoutExpired:
try:
os.killpg(process.pid, signal.SIGKILL)
except ProcessLookupError:
return
process.wait(timeout=5.0)
def _write_trace(log_path: Path, error: BaseException) -> None:
with log_path.open("a", encoding="utf-8") as stream:
stream.write("\n[one-command exception]\n")
traceback.print_exception(type(error), error, error.__traceback__, file=stream)
def _request_safe_abort(monitor: CalibrationMonitor, timeout_seconds: float = 35.0) -> None:
monitor.abort()
deadline = time.monotonic() + float(timeout_seconds)
while time.monotonic() < deadline and rclpy.ok():
rclpy.spin_once(monitor, timeout_sec=0.1)
if monitor.latest_status.get("state") == "ABORTED":
return
def _run_hardware_session(
config: ProductConfig,
session: Path,
*,
resume_from: Path | None = None,
) -> tuple[dict[str, Any], int]:
session.mkdir(parents=True, exist_ok=False)
(session / "raw_samples.jsonl").touch()
log_path = session / "calibration.log"
log_stream = log_path.open("a", encoding="utf-8", buffering=1)
atomic_session_pointer(config.session_root, "latest_attempt", session)
monitor = CalibrationMonitor()
console = ProgressConsole(config.serial_number)
process: subprocess.Popen[Any] | None = None
latest_status: dict[str, Any] = {
"state": "PREFLIGHT",
"reason": "starting_ros_stack",
"progress": 0.0,
"views": {},
"feedback_hz": 0.0,
}
exit_code = EXIT_QUALITY
try:
process = subprocess.Popen(
_launch_command(config, session, resume_from=resume_from),
cwd=config.workspace,
stdout=log_stream,
stderr=subprocess.STDOUT,
text=True,
start_new_session=True,
)
launched_at = time.monotonic()
last_render = 0.0
while True:
rclpy.spin_once(monitor, timeout_sec=0.1)
if monitor.latest_status:
latest_status = monitor.latest_status
monitor.maybe_start()
now = time.monotonic()
if now - last_render >= 0.5:
console.update(latest_status)
last_render = now
if monitor.start_future is not None and monitor.start_future.done():
response = monitor.start_future.result()
if response is None or not response.success:
message = "start service failed" if response is None else response.message
raise RuntimeError(f"CFG-START-008:{message}")
monitor.start_future = None
state = str(latest_status.get("state", ""))
if state == "COMPLETE":
exit_code = EXIT_PASS
break
if state in {"PAUSED", "ABORTED"}:
reason = str(latest_status.get("reason", "calibration_paused"))
exit_code = EXIT_SAFETY if "stall" in reason or state == "ABORTED" else EXIT_QUALITY
if state == "PAUSED" and "stall" not in reason:
# Ordinary quality failures return to the reviewed baseline
# before the process tree is stopped. Mechanical stalls
# deliberately skip this path and keep the current pose.
failure_status = dict(latest_status)
_request_safe_abort(monitor)
latest_status = failure_status
break
if process.poll() is not None:
raise RuntimeError(f"PUB-STACK-602:ROS stack exited with {process.returncode}")
if (
not monitor.latest_status
and now - launched_at > STATUS_TIMEOUT_SECONDS
):
raise RuntimeError("CAM-STATUS-202:no calibration status received")
if (
monitor.latest_status
and now - monitor.last_status_at > STATUS_TIMEOUT_SECONDS
):
raise RuntimeError("MOTION-COMM-303:calibration status stopped")
except KeyboardInterrupt as error:
latest_status["state"] = "ABORTED"
latest_status["reason"] = "operator_abort"
_request_safe_abort(monitor)
_write_trace(log_path, error)
exit_code = EXIT_SAFETY
except BaseException as error:
latest_status["state"] = "PAUSED"
latest_status["reason"] = str(error)
_write_trace(log_path, error)
exit_code = EXIT_QUALITY
finally:
if process is not None:
_stop_stack(process)
monitor.destroy_node()
log_stream.flush()
os.fsync(log_stream.fileno())
log_stream.close()
if exit_code != EXIT_PASS:
_, block = build_failure_report(
config,
session,
latest_status,
reason=str(latest_status.get("reason", "unknown_failure")),
)
print(block, flush=True)
return latest_status, exit_code
def _startup_failure_block(path: Path, error: BaseException) -> str:
return "\n".join(
[
"========== 请复制以下内容给开发者 ==========",
"结果:FAIL",
"错误代码:CFG-PRODUCT-001",
"失败阶段:启动静态预检",
f"问题:{error}",
f"产品配置:{path}",
"自动处理:未启动相机、SDK或机械手运动",
"建议:复制本诊断块给开发者,不要手工修改哈希绕过检查。",
"========== 复制结束 ==========",
]
)
def _automatic_resume_candidate(config: ProductConfig) -> Path | None:
"""Return the newest compatible failed attempt, never a passed session.
Do not trust only ``latest_attempt``. A process interrupted during the
device-only startup gate may have already moved that pointer while still
containing no ``session_start`` checkpoint. In that case walk backwards
to the preceding usable failed session instead of throwing away hours of
completed tasks.
"""
root = config.session_root
try:
resolved_root = root.resolve(strict=True)
except OSError:
return None
candidates: list[Path] = []
pointer = config.session_root / "latest_attempt"
if pointer.exists():
try:
candidates.append(pointer.resolve(strict=True))
except OSError:
pass
try:
candidates.extend(
sorted(
(
path
for path in root.iterdir()
if path.is_dir() and not path.name.startswith("latest_")
),
key=lambda path: path.name,
reverse=True,
)
)
except OSError:
return None
passed_pointer = config.session_root / "latest_passed"
passed: Path | None = None
if passed_pointer.exists():
try:
passed = passed_pointer.resolve(strict=True)
except OSError:
pass
seen: set[Path] = set()
for unresolved in candidates:
try:
candidate = unresolved.resolve(strict=True)
except OSError:
continue
if candidate in seen:
continue
seen.add(candidate)
if candidate.parent != resolved_root or not candidate.is_dir():
continue
# A failed attempt older than the current formal release is stale and
# must not seed a new independent calibration.
if passed is not None and candidate.name <= passed.name:
continue
raw_path = candidate / "raw_samples.jsonl"
if not raw_path.is_file():
continue
summary_path = candidate / "calibration_summary_zh.json"
summary: dict[str, Any] | None = None
if summary_path.is_file():
try:
loaded = json.loads(summary_path.read_text(encoding="utf-8"))
except (OSError, json.JSONDecodeError):
continue
if not isinstance(loaded, dict) or loaded.get("result") != "FAIL":
continue
summary = loaded
hashes = summary.get("hashes", {})
if not isinstance(hashes, Mapping):
continue
if (
str(hashes.get("source_urdf_sha256", ""))
!= config.source_urdf_sha256
or str(hashes.get("camera_extrinsics_sha256", ""))
!= config.camera_extrinsics_sha256
):
continue
start: dict[str, Any] | None = None
try:
with raw_path.open("r", encoding="utf-8") as stream:
for line in stream:
if not line.strip():
continue
value = json.loads(line)
if (
isinstance(value, dict)
and value.get("kind") == "session_start"
):
start = value
break
except (OSError, json.JSONDecodeError):
continue
if (
start is None
or start.get("hand_type") != "right"
or start.get("tag_layout") != "g20_right_15"
or start.get("source_urdf_sha256")
!= config.source_urdf_sha256
):
continue
if summary is None:
# Ctrl+C can terminate the ROS launch tree before the wrapper gets
# a chance to create calibration_summary_zh.json. The immutable
# checkpoint itself is enough to resume only after independently
# proving that its external geometry still matches the product.
try:
checkpoint_extrinsics = Path(
str(start["camera_extrinsics_file"])
).expanduser().resolve(strict=True)
if sha256_file(checkpoint_extrinsics) != (
config.camera_extrinsics_sha256
):
continue
except (KeyError, OSError, ValueError):
continue
return candidate
return None
def run(
config_path: str | Path,
*,
workspace: str | Path | None = None,
preflight_only: bool = False,
allow_resume: bool = True,
) -> int:
path = Path(config_path).expanduser().resolve()
try:
# Resolve every file and camera identity before allowing a hardware
# process to start. A second load enables the real CAN existence gate.
config = load_product_config(path, workspace=workspace, check_can=False)
load_product_config(path, workspace=workspace, check_can=True)
except BaseException as error:
print(_startup_failure_block(path, error), flush=True)
return EXIT_QUALITY
if preflight_only:
print("PASS:产品文件、相机内外参、15张Tag配置和CAN接口静态预检通过。")
return EXIT_PASS
config.session_root.mkdir(parents=True, exist_ok=True)
resume_candidate = (
_automatic_resume_candidate(config) if allow_resume else None
)
if resume_candidate is not None:
print(
"检测到兼容的失败会话,将恢复已完整通过的关节任务:"
f"{resume_candidate.name}。失败中的当前任务会从头重做。",
flush=True,
)
maximum_sessions = config.required_independent_passes
for pass_index in range(maximum_sessions):
stamp = datetime.now().strftime("%Y%m%d_%H%M%S")
session = config.session_root / stamp
while session.exists():
time.sleep(1.0)
stamp = datetime.now().strftime("%Y%m%d_%H%M%S")
session = config.session_root / stamp
if maximum_sessions > 1:
print(f"正式标定复验:第 {pass_index + 1}/{maximum_sessions}", flush=True)
status, code = _run_hardware_session(
config,
session,
resume_from=(resume_candidate if pass_index == 0 else None),
)
if code != EXIT_PASS:
return code
try:
summary, release_ready = finalize_session_artifacts(
config, session, node_status=status
)
except BaseException as error:
_write_trace(session / "calibration.log", error)
status = dict(status)
status["state"] = "PAUSED"
status["reason"] = f"PUB-ARTIFACT-601:{error}"
_, block = build_failure_report(config, session, status, reason=status["reason"])
print(block, flush=True)
return EXIT_QUALITY
if release_ready:
print(
"\n".join(
[
"PASS:G20右手标定、URDF修正和独立复验全部通过。",
f"正式结果:{config.session_root / 'latest_passed'}",
f"JSON{session / f'g20_right_{config.serial_number}_calibration.json'}",
f"URDF{summary['artifacts']['urdf']}",
]
),
flush=True,
)
return EXIT_PASS
print("本次会话质量PASS;正在自动执行第二次独立完整复验。", flush=True)
return EXIT_QUALITY
def main(args: list[str] | None = None) -> None:
parser = argparse.ArgumentParser(description="G20右手一键精密标定")
parser.add_argument("--config", default=str(_default_product_config()))
parser.add_argument("--workspace", default=None)
parser.add_argument("--preflight-only", action="store_true")
parser.add_argument(
"--no-resume",
action="store_true",
help="忽略失败会话,从第一个关节开始全新采集",
)
arguments = parser.parse_args(args)
configure_fastdds_large_image_transport()
ros_log_dir = Path(
os.environ.setdefault("ROS_LOG_DIR", "/tmp/g20_calibration_ros_logs")
)
ros_log_dir.mkdir(parents=True, exist_ok=True)
rclpy.init()
try:
code = run(
arguments.config,
workspace=arguments.workspace,
preflight_only=arguments.preflight_only,
allow_resume=not arguments.no_resume,
)
finally:
if rclpy.ok():
rclpy.shutdown()
raise SystemExit(code)
if __name__ == "__main__":
main()
@@ -0,0 +1,394 @@
"""Stable Chinese progress and copy/paste diagnostics for non-expert users."""
from __future__ import annotations
from dataclasses import dataclass
import json
import math
from pathlib import Path
import time
from typing import Any, Mapping
from .product import ProductConfig
from .storage import atomic_write_json
from .three_camera_diagnostics import JOINT_NAMES_ZH, STATE_NAMES_ZH, three_camera_reason_zh
def _duration(seconds: float | None) -> str:
if seconds is None or not math.isfinite(seconds) or seconds < 0.0:
return "计算中"
value = int(round(seconds))
return f"{value // 60}{value % 60:02d}"
def _task_tag_id_status(views: Mapping[str, Any]) -> str:
"""Render every currently required Tag ID with its live visibility."""
labels = {"front": "正面", "side": "侧面", "top": "顶部"}
ordered_views = [
*[name for name in ("front", "side", "top") if name in views],
*sorted(name for name in views if name not in labels),
]
groups: list[str] = []
for name in ordered_views:
item = views.get(name, {})
if not isinstance(item, Mapping):
continue
required = sorted({int(value) for value in item.get("required_tag_ids", [])})
if not required:
continue
visible = {int(value) for value in item.get("detected_tag_ids", [])}
locked = {
int(value)
for value in item.get("locked_reference_tag_ids", [])
}
ids = ",".join(
f"{tag_id}{'' if tag_id in locked else '' if tag_id in visible else ''}"
for tag_id in required
)
groups.append(f"{labels.get(name, name)}[{ids}]")
if not groups:
return ""
return (
" 当前任务ID"
+ " ".join(groups)
+ "(✓实时可见/锁=基准锁定/✗不可用)"
)
@dataclass
class ProgressEstimator:
started_at: float
last_progress: float = 0.0
last_progress_at: float = 0.0
seconds_per_fraction: float | None = None
@classmethod
def start(cls) -> "ProgressEstimator":
now = time.monotonic()
return cls(started_at=now, last_progress_at=now)
def remaining(self, progress: float, now: float | None = None) -> float | None:
current = time.monotonic() if now is None else float(now)
value = float(max(0.0, min(1.0, progress)))
delta = value - self.last_progress
elapsed = current - self.last_progress_at
if delta >= 0.002 and elapsed > 0.0:
estimate = elapsed / delta
self.seconds_per_fraction = (
estimate
if self.seconds_per_fraction is None
else 0.8 * self.seconds_per_fraction + 0.2 * estimate
)
self.last_progress = value
self.last_progress_at = current
if value >= 1.0:
return 0.0
if self.seconds_per_fraction is not None:
return max(0.0, self.seconds_per_fraction * (1.0 - value))
if value >= 0.02:
return max(0.0, (current - self.started_at) * (1.0 - value) / value)
return None
def render_progress_zh(
serial_number: str,
status: Mapping[str, Any],
estimator: ProgressEstimator,
) -> str:
progress = float(status.get("progress", 0.0))
active = status.get("active", {})
if not isinstance(active, Mapping):
active = {}
state = str(status.get("state", "PREFLIGHT"))
stage = STATE_NAMES_ZH.get(state, state)
preflight_mode = str(status.get("preflight_mode", ""))
if preflight_mode == "device_only_before_baseline":
stage = "设备连接预检"
elif preflight_mode == "recovering_baseline":
stage = "安全恢复基准形态"
elif preflight_mode == "baseline_tags_after_recovery":
stage = "基准姿态标签预检"
elif str(status.get("reason", "")) == (
"holding_same_finger_clearance_before_next_task"
):
stage = "保持避让姿态,切换同指下一项"
elif str(status.get("reason", "")) == (
"waiting_for_task_tags_at_sweep_start"
):
stage = "已到扫描起点,等待任务Tag"
joints = active.get("joints", [])
joint = "/".join(JOINT_NAMES_ZH.get(str(name), str(name)) for name in joints)
if str(active.get("task_name", "")).endswith("_roll_multiview") and joints:
joint = (
JOINT_NAMES_ZH.get(str(joints[0]), str(joints[0]))
+ "(正面+侧面同步)"
)
if not joint:
joint = str(active.get("label_zh", "")) or (
"等待设备" if state in {"PREFLIGHT", "WAIT_START"} else "全手"
)
direction = {
"decreasing": "递减",
"increasing": "递增",
}.get(str(active.get("direction", "")), "-")
cycle = active.get("cycle", "-")
repetitions = active.get("repetitions", 4)
requested = active.get(
"current_motion_target_u8",
active.get("command_u8", active.get("pose_name", "-")),
)
actual = active.get("actual_u8", "-")
views = status.get("views", {})
if not isinstance(views, Mapping):
views = {}
ready_cameras = sum(
bool(item.get("camera_info_valid") and item.get("camera_extrinsics_valid"))
for item in views.values()
if isinstance(item, Mapping)
)
required_tags = sum(
len(item.get("required_tag_ids", []))
for item in views.values()
if isinstance(item, Mapping)
)
visible_tags = sum(
len(
{
*item.get("detected_tag_ids", []),
*item.get("locked_reference_tag_ids", []),
}
)
for item in views.values()
if isinstance(item, Mapping)
)
locked_tag_count = sum(
len(item.get("locked_reference_tag_ids", []))
for item in views.values()
if isinstance(item, Mapping)
)
configured_tags = sum(
len(item.get("configured_tag_ids", item.get("required_tag_ids", [])))
for item in views.values()
if isinstance(item, Mapping)
)
visible_configured_tags = sum(
len(
item.get(
"visible_configured_tag_ids",
item.get("detected_tag_ids", []),
)
)
for item in views.values()
if isinstance(item, Mapping)
)
occlusion_allowed = bool(
state in {"PREFLIGHT", "WAIT_START"}
or active.get("pose_name")
)
if preflight_mode == "device_only_before_baseline":
tag_status = (
f"当前 {visible_configured_tags}/{configured_tags}"
"(基准恢复后检查)"
)
elif preflight_mode == "recovering_baseline":
tag_status = (
f"当前 {visible_configured_tags}/{configured_tags}"
"(恢复中不作为门槛)"
)
elif occlusion_allowed:
tag_status = (
f"当前 {visible_configured_tags}/{configured_tags}(允许遮挡) "
f"本阶段必需 {visible_tags}/{required_tags}"
)
else:
tag_status = (
f"{visible_tags}/{required_tags} 有效"
+ (f"(含锁定 {locked_tag_count}" if locked_tag_count else "")
)
task_tag_ids = (
_task_tag_id_status(views)
if state
in {
"PREPARE_SWEEP",
"SWEEP",
"VALIDATION_MOVE",
"VALIDATION_CAPTURE",
}
else ""
)
retry = int(active.get("automatic_retry_count", 0))
fit_attempt = int(active.get("fit_attempt", 1))
fit_attempt_limit = int(active.get("fit_attempt_limit", 1))
feedback_hz = float(status.get("feedback_hz", 0.0))
eta = (
"等待Tag"
if str(status.get("reason", ""))
== "waiting_for_task_tags_at_sweep_start"
else _duration(estimator.remaining(progress))
)
lines = [
f"[{serial_number}] 标定中 {progress * 100:5.1f}% 预计剩余 {eta}",
f"阶段:{stage}(第 {cycle}/{repetitions} 轮)",
f"任务:{joint} 命令/反馈:{requested}/{actual} 方向:{direction}",
f"Tag{tag_status}{task_tag_ids} 相机:{ready_cameras}/3 正常 反馈:{feedback_hz:.1f} Hz",
f"质量:有效帧 {active.get('valid_frames', 0)} 已自动重扫 {retry}",
]
if fit_attempt_limit > 1:
lines.append(
f"拟合:整关节第 {fit_attempt}/{fit_attempt_limit} 次尝试;"
"每次包含完整四轮双向扫描"
)
resume = status.get("resume", {})
if isinstance(resume, Mapping) and resume.get("used"):
lines.append(
"断点:已恢复 "
f"{int(resume.get('completed_task_count', 0))}/"
f"{int(resume.get('total_task_count', 16))} 个完整任务;"
"失败任务已丢弃并重新采集"
)
return "\n".join(lines)
def classify_error(reason: str, status: Mapping[str, Any]) -> tuple[str, str, str]:
value = str(reason)
active = status.get("active", {})
if not isinstance(active, Mapping):
active = {}
views = status.get("views", {})
missing = [
tag
for item in views.values()
if isinstance(item, Mapping)
for tag in item.get("missing_tag_ids", [])
] if isinstance(views, Mapping) else []
group_pnp_reasons = [
str(item.get("group_pnp_reason"))
for item in views.values()
if isinstance(item, Mapping) and item.get("group_pnp_reason")
] if isinstance(views, Mapping) else []
if value.startswith("CFG-"):
return value, "产品配置或文件预检失败", "不要移动相机;复制本诊断块给开发者。"
if "motor_state_stalled" in value:
return "MOTION-STALL-301", "电机反馈停止向目标推进,程序已保持当前位置", "先检查机械卡阻,未排除前不要重复强推。"
if any(
token in value
for token in (
"sweep_missing_endpoint_bin",
"sweep_bins_too_few",
"sweep_bin_gap_too_large",
"task_precheck_missing_command_127",
)
):
return "OBS-SAMPLE-104", "当前任务的有效视觉轨迹不完整", "根据诊断中的关节、机位和缺失区间处理遮挡或反光后重新运行。"
if "synchronised" in value and not missing and group_pnp_reasons:
return "CAM-GEOMETRY-201", "Tag可见,但整组PnP候选持续被几何检查拒绝", "不要调整Tag;复制本诊断块给开发者检查PnP候选选择。"
if missing or "tag" in value or "detection" in value or "synchronised" in value:
return "OBS-TAG-103", f"所需Tag不可用或持续丢失:{missing}", "检查Tag是否脱落、翘起、反光或被遮挡后重新运行。"
if "state" in value and ("timeout" in value or "lost" in value):
return "MOTION-COMM-302", "机械手反馈中断或频率不足", "检查CAN接口和机械手供电后重新运行。"
if "camera" in value or "pnp" in value or "extrinsics" in value:
return "CAM-GEOMETRY-201", "相机、内外参或位姿求解未通过", "确认三台相机没有移动,然后复制本诊断块给开发者。"
if "checkpoint" in value:
return "FIT-CHECKPOINT-402", "稳态检查点采集流程未完整结束", "完整样本已保留;复制本诊断块给开发者检查采集状态机。"
if "fit" in value or "trajectory" in value or "axis" in value:
return "FIT-MODEL-401", "关节轴或动态曲线拟合未达到精度门限", "不要放宽门限;复制本诊断块给开发者分析原始样本。"
if "validation" in value or "combination" in value or "quality" in value or "zero" in value:
return "VAL-QUALITY-501", "留出验证或URDF零位验证未通过", "结果不会发布;复制本诊断块给开发者。"
if "publish" in value or "artifact" in value or "URDF" in value:
return "PUB-ARTIFACT-601", "结果文件校验或原子发布失败", "原始URDF未被覆盖;复制本诊断块给开发者。"
return "PUB-UNEXPECTED-699", "标定程序出现未分类异常", "复制本诊断块给开发者,完整堆栈已写入calibration.log。"
def build_failure_report(
config: ProductConfig,
session: str | Path,
status: Mapping[str, Any],
*,
reason: str,
) -> tuple[dict[str, Any], str]:
directory = Path(session).resolve()
code, problem, suggestion = classify_error(reason, status)
active = status.get("active", {})
if not isinstance(active, Mapping):
active = {}
else:
active = dict(active)
views = status.get("views", {})
if isinstance(views, Mapping):
group_pnp_reasons = {
str(view): str(item.get("group_pnp_reason"))
for view, item in views.items()
if isinstance(item, Mapping) and item.get("group_pnp_reason")
}
if group_pnp_reasons:
active["group_pnp_reasons"] = group_pnp_reasons
try:
explanation, automatic_action = three_camera_reason_zh(
str(status.get("state", "")), reason, active
)
except Exception:
explanation, automatic_action = problem, "已停止本次发布并保留全部诊断数据"
camera_state = {
view: (
"OK"
if isinstance(item, Mapping)
and item.get("camera_info_valid", item.get("ready"))
and item.get("camera_extrinsics_valid", item.get("ready"))
and item.get("stream_alive", item.get("ready"))
else "NOT_READY"
)
for view, item in (views.items() if isinstance(views, Mapping) else [])
}
session_id = f"{config.serial_number}_{directory.name}"
metrics = {
"valid_frames": active.get("valid_frames"),
"sample": active.get("sample", {}),
"automatic_retry_count": active.get("automatic_retry_count", 0),
}
payload: dict[str, Any] = {
"schema_version": 1,
"serial_number": config.serial_number,
"session_id": session_id,
"result": "FAIL",
"error_code": code,
"stage": str(status.get("state", "startup")),
"reason": str(reason),
"problem_zh": problem,
"explanation_zh": explanation,
"automatic_action_zh": automatic_action,
"suggestion_zh": suggestion,
"metrics": metrics,
"camera_state": camera_state,
"feedback_hz": status.get("feedback_hz", 0.0),
"hashes": {
"product_config_sha256": __import__("hashlib").sha256(config.path.read_bytes()).hexdigest(),
"camera_extrinsics_sha256": config.camera_extrinsics_sha256,
"calibration_config_sha256": config.calibration_config_sha256,
"source_urdf_sha256": config.source_urdf_sha256,
},
"session_dir": str(directory),
"quality": {"passed": False},
}
atomic_write_json(directory / "calibration_summary_zh.json", payload)
block = "\n".join(
[
"========== 请复制以下内容给开发者 ==========",
f"会话编号:{session_id}",
"结果:FAIL",
f"错误代码:{code}",
f"失败阶段:{payload['stage']}",
f"问题:{problem}",
f"详细说明:{explanation}",
f"自动处理:{automatic_action}",
f"关键指标:{json.dumps(metrics, ensure_ascii=False, separators=(',', ':'))}",
f"相机状态:{json.dumps(camera_state, ensure_ascii=False, separators=(',', ':'))}",
f"反馈状态:{float(payload['feedback_hz'] or 0.0):.1f} Hz",
f"配置哈希:{payload['hashes']['product_config_sha256']}",
f"外参哈希:{config.camera_extrinsics_sha256}",
f"源 URDF 哈希:{config.source_urdf_sha256}",
f"会话目录:{directory}",
f"建议:{suggestion}",
"========== 复制结束 ==========",
]
)
return payload, block
@@ -477,6 +477,11 @@ def select_static_rigid_group_initialization(
relative_translation_scale_m: float,
normal_alignment_pairs: Sequence[tuple[str, str]] = (),
normal_alignment_scale_rad: float = math.radians(5.0),
task_reference_pairs: Mapping[
tuple[str, str], tuple[Rotation, np.ndarray]
] | None = None,
task_reference_rotation_scale_rad: float = math.radians(1.0),
task_reference_translation_scale_m: float = 0.01,
) -> tuple[list[dict[str, SquareTagPose]], dict[str, float | str]]:
"""Select a static multi-Tag IPPE branch path in bounded time.
@@ -497,13 +502,18 @@ def select_static_rigid_group_initialization(
normal_pair_names = tuple(
(str(first), str(second)) for first, second in normal_alignment_pairs
)
task_references = dict(task_reference_pairs or {})
if not frames:
raise ValueError("at least one PnP frame is required")
if not role_names or len(set(role_names)) != len(role_names):
raise ValueError("roles must be non-empty and unique")
if any(
parent not in role_names or child not in role_names
for parent, child in (*pair_names, *normal_pair_names)
for parent, child in (
*pair_names,
*normal_pair_names,
*task_references,
)
):
raise ValueError("geometry pairs must reference roles")
scales = (
@@ -513,6 +523,8 @@ def select_static_rigid_group_initialization(
float(relative_rotation_scale_rad),
float(relative_translation_scale_m),
float(normal_alignment_scale_rad),
float(task_reference_rotation_scale_rad),
float(task_reference_translation_scale_m),
)
if min(scales) <= 0.0:
raise ValueError("static initialization scales must be positive")
@@ -523,6 +535,8 @@ def select_static_rigid_group_initialization(
relative_rotation_scale,
relative_translation_scale,
normal_scale,
task_reference_rotation_scale,
task_reference_translation_scale,
) = scales
combinations_by_frame: list[list[dict[str, SquareTagPose]]] = []
@@ -578,6 +592,16 @@ def select_static_rigid_group_initialization(
+ float(np.linalg.norm(translation - reference_translation))
/ relative_translation_scale
)
for pair, (task_rotation, task_translation) in task_references.items():
rotation, translation = _relative_pose(
combination[pair[0]], combination[pair[1]]
)
score += (
float((task_rotation.inv() * rotation).magnitude())
/ task_reference_rotation_scale
+ float(np.linalg.norm(translation - task_translation))
/ task_reference_translation_scale
)
return float(score)
best_total = float("inf")
@@ -628,6 +652,9 @@ def select_static_rigid_group_initialization(
quality = dict(quality)
quality["total_cost"] = float(best_total)
quality["initialization_search"] = "static_reference"
quality["task_reference_used"] = (
"true" if task_references else "false"
)
return selected_path, quality
@@ -1013,6 +1040,13 @@ class SquareTagGroupPoseTracker:
normal_alignment_pairs: Sequence[tuple[str, str]] = (),
normal_alignment_scale_rad: float = math.radians(5.0),
maximum_normal_alignment_rad: float | None = None,
return_reference_rotation_scale_rad: float = math.radians(1.0),
return_reference_maximum_command_gap_u8: int = 8,
coupled_rotation_pairs: Sequence[
tuple[str, str, str, str, float]
] = (),
coupled_rotation_scale_rad: float = math.radians(3.0),
maximum_coupled_rotation_residual_rad: float | None = None,
) -> None:
self.roles = tuple(str(role) for role in roles)
self.adjacent_pairs = tuple(
@@ -1023,6 +1057,22 @@ class SquareTagGroupPoseTracker:
(str(first), str(second))
for first, second in normal_alignment_pairs
)
self.coupled_rotation_pairs = tuple(
(
str(driver_parent),
str(driver_child),
str(follower_parent),
str(follower_child),
float(multiplier),
)
for (
driver_parent,
driver_child,
follower_parent,
follower_child,
multiplier,
) in coupled_rotation_pairs
)
if not self.roles or len(set(self.roles)) != len(self.roles):
raise ValueError("roles must be non-empty and unique")
if any(
@@ -1054,6 +1104,20 @@ class SquareTagGroupPoseTracker:
if maximum_normal_alignment_rad is None
else float(maximum_normal_alignment_rad)
)
self.return_reference_rotation_scale_rad = float(
return_reference_rotation_scale_rad
)
self.return_reference_maximum_command_gap_u8 = int(
return_reference_maximum_command_gap_u8
)
self.coupled_rotation_scale_rad = float(
coupled_rotation_scale_rad
)
self.maximum_coupled_rotation_residual_rad = (
None
if maximum_coupled_rotation_residual_rad is None
else float(maximum_coupled_rotation_residual_rad)
)
reset_seconds = float(reset_after_seconds)
if min(
self.maximum_pose_jump_rad,
@@ -1062,6 +1126,8 @@ class SquareTagGroupPoseTracker:
self.relative_translation_scale_m,
self.reprojection_scale_px,
self.normal_alignment_scale_rad,
self.return_reference_rotation_scale_rad,
self.coupled_rotation_scale_rad,
reset_seconds,
) <= 0.0:
raise ValueError("group tracking scales must be positive")
@@ -1069,11 +1135,33 @@ class SquareTagGroupPoseTracker:
raise ValueError("reprojection_weight must be non-negative")
if self.initialization_frames < 1:
raise ValueError("initialization_frames must be positive")
if self.return_reference_maximum_command_gap_u8 < 0:
raise ValueError(
"return reference maximum command gap must be non-negative"
)
if (
self.maximum_normal_alignment_rad is not None
and self.maximum_normal_alignment_rad <= 0.0
):
raise ValueError("maximum normal alignment must be positive")
if any(
role not in self.roles
for coupling in self.coupled_rotation_pairs
for role in coupling[:4]
):
raise ValueError("coupled rotation pairs must reference roles")
if any(
multiplier <= 0.0
for *_, multiplier in self.coupled_rotation_pairs
):
raise ValueError("coupled rotation multipliers must be positive")
if (
self.maximum_coupled_rotation_residual_rad is not None
and self.maximum_coupled_rotation_residual_rad <= 0.0
):
raise ValueError(
"maximum coupled rotation residual must be positive"
)
self.reset_after_ns = int(reset_seconds * 1_000_000_000)
self._previous: dict[str, SquareTagPose] = {}
self._previous_stamp_ns: int | None = None
@@ -1083,22 +1171,200 @@ class SquareTagGroupPoseTracker:
self._initial_stamps_ns: list[int] = []
self.last_initialization_quality: dict[str, float | str] = {}
self.branch_correction_counts: dict[str, int] = {}
self._decreasing_relative_rotations: dict[
int, dict[tuple[str, str], Rotation]
] = {}
self._coupled_reference_rotations: dict[
tuple[str, str], Rotation
] = {}
self._task_reference_relative_poses: dict[
tuple[str, str], tuple[Rotation, np.ndarray]
] = {}
def reset(self) -> None:
def reset(self, *, preserve_task_reference: bool = False) -> None:
self._previous.clear()
self._previous_stamp_ns = None
self._initial_candidates.clear()
self._initial_stamps_ns.clear()
self.last_initialization_quality.clear()
self.branch_correction_counts.clear()
self._decreasing_relative_rotations.clear()
self._coupled_reference_rotations.clear()
if not preserve_task_reference:
self._task_reference_relative_poses.clear()
def _task_reference_cost(
self, combination: Mapping[str, SquareTagPose]
) -> float:
residual = 0.0
for pair, (expected_rotation, expected_translation) in (
self._task_reference_relative_poses.items()
):
rotation, translation = _relative_pose(
combination[pair[0]], combination[pair[1]]
)
residual += (
float((expected_rotation.inv() * rotation).magnitude())
/ self.return_reference_rotation_scale_rad
+ float(np.linalg.norm(translation - expected_translation))
/ self.relative_translation_scale_m
)
return residual
def _coupled_rotation_residuals(
self, combination: Mapping[str, SquareTagPose]
) -> tuple[float, ...]:
if not self.coupled_rotation_pairs:
return ()
residuals: list[float] = []
for (
driver_parent,
driver_child,
follower_parent,
follower_child,
multiplier,
) in self.coupled_rotation_pairs:
driver_pair = (driver_parent, driver_child)
follower_pair = (follower_parent, follower_child)
if (
driver_pair not in self._coupled_reference_rotations
or follower_pair not in self._coupled_reference_rotations
):
return ()
driver_rotation = _relative_pose(
combination[driver_parent], combination[driver_child]
)[0]
follower_rotation = _relative_pose(
combination[follower_parent], combination[follower_child]
)[0]
driver_travel = (
self._coupled_reference_rotations[driver_pair].inv()
* driver_rotation
).magnitude()
follower_travel = (
self._coupled_reference_rotations[follower_pair].inv()
* follower_rotation
).magnitude()
residuals.append(
abs(float(follower_travel) - multiplier * float(driver_travel))
)
return tuple(residuals)
def _informative_coupled_rotation_costs(
self,
combinations: Sequence[Mapping[str, SquareTagPose]],
) -> tuple[float, ...]:
"""Return branch costs only while the weak coupling prior is credible.
The URDF mimic ratio is useful for distinguishing two planar-IPPE
branches, but it is not measurement truth for a passive joint. Once
every otherwise viable combination disagrees with that ratio, using
it would bias the measured curve (and previously rejected every
frame). In that case fall back to visual continuity for this frame.
"""
residuals = tuple(
self._coupled_rotation_residuals(combination)
for combination in combinations
)
if not residuals or not any(residuals):
return tuple(0.0 for _ in combinations)
if (
self.maximum_coupled_rotation_residual_rad is not None
and not any(
values
and max(values)
<= self.maximum_coupled_rotation_residual_rad
for values in residuals
)
):
return tuple(0.0 for _ in combinations)
return tuple(
sum(values) / self.coupled_rotation_scale_rad
for values in residuals
)
def _return_reference(
self, command_u8: int | None
) -> dict[tuple[str, str], tuple[Rotation, np.ndarray | None]]:
if command_u8 is None or not self._decreasing_relative_rotations:
return {}
command = int(command_u8)
nearest = min(
self._decreasing_relative_rotations,
key=lambda candidate: abs(candidate - command),
)
if (
abs(nearest - command)
> self.return_reference_maximum_command_gap_u8
):
return {}
references = self._decreasing_relative_rotations[nearest]
commands = sorted(self._decreasing_relative_rotations)
axes: dict[tuple[str, str], np.ndarray | None] = {}
for pair in self.adjacent_pairs:
endpoint_delta = (
self._decreasing_relative_rotations[commands[-1]][pair].inv()
* self._decreasing_relative_rotations[commands[0]][pair]
).as_rotvec()
norm = float(np.linalg.norm(endpoint_delta))
axes[pair] = (
None
if norm < math.radians(5.0)
else endpoint_delta / norm
)
return {
pair: (rotation, axes[pair])
for pair, rotation in references.items()
}
def _return_reference_cost(
self,
combination: Mapping[str, SquareTagPose],
reference: Mapping[
tuple[str, str], tuple[Rotation, np.ndarray | None]
],
) -> float:
residual = 0.0
for pair, (expected, motion_axis) in reference.items():
vector = (
expected.inv()
* _relative_pose(
combination[pair[0]], combination[pair[1]]
)[0]
).as_rotvec()
if motion_axis is not None:
# The outbound trajectory identifies the physical one-DOF
# motion axis. Do not penalize return travel along that axis:
# it may contain real mechanical hysteresis that calibration
# must measure. A planar-IPPE mirror branch appears primarily
# as a large orthogonal tilt and is rejected by this residual.
vector = vector - motion_axis * float(vector @ motion_axis)
residual += float(np.linalg.norm(vector))
return residual / self.return_reference_rotation_scale_rad
def select(
self,
candidates_by_role: Mapping[str, Sequence[SquareTagPose]],
*,
stamp_ns: int,
trajectory_command_u8: int | None = None,
trajectory_direction: str | None = None,
) -> tuple[dict[str, SquareTagPose] | None, str]:
"""Return one mutually consistent pose for every configured role."""
direction = (
None
if trajectory_direction is None
else str(trajectory_direction)
)
if direction not in {None, "decreasing", "increasing"}:
raise ValueError(
"trajectory_direction must be decreasing or increasing"
)
return_reference = (
self._return_reference(trajectory_command_u8)
if direction == "increasing"
else {}
)
candidate_lists = [
tuple(candidates_by_role.get(role, ()))
for role in self.roles
@@ -1175,6 +1441,15 @@ class SquareTagGroupPoseTracker:
normal_alignment_scale_rad=(
self.normal_alignment_scale_rad
),
task_reference_pairs=(
self._task_reference_relative_poses
),
task_reference_rotation_scale_rad=(
self.return_reference_rotation_scale_rad
),
task_reference_translation_scale_m=(
self.relative_translation_scale_m
),
)
)
selected = selected_path[-1]
@@ -1195,23 +1470,31 @@ class SquareTagGroupPoseTracker:
):
return None, "group_normal_alignment"
else:
coupling_costs = self._informative_coupled_rotation_costs(
combinations
)
selected = min(
combinations,
key=lambda combination: (
zip(combinations, coupling_costs),
key=lambda item: (
sum(
pose.reprojection_error_px
for pose in combination.values()
for pose in item[0].values()
)
/ self.reprojection_scale_px
+ sum(
_normal_alignment_rad(
combination[first], combination[second]
item[0][first], item[0][second]
)
for first, second in self.normal_alignment_pairs
)
/ self.normal_alignment_scale_rad
+ self._return_reference_cost(
item[0], return_reference
)
+ item[1]
+ self._task_reference_cost(item[0])
),
)
)[0]
maximum_alignment = max(
(
_normal_alignment_rad(
@@ -1238,7 +1521,7 @@ class SquareTagGroupPoseTracker:
)
for pair in self.adjacent_pairs
}
scored: list[tuple[float, dict[str, SquareTagPose]]] = []
base_scored: list[tuple[float, dict[str, SquareTagPose]]] = []
for combination in combinations:
absolute_rotation_motion = 0.0
absolute_translation_motion = 0.0
@@ -1305,11 +1588,23 @@ class SquareTagGroupPoseTracker:
+ relative_translation_motion
/ self.relative_translation_scale_m
+ self.reprojection_weight * reprojection_penalty
+ self._return_reference_cost(
combination, return_reference
)
)
scored.append((float(score), combination))
base_scored.append((float(score), combination))
if not scored:
if not base_scored:
return None, "group_pose_jump"
coupling_costs = self._informative_coupled_rotation_costs(
[combination for _, combination in base_scored]
)
scored = [
(base_score + coupling_cost, combination)
for (base_score, combination), coupling_cost in zip(
base_scored, coupling_costs
)
]
selected = min(scored, key=lambda item: item[0])[1]
aligned: dict[str, SquareTagPose] = {}
@@ -1343,4 +1638,39 @@ class SquareTagGroupPoseTracker:
self._previous = aligned
self._previous_stamp_ns = stamp
if (
direction == "decreasing"
and trajectory_command_u8 is not None
and not self._coupled_reference_rotations
):
for (
driver_parent,
driver_child,
follower_parent,
follower_child,
_multiplier,
) in self.coupled_rotation_pairs:
for pair in (
(driver_parent, driver_child),
(follower_parent, follower_child),
):
self._coupled_reference_rotations[pair] = _relative_pose(
aligned[pair[0]], aligned[pair[1]]
)[0]
if direction == "decreasing" and trajectory_command_u8 is not None:
self._decreasing_relative_rotations[
int(trajectory_command_u8)
] = {
pair: _relative_pose(
aligned[pair[0]], aligned[pair[1]]
)[0]
for pair in self.adjacent_pairs
}
if not self._task_reference_relative_poses:
self._task_reference_relative_poses = {
pair: _relative_pose(
aligned[pair[0]], aligned[pair[1]]
)
for pair in self.adjacent_pairs
}
return dict(aligned), ""
@@ -0,0 +1,237 @@
"""Immutable G20 right-hand product identity and one-command preflight."""
from __future__ import annotations
from dataclasses import dataclass
import hashlib
from pathlib import Path
import re
from typing import Any, Mapping
import yaml
from .extrinsics import camera_info_fingerprint, load_three_camera_extrinsics
VIEWS = ("front", "side", "top")
def sha256_file(path: str | Path) -> str:
digest = hashlib.sha256()
with Path(path).open("rb") as stream:
for chunk in iter(lambda: stream.read(1024 * 1024), b""):
digest.update(chunk)
return digest.hexdigest()
def _mapping(value: Any, name: str) -> Mapping[str, Any]:
if not isinstance(value, Mapping):
raise ValueError(f"{name} must be a mapping")
return value
def _resolve_path(value: Any, *, workspace: Path, name: str) -> Path:
text = str(value).strip()
if not text:
raise ValueError(f"{name} is required")
candidate = Path(text).expanduser()
if not candidate.is_absolute():
candidate = workspace / candidate
return candidate.resolve()
def _camera_info_fingerprint(path: Path) -> str:
with path.open("r", encoding="utf-8") as stream:
payload = _mapping(yaml.safe_load(stream), str(path))
def values(key: str) -> list[float]:
item = _mapping(payload.get(key, {}), key)
return [float(value) for value in item.get("data", [])]
return camera_info_fingerprint(
width=int(payload["image_width"]),
height=int(payload["image_height"]),
camera_matrix=values("camera_matrix"),
distortion=values("distortion_coefficients"),
rectification=values("rectification_matrix"),
projection=values("projection_matrix"),
)
def _tag_ids(path: Path) -> set[int]:
with path.open("r", encoding="utf-8") as stream:
payload = _mapping(yaml.safe_load(stream), str(path))
result: set[int] = set()
for node in payload.values():
parameters = _mapping(_mapping(node, "tag node").get("ros__parameters"), "ros__parameters")
tag = _mapping(parameters.get("tag"), "tag")
ids = [int(value) for value in tag.get("ids", [])]
if result.intersection(ids):
raise ValueError("Tag IDs must be unique across the three views")
result.update(ids)
return result
@dataclass(frozen=True)
class ProductConfig:
path: Path
workspace: Path
serial_number: str
can_interface: str
source_urdf: Path
source_urdf_sha256: str
camera_extrinsics: Path
camera_extrinsics_sha256: str
calibration_config: Path
calibration_config_sha256: str
tag_config: Path
tag_config_sha256: str
output_root: Path
cameras: Mapping[str, Mapping[str, str]]
required_independent_passes: int
static_repeatability_rad: float
@property
def session_root(self) -> Path:
return self.output_root / self.serial_number
def load_product_config(
path: str | Path,
*,
workspace: str | Path | None = None,
check_can: bool = True,
) -> ProductConfig:
"""Load and fully verify the fixed G20_RIGHT_001 product configuration."""
source = Path(path).expanduser().resolve()
if not source.is_file():
raise ValueError(f"product config does not exist: {source}")
with source.open("r", encoding="utf-8") as stream:
raw = _mapping(yaml.safe_load(stream), str(source))
if int(raw.get("schema_version", -1)) != 1:
raise ValueError("product config schema_version must be 1")
root = Path.cwd().resolve() if workspace is None else Path(workspace).resolve()
serial = str(raw.get("serial_number", ""))
if re.fullmatch(r"[A-Za-z0-9_.-]+", serial) is None:
raise ValueError("serial_number is invalid")
if raw.get("model") != "G20" or raw.get("side") != "right":
raise ValueError("product config must describe the G20 right hand")
can_interface = str(raw.get("can_interface", "")).strip()
if not can_interface:
raise ValueError("can_interface is required")
if check_can and not (Path("/sys/class/net") / can_interface).exists():
raise ValueError(f"CAN interface does not exist: {can_interface}")
artifacts = _mapping(raw.get("artifacts"), "artifacts")
source_urdf = _resolve_path(
artifacts.get("source_urdf"), workspace=root, name="source_urdf"
)
extrinsics = _resolve_path(
artifacts.get("camera_extrinsics"),
workspace=root,
name="camera_extrinsics",
)
calibration_config = _resolve_path(
artifacts.get("calibration_config"),
workspace=root,
name="calibration_config",
)
tag_config = _resolve_path(
artifacts.get("tag_config"), workspace=root, name="tag_config"
)
for name, candidate in (
("source URDF", source_urdf),
("camera extrinsics", extrinsics),
("calibration config", calibration_config),
("Tag config", tag_config),
):
if not candidate.is_file():
raise ValueError(f"{name} does not exist: {candidate}")
expected_source_hash = str(artifacts.get("source_urdf_sha256", "")).lower()
expected_extrinsics_hash = str(
artifacts.get("camera_extrinsics_sha256", "")
).lower()
expected_tag_hash = str(artifacts.get("tag_config_sha256", "")).lower()
expected_calibration_hash = str(
artifacts.get("calibration_config_sha256", "")
).lower()
hashes = (
("source URDF", source_urdf, expected_source_hash),
("camera extrinsics", extrinsics, expected_extrinsics_hash),
("Tag config", tag_config, expected_tag_hash),
("calibration config", calibration_config, expected_calibration_hash),
)
for name, candidate, expected in hashes:
if re.fullmatch(r"[0-9a-f]{64}", expected) is None:
raise ValueError(f"{name} expected SHA-256 is invalid")
actual = sha256_file(candidate)
if actual != expected:
raise ValueError(f"{name} SHA-256 mismatch: expected={expected} actual={actual}")
expected_tag_ids = set(range(19)) - {7, 14, 16, 18}
if _tag_ids(tag_config) != expected_tag_ids:
raise ValueError(
"g20_right_15 Tag config must contain exactly IDs "
"0,1,2,3,4,5,6,8,9,10,11,12,13,15,17"
)
camera_raw = _mapping(raw.get("cameras"), "cameras")
if set(camera_raw) != set(VIEWS):
raise ValueError("cameras must contain front/side/top")
loaded_extrinsics = load_three_camera_extrinsics(extrinsics)
cameras: dict[str, dict[str, str]] = {}
serials: set[str] = set()
for view in VIEWS:
item = _mapping(camera_raw[view], f"cameras.{view}")
camera_serial = str(item.get("serial_number", ""))
info_path = _resolve_path(
item.get("camera_info"), workspace=root, name=f"{view}.camera_info"
)
if not info_path.is_file():
raise ValueError(f"{view} camera info does not exist: {info_path}")
if camera_serial in serials:
raise ValueError("the three camera serial numbers must be unique")
serials.add(camera_serial)
identity = loaded_extrinsics.cameras[view]
fingerprint = _camera_info_fingerprint(info_path)
if camera_serial != identity.serial_number:
raise ValueError(f"{view} camera serial does not match extrinsics")
if fingerprint != identity.intrinsics_sha256:
raise ValueError(f"{view} camera intrinsics do not match extrinsics")
cameras[view] = {
"serial_number": camera_serial,
"camera_info": str(info_path),
"camera_name": str(item.get("camera_name", f"hikrobot_{view}_{camera_serial}")),
}
output_root = _resolve_path(
raw.get("output_root", "calibration_output"),
workspace=root,
name="output_root",
)
release = _mapping(raw.get("release", {}), "release")
passes = int(release.get("required_independent_passes", 2))
if passes not in {1, 2}:
raise ValueError("required_independent_passes must be 1 or 2")
repeatability_deg = float(release.get("static_repeatability_deg", 1.0))
if not 0.0 < repeatability_deg <= 5.0:
raise ValueError("static_repeatability_deg must be in (0, 5]")
return ProductConfig(
path=source,
workspace=root,
serial_number=serial,
can_interface=can_interface,
source_urdf=source_urdf,
source_urdf_sha256=expected_source_hash,
camera_extrinsics=extrinsics,
camera_extrinsics_sha256=expected_extrinsics_hash,
calibration_config=calibration_config,
calibration_config_sha256=expected_calibration_hash,
tag_config=tag_config,
tag_config_sha256=expected_tag_hash,
output_root=output_root,
cameras=cameras,
required_independent_passes=passes,
static_repeatability_rad=repeatability_deg * 3.141592653589793 / 180.0,
)
@@ -0,0 +1,569 @@
"""Validate and atomically publish one inseparable G20 JSON/URDF session."""
from __future__ import annotations
from datetime import datetime, timezone
import copy
import json
import math
import os
from pathlib import Path
import re
from typing import Any, Mapping, Sequence
import xml.etree.ElementTree as ET
import numpy as np
from scipy.spatial.transform import Rotation
from .full_hand import (
G20_COMBINATION_REQUIRED_TARGET_KEYS,
G20_RIGHT_19_LAYOUT,
MIMIC_DERIVED_FINGER_DIPS,
get_hand_calibration_profile,
validate_compact_payload,
)
from .product import ProductConfig, sha256_file
from .storage import atomic_write_json
from .urdf_zero import get_zero_calibration_profile
PASSIVE_JOINTS = frozenset(
{"thumb_ip", "index_dip", "middle_dip", "ring_dip", "pinky_dip"}
)
ACTIVE_ZERO_JOINTS = frozenset(
get_zero_calibration_profile("right", G20_RIGHT_19_LAYOUT).direct_zero_joints
)
RETAINED_ACTIVE_ZERO_JOINTS = frozenset(
get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT).active_joints
) - ACTIVE_ZERO_JOINTS
def _load_json(path: Path) -> dict[str, Any]:
with path.open("r", encoding="utf-8") as stream:
payload = json.load(stream)
if not isinstance(payload, dict):
raise ValueError(f"{path} must contain a JSON object")
return payload
def _joint_blocks(text: str) -> dict[str, str]:
pattern = re.compile(
r"<joint\b[^>]*\bname\s*=\s*([\"'])(?P<name>[^\"']+)\1[^>]*>.*?</joint>",
re.DOTALL,
)
return {match.group("name"): match.group(0) for match in pattern.finditer(text)}
def _mask_origin_rpy(block: str) -> str:
return re.sub(
r"(<origin\b[^>]*\brpy\s*=\s*)([\"'])[^\"']*\2",
r"\1\2__CALIBRATED_RPY__\2",
block,
count=1,
)
def _mask_active_origin_rpy_fields(text: str) -> str:
pattern = re.compile(
r"<joint\b[^>]*\bname\s*=\s*([\"'])(?P<name>[^\"']+)\1[^>]*>.*?</joint>",
re.DOTALL,
)
def replace(match: re.Match[str]) -> str:
block = match.group(0)
if match.group("name") not in ACTIVE_ZERO_JOINTS:
return block
masked = _mask_origin_rpy(block)
if masked == block:
raise ValueError(
f"active joint {match.group('name')} has no origin.rpy"
)
return masked
return pattern.sub(replace, text)
def _triplet(value: str) -> np.ndarray:
result = np.asarray([float(item) for item in value.split()], dtype=float)
if result.shape != (3,) or not np.all(np.isfinite(result)):
raise ValueError(f"invalid URDF triplet: {value}")
return result
def _joint_elements(path: str | Path) -> dict[str, ET.Element]:
return {
str(joint.get("name")): joint
for joint in ET.parse(Path(path)).getroot().findall("joint")
}
def _verify_expected_origin_offsets(
source: str | Path,
corrected: str | Path,
expected_offsets_rad: Mapping[str, float],
) -> None:
offsets = {str(name): float(value) for name, value in expected_offsets_rad.items()}
if set(offsets) != ACTIVE_ZERO_JOINTS or any(
not math.isfinite(value) for value in offsets.values()
):
raise ValueError("expected offsets must contain 12 finite static-zero values")
before = _joint_elements(source)
after = _joint_elements(corrected)
maximum_rotation_error = 0.0
for name, original_joint in before.items():
corrected_joint = after[name]
original_origin = original_joint.find("origin")
corrected_origin = corrected_joint.find("origin")
if original_origin is None or corrected_origin is None:
if original_origin is not corrected_origin:
raise ValueError(f"corrected URDF changed origin presence for {name}")
continue
original_xyz = _triplet(original_origin.get("xyz", "0 0 0"))
corrected_xyz = _triplet(corrected_origin.get("xyz", "0 0 0"))
if not np.allclose(original_xyz, corrected_xyz, atol=1.0e-12, rtol=0.0):
raise ValueError(f"corrected URDF changed origin.xyz for {name}")
original_rotation = Rotation.from_euler(
"xyz", _triplet(original_origin.get("rpy", "0 0 0"))
)
corrected_rotation = Rotation.from_euler(
"xyz", _triplet(corrected_origin.get("rpy", "0 0 0"))
)
expected_rotation = original_rotation
if name in offsets:
axis_node = original_joint.find("axis")
axis = _triplet(
"1 0 0" if axis_node is None else axis_node.get("xyz", "1 0 0")
)
norm = float(np.linalg.norm(axis))
if norm <= 1.0e-12:
raise ValueError(f"source URDF joint {name} has a degenerate axis")
expected_rotation = expected_rotation * Rotation.from_rotvec(
axis / norm * offsets[name]
)
error = float((expected_rotation.inv() * corrected_rotation).magnitude())
maximum_rotation_error = max(maximum_rotation_error, error)
# Schema v4 intentionally stores zero offsets at eight decimal places.
# Older sessions wrote the corrected URDF from the full-precision solve,
# so comparing that file with the published JSON necessarily permits half
# of one last-place unit. This is about 2.9e-7 degrees and is far below
# any calibration or URDF numerical significance.
if maximum_rotation_error > 5.1e-9:
raise ValueError(
"corrected URDF origin.rpy does not match the published zero offsets"
)
def verify_corrected_urdf(
source: str | Path,
corrected: str | Path,
*,
expected_offsets_rad: Mapping[str, float] | None = None,
) -> tuple[str, ...]:
"""Prove that only active-joint origin.rpy attributes changed.
Passive joint blocks, including their mimic elements, are compared as raw
UTF-8 text so formatting and numeric spelling are protected as well.
"""
source_text = Path(source).read_text(encoding="utf-8")
corrected_text = Path(corrected).read_text(encoding="utf-8")
before = _joint_blocks(source_text)
after = _joint_blocks(corrected_text)
if set(before) != set(after):
raise ValueError("corrected URDF changed the joint set")
# Text patchers may normalize only the final newline. It has no URDF/XML
# semantics; every joint block and every non-rpy byte remains guarded
# below.
if _mask_active_origin_rpy_fields(source_text).rstrip("\r\n") != (
_mask_active_origin_rpy_fields(corrected_text).rstrip("\r\n")
):
raise ValueError("corrected URDF changed content outside active origin.rpy")
changed: list[str] = []
for name in before:
if before[name] == after[name]:
continue
if name not in ACTIVE_ZERO_JOINTS:
raise ValueError(f"corrected URDF changed protected joint {name}")
if _mask_origin_rpy(before[name]) != _mask_origin_rpy(after[name]):
raise ValueError(f"corrected URDF changed more than origin.rpy for {name}")
changed.append(name)
for name in PASSIVE_JOINTS:
if before.get(name) != after.get(name):
raise ValueError(f"passive joint/mimic block changed: {name}")
if expected_offsets_rad is not None:
_verify_expected_origin_offsets(source, corrected, expected_offsets_rad)
return tuple(sorted(changed))
def verify_urdf_mesh_resources(urdf: str | Path) -> dict[str, Path]:
"""Return every session-local mesh after proving it resolves safely."""
path = Path(urdf).expanduser().resolve()
root = ET.parse(path).getroot()
resources: dict[str, Path] = {}
for mesh in root.findall(".//mesh"):
filename = str(mesh.get("filename", "")).strip()
if not filename or "://" in filename or filename.startswith("package:"):
continue
relative = Path(filename)
if relative.is_absolute() or ".." in relative.parts:
raise ValueError(f"URDF has an unsafe local mesh path: {filename}")
resolved = (path.parent / relative).resolve()
try:
resolved.relative_to(path.parent)
except ValueError as error:
raise ValueError(f"URDF mesh escapes the session: {filename}") from error
if not resolved.is_file():
raise ValueError(f"URDF mesh resource is missing: {filename}")
resources[relative.as_posix()] = resolved
return dict(sorted(resources.items()))
def validate_runtime_curves_against_urdf_limits(
payload: Mapping[str, Any], source_urdf: str | Path
) -> None:
"""Reject a runtime curve whose commanded q leaves a CAD safety limit."""
validate_compact_payload(payload)
source_joints = _joint_elements(source_urdf)
for name, calibration in payload["joints"].items():
joint = source_joints.get(str(name))
if joint is None:
raise ValueError(f"source URDF is missing runtime joint {name}")
limit = joint.find("limit")
if limit is None or limit.get("lower") is None or limit.get("upper") is None:
raise ValueError(f"source URDF joint {name} has no finite position limit")
lower = float(limit.get("lower"))
upper = float(limit.get("upper"))
if not math.isfinite(lower) or not math.isfinite(upper) or lower > upper:
raise ValueError(f"source URDF joint {name} has invalid position limits")
curve = np.asarray(calibration["angle_rad"], dtype=float)
minimum = float(np.min(curve))
maximum = float(np.max(curve))
tolerance = 1.0e-7
if minimum < lower - tolerance or maximum > upper + tolerance:
raise ValueError(
f"runtime curve exceeds source URDF limit for {name}: "
f"[{minimum:.9g}, {maximum:.9g}] not within "
f"[{lower:.9g}, {upper:.9g}]"
)
def clamp_compact_payload_to_urdf_limits(
payload: Mapping[str, Any], source_urdf: str | Path
) -> tuple[dict[str, Any], dict[str, int]]:
"""Return a schema-preserving runtime payload bounded by CAD limits."""
result = copy.deepcopy(dict(payload))
validate_compact_payload(result)
source_joints = _joint_elements(source_urdf)
clipped_by_joint: dict[str, int] = {}
for name, calibration in result["joints"].items():
joint = source_joints.get(str(name))
limit = None if joint is None else joint.find("limit")
if (
limit is None
or limit.get("lower") is None
or limit.get("upper") is None
):
raise ValueError(f"source URDF joint {name} has no finite position limit")
lower = float(limit.get("lower"))
upper = float(limit.get("upper"))
if not math.isfinite(lower) or not math.isfinite(upper) or lower > upper:
raise ValueError(f"source URDF joint {name} has invalid position limits")
source = np.asarray(calibration["angle_rad"], dtype=float)
bounded = np.clip(source, lower, upper)
count = int(np.count_nonzero(bounded != source))
if count:
clipped_by_joint[str(name)] = count
calibration["angle_rad"] = [
round(float(value), 8) for value in bounded
]
validate_compact_payload(result)
return result, clipped_by_joint
def build_mujoco_validation_commands(
baseline: Sequence[int],
) -> dict[str, Any]:
if len(baseline) != 20:
raise ValueError("baseline must contain exactly 20 commands")
base = [int(value) for value in baseline]
if any(not 0 <= value <= 255 for value in base):
raise ValueError("baseline commands must be in [0, 255]")
def pose(name: str, label: str, changes: Mapping[int, int]) -> dict[str, Any]:
values = list(base)
for index, value in changes.items():
values[int(index)] = int(value)
return {"name": name, "label_zh": label, "command_u8": values}
return {
"schema_version": 1,
"topic": "/g20/cb_right_hand_control_cmd",
"serial_number": "G20_RIGHT_001",
"poses": [
pose("all_open", "全开", {}),
pose("thumb_middle", "拇指中位", {0: 160, 5: 160, 10: 160, 15: 160}),
pose("index_middle", "食指中位", {1: 160, 6: 127, 16: 160}),
pose("middle_middle", "中指中位", {2: 160, 7: 127, 17: 160}),
pose("ring_middle", "无名指中位", {3: 160, 8: 127, 18: 160}),
pose("pinky_middle", "小指中位", {4: 160, 9: 127, 19: 160}),
pose(
"half_grip",
"四指半握",
{1: 160, 2: 160, 3: 160, 4: 160, 16: 160, 17: 160, 18: 160, 19: 160},
),
pose(
"light_pinch",
"轻捏",
{0: 176, 5: 176, 10: 176, 15: 176, 1: 176, 6: 127, 16: 176},
),
],
}
def atomic_session_pointer(root: str | Path, name: str, session: str | Path) -> Path:
parent = Path(root).resolve()
target = Path(session).resolve()
if target.parent != parent:
raise ValueError("session pointer target must be a direct child of the serial root")
if not target.is_dir():
raise ValueError(f"session directory does not exist: {target}")
if name not in {"latest_attempt", "latest_passed"}:
raise ValueError("unsupported session pointer name")
parent.mkdir(parents=True, exist_ok=True)
destination = parent / name
temporary = parent / f".{name}.{os.getpid()}.tmp"
if temporary.is_symlink():
temporary.unlink()
elif temporary.exists():
raise ValueError(f"temporary pointer path is occupied: {temporary}")
os.symlink(target.name, temporary, target_is_directory=True)
os.replace(temporary, destination)
return destination
def active_offsets(payload: Mapping[str, Any]) -> dict[str, float]:
validate_compact_payload(payload)
result = {
name: float(payload["joints"][name]["zero_angles"]["urdf_zero_offset_rad"])
for name in ACTIVE_ZERO_JOINTS
}
if set(result) != ACTIVE_ZERO_JOINTS or any(
not math.isfinite(value) for value in result.values()
):
raise ValueError("payload does not contain 12 finite observable zero offsets")
return result
def compare_session_offsets(
first: Mapping[str, Any],
second: Mapping[str, Any],
*,
maximum_difference_rad: float,
) -> dict[str, float]:
left = active_offsets(first)
right = active_offsets(second)
differences = {name: abs(left[name] - right[name]) for name in sorted(left)}
failed = {name: value for name, value in differences.items() if value > maximum_difference_rad}
if failed:
details = ", ".join(
f"{name}={math.degrees(value):.3f}deg" for name, value in failed.items()
)
raise ValueError(f"independent-session static-zero mismatch: {details}")
return differences
def session_artifact_paths(session: str | Path, serial_number: str) -> dict[str, Path]:
directory = Path(session).resolve()
urdfs = sorted(directory.glob(f"*zero_calibrated_{serial_number}_*.urdf"))
if len(urdfs) != 1:
raise ValueError("session must contain exactly one corrected URDF")
return {
"json": directory / f"g20_right_{serial_number}_calibration.json",
"urdf": urdfs[0],
"summary": directory / "calibration_summary_zh.json",
"commands": directory / "mujoco_validation_commands.json",
"raw": directory / "raw_samples.jsonl",
"log": directory / "calibration.log",
}
def find_compatible_prior_session(
config: ProductConfig,
current: str | Path,
current_payload: Mapping[str, Any],
) -> tuple[Path | None, dict[str, float]]:
current_path = Path(current).resolve()
for candidate in sorted(config.session_root.glob("20??????_??????"), reverse=True):
if candidate.resolve() == current_path:
continue
json_path = candidate / f"g20_right_{config.serial_number}_calibration.json"
summary_path = candidate / "calibration_summary_zh.json"
if not json_path.is_file() or not summary_path.is_file():
continue
try:
summary = _load_json(summary_path)
if not bool(summary.get("quality", {}).get("passed")):
continue
payload = _load_json(json_path)
differences = compare_session_offsets(
payload,
current_payload,
maximum_difference_rad=config.static_repeatability_rad,
)
except (OSError, ValueError, KeyError, TypeError, json.JSONDecodeError):
continue
return candidate, differences
return None, {}
def _verify_combination_validation(combination: Mapping[str, Any]) -> None:
# The formal product uses the independently held-out fourth sweep cycle.
# The optional eight-pose check is a developer diagnostic because the
# 15-Tag layout has no DIP Tags and the axis-line zero solve does not
# establish an absolute Cartesian hand-base transform. When explicitly
# enabled, retain its strict coverage and error checks.
if not bool(combination.get("enabled")):
return
position_p95 = float(combination.get("position_p95_m", float("inf")))
orientation_p95 = float(
combination.get("orientation_p95_rad", float("inf"))
)
required = {str(value) for value in combination.get("required_targets", ())}
expected = set(G20_COMBINATION_REQUIRED_TARGET_KEYS)
observations = combination.get("observation_counts")
validations = combination.get("validation_counts")
if required != expected:
raise ValueError("combination validation has the wrong required-target set")
if not isinstance(observations, Mapping) or not isinstance(validations, Mapping):
raise ValueError("combination validation is missing per-target coverage")
missing_observations = sorted(
key for key in expected if int(observations.get(key, 0)) < 2
)
missing_validations = sorted(
key for key in expected if int(validations.get(key, 0)) < 1
)
if missing_observations or missing_validations:
raise ValueError(
"combination validation target coverage is incomplete: "
f"observations={','.join(missing_observations) or '-'}; "
f"validations={','.join(missing_validations) or '-'}"
)
if (
not bool(combination.get("completed"))
or int(combination.get("completed_poses", 0)) != 8
or position_p95 > 0.003
or orientation_p95 > math.radians(2.0)
):
raise ValueError(
"eight-pose four-finger combination validation is incomplete or failed"
)
def finalize_session_artifacts(
config: ProductConfig,
session: str | Path,
*,
node_status: Mapping[str, Any],
) -> tuple[dict[str, Any], bool]:
"""Create diagnostics and update latest_passed only after every guard."""
directory = Path(session).resolve()
paths = session_artifact_paths(directory, config.serial_number)
for name in ("json", "urdf", "raw", "log"):
if not paths[name].is_file():
raise ValueError(f"session is missing {name}: {paths[name]}")
payload = _load_json(paths["json"])
validate_compact_payload(payload)
if payload.get("schema_version") != 4 or payload.get("side") != "right":
raise ValueError("runtime JSON is not the compact right-hand schema v4")
if not bool(payload.get("quality", {}).get("passed")):
raise ValueError("runtime JSON quality is not passed")
payload, clipped_runtime_joints = clamp_compact_payload_to_urdf_limits(
payload, config.source_urdf
)
if clipped_runtime_joints:
# A node from before runtime-limit saturation may already have written
# an otherwise valid commit candidate. Normalize it before hashing
# and publication so completed sessions can be recovered offline.
atomic_write_json(paths["json"], payload)
combination = node_status.get("combination_validation")
if not isinstance(combination, Mapping):
raise ValueError("node status is missing combination validation")
_verify_combination_validation(combination)
offsets = active_offsets(payload)
changed_joints = verify_corrected_urdf(
config.source_urdf,
paths["urdf"],
expected_offsets_rad=offsets,
)
validate_runtime_curves_against_urdf_limits(payload, config.source_urdf)
mesh_resources = verify_urdf_mesh_resources(paths["urdf"])
mesh_hashes = {
name: sha256_file(path) for name, path in mesh_resources.items()
}
commands = build_mujoco_validation_commands(payload["baseline_command_u8"])
atomic_write_json(paths["commands"], commands)
prior, differences = find_compatible_prior_session(config, directory, payload)
release_ready = config.required_independent_passes == 1 or prior is not None
quality = dict(payload["quality"])
summary: dict[str, Any] = {
"schema_version": 1,
"serial_number": config.serial_number,
"session_id": f"{config.serial_number}_{directory.name}",
"result": "PASS" if release_ready else "PASS_AWAITING_SECOND_SESSION",
"quality": quality,
"runtime_limit_clipped_bins": clipped_runtime_joints,
"runtime_curve_domain": "requested_command_u8",
"static_zero_calibrated_joints": sorted(ACTIVE_ZERO_JOINTS),
"retained_active_urdf_zero_joints": sorted(
RETAINED_ACTIVE_ZERO_JOINTS
),
"retained_passive_urdf_joints": sorted(PASSIVE_JOINTS),
"mimic_derived_dynamic_joints": sorted(MIMIC_DERIVED_FINGER_DIPS),
"changed_urdf_joint_origins": list(changed_joints),
"hashes": {
"source_urdf_sha256": config.source_urdf_sha256,
"camera_extrinsics_sha256": config.camera_extrinsics_sha256,
"calibration_config_sha256": config.calibration_config_sha256,
"corrected_urdf_sha256": sha256_file(paths["urdf"]),
"mesh_resources_sha256": mesh_hashes,
"calibration_json_sha256": sha256_file(paths["json"]),
"raw_samples_sha256": sha256_file(paths["raw"]),
},
"holdout": {
"training_cycles": [0, 1, 2],
"validation_cycle": 3,
"isolated": True,
},
"combination_validation": dict(combination),
"formal_release": {
"passed": release_ready,
"required_independent_passes": config.required_independent_passes,
"comparison_session": None if prior is None else prior.name,
"maximum_static_difference_deg": (
None if not differences else math.degrees(max(differences.values()))
),
},
"node_status": dict(node_status),
"created_at_utc": datetime.now(timezone.utc).isoformat(),
"artifacts": {name: path.name for name, path in paths.items()},
}
atomic_write_json(paths["summary"], summary)
# Recompute hashes after all files exist and validate the pair once more
# immediately before the one atomic publication operation.
if sha256_file(config.source_urdf) != config.source_urdf_sha256:
raise ValueError("source URDF changed during calibration")
verify_corrected_urdf(
config.source_urdf,
paths["urdf"],
expected_offsets_rad=offsets,
)
validate_runtime_curves_against_urdf_limits(payload, config.source_urdf)
final_mesh_hashes = {
name: sha256_file(path)
for name, path in verify_urdf_mesh_resources(paths["urdf"]).items()
}
if final_mesh_hashes != mesh_hashes:
raise ValueError("URDF mesh resources changed during publication")
if release_ready:
atomic_session_pointer(config.session_root, "latest_passed", directory)
return summary, release_ready
@@ -48,6 +48,10 @@ JOINT_NAMES_ZH = {
"pinky_pip": "小指PIP",
"pinky_dip": "小指DIP(被动)",
"thumb_cmc_yaw": "拇指CMC侧摆",
"index_mcp_roll_side": "食指MCP侧摆(侧面校验)",
"middle_mcp_roll_side": "中指MCP侧摆(侧面校验)",
"ring_mcp_roll_side": "无名指MCP侧摆(侧面校验)",
"pinky_mcp_roll_side": "小指MCP侧摆(侧面校验)",
}
@@ -86,6 +90,12 @@ def _task_text(active: Mapping[str, Any]) -> str:
f"{_format_u8(active.get('target_u8'))}、实际"
f"{_format_u8(active.get('actual_u8'))}"
)
if active.get("kind") == "cross_view_roll_diagnostic":
return (
f"{active.get('finger', '?')}侧摆跨机位诊断完成:"
f"正面最大{float(active.get('front_maximum_deg', 0.0)):.2f}°,"
f"侧面最大{float(active.get('side_maximum_deg', 0.0)):.2f}°"
)
if active.get("kind") == "validation":
return (
f"{view}机位,随机复测,电机{active.get('motor_index')}"
@@ -132,10 +142,19 @@ def three_camera_reason_zh(
context = fields[1] if len(fields) > 1 else "unknown"
error_match = re.search(r"error_u8=([0-9.]+)", reason)
error = error_match.group(1) if error_match else "未知"
timeout_match = re.search(r"timeout_seconds=([0-9.]+)", reason)
timeout_value = active.get("timeout_seconds")
if timeout_value is None and timeout_match is not None:
timeout_value = float(timeout_match.group(1))
duration = (
f"连续{float(timeout_value):g}"
if timeout_value is not None
else "在规定时间内"
)
motor = active.get("motor_index")
if motor is not None:
return (
f"电机{motor}反馈连续8秒没有向目标推进;目标"
f"电机{motor}反馈{duration}没有向目标推进;目标"
f"{_format_u8(active.get('target_u8'))}、实际"
f"{_format_u8(active.get('actual_u8'))}、误差{error} u8"
f"允许容差±{_format_u8(active.get('tolerance_u8'))} u8"
@@ -144,12 +163,29 @@ def three_camera_reason_zh(
"重启;若仍在变化或有摩擦,则先排查机械问题,不要反复resume强推。",
)
return (
f"电机反馈连续8秒没有向目标推进;停止位置距目标{error}个u8"
f"电机反馈{duration}没有向目标推进;停止位置距目标{error}个u8"
f"(阶段={context})。程序已保持当前位置,防止机械碰撞或摩擦加重。",
"检查该电机是否在机械端点稳定饱和或存在碰撞。若实际反馈已是该型号的"
"正常端点,应配置该电机专用端点容差后重启标定;不要反复调用resume强推。",
)
base_reason, separator, reason_detail = reason.partition(":")
if base_reason in {
"sweep_missing_endpoint_bin",
"sweep_bins_too_few",
"sweep_bin_gap_too_large",
"task_precheck_missing_command_127",
"task_precheck_detection_rate_too_low",
"synchronised_tag_state_timeout",
}:
reason = base_reason
detail_label = (
JOINT_NAMES_ZH.get(reason_detail, reason_detail)
if separator and reason_detail
else ""
)
detail_prefix = f"{detail_label}" if detail_label else ""
if "URDF zero offset reached the configured" in reason:
bound_match = re.search(
r"configured\s+([0-9.]+)\s+degree bound", reason
@@ -174,7 +210,7 @@ def three_camera_reason_zh(
if reason == "sweep_missing_endpoint_bin":
missing_text = "".join(str(value) for value in missing) or "0或255"
return (
f"本方向已有{active.get('valid_frames', 0)}帧同步有效数据,但缺少"
f"{detail_prefix}本方向已有{active.get('valid_frames', 0)}帧同步有效数据,但缺少"
f"电机端点{missing_text}附近的有效分箱;采样到的实际电机范围为"
f"{sample_range},端点容差为±{tolerance}。这通常表示电机虽然运动到"
"端点,但该时刻没有同时取得有效Tag图像和电机状态。",
@@ -183,29 +219,64 @@ def three_camera_reason_zh(
)
if reason == "sweep_bins_too_few":
return (
f"有效电机分箱只有{sample.get('bin_count', 0)}个,要求至少"
f"{detail_prefix}有效电机分箱只有{sample.get('bin_count', 0)}个,要求至少"
f"{sample.get('minimum_bin_count', '?')}个;当前采样范围{sample_range}",
"检查Tag连续识别和电机状态频率,修正后调用resume重新扫描当前方向。",
)
if reason == "sweep_bin_gap_too_large":
gap_start = sample.get("maximum_bin_gap_start_u8")
gap_end = sample.get("maximum_bin_gap_end_u8")
gap_range = (
""
if gap_start is None or gap_end is None
else f"{gap_start}{gap_end}"
)
return (
f"轨迹相邻有效电机分箱的最大空缺为{sample.get('maximum_bin_gap', '?')}"
f"允许值不超过{sample.get('allowed_maximum_bin_gap', '?')}",
f"{detail_prefix}轨迹相邻有效电机分箱的最大空缺为{sample.get('maximum_bin_gap', '?')}"
f"{gap_range}允许值不超过"
f"{sample.get('allowed_maximum_bin_gap', '?')}",
"检查运动中Tag是否间歇丢失;修正遮挡、反光或对焦后调用resume。",
)
if reason == "synchronised_tag_state_timeout":
group_reasons = active.get("group_pnp_reasons", {})
if isinstance(group_reasons, Mapping) and group_reasons:
reason_text = "".join(
f"{VIEW_NAMES_ZH.get(str(view), str(view))}={value}"
for view, value in group_reasons.items()
)
return (
f"{detail_prefix}Tag仍可见且反馈正常,但连续图像帧被整组PnP几何检查拒绝"
f"{reason_text}),因此无法与电机状态形成有效轨迹帧。",
"不要调整或反复粘贴Tag;保留当前会话并复制诊断块给开发者检查PnP分支逻辑。",
)
return (
"运动过程中连续超过允许时间没有取得“所需Tag全部有效且能与电机状态"
f"{detail_prefix}运动过程中连续超过允许时间没有取得“所需Tag全部有效且能与电机状态"
"按时间戳配对”的图像帧。",
"查看下面活动机位的缺失Tag,确认状态话题仍在更新;修正后调用resume,"
"程序会重扫当前方向。",
)
if reason == "task_precheck_missing_command_127":
return (
f"{detail_prefix}低速预检没有取得反馈127附近的同步Tag样本。",
"检查中位姿态的Tag遮挡和反光;程序只会重扫当前物理任务。",
)
if reason == "task_precheck_detection_rate_too_low":
return (
f"{detail_prefix}低速预检的有效Tag识别率低于门限。",
"检查该机位当前任务Tag的遮挡、反光和对焦;程序只会重扫当前物理任务。",
)
if reason == "sweep_start_position_timeout":
return (
f"电机{active.get('motor_index')}未在规定时间到达扫描起点"
f"{active.get('start_u8')},当前实际值{_format_u8(active.get('actual_u8'))}",
"检查CAN、机械手使能和是否存在机械卡阻,确认安全后调用resume。",
)
if reason == "sweep_start_tag_timeout":
return (
"被测电机已经到达扫描起点,但当前任务所需的实时运动Tag没有形成足够的"
"同步有效帧。允许遮挡的固定掌部Tag会显示为“锁”,不会触发此错误。",
"只检查标记为✗的实时运动Tag、反光和外部遮挡;不要移动相机或手掌底座。",
)
if reason == "sweep_timeout":
return (
"当前方向在规定时间内未完成端点到达、有效帧数和行程覆盖要求。",
@@ -226,7 +297,7 @@ def three_camera_reason_zh(
"随机复测位置没有采集到足够的同步有效Tag帧。",
"检查当前机位Tag可见性后调用resume。",
)
if reason == "joint_fit_check_failed":
if reason in {"joint_fit_check_failed", "joint_fit_systematic_failure"}:
metric_names = {
"plane_rms_mm": "平面拟合RMS",
"radial_rms_mm": "圆半径拟合RMS",
@@ -237,6 +308,9 @@ def three_camera_reason_zh(
"arc_deg": "实测圆弧",
"monotonic_correction_deg": "最大单调修正",
"hysteresis_deg": "最大正反程差",
"baseline_hysteresis_deg": "baseline正反程关节角差",
"baseline_directional_gap_deg": "baseline方向分支间隙",
"baseline_directional_gap_range_deg": "baseline分支间隙跨轮极差",
"cycle_travel_range_deg": "三轮行程差",
"rotation_orthogonal_rms_deg": "三维旋转轴外残差RMS",
"axis_plane_rms_mm": "三维圆轴向RMS",
@@ -259,6 +333,9 @@ def three_camera_reason_zh(
"arc_deg": "°",
"monotonic_correction_deg": "°",
"hysteresis_deg": "°",
"baseline_hysteresis_deg": "°",
"baseline_directional_gap_deg": "°",
"baseline_directional_gap_range_deg": "°",
"cycle_travel_range_deg": "°",
"rotation_orthogonal_rms_deg": "°",
"axis_plane_rms_mm": "mm",
@@ -288,9 +365,14 @@ def three_camera_reason_zh(
)
cycle_travel = failure.get("cycle_travel_deg", [])
if cycle_travel:
detail += "轮=" + "/".join(
detail += "轮=" + "/".join(
f"{float(value):.2f}°" for value in cycle_travel
) + ""
cycle_values = failure.get("cycle_values_deg", [])
if cycle_values:
detail += "(各轮=" + "/".join(
f"{float(value):.2f}°" for value in cycle_values
) + ""
details.append(detail)
else:
cycle = failure.get("cycle")
@@ -300,11 +382,30 @@ def three_camera_reason_zh(
f"{failure.get('reason', '未知原因')}"
)
detail_text = "".join(details) or "当前关节的轨迹拟合未通过"
directional_gap_failure = any(
str(failure.get("metric", "")).startswith(
"baseline_directional_gap"
)
for failure in active.get("failures", [])
)
if reason == "joint_fit_systematic_failure":
suggestion = (
"各轮重复出现同一模型冲突,继续运动不会改善;程序已禁止自动重扫。"
"请直接复制诊断块给开发者,不要放宽门限。"
)
else:
suggestion = (
"方向分支已由软件保留,不要放宽门限;请检查传动回差或高支架刚度,"
"处理后重新执行一键标定命令,程序会从最近可靠断点继续。"
if directional_gap_failure
else "修正Tag位置、遮挡或机械行程后重新执行一键标定命令;"
"程序只清除当前失败关节的数据并重扫"
f"{active.get('directions_to_rescan', 6)}个方向,"
"不需要手工调用ROS服务。"
)
return (
detail_text + "。程序已在当前关节结束后立即停止后续步骤。",
"修正Tag位置、遮挡或机械行程后调用"
"/g20_calibration/resume;程序只清除当前失败关节的数据"
f"并重扫{active.get('directions_to_rescan', 6)}个方向,不要调用start。",
suggestion,
)
if reason == "zero_model_validation_failed":
reason_names = {
@@ -345,11 +446,37 @@ def three_camera_reason_zh(
"该类稳定模型失败不能靠重复运动修复,程序不会自动重扫;"
"请检查Tag固定、相机外参和原始URDF后重新启动新标定。",
)
if reason in {"waiting_for_devices_and_sdk", "device_preflight_lost"}:
return (
"正在等待三台相机数据、内外参身份以及机械手SDK反馈就绪;此阶段不以Tag可见性阻止基准恢复。",
"保持机械手运动范围无障碍;设备就绪后系统会先安全恢复基准形态,再检查掌部Tag。",
)
if reason in {"waiting_for_three_cameras_tags_and_sdk", "preflight_lost"}:
return (
"正在等待三台相机内参、外参身份匹配、帧率、全部必需Tag以及机械手SDK同时就绪。",
"根据下面每个机位的缺失Tag和有效率排查;全部就绪后程序会进入等待开始状态。",
)
if reason == "waiting_for_baseline_tags_after_recovery":
return (
"机械手已经稳定恢复到基准形态,正在用新采集的画面确认三台相机各自的固定掌部Tag。",
"若某个掌部Tag持续缺失,只调整遮挡手指或检查Tag固定情况,不要移动相机和手掌底座。",
)
if reason == "locking_fixed_base_references":
return (
"基准形态Tag预检已通过,正在把三台相机的固定掌部Tag稳健锁定为本会话参考。",
"无需操作;锁定完成后允许任务姿态遮挡固定掌部Tag。",
)
if reason == "waiting_for_task_tags_at_sweep_start":
return (
"电机已到扫描起点,正在等待当前任务的实时运动Tag;显示为“锁”的固定掌部Tag"
"允许被手指遮挡。",
"只检查标记为✗的实时运动Tag;若均为✓或锁,程序会自动开始运动。",
)
if reason == "call_start_for_baseline_recovery":
return (
"相机数据和机械手反馈已就绪,等待一键程序触发安全基准恢复。",
"保持机械手运动范围无障碍;程序会自动开始,无需手工调用ROS服务。",
)
if reason == "call_start":
return (
"三机位预检已经通过,等待操作员确认开始。",
@@ -361,14 +488,62 @@ def three_camera_reason_zh(
return "操作员终止了本次标定,程序保持终止时的当前姿态。", "需要重新启动一次新标定。"
if reason == "collecting_timestamp_synchronised_tag_centres":
return "正在按时间戳配对Tag图像和电机状态并采集当前轨迹。", "无需操作,保持相机、标签和底座不动。"
if reason == "collecting_dedicated_baseline_hold":
return (
"正在从当前方向到达关节baseline并静止采集Tag与电机反馈;这批数据单独用于回差验收。",
"无需操作,保持相机、标签和底座不动。",
)
if reason == "steady checkpoint target is missing":
return (
"首轮稳态检查点已经到达最终端点,但采集状态没有及时切换到端点完成阶段。",
"程序已停止发布并保留已采样数据;这是软件状态切换问题,不需要调整相机、Tag或机械手。",
)
if reason == "cross_view_roll_front_failure_deferred":
return (
"正面侧摆回差不合格已保留,诊断模式将继续采集同一手指的侧面数据。",
"无需操作;该诊断会锁定URDF发布。",
)
if reason == "cross_view_roll_diagnostic_complete":
interpretation = str(active.get("interpretation", ""))
explanations = {
"both_views_confirm_direction_dependent_pose": (
"正面和侧面都确认了方向相关姿态,优先判断为roll输出机构或共同下游链的真实回差。"
),
"front_only_difference_check_roll_tag_bracket_or_front_pnp": (
"只有正面差异超限,优先检查roll Tag高支架刚度和正面PnP。"
),
"side_only_difference_check_side_tag_chain_or_side_pnp": (
"只有侧面差异超限,优先检查侧面Tag链和侧面PnP。"
),
"both_views_within_formal_hysteresis_limit": (
"两个机位的静止回差均满足正式门限。"
),
}
return (
explanations.get(interpretation, "四指侧摆跨机位诊断已经完成。")
+ " 本次为诊断会话,不会生成或发布URDF。",
"保存当前状态和raw_samples.jsonl;根据两机位结论处理后重新启动正式标定。",
)
if reason == "capturing_random_validation_pose":
return "正在当前随机命令位置采集复测数据。", "无需操作,保持设备不动。"
if reason in {"calibration_passed", "calibration_complete"}:
return "三维轨迹、关节轴零位和第三轮留出验证已经完成。", "检查JSON、修正URDF路径和quality.passed。"
if reason == "quality_failed":
return "标定流程完成,但拟合或随机复测质量没有达到验收阈值。", "检查最终JSON的quality以及启动终端中的拟合日志。"
if reason == "combination_pose_prediction_failed":
return (
"单关节、零位和URDF几何验证已通过,但当前多关节组合姿态的Tag实测位姿与模型预测超过门限。",
"程序会在原姿态重新初始化PnP并自动复测;若最终仍失败,请把raw_samples.jsonl中的"
"combination_validation_failure记录交给开发者,不要重新采集16个单关节任务。",
)
if reason.startswith("prepare_") or state == "PREPARE_SWEEP":
return "正在把当前电机移动到本方向的扫描起点并等待稳定。", "无需操作。"
if reason == "holding_same_finger_clearance_before_next_task":
return (
"同一根手指的上一项已经完成;相邻手指继续保持当前避让姿态,只调整"
"被测关节以衔接下一项。",
"无需操作,不要手动展开正在避让的手指。",
)
if state == "RETURN_BASELINE":
return "正在把已使用的标定电机恢复到目标姿态。", "无需操作。"
if state == "FITTING":
@@ -431,6 +606,14 @@ def render_three_camera_status_text_zh(payload: Mapping[str, Any]) -> str:
f"实际采样范围{_format_u8(sample.get('minimum_u8'))}"
f"{_format_u8(sample.get('maximum_u8'))}"
)
detection_frames = int(active.get("detection_frames", 0))
if detection_frames:
lines.append(
"本方向Tag检出:"
f"{float(active.get('detection_rate', 0.0)):.1%}"
f"{active.get('detection_valid_frames', 0)}/"
f"{detection_frames}帧)"
)
auxiliary = active.get("auxiliary_motors", [])
if auxiliary:
lines.append(
File diff suppressed because it is too large Load Diff
@@ -634,6 +634,7 @@ def _fit_joint_curve(
values: Sequence[float],
*,
endpoint_reference: Mapping[str, Sequence[float]] | None = None,
preserve_direction_offset: bool = False,
) -> tuple[dict[str, Any], float, float]:
by_direction: dict[str, list[list[float]]] = {
direction: [[] for _ in range(256)] for direction in DIRECTIONS
@@ -669,9 +670,11 @@ def _fit_joint_curve(
],
dtype=float,
)
raw -= raw[-1]
if not preserve_direction_offset:
raw -= raw[-1]
projected_samples = isotonic_nonincreasing(raw)
projected_samples -= projected_samples[-1]
if not preserve_direction_offset:
projected_samples -= projected_samples[-1]
maximum_correction = max(
maximum_correction,
float(np.max(np.abs(projected_samples - raw))),
@@ -681,7 +684,8 @@ def _fit_joint_curve(
commands.astype(float),
projected_samples,
)
curve -= curve[255]
if not preserve_direction_offset:
curve -= curve[255]
if endpoint_reference is not None:
curve = _regularize_coupled_zero_tail(
curve,
@@ -695,7 +699,8 @@ def _fit_joint_curve(
increasing = np.asarray(fitted[DIRECTION_INCREASING], dtype=float)
hysteresis = float(np.max(np.abs(decreasing - increasing)))
combined = 0.5 * (decreasing + increasing)
combined -= combined[255]
if not preserve_direction_offset:
combined -= combined[255]
return (
{
"angle_rad": [round(float(value), 8) for value in combined],
File diff suppressed because it is too large Load Diff
@@ -1,4 +1,4 @@
"""Publish calibrated G20 URDF joint angles from raw u8 commands."""
"""Publish calibrated G20 URDF angles from raw command/feedback u8 values."""
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
@@ -3,6 +3,7 @@
from __future__ import annotations
from datetime import datetime
import hashlib
from pathlib import Path
import re
@@ -41,6 +42,28 @@ def _launch_stack(context):
hand_type = LaunchConfiguration("hand_type").perform(context).lower()
if hand_type not in {"left", "right"}:
raise RuntimeError("hand_type must be left or right")
tag_layout = LaunchConfiguration("tag_layout").perform(context).lower()
if tag_layout not in {"legacy_11", "g20_right_15"}:
raise RuntimeError("tag_layout must be legacy_11 or g20_right_15")
if tag_layout == "g20_right_15" and hand_type != "right":
raise RuntimeError("g20_right_15 requires hand_type:=right")
requested_tag_config = LaunchConfiguration("tag_config").perform(context)
package_share = Path(
get_package_share_directory("g20_thumb_apriltag_calibration")
)
tag_config = (
Path(requested_tag_config).expanduser().resolve()
if requested_tag_config
else package_share
/ "config"
/ (
"three_camera_tags_g20_right_15.yaml"
if tag_layout == "g20_right_15"
else "three_camera_tags.yaml"
)
)
if not tag_config.is_file():
raise RuntimeError(f"tag config does not exist: {tag_config}")
command_topic = f"/g20/cb_{hand_type}_hand_control_cmd"
state_topic = f"/g20/cb_{hand_type}_hand_state"
info_topic = f"/g20/cb_{hand_type}_hand_info"
@@ -52,6 +75,20 @@ def _launch_stack(context):
)
if not source_urdf.is_file():
raise RuntimeError(f"source URDF does not exist: {source_urdf}")
expected_source_hash = LaunchConfiguration(
"source_urdf_expected_sha256"
).perform(context).strip().lower()
if tag_layout == "g20_right_15":
if re.fullmatch(r"[0-9a-f]{64}", expected_source_hash) is None:
raise RuntimeError(
"g20_right_15 requires source_urdf_expected_sha256 confirmed "
"by the CAD/hardware owner"
)
actual_source_hash = hashlib.sha256(source_urdf.read_bytes()).hexdigest()
if actual_source_hash != expected_source_hash:
raise RuntimeError(
"source_urdf_expected_sha256 does not match source_urdf_path"
)
hand_serial = LaunchConfiguration("serial_number").perform(context)
if (
@@ -163,7 +200,7 @@ def _launch_stack(context):
name="apriltag",
namespace=detector_namespace,
parameters=[
LaunchConfiguration("tag_config"),
str(tag_config),
{
"detector.decimate": ParameterValue(
LaunchConfiguration("apriltag_decimate"),
@@ -234,7 +271,11 @@ def _launch_stack(context):
{
"serial_number": hand_serial,
"hand_type": hand_type,
"tag_layout": tag_layout,
"session_dir": str(session_dir),
"resume_raw_samples_path": LaunchConfiguration(
"resume_raw_samples_path"
),
# The SDK performs roughly 25 synchronous CAN queries whenever
# cb_<side>_hand_info has a subscriber. Calibration only used
# that topic to display a speed diagnostic, while those reads
@@ -246,6 +287,9 @@ def _launch_stack(context):
"camera_extrinsics_file"
),
"source_urdf_path": str(source_urdf),
"source_urdf_expected_sha256": LaunchConfiguration(
"source_urdf_expected_sha256"
),
"corrected_urdf_output_dir": LaunchConfiguration(
"corrected_urdf_output_dir"
),
@@ -267,6 +311,13 @@ def _launch_stack(context):
LaunchConfiguration("index_flex_calibration_speed"),
value_type=int,
),
"adaptive_formal_speed_enabled": ParameterValue(
LaunchConfiguration("adaptive_formal_speed_enabled"),
value_type=bool,
),
"cross_view_roll_diagnostic_finger": LaunchConfiguration(
"cross_view_roll_diagnostic_finger"
),
"validation_enabled": ParameterValue(
LaunchConfiguration("validation_enabled"), value_type=bool
),
@@ -300,7 +351,7 @@ def _launch_stack(context):
return [
LogInfo(
msg=(
f"G20 {hand_type} three-camera session: {session_dir}; "
f"G20 {hand_type} {tag_layout} three-camera session: {session_dir}; "
f"source_urdf={source_urdf}"
)
),
@@ -342,6 +393,7 @@ def generate_launch_description() -> LaunchDescription:
value=str(package_share / "config" / "fastdds_large_images.xml"),
),
DeclareLaunchArgument("hand_type", default_value="left"),
DeclareLaunchArgument("tag_layout", default_value="legacy_11"),
DeclareLaunchArgument("serial_number", default_value="UNSET"),
DeclareLaunchArgument(
"front_camera_serial", default_value="DB2163742"
@@ -387,6 +439,12 @@ def generate_launch_description() -> LaunchDescription:
DeclareLaunchArgument(
"index_flex_calibration_speed", default_value="10"
),
DeclareLaunchArgument(
"adaptive_formal_speed_enabled", default_value="true"
),
DeclareLaunchArgument(
"cross_view_roll_diagnostic_finger", default_value=""
),
DeclareLaunchArgument("validation_enabled", default_value="false"),
DeclareLaunchArgument(
"camera_extrinsics_file",
@@ -397,6 +455,9 @@ def generate_launch_description() -> LaunchDescription:
DeclareLaunchArgument(
"source_urdf_path", default_value=""
),
DeclareLaunchArgument(
"source_urdf_expected_sha256", default_value=""
),
DeclareLaunchArgument(
"corrected_urdf_output_dir", default_value=""
),
@@ -409,6 +470,7 @@ def generate_launch_description() -> LaunchDescription:
default_value=str(Path.cwd() / "calibration_output"),
),
DeclareLaunchArgument("session_dir", default_value=""),
DeclareLaunchArgument("resume_raw_samples_path", default_value=""),
DeclareLaunchArgument(
"calibration_config",
default_value=str(
@@ -417,9 +479,7 @@ def generate_launch_description() -> LaunchDescription:
),
DeclareLaunchArgument(
"tag_config",
default_value=str(
package_share / "config" / "three_camera_tags.yaml"
),
default_value="",
),
OpaqueFunction(function=_launch_stack),
]
@@ -3,7 +3,7 @@
<package format="3">
<name>g20_thumb_apriltag_calibration</name>
<version>0.1.0</version>
<description>Three-view Hikrobot AprilTag calibration for the complete left G20 hand.</description>
<description>One-command three-camera AprilTag calibration and zero-URDF correction for the G20 right hand.</description>
<maintainer email="support@linker-robotics.com">lxp</maintainer>
<license>MIT</license>
+5 -1
View File
@@ -26,7 +26,7 @@ setup(
zip_safe=True,
maintainer="lxp",
maintainer_email="support@linker-robotics.com",
description="Three-view Hikrobot AprilTag calibration for the complete left G20 hand",
description="One-command three-camera AprilTag calibration for the G20 right hand",
license="MIT",
entry_points={
"console_scripts": [
@@ -64,6 +64,10 @@ setup(
"g20_thumb_apriltag_calibration."
"calibrated_joint_state_bridge:main"
),
(
"calibrate_g20_right = "
"g20_thumb_apriltag_calibration.one_command:main"
),
],
},
)
@@ -6,10 +6,16 @@ from g20_thumb_apriltag_calibration.calibrated_joint_state_bridge import (
G20_COMMAND_NAMES,
G20_URDF_JOINT_NAMES,
CalibratedCommandMapper,
default_input_topic,
)
from g20_thumb_apriltag_calibration.full_hand import (
JointCurveFit,
build_compact_payload,
get_hand_calibration_profile,
)
from g20_thumb_apriltag_calibration.urdf_zero import (
get_zero_calibration_profile,
)
def _payload(side: str = "right") -> dict:
@@ -85,3 +91,87 @@ def test_mapper_rejects_incomplete_named_command() -> None:
mapper = CalibratedCommandMapper(_payload(), expected_side="right")
with pytest.raises(ValueError, match="missing named channels"):
mapper.map_positions([255.0], ["thumb_cmc_pitch"])
def test_right_19_schema_v4_mapper_uses_requested_command_midpoint_curve() -> None:
profile = get_hand_calibration_profile("right", "g20_right_15")
zero = get_zero_calibration_profile("right", "g20_right_15")
baseline = [255] * 20
baseline[6:10] = [127] * 4
fits = {}
diagnostics = {}
for name, spec in profile.joint_specs.items():
zero_command = baseline[spec.motor_index]
average = tuple(
0.001 * (zero_command - value) for value in range(256)
)
decreasing = tuple(1.1 * value for value in average)
increasing = tuple(0.9 * value for value in average)
fits[name] = JointCurveFit(
average,
decreasing,
increasing,
{},
0.0,
0.0,
{"rotation_orthogonal_rms_rad": 0.0, "arc_rad": 0.5},
)
diagnostics[name] = {
"cycle_travel_rad": [0.25] * 4,
"cycle_travel_range_rad": 0.0,
"baseline_hysteresis_by_cycle_rad": [0.0] * 4,
"holdout_cycle": 3,
"holdout_cycle_mae_rad": 0.0,
"holdout_cycle_max_rad": 0.0,
}
payload = build_compact_payload(
serial_number="TEST_RIGHT_V5",
measured_fits=fits,
urdf_zero_offsets_rad={name: 0.0 for name in profile.active_joints},
validation_errors_rad=[0.0],
passed=True,
baseline=baseline,
side="right",
layout_id="g20_right_15",
zero_uncertainty_rad={name: 0.0 for name in zero.direct_zero_joints},
zero_cycle_offsets_rad={
name: (0.0, 0.0, 0.0) for name in zero.direct_zero_joints
},
zero_observers=zero.offset_observer_joint,
artifact_hashes={
"source_urdf_sha256": "0" * 64,
"camera_extrinsics_sha256": "1" * 64,
"corrected_urdf_sha256": "2" * 64,
},
cross_view_roll_metrics={
f"{finger}_mcp_roll": {"angle_rad_rms_difference_rad": 0.0}
for finger in ("index", "middle", "ring", "pinky")
},
joint_dynamic_diagnostics=diagnostics,
zero_geometry_diagnostics={
"training_cycles": (0, 1, 2),
"validation_cycle": 3,
"axis_line_rms_m": 0.0,
"validation_line_error_by_joint_m": {},
},
)
mapper = CalibratedCommandMapper(payload, expected_side="right")
command = [255.0] * 20
command[0] = 100.0
mapped = dict(zip(G20_URDF_JOINT_NAMES, mapper.map_positions(command)))
assert mapper.input_domain == "command_u8"
assert mapper.layout_id == "g20_right_15"
# midpoint(1.1, 0.9) is the original 0.001-rad/u8 curve.
assert mapped["thumb_cmc_pitch"] == pytest.approx(0.155)
assert payload["joints"]["pinky_dip"]["passive"] is True
assert "source_joint" not in payload["joints"]["pinky_dip"]
def test_default_topics_use_the_g20_sdk_namespace() -> None:
assert default_input_topic("right", "command_u8") == (
"/g20/cb_right_hand_control_cmd"
)
assert default_input_topic("right", "feedback_u8") == (
"/g20/cb_right_hand_state"
)
@@ -80,6 +80,36 @@ def test_three_camera_tag_ids_and_topics_use_eleven_unique_tags() -> None:
]["ros__parameters"]["tag"]["frames"][0] == "side_base"
def test_right_15_tag_config_matches_the_physical_layout() -> None:
tags = yaml.safe_load(
(
PACKAGE_ROOT
/ "config"
/ "three_camera_tags_g20_right_15.yaml"
).read_text()
)
expected = {
"front": [0, 1, 2, 3, 10, 11, 12, 13],
"side": [4, 5, 6, 15, 17],
"top": [8, 9],
}
all_ids: set[int] = set()
for view, ids in expected.items():
parameters = tags[
f"/g20_calibration/{view}/apriltag/apriltag"
]["ros__parameters"]
assert parameters["tag"]["ids"] == ids
assert parameters["tag"]["sizes"] == [0.016] * len(ids)
all_ids.update(ids)
assert all_ids == set(range(19)) - {7, 14, 16, 18}
side_frames = tags[
"/g20_calibration/side/apriltag/apriltag"
]["ros__parameters"]["tag"]["frames"]
assert side_frames == [
"side_base", "ring_pip", "pinky_pip", "middle_pip", "index_pip",
]
def test_three_camera_calibration_has_hard_preflight_and_three_rounds() -> None:
config = yaml.safe_load(
(PACKAGE_ROOT / "config" / "three_camera_calibration.yaml").read_text()
@@ -114,14 +144,35 @@ def test_three_camera_calibration_has_hard_preflight_and_three_rounds() -> None:
assert parameters["normal_calibration_speed"] == 15
assert parameters["index_roll_calibration_speed"] == 5
assert parameters["index_flex_calibration_speed"] == 10
assert parameters["adaptive_formal_speed_enabled"] is True
assert parameters["adaptive_formal_speed_max_scale"] == 1.5
assert parameters["adaptive_formal_speed_minimum_bins"] == 64
assert parameters["adaptive_formal_speed_maximum_bin_gap"] == 8
assert parameters["right_thumb_yaw_255_endpoint_tolerance_u8"] == 5.0
assert parameters["speed_setting_settle_seconds"] >= 0.2
assert parameters["top_pnp_invalid_reset_seconds"] == 1.0
assert parameters["pnp_group_initialization_frames"] == 8
assert parameters["pnp_group_normal_alignment_scale_deg"] == 5.0
assert parameters["pnp_group_maximum_normal_alignment_deg"] == 15.0
assert parameters["thumb_ip_pnp_coupling_multiplier"] == 1.03
assert parameters["thumb_ip_pnp_coupling_scale_deg"] == 3.0
assert parameters["thumb_ip_pnp_maximum_coupling_residual_deg"] == 7.5
assert parameters["baseline_hold_seconds"] == 0.5
assert parameters["minimum_baseline_hold_frames"] == 10
assert parameters["directional_zero_maximum_branch_gap_deg"] == 2.0
assert parameters["directional_zero_maximum_branch_gap_range_deg"] == 0.3
assert (
parameters["cross_view_roll_maximum_branch_gap_difference_deg"]
== 0.3
)
assert parameters["cross_view_roll_diagnostic_finger"] == ""
assert parameters["repetitions"] == 3
assert parameters["g20_right_19_repetitions"] >= 4
assert parameters["validation_enabled"] is False
assert parameters["combination_validation_enabled"] is False
assert parameters["combination_validation_frames"] >= 10
assert parameters["combination_maximum_position_p95_m"] <= 0.003
assert parameters["combination_maximum_orientation_p95_deg"] <= 2.0
assert parameters["minimum_detection_rate"] == 0.95
assert parameters["minimum_detection_hz"] == 15.0
assert parameters["minimum_state_span_u8"] >= 240.0
@@ -130,14 +181,15 @@ def test_three_camera_calibration_has_hard_preflight_and_three_rounds() -> None:
assert parameters["pinky_pip_zero_endpoint_tolerance_u8"] == 5.0
assert parameters["minimum_sweep_bins"] >= 32
assert parameters["maximum_bin_gap"] <= 16
assert parameters["automatic_sweep_retry_limit"] == 3
assert parameters["automatic_sweep_retry_limit"] == 2
assert parameters["automatic_fit_retry_limit"] == 2
assert parameters["motor_stall_timeout_seconds"] >= 5.0
assert parameters["motor_stall_timeout_seconds"] == 2.0
assert parameters["motor_stall_startup_grace_seconds"] == 1.0
assert parameters["motor_stall_minimum_progress_u8"] == 1.0
assert parameters["automatic_motion_retry_limit"] == 2
assert parameters["provisional_warning_ratio"] == 1.25
assert parameters["retry_speed_scales"] == [0.8, 0.6, 0.5]
assert parameters["retry_endpoint_hold_seconds"] == [0.75, 1.0, 1.25]
assert parameters["retry_speed_scales"] == [0.8, 0.6]
assert parameters["retry_endpoint_hold_seconds"] == [0.75, 1.0]
assert parameters["position_timeout_seconds"] >= 20.0
assert parameters["maximum_state_image_skew_ms"] <= 50.0
assert parameters["axis_maximum_rotation_circle_difference_deg"] <= 1.0
@@ -147,6 +199,7 @@ def test_three_camera_calibration_has_hard_preflight_and_three_rounds() -> None:
assert parameters["passive_maximum_rotation_orthogonal_rms_deg"] == 7.5
assert parameters["zero_maximum_axis_cycle_difference_deg"] <= 0.75
assert parameters["zero_maximum_axis_cone_mismatch_deg"] <= 5.0
assert parameters["zero_maximum_observability_condition_number"] >= 1.0
assert parameters["zero_maximum_offset_deg"] <= 20.0
assert parameters["zero_finger_maximum_offset_deg"] <= 3.0
assert parameters["image_trajectory_maximum_radial_rms_px"] <= 2.0
@@ -155,7 +208,13 @@ def test_three_camera_calibration_has_hard_preflight_and_three_rounds() -> None:
assert parameters["trajectory_maximum_cycle_travel_difference_deg"] <= 3.0
assert parameters["passive_maximum_cycle_travel_difference_deg"] <= 10.0
assert parameters["passive_maximum_monotonic_correction_deg"] <= 3.0
assert parameters["passive_maximum_hysteresis_deg"] <= 7.5
assert parameters["maximum_hysteresis_deg"] <= 2.0
assert parameters["passive_maximum_hysteresis_deg"] <= 2.0
assert parameters["command_maximum_direction_gap_deg"] <= 2.0
assert parameters["maximum_validation_mae_deg"] <= 1.0
assert parameters["maximum_validation_p95_deg"] <= 2.0
assert parameters["maximum_validation_error_deg"] <= 3.0
assert parameters["zero_maximum_confidence_half_width_deg"] <= 1.5
for view in ("front", "side", "top"):
assert parameters[f"{view}_camera_info_topic"].startswith(
f"/g20_calibration/{view}/"
@@ -1,5 +1,6 @@
import math
from dataclasses import replace
from pathlib import Path
import numpy as np
import pytest
@@ -8,18 +9,26 @@ from g20_thumb_apriltag_calibration.full_hand import (
ACTIVE_JOINTS,
IMAGE_TRAJECTORY_JOINTS,
JOINT_SPECS,
JointCurveFit,
LEFT_HAND_PROFILE,
MEASURED_JOINTS,
PASSIVE_JOINTS,
RIGHT_HAND_PROFILE,
RIGHT_19_HAND_PROFILE,
SPLAY_JOINTS,
SWEEP_SPECS,
THREE_CAMERA_BASELINE_COMMAND,
VIEW_TAGS,
build_calibration_motion_command,
build_calibration_preparation_waypoints,
build_calibration_return_waypoints,
build_calibration_speed_profile,
build_compact_payload,
build_full_hand_command,
clamp_runtime_fits_to_urdf_limits,
center_splay_curve,
compare_cross_view_roll_curves,
derive_mimic_passive_fits,
fit_joint_center_curve,
fit_joint_image_curve,
fit_measured_joint_curve,
@@ -30,6 +39,375 @@ from g20_thumb_apriltag_calibration.full_hand import (
)
def test_runtime_fit_saturates_at_source_urdf_limit() -> None:
values = tuple(np.linspace(0.0, 2.0, 256))
fit = JointCurveFit(
angle_rad=values,
decreasing_rad=values,
increasing_rad=values,
circle={},
maximum_monotonic_correction_rad=0.0,
maximum_hysteresis_rad=0.0,
quality={},
)
bounded = clamp_runtime_fits_to_urdf_limits(
Path(__file__).resolve().parents[3]
/ "src/linkerhand_retarget/linkerhand_retarget/assets/robots/"
"hands/linker_hand/g20_right/linkerhand_g20_right.urdf",
{"thumb_cmc_yaw": fit},
)["thumb_cmc_yaw"]
assert max(bounded.angle_rad) == pytest.approx(1.57)
assert max(bounded.decreasing_rad) == pytest.approx(1.57)
assert bounded.circle["runtime_urdf_limit_clipped_bins"]["angle_rad"] > 0
def test_right_15_profile_has_exact_layout_tasks_and_derived_dips() -> None:
profile = get_hand_calibration_profile("right", "g20_right_15")
assert profile is RIGHT_19_HAND_PROFILE
assert len(profile.sweep_specs) == 16
assert len(profile.measured_joints) == 17
assert len(profile.record_joints) == 21
assert list(profile.view_tags["front"].values()) == [
0, 1, 2, 3, 10, 11, 12, 13
]
assert list(profile.view_tags["side"].values()) == [
4, 5, 6, 15, 17
]
assert list(profile.view_tags["top"].values()) == [8, 9]
assert profile.preflight_view_roles == {
"front": ("front_base",),
"side": ("side_base",),
"top": ("top_base",),
}
assert [spec.task_name for spec in profile.sweep_specs[:4]] == [
"thumb_cmc_pitch_front",
"thumb_cmc_roll_front",
"thumb_mcp_ip_front",
"thumb_cmc_yaw_top",
]
assert [
spec.task_name for spec in profile.sweep_specs[4:7]
] == [
"pinky_roll_multiview",
"pinky_pitch_side",
"pinky_pip_side",
]
assert profile.sweep_specs[4].joints == (
"pinky_mcp_roll",
"pinky_mcp_roll_side",
)
assert {
name: spec.source_joint
for name, spec in profile.joint_specs.items()
if spec.source_joint is not None
} == {
"index_dip": "index_pip",
"middle_dip": "middle_pip",
"ring_dip": "ring_pip",
"pinky_dip": "pinky_pip",
}
def test_right_15_derives_four_dip_curves_from_source_urdf_mimic() -> None:
profile = RIGHT_19_HAND_PROFILE
values = tuple(0.002 * (255 - command) for command in range(256))
fit = JointCurveFit(values, values, values, {}, 0.0, 0.01, {})
measured = {name: fit for name in profile.measured_joints}
source_urdf = (
Path(__file__).resolve().parents[3]
/ "src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/"
"linker_hand/g20_right/linkerhand_g20_right.urdf"
)
runtime = derive_mimic_passive_fits(
source_urdf, measured, profile=profile
)
assert set(runtime) == set(profile.joint_specs)
for finger in ("index", "middle", "ring", "pinky"):
pip = runtime[f"{finger}_pip"]
dip = runtime[f"{finger}_dip"]
assert dip.angle_rad == pytest.approx(
tuple(0.89 * value for value in pip.angle_rad)
)
assert dip.maximum_hysteresis_rad == pytest.approx(0.0089)
assert dip.circle["curve_source"] == f"{finger}_pip"
assert dip.circle["visual_measurement"] is False
def _changed_motors(
previous: tuple[int, ...] | list[int], waypoint: tuple[int, ...]
) -> list[int]:
return [
index
for index, (left, right) in enumerate(zip(previous, waypoint))
if left != right
]
def _phase_changes(
previous: tuple[int, ...] | list[int],
waypoints: tuple[tuple[int, ...], ...],
) -> list[list[int]]:
phases: list[list[int]] = []
for waypoint in waypoints:
phases.append(_changed_motors(previous, waypoint))
previous = waypoint
return phases
def test_right_19_side_clearance_waypoints_group_joint_classes() -> None:
profile = RIGHT_19_HAND_PROFILE
spec = next(
item for item in profile.sweep_specs
if item.task_name == "index_roll_multiview"
)
baseline = [255, 255, 255, 255, 255, 255, 127, 127, 127, 127,
255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
waypoints = build_calibration_preparation_waypoints(
spec,
255,
current_command=baseline,
profile=profile,
)
# Side clearance keeps all neighbouring roll axes neutral, flexes the
# neighbour PIPs together, then their pitches, and moves the index target
# last through the cleared swept volume.
assert _phase_changes(baseline, waypoints) == [
[17, 18, 19], [2, 3, 4], [6],
]
assert waypoints[-1][1] == 255 and waypoints[-1][16] == 255
returns = build_calibration_return_waypoints(
baseline,
current_command=waypoints[-1],
profile=profile,
)
assert _phase_changes(waypoints[-1], returns) == [
[6], [2, 3, 4], [17, 18, 19],
]
assert list(returns[-1]) == baseline
def test_right_19_side_clearance_waypoints_move_one_motor_at_a_time() -> None:
profile = RIGHT_19_HAND_PROFILE
spec = next(
item for item in profile.sweep_specs
if item.task_name == "index_roll_multiview"
)
waypoints = build_calibration_preparation_waypoints(
spec,
255,
current_command=[255, 255, 255, 255, 255, 255, 127, 127, 127, 127,
255, 255, 255, 255, 255, 255, 255, 255, 255, 255],
profile=profile,
parallel=False,
)
previous = tuple([255] * 6 + [127] * 4 + [255] * 10)
changed_motors = []
for waypoint in waypoints:
changed = _changed_motors(previous, waypoint)
assert len(changed) == 1
changed_motors.append(changed[0])
previous = waypoint
# Side clearance keeps all neighbouring roll axes neutral, flexes them out
# of the swept volume, and moves the index target last.
assert changed_motors[:7] == [19, 4, 18, 3, 17, 2, 6]
assert previous[1] == 255 and previous[16] == 255
returns = build_calibration_return_waypoints(
[255, 255, 255, 255, 255, 255, 127, 127, 127, 127,
255, 255, 255, 255, 255, 255, 255, 255, 255, 255],
current_command=previous,
profile=profile,
parallel=False,
)
for waypoint in returns:
changed = sum(left != right for left, right in zip(previous, waypoint))
assert changed == 1
previous = waypoint
def test_right_15_ring_multiview_keeps_pinky_roll_neutral_and_flexes_it() -> None:
profile = RIGHT_19_HAND_PROFILE
spec = next(
item for item in profile.sweep_specs
if item.task_name == "ring_roll_multiview"
)
current = tuple(THREE_CAMERA_BASELINE_COMMAND)
waypoints = build_calibration_preparation_waypoints(
spec,
255,
current_command=current,
profile=profile,
)
assert _phase_changes(current, waypoints) == [[6, 7], [19], [4], [8]]
assert all(waypoint[9] == 127 for waypoint in waypoints)
assert waypoints[-1][8] == 255
assert waypoints[-1][19] == 0
assert waypoints[-1][4] == 0
def test_middle_multiview_recovers_neighbour_from_observed_58_not_zero() -> None:
profile = RIGHT_19_HAND_PROFILE
spec = next(
item for item in profile.sweep_specs
if item.task_name == "middle_roll_multiview"
)
# Session 20260819_160238 stopped here: the legacy front-only avoidance
# asked neighbouring ring roll (motor 8) for 0 and it saturated at 58.
current = [254] * 20
current[6:10] = [254, 127, 58, 127]
current[10] = 252
current[11:15] = [0, 0, 0, 0]
waypoints = build_calibration_preparation_waypoints(
spec,
255,
current_command=current,
profile=profile,
)
# Both differing roll axes (parked motor 6 and the stuck motor 8) move in
# the single roll phase, still never commanded toward 0.
phases = _phase_changes(current, waypoints)
assert phases[0] == [6, 8]
assert waypoints[0][8] == 127
assert all(
waypoint[motor] != 0
for waypoint in waypoints
for motor in range(6, 10)
)
assert waypoints[-1][6:10] == (255, 255, 127, 127)
def test_right_19_ring_pitch_keeps_target_neutral_during_clearance() -> None:
profile = RIGHT_19_HAND_PROFILE
spec = next(
item for item in profile.sweep_specs
if item.task_name == "ring_pitch_side"
)
current = tuple(THREE_CAMERA_BASELINE_COMMAND)
waypoints = build_calibration_preparation_waypoints(
spec,
255,
current_command=current,
profile=profile,
)
assert _phase_changes(current, waypoints) == [[6, 7], [19], [4]]
assert all(waypoint[8] == 127 for waypoint in waypoints)
assert all(waypoint[9] == 127 for waypoint in waypoints)
assert waypoints[-1][9] == 127
assert waypoints[-1][19] == 0
assert waypoints[-1][4] == 0
def test_right_19_waypoints_never_wait_for_reserved_feedback_channels() -> None:
profile = RIGHT_19_HAND_PROFILE
spec = profile.sweep_specs[0]
# This is the real G20 feedback shape: reserved channels 11..14 report zero
# while active endpoint channels commonly settle one or two counts low.
current = [
255, 254, 254, 254, 254, 254, 127, 127, 127, 127,
253, 0, 0, 0, 0, 255, 253, 253, 253, 253,
]
waypoints = build_calibration_preparation_waypoints(
spec,
255,
current_command=current,
profile=profile,
)
assert waypoints
assert all(waypoint[11:15] == (0, 0, 0, 0) for waypoint in waypoints)
controlled = {joint.motor_index for joint in profile.joint_specs.values()}
for previous, waypoint in zip((tuple(current), *waypoints[:-1]), waypoints):
assert set(_changed_motors(previous, waypoint)) <= controlled
returns = build_calibration_return_waypoints(
list(THREE_CAMERA_BASELINE_COMMAND),
current_command=waypoints[-1],
profile=profile,
)
assert all(waypoint[11:15] == (0, 0, 0, 0) for waypoint in returns)
def test_right_19_parallel_return_moves_rolls_together_then_fingers() -> None:
# Full four-finger avoidance pose: rolls parked at 255 and every clearance
# finger flexed to 0.
current = list(THREE_CAMERA_BASELINE_COMMAND)
current[6:10] = [255, 255, 255, 255]
for motor in (1, 2, 3, 4, 16, 17, 18, 19):
current[motor] = 0
returns = build_calibration_return_waypoints(
list(THREE_CAMERA_BASELINE_COMMAND),
current_command=current,
profile=RIGHT_19_HAND_PROFILE,
anchor_roll_motor=9,
)
assert _phase_changes(current, returns) == [
[6, 7, 8, 9], [1, 2, 3, 4], [16, 17, 18, 19],
]
assert list(returns[-1]) == list(THREE_CAMERA_BASELINE_COMMAND)
def test_right_19_pinky_exit_returns_target_roll_before_other_fingers() -> None:
current = list(THREE_CAMERA_BASELINE_COMMAND)
current[6:10] = [255, 255, 255, 255]
returns = build_calibration_return_waypoints(
list(THREE_CAMERA_BASELINE_COMMAND),
current_command=current,
profile=RIGHT_19_HAND_PROFILE,
anchor_roll_motor=9,
parallel=False,
)
phases = _phase_changes(current, returns)
assert all(len(phase) == 1 for phase in phases)
assert [motor for phase in phases for motor in phase][:4] == [9, 8, 7, 6]
def test_right_19_ring_exit_returns_ring_before_neighbours() -> None:
current = list(THREE_CAMERA_BASELINE_COMMAND)
current[6:10] = [255, 255, 255, 0]
returns = build_calibration_return_waypoints(
list(THREE_CAMERA_BASELINE_COMMAND),
current_command=current,
profile=RIGHT_19_HAND_PROFILE,
anchor_roll_motor=8,
parallel=False,
)
first = returns[0]
assert first[8] == 127
assert first[6:8] == (255, 255)
assert first[9] == 0
def test_cross_view_roll_curve_must_agree_before_fusion() -> None:
values = tuple(0.5 * (127 - command) / 127 for command in range(256))
fit = JointCurveFit(values, values, values, {}, 0.0, 0.0, {})
assert compare_cross_view_roll_curves(fit, fit)[
"angle_rad_rms_difference_rad"
] == 0.0
bad_values = tuple(value + 0.03 * (command != 127) for command, value in enumerate(values))
bad = replace(fit, angle_rad=bad_values, decreasing_rad=bad_values,
increasing_rad=bad_values)
with pytest.raises(ValueError, match="cross_view_roll_curve"):
compare_cross_view_roll_curves(fit, bad)
reverse = replace(
fit,
increasing_rad=tuple(value + math.radians(0.5) for value in values),
)
with pytest.raises(ValueError, match="branch_gap_difference"):
compare_cross_view_roll_curves(fit, reverse)
def _records() -> list[dict[str, object]]:
commands = list(range(0, 256, 16))
if commands[-1] != 255:
@@ -498,3 +876,88 @@ def test_right_compact_payload_keeps_v4_shape_and_uses_pinky_sources() -> None:
assert joint["angle_rad"] == source["angle_rad"]
assert joint["zero_angles"] == {"urdf_zero_offset_rad": 0.0}
assert source["zero_angles"] == {"urdf_zero_offset_rad": 0.01}
def test_right_15_schema_v4_records_derived_dip_curves_and_12_static_zeros() -> None:
from g20_thumb_apriltag_calibration.urdf_zero import (
get_zero_calibration_profile,
)
profile = RIGHT_19_HAND_PROFILE
zero = get_zero_calibration_profile("right", "g20_right_15")
baseline = [255] * 20
baseline[6:10] = [127] * 4
fits = {}
diagnostics = {}
for name, spec in profile.joint_specs.items():
zero_command = baseline[spec.motor_index]
values = tuple(
0.002 * (zero_command - command) for command in range(256)
)
fits[name] = JointCurveFit(
values,
values,
values,
{},
0.0,
0.0,
{"rotation_orthogonal_rms_rad": 0.0, "arc_rad": 0.5},
)
diagnostics[name] = {
"cycle_travel_rad": [0.5, 0.5, 0.5, 0.5],
"cycle_travel_range_rad": 0.0,
"baseline_hysteresis_by_cycle_rad": [0.0, 0.0, 0.0, 0.0],
"holdout_cycle": 3,
"holdout_cycle_mae_rad": 0.0,
"holdout_cycle_max_rad": 0.0,
}
offsets = {name: 0.0 for name in profile.active_joints}
payload = build_compact_payload(
serial_number="G20_RIGHT_019",
measured_fits=fits,
urdf_zero_offsets_rad=offsets,
validation_errors_rad=[0.0],
passed=True,
baseline=baseline,
side="right",
layout_id="g20_right_15",
zero_uncertainty_rad={name: 0.0 for name in zero.direct_zero_joints},
zero_cycle_offsets_rad={
name: (0.0, 0.0, 0.0) for name in zero.direct_zero_joints
},
zero_observers=zero.offset_observer_joint,
artifact_hashes={
"source_urdf_sha256": "0" * 64,
"camera_extrinsics_sha256": "1" * 64,
"corrected_urdf_sha256": "2" * 64,
},
cross_view_roll_metrics={
f"{finger}_mcp_roll": {"angle_rad_rms_difference_rad": 0.0}
for finger in ("index", "middle", "ring", "pinky")
},
joint_dynamic_diagnostics=diagnostics,
zero_geometry_diagnostics={
"training_cycles": (0, 1, 2),
"validation_cycle": 3,
"axis_line_rms_m": 0.0,
"validation_line_error_by_joint_m": {},
},
)
validate_compact_payload(payload)
assert payload["schema_version"] == 4
assert set(payload) == {
"schema_version", "model", "side", "serial_number", "angle_unit",
"command_range", "baseline_command_u8", "joints", "quality",
}
assert len(zero.direct_zero_joints) == 12
assert payload["joints"]["thumb_mcp"]["zero_angles"] == {
"urdf_zero_offset_rad": 0.0
}
assert payload["joints"]["thumb_ip"]["passive"] is True
assert set(payload["joints"]["thumb_ip"]) == {
"motor_index", "angle_rad", "passive"
}
assert all(
"source_joint" not in joint for joint in payload["joints"].values()
)
File diff suppressed because it is too large Load Diff
@@ -234,6 +234,70 @@ def _pose(
)
def test_group_tracker_uses_coupling_to_choose_branch_but_never_rejects_measurement() -> None:
tracker = SquareTagGroupPoseTracker(
roles=("base", "mcp", "ip"),
adjacent_pairs=(("base", "mcp"), ("mcp", "ip")),
maximum_pose_jump_rad=np.deg2rad(360.0),
maximum_translation_jump_m=0.04,
relative_rotation_scale_rad=np.deg2rad(1000.0),
relative_translation_scale_m=1.0,
reprojection_scale_px=0.1,
reprojection_weight=1.0,
reset_after_seconds=5.0,
coupled_rotation_pairs=(
("base", "mcp", "mcp", "ip", 1.03),
),
coupled_rotation_scale_rad=np.deg2rad(3.0),
maximum_coupled_rotation_residual_rad=np.deg2rad(7.5),
)
baseline = {
"base": (_pose(0.0, 0.00, 0.05),),
"mcp": (_pose(0.0, 0.03, 0.05),),
"ip": (_pose(0.0, 0.06, 0.05),),
}
selected, reason = tracker.select(
baseline,
stamp_ns=1_000_000_000,
trajectory_command_u8=255,
trajectory_direction="decreasing",
)
assert selected is not None
assert reason == ""
driver = _pose(10.0, 0.03, 0.05)
measured_ip = _pose(20.3, 0.06, 0.20)
lower_reprojection_mirror = _pose(30.0, 0.06, 0.05)
selected, reason = tracker.select(
{
"base": (_pose(0.0, 0.00, 0.05),),
"mcp": (driver,),
"ip": (lower_reprojection_mirror, measured_ip),
},
stamp_ns=1_033_000_000,
trajectory_command_u8=128,
trajectory_direction="decreasing",
)
assert reason == ""
assert selected is not None
assert selected["ip"] == measured_ip
fallback, fallback_reason = tracker.select(
{
"base": (_pose(0.0, 0.00, 0.05),),
"mcp": (driver,),
"ip": (lower_reprojection_mirror,),
},
stamp_ns=1_066_000_000,
trajectory_command_u8=128,
trajectory_direction="decreasing",
)
assert fallback is not None
assert fallback_reason == ""
assert fallback["ip"] == lower_reprojection_mirror
def test_group_tracker_prevents_incompatible_t4_t5_branch_switch() -> None:
tracker = SquareTagGroupPoseTracker(
roles=("t0", "t3", "t4", "t5"),
@@ -324,6 +388,58 @@ def test_group_tracker_keeps_same_pair_across_sweep_turnaround() -> None:
assert selected == {"t4": return_t4, "t5": return_t5}
def test_group_tracker_uses_outbound_pose_at_same_command_on_return() -> None:
tracker = SquareTagGroupPoseTracker(
roles=("parent", "child"),
adjacent_pairs=(("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,
)
parent = _pose(0.0, 0.00, 0.05)
for stamp, command, angle in (
(1_000_000_000, 255, 20.0),
(1_033_000_000, 64, 45.0),
(1_066_000_000, 0, 50.0),
):
selected, reason = tracker.select(
{"parent": (parent,), "child": (_pose(angle, 0.03, 0.05),)},
stamp_ns=stamp,
trajectory_command_u8=command,
trajectory_direction="decreasing",
)
assert reason == ""
assert selected is not None
# The true return contains 3 deg of real hysteresis along the learned
# y-axis. The lower-error mirror candidate is temporally smoother but
# adds a 2 deg tilt outside that physical motion axis.
true_return = _pose(42.0, 0.03, 0.20)
smoother_mirror = SquareTagPose(
quaternion_xyzw=tuple(
Rotation.from_euler("xy", [2.0, 49.0], degrees=True).as_quat()
),
translation_xyz_m=(0.03, 0.0, 0.25),
reprojection_error_px=0.01,
)
selected, reason = tracker.select(
{
"parent": (parent,),
"child": (smoother_mirror, true_return),
},
stamp_ns=1_099_000_000,
trajectory_command_u8=64,
trajectory_direction="increasing",
)
assert reason == ""
assert selected == {"parent": parent, "child": true_return}
def test_group_tracker_initializes_from_multiple_static_frames() -> None:
tracker = SquareTagGroupPoseTracker(
roles=("parent", "child"),
@@ -372,6 +488,63 @@ def test_group_tracker_initializes_from_multiple_static_frames() -> None:
] < np.deg2rad(1.0)
def test_group_tracker_preserves_task_branch_anchor_across_cycles() -> None:
tracker = SquareTagGroupPoseTracker(
roles=("parent", "child"),
adjacent_pairs=(("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,
initialization_frames=8,
)
parent = _pose(0.0, 0.00, 0.05)
anchored_child = _pose(20.0, 0.03, 0.20)
for index in range(8):
selected, reason = tracker.select(
{"parent": (parent,), "child": (anchored_child,)},
stamp_ns=1_000_000_000 + index * 33_000_000,
trajectory_command_u8=255,
trajectory_direction="decreasing",
)
assert reason == ""
assert selected == {"parent": parent, "child": anchored_child}
tracker.reset(preserve_task_reference=True)
lower_error_mirror = _pose(5.0, 0.03, 0.01)
for index in range(8):
selected, reason = tracker.select(
{
"parent": (parent,),
"child": (lower_error_mirror, anchored_child),
},
stamp_ns=2_000_000_000 + index * 33_000_000,
trajectory_command_u8=255,
trajectory_direction="decreasing",
)
assert reason == ""
assert selected == {"parent": parent, "child": anchored_child}
assert tracker.last_initialization_quality["task_reference_used"] == "true"
tracker.reset()
for index in range(8):
selected, reason = tracker.select(
{
"parent": (parent,),
"child": (lower_error_mirror, anchored_child),
},
stamp_ns=3_000_000_000 + index * 33_000_000,
trajectory_command_u8=255,
trajectory_direction="decreasing",
)
assert reason == ""
assert selected == {"parent": parent, "child": lower_error_mirror}
def test_static_group_normal_prior_rejects_stable_ippe_mirror() -> None:
tracker = SquareTagGroupPoseTracker(
roles=("mcp", "pip", "dip"),
@@ -74,6 +74,36 @@ def test_missing_endpoint_has_specific_chinese_reason_and_recovery() -> None:
assert "/g20_calibration/resume" in text
def test_combination_prediction_failure_is_not_reported_as_unclassified() -> None:
explanation, action = three_camera_reason_zh(
"PAUSED", "combination_pose_prediction_failed", {}
)
assert "多关节组合姿态" in explanation
assert "未分类原因码" not in explanation
assert "combination_validation_failure" in action
assert "不要重新采集16个单关节任务" in action
def test_multiview_failure_names_the_camera_specific_joint() -> None:
explanation, action = three_camera_reason_zh(
"PAUSED",
"sweep_bin_gap_too_large:pinky_mcp_roll_side",
{
"sample": {
"maximum_bin_gap": 20,
"maximum_bin_gap_start_u8": 100,
"maximum_bin_gap_end_u8": 120,
"allowed_maximum_bin_gap": 16,
}
},
)
assert "小指MCP侧摆(侧面校验)" in explanation
assert "100→120" in explanation
assert "resume" in action
def test_preflight_lists_missing_tags_in_chinese() -> None:
payload = {
"state": "PREFLIGHT",
@@ -137,6 +167,82 @@ def test_joint_fit_failure_names_metric_and_selective_retry() -> None:
assert "运动采样:" not in text
def test_baseline_hysteresis_failure_shows_values_instead_of_unknown() -> None:
payload = {
"state": "PAUSED",
"reason": "joint_fit_check_failed",
"progress": 0.15,
"completed_sweeps": 24,
"total_sweeps": 160,
"active": {
"kind": "fit_failure",
"view": "front",
"motor_index": 15,
"joints": ["thumb_ip"],
"attempt": 3,
"directions_to_rescan": 6,
"failures": [
{
"joint": "thumb_ip",
"metric": "baseline_hysteresis_deg",
"actual": 1.34,
"limit": 0.5,
"comparison": "maximum",
"cycle_values_deg": [1.34, 0.04, 0.14],
}
],
},
"views": {},
"result_path": "",
}
text = render_three_camera_status_text_zh(payload)
assert "baseline正反程关节角差为1.34°" in text
assert "要求不超过0.50°" in text
assert "各轮=1.34°/0.04°/0.14°" in text
assert "未知原因" not in text
def test_sweep_gap_status_names_gap_bounds_and_full_detection_rate() -> None:
payload = {
"state": "PAUSED",
"reason": "sweep_bin_gap_too_large",
"progress": 0.1,
"completed_sweeps": 1,
"total_sweeps": 10,
"active": {
"kind": "sweep",
"view": "top",
"motor_index": 10,
"joints": ["thumb_cmc_yaw"],
"cycle": 1,
"repetitions": 3,
"start_u8": 255,
"target_u8": 0,
"actual_u8": 3.0,
"valid_frames": 610,
"detection_frames": 644,
"detection_valid_frames": 610,
"detection_rate": 610 / 644,
"sample": {
"minimum_u8": 3.0,
"maximum_u8": 252.0,
"maximum_bin_gap": 17,
"maximum_bin_gap_start_u8": 99,
"maximum_bin_gap_end_u8": 116,
"allowed_maximum_bin_gap": 16,
},
},
"views": {},
}
text = render_three_camera_status_text_zh(payload)
assert "最大空缺为17,(99→116" in text
assert "本方向Tag检出:94.7%610/644帧)" in text
def test_zero_model_failure_explains_that_rescan_will_not_help() -> None:
payload = {
"state": "PAUSED",
@@ -210,6 +316,7 @@ def test_baseline_stall_names_motor_target_actual_and_tolerance() -> None:
"reason": (
"motor_state_stalled:return_baseline:motor_index=10:"
"target_u8=255.0:actual_u8=250.0:tolerance_u8=4.0:"
"timeout_seconds=2.000:"
"error_u8=5.000"
),
"progress": 0.0,
@@ -224,6 +331,7 @@ def test_baseline_stall_names_motor_target_actual_and_tolerance() -> None:
"actual_u8": 250.0,
"error_u8": 5.0,
"tolerance_u8": 4.0,
"timeout_seconds": 2.0,
},
"views": {},
"result_path": "",
@@ -231,7 +339,7 @@ def test_baseline_stall_names_motor_target_actual_and_tolerance() -> None:
text = render_three_camera_status_text_zh(payload)
assert "电机10反馈连续8" in text
assert "电机10反馈连续2" in text
assert "目标255.0、实际250.0、误差5.000 u8" in text
assert "允许容差±4.0 u8" in text
assert "当前任务:电机10运动停滞,目标255.0、实际250.0" in text
@@ -371,3 +479,15 @@ def test_index_roll_status_prints_clearance_motor_feedback() -> None:
assert "阶段速度:五指目标[15, 5, 15, 15, 15]" in text
assert "SDK报告[15, 5, 15, 15, 15]" in text
assert "自动重试:当前方向已自动重扫1/2次" in text
def test_same_finger_transition_explains_that_clearance_stays_parked() -> None:
explanation, action = three_camera_reason_zh(
"RETURN_BASELINE",
"holding_same_finger_clearance_before_next_task",
{},
)
assert "继续保持当前避让姿态" in explanation
assert "只调整被测关节" in explanation
assert "不要手动展开" in action
File diff suppressed because it is too large Load Diff
@@ -19,6 +19,7 @@ from g20_thumb_apriltag_calibration.urdf_zero import (
UrdfKinematicModel,
_angles_from_state,
_zero_sensitive_axis_error_rad,
baseline_hysteresis_by_cycle_rad,
fit_joint_axis_measurement,
fit_rotation_joint_curve,
solve_urdf_zero_offsets,
@@ -157,6 +158,78 @@ def test_axis_and_curve_ignore_camera_and_tag_mount_rotation() -> None:
assert curve.angle_rad[0] == pytest.approx(math.radians(62.0), abs=1.0e-6)
def test_baseline_hysteresis_uses_only_revolute_axis_component() -> None:
off_axis_noise = Rotation.from_rotvec(
np.radians([1.2, 0.0, 0.2])
).as_quat().tolist()
records = []
for cycle in range(3):
records.extend(
(
{
"cycle": cycle,
"direction": "decreasing",
"command_u8": 255,
"relative_quaternion_xyzw": [0.0, 0.0, 0.0, 1.0],
},
{
"cycle": cycle,
"direction": "increasing",
"command_u8": 255,
"relative_quaternion_xyzw": off_axis_noise,
},
)
)
full_pose = baseline_hysteresis_by_cycle_rad(
records, zero_command_u8=255
)
joint_angle = baseline_hysteresis_by_cycle_rad(
records, zero_command_u8=255, axis_xyz=[0.0, 0.0, 1.0]
)
assert math.degrees(full_pose[0]) == pytest.approx(
math.hypot(1.2, 0.2), abs=1.0e-9
)
assert math.degrees(joint_angle[0]) == pytest.approx(0.2, abs=1.0e-9)
def test_canonical_zero_preserves_opposite_direction_baseline_offset() -> None:
records = []
commands = sorted({0, 127, 255, *range(0, 256, 16)})
branch_offset = math.radians(1.0)
for cycle in range(3):
for direction in ("decreasing", "increasing"):
offset = branch_offset if direction == "increasing" else 0.0
for command in commands:
angle = math.radians(50.0) * (127.0 - command) / 255.0
angle += offset
rotation = Rotation.from_rotvec([0.0, 0.0, angle])
records.append(
{
"cycle": cycle,
"direction": direction,
"command_u8": command,
"relative_quaternion_xyzw": rotation.as_quat().tolist(),
}
)
curve = fit_rotation_joint_curve(
records,
zero_command_u8=127,
canonical_zero_direction="decreasing",
)
assert curve.angle_rad == curve.decreasing_rad
assert curve.decreasing_rad[127] == pytest.approx(0.0, abs=1.0e-9)
assert curve.increasing_rad[127] == pytest.approx(
branch_offset, abs=1.0e-6
)
assert curve.maximum_hysteresis_rad == pytest.approx(
branch_offset, abs=1.0e-6
)
@pytest.mark.parametrize("joint", ["thumb_cmc_pitch", "index_mcp_pitch"])
def test_image_plane_joint_uses_rotation_axis_to_constrain_noisy_depth(
joint: str,
@@ -234,6 +307,35 @@ def test_pose_axis_point_rejects_end_on_optical_depth_bias() -> None:
) < 1.0e-6
def test_side_roll_axis_point_ignores_real_screw_translation() -> None:
records, expected_axis, expected_point = _arbitrary_tag_records()
common_from_parent = Rotation.from_euler("xyz", [-0.8, 0.55, 1.3])
axis_parent = common_from_parent.inv().apply(expected_axis)
screw_records = []
for record in records:
changed = dict(record)
fraction = (255.0 - float(record["command_u8"])) / 255.0
changed["relative_translation_xyz_m"] = (
np.asarray(record["relative_translation_xyz_m"], dtype=float)
+ 0.008 * fraction * axis_parent
).tolist()
screw_records.append(changed)
measurement = fit_joint_axis_measurement(
"middle_mcp_roll_side",
screw_records,
cycle=0,
zero_command_u8=255,
constrained_circle_joints=frozenset({"middle_mcp_roll_side"}),
)
observed_point = np.asarray(measurement.point_common_xyz_m)
assert measurement.pose_axis_line_rms_m < 1.0e-6
assert np.linalg.norm(
np.cross(observed_point - expected_point, expected_axis)
) < 1.0e-6
def test_splay_zero_interpolates_when_scan_does_not_hit_command_127() -> None:
records, expected_axis, _ = _arbitrary_tag_records()
assert not any(record["command_u8"] == 127 for record in records)
@@ -479,15 +581,17 @@ def _solve_synthetic_offsets(
side: str,
offset_degrees: list[float],
*,
layout_id: str = "legacy_11",
inject_oblique_optical_depth_bias: bool = False,
inject_secondary_root_axis_bias_degrees: float = 0.0,
inject_secondary_root_point_bias_m: float = 0.0,
inject_observer_cone_bias_degrees: float = 0.0,
pose_axis_line_rms_by_joint_m: dict[str, float] | None = None,
joint_maximum_offset_degrees: dict[str, float] | None = None,
validation_offset_bias_degrees: dict[str, float] | None = None,
):
hand = get_hand_calibration_profile(side)
zero = get_zero_calibration_profile(side)
hand = get_hand_calibration_profile(side, layout_id)
zero = get_zero_calibration_profile(side, layout_id)
source = SOURCE_URDF if side == "left" else RIGHT_SOURCE_URDF
baseline = [255.0] * 20
baseline[6:10] = [127.0] * 4
@@ -517,7 +621,9 @@ def _solve_synthetic_offsets(
base_rotation = Rotation.from_euler("xyz", [0.5, -0.4, 0.8])
base_translation = np.asarray([0.31, -0.19, 0.72])
measurements: list[JointAxisMeasurement] = []
for cycle in range(3):
validation_cycle = 3 if layout_id == "g20_right_15" else 2
training_cycles = tuple(range(validation_cycle))
for cycle in range(validation_cycle + 1):
for joint in zero.axis_joints:
state = list(baseline)
if joint == "thumb_cmc_yaw":
@@ -528,8 +634,18 @@ def _solve_synthetic_offsets(
motor_by_joint=motor_by_joint,
inherited_zero_joints=zero.inherited_zero_joints,
)
cycle_offsets = dict(offsets)
if cycle == validation_cycle:
cycle_offsets.update(
{
name: cycle_offsets[name] + math.radians(value)
for name, value in (
validation_offset_bias_degrees or {}
).items()
}
)
axis, point = model.axis_line(
joint, zero_offsets=offsets, joint_angles=angles
joint, zero_offsets=cycle_offsets, joint_angles=angles
)
point_common = base_rotation.apply(point) + base_translation
if (
@@ -622,14 +738,229 @@ def _solve_synthetic_offsets(
curves=curves,
motor_by_joint=motor_by_joint,
hand_type=side,
tag_layout=layout_id,
joint_maximum_offset_rad={
name: math.radians(value)
for name, value in (joint_maximum_offset_degrees or {}).items()
},
training_cycles=training_cycles,
validation_cycle=validation_cycle,
)
return zero, result
def test_right_15_solver_recovers_11_targets_and_keeps_thumb_mcp_cad_zero() -> None:
injected = [
2.0, -3.0, 4.0, -1.5,
1.0, -1.0,
0.8, -0.7,
-0.5, 0.6,
1.1, -1.0,
]
zero, result = _solve_synthetic_offsets(
"right", injected, layout_id="g20_right_15"
)
assert len(zero.direct_zero_joints) == 12
assert len(zero.axis_joints) == 17
assert result.passed is True
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
]
)
)
for name, expected in zip(zero.direct_zero_joints, injected):
if name == "thumb_mcp":
expected = 0.0
elif name in finger_rolls:
expected -= roll_common
assert math.degrees(result.direct_offsets_rad[name]) == pytest.approx(
expected, abs=0.05
)
assert math.degrees(result.all_active_offsets_rad["thumb_mcp"]) == pytest.approx(
0.0, abs=0.05
)
assert result.training_cycles == (0, 1, 2)
assert result.validation_cycle == 3
assert all(
value == pytest.approx(0.0, abs=1.0e-12)
for value in result.offset_confidence_half_width_rad.values()
)
assert all(
result.all_active_offsets_rad[f"{finger}_pip"] == 0.0
for finger in ("index", "middle", "ring", "pinky")
)
assert result.observability_parameter_count == 18
assert result.observability_rank == 18
assert math.isfinite(result.observability_condition_number)
assert set(result.offset_covariance_rad2) == set(zero.direct_zero_joints)
def test_right_15_finger_roll_limit_applies_to_independent_deviation() -> None:
# A shared electrical centre belongs to the four-motor common datum; the
# strict 3 deg assembly guard applies to finger-to-finger deviations.
injected = [
2.0, -3.0, 4.0, -1.5,
6.0, -1.0,
6.4, -0.7,
5.7, 0.6,
6.2, -1.0,
]
zero, result = _solve_synthetic_offsets(
"right", injected, layout_id="g20_right_15"
)
assert result.passed is True
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
]
)
)
for name, expected in zip(zero.direct_zero_joints, injected):
if name == "thumb_mcp":
expected = 0.0
elif name in finger_rolls:
expected -= roll_common
assert math.degrees(result.direct_offsets_rad[name]) == pytest.approx(
expected, abs=0.05
)
assert np.median(
[result.direct_offsets_rad[name] for name in finger_rolls]
) == pytest.approx(0.0, abs=1.0e-10)
def test_right_19_holdout_never_changes_frozen_training_offsets() -> None:
injected = [
2.0, -3.0, 4.0, -1.5,
1.0, -1.0,
0.8, -0.7,
-0.5, 0.6,
1.1, -1.0,
]
zero, result = _solve_synthetic_offsets(
"right",
injected,
layout_id="g20_right_15",
validation_offset_bias_degrees={"thumb_cmc_roll": 2.0},
)
assert math.degrees(
result.direct_offsets_rad["thumb_cmc_roll"]
) == pytest.approx(injected[0], abs=0.05)
assert result.validation_cycle not in result.training_cycles
def test_right_15_palm_pose_and_12_zero_observation_jacobian_is_full_rank() -> None:
"""Guard the reviewed 6-palm-DOF plus 12-static-zero observability."""
zero = get_zero_calibration_profile("right", "g20_right_15")
model = UrdfKinematicModel(RIGHT_SOURCE_URDF)
parameter_count = 6 + len(zero.direct_zero_joints)
def line_observations(parameters: np.ndarray) -> np.ndarray:
palm_rotation = Rotation.from_rotvec(parameters[:3])
palm_translation = parameters[3:6]
offsets = {
name: float(value)
for name, value in zip(
zero.direct_zero_joints, parameters[6:]
)
}
result: list[float] = []
for joint in zero.axis_joints:
axis, point = model.axis_line(
joint, zero_offsets=offsets, joint_angles={}
)
axis = palm_rotation.apply(axis)
point = palm_rotation.apply(point) + palm_translation
# An oriented 3-D line is represented by its direction and
# Pluecker moment. The moment is invariant to choosing a different
# point along the same axis, so no unobservable along-axis Tag
# placement is accidentally counted as information.
result.extend(float(value) for value in axis)
result.extend(float(value) for value in np.cross(point, axis))
return np.asarray(result, dtype=float)
origin = np.zeros(parameter_count, dtype=float)
step = 1.0e-6
jacobian = np.column_stack(
[
(
line_observations(
origin + np.eye(parameter_count, dtype=float)[index] * step
)
- line_observations(
origin - np.eye(parameter_count, dtype=float)[index] * step
)
)
/ (2.0 * step)
for index in range(parameter_count)
]
)
assert parameter_count == 18
assert np.linalg.matrix_rank(jacobian, tol=1.0e-7) == parameter_count
def test_right_15_urdf_writer_changes_only_the_12_static_targets(
tmp_path: Path,
) -> None:
zero = get_zero_calibration_profile("right", "g20_right_15")
offsets = {name: math.radians(1.0) for name in zero.direct_zero_joints}
destination = write_zero_corrected_urdf(
source_urdf=RIGHT_SOURCE_URDF,
output_directory=tmp_path,
serial_number="G20_RIGHT_019",
offsets_rad=offsets,
timestamp="20260818_120000",
)
original = {
str(joint.get("name")): joint
for joint in ET.parse(RIGHT_SOURCE_URDF).getroot().findall("joint")
}
corrected = {
str(joint.get("name")): joint
for joint in ET.parse(destination).getroot().findall("joint")
}
for mesh in ET.parse(destination).getroot().findall(".//mesh"):
relative = Path(mesh.get("filename"))
copied = destination.parent / relative
source = RIGHT_SOURCE_URDF.parent / relative
assert copied.is_file()
assert copied.stat().st_size == source.stat().st_size
changed = set()
for name in original:
original_origin = original[name].find("origin")
corrected_origin = corrected[name].find("origin")
if original_origin is None or corrected_origin is None:
continue
if original_origin.get("rpy") != corrected_origin.get("rpy"):
changed.add(name)
assert original_origin.get("xyz") == corrected_origin.get("xyz")
assert changed == set(zero.direct_zero_joints)
assert "thumb_mcp" in changed
assert not set(get_hand_calibration_profile(
"right", "g20_right_15"
).passive_joints) & changed
def test_small_stable_offsets_are_validated_without_rewriting_urdf_zero() -> None:
zero, result = _solve_synthetic_offsets("right", [0.1] * 7)
assert result.passed is True
@@ -712,7 +1043,7 @@ def test_thumb_mcp_static_phase_bias_cannot_override_original_cad_zero() -> None
assert result.passed is True
assert result.direct_offsets_rad["thumb_mcp"] == pytest.approx(0.0)
assert result.cycle_offsets_rad["thumb_mcp"] == pytest.approx((0.0, 0.0, 0.0))
assert result.cycle_offsets_rad["thumb_mcp"] == pytest.approx((0.0, 0.0))
assert "thumb_ip" not in result.validation_error_by_joint_rad
for name in ("thumb_cmc_roll", "thumb_cmc_yaw", "thumb_cmc_pitch"):
assert abs(math.degrees(result.direct_offsets_rad[name])) <= 20.0