大拇指单独标定,yaw正确和稳定性修改

This commit is contained in:
lxp
2026-08-31 17:17:39 +08:00
parent 4dadfb954b
commit 286581bcba
15 changed files with 3351 additions and 206 deletions
+43 -6
View File
@@ -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",
}
),
)
@@ -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",
@@ -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
@@ -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"
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")