G20右手四指独立标定(少thumb_mcp)
This commit is contained in:
@@ -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。
|
||||
当前 30~38 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
|
||||
|
||||
+82
-19
@@ -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)
|
||||
|
||||
+17
-1
@@ -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,
|
||||
|
||||
+14
-1
@@ -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": "°",
|
||||
|
||||
+419
-35
@@ -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
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user