大拇指单独标定,yaw正确和稳定性修改
This commit is contained in:
@@ -8,7 +8,19 @@
|
||||
ros2 run g20_thumb_apriltag_calibration calibrate_g20_right
|
||||
```
|
||||
|
||||
只重新采集大拇指的4项任务、四指沿用一份已经正式通过的完整会话时,使用:
|
||||
完全独立地只标定大拇指4项任务时,使用:
|
||||
|
||||
```bash
|
||||
ros2 run g20_thumb_apriltag_calibration calibrate_g20_right \
|
||||
--scope thumb
|
||||
```
|
||||
|
||||
该模式不读取任何已标定四指数据。拇指零位求解只使用5条拇指轴、两条顶部同相机
|
||||
方向观测以及拇指自己的机械端点;输出URDF从原始CAD生成,只修改4个拇指主动
|
||||
关节,12个四指关节保持CAD零位。独立结果发布到`latest_thumb_passed`,其JSON是
|
||||
拇指标定/诊断产物,不冒充可直接运行的完整整手曲线JSON。
|
||||
|
||||
如果确实需要把新的拇指结果合并到一份已经通过的完整整手标定,才额外使用:
|
||||
|
||||
```bash
|
||||
ros2 run g20_thumb_apriltag_calibration calibrate_g20_right \
|
||||
@@ -16,24 +28,44 @@ ros2 run g20_thumb_apriltag_calibration calibrate_g20_right \
|
||||
--base-session calibration_output/G20_RIGHT_001/latest_passed
|
||||
```
|
||||
|
||||
合并模式会冻结基础会话中的12个四指主动零位;发布前再次读取基础会话JSON核对,
|
||||
任何四指零位变化都会拒绝发布。基础四指数据仍不参与4个拇指零位的数值求解。
|
||||
|
||||
拇指专项结果重复性通过后,不必再做原来的16项整手扫描。以该拇指会话为基础,
|
||||
只重新采集12项四指任务并合成完整整手URDF:
|
||||
|
||||
```bash
|
||||
ros2 run g20_thumb_apriltag_calibration calibrate_g20_right \
|
||||
--scope fingers \
|
||||
--base-session calibration_output/G20_RIGHT_001/<已通过的拇指会话时间戳>
|
||||
```
|
||||
|
||||
`fingers`模式严格冻结基础会话中的4个拇指主动零位;最终完整整手URDF中的拇指
|
||||
零位与专项会话schema-v4数值完全一致。默认`full`也先调用与`thumb`完全相同的
|
||||
独立拇指内核,再冻结这4个结果求解12个四指零位,因此四指数据不能反向改写
|
||||
拇指结果。原来的默认`full`仍保留,用于需要16项全部重新采集的情况。
|
||||
|
||||
采样文件中的运动域是显式且不可混用的:
|
||||
`requested_command_u8` 表示下发命令,`feedback_u8` 表示电机反馈。
|
||||
在线拟合和离线重放通过同一个数据契约投影到曲线索引;新会话不会把含糊的
|
||||
`command_u8` 写入 `raw_samples.jsonl`。基础会话导入期间状态会显示为
|
||||
`IMPORTING_BASE` 和 `REVALIDATING_INHERITED`,完成复核后才允许机械手运动。
|
||||
|
||||
`--base-session`必须解析到同一序列号目录下的完整PASS会话;启动前会校验源CAD
|
||||
提供`--base-session`时,它必须解析到同一序列号目录下的完整PASS会话;启动前会校验源CAD
|
||||
URDF、相机外参和标定配置哈希。新会话从原始CAD重新生成完整URDF,不在旧校准
|
||||
URDF上叠加。该模式只重采`thumb_cmc_pitch`、`thumb_cmc_roll`、
|
||||
URDF上叠加。两种thumb模式都只重采`thumb_cmc_pitch`、`thumb_cmc_roll`、
|
||||
`thumb_cmc_yaw`、`thumb_mcp/thumb_ip`四项物理任务,其余任务的原始记录导入后仍按
|
||||
当前硬门限重新验证。
|
||||
数据契约和产物哈希检查,但不会以历史四指拟合结果否决本次拇指专项标定。
|
||||
|
||||
开发阶段若上一次会话失败,同一命令会自动校验硬件/几何哈希,并恢复已经
|
||||
完整提交的关节任务;失败中的当前任务始终丢弃重做,位于它后面但已经完整通过的
|
||||
独立任务仍会复用,不再因“连续前缀”限制整段重采。导入的任务会立即用与
|
||||
最终验收相同的硬门限复检(不含视口实时有效率):只以预警带余量通过的旧数据
|
||||
当场剔除并从其在扫掠顺序中的原始位置重采,避免全部任务采完后才在最终验收
|
||||
失败、把会话拉回靠前的关节。运行中的多视角任务按正面主测量和侧面校验测量
|
||||
失败、把会话拉回靠前的关节。方向级自动重扫事件是追加日志中的持久失效标记;
|
||||
恢复时只读取该标记之后的替代采集,不能把同一尝试编号下重扫前后的稳态点合并。
|
||||
因此已经在线硬门限验收的任务保持已完成,暂停中的任务从任务开头重采,不会因
|
||||
日志中仍保留被自动重扫淘汰的旧点而倒退到更早任务。运行中的多视角任务按正面主测量和侧面校验测量
|
||||
独立保留;单轮转轴异常且其余三轮形成一致簇时只补扫异常轮的两个方向。侧面
|
||||
轴线位置若也能明确定位为单轮异常,同样只补扫该轮;补扫会保留任务预检和前次
|
||||
采集确定的PnP分支参考,不会因重新初始化切换到另一组平面Tag镜像解。侧面
|
||||
@@ -43,7 +75,12 @@ URDF上叠加。该模式只重采`thumb_cmc_pitch`、`thumb_cmc_roll`、
|
||||
门限保持不变。每个任务的低速往返预检、首轮交接和四轮双向正式扫描属于同一
|
||||
采集事务:相邻方向共享已验证端点和任务级PnP参考。G20右手正式扫描固定使用
|
||||
产品审定速度,不再根据单次识别密度自动提速,确保不同会话测量的是同一动态
|
||||
过程。需要强制从第一个关节
|
||||
过程。顶部单目`thumb_cmc_yaw`在最终求解后另做零偏轮次重复性检查:前三轮
|
||||
极差默认不得超过0.5°,95%置信半宽不得超过0.75°。若两轮形成不超过门限20%的
|
||||
紧密簇、仅另一轮越界,自动补采该轮两个方向;无法明确定位时只重采完整yaw任务,
|
||||
不会回退重采整手。与`latest_passed`中上一正式结果相差超过0.75°时另写入
|
||||
`thumb_yaw_cross_session_diagnostic`提示检查机械手位置和Tag安装,但该历史差值
|
||||
不直接否决当前会话,也不会用旧结果约束新零位。需要强制从第一个关节
|
||||
重新采集时使用 `--no-resume`。升级前已经分别完成的正面/侧面roll也会合并为
|
||||
一个完整同步任务断点;只有两边数据都完整时才复用。
|
||||
|
||||
|
||||
@@ -26,7 +26,7 @@ artifacts:
|
||||
camera_extrinsics: config/g20_three_camera_extrinsics.yaml
|
||||
camera_extrinsics_sha256: dd623572df3cb83fdefcbe92204dab54a60f2c68eb3a8c9bdb08407e8f0e5d80
|
||||
calibration_config: src/g20_thumb_apriltag_calibration/config/three_camera_calibration.yaml
|
||||
calibration_config_sha256: 8fa16814d7a3462a00aae4eadc273e66bc167bedd5908b0fed40ce6d505b2aeb
|
||||
calibration_config_sha256: 0faaf891ebb616c4c8a3bb3052c48fa4b6c8aa0c5fdc5abaaa89f4fc29cca1c3
|
||||
tag_config: src/g20_thumb_apriltag_calibration/config/three_camera_tags_g20_right_19.yaml
|
||||
tag_config_sha256: b1ab45e97ae42d57b0a3a63c725107b5aa3828c06b2ced16f8222d6e9ebadc41
|
||||
|
||||
|
||||
@@ -55,6 +55,10 @@ g20_calibration:
|
||||
# 侧面当前任务所需Tag在初始化端点的贴面法向应一致;用此先验消除静态IPPE镜像双解。
|
||||
pnp_group_normal_alignment_scale_deg: 5.0
|
||||
pnp_group_maximum_normal_alignment_deg: 15.0
|
||||
# 三个拇指顶部任务共用预检时冻结的Tag 8位姿。Tag 8仍须实时可见;
|
||||
# 任一角点相对会话基准漂移超过2 px并连续5帧时,判定标定中基准被移动。
|
||||
fixed_base_maximum_corner_drift_px: 2.0
|
||||
fixed_base_movement_confirmation_frames: 5
|
||||
# 仅在拇指MCP/IP同步运动且至少一个候选落入可信区间时,用源URDF mimic
|
||||
# 关系辅助选择IPPE分支;若全部候选超限则退回纯视觉,绝不丢帧,也不生成、
|
||||
# 缩放或替代被动IP的自身Tag实测曲线。
|
||||
|
||||
@@ -700,7 +700,7 @@ def _build_right_19_profile() -> HandCalibrationProfile:
|
||||
"stable_cross_view_cone_bias",
|
||||
"measured_passive_dips",
|
||||
"urdf_zero_publication",
|
||||
"palm_axis_side_channel_v2",
|
||||
"palm_axis_relative_motion_v3",
|
||||
}
|
||||
),
|
||||
)
|
||||
|
||||
+66
-4
@@ -54,11 +54,13 @@ from .urdf_zero import (
|
||||
fit_partial_palm_orientation_measurements,
|
||||
fit_rotation_joint_curve,
|
||||
get_zero_calibration_profile,
|
||||
get_right_19_thumb_zero_profile,
|
||||
derive_right_19_mechanical_endpoint_offsets,
|
||||
joint_curve_holdout_errors,
|
||||
refit_axis_line_group_with_shared_radius,
|
||||
RIGHT_19_MECHANICAL_ENDPOINT_JOINTS,
|
||||
RIGHT_19_POST_SOLVE_ENDPOINT_JOINTS,
|
||||
merge_right_19_thumb_zero_result,
|
||||
select_cross_view_roll_direction_source,
|
||||
solve_urdf_zero_offsets,
|
||||
with_depth_free_axis_projection,
|
||||
@@ -1537,7 +1539,63 @@ def replay_session(
|
||||
**post_solve_endpoint_offsets,
|
||||
},
|
||||
}
|
||||
holdout_zero = solve_urdf_zero_offsets(curves=training_fits, **solve_arguments)
|
||||
def solve_with_product_partition(
|
||||
fits: Mapping[str, JointCurveFit],
|
||||
arguments: Mapping[str, Any],
|
||||
):
|
||||
if layout_id != G20_RIGHT_19_LAYOUT:
|
||||
return solve_urdf_zero_offsets(curves=fits, **arguments)
|
||||
thumb_profile = get_right_19_thumb_zero_profile()
|
||||
thumb_names = set(thumb_profile.direct_zero_joints)
|
||||
thumb = solve_urdf_zero_offsets(
|
||||
curves=fits,
|
||||
**{
|
||||
**arguments,
|
||||
"measurements": [
|
||||
item
|
||||
for item in arguments["measurements"]
|
||||
if item.joint in thumb_profile.axis_joints
|
||||
],
|
||||
"joint_maximum_offset_rad": {
|
||||
name: value
|
||||
for name, value in arguments[
|
||||
"joint_maximum_offset_rad"
|
||||
].items()
|
||||
if name in thumb_names
|
||||
},
|
||||
"fixed_direct_zero_offsets_rad": {
|
||||
name: value
|
||||
for name, value in arguments[
|
||||
"fixed_direct_zero_offsets_rad"
|
||||
].items()
|
||||
if name in thumb_names
|
||||
},
|
||||
"static_output_zero_offsets_rad": {
|
||||
name: value
|
||||
for name, value in arguments[
|
||||
"static_output_zero_offsets_rad"
|
||||
].items()
|
||||
if name in thumb_names
|
||||
},
|
||||
"zero_profile": thumb_profile,
|
||||
},
|
||||
)
|
||||
companion = solve_urdf_zero_offsets(
|
||||
curves=fits,
|
||||
**{
|
||||
**arguments,
|
||||
"fixed_direct_zero_offsets_rad": {
|
||||
**arguments["fixed_direct_zero_offsets_rad"],
|
||||
**thumb.direct_offsets_rad,
|
||||
},
|
||||
},
|
||||
)
|
||||
return merge_right_19_thumb_zero_result(
|
||||
thumb_result=thumb,
|
||||
companion_result=companion,
|
||||
)
|
||||
|
||||
holdout_zero = solve_with_product_partition(training_fits, solve_arguments)
|
||||
if not holdout_zero.passed:
|
||||
failure = {
|
||||
"reasons": dict(holdout_zero.failure_reasons),
|
||||
@@ -1625,14 +1683,14 @@ def replay_session(
|
||||
motor_by_joint=motor_by_joint,
|
||||
inherited_zero_joints=zero_profile.inherited_zero_joints,
|
||||
)
|
||||
residual_zero = solve_urdf_zero_offsets(
|
||||
residual_zero = solve_with_product_partition(
|
||||
# Re-check the corrected URDF in the exact frozen trajectory
|
||||
# domain that produced the accepted zero offsets. Runtime
|
||||
# requested-command curves are a separate transfer function and
|
||||
# differ slightly at settled checkpoints; using them here creates
|
||||
# a fictitious residual zero after a structurally correct write.
|
||||
curves=training_fits,
|
||||
**{
|
||||
training_fits,
|
||||
{
|
||||
**solve_arguments,
|
||||
"static_output_zero_offsets_rad": {
|
||||
name: 0.0
|
||||
@@ -1727,6 +1785,10 @@ def replay_session(
|
||||
"rotation_orthogonal_rms_rad": (
|
||||
item.rotation_orthogonal_rms_rad
|
||||
),
|
||||
"axis_estimator": item.axis_estimator,
|
||||
"incremental_pair_count": (
|
||||
item.incremental_pair_count
|
||||
),
|
||||
}
|
||||
for item in palm_orientation_measurements
|
||||
],
|
||||
|
||||
@@ -24,6 +24,7 @@ from .hikrobot_camera import configure_fastdds_large_image_transport
|
||||
from .operator_report import ProgressEstimator, build_failure_report, render_progress_zh
|
||||
from .product import ProductConfig, load_product_config, sha256_file
|
||||
from .publication import atomic_session_pointer, finalize_session_artifacts
|
||||
from .storage import atomic_write_json
|
||||
|
||||
|
||||
EXIT_PASS = 0
|
||||
@@ -502,7 +503,7 @@ def _resolve_partial_base_session(
|
||||
"""Validate the complete passed session that donates non-target tasks."""
|
||||
if base_session is None or not str(base_session).strip():
|
||||
raise ValueError(
|
||||
"--scope thumb requires --base-session pointing to a passed "
|
||||
"partial scope requires --base-session pointing to a passed "
|
||||
"complete G20 right session"
|
||||
)
|
||||
candidate = Path(base_session).expanduser().resolve(strict=True)
|
||||
@@ -564,15 +565,15 @@ def run(
|
||||
config = load_product_config(path, workspace=workspace, check_can=False)
|
||||
load_product_config(path, workspace=workspace, check_can=True)
|
||||
selected_scope = str(scope).strip().lower()
|
||||
if selected_scope not in {"full", "thumb"}:
|
||||
raise ValueError("scope must be one of: full, thumb")
|
||||
if selected_scope not in {"full", "thumb", "fingers"}:
|
||||
raise ValueError("scope must be one of: full, thumb, fingers")
|
||||
if selected_scope == "full" and base_session is not None:
|
||||
raise ValueError("--base-session is valid only with --scope thumb")
|
||||
partial_base = (
|
||||
None
|
||||
if selected_scope == "full"
|
||||
else _resolve_partial_base_session(config, base_session)
|
||||
)
|
||||
raise ValueError(
|
||||
"--base-session is valid only with --scope thumb/fingers"
|
||||
)
|
||||
partial_base = None
|
||||
if selected_scope == "fingers" or base_session is not None:
|
||||
partial_base = _resolve_partial_base_session(config, base_session)
|
||||
except BaseException as error:
|
||||
print(_startup_failure_block(path, error), flush=True)
|
||||
return EXIT_QUALITY
|
||||
@@ -583,14 +584,21 @@ def run(
|
||||
config.session_root.mkdir(parents=True, exist_ok=True)
|
||||
resume_candidate = (
|
||||
partial_base
|
||||
if selected_scope == "thumb"
|
||||
if selected_scope != "full"
|
||||
else (_automatic_resume_candidate(config) if allow_resume else None)
|
||||
)
|
||||
if resume_candidate is not None:
|
||||
if selected_scope == "thumb":
|
||||
print(
|
||||
"拇指专项标定:四指任务继承自已通过会话 "
|
||||
f"{resume_candidate.name};4项拇指任务将全部重新采集。",
|
||||
f"{resume_candidate.name};4项拇指任务将全部重新采集,"
|
||||
"四指零位保持不变。",
|
||||
flush=True,
|
||||
)
|
||||
elif selected_scope == "fingers":
|
||||
print(
|
||||
"四指专项标定:拇指任务和4个拇指零位继承自已通过会话 "
|
||||
f"{resume_candidate.name};12项四指任务将全部重新采集。",
|
||||
flush=True,
|
||||
)
|
||||
else:
|
||||
@@ -599,6 +607,12 @@ def run(
|
||||
f"{resume_candidate.name}。失败中的当前任务会从头重做。",
|
||||
flush=True,
|
||||
)
|
||||
elif selected_scope == "thumb":
|
||||
print(
|
||||
"独立拇指标定:不导入四指会话;仅采集4项拇指任务,"
|
||||
"四指URDF零位保持原始CAD值。",
|
||||
flush=True,
|
||||
)
|
||||
maximum_sessions = config.required_independent_passes
|
||||
for pass_index in range(maximum_sessions):
|
||||
stamp = datetime.now().strftime("%Y%m%d_%H%M%S")
|
||||
@@ -614,13 +628,18 @@ def run(
|
||||
session,
|
||||
resume_from=(
|
||||
resume_candidate
|
||||
if selected_scope == "thumb" or pass_index == 0
|
||||
if selected_scope != "full" or pass_index == 0
|
||||
else None
|
||||
),
|
||||
recalibration_scope=selected_scope,
|
||||
)
|
||||
if code != EXIT_PASS:
|
||||
return code
|
||||
# Keep the exact node-side completion contract durable before the
|
||||
# independent publication layer starts. If publication itself fails,
|
||||
# developers can re-run artifact checks without repeating motion or
|
||||
# inventing lost combination-validation metrics.
|
||||
atomic_write_json(session / "node_status.json", status)
|
||||
try:
|
||||
summary, release_ready = finalize_session_artifacts(
|
||||
config, session, node_status=status
|
||||
@@ -634,11 +653,16 @@ def run(
|
||||
print(block, flush=True)
|
||||
return EXIT_QUALITY
|
||||
if release_ready:
|
||||
result_pointer = (
|
||||
config.session_root / "latest_thumb_passed"
|
||||
if selected_scope == "thumb" and partial_base is None
|
||||
else config.session_root / "latest_passed"
|
||||
)
|
||||
print(
|
||||
"\n".join(
|
||||
[
|
||||
f"PASS:{config.model} {config.side} 标定、URDF修正和复验全部通过。",
|
||||
f"正式结果:{config.session_root / 'latest_passed'}",
|
||||
f"正式结果:{result_pointer}",
|
||||
f"JSON:{session / summary['artifacts']['json']}",
|
||||
f"URDF:{summary['artifacts']['urdf']}",
|
||||
]
|
||||
@@ -657,14 +681,20 @@ def main(args: list[str] | None = None) -> None:
|
||||
parser.add_argument("--preflight-only", action="store_true")
|
||||
parser.add_argument(
|
||||
"--scope",
|
||||
choices=("full", "thumb"),
|
||||
choices=("full", "thumb", "fingers"),
|
||||
default="full",
|
||||
help="full重新标定全手;thumb仅重采4项拇指任务",
|
||||
help=(
|
||||
"full重新标定全手;thumb仅重采4项拇指任务;"
|
||||
"fingers复用已认证拇指并仅重采12项四指任务"
|
||||
),
|
||||
)
|
||||
parser.add_argument(
|
||||
"--base-session",
|
||||
default=None,
|
||||
help="thumb模式继承四指数据的已通过完整会话目录",
|
||||
help=(
|
||||
"可选:thumb模式将结果合并到该完整会话;"
|
||||
"fingers模式必须提供该基础会话"
|
||||
),
|
||||
)
|
||||
parser.add_argument(
|
||||
"--no-resume",
|
||||
|
||||
+16
-4
@@ -289,10 +289,16 @@ def render_progress_zh(
|
||||
]
|
||||
if fit_attempt > 1 and fit_retry_cycles:
|
||||
cycle_text = "/".join(str(cycle) for cycle in fit_retry_cycles)
|
||||
lines.append(
|
||||
f"拟合:首次拟合定位第 {cycle_text} 轮异常,"
|
||||
"仅补采该轮双向;其余轮次数据保留"
|
||||
)
|
||||
if len(fit_retry_cycles) >= repetitions:
|
||||
lines.append(
|
||||
"拟合:首次拟合未能定位单一异常轮,"
|
||||
f"联合补采第 {cycle_text} 轮双向"
|
||||
)
|
||||
else:
|
||||
lines.append(
|
||||
f"拟合:首次拟合定位第 {cycle_text} 轮需复核,"
|
||||
"仅补采对应轮次双向;其余轮次数据保留"
|
||||
)
|
||||
elif fit_attempt_limit > 1:
|
||||
lines.append(
|
||||
f"拟合:任务级硬门限检查(最多允许 "
|
||||
@@ -366,6 +372,12 @@ def classify_error(reason: str, status: Mapping[str, Any]) -> tuple[str, str, st
|
||||
"标定节点状态心跳停止更新",
|
||||
"已采集的完整任务会保留;检查calibration.log后从断点继续,不要重新采集。",
|
||||
)
|
||||
if value == "fixed_base_reference_moved":
|
||||
return (
|
||||
"CAM-REFERENCE-204",
|
||||
"顶部基准Tag在本次标定开始后发生移动",
|
||||
"Tag 8可在新会话预检前重新摆放;固定Tag 8和顶部相机后启动新会话。",
|
||||
)
|
||||
if "palm_orientation_quality_failed" in value:
|
||||
return (
|
||||
"VAL-QUALITY-501",
|
||||
|
||||
@@ -420,7 +420,7 @@ def atomic_session_pointer(root: str | Path, name: str, session: str | Path) ->
|
||||
raise ValueError("session pointer target must be a direct child of the serial root")
|
||||
if not target.is_dir():
|
||||
raise ValueError(f"session directory does not exist: {target}")
|
||||
if name not in {"latest_attempt", "latest_passed"}:
|
||||
if name not in {"latest_attempt", "latest_passed", "latest_thumb_passed"}:
|
||||
raise ValueError("unsupported session pointer name")
|
||||
parent.mkdir(parents=True, exist_ok=True)
|
||||
destination = parent / name
|
||||
@@ -449,14 +449,124 @@ def active_offsets(payload: Mapping[str, Any]) -> dict[str, float]:
|
||||
return result
|
||||
|
||||
|
||||
def standalone_thumb_offsets(payload: Mapping[str, Any]) -> dict[str, float]:
|
||||
"""Validate a standalone thumb artifact and expand CAD finger zeros."""
|
||||
if (
|
||||
payload.get("schema_version") != 1
|
||||
or payload.get("artifact_type")
|
||||
!= "g20_right_standalone_thumb_calibration"
|
||||
or payload.get("side") != "right"
|
||||
or payload.get("non_thumb_zero_policy") != "source_cad_unchanged"
|
||||
or not bool(payload.get("quality", {}).get("passed"))
|
||||
):
|
||||
raise ValueError("standalone thumb JSON contract is invalid")
|
||||
joints = payload.get("joints")
|
||||
expected_joint_names = {
|
||||
"thumb_cmc_roll",
|
||||
"thumb_cmc_yaw",
|
||||
"thumb_cmc_pitch",
|
||||
"thumb_mcp",
|
||||
"thumb_ip",
|
||||
}
|
||||
if not isinstance(joints, Mapping) or set(joints) != expected_joint_names:
|
||||
raise ValueError("standalone thumb JSON has the wrong joint set")
|
||||
thumb_names = expected_joint_names - {"thumb_ip"}
|
||||
result = {name: 0.0 for name in ACTIVE_ZERO_JOINTS}
|
||||
for name in thumb_names:
|
||||
try:
|
||||
value = float(joints[name]["zero_angles"]["urdf_zero_offset_rad"])
|
||||
except (KeyError, TypeError, ValueError) as error:
|
||||
raise ValueError(
|
||||
f"standalone thumb JSON is missing zero for {name}"
|
||||
) from error
|
||||
curve = np.asarray(joints[name].get("angle_rad", ()), dtype=float)
|
||||
if curve.shape != (256,) or not np.all(np.isfinite(curve)):
|
||||
raise ValueError(f"standalone thumb JSON curve is invalid: {name}")
|
||||
result[name] = value
|
||||
passive_curve = np.asarray(
|
||||
joints["thumb_ip"].get("angle_rad", ()), dtype=float
|
||||
)
|
||||
if (
|
||||
passive_curve.shape != (256,)
|
||||
or not np.all(np.isfinite(passive_curve))
|
||||
or not bool(joints["thumb_ip"].get("passive"))
|
||||
or any(not math.isfinite(value) for value in result.values())
|
||||
):
|
||||
raise ValueError("standalone thumb JSON passive curve is invalid")
|
||||
return result
|
||||
|
||||
|
||||
def verify_partial_scope_preserves_certified_zeros(
|
||||
*,
|
||||
scope: str,
|
||||
source_session: str | Path,
|
||||
serial_root: str | Path | None = None,
|
||||
serial_number: str,
|
||||
current_offsets: Mapping[str, float],
|
||||
) -> None:
|
||||
"""Reject a partial artifact that changes any certified non-target zero."""
|
||||
selected = str(scope).strip().lower()
|
||||
if selected not in {"thumb", "fingers"}:
|
||||
return
|
||||
source = Path(source_session).expanduser()
|
||||
if not source.is_absolute():
|
||||
if serial_root is None:
|
||||
raise ValueError(
|
||||
"relative partial base session requires its serial root"
|
||||
)
|
||||
source = Path(serial_root).expanduser() / source
|
||||
source = source.resolve()
|
||||
base_payload = _load_json(
|
||||
source / f"g20_right_{serial_number}_calibration.json"
|
||||
)
|
||||
base_offsets = (
|
||||
standalone_thumb_offsets(base_payload)
|
||||
if base_payload.get("artifact_type")
|
||||
== "g20_right_standalone_thumb_calibration"
|
||||
else active_offsets(base_payload)
|
||||
)
|
||||
preserve_thumb = selected == "fingers"
|
||||
changed = {
|
||||
name: abs(float(current_offsets[name]) - float(base_offsets[name]))
|
||||
for name in ACTIVE_ZERO_JOINTS
|
||||
if name.startswith("thumb_") == preserve_thumb
|
||||
and abs(float(current_offsets[name]) - float(base_offsets[name]))
|
||||
> SCHEMA_V4_OFFSET_QUANTIZATION_TOLERANCE_RAD
|
||||
}
|
||||
if changed:
|
||||
raise ValueError(
|
||||
f"partial {selected} calibration changed certified non-target "
|
||||
"zeros: "
|
||||
+ ",".join(sorted(changed))
|
||||
)
|
||||
|
||||
|
||||
def compare_session_offsets(
|
||||
first: Mapping[str, Any],
|
||||
second: Mapping[str, Any],
|
||||
*,
|
||||
maximum_difference_rad: float,
|
||||
) -> dict[str, float]:
|
||||
left = active_offsets(first)
|
||||
right = active_offsets(second)
|
||||
first_standalone = (
|
||||
first.get("artifact_type")
|
||||
== "g20_right_standalone_thumb_calibration"
|
||||
)
|
||||
second_standalone = (
|
||||
second.get("artifact_type")
|
||||
== "g20_right_standalone_thumb_calibration"
|
||||
)
|
||||
if first_standalone != second_standalone:
|
||||
raise ValueError("cannot compare standalone thumb and full-hand artifacts")
|
||||
left = (
|
||||
standalone_thumb_offsets(first)
|
||||
if first_standalone
|
||||
else active_offsets(first)
|
||||
)
|
||||
right = (
|
||||
standalone_thumb_offsets(second)
|
||||
if second_standalone
|
||||
else active_offsets(second)
|
||||
)
|
||||
differences = {
|
||||
name: abs(left[name] - right[name])
|
||||
for name in sorted(left)
|
||||
@@ -570,11 +680,18 @@ def finalize_session_artifacts(
|
||||
if not paths[name].is_file():
|
||||
raise ValueError(f"session is missing {name}: {paths[name]}")
|
||||
payload = _load_json(paths["json"])
|
||||
validate_compact_payload(payload)
|
||||
if payload.get("schema_version") != 4 or payload.get("side") != "right":
|
||||
raise ValueError("runtime JSON is not the compact right-hand schema v4")
|
||||
if not bool(payload.get("quality", {}).get("passed")):
|
||||
raise ValueError("runtime JSON quality is not passed")
|
||||
standalone_thumb = bool(
|
||||
payload.get("artifact_type")
|
||||
== "g20_right_standalone_thumb_calibration"
|
||||
)
|
||||
if standalone_thumb:
|
||||
standalone_thumb_offsets(payload)
|
||||
else:
|
||||
validate_compact_payload(payload)
|
||||
if payload.get("schema_version") != 4 or payload.get("side") != "right":
|
||||
raise ValueError("runtime JSON is not the compact right-hand schema v4")
|
||||
if not bool(payload.get("quality", {}).get("passed")):
|
||||
raise ValueError("runtime JSON quality is not passed")
|
||||
combination = node_status.get("combination_validation")
|
||||
if not isinstance(combination, Mapping):
|
||||
raise ValueError("node status is missing combination validation")
|
||||
@@ -585,12 +702,12 @@ def finalize_session_artifacts(
|
||||
calibration_scope = str(
|
||||
resume.get("recalibration_scope", "full")
|
||||
).strip().lower()
|
||||
if calibration_scope not in {"full", "thumb"}:
|
||||
if calibration_scope not in {"full", "thumb", "fingers"}:
|
||||
raise ValueError("node status has an unsupported recalibration scope")
|
||||
recalibration_tasks = tuple(
|
||||
str(value) for value in resume.get("recalibration_task_keys", ())
|
||||
)
|
||||
if calibration_scope == "thumb" and (
|
||||
if calibration_scope == "thumb" and not standalone_thumb and (
|
||||
not bool(resume.get("used"))
|
||||
or not str(resume.get("source_session", ""))
|
||||
or len(recalibration_tasks) != 4
|
||||
@@ -599,7 +716,31 @@ def finalize_session_artifacts(
|
||||
raise ValueError(
|
||||
"thumb recalibration is missing its passed base-session provenance"
|
||||
)
|
||||
offsets = active_offsets(payload)
|
||||
if calibration_scope == "fingers" and (
|
||||
not bool(resume.get("used"))
|
||||
or not str(resume.get("source_session", ""))
|
||||
or len(recalibration_tasks) != 12
|
||||
or any("thumb_" in name for name in recalibration_tasks)
|
||||
):
|
||||
raise ValueError(
|
||||
"finger recalibration is missing its certified thumb-session "
|
||||
"provenance"
|
||||
)
|
||||
if standalone_thumb and calibration_scope != "thumb":
|
||||
raise ValueError("standalone thumb artifact has the wrong node scope")
|
||||
offsets = (
|
||||
standalone_thumb_offsets(payload)
|
||||
if standalone_thumb
|
||||
else active_offsets(payload)
|
||||
)
|
||||
if calibration_scope != "full" and not standalone_thumb:
|
||||
verify_partial_scope_preserves_certified_zeros(
|
||||
scope=calibration_scope,
|
||||
source_session=str(resume.get("source_session", "")),
|
||||
serial_root=directory.parent,
|
||||
serial_number=config.serial_number,
|
||||
current_offsets=offsets,
|
||||
)
|
||||
endpoint_offsets = {
|
||||
name: offsets[name]
|
||||
for name in RIGHT_19_MECHANICAL_ENDPOINT_JOINTS
|
||||
@@ -610,22 +751,53 @@ def finalize_session_artifacts(
|
||||
expected_offsets_rad=offsets,
|
||||
endpoint_anchored_offsets_rad=endpoint_offsets,
|
||||
)
|
||||
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"])
|
||||
if standalone_thumb:
|
||||
clipped_runtime_joints = {}
|
||||
else:
|
||||
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()
|
||||
}
|
||||
commands = build_mujoco_validation_commands(payload["baseline_command_u8"])
|
||||
commands = (
|
||||
{
|
||||
"artifact_type": "standalone_thumb_no_full_hand_validation",
|
||||
"poses": [],
|
||||
}
|
||||
if standalone_thumb
|
||||
else build_mujoco_validation_commands(payload["baseline_command_u8"])
|
||||
)
|
||||
atomic_write_json(paths["commands"], commands)
|
||||
|
||||
prior, differences = find_compatible_prior_session(config, directory, payload)
|
||||
release_ready = config.required_independent_passes == 1 or prior is not None
|
||||
quality = dict(payload["quality"])
|
||||
preserved_certified_zeros = sorted(
|
||||
name
|
||||
for name in ACTIVE_ZERO_JOINTS
|
||||
if (
|
||||
calibration_scope == "thumb"
|
||||
and not name.startswith("thumb_")
|
||||
)
|
||||
or (
|
||||
calibration_scope == "fingers"
|
||||
and name.startswith("thumb_")
|
||||
)
|
||||
)
|
||||
if standalone_thumb:
|
||||
preserved_certified_zeros = []
|
||||
calibrated_zero_joints = (
|
||||
sorted(
|
||||
name for name in ACTIVE_ZERO_JOINTS if name.startswith("thumb_")
|
||||
)
|
||||
if standalone_thumb
|
||||
else sorted(ACTIVE_ZERO_JOINTS)
|
||||
)
|
||||
summary: dict[str, Any] = {
|
||||
"schema_version": 1,
|
||||
"serial_number": config.serial_number,
|
||||
@@ -633,15 +805,23 @@ def finalize_session_artifacts(
|
||||
"calibration_scope": calibration_scope,
|
||||
"inherited_base_session": (
|
||||
None
|
||||
if calibration_scope == "full"
|
||||
if calibration_scope == "full" or standalone_thumb
|
||||
else str(resume.get("source_session"))
|
||||
),
|
||||
"freshly_calibrated_task_keys": list(recalibration_tasks),
|
||||
"preserved_certified_zero_joints": preserved_certified_zeros,
|
||||
"result": "PASS" if release_ready else "PASS_AWAITING_SECOND_SESSION",
|
||||
"quality": quality,
|
||||
"runtime_limit_clipped_bins": clipped_runtime_joints,
|
||||
"runtime_curve_domain": "requested_command_u8",
|
||||
"static_zero_calibrated_joints": sorted(ACTIVE_ZERO_JOINTS),
|
||||
"runtime_curve_domain": (
|
||||
"thumb_diagnostic_only"
|
||||
if standalone_thumb
|
||||
else "requested_command_u8"
|
||||
),
|
||||
"static_zero_calibrated_joints": calibrated_zero_joints,
|
||||
"non_thumb_zero_policy": (
|
||||
"source_cad_unchanged" if standalone_thumb else None
|
||||
),
|
||||
"retained_active_urdf_zero_joints": sorted(
|
||||
RETAINED_ACTIVE_ZERO_JOINTS
|
||||
),
|
||||
@@ -693,7 +873,8 @@ def finalize_session_artifacts(
|
||||
# verify_corrected_urdf proves that corrected endpoint limits map back to
|
||||
# the original physical CAD endpoints, while the runtime curves must stay
|
||||
# inside those corrected-coordinate limits.
|
||||
validate_runtime_curves_against_urdf_limits(payload, paths["urdf"])
|
||||
if not standalone_thumb:
|
||||
validate_runtime_curves_against_urdf_limits(payload, paths["urdf"])
|
||||
final_mesh_hashes = {
|
||||
name: sha256_file(path)
|
||||
for name, path in verify_urdf_mesh_resources(paths["urdf"]).items()
|
||||
@@ -701,5 +882,9 @@ def finalize_session_artifacts(
|
||||
if final_mesh_hashes != mesh_hashes:
|
||||
raise ValueError("URDF mesh resources changed during publication")
|
||||
if release_ready:
|
||||
atomic_session_pointer(config.session_root, "latest_passed", directory)
|
||||
atomic_session_pointer(
|
||||
config.session_root,
|
||||
"latest_thumb_passed" if standalone_thumb else "latest_passed",
|
||||
directory,
|
||||
)
|
||||
return summary, release_ready
|
||||
|
||||
+29
-4
@@ -498,6 +498,8 @@ def three_camera_reason_zh(
|
||||
"third_cycle_axis_holdout_deg": "第三轮留出零位可观测轴向误差",
|
||||
"third_cycle_axis_line_rms_mm": "第三轮留出轴线RMS",
|
||||
"third_cycle_trajectory_p95_deg": "第三轮留出轨迹误差P95",
|
||||
"zero_cycle_offset_range_deg": "训练轮零位极差",
|
||||
"zero_confidence_95_half_width_deg": "零位95%置信半宽",
|
||||
"state_image_sync_p95_ms": "图像与电机状态同步误差P95",
|
||||
"tag_valid_rate_percent": "所需Tag同时有效率",
|
||||
}
|
||||
@@ -525,6 +527,8 @@ def three_camera_reason_zh(
|
||||
"third_cycle_axis_holdout_deg": "°",
|
||||
"third_cycle_axis_line_rms_mm": "mm",
|
||||
"third_cycle_trajectory_p95_deg": "°",
|
||||
"zero_cycle_offset_range_deg": "°",
|
||||
"zero_confidence_95_half_width_deg": "°",
|
||||
"state_image_sync_p95_ms": "ms",
|
||||
"tag_valid_rate_percent": "%",
|
||||
"cross_view_roll_curve": "°",
|
||||
@@ -550,6 +554,8 @@ def three_camera_reason_zh(
|
||||
f"{float(value):.2f}°" for value in cycle_travel
|
||||
) + ")"
|
||||
cycle_values = failure.get("cycle_values_deg", [])
|
||||
if not cycle_values:
|
||||
cycle_values = failure.get("cycle_offset_deg", [])
|
||||
if cycle_values:
|
||||
detail += "(各轮=" + "/".join(
|
||||
f"{float(value):.2f}°" for value in cycle_values
|
||||
@@ -586,14 +592,26 @@ def three_camera_reason_zh(
|
||||
"请直接复制诊断块给开发者,不要放宽门限。"
|
||||
)
|
||||
else:
|
||||
source_task_names = set(active.get("source_task_names", []))
|
||||
thumb_yaw_source_retry = source_task_names == {
|
||||
"thumb_cmc_pitch_front",
|
||||
"thumb_cmc_roll_front",
|
||||
}
|
||||
suggestion = (
|
||||
"方向分支已由软件保留,不要放宽门限;请检查传动回差或高支架刚度,"
|
||||
"处理后重新执行一键标定命令,程序会从最近可靠断点继续。"
|
||||
if directional_gap_failure
|
||||
else "修正Tag位置、遮挡或机械行程后重新执行一键标定命令;"
|
||||
"程序只清除当前失败关节的数据并重扫"
|
||||
f"{active.get('directions_to_rescan', 6)}个方向,"
|
||||
"不需要手工调用ROS服务。"
|
||||
else (
|
||||
"保持顶部Tag 8/9无遮挡;程序只替换决定yaw零位的"
|
||||
"CMC pitch/roll顶部轴观测并重扫"
|
||||
f"{active.get('directions_to_rescan', 16)}个方向,"
|
||||
"不会无效重扫yaw侧摆。"
|
||||
if thumb_yaw_source_retry
|
||||
else "修正Tag位置、遮挡或机械行程后重新执行一键标定命令;"
|
||||
"程序只清除当前失败关节的数据并重扫"
|
||||
f"{active.get('directions_to_rescan', 6)}个方向,"
|
||||
"不需要手工调用ROS服务。"
|
||||
)
|
||||
)
|
||||
return (
|
||||
detail_text + "。程序已在当前关节结束后立即停止后续步骤。",
|
||||
@@ -658,6 +676,13 @@ def three_camera_reason_zh(
|
||||
"基准形态Tag预检已通过,正在把三台相机的固定掌部Tag稳健锁定为本会话参考。",
|
||||
"无需操作;锁定完成后允许任务姿态遮挡固定掌部Tag。",
|
||||
)
|
||||
if reason == "fixed_base_reference_moved":
|
||||
return (
|
||||
"顶部Tag 8在本会话基准锁定后连续多帧发生角点位移;程序已立即保持机械手当前位置,"
|
||||
"本会话中已采集数据不再用于发布。",
|
||||
"Tag 8允许在下一次标定预检前重新摆放,但本次不能继续;固定Tag 8和顶部相机后"
|
||||
"重新启动新会话。",
|
||||
)
|
||||
if reason == "waiting_for_task_tags_at_sweep_start":
|
||||
return (
|
||||
"电机已到扫描起点,正在等待当前任务的实时运动Tag;显示为“锁”的固定掌部Tag"
|
||||
|
||||
+1219
-123
File diff suppressed because it is too large
Load Diff
@@ -62,6 +62,10 @@ class ZeroCalibrationProfile:
|
||||
same_view_axis_pair_by_offset: Mapping[str, tuple[str, str]]
|
||||
fixed_direct_zero_offsets_rad: Mapping[str, float]
|
||||
static_output_zero_offsets_rad: Mapping[str, float]
|
||||
# Product full-hand fitting uses the five root-axis line pattern. The
|
||||
# thumb kernel instead uses only the serial thumb chain, so its nuisance
|
||||
# palm pose cannot be influenced by finger observations.
|
||||
base_pose_strategy: str = "full_hand"
|
||||
|
||||
@property
|
||||
def reference_finger(self) -> str:
|
||||
@@ -260,6 +264,56 @@ def _build_right_19_zero_profile(
|
||||
)
|
||||
|
||||
|
||||
def get_right_19_thumb_zero_profile() -> ZeroCalibrationProfile:
|
||||
"""Return the independent four-zero thumb observer graph.
|
||||
|
||||
Roll and MCP are supplied as mechanical endpoint datums by the caller;
|
||||
yaw is observed by the same-view roll/pitch axis pair and pitch by the
|
||||
pitch/MCP serial phase. No finger axis is part of this profile.
|
||||
"""
|
||||
full = get_zero_calibration_profile("right", G20_RIGHT_19_LAYOUT)
|
||||
thumb_direct = tuple(
|
||||
name for name in full.direct_zero_joints if name.startswith("thumb_")
|
||||
)
|
||||
thumb_axes = tuple(
|
||||
name for name in full.axis_joints if name.startswith("thumb_")
|
||||
)
|
||||
return replace(
|
||||
full,
|
||||
direct_zero_joints=thumb_direct,
|
||||
axis_joints=thumb_axes,
|
||||
constrained_circle_joints=frozenset(
|
||||
name
|
||||
for name in full.constrained_circle_joints
|
||||
if name.startswith("thumb_")
|
||||
),
|
||||
root_anchor_joints=frozenset({"thumb_cmc_roll"}),
|
||||
axis_parent_joint={
|
||||
child: parent
|
||||
for child, parent in full.axis_parent_joint.items()
|
||||
if child.startswith("thumb_") and parent.startswith("thumb_")
|
||||
},
|
||||
phase_parent_joint={
|
||||
child: parent
|
||||
for child, parent in full.phase_parent_joint.items()
|
||||
if child.startswith("thumb_") and parent.startswith("thumb_")
|
||||
},
|
||||
offset_observer_joint={
|
||||
target: observer
|
||||
for target, observer in full.offset_observer_joint.items()
|
||||
if target.startswith("thumb_") and observer.startswith("thumb_")
|
||||
},
|
||||
same_view_axis_pair_by_offset={
|
||||
target: pair
|
||||
for target, pair in full.same_view_axis_pair_by_offset.items()
|
||||
if target.startswith("thumb_")
|
||||
},
|
||||
fixed_direct_zero_offsets_rad={},
|
||||
static_output_zero_offsets_rad={},
|
||||
base_pose_strategy="thumb_serial",
|
||||
)
|
||||
|
||||
|
||||
RIGHT_19_MECHANICAL_ENDPOINT_JOINTS = frozenset(
|
||||
{
|
||||
"thumb_mcp",
|
||||
@@ -291,6 +345,7 @@ def anchor_right_19_mechanical_endpoint_curves(
|
||||
maximum_direction_difference_rad: float = math.radians(1.0),
|
||||
maximum_curve_correction_rad: float = math.radians(3.0),
|
||||
feedback_endpoint_joints: frozenset[str] = frozenset(),
|
||||
endpoint_joints: frozenset[str] | None = None,
|
||||
) -> dict[str, JointCurveFit]:
|
||||
"""Set mechanical-endpoint curve scale from direct SO(3) travel.
|
||||
|
||||
@@ -319,6 +374,19 @@ def anchor_right_19_mechanical_endpoint_curves(
|
||||
raise ValueError("endpoint curve correction limit must be positive")
|
||||
|
||||
result = dict(curves)
|
||||
selected_endpoint_joints = (
|
||||
RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS
|
||||
if endpoint_joints is None
|
||||
else frozenset(str(name) for name in endpoint_joints)
|
||||
)
|
||||
unknown_endpoint_joints = sorted(
|
||||
set(selected_endpoint_joints) - set(RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS)
|
||||
)
|
||||
if unknown_endpoint_joints:
|
||||
raise ValueError(
|
||||
"endpoint joints are not endpoint measurements: "
|
||||
+ ",".join(unknown_endpoint_joints)
|
||||
)
|
||||
unknown_feedback_joints = sorted(
|
||||
set(feedback_endpoint_joints) - set(RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS)
|
||||
)
|
||||
@@ -328,7 +396,7 @@ def anchor_right_19_mechanical_endpoint_curves(
|
||||
+ ",".join(unknown_feedback_joints)
|
||||
)
|
||||
|
||||
for name in sorted(RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS):
|
||||
for name in sorted(selected_endpoint_joints):
|
||||
fit = curves.get(name)
|
||||
records = list(records_by_joint.get(name, ()))
|
||||
if fit is None:
|
||||
@@ -456,6 +524,7 @@ def derive_right_19_mechanical_endpoint_offsets(
|
||||
curves: Mapping[str, JointCurveFit],
|
||||
*,
|
||||
maximum_offset_rad: float = math.radians(5.0),
|
||||
endpoint_joints: frozenset[str] | None = None,
|
||||
) -> dict[str, float]:
|
||||
"""Estimate encoder origins from repeatable measured mechanical endpoints.
|
||||
|
||||
@@ -479,7 +548,20 @@ def derive_right_19_mechanical_endpoint_offsets(
|
||||
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_ENDPOINT_MEASUREMENT_JOINTS):
|
||||
selected_endpoint_joints = (
|
||||
RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS
|
||||
if endpoint_joints is None
|
||||
else frozenset(str(name) for name in endpoint_joints)
|
||||
)
|
||||
unknown_endpoint_joints = sorted(
|
||||
set(selected_endpoint_joints) - set(RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS)
|
||||
)
|
||||
if unknown_endpoint_joints:
|
||||
raise ValueError(
|
||||
"endpoint joints are not endpoint measurements: "
|
||||
+ ",".join(unknown_endpoint_joints)
|
||||
)
|
||||
for name in sorted(selected_endpoint_joints):
|
||||
fit = curves.get(name)
|
||||
joint = joints.get(name)
|
||||
limit = None if joint is None else joint.find("limit")
|
||||
@@ -1132,6 +1214,134 @@ class PalmOrientationMeasurement:
|
||||
condition_state_u8: tuple[float, ...]
|
||||
observed_arc_rad: float
|
||||
rotation_orthogonal_rms_rad: float
|
||||
axis_estimator: str = "baseline_relative_so3"
|
||||
incremental_pair_count: int = 0
|
||||
|
||||
|
||||
PALM_AXIS_INCREMENT_COMMAND_SEPARATION_U8 = 24
|
||||
PALM_AXIS_INCREMENT_MINIMUM_ROTATION_RAD = math.radians(0.3)
|
||||
PALM_AXIS_INCREMENT_MINIMUM_PAIR_COUNT = 24
|
||||
PALM_AXIS_INCREMENT_CONSENSUS_PERCENTILE = 75.0
|
||||
|
||||
|
||||
def _incremental_common_rotation_axis(
|
||||
samples_by_bin: Mapping[
|
||||
tuple[str, int], Sequence[Mapping[str, Any]]
|
||||
],
|
||||
*,
|
||||
command_separation_u8: int = (
|
||||
PALM_AXIS_INCREMENT_COMMAND_SEPARATION_U8
|
||||
),
|
||||
minimum_rotation_rad: float = (
|
||||
PALM_AXIS_INCREMENT_MINIMUM_ROTATION_RAD
|
||||
),
|
||||
minimum_pair_count: int = PALM_AXIS_INCREMENT_MINIMUM_PAIR_COUNT,
|
||||
consensus_percentile: float = (
|
||||
PALM_AXIS_INCREMENT_CONSENSUS_PERCENTILE
|
||||
),
|
||||
) -> tuple[np.ndarray, int]:
|
||||
"""Fit one physical axis from robust local Tag rotations.
|
||||
|
||||
A single baseline-to-endpoint logarithm is sensitive to smooth planar-PnP
|
||||
curvature: the fitted axis then changes when the same Tag trajectory is
|
||||
translated to another part of the image. Local finite rotations are
|
||||
expressed directly in the common camera frame, and their undirected
|
||||
weighted consensus estimates the physical revolute axis. The worst
|
||||
quartile is discarded once more after the initial consensus, which is
|
||||
enough to reject endpoint branch curvature without learning a
|
||||
serial-specific yaw value or an image-position correction table.
|
||||
"""
|
||||
separation = int(command_separation_u8)
|
||||
if separation < 4 or separation > 64:
|
||||
raise ValueError("palm axis command separation must be in [4, 64]")
|
||||
if not 0.0 < float(minimum_rotation_rad) < math.pi:
|
||||
raise ValueError("palm axis minimum increment must be in (0, pi)")
|
||||
if int(minimum_pair_count) < 6:
|
||||
raise ValueError("palm axis minimum pair count must be at least six")
|
||||
percentile = float(consensus_percentile)
|
||||
if not 50.0 <= percentile <= 90.0:
|
||||
raise ValueError("palm axis consensus percentile must be in [50, 90]")
|
||||
|
||||
binned_common_rotations: dict[tuple[str, int], Rotation] = {}
|
||||
for key, group in samples_by_bin.items():
|
||||
child_quaternions = []
|
||||
for record in group:
|
||||
pose = record.get("child_pose_common")
|
||||
if not isinstance(pose, Mapping):
|
||||
continue
|
||||
quaternion = pose.get("quaternion_xyzw")
|
||||
if quaternion is not None:
|
||||
child_quaternions.append(quaternion)
|
||||
if not child_quaternions:
|
||||
continue
|
||||
binned_common_rotations[(str(key[0]), int(key[1]))] = (
|
||||
Rotation.from_quat(
|
||||
robust_rotation_summary(child_quaternions)[0]
|
||||
)
|
||||
)
|
||||
|
||||
directions: list[np.ndarray] = []
|
||||
weights: list[float] = []
|
||||
for direction in ("decreasing", "increasing"):
|
||||
commands = sorted(
|
||||
command
|
||||
for candidate_direction, command in binned_common_rotations
|
||||
if candidate_direction == direction
|
||||
)
|
||||
for start_command in commands:
|
||||
candidates = [
|
||||
command
|
||||
for command in commands
|
||||
if separation <= command - start_command <= separation + 2
|
||||
]
|
||||
if not candidates:
|
||||
continue
|
||||
end_command = candidates[0]
|
||||
start = binned_common_rotations[(direction, start_command)]
|
||||
end = binned_common_rotations[(direction, end_command)]
|
||||
local_rotvec = (start.inv() * end).as_rotvec()
|
||||
common_rotvec = start.apply(local_rotvec)
|
||||
increment = float(np.linalg.norm(common_rotvec))
|
||||
if increment < float(minimum_rotation_rad):
|
||||
continue
|
||||
directions.append(common_rotvec / increment)
|
||||
weights.append(increment)
|
||||
|
||||
if len(directions) < int(minimum_pair_count):
|
||||
raise ValueError(
|
||||
"palm axis has too few full-stroke local rotation pairs: "
|
||||
f"{len(directions)}/{int(minimum_pair_count)}"
|
||||
)
|
||||
vectors = np.asarray(directions, dtype=float)
|
||||
increments = np.asarray(weights, dtype=float)
|
||||
|
||||
def consensus(selected: np.ndarray) -> np.ndarray:
|
||||
scatter = np.einsum(
|
||||
"n,ni,nj->ij",
|
||||
increments[selected],
|
||||
vectors[selected],
|
||||
vectors[selected],
|
||||
)
|
||||
eigenvalues, eigenvectors = np.linalg.eigh(scatter)
|
||||
if not np.all(np.isfinite(eigenvalues)):
|
||||
raise ValueError("palm axis local-rotation consensus is invalid")
|
||||
axis = eigenvectors[:, -1]
|
||||
return axis / np.linalg.norm(axis)
|
||||
|
||||
selected = np.ones(len(vectors), dtype=bool)
|
||||
axis = consensus(selected)
|
||||
# Two deterministic refinement rounds prevent the initial scatter from
|
||||
# being pulled toward a dense endpoint-bias cluster.
|
||||
for _ in range(2):
|
||||
deviations = np.arccos(
|
||||
np.clip(np.abs(vectors @ axis), -1.0, 1.0)
|
||||
)
|
||||
limit = float(np.percentile(deviations, percentile))
|
||||
selected = deviations <= limit
|
||||
if int(np.count_nonzero(selected)) < int(minimum_pair_count):
|
||||
raise ValueError("palm axis robust consensus retained too few pairs")
|
||||
axis = consensus(selected)
|
||||
return axis, int(np.count_nonzero(selected))
|
||||
|
||||
|
||||
def fit_partial_palm_orientation_measurement(
|
||||
@@ -1252,6 +1462,33 @@ def fit_partial_palm_orientation_measurement(
|
||||
axis_parent = reference.apply(axis_child)
|
||||
axis_common = Rotation.from_quat(parent_quaternion).apply(axis_parent)
|
||||
axis_common /= np.linalg.norm(axis_common)
|
||||
axis_estimator = "baseline_relative_so3"
|
||||
incremental_pair_count = 0
|
||||
# Product recordings persist the selected moving-Tag pose in the common
|
||||
# camera frame. Use the complete visible stroke only for a local-motion
|
||||
# axis consensus; retain the zero-adjacent fit above as the unchanged arc
|
||||
# and residual quality gate. Legacy/synthetic records without the common
|
||||
# child pose keep their previous estimator exactly.
|
||||
if any(
|
||||
isinstance(record.get("child_pose_common"), Mapping)
|
||||
for record in cycle_samples
|
||||
):
|
||||
full_stroke_bins: dict[
|
||||
tuple[str, int], list[Mapping[str, Any]]
|
||||
] = {}
|
||||
for record in cycle_samples:
|
||||
key = (
|
||||
str(record.get("direction", "")),
|
||||
int(record["command_u8"]),
|
||||
)
|
||||
full_stroke_bins.setdefault(key, []).append(record)
|
||||
incremental_axis, incremental_pair_count = (
|
||||
_incremental_common_rotation_axis(full_stroke_bins)
|
||||
)
|
||||
if float(incremental_axis @ axis_common) < 0.0:
|
||||
incremental_axis = -incremental_axis
|
||||
axis_common = incremental_axis
|
||||
axis_estimator = "robust_full_stroke_local_so3_v1"
|
||||
state = np.median(
|
||||
np.asarray(
|
||||
[record["state_u8"] for record in zero_records], dtype=float
|
||||
@@ -1266,6 +1503,8 @@ def fit_partial_palm_orientation_measurement(
|
||||
condition_state_u8=tuple(float(value) for value in state),
|
||||
observed_arc_rad=observed_arc,
|
||||
rotation_orthogonal_rms_rad=orthogonal_rms,
|
||||
axis_estimator=axis_estimator,
|
||||
incremental_pair_count=incremental_pair_count,
|
||||
)
|
||||
|
||||
|
||||
@@ -2242,6 +2481,148 @@ class ZeroSolveResult:
|
||||
failure_reasons: Mapping[str, str]
|
||||
|
||||
|
||||
def merge_right_19_thumb_zero_result(
|
||||
*,
|
||||
thumb_result: ZeroSolveResult,
|
||||
preserved_offsets_rad: Mapping[str, float] | None = None,
|
||||
companion_result: ZeroSolveResult | None = None,
|
||||
) -> ZeroSolveResult:
|
||||
"""Merge an independent thumb solve without allowing reverse coupling.
|
||||
|
||||
``companion_result`` is the finger/full solver result whose thumb values
|
||||
were fixed to ``thumb_result``. For a thumb-only run, the 12 certified
|
||||
finger values are supplied through ``preserved_offsets_rad`` instead.
|
||||
"""
|
||||
thumb_names = {
|
||||
"thumb_cmc_roll",
|
||||
"thumb_cmc_yaw",
|
||||
"thumb_cmc_pitch",
|
||||
"thumb_mcp",
|
||||
}
|
||||
if set(thumb_result.direct_offsets_rad) != thumb_names:
|
||||
raise ValueError("independent thumb result must contain exactly four zeros")
|
||||
full_profile = get_zero_calibration_profile("right", G20_RIGHT_19_LAYOUT)
|
||||
direct_names = set(full_profile.direct_zero_joints)
|
||||
preserved = {
|
||||
str(name): float(value)
|
||||
for name, value in (preserved_offsets_rad or {}).items()
|
||||
}
|
||||
if set(preserved) - (direct_names - thumb_names):
|
||||
raise ValueError("preserved offsets contain a thumb or unknown joint")
|
||||
if any(not math.isfinite(value) for value in preserved.values()):
|
||||
raise ValueError("preserved offsets must be finite")
|
||||
|
||||
if companion_result is None:
|
||||
if set(preserved) != direct_names - thumb_names:
|
||||
raise ValueError("thumb-only merge requires all 12 certified finger zeros")
|
||||
direct = dict(preserved)
|
||||
all_active = {name: 0.0 for name in full_profile.hand.active_joints}
|
||||
all_active.update(preserved)
|
||||
base_result = thumb_result
|
||||
else:
|
||||
if set(companion_result.direct_offsets_rad) != direct_names:
|
||||
raise ValueError("full companion result must contain all 16 zeros")
|
||||
direct = dict(companion_result.direct_offsets_rad)
|
||||
all_active = dict(companion_result.all_active_offsets_rad)
|
||||
base_result = companion_result
|
||||
direct.update(thumb_result.direct_offsets_rad)
|
||||
all_active.update(thumb_result.direct_offsets_rad)
|
||||
|
||||
def merged_map(name: str) -> dict[str, Any]:
|
||||
values = (
|
||||
{}
|
||||
if companion_result is None
|
||||
else dict(getattr(companion_result, name))
|
||||
)
|
||||
values.update(dict(getattr(thumb_result, name)))
|
||||
return values
|
||||
|
||||
failure_reasons = merged_map("failure_reasons")
|
||||
passed = bool(
|
||||
thumb_result.passed
|
||||
and (companion_result is None or companion_result.passed)
|
||||
)
|
||||
return replace(
|
||||
base_result,
|
||||
direct_offsets_rad=direct,
|
||||
all_active_offsets_rad=all_active,
|
||||
validation_errors_rad=(
|
||||
tuple(thumb_result.validation_errors_rad)
|
||||
if companion_result is None
|
||||
else (
|
||||
*companion_result.validation_errors_rad,
|
||||
*thumb_result.validation_errors_rad,
|
||||
)
|
||||
),
|
||||
validation_error_by_joint_rad=merged_map(
|
||||
"validation_error_by_joint_rad"
|
||||
),
|
||||
validation_line_error_by_joint_m=merged_map(
|
||||
"validation_line_error_by_joint_m"
|
||||
),
|
||||
axis_line_rms_m=(
|
||||
thumb_result.axis_line_rms_m
|
||||
if companion_result is None
|
||||
else max(
|
||||
companion_result.axis_line_rms_m,
|
||||
thumb_result.axis_line_rms_m,
|
||||
)
|
||||
),
|
||||
passed=passed,
|
||||
cycle_offsets_rad=merged_map("cycle_offsets_rad"),
|
||||
offset_uncertainty_rad=merged_map("offset_uncertainty_rad"),
|
||||
offset_confidence_half_width_rad=merged_map(
|
||||
"offset_confidence_half_width_rad"
|
||||
),
|
||||
validation_original_error_by_joint_rad=merged_map(
|
||||
"validation_original_error_by_joint_rad"
|
||||
),
|
||||
validation_improvement_by_joint_rad=merged_map(
|
||||
"validation_improvement_by_joint_rad"
|
||||
),
|
||||
validation_improvement_confidence_lower_rad=merged_map(
|
||||
"validation_improvement_confidence_lower_rad"
|
||||
),
|
||||
offset_covariance_rad2=merged_map("offset_covariance_rad2"),
|
||||
axis_cone_mismatch_by_joint_rad=merged_map(
|
||||
"axis_cone_mismatch_by_joint_rad"
|
||||
),
|
||||
axis_cone_bias_classification_by_joint=merged_map(
|
||||
"axis_cone_bias_classification_by_joint"
|
||||
),
|
||||
failure_reasons=failure_reasons,
|
||||
)
|
||||
|
||||
|
||||
def expand_right_19_thumb_zero_result_with_cad_fingers(
|
||||
thumb_result: ZeroSolveResult,
|
||||
) -> ZeroSolveResult:
|
||||
"""Build a standalone thumb result with every finger left at source CAD.
|
||||
|
||||
This is intentionally different from inheriting a prior finger
|
||||
calibration: the twelve non-thumb values are explicit zeros and no finger
|
||||
observation or artifact is required.
|
||||
"""
|
||||
full_profile = get_zero_calibration_profile("right", G20_RIGHT_19_LAYOUT)
|
||||
thumb_names = {
|
||||
"thumb_cmc_roll",
|
||||
"thumb_cmc_yaw",
|
||||
"thumb_cmc_pitch",
|
||||
"thumb_mcp",
|
||||
}
|
||||
if set(thumb_result.direct_offsets_rad) != thumb_names:
|
||||
raise ValueError("independent thumb result must contain exactly four zeros")
|
||||
direct = {name: 0.0 for name in full_profile.direct_zero_joints}
|
||||
direct.update(thumb_result.direct_offsets_rad)
|
||||
all_active = {name: 0.0 for name in full_profile.hand.active_joints}
|
||||
all_active.update(thumb_result.direct_offsets_rad)
|
||||
return replace(
|
||||
thumb_result,
|
||||
direct_offsets_rad=direct,
|
||||
all_active_offsets_rad=all_active,
|
||||
)
|
||||
|
||||
|
||||
def _angles_from_state(
|
||||
state_u8: Sequence[float],
|
||||
*,
|
||||
@@ -2289,6 +2670,7 @@ def solve_urdf_zero_offsets(
|
||||
tag_layout: str = "legacy_11",
|
||||
fixed_direct_zero_offsets_rad: Mapping[str, float] | None = None,
|
||||
static_output_zero_offsets_rad: Mapping[str, float] | None = None,
|
||||
zero_profile: ZeroCalibrationProfile | None = None,
|
||||
) -> ZeroSolveResult:
|
||||
"""Solve all directly observable zero offsets without fitting CAD errors.
|
||||
|
||||
@@ -2299,7 +2681,15 @@ def solve_urdf_zero_offsets(
|
||||
per joint. This prevents link length, along-axis Tag placement and
|
||||
monocular depth bias from becoming encoder-zero corrections.
|
||||
"""
|
||||
profile = get_zero_calibration_profile(hand_type, tag_layout)
|
||||
profile = (
|
||||
get_zero_calibration_profile(hand_type, tag_layout)
|
||||
if zero_profile is None
|
||||
else zero_profile
|
||||
)
|
||||
if profile.hand.side != str(hand_type).lower():
|
||||
raise ValueError("zero profile hand side does not match hand_type")
|
||||
if profile.hand.layout_id != str(tag_layout):
|
||||
raise ValueError("zero profile layout does not match tag_layout")
|
||||
model = UrdfKinematicModel(source_urdf)
|
||||
training = [m for m in measurements if m.cycle in set(training_cycles)]
|
||||
validation = [m for m in measurements if m.cycle == int(validation_cycle)]
|
||||
@@ -2515,7 +2905,9 @@ def solve_urdf_zero_offsets(
|
||||
zero_offsets.update(fixed_offsets)
|
||||
def fit_base_pose(
|
||||
selected: Sequence[JointAxisMeasurement],
|
||||
pose_offsets: Mapping[str, float] | None = None,
|
||||
) -> tuple[Rotation, np.ndarray]:
|
||||
pose_zero_offsets = zero_offsets if pose_offsets is None else pose_offsets
|
||||
anchors_by_joint = {
|
||||
name: [item for item in selected if item.joint == name]
|
||||
for name in profile.root_anchor_joints
|
||||
@@ -2542,7 +2934,7 @@ def solve_urdf_zero_offsets(
|
||||
observed_points: dict[str, np.ndarray] = {}
|
||||
for name in root_names:
|
||||
items = anchors_by_joint[name]
|
||||
predicted = [predicted_local(item, zero_offsets) for item in items]
|
||||
predicted = [predicted_local(item, pose_zero_offsets) for item in items]
|
||||
predicted_axes[name] = undirected_axis_average(
|
||||
[axis for axis, _ in predicted]
|
||||
)
|
||||
@@ -2559,7 +2951,10 @@ def solve_urdf_zero_offsets(
|
||||
axis=0,
|
||||
)
|
||||
|
||||
if profile.hand.layout_id == G20_RIGHT_19_LAYOUT:
|
||||
if (
|
||||
profile.hand.layout_id == G20_RIGHT_19_LAYOUT
|
||||
and profile.base_pose_strategy == "full_hand"
|
||||
):
|
||||
# In the product layout none of the four finger-roll zeros is a
|
||||
# mechanical prior. Using one finger's pitch axis to orient the
|
||||
# palm would therefore absorb that finger's roll zero into the
|
||||
@@ -2739,7 +3134,7 @@ def solve_urdf_zero_offsets(
|
||||
)
|
||||
axis_errors: list[float] = []
|
||||
for item in selected:
|
||||
predicted_axis, _ = predicted_local(item, zero_offsets)
|
||||
predicted_axis, _ = predicted_local(item, pose_zero_offsets)
|
||||
observed_axis = np.asarray(item.axis_common_xyz, dtype=float)
|
||||
axis_errors.append(
|
||||
math.acos(
|
||||
@@ -2794,7 +3189,7 @@ def solve_urdf_zero_offsets(
|
||||
residuals: list[float] = []
|
||||
for item in non_root_items:
|
||||
predicted_axis, predicted_point = predicted_local(
|
||||
item, zero_offsets
|
||||
item, pose_zero_offsets
|
||||
)
|
||||
predicted_axis = rotation.apply(predicted_axis)
|
||||
predicted_point = (
|
||||
@@ -2842,7 +3237,11 @@ def solve_urdf_zero_offsets(
|
||||
np.ptp(np.asarray(curves[name].angle_rad))
|
||||
),
|
||||
)
|
||||
orientation_anchor = f"{profile.reference_finger}_mcp_pitch"
|
||||
orientation_anchor = (
|
||||
"thumb_cmc_pitch"
|
||||
if profile.base_pose_strategy == "thumb_serial"
|
||||
else f"{profile.reference_finger}_mcp_pitch"
|
||||
)
|
||||
orientation_items = [
|
||||
item for item in selected if item.joint == orientation_anchor
|
||||
]
|
||||
@@ -2852,7 +3251,10 @@ def solve_urdf_zero_offsets(
|
||||
f"{orientation_anchor}"
|
||||
)
|
||||
predicted_orientation_axis = undirected_axis_average(
|
||||
[predicted_local(item, zero_offsets)[0] for item in orientation_items]
|
||||
[
|
||||
predicted_local(item, pose_zero_offsets)[0]
|
||||
for item in orientation_items
|
||||
]
|
||||
)
|
||||
observed_orientation_axis = undirected_axis_average(
|
||||
[item.axis_common_xyz for item in orientation_items]
|
||||
@@ -2899,7 +3301,7 @@ def solve_urdf_zero_offsets(
|
||||
# it does not fit link geometry or encoder offsets.
|
||||
errors_by_joint: dict[str, list[float]] = {}
|
||||
for item in selected:
|
||||
predicted_axis, _ = predicted_local(item, zero_offsets)
|
||||
predicted_axis, _ = predicted_local(item, pose_zero_offsets)
|
||||
predicted_axis = rotation.apply(predicted_axis)
|
||||
observed_axis = np.asarray(item.axis_common_xyz, dtype=float)
|
||||
error = math.acos(
|
||||
@@ -3284,6 +3686,15 @@ def solve_urdf_zero_offsets(
|
||||
offsets = optimise_offsets(
|
||||
selected, rotation, translation, initial=initial
|
||||
)
|
||||
if profile.base_pose_strategy == "thumb_serial":
|
||||
# The first pose estimate only supplies a branch for the invariant
|
||||
# same-view yaw solve. Refit after yaw is known so that the
|
||||
# downstream pitch phase is evaluated in the corrected serial
|
||||
# thumb frame, then freeze that pose for holdout validation.
|
||||
rotation, translation = fit_base_pose(selected, offsets)
|
||||
offsets = optimise_offsets(
|
||||
selected, rotation, translation, initial=offsets
|
||||
)
|
||||
return rotation, translation, offsets
|
||||
|
||||
base_rotation, base_translation, training_offsets = solve_selected(training)
|
||||
@@ -3771,7 +4182,11 @@ def solve_urdf_zero_offsets(
|
||||
for name, limit in zip(profile.direct_zero_joints, offset_limits):
|
||||
if name in fixed_offsets:
|
||||
continue
|
||||
checked_offset = training_offsets[name]
|
||||
# A post-solve mechanical endpoint datum is the value that will be
|
||||
# published for this joint. The visual root-axis scalar remains a
|
||||
# nuisance gauge used for holdout geometry and must not be compared
|
||||
# with the safety bound of a different, endpoint-anchored output.
|
||||
checked_offset = output_offsets.get(name, training_offsets[name])
|
||||
if name in product_finger_rolls:
|
||||
# The four roll motors share the same electrical centre and the
|
||||
# absolute palm axial datum is recovered from the root-line
|
||||
@@ -3795,7 +4210,7 @@ def solve_urdf_zero_offsets(
|
||||
):
|
||||
if name in fixed_offsets:
|
||||
continue
|
||||
checked_offset = training_offsets[name]
|
||||
checked_offset = output_offsets.get(name, 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
|
||||
@@ -3808,15 +4223,19 @@ def solve_urdf_zero_offsets(
|
||||
failure_reasons["palm_orientation"] = (
|
||||
"palm_orientation_holdout_too_large"
|
||||
)
|
||||
if (
|
||||
requires_full_observability = bool(
|
||||
profile.hand.layout_id == G20_RIGHT_19_LAYOUT
|
||||
and profile.base_pose_strategy == "full_hand"
|
||||
)
|
||||
if (
|
||||
requires_full_observability
|
||||
and observability_rank < observability_parameter_count
|
||||
):
|
||||
failure_reasons["palm_and_static_zero"] = (
|
||||
"zero_observation_jacobian_rank_deficient"
|
||||
)
|
||||
elif (
|
||||
profile.hand.layout_id == G20_RIGHT_19_LAYOUT
|
||||
requires_full_observability
|
||||
and observability_condition_number
|
||||
> maximum_observability_condition_number
|
||||
):
|
||||
@@ -3846,7 +4265,11 @@ def solve_urdf_zero_offsets(
|
||||
# above for every phase observation that actually uses line position.
|
||||
if not improvement_passed:
|
||||
for name, value in applied_training.items():
|
||||
if value != 0.0 and improvement_confidence_lower.get(name, 0.0) <= 0.0:
|
||||
if (
|
||||
name not in fixed_offsets
|
||||
and value != 0.0
|
||||
and improvement_confidence_lower.get(name, 0.0) <= 0.0
|
||||
):
|
||||
failure_reasons[name] = "zero_offset_did_not_improve_with_95pct_confidence"
|
||||
# Geometry failures are the root cause and must not be hidden by the
|
||||
# downstream validation symptom produced by the same bad observation.
|
||||
@@ -3868,7 +4291,7 @@ def solve_urdf_zero_offsets(
|
||||
and not diagnostic_bound_hits
|
||||
and not observation_failures
|
||||
and (
|
||||
profile.hand.layout_id != G20_RIGHT_19_LAYOUT
|
||||
not requires_full_observability
|
||||
or (
|
||||
observability_rank == observability_parameter_count
|
||||
and observability_condition_number
|
||||
|
||||
@@ -43,8 +43,10 @@ from g20_thumb_apriltag_calibration.publication import (
|
||||
build_mujoco_validation_commands,
|
||||
finalize_session_artifacts,
|
||||
session_artifact_paths,
|
||||
standalone_thumb_offsets,
|
||||
validate_runtime_curves_against_urdf_limits,
|
||||
verify_corrected_urdf,
|
||||
verify_partial_scope_preserves_certified_zeros,
|
||||
verify_urdf_mesh_resources,
|
||||
)
|
||||
from g20_thumb_apriltag_calibration.storage import atomic_write_json
|
||||
@@ -56,8 +58,11 @@ from g20_thumb_apriltag_calibration.three_camera_node import (
|
||||
_combination_motor_directions,
|
||||
_combination_validation_items,
|
||||
_model_link_in_observer_base,
|
||||
_partial_scope_frozen_zero_offsets,
|
||||
_palm_axis_observer_schema,
|
||||
_palm_axis_resume_policy,
|
||||
build_standalone_thumb_payload,
|
||||
recalibration_quality_joints,
|
||||
recalibration_task_keys,
|
||||
_steady_checkpoint_commands,
|
||||
_unresolved_fit_failure_tasks,
|
||||
@@ -348,7 +353,7 @@ def test_resume_preserves_palm_axis_side_channel_without_requiring_it() -> None:
|
||||
def test_checkpoint_reacquires_new_thumb_yaw_side_channel_once() -> None:
|
||||
profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT)
|
||||
old_capabilities = set(profile.capabilities) - {
|
||||
"palm_axis_side_channel_v2"
|
||||
"palm_axis_relative_motion_v3"
|
||||
}
|
||||
|
||||
compatible, invalidated = _palm_axis_resume_policy(
|
||||
@@ -600,6 +605,102 @@ def test_resume_falls_back_from_an_interrupted_retry_to_complete_attempt() -> No
|
||||
assert {int(row.get("attempt", 1)) for row in checkpoints} == {1}
|
||||
|
||||
|
||||
def test_resume_retry_tombstone_discards_superseded_rows() -> None:
|
||||
profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT)
|
||||
task = next(
|
||||
spec
|
||||
for spec in profile.sweep_specs
|
||||
if spec.key == "pinky_roll_multiview"
|
||||
)
|
||||
old_checkpoints = []
|
||||
for joint in task.joints:
|
||||
old_checkpoints.extend(
|
||||
{
|
||||
"kind": "steady_command_sample",
|
||||
"attempt": 1,
|
||||
"task_name": task.key,
|
||||
"joint": joint,
|
||||
"motor_index": task.motor_index,
|
||||
"cycle": 0,
|
||||
"direction": "decreasing",
|
||||
"requested_command_u8": command,
|
||||
"feedback_u8": command,
|
||||
"resume_test_generation": "superseded",
|
||||
}
|
||||
for command in STEADY_COMMAND_CHECKPOINTS
|
||||
)
|
||||
rows = [
|
||||
*old_checkpoints,
|
||||
{
|
||||
# Legacy retry events had no task_name or attempt. Motor, joint,
|
||||
# cycle and direction must still act as a durable tombstone.
|
||||
"kind": "automatic_sweep_retry",
|
||||
"view": task.view,
|
||||
"motor_index": task.motor_index,
|
||||
"joints": list(task.joints),
|
||||
"cycle": 0,
|
||||
"direction": "decreasing",
|
||||
"retry": 1,
|
||||
},
|
||||
*_complete_resume_task_rows(profile, task),
|
||||
]
|
||||
|
||||
completed, reusable = resumable_completed_task_prefix(
|
||||
profile,
|
||||
4,
|
||||
[255] * 20,
|
||||
rows,
|
||||
allow_sparse=True,
|
||||
)
|
||||
|
||||
assert completed == (task.key,)
|
||||
restored_checkpoints = [
|
||||
row
|
||||
for row in reusable
|
||||
if row["kind"] == "steady_command_sample"
|
||||
and row["direction"] == "decreasing"
|
||||
]
|
||||
assert len(restored_checkpoints) == 2 * len(STEADY_COMMAND_CHECKPOINTS)
|
||||
assert all(
|
||||
row.get("resume_test_generation") != "superseded"
|
||||
for row in restored_checkpoints
|
||||
)
|
||||
|
||||
|
||||
def test_interrupted_retry_does_not_resurrect_rejected_sweep() -> None:
|
||||
profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT)
|
||||
task = next(
|
||||
spec
|
||||
for spec in profile.sweep_specs
|
||||
if spec.key == "pinky_roll_multiview"
|
||||
)
|
||||
rows = [
|
||||
*_complete_resume_task_rows(profile, task),
|
||||
{
|
||||
"kind": "automatic_sweep_retry",
|
||||
"task_name": task.key,
|
||||
"attempt": 1,
|
||||
"view": task.view,
|
||||
"motor_index": task.motor_index,
|
||||
"joints": list(task.joints),
|
||||
"cycle": 0,
|
||||
"direction": "decreasing",
|
||||
"retry": 1,
|
||||
},
|
||||
]
|
||||
|
||||
completed, reusable = resumable_completed_task_prefix(
|
||||
profile,
|
||||
4,
|
||||
[255] * 20,
|
||||
rows,
|
||||
allow_sparse=True,
|
||||
)
|
||||
|
||||
assert completed == ()
|
||||
assert reusable == ()
|
||||
|
||||
|
||||
def test_resume_rejects_unresolved_fit_but_accepts_retired_model_metric() -> None:
|
||||
profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT)
|
||||
first = profile.sweep_specs[0]
|
||||
@@ -804,6 +905,120 @@ def test_thumb_scope_resolves_passed_base_and_launches_partial_mode(
|
||||
assert f"resume_raw_samples_path:={session / 'raw_samples.jsonl'}" in command
|
||||
|
||||
|
||||
def test_fingers_scope_reuses_thumb_and_recollects_twelve_tasks(
|
||||
tmp_path: Path,
|
||||
) -> None:
|
||||
config = _config(tmp_path)
|
||||
profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT)
|
||||
tasks = recalibration_task_keys(profile, "fingers")
|
||||
command = _launch_command(
|
||||
config,
|
||||
config.session_root / "20260831_130000",
|
||||
resume_from=config.session_root / "20260831_120000",
|
||||
recalibration_scope="fingers",
|
||||
)
|
||||
|
||||
assert len(tasks) == 12
|
||||
assert all("thumb_" not in name for name in tasks)
|
||||
assert "recalibration_scope:=fingers" in command
|
||||
|
||||
|
||||
def test_standalone_thumb_launch_has_no_resume_dependency(tmp_path: Path) -> None:
|
||||
config = _config(tmp_path)
|
||||
command = _launch_command(
|
||||
config,
|
||||
config.session_root / "20260831_140000",
|
||||
recalibration_scope="thumb",
|
||||
)
|
||||
|
||||
assert "recalibration_scope:=thumb" in command
|
||||
assert all("resume_raw_samples_path" not in value for value in command)
|
||||
|
||||
|
||||
def test_partial_quality_gates_only_fresh_scope_joints() -> None:
|
||||
profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT)
|
||||
thumb = recalibration_quality_joints(profile, "thumb")
|
||||
fingers = recalibration_quality_joints(profile, "fingers")
|
||||
|
||||
assert thumb
|
||||
assert fingers
|
||||
assert all(name.startswith("thumb_") for name in thumb)
|
||||
assert all(not name.startswith("thumb_") for name in fingers)
|
||||
assert set(thumb) | set(fingers) == set(profile.measured_joints)
|
||||
assert set(thumb).isdisjoint(fingers)
|
||||
|
||||
|
||||
def test_standalone_thumb_payload_expands_non_thumb_to_cad_zero() -> None:
|
||||
profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT)
|
||||
curve = tuple(0.001 * (127 - value) for value in range(256))
|
||||
fits = {
|
||||
name: JointCurveFit(
|
||||
angle_rad=curve,
|
||||
decreasing_rad=curve,
|
||||
increasing_rad=curve,
|
||||
circle={},
|
||||
maximum_monotonic_correction_rad=0.0,
|
||||
maximum_hysteresis_rad=0.0,
|
||||
quality={},
|
||||
)
|
||||
for name in recalibration_quality_joints(profile, "thumb")
|
||||
}
|
||||
thumb_offsets = {
|
||||
"thumb_cmc_roll": 0.01,
|
||||
"thumb_cmc_yaw": -0.02,
|
||||
"thumb_cmc_pitch": 0.03,
|
||||
"thumb_mcp": -0.01,
|
||||
}
|
||||
payload = build_standalone_thumb_payload(
|
||||
profile=profile,
|
||||
serial_number="G20_RIGHT_001",
|
||||
measured_fits=fits,
|
||||
thumb_offsets_rad=thumb_offsets,
|
||||
validation_errors_rad=(0.001, -0.002),
|
||||
baseline_command_u8=profile.baseline_command,
|
||||
source_urdf_sha256="a" * 64,
|
||||
camera_extrinsics_sha256="b" * 64,
|
||||
corrected_urdf_sha256="c" * 64,
|
||||
)
|
||||
|
||||
expanded = standalone_thumb_offsets(payload)
|
||||
assert set(expanded) == set(ACTIVE_ZERO_JOINTS)
|
||||
assert all(expanded[name] == value for name, value in thumb_offsets.items())
|
||||
assert all(
|
||||
value == 0.0
|
||||
for name, value in expanded.items()
|
||||
if not name.startswith("thumb_")
|
||||
)
|
||||
|
||||
|
||||
def test_partial_scopes_freeze_exactly_the_non_target_zeros() -> None:
|
||||
profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT)
|
||||
offsets = {
|
||||
name: 0.001 * (index + 1)
|
||||
for index, name in enumerate(sorted(ACTIVE_ZERO_JOINTS))
|
||||
}
|
||||
payload = _payload(zero_offsets=offsets)
|
||||
|
||||
thumb_frozen = _partial_scope_frozen_zero_offsets(
|
||||
profile, "thumb", payload
|
||||
)
|
||||
finger_frozen = _partial_scope_frozen_zero_offsets(
|
||||
profile, "fingers", payload
|
||||
)
|
||||
|
||||
assert len(thumb_frozen) == 12
|
||||
assert all(not name.startswith("thumb_") for name in thumb_frozen)
|
||||
assert len(finger_frozen) == 4
|
||||
assert all(name.startswith("thumb_") for name in finger_frozen)
|
||||
expected = {
|
||||
name: float(
|
||||
payload["joints"][name]["zero_angles"]["urdf_zero_offset_rad"]
|
||||
)
|
||||
for name in ACTIVE_ZERO_JOINTS
|
||||
}
|
||||
assert {**thumb_frozen, **finger_frozen} == expected
|
||||
|
||||
|
||||
def test_thumb_scope_rejects_base_with_different_geometry(tmp_path: Path) -> None:
|
||||
config = _config(tmp_path)
|
||||
session = config.session_root / "20260830_120000"
|
||||
@@ -1126,6 +1341,7 @@ def test_publication_protects_passive_xml_and_requires_two_matching_sessions(
|
||||
|
||||
def test_publication_records_thumb_recalibration_provenance(tmp_path: Path) -> None:
|
||||
config = _config(tmp_path, passes=1)
|
||||
base_session = _make_passed_session(config, "20260828_205005")
|
||||
session = _make_passed_session(config, "20260830_140000")
|
||||
status = _passed_node_status()
|
||||
tasks = list(
|
||||
@@ -1136,7 +1352,7 @@ def test_publication_records_thumb_recalibration_provenance(tmp_path: Path) -> N
|
||||
)
|
||||
status["resume"] = {
|
||||
"used": True,
|
||||
"source_session": "20260828_205005",
|
||||
"source_session": str(base_session),
|
||||
"recalibration_scope": "thumb",
|
||||
"recalibration_task_keys": tasks,
|
||||
}
|
||||
@@ -1147,8 +1363,202 @@ def test_publication_records_thumb_recalibration_provenance(tmp_path: Path) -> N
|
||||
|
||||
assert ready is True
|
||||
assert summary["calibration_scope"] == "thumb"
|
||||
assert summary["inherited_base_session"] == "20260828_205005"
|
||||
assert summary["inherited_base_session"] == str(base_session)
|
||||
assert summary["freshly_calibrated_task_keys"] == tasks
|
||||
assert len(summary["preserved_certified_zero_joints"]) == 12
|
||||
assert all(
|
||||
not name.startswith("thumb_")
|
||||
for name in summary["preserved_certified_zero_joints"]
|
||||
)
|
||||
|
||||
|
||||
def test_publication_accepts_standalone_thumb_without_base_session(
|
||||
tmp_path: Path,
|
||||
) -> None:
|
||||
config = _config(tmp_path, passes=1)
|
||||
thumb_offsets = {
|
||||
"thumb_cmc_roll": 0.01,
|
||||
"thumb_cmc_yaw": -0.02,
|
||||
"thumb_cmc_pitch": 0.03,
|
||||
"thumb_mcp": -0.01,
|
||||
}
|
||||
all_offsets = {name: 0.0 for name in ACTIVE_ZERO_JOINTS}
|
||||
all_offsets.update(thumb_offsets)
|
||||
session = _make_passed_session(
|
||||
config, "20260831_150000", zero_offsets=all_offsets
|
||||
)
|
||||
full_payload = _payload(
|
||||
config.serial_number, zero_offsets=all_offsets
|
||||
)
|
||||
standalone_payload = {
|
||||
"schema_version": 1,
|
||||
"artifact_type": "g20_right_standalone_thumb_calibration",
|
||||
"model": "G20",
|
||||
"side": "right",
|
||||
"serial_number": config.serial_number,
|
||||
"angle_unit": "rad",
|
||||
"command_range": [0, 255],
|
||||
"baseline_command_u8": full_payload["baseline_command_u8"],
|
||||
"non_thumb_zero_policy": "source_cad_unchanged",
|
||||
"joints": {
|
||||
name: full_payload["joints"][name]
|
||||
for name in (*thumb_offsets, "thumb_ip")
|
||||
},
|
||||
"quality": {
|
||||
"passed": True,
|
||||
"validation_mae_rad": 0.001,
|
||||
"validation_p95_rad": 0.002,
|
||||
},
|
||||
"hashes": {},
|
||||
}
|
||||
atomic_write_json(
|
||||
session / f"g20_right_{config.serial_number}_calibration.json",
|
||||
standalone_payload,
|
||||
)
|
||||
status = _passed_node_status()
|
||||
status["combination_validation"] = {"enabled": False}
|
||||
status["resume"] = {
|
||||
"used": False,
|
||||
"source_session": "",
|
||||
"recalibration_scope": "thumb",
|
||||
"recalibration_task_keys": list(
|
||||
recalibration_task_keys(
|
||||
get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT),
|
||||
"thumb",
|
||||
)
|
||||
),
|
||||
}
|
||||
|
||||
summary, ready = finalize_session_artifacts(
|
||||
config, session, node_status=status
|
||||
)
|
||||
|
||||
assert ready is True
|
||||
assert summary["inherited_base_session"] is None
|
||||
assert summary["non_thumb_zero_policy"] == "source_cad_unchanged"
|
||||
assert summary["static_zero_calibrated_joints"] == sorted(thumb_offsets)
|
||||
assert not (config.session_root / "latest_passed").exists()
|
||||
assert (config.session_root / "latest_thumb_passed").resolve() == session
|
||||
combined_offsets = dict(all_offsets)
|
||||
combined_offsets["index_mcp_pitch"] = 0.02
|
||||
verify_partial_scope_preserves_certified_zeros(
|
||||
scope="fingers",
|
||||
source_session=session,
|
||||
serial_number=config.serial_number,
|
||||
current_offsets=combined_offsets,
|
||||
)
|
||||
|
||||
|
||||
def test_publication_resolves_node_short_base_session_as_sibling(
|
||||
tmp_path: Path,
|
||||
) -> None:
|
||||
config = _config(tmp_path, passes=1)
|
||||
base_session = _make_passed_session(config, "20260828_205005")
|
||||
session = _make_passed_session(config, "20260831_121828")
|
||||
status = _passed_node_status()
|
||||
tasks = list(
|
||||
recalibration_task_keys(
|
||||
get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT),
|
||||
"thumb",
|
||||
)
|
||||
)
|
||||
status["resume"] = {
|
||||
"used": True,
|
||||
# The calibration node intentionally publishes the durable sibling
|
||||
# session identifier, not a machine-specific absolute path.
|
||||
"source_session": base_session.name,
|
||||
"recalibration_scope": "thumb",
|
||||
"recalibration_task_keys": tasks,
|
||||
}
|
||||
|
||||
summary, ready = finalize_session_artifacts(
|
||||
config, session, node_status=status
|
||||
)
|
||||
|
||||
assert ready is True
|
||||
assert summary["inherited_base_session"] == base_session.name
|
||||
|
||||
|
||||
def test_partial_publication_rejects_any_non_target_zero_change(
|
||||
tmp_path: Path,
|
||||
) -> None:
|
||||
config = _config(tmp_path, passes=1)
|
||||
base_offsets = {
|
||||
name: 0.001 * (index + 1)
|
||||
for index, name in enumerate(sorted(ACTIVE_ZERO_JOINTS))
|
||||
}
|
||||
base_session = _make_passed_session(
|
||||
config, "20260831_100000", zero_offsets=base_offsets
|
||||
)
|
||||
|
||||
thumb_result = dict(base_offsets)
|
||||
thumb_result["thumb_cmc_yaw"] += 0.01
|
||||
verify_partial_scope_preserves_certified_zeros(
|
||||
scope="thumb",
|
||||
source_session=base_session,
|
||||
serial_number=config.serial_number,
|
||||
current_offsets=thumb_result,
|
||||
)
|
||||
changed_finger = dict(thumb_result)
|
||||
changed_finger["index_mcp_pitch"] += 0.001
|
||||
with pytest.raises(ValueError, match="non-target zeros"):
|
||||
verify_partial_scope_preserves_certified_zeros(
|
||||
scope="thumb",
|
||||
source_session=base_session,
|
||||
serial_number=config.serial_number,
|
||||
current_offsets=changed_finger,
|
||||
)
|
||||
|
||||
finger_result = dict(base_offsets)
|
||||
finger_result["index_mcp_pitch"] += 0.01
|
||||
verify_partial_scope_preserves_certified_zeros(
|
||||
scope="fingers",
|
||||
source_session=base_session,
|
||||
serial_number=config.serial_number,
|
||||
current_offsets=finger_result,
|
||||
)
|
||||
changed_thumb = dict(finger_result)
|
||||
changed_thumb["thumb_mcp"] += 0.001
|
||||
with pytest.raises(ValueError, match="non-target zeros"):
|
||||
verify_partial_scope_preserves_certified_zeros(
|
||||
scope="fingers",
|
||||
source_session=base_session,
|
||||
serial_number=config.serial_number,
|
||||
current_offsets=changed_thumb,
|
||||
)
|
||||
|
||||
|
||||
def test_publication_records_fingers_scope_with_certified_thumb(
|
||||
tmp_path: Path,
|
||||
) -> None:
|
||||
config = _config(tmp_path, passes=1)
|
||||
base_session = _make_passed_session(config, "20260831_110000")
|
||||
session = _make_passed_session(config, "20260831_120000")
|
||||
tasks = list(
|
||||
recalibration_task_keys(
|
||||
get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT),
|
||||
"fingers",
|
||||
)
|
||||
)
|
||||
status = _passed_node_status()
|
||||
status["resume"] = {
|
||||
"used": True,
|
||||
"source_session": str(base_session),
|
||||
"recalibration_scope": "fingers",
|
||||
"recalibration_task_keys": tasks,
|
||||
}
|
||||
|
||||
summary, ready = finalize_session_artifacts(
|
||||
config, session, node_status=status
|
||||
)
|
||||
|
||||
assert ready is True
|
||||
assert summary["calibration_scope"] == "fingers"
|
||||
assert summary["inherited_base_session"] == str(base_session)
|
||||
assert summary["freshly_calibrated_task_keys"] == tasks
|
||||
assert summary["preserved_certified_zero_joints"] == sorted(
|
||||
name for name in ACTIVE_ZERO_JOINTS if name.startswith("thumb_")
|
||||
)
|
||||
|
||||
|
||||
def test_publication_numerically_binds_json_offsets_and_urdf_limits(
|
||||
@@ -1537,6 +1947,16 @@ def test_stopped_status_heartbeat_has_stable_communication_code() -> None:
|
||||
assert "断点" in suggestion
|
||||
|
||||
|
||||
def test_fixed_base_reference_movement_has_stable_camera_code() -> None:
|
||||
code, problem, suggestion = classify_error(
|
||||
"fixed_base_reference_moved", {}
|
||||
)
|
||||
|
||||
assert code == "CAM-REFERENCE-204"
|
||||
assert "Tag" 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", {}
|
||||
@@ -1617,7 +2037,33 @@ def test_progress_contains_stage_eta_tags_cameras_and_feedback() -> None:
|
||||
assert "反馈:98.0 Hz" in text
|
||||
assert "当前任务ID:正面[0✓] 侧面[0✓] 顶部[0✓]" in text
|
||||
assert "断点:已恢复 3/16 个完整任务" in text
|
||||
assert "首次拟合定位第 2 轮异常,仅补采该轮双向" in text
|
||||
assert "首次拟合定位第 2 轮需复核,仅补采对应轮次双向" in text
|
||||
|
||||
|
||||
def test_progress_describes_nonlocalized_full_fit_retry() -> None:
|
||||
status = {
|
||||
"state": "SWEEP",
|
||||
"progress": 0.9,
|
||||
"feedback_hz": 58.0,
|
||||
"active": {
|
||||
"joints": ["thumb_cmc_pitch"],
|
||||
"cycle": 1,
|
||||
"repetitions": 4,
|
||||
"direction": "decreasing",
|
||||
"valid_frames": 20,
|
||||
"automatic_retry_count": 1,
|
||||
"fit_attempt": 2,
|
||||
"fit_attempt_limit": 3,
|
||||
"fit_retry_cycles": [1, 2, 3, 4],
|
||||
},
|
||||
"views": {},
|
||||
}
|
||||
|
||||
text = render_progress_zh(
|
||||
"G20_RIGHT_001", status, ProgressEstimator(started_at=0.0)
|
||||
)
|
||||
|
||||
assert "未能定位单一异常轮,联合补采第 1/2/3/4 轮双向" in text
|
||||
|
||||
|
||||
def test_preflight_progress_marks_non_base_tag_occlusion_as_allowed() -> None:
|
||||
@@ -2102,3 +2548,68 @@ def test_node_drops_imported_task_failing_hard_gates(
|
||||
import_record["revalidation_dropped_tasks"][0]["task"]
|
||||
== profile.sweep_specs[0].key
|
||||
)
|
||||
|
||||
|
||||
def test_sparse_resume_revalidates_against_only_retained_dependencies(
|
||||
monkeypatch: pytest.MonkeyPatch,
|
||||
) -> None:
|
||||
profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT)
|
||||
pinky_key = "pinky_roll_multiview"
|
||||
ring_key = "ring_roll_multiview"
|
||||
pinky_spec = next(
|
||||
spec for spec in profile.sweep_specs if spec.key == pinky_key
|
||||
)
|
||||
ring_spec = next(spec for spec in profile.sweep_specs if spec.key == ring_key)
|
||||
records_by_joint = {name: [] for name in profile.record_joints}
|
||||
baseline_records_by_joint = {name: [] for name in profile.record_joints}
|
||||
command_records_by_joint = {name: [] for name in profile.record_joints}
|
||||
for joint_name in (*pinky_spec.joints, *ring_spec.joints):
|
||||
records_by_joint[joint_name].append({"task_name": "fixture"})
|
||||
|
||||
node = SimpleNamespace(
|
||||
state="",
|
||||
profile=profile,
|
||||
sweep_items=[
|
||||
SimpleNamespace(spec=pinky_spec),
|
||||
SimpleNamespace(spec=ring_spec),
|
||||
],
|
||||
records_by_joint=records_by_joint,
|
||||
baseline_records_by_joint=baseline_records_by_joint,
|
||||
command_records_by_joint=command_records_by_joint,
|
||||
palm_axis_records_by_source={
|
||||
observer.source_name: []
|
||||
for observer in profile.palm_axis_observers
|
||||
},
|
||||
)
|
||||
|
||||
def failures(_node, spec, *, include_view_validity):
|
||||
assert include_view_validity is False
|
||||
if spec.key == pinky_key:
|
||||
return [{"joint": pinky_spec.joints[0], "metric": "fixture_failure"}]
|
||||
pinky_records_remain = any(
|
||||
records_by_joint[joint_name] for joint_name in pinky_spec.joints
|
||||
)
|
||||
return [] if pinky_records_remain else [
|
||||
{"joint": ring_spec.joints[0], "metric": "missing_reference"}
|
||||
]
|
||||
|
||||
monkeypatch.setattr(
|
||||
G20ThreeCameraCalibrationNode,
|
||||
"_provisional_fit_failures",
|
||||
failures,
|
||||
)
|
||||
|
||||
accepted, dropped = (
|
||||
G20ThreeCameraCalibrationNode._revalidate_imported_tasks(
|
||||
node,
|
||||
[pinky_key, ring_key],
|
||||
allow_sparse=True,
|
||||
)
|
||||
)
|
||||
|
||||
assert accepted == []
|
||||
assert [failure["task"] for failure in dropped] == [pinky_key, ring_key]
|
||||
assert all(
|
||||
not records_by_joint[joint_name]
|
||||
for joint_name in (*pinky_spec.joints, *ring_spec.joints)
|
||||
)
|
||||
|
||||
@@ -362,6 +362,38 @@ def test_baseline_hysteresis_failure_shows_values_instead_of_unknown() -> None:
|
||||
assert "未知原因" not in text
|
||||
|
||||
|
||||
def test_thumb_yaw_repeatability_failure_names_real_source_retry() -> None:
|
||||
explanation, suggestion = three_camera_reason_zh(
|
||||
"PAUSED",
|
||||
"joint_fit_check_failed",
|
||||
{
|
||||
"source_task_names": [
|
||||
"thumb_cmc_pitch_front",
|
||||
"thumb_cmc_roll_front",
|
||||
],
|
||||
"directions_to_rescan": 16,
|
||||
"failures": [
|
||||
{
|
||||
"joint": "thumb_cmc_yaw",
|
||||
"metric": "zero_cycle_offset_range_deg",
|
||||
"actual": 0.555836,
|
||||
"limit": 0.5,
|
||||
"comparison": "maximum",
|
||||
"cycle": 3,
|
||||
"cycle_offset_deg": [-2.650924, -2.615277, -2.095088],
|
||||
}
|
||||
],
|
||||
},
|
||||
)
|
||||
|
||||
assert "训练轮零位极差为0.56°" in explanation
|
||||
assert "各轮=-2.65°/-2.62°/-2.10°" in explanation
|
||||
assert "未知原因" not in explanation
|
||||
assert "CMC pitch/roll顶部轴观测" in suggestion
|
||||
assert "重扫16个方向" in suggestion
|
||||
assert "不会无效重扫yaw侧摆" in suggestion
|
||||
|
||||
|
||||
def test_sweep_gap_status_names_gap_bounds_and_full_detection_rate() -> None:
|
||||
payload = {
|
||||
"state": "PAUSED",
|
||||
@@ -650,3 +682,16 @@ def test_same_finger_transition_explains_that_clearance_stays_parked() -> None:
|
||||
assert "继续保持当前避让姿态" in explanation
|
||||
assert "只调整被测关节" in explanation
|
||||
assert "不要手动展开" in action
|
||||
|
||||
|
||||
def test_fixed_base_reference_movement_requires_a_new_session() -> None:
|
||||
explanation, action = three_camera_reason_zh(
|
||||
"PAUSED",
|
||||
"fixed_base_reference_moved",
|
||||
{},
|
||||
)
|
||||
|
||||
assert "Tag 8" in explanation
|
||||
assert "基准锁定后" in explanation
|
||||
assert "下一次标定预检前重新摆放" in action
|
||||
assert "重新启动新会话" in action
|
||||
|
||||
@@ -43,13 +43,18 @@ from g20_thumb_apriltag_calibration.three_camera_node import (
|
||||
_frames_cover_sweep_joints,
|
||||
_isolated_axis_cycle_outliers,
|
||||
_isolated_axis_line_cycle_outliers,
|
||||
_isolated_scalar_cycle_outlier,
|
||||
_maximum_corner_drift_px,
|
||||
_overall_progress,
|
||||
_preserve_pnp_task_reference_for_sweep,
|
||||
_previous_passed_joint_zero_offset,
|
||||
_palm_axis_observer_for_sweep,
|
||||
_requires_pnp_tracker_reset_for_sweep,
|
||||
_selected_pose_qualities,
|
||||
_sweep_uses_locked_base_reference,
|
||||
_sweep_plan_transition,
|
||||
_sweep_views,
|
||||
_thumb_yaw_zero_repeatability_failures,
|
||||
)
|
||||
from g20_thumb_apriltag_calibration.pnp import SquareTagPose
|
||||
from g20_thumb_apriltag_calibration.urdf_zero import (
|
||||
@@ -364,6 +369,12 @@ def test_pnp_reset_policy_preserves_legacy_and_retry_recovery() -> None:
|
||||
SweepItem(right_spec, 2, DIRECTION_DECREASING),
|
||||
is_fit_retry=True,
|
||||
)
|
||||
assert not _requires_pnp_tracker_reset_for_sweep(
|
||||
RIGHT_19_HAND_PROFILE,
|
||||
SweepItem(right_spec, 2, DIRECTION_DECREASING),
|
||||
is_fit_retry=True,
|
||||
preserve_retry_continuity=True,
|
||||
)
|
||||
assert _requires_pnp_tracker_reset_for_sweep(
|
||||
RIGHT_HAND_PROFILE,
|
||||
SweepItem(legacy_spec, 2, DIRECTION_DECREASING),
|
||||
@@ -703,6 +714,78 @@ def test_multiview_roll_uses_locked_front_base_but_live_moving_tag() -> None:
|
||||
assert live == ("middle_roll",)
|
||||
|
||||
|
||||
@pytest.mark.parametrize(
|
||||
"task_key",
|
||||
(
|
||||
"thumb_cmc_pitch_front",
|
||||
"thumb_cmc_roll_front",
|
||||
"thumb_cmc_yaw_top",
|
||||
),
|
||||
)
|
||||
def test_thumb_top_tasks_freeze_tag8_but_keep_it_live(task_key: str) -> None:
|
||||
spec = next(
|
||||
item
|
||||
for item in RIGHT_19_HAND_PROFILE.sweep_specs
|
||||
if item.key == task_key
|
||||
)
|
||||
top = SimpleNamespace(
|
||||
roles=tuple(RIGHT_19_HAND_PROFILE.view_tags["top"]),
|
||||
preflight_roles=("top_base", "thumb_yaw"),
|
||||
locked_base_pose=SquareTagPose(
|
||||
(0.0, 0.0, 0.0, 1.0), (0.0, 0.0, 1.0), 0.1
|
||||
),
|
||||
locked_base_center_xy_px=(100.0, 200.0),
|
||||
locked_base_quality=TagQuality(0, 100.0, 30.0, 0.1),
|
||||
)
|
||||
node = SimpleNamespace(
|
||||
profile=RIGHT_19_HAND_PROFILE,
|
||||
views={"top": top},
|
||||
active_sweep=SweepItem(spec, 0, DIRECTION_DECREASING),
|
||||
active_validation=None,
|
||||
retry_sweep_spec=None,
|
||||
active_combination_validation=None,
|
||||
)
|
||||
node._locked_base_role_for_active_capture = lambda view: (
|
||||
G20ThreeCameraCalibrationNode._locked_base_role_for_active_capture(
|
||||
node, view
|
||||
)
|
||||
)
|
||||
|
||||
assert _sweep_uses_locked_base_reference(
|
||||
RIGHT_19_HAND_PROFILE, spec, "top"
|
||||
)
|
||||
assert (
|
||||
G20ThreeCameraCalibrationNode._locked_base_role_for_active_capture(
|
||||
node, "top"
|
||||
)
|
||||
== "top_base"
|
||||
)
|
||||
required = G20ThreeCameraCalibrationNode._required_roles_for_view(
|
||||
node, "top"
|
||||
)
|
||||
live = G20ThreeCameraCalibrationNode._live_required_roles_for_view(
|
||||
node, "top", required
|
||||
)
|
||||
assert "top_base" in required
|
||||
assert "top_base" in live
|
||||
|
||||
|
||||
def test_fixed_base_corner_drift_uses_all_four_ordered_corners() -> None:
|
||||
reference = np.asarray(
|
||||
[[10.0, 10.0], [30.0, 10.0], [30.0, 30.0], [10.0, 30.0]]
|
||||
)
|
||||
subpixel_noise = reference + np.asarray(
|
||||
[[0.1, -0.1], [0.2, 0.0], [0.0, 0.2], [-0.1, 0.1]]
|
||||
)
|
||||
moved = reference.copy()
|
||||
moved[2] += [1.5, 2.0]
|
||||
|
||||
assert _maximum_corner_drift_px(
|
||||
reference, subpixel_noise
|
||||
) < 0.3
|
||||
assert _maximum_corner_drift_px(reference, moved) == pytest.approx(2.5)
|
||||
|
||||
|
||||
def test_side_only_finger_task_keeps_occluded_inactive_front_base_locked() -> None:
|
||||
spec = next(
|
||||
item
|
||||
@@ -773,8 +856,23 @@ def test_baseline_preflight_locks_robust_fixed_tag_reference(tmp_path) -> None:
|
||||
views = {
|
||||
view: SimpleNamespace(
|
||||
fixed_base_observations=deque(observations, maxlen=60),
|
||||
fixed_base_corner_observations=deque(
|
||||
(
|
||||
np.asarray(
|
||||
[
|
||||
[90.0 + index % 2, 190.0],
|
||||
[110.0 + index % 2, 190.0],
|
||||
[110.0 + index % 2, 210.0],
|
||||
[90.0 + index % 2, 210.0],
|
||||
]
|
||||
)
|
||||
for index in range(30)
|
||||
),
|
||||
maxlen=60,
|
||||
),
|
||||
locked_base_pose=None,
|
||||
locked_base_center_xy_px=None,
|
||||
locked_base_corners_xy=None,
|
||||
locked_base_quality=None,
|
||||
view_tags={
|
||||
{"front": "front_base", "side": "side_base", "top": "top_base"}[
|
||||
@@ -793,6 +891,12 @@ def test_baseline_preflight_locks_robust_fixed_tag_reference(tmp_path) -> None:
|
||||
assert G20ThreeCameraCalibrationNode._lock_fixed_base_references(node)
|
||||
assert views["front"].locked_base_pose is not None
|
||||
assert views["front"].locked_base_center_xy_px == (100.5, 200.0)
|
||||
assert views["front"].locked_base_corners_xy == (
|
||||
(90.5, 190.0),
|
||||
(110.5, 190.0),
|
||||
(110.5, 210.0),
|
||||
(90.5, 210.0),
|
||||
)
|
||||
events = [
|
||||
json.loads(line)
|
||||
for line in node.raw_path.read_text().splitlines()
|
||||
@@ -2631,6 +2735,121 @@ def test_resume_discards_only_failed_specs_samples(tmp_path) -> None:
|
||||
}
|
||||
|
||||
|
||||
def test_thumb_yaw_retry_replaces_palm_axis_source_tasks_not_yaw_sweep(
|
||||
tmp_path,
|
||||
) -> None:
|
||||
yaw_spec = next(
|
||||
item
|
||||
for item in RIGHT_19_HAND_PROFILE.sweep_specs
|
||||
if item.key == "thumb_cmc_yaw_top"
|
||||
)
|
||||
source_specs = [
|
||||
item
|
||||
for item in RIGHT_19_HAND_PROFILE.sweep_specs
|
||||
if item.key
|
||||
in {"thumb_cmc_pitch_front", "thumb_cmc_roll_front"}
|
||||
]
|
||||
source_joints = {
|
||||
joint for item in source_specs for joint in item.joints
|
||||
}
|
||||
records = {
|
||||
**{
|
||||
joint: [
|
||||
{"cycle": 0, "keep": joint},
|
||||
{"cycle": 2, "old": joint},
|
||||
]
|
||||
for joint in source_joints
|
||||
},
|
||||
"thumb_cmc_yaw": [{"cycle": 2, "keep": "yaw"}],
|
||||
}
|
||||
source_names = {
|
||||
observer.source_name
|
||||
for observer in RIGHT_19_HAND_PROFILE.palm_axis_observers
|
||||
if observer.task_name in {item.key for item in source_specs}
|
||||
}
|
||||
palm_records = {
|
||||
name: [
|
||||
{"cycle": 0, "keep": name},
|
||||
{"cycle": 2, "old": name},
|
||||
]
|
||||
for name in source_names
|
||||
}
|
||||
resets: list[str] = []
|
||||
|
||||
def runtime(name: str) -> SimpleNamespace:
|
||||
return SimpleNamespace(
|
||||
name=name,
|
||||
pnp_invalid_since=1.0,
|
||||
pnp_reset_count=0,
|
||||
task_valid_frames=100,
|
||||
task_total_frames=110,
|
||||
)
|
||||
|
||||
views = {name: runtime(name) for name in ("front", "top")}
|
||||
node = SimpleNamespace(
|
||||
profile=RIGHT_19_HAND_PROFILE,
|
||||
retry_sweep_spec=yaw_spec,
|
||||
retry_joint_names={"thumb_cmc_yaw"},
|
||||
retry_source_failure_task_key=yaw_spec.key,
|
||||
retry_source_task_keys=tuple(item.key for item in source_specs),
|
||||
retry_cycles={2},
|
||||
records_by_joint=records,
|
||||
baseline_records_by_joint={joint: [] for joint in source_joints},
|
||||
command_records_by_joint={joint: [] for joint in source_joints},
|
||||
palm_axis_records_by_source=palm_records,
|
||||
sweep_attempts={yaw_spec.key: 1},
|
||||
sweep_retry_counts={},
|
||||
raw_path=tmp_path / "raw_samples.jsonl",
|
||||
paused_reason="joint_fit_check_failed",
|
||||
fit_failure={"failures": ["old"]},
|
||||
repetitions=4,
|
||||
sweep_items=[
|
||||
SweepItem(item, cycle, direction)
|
||||
for item in source_specs
|
||||
for cycle in range(4)
|
||||
for direction in (
|
||||
DIRECTION_DECREASING,
|
||||
DIRECTION_INCREASING,
|
||||
)
|
||||
],
|
||||
views=views,
|
||||
_reset_view_trackers=(
|
||||
lambda item, preserve_task_reference=False: resets.append(
|
||||
item.name
|
||||
)
|
||||
),
|
||||
)
|
||||
|
||||
G20ThreeCameraCalibrationNode._prepare_failed_sweep_retry(node)
|
||||
|
||||
assert records["thumb_cmc_yaw"] == [
|
||||
{"cycle": 2, "keep": "yaw"}
|
||||
]
|
||||
assert all(
|
||||
records[joint] == [{"cycle": 0, "keep": joint}]
|
||||
for joint in source_joints
|
||||
)
|
||||
assert all(
|
||||
palm_records[name] == [{"cycle": 0, "keep": name}]
|
||||
for name in source_names
|
||||
)
|
||||
assert [(item.spec.key, item.cycle) for item in node.retry_sweep_items] == [
|
||||
(item.key, 2)
|
||||
for item in source_specs
|
||||
for _direction in range(2)
|
||||
]
|
||||
assert resets == []
|
||||
assert node.retry_preserve_pnp_continuity is True
|
||||
event = json.loads((tmp_path / "raw_samples.jsonl").read_text())
|
||||
assert event["source_task_names"] == [item.key for item in source_specs]
|
||||
assert event["source_attempts"] == {item.key: 2 for item in source_specs}
|
||||
assert event["joints_to_rescan"] == sorted(source_joints)
|
||||
assert event["cycles"] == [3]
|
||||
assert event["pnp_reference_policy"] == "preserve_existing_generation"
|
||||
assert all(runtime.task_valid_frames == 0 for runtime in views.values())
|
||||
assert all(runtime.task_total_frames == 0 for runtime in views.values())
|
||||
|
||||
|
||||
def test_multiview_retry_discards_only_failed_side_measurement(tmp_path) -> None:
|
||||
spec = next(
|
||||
item
|
||||
@@ -3280,6 +3499,222 @@ def test_axis_cycle_spread_localizes_one_outlier_for_four_rounds() -> None:
|
||||
) == {0}
|
||||
|
||||
|
||||
def test_thumb_yaw_zero_spread_localizes_181154_second_cycle() -> None:
|
||||
cycle_offsets = [
|
||||
math.radians(-2.518678),
|
||||
math.radians(-3.098037),
|
||||
math.radians(-2.544640),
|
||||
]
|
||||
result = SimpleNamespace(
|
||||
cycle_offsets_rad={"thumb_cmc_yaw": cycle_offsets},
|
||||
offset_confidence_half_width_rad={
|
||||
"thumb_cmc_yaw": math.radians(0.81295)
|
||||
},
|
||||
)
|
||||
|
||||
assert _isolated_scalar_cycle_outlier(
|
||||
cycle_offsets, math.radians(0.5)
|
||||
) == {1}
|
||||
assert _thumb_yaw_zero_repeatability_failures(
|
||||
result,
|
||||
maximum_cycle_range_rad=math.radians(0.5),
|
||||
maximum_confidence_half_width_rad=math.radians(0.75),
|
||||
) == [
|
||||
{
|
||||
"joint": "thumb_cmc_yaw",
|
||||
"metric": "zero_cycle_offset_range_deg",
|
||||
"actual": 0.579359,
|
||||
"limit": 0.5,
|
||||
"comparison": "maximum",
|
||||
"cycle_offset_deg": [-2.518678, -3.098037, -2.54464],
|
||||
"cycle": 2,
|
||||
"inlier_cycles": [1, 3],
|
||||
}
|
||||
]
|
||||
|
||||
|
||||
def test_thumb_yaw_zero_spread_localizes_142322_marginal_cycle() -> None:
|
||||
cycle_offsets = [
|
||||
math.radians(-2.589022),
|
||||
math.radians(-2.915619),
|
||||
math.radians(-2.391267),
|
||||
]
|
||||
result = SimpleNamespace(
|
||||
cycle_offsets_rad={"thumb_cmc_yaw": cycle_offsets},
|
||||
offset_confidence_half_width_rad={
|
||||
"thumb_cmc_yaw": math.radians(0.658)
|
||||
},
|
||||
)
|
||||
|
||||
assert _isolated_scalar_cycle_outlier(
|
||||
cycle_offsets, math.radians(0.5)
|
||||
) == {1}
|
||||
failures = _thumb_yaw_zero_repeatability_failures(
|
||||
result,
|
||||
maximum_cycle_range_rad=math.radians(0.5),
|
||||
maximum_confidence_half_width_rad=math.radians(0.75),
|
||||
)
|
||||
assert failures[0]["actual"] == 0.524352
|
||||
assert failures[0]["cycle"] == 2
|
||||
assert failures[0]["inlier_cycles"] == [1, 3]
|
||||
|
||||
|
||||
def test_thumb_yaw_zero_spread_keeps_symmetric_drift_nonlocalized() -> None:
|
||||
cycle_offsets = [
|
||||
math.radians(-2.76),
|
||||
math.radians(-2.50),
|
||||
math.radians(-2.24),
|
||||
]
|
||||
|
||||
assert _isolated_scalar_cycle_outlier(
|
||||
cycle_offsets, math.radians(0.5)
|
||||
) == set()
|
||||
|
||||
|
||||
def test_thumb_yaw_zero_confidence_failure_requests_full_yaw_retry() -> None:
|
||||
result = SimpleNamespace(
|
||||
cycle_offsets_rad={
|
||||
"thumb_cmc_yaw": [
|
||||
math.radians(-2.50),
|
||||
math.radians(-2.70),
|
||||
math.radians(-2.30),
|
||||
]
|
||||
},
|
||||
offset_confidence_half_width_rad={
|
||||
"thumb_cmc_yaw": math.radians(0.80)
|
||||
},
|
||||
)
|
||||
|
||||
assert _thumb_yaw_zero_repeatability_failures(
|
||||
result,
|
||||
maximum_cycle_range_rad=math.radians(0.5),
|
||||
maximum_confidence_half_width_rad=math.radians(0.75),
|
||||
) == [
|
||||
{
|
||||
"joint": "thumb_cmc_yaw",
|
||||
"metric": "zero_confidence_95_half_width_deg",
|
||||
"actual": 0.8,
|
||||
"limit": 0.75,
|
||||
"comparison": "maximum",
|
||||
}
|
||||
]
|
||||
|
||||
|
||||
def test_thumb_yaw_zero_repeatability_accepts_171406_quality() -> None:
|
||||
result = SimpleNamespace(
|
||||
cycle_offsets_rad={
|
||||
"thumb_cmc_yaw": [
|
||||
math.radians(-2.53708),
|
||||
math.radians(-2.69820),
|
||||
math.radians(-2.29168),
|
||||
]
|
||||
},
|
||||
offset_confidence_half_width_rad={
|
||||
"thumb_cmc_yaw": math.radians(0.50853)
|
||||
},
|
||||
)
|
||||
|
||||
assert _thumb_yaw_zero_repeatability_failures(
|
||||
result,
|
||||
maximum_cycle_range_rad=math.radians(0.5),
|
||||
maximum_confidence_half_width_rad=math.radians(0.75),
|
||||
) == []
|
||||
|
||||
|
||||
def test_thumb_yaw_zero_outlier_retries_only_localized_source_cycle(
|
||||
tmp_path,
|
||||
) -> None:
|
||||
spec = next(
|
||||
item
|
||||
for item in RIGHT_19_HAND_PROFILE.sweep_specs
|
||||
if item.key == "thumb_cmc_yaw_top"
|
||||
)
|
||||
failures = _thumb_yaw_zero_repeatability_failures(
|
||||
SimpleNamespace(
|
||||
cycle_offsets_rad={
|
||||
"thumb_cmc_yaw": [
|
||||
math.radians(-2.518678),
|
||||
math.radians(-3.098037),
|
||||
math.radians(-2.544640),
|
||||
]
|
||||
},
|
||||
offset_confidence_half_width_rad={
|
||||
"thumb_cmc_yaw": math.radians(0.81295)
|
||||
},
|
||||
),
|
||||
maximum_cycle_range_rad=math.radians(0.5),
|
||||
maximum_confidence_half_width_rad=math.radians(0.75),
|
||||
)
|
||||
calls: list[str] = []
|
||||
node = SimpleNamespace(
|
||||
profile=RIGHT_19_HAND_PROFILE,
|
||||
raw_path=tmp_path / "raw_samples.jsonl",
|
||||
sweep_attempts={},
|
||||
retry_resume_index=None,
|
||||
retry_sweep_spec=None,
|
||||
retry_cycles=set(),
|
||||
retry_joint_names=set(),
|
||||
fit_failure={},
|
||||
sweep_index=96,
|
||||
repetitions=3,
|
||||
automatic_fit_retry_limit=2,
|
||||
retry_source_failure_task_key=spec.key,
|
||||
retry_source_task_keys=(
|
||||
"thumb_cmc_pitch_front",
|
||||
"thumb_cmc_roll_front",
|
||||
),
|
||||
retry_cycle_override=set(),
|
||||
paused_reason="",
|
||||
reason="",
|
||||
_prepare_failed_sweep_retry=lambda: calls.append("prepare"),
|
||||
_begin_return_baseline=lambda after: calls.append(after),
|
||||
_pause=lambda reason: calls.append(f"pause:{reason}"),
|
||||
)
|
||||
|
||||
handled = G20ThreeCameraCalibrationNode._pause_for_provisional_fit_failure(
|
||||
node, spec, failures
|
||||
)
|
||||
|
||||
assert handled is True
|
||||
assert node.retry_cycles == {1}
|
||||
assert node.retry_joint_names == {
|
||||
"thumb_cmc_pitch",
|
||||
"thumb_cmc_roll",
|
||||
}
|
||||
assert node.fit_failure["cycles_to_rescan"] == [2]
|
||||
assert node.fit_failure["directions_to_rescan"] == 4
|
||||
assert node.fit_failure["source_task_names"] == [
|
||||
"thumb_cmc_pitch_front",
|
||||
"thumb_cmc_roll_front",
|
||||
]
|
||||
assert calls == ["prepare", "resume_sweep"]
|
||||
|
||||
|
||||
def test_previous_passed_joint_zero_offset_reads_formal_pointer(
|
||||
tmp_path,
|
||||
) -> None:
|
||||
root = tmp_path / "G20_RIGHT_001"
|
||||
previous = root / "20260830_171406"
|
||||
current = root / "20260831_100000"
|
||||
previous.mkdir(parents=True)
|
||||
current.mkdir()
|
||||
payload = {
|
||||
"joints": {
|
||||
"thumb_cmc_yaw": {
|
||||
"zero_angles": {"urdf_zero_offset_rad": -0.04736713}
|
||||
}
|
||||
}
|
||||
}
|
||||
(previous / "g20_right_G20_RIGHT_001_calibration.json").write_text(
|
||||
json.dumps(payload), encoding="utf-8"
|
||||
)
|
||||
(root / "latest_passed").symlink_to(previous, target_is_directory=True)
|
||||
|
||||
assert _previous_passed_joint_zero_offset(
|
||||
current, "G20_RIGHT_001", "thumb_cmc_yaw"
|
||||
) == (previous, -0.04736713)
|
||||
|
||||
|
||||
def test_axis_line_spread_localizes_one_outlier_for_four_rounds() -> None:
|
||||
axes = [
|
||||
[-0.043565, 0.036698, -0.998376],
|
||||
|
||||
@@ -30,6 +30,9 @@ from g20_thumb_apriltag_calibration.urdf_zero import (
|
||||
select_cross_view_roll_direction_source,
|
||||
solve_urdf_zero_offsets,
|
||||
get_zero_calibration_profile,
|
||||
get_right_19_thumb_zero_profile,
|
||||
expand_right_19_thumb_zero_result_with_cad_fingers,
|
||||
merge_right_19_thumb_zero_result,
|
||||
write_zero_corrected_urdf,
|
||||
)
|
||||
from g20_thumb_apriltag_calibration.full_hand import (
|
||||
@@ -623,8 +626,10 @@ def _solve_synthetic_offsets(
|
||||
joint_maximum_offset_degrees: dict[str, float] | None = None,
|
||||
validation_offset_bias_degrees: dict[str, float] | None = None,
|
||||
static_output_offsets_degrees: dict[str, float] | None = None,
|
||||
fixed_direct_offsets_degrees: dict[str, float] | None = None,
|
||||
base_euler_xyz_rad: tuple[float, float, float] = (0.5, -0.4, 0.8),
|
||||
base_translation_xyz_m: tuple[float, float, float] = (0.31, -0.19, 0.72),
|
||||
solve_thumb_only: bool = False,
|
||||
):
|
||||
hand = get_hand_calibration_profile(side, layout_id)
|
||||
zero = get_zero_calibration_profile(side, layout_id)
|
||||
@@ -803,9 +808,14 @@ def _solve_synthetic_offsets(
|
||||
rotation_orthogonal_rms_rad=math.radians(0.1),
|
||||
)
|
||||
)
|
||||
solve_profile = (
|
||||
get_right_19_thumb_zero_profile() if solve_thumb_only else zero
|
||||
)
|
||||
result = solve_urdf_zero_offsets(
|
||||
source_urdf=source,
|
||||
measurements=measurements,
|
||||
measurements=[
|
||||
item for item in measurements if item.joint in solve_profile.axis_joints
|
||||
],
|
||||
palm_orientation_measurements=palm_orientation_measurements,
|
||||
curves=curves,
|
||||
motor_by_joint=motor_by_joint,
|
||||
@@ -828,6 +838,16 @@ def _solve_synthetic_offsets(
|
||||
name: math.radians(value)
|
||||
for name, value in (static_output_offsets_degrees or {}).items()
|
||||
},
|
||||
fixed_direct_zero_offsets_rad=(
|
||||
None
|
||||
if fixed_direct_offsets_degrees is None
|
||||
else {
|
||||
name: math.radians(value)
|
||||
for name, value in fixed_direct_offsets_degrees.items()
|
||||
if name in solve_profile.direct_zero_joints
|
||||
}
|
||||
),
|
||||
zero_profile=solve_profile,
|
||||
)
|
||||
return zero, result
|
||||
|
||||
@@ -884,6 +904,128 @@ def test_right_19_solver_recovers_visual_targets_when_no_endpoint_anchor_is_supp
|
||||
assert set(result.offset_covariance_rad2) == set(zero.direct_zero_joints)
|
||||
|
||||
|
||||
def test_right_19_thumb_kernel_is_independent_and_matches_full_hand() -> None:
|
||||
injected = [
|
||||
2.0, -3.0, 4.0, -1.5,
|
||||
1.0, -1.0, 0.7,
|
||||
0.8, -0.7, -0.8,
|
||||
-0.5, 0.6, 0.9,
|
||||
1.1, -1.0, -0.6,
|
||||
]
|
||||
_, full = _solve_synthetic_offsets(
|
||||
"right",
|
||||
injected,
|
||||
layout_id="g20_right_19",
|
||||
fixed_direct_offsets_degrees={"thumb_mcp": -1.5},
|
||||
static_output_offsets_degrees={"thumb_cmc_roll": 2.0},
|
||||
)
|
||||
_, thumb = _solve_synthetic_offsets(
|
||||
"right",
|
||||
injected,
|
||||
layout_id="g20_right_19",
|
||||
solve_thumb_only=True,
|
||||
fixed_direct_offsets_degrees={"thumb_mcp": -1.5},
|
||||
static_output_offsets_degrees={"thumb_cmc_roll": 2.0},
|
||||
)
|
||||
|
||||
assert thumb.passed is True
|
||||
assert set(thumb.direct_offsets_rad) == {
|
||||
"thumb_cmc_roll",
|
||||
"thumb_cmc_yaw",
|
||||
"thumb_cmc_pitch",
|
||||
"thumb_mcp",
|
||||
}
|
||||
for name in thumb.direct_offsets_rad:
|
||||
assert thumb.direct_offsets_rad[name] == pytest.approx(
|
||||
full.direct_offsets_rad[name], abs=math.radians(0.05)
|
||||
)
|
||||
|
||||
finger_offsets = {
|
||||
name: full.direct_offsets_rad[name]
|
||||
for name in get_zero_calibration_profile(
|
||||
"right", "g20_right_19"
|
||||
).direct_zero_joints
|
||||
if not name.startswith("thumb_")
|
||||
}
|
||||
thumb_only = merge_right_19_thumb_zero_result(
|
||||
thumb_result=thumb,
|
||||
preserved_offsets_rad=finger_offsets,
|
||||
)
|
||||
full_hand = merge_right_19_thumb_zero_result(
|
||||
thumb_result=thumb,
|
||||
companion_result=full,
|
||||
)
|
||||
for name in thumb.direct_offsets_rad:
|
||||
assert thumb_only.direct_offsets_rad[name] == pytest.approx(
|
||||
full_hand.direct_offsets_rad[name], abs=1.0e-12
|
||||
)
|
||||
standalone = expand_right_19_thumb_zero_result_with_cad_fingers(thumb)
|
||||
assert all(
|
||||
standalone.direct_offsets_rad[name] == 0.0
|
||||
for name in standalone.direct_offsets_rad
|
||||
if not name.startswith("thumb_")
|
||||
)
|
||||
|
||||
|
||||
def test_thumb_endpoint_output_uses_published_offset_safety_limit() -> None:
|
||||
# The visual roll scalar may carry a root-frame gauge beyond the global
|
||||
# limit. A separately validated mechanical endpoint is the value written
|
||||
# to the URDF and therefore the value the configured safety gate guards.
|
||||
injected = [
|
||||
21.0, -3.0, 4.0, -1.5,
|
||||
1.0, -1.0, 0.7,
|
||||
0.8, -0.7, -0.8,
|
||||
-0.5, 0.6, 0.9,
|
||||
1.1, -1.0, -0.6,
|
||||
]
|
||||
|
||||
_, result = _solve_synthetic_offsets(
|
||||
"right",
|
||||
injected,
|
||||
layout_id="g20_right_19",
|
||||
solve_thumb_only=True,
|
||||
fixed_direct_offsets_degrees={"thumb_mcp": -1.5},
|
||||
static_output_offsets_degrees={"thumb_cmc_roll": 3.45},
|
||||
)
|
||||
|
||||
assert result.passed is True
|
||||
assert "thumb_cmc_roll" not in result.failure_reasons
|
||||
assert math.degrees(
|
||||
result.direct_offsets_rad["thumb_cmc_roll"]
|
||||
) == pytest.approx(3.45, abs=0.01)
|
||||
|
||||
|
||||
def test_right_19_thumb_kernel_is_invariant_to_rigid_hand_movement() -> None:
|
||||
injected = [
|
||||
2.0, -3.0, 4.0, -1.5,
|
||||
1.0, -1.0, 0.7,
|
||||
0.8, -0.7, -0.8,
|
||||
-0.5, 0.6, 0.9,
|
||||
1.1, -1.0, -0.6,
|
||||
]
|
||||
arguments = {
|
||||
"layout_id": "g20_right_19",
|
||||
"solve_thumb_only": True,
|
||||
"fixed_direct_offsets_degrees": {"thumb_mcp": -1.5},
|
||||
"static_output_offsets_degrees": {"thumb_cmc_roll": 2.0},
|
||||
}
|
||||
_, first = _solve_synthetic_offsets("right", injected, **arguments)
|
||||
_, moved = _solve_synthetic_offsets(
|
||||
"right",
|
||||
injected,
|
||||
base_euler_xyz_rad=(-0.2, 0.7, -0.35),
|
||||
base_translation_xyz_m=(-0.15, 0.42, 0.55),
|
||||
**arguments,
|
||||
)
|
||||
|
||||
assert first.passed is True
|
||||
assert moved.passed is True
|
||||
for name in first.direct_offsets_rad:
|
||||
assert moved.direct_offsets_rad[name] == pytest.approx(
|
||||
first.direct_offsets_rad[name], abs=math.radians(0.01)
|
||||
)
|
||||
|
||||
|
||||
def test_right_19_independent_roll_output_cannot_change_yaw_solution() -> None:
|
||||
injected = [
|
||||
2.0, -3.0, 4.0, -1.5,
|
||||
@@ -919,6 +1061,7 @@ def _partial_orientation_records(
|
||||
common_rotation: Rotation,
|
||||
maximum_angle_deg: float = 30.0,
|
||||
tag_offset_xyz_m: tuple[float, float, float] = (0.01, 0.02, -0.015),
|
||||
include_child_pose_common: bool = False,
|
||||
) -> list[dict[str, object]]:
|
||||
axis_parent = np.asarray([0.0, 1.0, 0.0])
|
||||
parent_pose = {
|
||||
@@ -942,8 +1085,7 @@ def _partial_orientation_records(
|
||||
).apply(tag_offset_xyz_m)
|
||||
state = [255.0] * 20
|
||||
state[1] = float(command)
|
||||
records.append(
|
||||
{
|
||||
record: dict[str, object] = {
|
||||
"cycle": 0,
|
||||
"direction": direction,
|
||||
"command_u8": command,
|
||||
@@ -954,7 +1096,16 @@ def _partial_orientation_records(
|
||||
"parent_pose_common": parent_pose,
|
||||
"state_u8": state,
|
||||
}
|
||||
)
|
||||
if include_child_pose_common:
|
||||
child_common = common_rotation * relative
|
||||
record["child_pose_common"] = {
|
||||
"translation_xyz_m": list(
|
||||
np.asarray(parent_pose["translation_xyz_m"])
|
||||
+ common_rotation.apply(relative_translation)
|
||||
),
|
||||
"quaternion_xyzw": list(child_common.as_quat()),
|
||||
}
|
||||
records.append(record)
|
||||
return records
|
||||
|
||||
|
||||
@@ -1083,6 +1234,105 @@ def test_partial_palm_direction_ignores_far_stroke_pnp_bias() -> None:
|
||||
) == pytest.approx(1.0, abs=1.0e-10)
|
||||
|
||||
|
||||
def test_partial_palm_direction_uses_robust_full_stroke_local_motion() -> None:
|
||||
common_rotation = Rotation.from_euler("xyz", [0.35, -0.2, 0.6])
|
||||
records = _partial_orientation_records(
|
||||
tag_mount=Rotation.from_euler("xyz", [0.2, -0.1, 0.3]),
|
||||
common_rotation=common_rotation,
|
||||
maximum_angle_deg=48.0,
|
||||
include_child_pose_common=True,
|
||||
)
|
||||
expected = fit_partial_palm_orientation_measurement(
|
||||
"thumb_cmc_roll_top_axis",
|
||||
"thumb_cmc_roll",
|
||||
records,
|
||||
cycle=0,
|
||||
zero_command_u8=255,
|
||||
)
|
||||
|
||||
biased: list[dict[str, object]] = []
|
||||
for source in records:
|
||||
record = dict(source)
|
||||
pose = dict(record["child_pose_common"])
|
||||
# Simulate a command-correlated planar-PnP branch curvature affecting
|
||||
# fewer than one quarter of the full-stroke local increments. The
|
||||
# zero-adjacent quality trajectory remains unchanged.
|
||||
if int(record["command_u8"]) <= 187:
|
||||
rotation = Rotation.from_quat(pose["quaternion_xyzw"])
|
||||
amount = math.radians(8.0) * (
|
||||
(187 - int(record["command_u8"])) / 12.0
|
||||
)
|
||||
pose["quaternion_xyzw"] = list(
|
||||
(
|
||||
Rotation.from_rotvec([amount, 0.0, 0.0]) * rotation
|
||||
).as_quat()
|
||||
)
|
||||
record["child_pose_common"] = pose
|
||||
biased.append(record)
|
||||
|
||||
fitted = fit_partial_palm_orientation_measurement(
|
||||
"thumb_cmc_roll_top_axis",
|
||||
"thumb_cmc_roll",
|
||||
biased,
|
||||
cycle=0,
|
||||
zero_command_u8=255,
|
||||
)
|
||||
difference = math.acos(
|
||||
abs(
|
||||
float(
|
||||
np.clip(
|
||||
np.dot(expected.axis_common_xyz, fitted.axis_common_xyz),
|
||||
-1.0,
|
||||
1.0,
|
||||
)
|
||||
)
|
||||
)
|
||||
)
|
||||
|
||||
assert expected.axis_estimator == "robust_full_stroke_local_so3_v1"
|
||||
assert expected.incremental_pair_count >= 24
|
||||
assert fitted.axis_estimator == expected.axis_estimator
|
||||
assert difference < math.radians(0.25)
|
||||
|
||||
|
||||
def test_full_stroke_palm_axis_tracks_reestablished_common_frame() -> None:
|
||||
tag_mount = Rotation.from_euler("xyz", [0.2, -0.1, 0.3])
|
||||
first_common = Rotation.from_euler("xyz", [0.35, -0.2, 0.6])
|
||||
repositioning = Rotation.from_euler("xyz", [-0.18, 0.27, -0.41])
|
||||
second_common = repositioning * first_common
|
||||
first = fit_partial_palm_orientation_measurement(
|
||||
"thumb_cmc_roll_top_axis",
|
||||
"thumb_cmc_roll",
|
||||
_partial_orientation_records(
|
||||
tag_mount=tag_mount,
|
||||
common_rotation=first_common,
|
||||
maximum_angle_deg=48.0,
|
||||
include_child_pose_common=True,
|
||||
),
|
||||
cycle=0,
|
||||
zero_command_u8=255,
|
||||
)
|
||||
second = fit_partial_palm_orientation_measurement(
|
||||
"thumb_cmc_roll_top_axis",
|
||||
"thumb_cmc_roll",
|
||||
_partial_orientation_records(
|
||||
tag_mount=tag_mount,
|
||||
common_rotation=second_common,
|
||||
maximum_angle_deg=48.0,
|
||||
include_child_pose_common=True,
|
||||
),
|
||||
cycle=0,
|
||||
zero_command_u8=255,
|
||||
)
|
||||
expected_second = repositioning.apply(first.axis_common_xyz)
|
||||
|
||||
assert first.axis_estimator == "robust_full_stroke_local_so3_v1"
|
||||
assert second.axis_estimator == first.axis_estimator
|
||||
assert abs(float(np.dot(expected_second, second.axis_common_xyz))) == (
|
||||
pytest.approx(1.0, abs=1.0e-8)
|
||||
)
|
||||
|
||||
|
||||
def test_right_19_top_axis_pair_holdout_ignores_shared_frame_bias() -> None:
|
||||
injected = [0.0] * 16
|
||||
_, result = _solve_synthetic_offsets(
|
||||
@@ -1431,6 +1681,36 @@ def test_right_19_holdout_never_changes_frozen_training_offsets() -> None:
|
||||
assert result.validation_cycle not in result.training_cycles
|
||||
|
||||
|
||||
def test_failed_thumb_holdout_does_not_blame_fixed_partial_scope_zeros() -> None:
|
||||
zero = get_zero_calibration_profile("right", "g20_right_19")
|
||||
injected = [
|
||||
2.0, -3.0, 4.0, -1.5,
|
||||
1.0, -1.0, 0.7,
|
||||
0.8, -0.7, -0.8,
|
||||
-0.5, 0.6, 0.9,
|
||||
1.1, -1.0, -0.6,
|
||||
]
|
||||
frozen = {
|
||||
name: injected[index]
|
||||
for index, name in enumerate(zero.direct_zero_joints)
|
||||
if not name.startswith("thumb_")
|
||||
}
|
||||
|
||||
_, result = _solve_synthetic_offsets(
|
||||
"right",
|
||||
injected,
|
||||
layout_id="g20_right_19",
|
||||
validation_offset_bias_degrees={"thumb_cmc_yaw": 8.0},
|
||||
fixed_direct_offsets_degrees=frozen,
|
||||
)
|
||||
|
||||
assert result.passed is False
|
||||
assert result.failure_reasons == {
|
||||
"palm_orientation": "palm_orientation_holdout_too_large"
|
||||
}
|
||||
assert set(frozen).isdisjoint(result.failure_reasons)
|
||||
|
||||
|
||||
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")
|
||||
|
||||
Reference in New Issue
Block a user