G20右手四指独立标定(少thumb_mcp)

This commit is contained in:
lxp
2026-08-24 10:12:23 +08:00
parent ef65681230
commit 4e594ddb09
22 changed files with 1605 additions and 262 deletions
+39 -28
View File
@@ -2,7 +2,7 @@
## G20右手正式一键标定
固定三相机和15张Tag安装完成后,用户只运行:
固定三相机和19张Tag安装完成后,用户只运行:
```bash
ros2 run g20_thumb_apriltag_calibration calibrate_g20_right
@@ -13,21 +13,26 @@ ros2 run g20_thumb_apriltag_calibration calibrate_g20_right
独立任务仍会复用,不再因“连续前缀”限制整段重采。导入的任务会立即用与
最终验收相同的硬门限复检(不含视口实时有效率):只以预警带余量通过的旧数据
当场剔除并从其在扫掠顺序中的原始位置重采,避免全部任务采完后才在最终验收
失败、把会话拉回靠前的关节。需要强制从第一个关节
失败、把会话拉回靠前的关节。运行中的多视角任务按正面主测量和侧面校验测量
独立保留;单轮转轴异常且其余三轮形成一致簇时只补扫异常轮的两个方向。侧面
校验视角的任务级有效率只记录为诊断,完整端点、分箱覆盖和模型质量门限仍保持
不变。需要强制从第一个关节
重新采集时使用 `--no-resume`。升级前已经分别完成的正面/侧面roll也会合并为
一个完整同步任务断点;只有两边数据都完整时才复用。
命令自动完成产品哈希预检、运动、当前任务补扫、前三轮训练、第四轮隔离留出、
11个可求解主动关节URDF零位修正(`thumb_mcp`固定CAD零位)、
17条实测命令曲线以及4条
源URDF mimic派生DIP曲线发布。
15个会话数据求解主动关节URDF零位修正(`thumb_mcp`固定CAD参考)、
21条视觉实测命令曲线发布;四指PIP/DIP的动态曲线均实测,四指DIP静态零位保留CAD。
终端只显示中文进度和问题;失败时复制“请复制以下内容给开发者”块即可。
四指末端的16 mm Tag允许使用刚性延长杆避挡;软件不假设末端Tag平面与中节Tag
平面平行。延长杆和Tag在一次标定期间必须完全刚性,不能晃动、扭转或重新调整。
正式结果位于 `calibration_output/G20_RIGHT_001/latest_passed`。该指针只在
JSON、URDF数值等价、mesh完整性、21条曲线CAD限位、被动关节保护和隔离留出验证
全部通过后更新。
## G20右手15-Tag底层调试入口
## G20右手19-Tag底层调试入口
以下内容仅保留给旧会话回放和开发调试;正式一键命令只发布上面的精简
schema v4 JSON,不再生成schema v5运行文件。
@@ -38,20 +43,21 @@ schema v4 JSON,不再生成schema v5运行文件。
```bash
ros2 launch g20_thumb_apriltag_calibration three_camera_calibration.launch.py \
hand_type:=right \
tag_layout:=g20_right_15 \
tag_layout:=g20_right_19 \
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项拇指任务,
当前产品布局共19张`tag36h11`,所有Tag的黑色码区边长均为16 mm;四指末节ID
`7,14,16,18`。正面为`0,1,2,3,10,11,12,13`,侧面为
`4,5,6,7,14,15,16,17,18`,上面为`8,9`。详细角色和逐ID尺寸
`config/three_camera_tags_g20_right_19.yaml`为唯一软件配置源。程序执行4项拇指任务,
以及小指、无名指、中指、食指各自的正面+侧面同步roll、侧面pitch、
侧面PIP,共16个物理运动任务。同步roll只驱动电机一次,但两台相机仍分别拟合并通过
各自的观测质量门限。四个DIP动态曲线由同次PIP实测曲线乘源URDF中受保护的mimic
multiplier生成,不参与视觉拟合或视觉质量门限。每项正式四轮之前自动低速往返预检
侧面PIP/DIP联合任务,共16个物理运动任务。同步roll只驱动电机一次,但两台相机仍分别拟合并通过
各自的观测质量门限。每个PIP任务只驱动一次对应电机,同时用“手掌→中节Tag”实测PIP、
用“中节Tag→末节Tag”实测被动DIP;四个DIP不再由URDF mimic系数生成,并参加完整视觉
拟合和质量门限。每项正式四轮之前自动低速往返预检
0/127/255可见性;
基准形态恢复完成后,程序先用至少30帧稳健锁定正面ID 0、侧面ID 4和顶部ID 8的
@@ -63,9 +69,14 @@ multiplier生成,不参与视觉拟合或视觉质量门限。每项正式四
任务结束后角色要求会切回预检全套标签,静止期帧不参与该门限,避免把
采集质量良好的任务误判为可见性失败。
取消末端Tag后,四个PIP缺少下游DIP轴作为任意粘贴Tag条件下的绝对零位观察基准;
因此PIP动态曲线仍为视觉实测,但PIP的URDF静态零位保留源CAD,不猜测写入。
正式修正范围为拇指4个主动关节,以及四指各自的MCP roll/MCP pitch,共12个。
恢复末端Tag后,程序能够独立实测四指PIP和DIP的转轴及动态命令曲线。但同一次
相机外参和同一套Tag安装下的重复扫描无法排除固定安装相位偏差;实体手在反馈0端
能够触掌是独立的机械端点约束。因此四指MCP pitch、PIP的URDF静态零位由本次
实测全屈曲行程与CAD触掌角之差求出,不再把同相机的平行轴PnP相位直接当作绝对
零位,也不写死为0。生成URDF时同步修正这些关节的坐标上限和DIP mimic坐标偏置,
保证非零零偏不会缩短最大闭合量。末节Tag继续用于DIP动态曲线、轴线质量、遮挡和
第四轮留出检查。正式数据求解静态修正范围为拇指CMC三个主动关节、四指MCP roll、
MCP pitch及PIP,共15个;只有`thumb_mcp`固定为既有CAD参考。
侧面累计避障按“PIP→MCP pitch→roll”的安全顺序分阶段进入,并按逆序分阶段退出;
同类辅助电机(全部邻指滚转、全部PIP、全部MCP pitch)合并为同一个并行航点同时
运动,被测通道最后单独进入。“滚转全部回中前不展开弯曲手指”“每指pitch先于PIP”
@@ -92,11 +103,11 @@ IPPE镜像解。baseline标准接近和全部质量门限保持不变。
排除与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文件仅作历史回放兼容,
当前19-Tag产品流程发布精简schema v4:21条运行时曲线全部来自当前会话的视觉实测。
URDF零位字段覆盖拇指4个主动关节和四指各自的`mcp_roll/mcp_pitch/pip`,共16个,
其中四指`mcp_pitch/pip`共8个字段由视觉行程+机械端点联合求解,只有`thumb_mcp`
字段固定为0
`thumb_ip`及四指DIP静态零位保留源CAD。旧schema v5文件仅作历史回放兼容,
当前一键流程不再生成它。正面/侧面roll在同一次运动中独立拟合;方向、
轴线和动态曲线均通过时做不确定度加权轴融合。侧面PIP连杆标签在滚转扫掠中
相对侧相机视线倾斜约13°~20°,平面标签的单目IPPE姿态二义性会给侧视姿态引入
@@ -115,9 +126,8 @@ URDF零位字段只覆盖拇指4个主动关节和四指各自的`mcp_roll/mcp_p
姿态轴约束,因此自由三维圆平面与姿态轴的夹角只保留诊断,不再被重复作为硬门限;
径向残差、SE(3)轴线残差和正侧面轴/曲线一致性仍是硬门限。任一静态目标、第四轮留出、
遮挡、PnP或跨机位检查失败时,只保留原始轨迹和`passed:false`诊断,不发布正式URDF。
8个组合姿态仅保留为开发诊断,正式产品默认不执行。15-Tag布局的侧面每根手指只有一张
PIP Tag,并不存在可独立验证的DIP Tag;同时轴线零位求解不提供适合绝对笛卡尔位置验收的
手基座变换,因此不能用该诊断推翻已经通过的单关节隔离留出结果。三个CMC轴恢复使用
8个组合姿态仅保留为开发诊断,正式产品默认不执行。轴线零位求解不提供适合绝对笛卡尔
位置验收的手基座变换,因此不能用该诊断推翻已经通过的单关节隔离留出结果。三个CMC轴恢复使用
`a609d521`验证过的完整四轮相对旋转曲线;全部实测关节均由隔离第四轮逐关节验收。
现场需要区分某根手指的roll机构回差与单机位误差时,可设置
`cross_view_roll_diagnostic_finger:=pinky|ring|middle|index`。该会话只执行目标手指的一次
@@ -421,8 +431,9 @@ calibration_output/G20_RIGHT_001/<时间戳>/
`zero_angles.urdf_zero_offset_rad`、5个被动标记、模板来源和总体质量。新URDF每次从
指定原始CAD文件生成,采用 `T_original × Rot(axis, offset)`,绝不叠加旧校准文件或
覆盖原文件;只有通过独立求解验证或明确机械装配基准授权的主动关节 `origin.rpy` 可能
改变;当前15-Tag右手保留12个主动静态零位字段,其中11个由数据求解,`thumb_mcp`
固定为原始CAD零位0;四指PIP虽保留字段但数值固定为0。
改变;当前19-Tag右手保留16个主动静态零位字段,其中15个由本会话数据求解,只有
`thumb_mcp`固定为原始CAD零位0;四指DIP为被动关节,发布实测动态曲线并保留CAD
静态零位,其mimic坐标偏置只随上游PIP坐标系变换作等价调整。
未观测关节和其他URDF文本保持不变。源URDF中的相对mesh资源会按原相对路径复制到
同一会话,保证会话内URDF可独立加载,并在正式发布时逐文件记录SHA256。每帧Tag SE(3)、
图像时间戳、20通道状态、同步误差和PnP误差只进入 `raw_samples.jsonl`
@@ -775,7 +786,7 @@ for name, joint in data["joints"].items():
不影响 AprilTag 的 ROI 输入。
静态预检先在单 Tag 层拒绝高重投影误差,再检查三组相对中心的位置内点率和毫米级 RMS。
当前 3038 px 的 10 mm Tag 属于试标定尺寸,如果中心位置 RMS 持续不合格,应优先增加照明、缩短
当前末节16 mm Tag如果中心位置 RMS 持续不合格,应优先增加照明、缩短
相机距离或提高 Tag 有效像素,而不是放宽最终随机复测精度。
启用 rosbag 后保存裁剪后的原始图像和配套 `CameraInfo`,避免新增一个全分辨率图像
@@ -44,7 +44,7 @@ g20_thumb_calibration:
minimum_detection_hz: 15.0
maximum_hamming: 0
minimum_decision_margin: 30.0
# Trial threshold for the current 10 mm tags (observed at 32-38 px).
# Trial threshold for small/far tags (historically observed at 32-38 px).
# Final acceptance is still guarded by static RMS and random validation.
minimum_edge_pixels: 30.0
# Current 30 px tags measure about 0.50-0.53 deg RMS while stationary.
@@ -25,9 +25,9 @@ artifacts:
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
calibration_config_sha256: d1a8be37c9ae21cb4f975ddd66c3cf93fed25dae98ddcf971259b395fa69b521
tag_config: src/g20_thumb_apriltag_calibration/config/three_camera_tags_g20_right_19.yaml
tag_config_sha256: b1ab45e97ae42d57b0a3a63c725107b5aa3828c06b2ced16f8222d6e9ebadc41
release:
# Each task already contains three training cycles plus an isolated fourth
@@ -16,7 +16,7 @@ g20_calibration:
normal_calibration_speed: 15
index_roll_calibration_speed: 5
index_flex_calibration_speed: 10
# 15-Tag产品预检仍使用上面保守速度;只有正反预检都留出至少双倍正式分箱余量,
# 19-Tag产品预检仍使用上面保守速度;只有正反预检都留出至少双倍正式分箱余量,
# 才把非roll任务正式扫描最多提速1.5倍。四指roll受0.5°回差门限约束,
# 始终保持速度5;任一方向采样余量不足也保持原速度。
adaptive_formal_speed_enabled: true
@@ -25,10 +25,16 @@ g20_calibration:
adaptive_formal_speed_maximum_bin_gap: 8
speed_setting_settle_seconds: 0.25
# tag36h11检测角点围成的黑色正方形实测为16 mm不包含外围白边。
# tag36h11尺寸是检测角点围成的黑色正方形边长,不包含外围白边。
# 19张Tag的黑色码区外边长均为16 mm。自定义PnP必须与
# apriltag_ros逐ID尺寸一致,禁止用纸张/白边尺寸代替码区尺寸。
tag_size_m: 0.016
# ROS 2无法从YAML空数组推断整数/浮点数组类型。这四个
# 末端Tag仍显式写16 mm,防止节点启动时得到未初始化参数。
tag_size_override_ids: [7, 14, 16, 18]
tag_size_overrides_m: [0.016, 0.016, 0.016, 0.016]
repetitions: 3
# 15-Tag产品正式零位使用前三轮训练、最后一轮完全留出;旧11-Tag仍读取repetitions=3。
# 19-Tag产品正式零位使用前三轮训练、最后一轮完全留出;旧11-Tag仍读取repetitions=3。
g20_right_19_repetitions: 4
preflight_frames: 60
minimum_detection_rate: 0.95
@@ -78,8 +84,10 @@ g20_calibration:
zero_maximum_axis_cone_mismatch_deg: 5.0
zero_maximum_observability_condition_number: 10000000000.0
zero_maximum_offset_deg: 20.0
# 四指MCP roll/pitch绝对静态零偏的保护范围;PIP仍保留CAD静态零位
# 四指MCP roll保留严格的装配保护范围。MCP pitch/PIP静态零位
# 实测全屈曲行程与反馈0触掌的CAD机械端点联合求解,不允许写死为0。
zero_finger_maximum_offset_deg: 3.0
flexion_endpoint_maximum_offset_deg: 5.0
endpoint_tolerance_u8: 2.0
# 请求命令与固件反馈是两个标定域。稳态检查点允许小幅死区,但反馈
@@ -96,7 +104,7 @@ g20_calibration:
# roll零位127必须从两个方向到位并静止采集,禁止用运动中经过127的帧判回差。
baseline_hold_seconds: 0.5
minimum_baseline_hold_frames: 10
# 15-Tag产品每项正式四轮前先做一次低速往返,端点Tag稳定至少2秒。
# 19-Tag产品每项正式四轮前先做一次低速往返,端点Tag稳定至少2秒。
task_precheck_hold_seconds: 2.0
position_timeout_seconds: 30.0
sweep_timeout_seconds: 90.0
@@ -136,11 +144,11 @@ g20_calibration:
trajectory_maximum_cycle_travel_difference_deg: 3.0
passive_maximum_cycle_travel_difference_deg: 10.0
maximum_monotonic_correction_deg: 2.0
# 旧布局仍用连续扫描正反程差门限;15-Tag产品的连续运动包含速度相关滞后,
# 旧布局仍用连续扫描正反程差门限;19-Tag产品的连续运动包含速度相关滞后,
# 由方向曲线和最终留出验证建模,不再重复硬判。其绝对正反程门禁使用下面
# 的九点稳态command_maximum_direction_gap_deg。
maximum_hysteresis_deg: 2.0
# 15-Tag产品模式额外要求每轮正反方向在各自baseline处绕实测关节轴的角度差
# 19-Tag产品模式额外要求每轮正反方向在各自baseline处绕实测关节轴的角度差
# 不超过0.5°;四指roll例外:127以255→127为唯一物理零位,反向分支
# 保留实测偏差,并改为检查分支间隙上限及跨轮稳定性。
baseline_maximum_hysteresis_deg: 0.5
@@ -153,13 +161,12 @@ g20_calibration:
# 的两条运行曲线仍原样保留固件方向死区,不能把command/feedback差算成回差。
command_maximum_direction_gap_deg: 2.0
# 默认无额外随机动作;15-Tag产品最终一轮始终作为不可关闭的留出验证。
# 默认无额外随机动作;19-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.
# Developer diagnostic only. The formal fourth sweep cycle already gives
# every isolated PIP/DIP pair an independent holdout.
combination_validation_enabled: false
combination_validation_frames: 10
combination_maximum_position_p95_m: 0.003
@@ -170,6 +177,6 @@ g20_calibration:
validation_timeout_seconds: 20.0
maximum_validation_mae_deg: 1.0
maximum_validation_p95_deg: 2.0
# 15-Tag产品模式使用更严格的任一点及静态零偏95%置信区间门限。
# 19-Tag产品模式使用更严格的任一点及静态零偏95%置信区间门限。
maximum_validation_error_deg: 3.0
zero_maximum_confidence_half_width_deg: 1.5
@@ -0,0 +1,63 @@
/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
# Distal Tags use the same measured 16 mm black-code edge as all others.
decimate: 1.0
blur: 0.0
refine: true
sharpening: 0.25
debug: false
pose_estimation_method: pnp
tag:
ids: [4, 5, 6, 7, 14, 15, 16, 17, 18]
frames: [side_base, ring_pip, pinky_pip, pinky_dip, ring_dip, middle_pip, middle_dip, index_pip, index_dip]
sizes: [0.016, 0.016, 0.016, 0.016, 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]
@@ -160,6 +160,12 @@ RIGHT_19_END_ON_IMAGE_CURVE_JOINTS = frozenset(
*(f"{finger}_pip" for finger in _FINGERS),
}
)
# These joints have no independent actuator. Their distal Tags directly
# measure the passive dynamic curve, while their fitted axes serve only as
# downstream phase observers for the corresponding active PIP.
RIGHT_19_VISUALLY_MEASURED_PASSIVE_DIPS = frozenset(
f"{finger}_dip" for finger in _FINGERS
)
_MOTOR_BY_JOINT: dict[str, int] = {
**{f"{finger}_mcp_pitch": index + 1 for index, finger in enumerate(_FINGERS)},
**{f"{finger}_mcp_roll": index + 6 for index, finger in enumerate(_FINGERS)},
@@ -337,11 +343,13 @@ LEFT_HAND_PROFILE = _build_hand_profile("left")
RIGHT_HAND_PROFILE = _build_hand_profile("right")
# ``g20_right_15`` remains accepted as a read/CLI compatibility alias, but a
# newly recorded product session always persists the physical 19-Tag layout
# identifier. Together with the exact ``view_tags`` resume check this keeps a
# former 15-Tag checkpoint from being imported after the four distal Tags are
# installed again.
G20_RIGHT_15_LAYOUT = "g20_right_15"
# Retain the established Python symbol while the product layout moves from 19
# to 15 physical Tags. Persisted/session-facing layout IDs use the new value,
# so old 19-Tag checkpoints cannot be resumed accidentally.
G20_RIGHT_19_LAYOUT = G20_RIGHT_15_LAYOUT
G20_RIGHT_19_LAYOUT = "g20_right_19"
# The three CMC axes keep the calibration domain that was proven on this hand
# in a609d521: dense relative-rotation sweeps with an isolated-cycle holdout.
@@ -423,7 +431,7 @@ def _right_19_auxiliary_commands(
def _build_right_19_profile() -> HandCalibrationProfile:
"""Build the 15-Tag/16-motion-task right-hand profile."""
"""Build the 19-Tag/16-motion-task right-hand profile."""
view_tags: dict[str, dict[str, int]] = {
"front": {
"front_base": 0,
@@ -439,8 +447,12 @@ def _build_right_19_profile() -> HandCalibrationProfile:
"side_base": 4,
"ring_pip": 5,
"pinky_pip": 6,
"pinky_dip": 7,
"ring_dip": 14,
"middle_pip": 15,
"middle_dip": 16,
"index_pip": 17,
"index_dip": 18,
},
"top": {"top_base": 8, "thumb_yaw": 9},
}
@@ -449,7 +461,7 @@ def _build_right_19_profile() -> HandCalibrationProfile:
# palm references. Every moving Tag is still mandatory, but is checked
# by its task-local visibility sweep after the other fingers have moved to
# that task's clearance pose. Requiring all 15 at one instant would make
# a physically correct installation impossible to start.
# a physically correct 19-Tag installation impossible to start.
preflight_view_roles = {
"front": ("front_base",),
"side": ("side_base",),
@@ -496,9 +508,19 @@ def _build_right_19_profile() -> HandCalibrationProfile:
pip, _MOTOR_BY_JOINT[pip], True, "side", "side_base",
f"{finger}_pip", zero_kind="urdf_axis_chain",
)
# DIP is passive, but no longer unobserved. The PIP sweep moves the
# complete linkage once while the middle/distal Tag pair directly
# measures DIP. Its dynamic curve is therefore visual truth; its
# absolute static URDF zero deliberately remains source CAD because an
# arbitrary distal-Tag mounting angle is indistinguishable from a DIP
# zero offset without a mechanical zero fixture.
joint_specs[dip] = JointSpec(
dip, _MOTOR_BY_JOINT[pip], False, None, None, None,
source_joint=pip,
dip,
_MOTOR_BY_JOINT[pip],
False,
"side",
f"{finger}_pip",
f"{finger}_dip",
)
measurement_specs = dict(joint_specs)
@@ -560,7 +582,7 @@ def _build_right_19_profile() -> HandCalibrationProfile:
SweepSpec(
"side",
pip_motor,
(f"{finger}_pip",),
(f"{finger}_pip", f"{finger}_dip"),
f"{finger}_pip_side",
_right_19_auxiliary_commands(
finger, side_view=True, target_kind="pip"
@@ -601,9 +623,9 @@ def get_hand_calibration_profile(
) -> HandCalibrationProfile:
value = str(side).lower()
layout = str(layout_id).lower()
if layout == G20_RIGHT_19_LAYOUT:
if layout in {G20_RIGHT_19_LAYOUT, G20_RIGHT_15_LAYOUT}:
if value != "right":
raise ValueError("g20_right_15 layout requires hand side right")
raise ValueError("G20 right product layout requires hand side right")
return RIGHT_19_HAND_PROFILE
if layout != "legacy_11":
raise ValueError(f"unknown G20 tag layout: {layout_id}")
@@ -620,7 +642,7 @@ def derive_mimic_passive_fits(
*,
profile: HandCalibrationProfile,
) -> dict[str, JointCurveFit]:
"""Add four non-visual DIP curves using the source URDF mimic contract.
"""Complete legacy non-visual DIP curves using the URDF mimic contract.
The runtime curves are relative to the baseline command, so a URDF mimic
offset cancels while the multiplier scales the measured PIP motion. The
@@ -629,6 +651,18 @@ def derive_mimic_passive_fits(
result = dict(fits)
if profile.layout_id != G20_RIGHT_19_LAYOUT:
return result
visually_measured = all(
profile.joint_specs[name].source_joint is None
for name in MIMIC_DERIVED_FINGER_DIPS
)
if visually_measured:
missing = sorted(set(profile.joint_specs) - set(result))
extra = sorted(set(result) - set(profile.joint_specs))
if missing or extra:
raise ValueError(
f"runtime fit set is incomplete: missing={missing} extra={extra}"
)
return result
path = Path(source_urdf).expanduser().resolve()
if not path.is_file():
raise ValueError(f"source URDF does not exist: {path}")
@@ -694,12 +728,16 @@ def derive_mimic_passive_fits(
def clamp_runtime_fits_to_urdf_limits(
source_urdf: str | Path,
fits: Mapping[str, JointCurveFit],
*,
endpoint_anchored_offsets_rad: Mapping[str, float] | None = None,
) -> dict[str, JointCurveFit]:
"""Saturate only published/runtime curves at protected CAD limits.
"""Saturate published curves at protected physical limits.
Raw trajectory fits and all fit/zero diagnostics remain untouched. The
returned transfer functions are the commands a URDF/MuJoCo runtime can
actually represent safely.
actually represent safely. An endpoint-anchored joint uses the corrected
coordinate upper limit ``CAD_upper - origin_offset``; its physical maximum
remains exactly the original CAD maximum after the origin correction.
"""
path = Path(source_urdf).expanduser().resolve()
joints = {
@@ -707,6 +745,10 @@ def clamp_runtime_fits_to_urdf_limits(
for node in ET.parse(path).getroot().findall("joint")
}
result: dict[str, JointCurveFit] = {}
endpoint_offsets = {
str(name): float(value)
for name, value in dict(endpoint_anchored_offsets_rad or {}).items()
}
for name, fit in fits.items():
joint = joints.get(name)
limit = None if joint is None else joint.find("limit")
@@ -718,6 +760,8 @@ def clamp_runtime_fits_to_urdf_limits(
raise ValueError(f"source URDF joint {name} has no finite position limit")
lower = float(limit.get("lower"))
upper = float(limit.get("upper"))
if name in endpoint_offsets:
upper -= endpoint_offsets[name]
if not math.isfinite(lower) or not math.isfinite(upper) or lower > upper:
raise ValueError(f"source URDF joint {name} has invalid position limits")
@@ -1712,7 +1756,7 @@ def validate_compact_payload(payload: Mapping[str, Any]) -> None:
if payload["model"] != "G20" or payload["side"] not in {"left", "right"}:
raise ValueError("payload must describe a left or right G20")
# schema-v4 has no layout field. A right-hand payload without inherited
# source_joint fields is the 15-Tag product layout; legacy
# source_joint fields is the independent right-hand product layout; legacy
# payloads retain their source_joint markers and remain readable.
layout_id = infer_compact_payload_layout(payload)
independent_right = layout_id == G20_RIGHT_19_LAYOUT
@@ -46,6 +46,7 @@ from .urdf_zero import (
fit_joint_axis_measurement,
fit_rotation_joint_curve,
get_zero_calibration_profile,
derive_right_19_flexion_endpoint_offsets,
joint_curve_holdout_errors,
solve_urdf_zero_offsets,
write_zero_corrected_urdf,
@@ -694,6 +695,7 @@ def _validate_corrected_urdf(
source: Path,
corrected: Path,
offsets: Mapping[str, float],
endpoint_anchored_offsets: Mapping[str, float],
axes: Sequence[JointAxisMeasurement],
curves: Mapping[str, JointCurveFit],
motor_by_joint: Mapping[str, int],
@@ -738,11 +740,29 @@ def _validate_corrected_urdf(
original_limit = original_joint.find("limit")
corrected_limit = corrected_joint.find("limit")
if original_limit is not None and corrected_limit is not None:
expected_lower = float(original_limit.get("lower"))
expected_upper = float(original_limit.get("upper"))
if name in endpoint_anchored_offsets:
expected_upper -= float(endpoint_anchored_offsets[name])
if (
original_limit.get("lower") != corrected_limit.get("lower")
or original_limit.get("upper") != corrected_limit.get("upper")
abs(float(corrected_limit.get("lower")) - expected_lower) > 1.0e-12
or abs(float(corrected_limit.get("upper")) - expected_upper) > 1.0e-12
):
raise ValueError(f"corrected URDF unexpectedly changed {name} limits")
original_mimic = original_joint.find("mimic")
corrected_mimic = corrected_joint.find("mimic")
if original_mimic is not None and corrected_mimic is not None:
source_joint = str(original_mimic.get("joint"))
multiplier = float(original_mimic.get("multiplier", "1"))
expected_mimic_offset = float(original_mimic.get("offset", "0"))
expected_mimic_offset += multiplier * float(
endpoint_anchored_offsets.get(source_joint, 0.0)
)
if abs(
float(corrected_mimic.get("offset", "0"))
- expected_mimic_offset
) > 1.0e-12:
raise ValueError(f"corrected URDF has invalid {name} mimic offset")
if maximum_origin_translation_error > 1.0e-12:
raise ValueError("corrected URDF changed a joint origin translation")
if maximum_origin_rotation_error > 1.0e-10:
@@ -815,13 +835,23 @@ def replay_session(
raw_path,
) = _load_raw_session(session)
side = str(start["hand_type"]).lower()
layout_id = str(start.get("tag_layout", "legacy_11")).lower()
profile = get_hand_calibration_profile(side, layout_id)
requested_layout_id = str(
start.get("tag_layout", "legacy_11")
).lower()
profile = get_hand_calibration_profile(side, requested_layout_id)
# Normalise the former g20_right_15 compatibility alias immediately so
# every downstream product guard uses the actual physical 19-Tag profile
# instead of silently falling through legacy-11 logic.
layout_id = profile.layout_id
zero_profile = get_zero_calibration_profile(side, layout_id)
baseline = tuple(int(value) for value in start["baseline_command_u8"])
if len(baseline) != 20:
raise ValueError("session baseline must contain exactly 20 commands")
if layout_id == G20_RIGHT_19_LAYOUT and write_outputs:
if (
layout_id == G20_RIGHT_19_LAYOUT
and write_outputs
and bool(parameters.get("combination_validation_enabled", False))
):
replay_rows = [
json.loads(line)
for line in raw_path.read_text(encoding="utf-8").splitlines()
@@ -923,7 +953,7 @@ def replay_session(
training_cycle_set = set(training_cycles)
if layout_id == G20_RIGHT_19_LAYOUT and len(training_cycles) < 3:
raise ValueError(
"g20_right_15 replay requires at least 3 training cycles and "
"g20_right_19 replay requires at least 3 training cycles and "
"one isolated holdout"
)
training_fits = {
@@ -943,7 +973,7 @@ def replay_session(
if layout_id == G20_RIGHT_19_LAYOUT:
if set(command_records_by_joint) != set(profile.record_joints):
raise ValueError(
"g20_right_15 session is incomplete: missing steady command checkpoints"
"g20_right_19 session is incomplete: missing steady command checkpoints"
)
command_fits = {}
for name in profile.measured_joints:
@@ -1000,13 +1030,10 @@ def replay_session(
for name, value in failed_command_gaps.items()
)
)
command_fits = clamp_runtime_fits_to_urdf_limits(
command_fits = derive_mimic_passive_fits(
source_urdf,
derive_mimic_passive_fits(
source_urdf,
command_fits,
profile=profile,
),
command_fits,
profile=profile,
)
cross_view_roll_metrics: dict[str, dict[str, float]] = {}
for name, validation_name in (
@@ -1195,6 +1222,22 @@ def replay_session(
motor_by_joint = {
name: int(spec.motor_index) for name, spec in profile.joint_specs.items()
}
endpoint_zero_offsets = (
derive_right_19_flexion_endpoint_offsets(
source_urdf,
command_fits,
maximum_offset_rad=math.radians(
float(parameters.get("flexion_endpoint_maximum_offset_deg", 5.0))
),
)
if layout_id == G20_RIGHT_19_LAYOUT
else {}
)
command_fits = clamp_runtime_fits_to_urdf_limits(
source_urdf,
command_fits,
endpoint_anchored_offsets_rad=endpoint_zero_offsets,
)
joint_limits: dict[str, float] = {}
solve_arguments = {
"source_urdf": source_urdf,
@@ -1243,6 +1286,10 @@ def replay_session(
"tag_layout": layout_id,
"training_cycles": training_cycles,
"validation_cycle": validation_cycle,
"fixed_direct_zero_offsets_rad": {
**zero_profile.fixed_direct_zero_offsets_rad,
**endpoint_zero_offsets,
},
}
holdout_zero = solve_urdf_zero_offsets(curves=training_fits, **solve_arguments)
if not holdout_zero.passed:
@@ -1319,12 +1366,14 @@ def replay_session(
output_directory=temporary,
serial_number=hand_serial,
offsets_rad=urdf_offsets,
endpoint_anchored_offsets_rad=endpoint_zero_offsets,
timestamp=stamp,
)
urdf_checks = _validate_corrected_urdf(
source=source_urdf,
corrected=candidate,
offsets=urdf_offsets,
endpoint_anchored_offsets=endpoint_zero_offsets,
axes=axes,
curves=training_fits,
motor_by_joint=motor_by_joint,
@@ -1337,15 +1386,21 @@ def replay_session(
# differ slightly at settled checkpoints; using them here creates
# a fictitious residual zero after a structurally correct write.
curves=training_fits,
fixed_direct_zero_offsets_rad={
name: 0.0
for name in zero_profile.fixed_direct_zero_offsets_rad
},
static_output_zero_offsets_rad={
name: 0.0
for name in zero_profile.static_output_zero_offsets_rad
},
**{**solve_arguments, "source_urdf": candidate},
**{
**solve_arguments,
"source_urdf": candidate,
"fixed_direct_zero_offsets_rad": {
name: 0.0
for name in (
set(zero_profile.fixed_direct_zero_offsets_rad)
| set(endpoint_zero_offsets)
)
},
},
)
maximum_residual_offset = max(
abs(float(value)) for value in residual_zero.direct_offsets_rad.values()
@@ -1452,11 +1507,19 @@ def replay_session(
),
"thumb_default": float(parameters["zero_maximum_offset_deg"]),
},
"static_zero_policy": "direct_measurements_only",
"static_zero_policy": (
"visual_axes_plus_measured_contact_endpoint"
if layout_id == G20_RIGHT_19_LAYOUT
else "direct_measurements_only"
),
"fixed_zero_offsets_deg": {
name: math.degrees(value)
for name, value in zero_profile.fixed_direct_zero_offsets_rad.items()
},
"endpoint_anchored_zero_offsets_deg": {
name: math.degrees(value)
for name, value in endpoint_zero_offsets.items()
},
"static_output_zero_offsets_deg": {
name: math.degrees(value)
for name, value in zero_profile.static_output_zero_offsets_rad.items()
@@ -30,6 +30,38 @@ EXIT_PASS = 0
EXIT_QUALITY = 2
EXIT_SAFETY = 3
STATUS_TIMEOUT_SECONDS = 90.0
FITTING_STATUS_TIMEOUT_SECONDS = 600.0
def _status_timeout_seconds(status: Mapping[str, Any]) -> float:
"""Return the watchdog deadline for the node's current phase.
Motion and acquisition are expected to publish twice a second and retain
the strict transport watchdog. The final 3-D fit is intentionally a
synchronous, CPU-bound operation, so its executor cannot service the
status timer until the fit returns. The node publishes an explicit
FITTING status immediately before entering that operation; allow that
known phase enough time without weakening motion safety checks.
"""
if str(status.get("state", "")) == "FITTING":
return FITTING_STATUS_TIMEOUT_SECONDS
return STATUS_TIMEOUT_SECONDS
def _calibration_node_exited_before_status(log_path: Path) -> bool:
"""Detect a launch child crash while the parent launch is still alive."""
try:
with log_path.open("rb") as stream:
stream.seek(0, os.SEEK_END)
size = stream.tell()
stream.seek(max(0, size - 65536), os.SEEK_SET)
tail = stream.read().decode("utf-8", errors="replace")
except OSError:
return False
return (
"[three_camera_calibration_node-" in tail
and "]: process has died" in tail
)
class CalibrationMonitor(Node):
@@ -124,7 +156,7 @@ def _launch_command(
) -> list[str]:
values = {
"hand_type": "right",
"tag_layout": "g20_right_15",
"tag_layout": "g20_right_19",
"serial_number": config.serial_number,
"can_interface": config.can_interface,
"session_dir": str(session),
@@ -230,6 +262,7 @@ def _run_hardware_session(
)
launched_at = time.monotonic()
last_render = 0.0
last_startup_log_check = 0.0
while True:
rclpy.spin_once(monitor, timeout_sec=0.1)
if monitor.latest_status:
@@ -262,6 +295,16 @@ def _run_hardware_session(
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 - last_startup_log_check >= 0.5
):
last_startup_log_check = now
log_stream.flush()
if _calibration_node_exited_before_status(log_path):
raise RuntimeError(
"CAM-STATUS-202:calibration node exited before status"
)
if (
not monitor.latest_status
and now - launched_at > STATUS_TIMEOUT_SECONDS
@@ -269,7 +312,8 @@ def _run_hardware_session(
raise RuntimeError("CAM-STATUS-202:no calibration status received")
if (
monitor.latest_status
and now - monitor.last_status_at > STATUS_TIMEOUT_SECONDS
and now - monitor.last_status_at
> _status_timeout_seconds(monitor.latest_status)
):
raise RuntimeError("MOTION-COMM-303:calibration status stopped")
except KeyboardInterrupt as error:
@@ -419,7 +463,7 @@ def _automatic_resume_candidate(config: ProductConfig) -> Path | None:
if (
start is None
or start.get("hand_type") != "right"
or start.get("tag_layout") != "g20_right_15"
or start.get("tag_layout") != "g20_right_19"
or start.get("source_urdf_sha256")
!= config.source_urdf_sha256
):
@@ -460,7 +504,7 @@ def run(
print(_startup_failure_block(path, error), flush=True)
return EXIT_QUALITY
if preflight_only:
print("PASS:产品文件、相机内外参、15张Tag配置和CAN接口静态预检通过。")
print("PASS:产品文件、相机内外参、19张Tag配置和CAN接口静态预检通过。")
return EXIT_PASS
config.session_root.mkdir(parents=True, exist_ok=True)
@@ -268,6 +268,18 @@ def classify_error(reason: str, status: Mapping[str, Any]) -> tuple[str, str, st
] if isinstance(views, Mapping) else []
if value.startswith("CFG-"):
return value, "产品配置或文件预检失败", "不要移动相机;复制本诊断块给开发者。"
if value.startswith("CAM-STATUS-202"):
return (
"CAM-STATUS-202",
"标定节点启动后未发布状态",
"机械手尚未开始运动;查看calibration.log中首个节点异常后从断点继续。",
)
if value.startswith("MOTION-COMM-303"):
return (
"MOTION-COMM-303",
"标定节点状态心跳停止更新",
"已采集的完整任务会保留;检查calibration.log后从断点继续,不要重新采集。",
)
if "motor_state_stalled" in value:
return "MOTION-STALL-301", "电机反馈停止向目标推进,程序已保持当前位置", "先检查机械卡阻,未排除前不要重复强推。"
if any(
@@ -280,7 +292,11 @@ def classify_error(reason: str, status: Mapping[str, Any]) -> tuple[str, str, st
)
):
return "OBS-SAMPLE-104", "当前任务的有效视觉轨迹不完整", "根据诊断中的关节、机位和缺失区间处理遮挡或反光后重新运行。"
if "synchronised" in value and not missing and group_pnp_reasons:
if (
("synchronised" in value or "sweep_start_tag_timeout" 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是否脱落、翘起、反光或被遮挡后重新运行。"
@@ -58,17 +58,45 @@ def _camera_info_fingerprint(path: Path) -> str:
)
def _tag_ids(path: Path) -> set[int]:
def _tag_sizes_m_by_id(path: Path) -> dict[int, float]:
with path.open("r", encoding="utf-8") as stream:
payload = _mapping(yaml.safe_load(stream), str(path))
result: set[int] = set()
result: dict[int, float] = {}
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):
sizes = [float(value) for value in tag.get("sizes", [])]
if len(ids) != len(sizes):
raise ValueError("Tag IDs and sizes must have the same length")
if set(result).intersection(ids):
raise ValueError("Tag IDs must be unique across the three views")
result.update(ids)
if any(not 0.0 < size < 0.1 for size in sizes):
raise ValueError("Tag sizes must be in (0, 0.1) metres")
result.update(zip(ids, sizes))
return result
def _custom_pnp_tag_sizes_m_by_id(
path: Path, configured_ids: set[int]
) -> dict[int, float]:
with path.open("r", encoding="utf-8") as stream:
payload = _mapping(yaml.safe_load(stream), str(path))
parameters = _mapping(
_mapping(payload.get("g20_calibration"), "g20_calibration").get(
"ros__parameters"
),
"ros__parameters",
)
default = float(parameters.get("tag_size_m", 0.0))
ids = [int(value) for value in parameters.get("tag_size_override_ids", [])]
sizes = [float(value) for value in parameters.get("tag_size_overrides_m", [])]
if len(ids) != len(sizes) or len(set(ids)) != len(ids):
raise ValueError("custom PnP Tag-size overrides are invalid")
if any(tag_id not in configured_ids for tag_id in ids):
raise ValueError("custom PnP Tag-size override ID is not configured")
result = {tag_id: default for tag_id in configured_ids}
result.update(zip(ids, sizes))
return result
@@ -169,11 +197,22 @@ def load_product_config(
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:
expected_tag_ids = set(range(19))
tag_sizes = _tag_sizes_m_by_id(tag_config)
if set(tag_sizes) != 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"
"g20_right_19 Tag config must contain exactly IDs 0 through 18"
)
expected_tag_sizes = {tag_id: 0.016 for tag_id in expected_tag_ids}
if tag_sizes != expected_tag_sizes:
raise ValueError(
"g20_right_19 black-code Tag sizes must be 16 mm for every ID"
)
if _custom_pnp_tag_sizes_m_by_id(
calibration_config, expected_tag_ids
) != tag_sizes:
raise ValueError(
"custom PnP Tag sizes do not match the apriltag_ros Tag config"
)
camera_raw = _mapping(raw.get("cameras"), "cameras")
@@ -18,18 +18,23 @@ 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
from .urdf_zero import (
RIGHT_19_FLEXION_ENDPOINT_JOINTS,
get_zero_calibration_profile,
)
PASSIVE_JOINTS = frozenset(
{"thumb_ip", "index_dip", "middle_dip", "ring_dip", "pinky_dip"}
)
VISUALLY_MEASURED_FINGER_DIPS = frozenset(
{"index_dip", "middle_dip", "ring_dip", "pinky_dip"}
)
ACTIVE_ZERO_JOINTS = frozenset(
get_zero_calibration_profile("right", G20_RIGHT_19_LAYOUT).direct_zero_joints
)
@@ -83,6 +88,40 @@ def _mask_active_origin_rpy_fields(text: str) -> str:
return pattern.sub(replace, text)
def _mask_endpoint_coordinate_fields(text: str) -> str:
"""Mask only limit/mimic fields induced by flexion zero coordinates."""
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)
name = match.group("name")
if name in RIGHT_19_FLEXION_ENDPOINT_JOINTS:
return re.sub(
r"(<limit\b[^>]*\bupper\s*=\s*)([\"'])[^\"']*\2",
r"\1\2__CALIBRATED_UPPER__\2",
block,
count=1,
)
mimic = re.search(
r"<mimic\b[^>]*\bjoint\s*=\s*([\"'])(?P<source>[^\"']+)\1[^>]*>",
block,
re.DOTALL,
)
if mimic is not None and mimic.group("source") in RIGHT_19_FLEXION_ENDPOINT_JOINTS:
return re.sub(
r"(<mimic\b[^>]*\boffset\s*=\s*)([\"'])[^\"']*\2",
r"\1\2__CALIBRATED_MIMIC_OFFSET__\2",
block,
count=1,
)
return block
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)):
@@ -158,6 +197,7 @@ def verify_corrected_urdf(
corrected: str | Path,
*,
expected_offsets_rad: Mapping[str, float] | None = None,
endpoint_anchored_offsets_rad: Mapping[str, float] | None = None,
) -> tuple[str, ...]:
"""Prove that only active-joint origin.rpy attributes changed.
@@ -173,24 +213,50 @@ def verify_corrected_urdf(
# 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")
masked_source = _mask_endpoint_coordinate_fields(
_mask_active_origin_rpy_fields(source_text)
).rstrip("\r\n")
masked_corrected = _mask_endpoint_coordinate_fields(
_mask_active_origin_rpy_fields(corrected_text)
).rstrip("\r\n")
if masked_source != masked_corrected:
raise ValueError(
"corrected URDF changed content outside active origin.rpy or "
"endpoint-calibrated fields"
)
changed: list[str] = []
for name in before:
if before[name] == after[name]:
continue
if name not in ACTIVE_ZERO_JOINTS:
if name not in ACTIVE_ZERO_JOINTS and name not in PASSIVE_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 name in ACTIVE_ZERO_JOINTS:
changed.append(name)
if expected_offsets_rad is not None:
_verify_expected_origin_offsets(source, corrected, expected_offsets_rad)
endpoint_offsets = {
str(name): float(value)
for name, value in dict(endpoint_anchored_offsets_rad or {}).items()
}
if endpoint_offsets:
source_joints = _joint_elements(source)
corrected_joints = _joint_elements(corrected)
for name, offset in endpoint_offsets.items():
original_limit = source_joints[name].find("limit")
corrected_limit = corrected_joints[name].find("limit")
expected_upper = float(original_limit.get("upper")) - offset
if abs(float(corrected_limit.get("upper")) - expected_upper) > 1.0e-10:
raise ValueError(f"corrected URDF has invalid {name} upper limit")
for name in PASSIVE_JOINTS:
original_mimic = source_joints[name].find("mimic")
corrected_mimic = corrected_joints[name].find("mimic")
if original_mimic is None or corrected_mimic is None:
continue
source_name = str(original_mimic.get("joint"))
expected = float(original_mimic.get("offset", "0"))
expected += float(original_mimic.get("multiplier", "1")) * endpoint_offsets.get(source_name, 0.0)
if abs(float(corrected_mimic.get("offset", "0")) - expected) > 1.0e-10:
raise ValueError(f"corrected URDF has invalid {name} mimic offset")
return tuple(sorted(changed))
@@ -349,7 +415,9 @@ def active_offsets(payload: Mapping[str, Any]) -> dict[str, float]:
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")
raise ValueError(
"payload does not contain all finite observable active zero offsets"
)
return result
@@ -417,10 +485,10 @@ def find_compatible_prior_session(
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.
# The optional eight-pose check remains a developer diagnostic because 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")))
@@ -476,14 +544,6 @@ def finalize_session_artifacts(
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")
@@ -493,8 +553,17 @@ def finalize_session_artifacts(
config.source_urdf,
paths["urdf"],
expected_offsets_rad=offsets,
endpoint_anchored_offsets_rad={
name: offsets[name]
for name in RIGHT_19_FLEXION_ENDPOINT_JOINTS
},
)
validate_runtime_curves_against_urdf_limits(payload, config.source_urdf)
payload, clipped_runtime_joints = clamp_compact_payload_to_urdf_limits(
payload, paths["urdf"]
)
if clipped_runtime_joints:
atomic_write_json(paths["json"], payload)
validate_runtime_curves_against_urdf_limits(payload, paths["urdf"])
mesh_resources = verify_urdf_mesh_resources(paths["urdf"])
mesh_hashes = {
name: sha256_file(path) for name, path in mesh_resources.items()
@@ -518,7 +587,10 @@ def finalize_session_artifacts(
RETAINED_ACTIVE_ZERO_JOINTS
),
"retained_passive_urdf_joints": sorted(PASSIVE_JOINTS),
"mimic_derived_dynamic_joints": sorted(MIMIC_DERIVED_FINGER_DIPS),
"mimic_derived_dynamic_joints": [],
"visually_measured_passive_joints": sorted(
VISUALLY_MEASURED_FINGER_DIPS
),
"changed_urdf_joint_origins": list(changed_joints),
"hashes": {
"source_urdf_sha256": config.source_urdf_sha256,
@@ -272,6 +272,17 @@ def three_camera_reason_zh(
"检查CAN、机械手使能和是否存在机械卡阻,确认安全后调用resume。",
)
if reason == "sweep_start_tag_timeout":
group_reasons = active.get("group_pnp_reasons", {})
if isinstance(group_reasons, Mapping) and group_reasons:
reason_text = "".join(
f"{VIEW_NAMES_ZH.get(str(view), str(view))}={value}"
for view, value in group_reasons.items()
)
return (
"被测电机已经到达扫描起点,所需Tag也可见,但整组PnP初始化持续"
f"拒绝候选({reason_text}),因此没有生成同步端点帧。",
"不要重新粘贴可见Tag;保留当前会话并复制诊断块给开发者检查PnP安装先验。",
)
return (
"被测电机已经到达扫描起点,但当前任务所需的实时运动Tag没有形成足够的"
"同步有效帧。允许遮挡的固定掌部Tag会显示为“锁”,不会触发此错误。",
@@ -315,8 +326,9 @@ def three_camera_reason_zh(
"rotation_orthogonal_rms_deg": "三维旋转轴外残差RMS",
"axis_plane_rms_mm": "三维圆轴向RMS",
"axis_radial_rms_mm": "三维圆半径RMS",
"axis_pose_line_rms_mm": "姿态轨迹轴线RMS",
"rotation_circle_axis_difference_deg": "姿态轴与圆轨迹轴夹角",
"axis_cycle_difference_deg": "轮转轴方向极差",
"axis_cycle_difference_deg": "轮转轴方向极差",
"third_cycle_axis_holdout_deg": "第三轮留出零位可观测轴向误差",
"third_cycle_axis_line_rms_mm": "第三轮留出轴线RMS",
"third_cycle_trajectory_p95_deg": "第三轮留出轨迹误差P95",
@@ -340,6 +352,7 @@ def three_camera_reason_zh(
"rotation_orthogonal_rms_deg": "°",
"axis_plane_rms_mm": "mm",
"axis_radial_rms_mm": "mm",
"axis_pose_line_rms_mm": "mm",
"rotation_circle_axis_difference_deg": "°",
"axis_cycle_difference_deg": "°",
"third_cycle_axis_holdout_deg": "°",
@@ -17,7 +17,9 @@ from typing import Any, Mapping, Sequence
import numpy as np
import rclpy
from apriltag_msgs.msg import AprilTagDetectionArray
from rclpy.exceptions import ParameterUninitializedException
from rclpy.node import Node
from rclpy.parameter import Parameter
from rclpy.qos import qos_profile_sensor_data
from scipy.spatial.transform import Rotation
from sensor_msgs.msg import CameraInfo, JointState
@@ -51,6 +53,7 @@ from .full_hand import (
G20_RIGHT_19_LAYOUT,
LEFT_HAND_PROFILE,
RIGHT_19_END_ON_IMAGE_CURVE_JOINTS,
RIGHT_19_VISUALLY_MEASURED_PASSIVE_DIPS,
THREE_CAMERA_BASELINE_COMMAND,
HandCalibrationProfile,
JointCurveFit,
@@ -86,6 +89,7 @@ from .urdf_zero import (
circle_direction_is_constrained,
fit_joint_axis_measurement,
fit_rotation_joint_curve,
derive_right_19_flexion_endpoint_offsets,
joint_curve_holdout_errors,
measure_joint_curve_observation,
solve_urdf_zero_offsets,
@@ -264,6 +268,130 @@ def _sweep_joints_for_view(
)
def resume_tasks_invalidated_by_tag_size_changes(
profile: HandCalibrationProfile,
previous_sizes_m_by_id: Mapping[int, float],
current_sizes_m_by_id: Mapping[int, float],
) -> tuple[tuple[int, ...], tuple[str, ...]]:
"""Localise a corrected physical Tag size to dependent motion tasks.
PnP translations stored in a checkpoint are already scaled by the Tag
size used during acquisition and cannot be repaired exactly afterwards.
A size-only product correction therefore invalidates every task that used
one of those Tags, while complete tasks whose required Tags are unchanged
remain valid sparse checkpoints.
"""
previous = {
int(key): float(value)
for key, value in previous_sizes_m_by_id.items()
}
current = {
int(key): float(value)
for key, value in current_sizes_m_by_id.items()
}
if set(previous) != set(current):
raise ValueError("resume Tag-size maps contain different Tag IDs")
if any(
not math.isfinite(value) or value <= 0.0
for value in (*previous.values(), *current.values())
):
raise ValueError("resume Tag sizes must be finite and positive")
changed_ids = {
tag_id
for tag_id in current
if not math.isclose(
previous[tag_id], current[tag_id], rel_tol=0.0, abs_tol=1.0e-12
)
}
affected: list[str] = []
base_role_by_view = {
"front": "front_base",
"side": "side_base",
"top": "top_base",
}
for spec in profile.sweep_specs:
used_ids: set[int] = set()
for view in _sweep_views(profile, spec):
used_ids.add(int(profile.view_tags[view][base_role_by_view[view]]))
for joint_name in _sweep_joints_for_view(profile, spec, view):
joint = profile.record_specs[joint_name]
for role in (joint.parent_role, joint.child_role):
if role is not None:
used_ids.add(int(profile.view_tags[view][role]))
if used_ids & changed_ids:
affected.append(spec.key)
return tuple(sorted(changed_ids)), tuple(affected)
def _fit_retry_joint_names(
profile: HandCalibrationProfile,
spec: SweepSpec,
failures: Sequence[Mapping[str, Any]],
) -> set[str]:
"""Resolve the independently failed measurements in a physical task."""
selected: set[str] = set()
validation_sources = profile.axis_validation_sources or {}
for failure in failures:
name = str(failure.get("joint", ""))
if str(failure.get("metric", "")) == "cross_view_roll_curve":
name = str(validation_sources.get(name, name))
if name in spec.joints:
selected.add(name)
return selected or set(spec.joints)
def _isolated_axis_cycle_outliers(
axes: Sequence[np.ndarray], limit_rad: float
) -> set[int]:
"""Return one unambiguous outlier outside an all-other-cycle cluster.
At least three mutually consistent inlier cycles are required. Ambiguous
or generally scattered axes deliberately return an empty set so callers
retain the conservative full-task retry.
"""
if len(axes) < 4:
return set()
candidates: list[tuple[float, int]] = []
for excluded in range(len(axes)):
inliers = [
np.asarray(axis, dtype=float)
for index, axis in enumerate(axes)
if index != excluded
]
consistent = True
maximum_inlier_difference = 0.0
for left_index, left in enumerate(inliers):
for right in inliers[left_index + 1 :]:
difference = math.acos(
abs(float(np.clip(left @ right, -1.0, 1.0)))
)
maximum_inlier_difference = max(
maximum_inlier_difference, difference
)
if difference > float(limit_rad):
consistent = False
break
if not consistent:
break
if consistent:
candidates.append((maximum_inlier_difference, excluded))
if not candidates:
return set()
candidates.sort()
best_spread, best_excluded = candidates[0]
# A gradual end-to-end drift can produce two different all-but-one
# subsets just under the formal limit. Localize only when one three-cycle
# cluster is substantially tighter than the acceptance band.
if best_spread > 0.5 * float(limit_rad):
return set()
if (
len(candidates) > 1
and abs(candidates[1][0] - best_spread) <= math.radians(0.01)
):
return set()
return {best_excluded}
def _fixed_base_role(view: str) -> str:
return {
"front": "front_base",
@@ -575,6 +703,19 @@ def _unresolved_fit_failure_tasks(
# failures that mixed velocity lag or firmware tracking
# deadband with mechanical hysteresis are safe to revalidate.
return True
if (
profile.layout_id == G20_RIGHT_19_LAYOUT
and joint_name
in RIGHT_19_VISUALLY_MEASURED_PASSIVE_DIPS
and metric == "axis_pose_line_rms_mm"
):
# Product v19 originally treated the correlated per-frame
# pose-line residual of an incorrectly scaled passive-DIP Tag as the
# uncertainty of its fitted axis centre. Current code
# revalidates those complete raw trajectories using radial,
# cross-cycle and holdout gates, so the old rejection itself
# must not force another eight-direction scan.
return True
return metric in {
"rotation_circle_axis_difference_deg",
"axis_plane_rms_mm",
@@ -1085,6 +1226,7 @@ class ViewRuntime:
tracker: SquareTagPoseTracker
group_tracker: SquareTagGroupPoseTracker
view_tags: Mapping[str, int] = field(default_factory=dict)
tag_sizes_m_by_role: Mapping[str, float] = field(default_factory=dict)
preflight_roles: tuple[str, ...] = ()
role_by_id: dict[int, str] = field(default_factory=dict)
group_trackers: dict[
@@ -1132,6 +1274,13 @@ class ViewRuntime:
self.preflight_roles = tuple(self.view_tags)
if any(role not in self.view_tags for role in self.preflight_roles):
raise ValueError("preflight roles must be configured for the view")
if any(role not in self.view_tags for role in self.tag_sizes_m_by_role):
raise ValueError("Tag-size roles must be configured for the view")
if any(
not math.isfinite(float(size)) or float(size) <= 0.0
for size in self.tag_sizes_m_by_role.values()
):
raise ValueError("Tag sizes must be finite and positive")
self.current_required_roles = tuple(self.preflight_roles)
self.group_trackers[tuple(self.preflight_roles)] = self.group_tracker
@@ -1237,6 +1386,7 @@ class G20ThreeCameraCalibrationNode(Node):
self.retry_sweep_spec: SweepSpec | None = None
self.retry_resume_index: int | None = None
self.retry_sweep_items: list[SweepItem] = []
self.retry_joint_names: set[str] = set()
self.active_sweep_is_fit_retry = False
self.fit_failure: dict[str, Any] = {}
self.fit_failure_history_by_task: dict[
@@ -1359,6 +1509,12 @@ class G20ThreeCameraCalibrationNode(Node):
f"/g20_calibration/{view}/apriltag/detections",
)
self.declare_parameter("tag_size_m", 0.016)
self.declare_parameter(
"tag_size_override_ids", Parameter.Type.INTEGER_ARRAY
)
self.declare_parameter(
"tag_size_overrides_m", Parameter.Type.DOUBLE_ARRAY
)
self.declare_parameter(
"baseline_command_u8", list(THREE_CAMERA_BASELINE_COMMAND)
)
@@ -1414,6 +1570,7 @@ class G20ThreeCameraCalibrationNode(Node):
)
self.declare_parameter("zero_maximum_offset_deg", 20.0)
self.declare_parameter("zero_finger_maximum_offset_deg", 3.0)
self.declare_parameter("flexion_endpoint_maximum_offset_deg", 5.0)
self.declare_parameter("endpoint_tolerance_u8", 2.0)
self.declare_parameter(
"steady_checkpoint_command_feedback_tolerance_u8", 8.0
@@ -1510,6 +1667,17 @@ class G20ThreeCameraCalibrationNode(Node):
def value(name: str) -> Any:
return self.get_parameter(name).value
def optional_array(name: str) -> Sequence[Any]:
# An empty YAML sequence has no element type. With a typed ROS 2
# declaration it can therefore remain NOT_SET and ``.value``
# raises instead of returning an empty list. These overrides are
# optional, so preserve the default-size semantics in that case.
try:
result = value(name)
except ParameterUninitializedException:
return ()
return () if result is None else result
self.hand_type = str(value("hand_type")).lower()
self.tag_layout = str(value("tag_layout")).lower()
self.profile: HandCalibrationProfile = get_hand_calibration_profile(
@@ -1578,6 +1746,46 @@ class G20ThreeCameraCalibrationNode(Node):
for view in ("front", "side", "top")
}
self.tag_size_m = float(value("tag_size_m"))
override_ids = tuple(
int(item) for item in optional_array("tag_size_override_ids")
)
override_sizes = tuple(
float(item) for item in optional_array("tag_size_overrides_m")
)
if len(override_ids) != len(override_sizes):
raise ValueError(
"tag_size_override_ids and tag_size_overrides_m must have "
"the same length"
)
if len(set(override_ids)) != len(override_ids):
raise ValueError("tag_size_override_ids must be unique")
configured_tag_ids = {
int(tag_id)
for tags in self.profile.view_tags.values()
for tag_id in tags.values()
}
if (
self.profile.layout_id == G20_RIGHT_19_LAYOUT
and any(tag_id not in configured_tag_ids for tag_id in override_ids)
):
raise ValueError("Tag-size override contains an unconfigured Tag ID")
if (
not math.isfinite(self.tag_size_m)
or self.tag_size_m <= 0.0
or any(
not math.isfinite(size) or size <= 0.0
for size in override_sizes
)
):
raise ValueError("Tag sizes must be finite and positive")
self.tag_sizes_m_by_id = {
tag_id: self.tag_size_m for tag_id in configured_tag_ids
}
self.tag_sizes_m_by_id.update(
(tag_id, size)
for tag_id, size in zip(override_ids, override_sizes)
if tag_id in configured_tag_ids
)
self.baseline_command = tuple(
int(item) for item in value("baseline_command_u8")
)
@@ -1688,7 +1896,10 @@ class G20ThreeCameraCalibrationNode(Node):
self.zero_finger_maximum_offset_rad = math.radians(
float(value("zero_finger_maximum_offset_deg"))
)
self.zero_joint_maximum_offsets_rad: dict[str, float] = {}
self.flexion_endpoint_maximum_offset_rad = math.radians(
float(value("flexion_endpoint_maximum_offset_deg"))
)
self.zero_joint_maximum_offsets_rad = {}
self.endpoint_tolerance_u8 = float(value("endpoint_tolerance_u8"))
self.steady_checkpoint_command_feedback_tolerance_u8 = float(
value("steady_checkpoint_command_feedback_tolerance_u8")
@@ -1867,7 +2078,7 @@ class G20ThreeCameraCalibrationNode(Node):
r"[0-9a-f]{64}", self.source_urdf_expected_sha256
) is None:
raise ValueError(
"g20_right_15 requires source_urdf_expected_sha256 from "
"g20_right_19 requires source_urdf_expected_sha256 from "
"the CAD/hardware owner; unconfirmed source CAD cannot "
"produce a formal corrected URDF"
)
@@ -2006,6 +2217,7 @@ class G20ThreeCameraCalibrationNode(Node):
self.zero_maximum_axis_cone_mismatch_rad,
self.zero_maximum_offset_rad,
self.zero_finger_maximum_offset_rad,
self.flexion_endpoint_maximum_offset_rad,
self.maximum_validation_error_rad,
self.zero_maximum_confidence_half_width_rad,
self.task_precheck_hold_seconds,
@@ -2033,7 +2245,7 @@ class G20ThreeCameraCalibrationNode(Node):
and self.repetitions < 4
):
raise ValueError(
"g20_right_15 requires at least 4 repetitions: at least 3 "
"g20_right_19 requires at least 4 repetitions: at least 3 "
"training cycles and one isolated holdout"
)
if (
@@ -2067,7 +2279,7 @@ class G20ThreeCameraCalibrationNode(Node):
and self.profile.layout_id != G20_RIGHT_19_LAYOUT
):
raise ValueError(
"cross-view roll diagnostic requires g20_right_15"
"cross-view roll diagnostic requires g20_right_19"
)
if not 1.0 <= self.provisional_warning_ratio <= 2.0:
raise ValueError("provisional_warning_ratio must be in [1, 2]")
@@ -2121,7 +2333,19 @@ class G20ThreeCameraCalibrationNode(Node):
# static IPPE mirror branches; their face-normal consistency can. Do
# not apply this prior to the articulated front thumb chain.
normal_alignment_pairs = (
adjacent_pairs if name == "side" and len(role_tuple) <= 4 else ()
tuple(
pair
for pair in adjacent_pairs
# Distal 16 mm Tags may be carried by a rigid extension rod
# to remain visible through PIP flexion. Its mounting plane
# is arbitrary and must not be required to be parallel to the
# middle-phalanx Tag. Static rigidity and relative-pose
# continuity still disambiguate that PIP->DIP pair; retain
# the palm/base->middle-Tag normal prior unchanged.
if not pair[1].endswith("_dip")
)
if name == "side" and len(role_tuple) <= 4
else ()
)
thumb_mcp_ip_roles = {"thumb_cmc", "thumb_mcp", "thumb_ip"}
thumb_mcp_ip_coupling = ()
@@ -2198,6 +2422,10 @@ class G20ThreeCameraCalibrationNode(Node):
tracker=tracker,
group_tracker=group,
view_tags=self.profile.view_tags[name],
tag_sizes_m_by_role={
role: self.tag_sizes_m_by_id[int(tag_id)]
for role, tag_id in self.profile.view_tags[name].items()
},
preflight_roles=preflight_roles,
)
@@ -2606,7 +2834,11 @@ class G20ThreeCameraCalibrationNode(Node):
pose, rejection = runtime.tracker.estimate(
role,
corners_by_role[role],
tag_size_m=runtime.tag_size_m,
tag_size_m=float(
runtime.tag_sizes_m_by_role.get(
role, runtime.tag_size_m
)
),
camera_matrix=runtime.camera_matrix,
stamp_ns=stamp,
)
@@ -2749,9 +2981,23 @@ class G20ThreeCameraCalibrationNode(Node):
runtime.last_message_at = now
runtime.detection_times.append(now)
runtime.valid_flags.append(valid)
if runtime.current_required_roles != tuple(
runtime.preflight_roles
task_sweep_capture = bool(
self.state == STATE_SWEEP
and self.active_sweep is not None
and view in _sweep_views(
_node_profile(self), self.active_sweep.spec
)
)
if (
runtime.current_required_roles
!= tuple(runtime.preflight_roles)
and task_sweep_capture
):
# Attempt-level validity is a motion-trajectory metric. PREPARE
# deliberately rejects the first N frames while the group PnP
# tracker initializes; counting those expected frames made clean
# side tasks report about 94% and triggered full four-round
# rescans despite 100% visibility prechecks and dense trajectories.
runtime.task_total_frames += 1
if valid:
runtime.task_valid_frames += 1
@@ -3060,16 +3306,23 @@ class G20ThreeCameraCalibrationNode(Node):
)
def _prepare_failed_sweep_retry(self) -> SweepSpec:
"""Discard only failed cycles and queue their two sweep directions."""
"""Discard only failed measurements/cycles and queue two directions."""
if self.retry_sweep_spec is None:
raise RuntimeError("no failed sweep is pending retry")
spec = self.retry_sweep_spec
repetitions = int(getattr(self, "repetitions", 3))
cycles = set(getattr(self, "retry_cycles", set(range(repetitions))))
retry_joint_names = set(
getattr(self, "retry_joint_names", set()) or spec.joints
)
retry_joint_names.intersection_update(spec.joints)
if not retry_joint_names:
retry_joint_names = set(spec.joints)
self.retry_joint_names = retry_joint_names
baseline_records_by_joint = getattr(
self, "baseline_records_by_joint", {}
)
for joint_name in spec.joints:
for joint_name in retry_joint_names:
self.records_by_joint[joint_name][:] = [
record
for record in self.records_by_joint[joint_name]
@@ -3110,13 +3363,24 @@ class G20ThreeCameraCalibrationNode(Node):
# retry. Raw samples remain logged and all final quality gates stay
# unchanged.
reset_trackers = getattr(self, "_reset_view_trackers", None)
for view in _sweep_views(_node_profile(self), spec):
retry_views = {
_node_profile(self).record_specs[name].view
for name in retry_joint_names
}
for view in retry_views:
if view is None:
continue
runtime = getattr(self, "views", {}).get(view)
if runtime is None or reset_trackers is None:
continue
reset_trackers(runtime)
runtime.pnp_invalid_since = None
runtime.pnp_reset_count += 1
# Detection quality is attempt-local. Carrying the previous
# attempt into this retry made its percentage describe two PnP
# branches instead of the data currently being admitted.
runtime.task_valid_frames = 0
runtime.task_total_frames = 0
storage_key = _sweep_storage_key(spec)
self.sweep_attempts[storage_key] = (
self.sweep_attempts.get(storage_key, 1) + 1
@@ -3132,6 +3396,7 @@ class G20ThreeCameraCalibrationNode(Node):
**({"task_name": spec.key} if spec.task_name else {}),
"motor_index": spec.motor_index,
"joints": list(spec.joints),
"joints_to_rescan": sorted(retry_joint_names),
"attempt": self.sweep_attempts[storage_key],
"reason": self.paused_reason,
"cycles": [cycle + 1 for cycle in sorted(cycles)],
@@ -3167,6 +3432,13 @@ class G20ThreeCameraCalibrationNode(Node):
if len(starts) != 1:
raise RuntimeError("resume raw must contain exactly one session_start")
start = starts[0]
previous_tag_sizes = {
int(tag_id): float(size)
for tag_id, size in dict(
start.get("tag_sizes_m_by_id", {})
).items()
}
current_tag_sizes = dict(getattr(self, "tag_sizes_m_by_id", {}))
if (
str(start.get("hand_type")) != self.hand_type
or str(start.get("tag_layout")) != self.profile.layout_id
@@ -3180,6 +3452,25 @@ class G20ThreeCameraCalibrationNode(Node):
raise RuntimeError(
"resume checkpoint geometry, Tag layout, baseline or source URDF differs"
)
try:
changed_tag_size_ids, size_invalidated_tasks = (
resume_tasks_invalidated_by_tag_size_changes(
self.profile,
previous_tag_sizes,
current_tag_sizes,
)
)
except ValueError as error:
raise RuntimeError(
"resume checkpoint Tag-size geometry differs incompatibly"
) from error
invalidated_task_set = set(size_invalidated_tasks)
if invalidated_task_set:
rows = [
row
for row in rows
if str(row.get("task_name", "")) not in invalidated_task_set
]
completed, reusable = resumable_completed_task_prefix(
self.profile,
self.repetitions,
@@ -3241,6 +3532,8 @@ class G20ThreeCameraCalibrationNode(Node):
),
"pending_task_keys": missing_tasks,
"revalidation_dropped_tasks": dropped,
"tag_size_changed_ids": list(changed_tag_size_ids),
"tag_size_invalidated_task_keys": list(size_invalidated_tasks),
"imported_record_count": len(reusable),
"source_raw_samples_sha256": _file_sha256(source),
},
@@ -3377,6 +3670,7 @@ class G20ThreeCameraCalibrationNode(Node):
self.retry_sweep_spec = None
self.retry_resume_index = None
self.retry_sweep_items.clear()
self.retry_joint_names.clear()
self.active_sweep_is_fit_retry = False
self.fit_failure = {}
self.fit_failure_history_by_task.clear()
@@ -3433,6 +3727,10 @@ class G20ThreeCameraCalibrationNode(Node):
},
"tag_family": "36h11",
"tag_size_m": float(self.tag_size_m),
"tag_sizes_m_by_id": {
str(tag_id): float(self.tag_sizes_m_by_id[tag_id])
for tag_id in sorted(self.tag_sizes_m_by_id)
},
"baseline_command_u8": [
int(value) for value in self.baseline_command
],
@@ -4659,7 +4957,16 @@ class G20ThreeCameraCalibrationNode(Node):
preserve_task_reference = bool(
not item.precheck and item.cycle > 0
)
for view in _sweep_views(_node_profile(self), item.spec):
reset_views = set(_sweep_views(_node_profile(self), item.spec))
if getattr(self, "active_sweep_is_fit_retry", False):
scoped = set(getattr(self, "retry_joint_names", set()))
if scoped:
reset_views = {
_node_profile(self).record_specs[name].view
for name in scoped
}
reset_views.discard(None)
for view in reset_views:
runtime = getattr(self, "views", {}).get(view)
if runtime is None or reset_trackers is None:
continue
@@ -4828,7 +5135,14 @@ class G20ThreeCameraCalibrationNode(Node):
) -> None:
"""Persist one steady requested-command/feedback/vision observation."""
motor = int(item.spec.motor_index)
for joint_name in item.spec.joints:
record_joint_names = tuple(item.spec.joints)
if getattr(self, "active_sweep_is_fit_retry", False):
scoped = set(getattr(self, "retry_joint_names", set()))
if scoped:
record_joint_names = tuple(
name for name in item.spec.joints if name in scoped
)
for joint_name in record_joint_names:
selected = _frames_for_joint(frames, joint_name)
if len(selected) < 3:
raise RuntimeError(
@@ -5415,6 +5729,21 @@ class G20ThreeCameraCalibrationNode(Node):
if include_view_validity and hasattr(self, "views"):
for view in _sweep_views(profile, spec):
runtime = self.views[view]
view_joints = _sweep_joints_for_view(profile, spec, view)
validation_only_view = bool(view_joints) and all(
profile.record_specs[name].zero_kind
== "axis_cross_view_validation"
for name in view_joints
)
retry_scope = set(
getattr(self, "retry_joint_names", set())
if getattr(self, "retry_sweep_spec", None) == spec
else ()
)
if retry_scope and not retry_scope.intersection(view_joints):
# This view's accepted records were intentionally kept;
# only the independently failed view is being measured.
continue
valid_rate = float(runtime.valid_rate)
# Prefer the task-scoped counters: after the last sweep the
# required-role set flips back to the full preflight set and
@@ -5434,10 +5763,8 @@ class G20ThreeCameraCalibrationNode(Node):
if (
not imported_without_capture
and valid_rate < self.minimum_detection_rate
and not validation_only_view
):
view_joints = _sweep_joints_for_view(
profile, spec, view
)
failures.append(
{
"joint": view_joints[0],
@@ -5921,20 +6248,31 @@ class G20ThreeCameraCalibrationNode(Node):
),
)
if maximum_difference > self.zero_maximum_axis_cycle_difference_rad:
failures.append(
{
"joint": joint_name,
"metric": "axis_cycle_difference_deg",
"actual": round(math.degrees(maximum_difference), 6),
"limit": round(
math.degrees(
self.zero_maximum_axis_cycle_difference_rad
),
6,
),
"comparison": "maximum",
}
outliers = _isolated_axis_cycle_outliers(
cycle_axes,
self.zero_maximum_axis_cycle_difference_rad,
)
failure = {
"joint": joint_name,
"metric": "axis_cycle_difference_deg",
"actual": round(math.degrees(maximum_difference), 6),
"limit": round(
math.degrees(
self.zero_maximum_axis_cycle_difference_rad
),
6,
),
"comparison": "maximum",
}
if outliers:
outlier = next(iter(outliers))
failure["cycle"] = outlier + 1
failure["inlier_cycles"] = [
index + 1
for index in range(len(cycle_axes))
if index != outlier
]
failures.append(failure)
return failures
def _cross_view_roll_diagnostic_role(
@@ -6099,6 +6437,9 @@ class G20ThreeCameraCalibrationNode(Node):
attempt = self.sweep_attempts.get(_sweep_storage_key(spec), 1)
self.retry_resume_index = self.sweep_index
self.retry_sweep_spec = spec
self.retry_joint_names = _fit_retry_joint_names(
_node_profile(self), spec, failures
)
localized_cycles = {
int(item["cycle"]) - 1
for item in failures
@@ -6141,6 +6482,7 @@ class G20ThreeCameraCalibrationNode(Node):
"task_name": spec.key,
"motor_index": spec.motor_index,
"joints": list(spec.joints),
"joints_to_rescan": sorted(self.retry_joint_names),
"attempt": attempt,
"cycles_to_rescan": [
cycle + 1 for cycle in sorted(self.retry_cycles)
@@ -6264,6 +6606,7 @@ class G20ThreeCameraCalibrationNode(Node):
self.retry_sweep_spec = None
self.retry_resume_index = None
self.retry_cycles = set()
self.retry_joint_names = set()
self.fit_failure = {
"kind": "zero_model_failure",
"view": spec.view,
@@ -6296,7 +6639,7 @@ class G20ThreeCameraCalibrationNode(Node):
)
if self.profile.layout_id == G20_RIGHT_19_LAYOUT:
atomic_write_json(
self.session_dir / "g20_right_15_failed_diagnostics.json",
self.session_dir / "g20_right_19_failed_diagnostics.json",
{
"schema_version": 5,
"layout_id": G20_RIGHT_19_LAYOUT,
@@ -6410,7 +6753,14 @@ class G20ThreeCameraCalibrationNode(Node):
motor = item.spec.motor_index
command = int(self.baseline_command[motor])
attempt = self.sweep_attempts.get(_sweep_storage_key(item.spec), 1)
for joint_name in item.spec.joints:
record_joint_names = tuple(item.spec.joints)
if getattr(self, "active_sweep_is_fit_retry", False):
scoped = set(getattr(self, "retry_joint_names", set()))
if scoped:
record_joint_names = tuple(
name for name in item.spec.joints if name in scoped
)
for joint_name in record_joint_names:
selected = _frames_for_joint(frames, joint_name)
if len(selected) < minimum_frames:
raise RuntimeError(
@@ -6549,8 +6899,15 @@ class G20ThreeCameraCalibrationNode(Node):
target_tolerance = self._endpoint_tolerance_for_spec(
item.spec, item.target_u8
)
record_joint_names = tuple(item.spec.joints)
if getattr(self, "active_sweep_is_fit_retry", False):
scoped = set(getattr(self, "retry_joint_names", set()))
if scoped:
record_joint_names = tuple(
name for name in item.spec.joints if name in scoped
)
joint_bins: dict[str, dict[int, list[FrameObservation]]] = {}
for joint_name in item.spec.joints:
for joint_name in record_joint_names:
bins: dict[int, list[FrameObservation]] = {}
for frame in _frames_for_joint(self.sweep_frames, joint_name):
state = float(frame.state_u8[motor])
@@ -6704,7 +7061,7 @@ class G20ThreeCameraCalibrationNode(Node):
) -> dict[str, Any]:
names = [
name
for name in item.spec.joints
for name in record_joint_names
if name in frame.joint_quaternions_xyzw
]
return {
@@ -6755,12 +7112,12 @@ class G20ThreeCameraCalibrationNode(Node):
for frame in self.sweep_frames
if any(
name in frame.joint_quaternions_xyzw
for name in item.spec.joints
for name in record_joint_names
)
),
)
for joint_name in item.spec.joints:
for joint_name in record_joint_names:
bins = joint_bins[joint_name]
commands = sorted(bins)
for command in commands:
@@ -6905,6 +7262,8 @@ class G20ThreeCameraCalibrationNode(Node):
return
self.retry_sweep_spec = None
self.retry_resume_index = None
self.retry_cycles = set()
self.retry_joint_names = set()
self.fit_failure = {}
if self.sweep_index >= len(self.sweep_items):
self._begin_return_baseline("fit")
@@ -6939,6 +7298,8 @@ class G20ThreeCameraCalibrationNode(Node):
resume_index = self.retry_resume_index
self.retry_sweep_spec = None
self.retry_resume_index = None
self.retry_cycles = set()
self.retry_joint_names = set()
if resume_index is not None:
self.sweep_index = resume_index
self.fit_failure = {}
@@ -6965,6 +7326,14 @@ class G20ThreeCameraCalibrationNode(Node):
def _fit_all_curves(self) -> None:
self.state = STATE_FITTING
self.reason = "fitting_3d_axes_and_urdf_zero_offsets"
# Fitting is deliberately synchronous so no capture callback can
# mutate the observation set midway through a model fit. Announce
# the state before entering the CPU-bound section: the outer product
# runner then applies its fitting-specific watchdog instead of
# mistaking a blocked ROS timer for a motion communication failure.
fitting_started_at = time.monotonic()
self.last_status_publish = fitting_started_at
self._publish_status(fitting_started_at)
# Provisional checks have a warning band so collection can continue,
# but the final fit always re-applies the unmodified hard thresholds.
for spec in self.profile.sweep_specs:
@@ -7233,6 +7602,15 @@ class G20ThreeCameraCalibrationNode(Node):
name: int(spec.motor_index)
for name, spec in self.profile.joint_specs.items()
}
endpoint_zero_offsets = (
derive_right_19_flexion_endpoint_offsets(
self.source_urdf_path,
command_fits,
maximum_offset_rad=self.flexion_endpoint_maximum_offset_rad,
)
if self.profile.layout_id == G20_RIGHT_19_LAYOUT
else {}
)
holdout_zero_result = solve_urdf_zero_offsets(
source_urdf=self.source_urdf_path,
measurements=axes,
@@ -7269,6 +7647,10 @@ class G20ThreeCameraCalibrationNode(Node):
tag_layout=self.profile.layout_id,
training_cycles=training_cycles,
validation_cycle=validation_cycle,
fixed_direct_zero_offsets_rad={
**self.zero_profile.fixed_direct_zero_offsets_rad,
**endpoint_zero_offsets,
},
)
append_jsonl(
self.raw_path,
@@ -7492,6 +7874,7 @@ class G20ThreeCameraCalibrationNode(Node):
command_fits,
profile=self.profile,
),
endpoint_anchored_offsets_rad=endpoint_zero_offsets,
)
self.axis_measurements = axes
self.zero_result = zero_result
@@ -8046,6 +8429,7 @@ class G20ThreeCameraCalibrationNode(Node):
output_directory=self.corrected_urdf_output_dir,
serial_number=self.serial_number,
offsets_rad=urdf_offsets,
endpoint_anchored_offsets_rad=endpoint_zero_offsets,
timestamp=stamp,
)
try:
@@ -23,6 +23,7 @@ from .full_hand import (
IMAGE_TRAJECTORY_JOINTS,
LEFT_HAND_PROFILE,
RIGHT_19_END_ON_IMAGE_CURVE_JOINTS,
RIGHT_19_VISUALLY_MEASURED_PASSIVE_DIPS,
HandCalibrationProfile,
JointCurveFit,
get_hand_calibration_profile,
@@ -137,7 +138,15 @@ def _build_zero_profile(hand: HandCalibrationProfile) -> ZeroCalibrationProfile:
def _build_right_19_zero_profile(
hand: HandCalibrationProfile,
) -> ZeroCalibrationProfile:
"""Return the 12-observable-static-zero graph for the 15-Tag layout."""
"""Return the 16-active-zero graph for the 19-Tag layout.
Distal Tags recover all four PIP/DIP dynamic curves and axis quality, but
repeated motion with one unchanged camera/Tag installation cannot by
itself distinguish a fixed parallel-axis phase bias from an encoder zero.
The caller therefore anchors MCP-pitch and PIP to the independently known
full-flexion contact endpoint using their measured command curves. DIP
stays passive and has no independently identifiable static zero.
"""
fingers = ("index", "middle", "ring", "pinky")
direct = (
"thumb_cmc_roll",
@@ -147,7 +156,7 @@ def _build_right_19_zero_profile(
*(
f"{finger}_{suffix}"
for finger in fingers
for suffix in ("mcp_roll", "mcp_pitch")
for suffix in ("mcp_roll", "mcp_pitch", "pip")
),
)
axis_joints = (
@@ -159,7 +168,7 @@ def _build_right_19_zero_profile(
*(
f"{finger}_{suffix}"
for finger in fingers
for suffix in ("mcp_roll", "mcp_pitch", "pip")
for suffix in ("mcp_roll", "mcp_pitch", "pip", "dip")
),
)
axis_parent = {
@@ -176,7 +185,10 @@ def _build_right_19_zero_profile(
**{
f"{finger}_{child}": f"{finger}_{parent}"
for finger in fingers
for child, parent in (("pip", "mcp_pitch"),)
for child, parent in (
("pip", "mcp_pitch"),
("dip", "pip"),
)
},
}
observer = {
@@ -190,6 +202,7 @@ def _build_right_19_zero_profile(
for target, observed in (
("mcp_roll", "mcp_pitch"),
("mcp_pitch", "pip"),
("pip", "dip"),
)
},
}
@@ -206,6 +219,11 @@ def _build_right_19_zero_profile(
# axis to constrain that circle; PIP retains its independently fitted
# axis line and all final SE(3)/holdout quality gates.
| set(RIGHT_19_END_ON_IMAGE_CURVE_JOINTS)
# _fit_axis_measurement_raw constrains every passive DIP direction to
# its source-URDF-parallel upstream PIP. Declare the same fact here
# so validation does not compare that trusted direction with the
# depth-biased free plane of a distal Tag trajectory.
| set(RIGHT_19_VISUALLY_MEASURED_PASSIVE_DIPS)
)
return ZeroCalibrationProfile(
hand=hand,
@@ -218,15 +236,60 @@ def _build_right_19_zero_profile(
axis_parent_joint=axis_parent,
phase_parent_joint=phase_parent,
offset_observer_joint=observer,
# Keep the commit-proven thumb reference convention: the passive-IP
# observation is sufficiently good for the CMC/MCP trajectory, but
# not for redefining the MCP CAD zero. Optimising this value made the
# whole downstream thumb chain move during otherwise good CMC fits.
# thumb_mcp retains the commit-proven CAD reference. Four-finger
# flexion offsets are supplied per session from the measured endpoint,
# not stored as constants in this profile.
fixed_direct_zero_offsets_rad={"thumb_mcp": 0.0},
static_output_zero_offsets_rad={},
)
RIGHT_19_FLEXION_ENDPOINT_JOINTS = frozenset(
f"{finger}_{suffix}"
for finger in ("index", "middle", "ring", "pinky")
for suffix in ("mcp_pitch", "pip")
)
def derive_right_19_flexion_endpoint_offsets(
source_urdf: str | Path,
curves: Mapping[str, JointCurveFit],
*,
maximum_offset_rad: float = math.radians(5.0),
) -> dict[str, float]:
"""Estimate flexion origins from the measured full-flexion contact.
G20 feedback 0 is the independently verified palm-contact endpoint. The
source CAD upper angle describes that same physical endpoint. Therefore
``origin_offset + measured_endpoint == CAD_upper``. This makes every
returned value session-derived while avoiding the fixed planar-PnP phase
bias that affected the parallel MCP/PIP/DIP axes.
"""
if not math.isfinite(maximum_offset_rad) or maximum_offset_rad <= 0.0:
raise ValueError("flexion endpoint maximum offset must be positive")
root = ET.parse(Path(source_urdf).expanduser().resolve()).getroot()
joints = {str(node.get("name")): node for node in root.findall("joint")}
result: dict[str, float] = {}
for name in sorted(RIGHT_19_FLEXION_ENDPOINT_JOINTS):
fit = curves.get(name)
joint = joints.get(name)
limit = None if joint is None else joint.find("limit")
if fit is None:
raise ValueError(f"missing measured endpoint curve for {name}")
if limit is None or limit.get("upper") is None:
raise ValueError(f"source URDF joint {name} has no upper limit")
upper = float(limit.get("upper"))
measured_endpoint = float(np.max(np.asarray(fit.angle_rad, dtype=float)))
offset = upper - measured_endpoint
if not math.isfinite(offset) or abs(offset) > maximum_offset_rad:
raise ValueError(
f"{name} endpoint-derived zero offset is outside the safe range: "
f"{math.degrees(offset):.3f}deg"
)
result[name] = offset
return result
def get_zero_calibration_profile(
side: str, layout_id: str = "legacy_11"
) -> ZeroCalibrationProfile:
@@ -2398,10 +2461,13 @@ def solve_urdf_zero_offsets(
failures[offset_joint] = (
"zero_phase_axis_line_angle_uncertainty_too_large"
)
elif any(
item.pose_axis_line_rms_m
> maximum_pose_axis_line_rms_m
for item in phase_items
elif (
profile.hand.layout_id != G20_RIGHT_19_LAYOUT
and any(
item.pose_axis_line_rms_m
> maximum_pose_axis_line_rms_m
for item in phase_items
)
):
failures[offset_joint] = (
"zero_phase_axis_line_residual_too_large"
@@ -2548,6 +2614,7 @@ def solve_urdf_zero_offsets(
improvement_confidence_lower[offset_joint] = lower
if lower <= 0.0:
improvement_passed = False
validation_errors = np.asarray(
list(validation_error_by_joint.values()), dtype=float
)
@@ -2634,14 +2701,20 @@ def solve_urdf_zero_offsets(
> maximum_offset_rad + math.radians(0.01)
):
configured_limit_exceeded.append("finger_mcp_roll_common_mode")
diagnostic_bound_hits = [
name
for name, limit in zip(
profile.direct_zero_joints, diagnostic_offset_limits
)
if name not in fixed_offsets
and abs(training_offsets[name]) >= limit - math.radians(0.01)
]
diagnostic_bound_hits: list[str] = []
for name, limit in zip(
profile.direct_zero_joints, diagnostic_offset_limits
):
if name in fixed_offsets:
continue
checked_offset = training_offsets[name]
if name in product_finger_rolls:
# Match the configured-limit and publication convention above.
# The raw common roll is a fitted-palm-frame gauge; only the
# finger-to-finger deviation is a physical zero correction.
checked_offset -= finger_roll_common_mode
if abs(checked_offset) >= limit - math.radians(0.01):
diagnostic_bound_hits.append(name)
failure_reasons: dict[str, str] = {}
if (
profile.hand.layout_id == G20_RIGHT_19_LAYOUT
@@ -2859,6 +2932,7 @@ def write_zero_corrected_urdf(
output_directory: str | Path,
serial_number: str,
offsets_rad: Mapping[str, float],
endpoint_anchored_offsets_rad: Mapping[str, float] | None = None,
timestamp: str | None = None,
) -> Path:
source = Path(source_urdf).expanduser().resolve()
@@ -2875,6 +2949,12 @@ def write_zero_corrected_urdf(
if not offsets_rad:
raise ValueError("offsets_rad must contain at least one joint")
offsets = {str(name): float(value) for name, value in offsets_rad.items()}
endpoint_offsets = {
str(name): float(value)
for name, value in dict(endpoint_anchored_offsets_rad or {}).items()
}
if not set(endpoint_offsets) <= set(offsets):
raise ValueError("endpoint-anchored offsets must be URDF zero targets")
if any(
not math.isfinite(value) or abs(value) > math.radians(90.0)
for value in offsets.values()
@@ -2897,12 +2977,30 @@ def write_zero_corrected_urdf(
root = tree.getroot()
original_text = source.read_text(encoding="utf-8")
replacement_rpy: dict[str, str] = {}
replacement_upper: dict[str, str] = {}
replacement_mimic_offset: dict[str, str] = {}
found: set[str] = set()
for joint in root.findall("joint"):
name = str(joint.get("name"))
if name not in offsets:
mimic = joint.find("mimic")
source_joint = None if mimic is None else str(mimic.get("joint"))
if source_joint in endpoint_offsets:
multiplier = float(mimic.get("multiplier", "1"))
old_offset = float(mimic.get("offset", "0"))
calibrated_mimic_offset = (
old_offset + multiplier * endpoint_offsets[source_joint]
)
replacement_mimic_offset[name] = (
f"{calibrated_mimic_offset:.15g}"
)
continue
found.add(name)
if name in endpoint_offsets:
limit = joint.find("limit")
if limit is None or limit.get("upper") is None:
raise ValueError(f"joint {name} has no upper limit")
replacement_upper[name] = f"{float(limit.get('upper')) - endpoint_offsets[name]:.15g}"
# A statistically insignificant correction is represented as exact
# zero. Preserve that joint's source text byte-for-byte instead of
# serialising an equivalent Euler triplet.
@@ -2944,19 +3042,53 @@ def write_zero_corrected_urdf(
for match in joint_pattern.finditer(original_text):
name = match.group("name")
if name not in replacement_rpy:
continue
if name not in replacement_upper and name not in replacement_mimic_offset:
continue
block = match.group(0)
origin_match = re.search(
r"<origin\b[^>]*\brpy\s*=\s*([\"'])(?P<rpy>[^\"']*)\1",
block,
re.DOTALL,
)
if origin_match is None:
raise ValueError(f"joint {name} origin has no rpy attribute")
start = match.start() + origin_match.start("rpy")
end = match.start() + origin_match.end("rpy")
edits.append((start, end, replacement_rpy[name]))
expected_edit_count = len(replacement_rpy)
if name in replacement_rpy:
origin_match = re.search(
r"<origin\b[^>]*\brpy\s*=\s*([\"'])(?P<rpy>[^\"']*)\1",
block,
re.DOTALL,
)
if origin_match is None:
raise ValueError(f"joint {name} origin has no rpy attribute")
edits.append((
match.start() + origin_match.start("rpy"),
match.start() + origin_match.end("rpy"),
replacement_rpy[name],
))
if name in replacement_upper:
limit_match = re.search(
r"<limit\b[^>]*\bupper\s*=\s*([\"'])(?P<upper>[^\"']*)\1",
block,
re.DOTALL,
)
if limit_match is None:
raise ValueError(f"joint {name} limit has no upper attribute")
edits.append((
match.start() + limit_match.start("upper"),
match.start() + limit_match.end("upper"),
replacement_upper[name],
))
if name in replacement_mimic_offset:
mimic_match = re.search(
r"<mimic\b[^>]*\boffset\s*=\s*([\"'])(?P<offset>[^\"']*)\1",
block,
re.DOTALL,
)
if mimic_match is None:
raise ValueError(f"joint {name} mimic has no offset attribute")
edits.append((
match.start() + mimic_match.start("offset"),
match.start() + mimic_match.end("offset"),
replacement_mimic_offset[name],
))
expected_edit_count = (
len(replacement_rpy)
+ len(replacement_upper)
+ len(replacement_mimic_offset)
)
if len(edits) != expected_edit_count:
raise ValueError("could not locate every target joint field in source URDF text")
corrected_text = original_text
@@ -43,10 +43,12 @@ def _launch_stack(context):
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")
if tag_layout not in {"legacy_11", "g20_right_15", "g20_right_19"}:
raise RuntimeError(
"tag_layout must be legacy_11, g20_right_15 or g20_right_19"
)
if tag_layout in {"g20_right_15", "g20_right_19"} and hand_type != "right":
raise RuntimeError("G20 right product layout requires hand_type:=right")
requested_tag_config = LaunchConfiguration("tag_config").perform(context)
package_share = Path(
get_package_share_directory("g20_thumb_apriltag_calibration")
@@ -57,7 +59,9 @@ def _launch_stack(context):
else package_share
/ "config"
/ (
"three_camera_tags_g20_right_15.yaml"
"three_camera_tags_g20_right_19.yaml"
if tag_layout == "g20_right_19"
else "three_camera_tags_g20_right_15.yaml"
if tag_layout == "g20_right_15"
else "three_camera_tags.yaml"
)
@@ -78,10 +82,10 @@ def _launch_stack(context):
expected_source_hash = LaunchConfiguration(
"source_urdf_expected_sha256"
).perform(context).strip().lower()
if tag_layout == "g20_right_15":
if tag_layout in {"g20_right_15", "g20_right_19"}:
if re.fullmatch(r"[0-9a-f]{64}", expected_source_hash) is None:
raise RuntimeError(
"g20_right_15 requires source_urdf_expected_sha256 confirmed "
"G20 right product layout requires source_urdf_expected_sha256 confirmed "
"by the CAD/hardware owner"
)
actual_source_hash = hashlib.sha256(source_urdf.read_bytes()).hexdigest()
@@ -161,7 +161,7 @@ def test_right_19_schema_v4_mapper_uses_requested_command_midpoint_curve() -> No
mapped = dict(zip(G20_URDF_JOINT_NAMES, mapper.map_positions(command)))
assert mapper.input_domain == "command_u8"
assert mapper.layout_id == "g20_right_15"
assert mapper.layout_id == "g20_right_19"
# 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
@@ -80,17 +80,17 @@ 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:
def test_right_19_tag_config_matches_the_physical_layout() -> None:
tags = yaml.safe_load(
(
PACKAGE_ROOT
/ "config"
/ "three_camera_tags_g20_right_15.yaml"
/ "three_camera_tags_g20_right_19.yaml"
).read_text()
)
expected = {
"front": [0, 1, 2, 3, 10, 11, 12, 13],
"side": [4, 5, 6, 15, 17],
"side": [4, 5, 6, 7, 14, 15, 16, 17, 18],
"top": [8, 9],
}
all_ids: set[int] = set()
@@ -99,14 +99,16 @@ def test_right_15_tag_config_matches_the_physical_layout() -> None:
f"/g20_calibration/{view}/apriltag/apriltag"
]["ros__parameters"]
assert parameters["tag"]["ids"] == ids
assert parameters["tag"]["sizes"] == [0.016] * len(ids)
expected_sizes = [0.016 for _tag_id in ids]
assert parameters["tag"]["sizes"] == expected_sizes
all_ids.update(ids)
assert all_ids == set(range(19)) - {7, 14, 16, 18}
assert all_ids == set(range(19))
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",
"side_base", "ring_pip", "pinky_pip", "pinky_dip", "ring_dip",
"middle_pip", "middle_dip", "index_pip", "index_dip",
]
@@ -117,6 +119,8 @@ def test_three_camera_calibration_has_hard_preflight_and_three_rounds() -> None:
parameters = config["g20_calibration"]["ros__parameters"]
assert parameters["tag_size_m"] == 0.016
assert parameters["tag_size_override_ids"] == [7, 14, 16, 18]
assert parameters["tag_size_overrides_m"] == [0.016] * 4
assert parameters["baseline_command_u8"] == [
255,
@@ -202,6 +206,7 @@ def test_three_camera_calibration_has_hard_preflight_and_three_rounds() -> None:
assert parameters["zero_maximum_observability_condition_number"] >= 1.0
assert parameters["zero_maximum_offset_deg"] <= 20.0
assert parameters["zero_finger_maximum_offset_deg"] <= 3.0
assert parameters["flexion_endpoint_maximum_offset_deg"] == 5.0
assert parameters["image_trajectory_maximum_radial_rms_px"] <= 2.0
assert parameters["image_trajectory_maximum_radial_p95_px"] <= 3.5
assert parameters["image_trajectory_minimum_radius_px"] >= 20.0
@@ -63,17 +63,17 @@ def test_runtime_fit_saturates_at_source_urdf_limit() -> None:
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")
def test_right_19_profile_has_exact_layout_tasks_and_measured_dips() -> None:
profile = get_hand_calibration_profile("right", "g20_right_19")
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 len(profile.measured_joints) == 21
assert len(profile.record_joints) == 25
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
4, 5, 6, 7, 14, 15, 16, 17, 18
]
assert list(profile.view_tags["top"].values()) == [8, 9]
assert profile.preflight_view_roles == {
@@ -98,23 +98,30 @@ def test_right_15_profile_has_exact_layout_tasks_and_derived_dips() -> None:
"pinky_mcp_roll",
"pinky_mcp_roll_side",
)
assert profile.sweep_specs[6].joints == ("pinky_pip", "pinky_dip")
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:
def test_right_19_keeps_four_visual_dip_curves_independent() -> 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}
dip_values = tuple(0.0013 * (255 - command) for command in range(256))
for finger in ("index", "middle", "ring", "pinky"):
measured[f"{finger}_dip"] = JointCurveFit(
dip_values,
dip_values,
dip_values,
{"visual_measurement": True},
0.0,
0.007,
{"visual_measurement": 1.0},
)
source_urdf = (
Path(__file__).resolve().parents[3]
/ "src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/"
@@ -127,14 +134,10 @@ def test_right_15_derives_four_dip_curves_from_source_urdf_mimic() -> None:
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
assert dip.angle_rad == pytest.approx(dip_values)
assert dip.maximum_hysteresis_rad == pytest.approx(0.007)
assert dip.circle["visual_measurement"] is True
def _changed_motors(
@@ -878,13 +881,13 @@ def test_right_compact_payload_keeps_v4_shape_and_uses_pinky_sources() -> None:
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:
def test_right_19_schema_v4_records_visual_dips_and_16_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")
zero = get_zero_calibration_profile("right", "g20_right_19")
baseline = [255] * 20
baseline[6:10] = [127] * 4
fits = {}
@@ -920,7 +923,7 @@ def test_right_15_schema_v4_records_derived_dip_curves_and_12_static_zeros() ->
passed=True,
baseline=baseline,
side="right",
layout_id="g20_right_15",
layout_id="g20_right_19",
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
@@ -950,7 +953,12 @@ def test_right_15_schema_v4_records_derived_dip_curves_and_12_static_zeros() ->
"schema_version", "model", "side", "serial_number", "angle_unit",
"command_range", "baseline_command_u8", "joints", "quality",
}
assert len(zero.direct_zero_joints) == 12
assert len(zero.direct_zero_joints) == 16
assert all(
f"{finger}_pip" in zero.direct_zero_joints
and f"{finger}_dip" not in zero.direct_zero_joints
for finger in ("index", "middle", "ring", "pinky")
)
assert payload["joints"]["thumb_mcp"]["zero_angles"] == {
"urdf_zero_offset_rad": 0.0
}
@@ -29,6 +29,8 @@ from g20_thumb_apriltag_calibration.operator_report import (
)
from g20_thumb_apriltag_calibration.one_command import (
_automatic_resume_candidate,
_calibration_node_exited_before_status,
_status_timeout_seconds,
)
from g20_thumb_apriltag_calibration.product import load_product_config
from g20_thumb_apriltag_calibration.publication import (
@@ -53,9 +55,11 @@ from g20_thumb_apriltag_calibration.three_camera_node import (
_steady_checkpoint_commands,
_unresolved_fit_failure_tasks,
combination_target_coverage,
resume_tasks_invalidated_by_tag_size_changes,
resumable_completed_task_prefix,
)
from g20_thumb_apriltag_calibration.urdf_zero import (
RIGHT_19_FLEXION_ENDPOINT_JOINTS,
UrdfKinematicModel,
get_zero_calibration_profile,
write_zero_corrected_urdf,
@@ -117,6 +121,9 @@ def _make_passed_session(config, stamp: str) -> Path:
output_directory=session,
serial_number=config.serial_number,
offsets_rad={name: 0.01 for name in ACTIVE_ZERO_JOINTS},
endpoint_anchored_offsets_rad={
name: 0.01 for name in RIGHT_19_FLEXION_ENDPOINT_JOINTS
},
timestamp=stamp,
)
(session / "raw_samples.jsonl").write_text('{"kind":"session_start"}\n')
@@ -299,6 +306,28 @@ def test_sparse_resume_keeps_complete_tasks_after_failed_task() -> None:
}
def test_corrected_distal_tag_sizes_invalidate_only_four_pip_tasks() -> None:
profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT)
previous = {
tag_id: (0.010 if tag_id in {7, 14, 16, 18} else 0.016)
for tag_id in range(19)
}
current = {tag_id: 0.016 for tag_id in range(19)}
changed, invalidated = resume_tasks_invalidated_by_tag_size_changes(
profile, previous, current
)
assert changed == (7, 14, 16, 18)
assert invalidated == (
"pinky_pip_side",
"ring_pip_side",
"middle_pip_side",
"index_pip_side",
)
assert len(profile.sweep_specs) - len(invalidated) == 12
def test_sparse_resume_revalidates_retired_product_dynamic_hysteresis() -> None:
profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT)
task = next(
@@ -523,6 +552,44 @@ def test_resume_rejects_unresolved_fit_but_accepts_retired_model_metric() -> Non
assert _unresolved_fit_failure_tasks(profile, retired_rows) == set()
def test_resume_revalidates_retired_passive_dip_position_metrics() -> None:
profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT)
pip = next(
spec for spec in profile.sweep_specs if spec.key == "pinky_pip_side"
)
rows = [
{
"kind": "sample",
"task_name": pip.key,
"attempt": 3,
},
{
"kind": "fit_failure",
"task_name": pip.key,
"view": pip.view,
"motor_index": pip.motor_index,
"joints": list(pip.joints),
"attempt": 3,
"failures": [
{
"joint": "pinky_dip",
"metric": "axis_pose_line_rms_mm",
"actual": 1.9,
"limit": 1.0,
},
{
"joint": "pinky_dip",
"metric": "rotation_circle_axis_difference_deg",
"actual": 15.5,
"limit": 1.0,
},
],
},
]
assert _unresolved_fit_failure_tasks(profile, rows) == set()
def test_automatic_resume_requires_failed_matching_geometry(tmp_path: Path) -> None:
config = _config(tmp_path)
session = config.session_root / "20260819_150000"
@@ -926,6 +993,34 @@ def test_synchronised_timeout_with_visible_tags_reports_pnp_geometry(tmp_path: P
assert "不要调整" in payload["automatic_action_zh"]
def test_sweep_start_timeout_with_visible_tags_reports_pnp_geometry(tmp_path: Path) -> None:
config = _config(tmp_path, passes=1)
session = config.session_root / "20260821_154105"
session.mkdir(parents=True)
status = {
"state": "PAUSED",
"reason": "sweep_start_tag_timeout",
"feedback_hz": 58.8,
"active": {"valid_frames": 0},
"views": {
"front": {"ready": True, "missing_tag_ids": []},
"side": {
"ready": True,
"missing_tag_ids": [],
"group_pnp_reason": "group_normal_alignment",
},
"top": {"ready": True, "missing_tag_ids": []},
},
}
payload, _block = build_failure_report(
config, session, status, reason=status["reason"]
)
assert payload["error_code"] == "CAM-GEOMETRY-201"
assert "PnP" in payload["problem_zh"]
def test_multiview_sample_failure_has_stable_observation_code() -> None:
code, problem, _suggestion = classify_error(
"sweep_bins_too_few:pinky_mcp_roll_side", {}
@@ -935,6 +1030,69 @@ def test_multiview_sample_failure_has_stable_observation_code() -> None:
assert "轨迹不完整" in problem
def test_fitting_uses_long_watchdog_without_weakening_motion_timeout() -> None:
assert _status_timeout_seconds({"state": "FITTING"}) == 600.0
assert _status_timeout_seconds({"state": "SWEEP"}) == 90.0
assert _status_timeout_seconds({"state": "RETURN_BASELINE"}) == 90.0
def test_fitting_announces_state_before_synchronous_model_work() -> None:
spec = SimpleNamespace()
published = []
node = SimpleNamespace(
profile=SimpleNamespace(sweep_specs=(spec,)),
last_status_publish=0.0,
_publish_status=lambda now: published.append(
(node.state, node.reason, now)
),
_provisional_fit_failures=lambda _spec: [{"metric": "test_failure"}],
_pause_for_provisional_fit_failure=lambda *_args, **_kwargs: None,
)
G20ThreeCameraCalibrationNode._fit_all_curves(node)
assert len(published) == 1
assert published[0][0] == "FITTING"
assert published[0][1] == "fitting_3d_axes_and_urdf_zero_offsets"
assert node.last_status_publish == published[0][2]
def test_stopped_status_heartbeat_has_stable_communication_code() -> None:
code, problem, suggestion = classify_error(
"MOTION-COMM-303:calibration status stopped", {}
)
assert code == "MOTION-COMM-303"
assert "状态心跳" in problem
assert "断点" in suggestion
def test_missing_startup_status_has_specific_camera_status_code() -> None:
code, problem, suggestion = classify_error(
"CAM-STATUS-202:no calibration status received", {}
)
assert code == "CAM-STATUS-202"
assert "未发布状态" in problem
assert "尚未开始运动" in suggestion
def test_startup_watchdog_detects_only_calibration_child_exit(
tmp_path: Path,
) -> None:
log = tmp_path / "calibration.log"
log.write_text(
"[three_camera_calibration_node-6]: process has died "
"[pid 123, exit code 1]\n",
encoding="utf-8",
)
assert _calibration_node_exited_before_status(log) is True
log.write_text(
"[component_container_mt-4]: process has died [exit code -11]\n",
encoding="utf-8",
)
assert _calibration_node_exited_before_status(log) is False
def test_progress_contains_stage_eta_tags_cameras_and_feedback() -> None:
@@ -85,6 +85,22 @@ def test_combination_prediction_failure_is_not_reported_as_unclassified() -> Non
assert "不要重新采集16个单关节任务" in action
def test_sweep_start_visible_tags_reports_group_pnp_rejection() -> None:
explanation, action = three_camera_reason_zh(
"PAUSED",
"sweep_start_tag_timeout",
{
"group_pnp_reasons": {
"side": "group_normal_alignment",
}
},
)
assert "所需Tag也可见" in explanation
assert "group_normal_alignment" in explanation
assert "不要重新粘贴" in action
def test_multiview_failure_names_the_camera_specific_joint() -> None:
explanation, action = three_camera_reason_zh(
"PAUSED",
@@ -16,6 +16,7 @@ from g20_thumb_apriltag_calibration.acquisition import TagQuality
from g20_thumb_apriltag_calibration.full_hand import (
RIGHT_19_END_ON_IMAGE_CURVE_JOINTS,
RIGHT_19_HAND_PROFILE,
RIGHT_19_VISUALLY_MEASURED_PASSIVE_DIPS,
RIGHT_HAND_PROFILE,
SWEEP_SPECS,
THREE_CAMERA_BASELINE_COMMAND,
@@ -33,6 +34,7 @@ from g20_thumb_apriltag_calibration.three_camera_node import (
_combination_observable_joints,
_fit_failure_is_systematic,
_frames_cover_sweep_joints,
_isolated_axis_cycle_outliers,
_overall_progress,
_selected_pose_qualities,
_sweep_views,
@@ -486,7 +488,7 @@ def test_right_19_combination_allows_occluded_neighbouring_fingers() -> None:
)
assert required == ("side_base",)
assert observable == ("index_pip",)
assert observable == ("index_pip", "index_dip")
def test_right_19_task_requires_only_target_chain_after_clearance_pose() -> None:
@@ -513,7 +515,7 @@ def test_right_19_task_requires_only_target_chain_after_clearance_pose() -> None
node, "side"
)
assert required == ("side_base", "index_pip")
assert required == ("side_base", "index_pip", "index_dip")
def test_right_pip_sweep_keeps_side_base_as_pnp_branch_anchor() -> None:
@@ -1694,6 +1696,35 @@ def test_thumb_mcp_ip_group_uses_mimic_only_as_pnp_branch_prior() -> None:
)
def test_side_pip_group_allows_arbitrary_distal_extension_tag_plane() -> None:
node = SimpleNamespace(
profile=RIGHT_19_HAND_PROFILE,
pnp_maximum_pose_jump_rad=math.radians(35.0),
pnp_maximum_translation_jump_m=0.04,
pnp_group_initialization_frames=8,
pnp_group_normal_alignment_scale_rad=math.radians(5.0),
pnp_group_maximum_normal_alignment_rad=math.radians(15.0),
pnp_tracker_reset_seconds=5.0,
thumb_ip_pnp_coupling_multiplier=1.03,
thumb_ip_pnp_coupling_scale_rad=math.radians(3.0),
thumb_ip_pnp_maximum_coupling_residual_rad=math.radians(7.5),
)
tracker = G20ThreeCameraCalibrationNode._make_group_pose_tracker(
node,
"side",
("side_base", "middle_pip", "middle_dip"),
)
assert tracker.adjacent_pairs == (
("side_base", "middle_pip"),
("middle_pip", "middle_dip"),
)
assert tracker.normal_alignment_pairs == (
("side_base", "middle_pip"),
)
def test_fit_failure_preserves_main_progress_and_selects_retry_scope(tmp_path) -> None:
spec = next(item for item in SWEEP_SPECS if item.motor_index == 6)
node = SimpleNamespace(
@@ -1850,12 +1881,78 @@ def test_resume_discards_only_failed_specs_samples(tmp_path) -> None:
"view": "front",
"motor_index": 15,
"joints": ["thumb_mcp", "thumb_ip"],
"joints_to_rescan": ["thumb_ip", "thumb_mcp"],
"attempt": 2,
"reason": "joint_fit_check_failed",
"cycles": [2],
}
def test_multiview_retry_discards_only_failed_side_measurement(tmp_path) -> None:
spec = next(
item
for item in RIGHT_19_HAND_PROFILE.sweep_specs
if item.key == "middle_roll_multiview"
)
primary, validation = spec.joints
records = {
primary: [
{"cycle": 0, "accepted": "front"},
{"cycle": 1, "accepted": "front"},
],
validation: [
{"cycle": 0, "old": "side"},
{"cycle": 1, "accepted": "side"},
],
}
side_runtime = SimpleNamespace(
pnp_invalid_since=12.0,
pnp_reset_count=0,
task_valid_frames=4927,
task_total_frames=5212,
)
node = SimpleNamespace(
profile=RIGHT_19_HAND_PROFILE,
retry_sweep_spec=spec,
retry_joint_names={validation},
records_by_joint=records,
baseline_records_by_joint={primary: [], validation: []},
command_records_by_joint={primary: [], validation: []},
sweep_attempts={spec.key: 1},
raw_path=tmp_path / "raw_samples.jsonl",
paused_reason="joint_fit_check_failed",
fit_failure={"failures": ["old"]},
retry_cycles={0},
repetitions=4,
sweep_items=[
SweepItem(spec, cycle, direction)
for cycle in range(4)
for direction in (
DIRECTION_DECREASING,
DIRECTION_INCREASING,
)
],
views={"side": side_runtime},
_reset_view_trackers=lambda runtime: None,
)
G20ThreeCameraCalibrationNode._prepare_failed_sweep_retry(node)
assert records[primary] == [
{"cycle": 0, "accepted": "front"},
{"cycle": 1, "accepted": "front"},
]
assert records[validation] == [
{"cycle": 1, "accepted": "side"}
]
assert node.retry_joint_names == {validation}
assert side_runtime.task_valid_frames == 0
assert side_runtime.task_total_frames == 0
event = json.loads((tmp_path / "raw_samples.jsonl").read_text())
assert event["joints_to_rescan"] == [validation]
assert event["cycles"] == [1]
def test_recoverable_sweep_failure_retries_three_times_before_pause(tmp_path) -> None:
spec = next(item for item in SWEEP_SPECS if item.motor_index == 6)
item = SweepItem(spec, 0, DIRECTION_INCREASING)
@@ -2239,6 +2336,52 @@ def test_provisional_fit_accepts_constrained_circle_for_image_joint() -> None:
]
def test_axis_cycle_spread_localizes_one_outlier_for_four_rounds() -> None:
axes = [
np.asarray([-0.05713560, -0.06623617, -0.99616680]),
np.asarray([-0.05620951, -0.05515776, -0.99689423]),
np.asarray([-0.05700579, -0.05346630, -0.99694117]),
np.asarray([-0.05589426, -0.05231845, -0.99706500]),
]
assert _isolated_axis_cycle_outliers(
axes, math.radians(0.75)
) == {0}
def test_provisional_axis_spread_marks_only_the_isolated_round() -> None:
spec = next(item for item in SWEEP_SPECS if item.motor_index == 0)
records = _image_cycle_records([math.radians(47.0)] * 4)
node = _fit_check_node({"thumb_cmc_pitch": records})
node.repetitions = 4
axes = [
[-0.05713560, -0.06623617, -0.99616680],
[-0.05620951, -0.05515776, -0.99689423],
[-0.05700579, -0.05346630, -0.99694117],
[-0.05589426, -0.05231845, -0.99706500],
]
node._fit_axis_measurement = lambda name, cycle: SimpleNamespace(
axis_common_xyz=axes[cycle],
axis_direction_source="pose_rotation_3d",
radial_rms_m=0.0001,
pose_axis_line_rms_m=0.0001,
plane_rms_m=0.0001,
rotation_circle_axis_difference_rad=0.0,
)
failures = G20ThreeCameraCalibrationNode._provisional_fit_failures(
node, spec, include_view_validity=False
)
spread = next(
failure
for failure in failures
if failure["metric"] == "axis_cycle_difference_deg"
)
assert spread["cycle"] == 1
assert spread["inlier_cycles"] == [2, 3, 4]
def test_side_roll_alias_does_not_reject_free_circle_axis_disagreement() -> None:
spec = next(
item
@@ -2258,6 +2401,14 @@ def test_side_roll_alias_does_not_reject_free_circle_axis_disagreement() -> None
)
node.baseline_command = list(THREE_CAMERA_BASELINE_COMMAND)
node.baseline_records_by_joint = {name: records}
node.minimum_detection_rate = 0.95
node.views = {
"side": SimpleNamespace(
valid_rate=0.9453,
task_valid_frames=2653,
task_total_frames=2807,
)
}
failures = G20ThreeCameraCalibrationNode._provisional_fit_failures(
node, spec
@@ -2270,6 +2421,7 @@ def test_side_roll_alias_does_not_reject_free_circle_axis_disagreement() -> None
in {
"rotation_circle_axis_difference_deg",
"axis_plane_rms_mm",
"tag_valid_rate_percent",
}
]
@@ -2674,6 +2826,60 @@ def test_side_alias_skips_pose_line_rms_gate(tmp_path) -> None:
]
def test_right_19_passive_dip_requires_axis_point_for_pip_zero_phase() -> None:
records = _image_cycle_records([math.radians(47.0)] * 3)
node = _fit_check_node(
{"pinky_pip": records, "pinky_dip": records}
)
node.profile = RIGHT_19_HAND_PROFILE
node.zero_profile = get_zero_calibration_profile(
"right", "g20_right_19"
)
node.baseline_command = list(THREE_CAMERA_BASELINE_COMMAND)
node.baseline_records_by_joint = {
"pinky_pip": records,
"pinky_dip": records,
}
def axis_measurement(name, cycle):
is_dip = name in RIGHT_19_VISUALLY_MEASURED_PASSIVE_DIPS
return SimpleNamespace(
axis_common_xyz=(0.0, 0.0, 1.0),
axis_direction_source="upstream_constraint",
radial_rms_m=0.0002,
pose_axis_line_rms_m=0.002 if is_dip else 0.0002,
plane_rms_m=0.006 if is_dip else 0.0002,
rotation_circle_axis_difference_rad=(
math.radians(16.0) if is_dip else math.radians(0.1)
),
)
node._fit_axis_measurement = axis_measurement
spec = next(
item
for item in RIGHT_19_HAND_PROFILE.sweep_specs
if item.key == "pinky_pip_side"
)
failures = G20ThreeCameraCalibrationNode._provisional_fit_failures(
node, spec, include_view_validity=False
)
pose_failures = [
failure
for failure in failures
if failure["joint"] == "pinky_dip"
and failure["metric"] == "axis_pose_line_rms_mm"
]
assert len(pose_failures) == 3
assert not [
failure
for failure in failures
if failure["joint"] == "pinky_dip"
and failure["metric"]
in {"axis_plane_rms_mm", "rotation_circle_axis_difference_deg"}
]
def _ring_group_end_state() -> list[float]:
"""Ring-group avoidance pose: pinky flexed, index/middle rolls parked."""
state = [float(value) for value in THREE_CAMERA_BASELINE_COMMAND]
@@ -20,6 +20,7 @@ from g20_thumb_apriltag_calibration.urdf_zero import (
_angles_from_state,
_zero_sensitive_axis_error_rad,
baseline_hysteresis_by_cycle_rad,
derive_right_19_flexion_endpoint_offsets,
fit_joint_axis_measurement,
fit_rotation_joint_curve,
solve_urdf_zero_offsets,
@@ -31,6 +32,7 @@ from g20_thumb_apriltag_calibration.full_hand import (
JOINT_SPECS,
MEASURED_JOINTS,
PASSIVE_JOINTS,
RIGHT_19_VISUALLY_MEASURED_PASSIVE_DIPS,
JointCurveFit,
get_hand_calibration_profile,
)
@@ -621,7 +623,7 @@ 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] = []
validation_cycle = 3 if layout_id == "g20_right_15" else 2
validation_cycle = 3 if hand.layout_id == "g20_right_19" else 2
training_cycles = tuple(range(validation_cycle))
for cycle in range(validation_cycle + 1):
for joint in zero.axis_joints:
@@ -749,20 +751,20 @@ def _solve_synthetic_offsets(
return zero, result
def test_right_15_solver_recovers_11_targets_and_keeps_thumb_mcp_cad_zero() -> None:
def test_right_19_solver_recovers_visual_targets_when_no_endpoint_anchor_is_supplied() -> 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,
1.0, -1.0, 0.7,
0.8, -0.7, -0.8,
-0.5, 0.6, 0.9,
1.1, -1.0, -0.6,
]
zero, result = _solve_synthetic_offsets(
"right", injected, layout_id="g20_right_15"
"right", injected, layout_id="g20_right_19"
)
assert len(zero.direct_zero_joints) == 12
assert len(zero.axis_joints) == 17
assert len(zero.direct_zero_joints) == 16
assert len(zero.axis_joints) == 21
assert result.passed is True
finger_rolls = tuple(
name
@@ -778,8 +780,8 @@ def test_right_15_solver_recovers_11_targets_and_keeps_thumb_mcp_cad_zero() -> N
)
)
for name, expected in zip(zero.direct_zero_joints, injected):
if name == "thumb_mcp":
expected = 0.0
if name in zero.fixed_direct_zero_offsets_rad:
expected = zero.fixed_direct_zero_offsets_rad[name]
elif name in finger_rolls:
expected -= roll_common
assert math.degrees(result.direct_offsets_rad[name]) == pytest.approx(
@@ -788,35 +790,63 @@ def test_right_15_solver_recovers_11_targets_and_keeps_thumb_mcp_cad_zero() -> N
assert math.degrees(result.all_active_offsets_rad["thumb_mcp"]) == pytest.approx(
0.0, abs=0.05
)
assert zero.fixed_direct_zero_offsets_rad == {"thumb_mcp": 0.0}
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 result.observability_parameter_count == 22
assert result.observability_rank == 22
assert math.isfinite(result.observability_condition_number)
assert set(result.offset_covariance_rad2) == set(zero.direct_zero_joints)
def test_right_19_flexion_zero_is_derived_from_measured_contact_endpoint() -> None:
hand = get_hand_calibration_profile("right", "g20_right_19")
curves = {
name: _synthetic_curve(255, math.radians(50.0))
for name in hand.measured_joints
}
for finger in ("index", "middle", "ring", "pinky"):
curves[f"{finger}_mcp_pitch"] = _synthetic_curve(
255, math.radians(71.0)
)
curves[f"{finger}_pip"] = _synthetic_curve(
255, math.radians(103.0)
)
offsets = derive_right_19_flexion_endpoint_offsets(
RIGHT_SOURCE_URDF, curves
)
assert set(offsets) == {
f"{finger}_{suffix}"
for finger in ("index", "middle", "ring", "pinky")
for suffix in ("mcp_pitch", "pip")
}
assert math.degrees(offsets["index_mcp_pitch"]) == pytest.approx(
math.degrees(1.22) - 71.0
)
assert math.degrees(offsets["index_pip"]) == pytest.approx(
math.degrees(1.75) - 103.0
)
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,
6.0, -1.0, 0.7,
6.4, -0.7, -0.8,
5.7, 0.6, 0.9,
6.2, -1.0, -0.6,
]
zero, result = _solve_synthetic_offsets(
"right", injected, layout_id="g20_right_15"
"right", injected, layout_id="g20_right_19"
)
assert result.passed is True
@@ -834,8 +864,8 @@ def test_right_15_finger_roll_limit_applies_to_independent_deviation() -> None:
)
)
for name, expected in zip(zero.direct_zero_joints, injected):
if name == "thumb_mcp":
expected = 0.0
if name in zero.fixed_direct_zero_offsets_rad:
expected = zero.fixed_direct_zero_offsets_rad[name]
elif name in finger_rolls:
expected -= roll_common
assert math.degrees(result.direct_offsets_rad[name]) == pytest.approx(
@@ -846,18 +876,46 @@ def test_right_15_finger_roll_limit_applies_to_independent_deviation() -> None:
) == pytest.approx(0.0, abs=1.0e-10)
def test_right_19_roll_gauge_at_search_bound_uses_relative_deviation() -> None:
# One raw roll reaches the diagnostic search edge, as in session 160326,
# while all four physical deviations from their shared datum remain safe.
injected = [
2.0, -3.0, 4.0, -1.5,
9.3, -1.0, 0.0,
8.4, -0.7, 0.0,
8.7, 0.6, 0.0,
8.8, -1.0, 0.0,
]
zero, result = _solve_synthetic_offsets(
"right", injected, layout_id="g20_right_19"
)
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_")
)
assert max(
abs(math.degrees(result.direct_offsets_rad[name]))
for name in finger_rolls
) < 3.0
assert not result.failure_reasons
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,
1.0, -1.0, 0.7,
0.8, -0.7, -0.8,
-0.5, 0.6, 0.9,
1.1, -1.0, -0.6,
]
zero, result = _solve_synthetic_offsets(
"right",
injected,
layout_id="g20_right_15",
layout_id="g20_right_19",
validation_offset_bias_degrees={"thumb_cmc_roll": 2.0},
)
@@ -867,9 +925,9 @@ def test_right_19_holdout_never_changes_frozen_training_offsets() -> None:
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")
def test_right_19_palm_pose_and_16_zero_observation_jacobian_is_full_rank() -> None:
"""Guard the reviewed 6-palm-DOF plus 16-static-zero observability."""
zero = get_zero_calibration_profile("right", "g20_right_19")
model = UrdfKinematicModel(RIGHT_SOURCE_URDF)
parameter_count = 6 + len(zero.direct_zero_joints)
@@ -914,14 +972,14 @@ def test_right_15_palm_pose_and_12_zero_observation_jacobian_is_full_rank() -> N
]
)
assert parameter_count == 18
assert parameter_count == 22
assert np.linalg.matrix_rank(jacobian, tol=1.0e-7) == parameter_count
def test_right_15_urdf_writer_changes_only_the_12_static_targets(
def test_right_19_urdf_writer_changes_only_the_16_static_targets(
tmp_path: Path,
) -> None:
zero = get_zero_calibration_profile("right", "g20_right_15")
zero = get_zero_calibration_profile("right", "g20_right_19")
offsets = {name: math.radians(1.0) for name in zero.direct_zero_joints}
destination = write_zero_corrected_urdf(
source_urdf=RIGHT_SOURCE_URDF,
@@ -957,7 +1015,7 @@ def test_right_15_urdf_writer_changes_only_the_12_static_targets(
assert changed == set(zero.direct_zero_joints)
assert "thumb_mcp" in changed
assert not set(get_hand_calibration_profile(
"right", "g20_right_15"
"right", "g20_right_19"
).passive_joints) & changed