大拇指零位正确性稳定性修改

This commit is contained in:
lxp
2026-08-30 17:35:23 +08:00
parent 83c69b48c2
commit 4dadfb954b
19 changed files with 1478 additions and 219 deletions
@@ -8,6 +8,26 @@
ros2 run g20_thumb_apriltag_calibration calibrate_g20_right
```
只重新采集大拇指的4项任务、四指沿用一份已经正式通过的完整会话时,使用:
```bash
ros2 run g20_thumb_apriltag_calibration calibrate_g20_right \
--scope thumb \
--base-session calibration_output/G20_RIGHT_001/latest_passed
```
采样文件中的运动域是显式且不可混用的:
`requested_command_u8` 表示下发命令,`feedback_u8` 表示电机反馈。
在线拟合和离线重放通过同一个数据契约投影到曲线索引;新会话不会把含糊的
`command_u8` 写入 `raw_samples.jsonl`。基础会话导入期间状态会显示为
`IMPORTING_BASE``REVALIDATING_INHERITED`,完成复核后才允许机械手运动。
`--base-session`必须解析到同一序列号目录下的完整PASS会话;启动前会校验源CAD
URDF、相机外参和标定配置哈希。新会话从原始CAD重新生成完整URDF,不在旧校准
URDF上叠加。该模式只重采`thumb_cmc_pitch``thumb_cmc_roll`
`thumb_cmc_yaw``thumb_mcp/thumb_ip`四项物理任务,其余任务的原始记录导入后仍按
当前硬门限重新验证。
开发阶段若上一次会话失败,同一命令会自动校验硬件/几何哈希,并恢复已经
完整提交的关节任务;失败中的当前任务始终丢弃重做,位于它后面但已经完整通过的
独立任务仍会复用,不再因“连续前缀”限制整段重采。导入的任务会立即用与
@@ -24,7 +24,7 @@ artifacts:
source_urdf: src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/linkerhand_g20_right.urdf
source_urdf_sha256: eeb6ffb0e95d2a6acd4c26331ae68062e0d74160de4b552b4f6d395cce5ca4e8
camera_extrinsics: config/g20_three_camera_extrinsics.yaml
camera_extrinsics_sha256: 59ab7510ad0a2912ca636876039427c4c25906cbbfa84e96300d46b86e929334
camera_extrinsics_sha256: dd623572df3cb83fdefcbe92204dab54a60f2c68eb3a8c9bdb08407e8f0e5d80
calibration_config: src/g20_thumb_apriltag_calibration/config/three_camera_calibration.yaml
calibration_config_sha256: 8fa16814d7a3462a00aae4eadc273e66bc167bedd5908b0fed40ce6d505b2aeb
tag_config: src/g20_thumb_apriltag_calibration/config/three_camera_tags_g20_right_19.yaml
@@ -119,6 +119,8 @@ class HandCalibrationProfile:
axis_validation_sources: Mapping[str, str] | None = None
palm_axis_observers: tuple[PalmAxisObserver, ...] = ()
minimum_palm_orientation_sources: int = 0
palm_orientation_minimum_arc_rad: float = math.radians(15.0)
palm_orientation_maximum_command_distance_u8: int = 255
# Hardware and algorithm differences belong to the product profile, not
# to the acquisition state machine. Keeping these fields here lets a new
# model (or the mirrored hand) reuse the same recorder/fitter by supplying
@@ -662,8 +664,32 @@ def _build_right_19_profile() -> HandCalibrationProfile:
layout_id=G20_RIGHT_19_LAYOUT,
measurement_specs=measurement_specs,
axis_validation_sources=axis_validation_sources,
palm_axis_observers=(),
minimum_palm_orientation_sources=0,
# Observe the zero-adjacent roll and pitch axes from Tag 9 while the
# existing front sweeps run. Their same-view angle identifies the
# intervening yaw zero without adding a motion task or reference URDF.
palm_axis_observers=(
PalmAxisObserver(
source_name="thumb_cmc_pitch_top_axis",
task_name="thumb_cmc_pitch_front",
view="top",
parent_role="top_base",
child_role="thumb_yaw",
model_joint="thumb_cmc_pitch",
motor_index=joint_specs["thumb_cmc_pitch"].motor_index,
),
PalmAxisObserver(
source_name="thumb_cmc_roll_top_axis",
task_name="thumb_cmc_roll_front",
view="top",
parent_role="top_base",
child_role="thumb_yaw",
model_joint="thumb_cmc_roll",
motor_index=joint_specs["thumb_cmc_roll"].motor_index,
),
),
minimum_palm_orientation_sources=2,
palm_orientation_minimum_arc_rad=math.radians(10.0),
palm_orientation_maximum_command_distance_u8=64,
capabilities=frozenset(
{
"precheck_sweeps",
@@ -674,6 +700,7 @@ def _build_right_19_profile() -> HandCalibrationProfile:
"stable_cross_view_cone_bias",
"measured_passive_dips",
"urdf_zero_publication",
"palm_axis_side_channel_v2",
}
),
)
@@ -37,9 +37,11 @@ from .full_hand import (
validate_compact_payload,
)
from .product import get_product_calibration_contract
from .sample_schema import fitting_sample_record, fitting_sample_records
from .storage import atomic_write_json
from .urdf_zero import (
JointAxisMeasurement,
RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS,
UrdfKinematicModel,
_angles_from_state,
anchor_right_19_mechanical_endpoint_curves,
@@ -55,6 +57,8 @@ from .urdf_zero import (
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,
select_cross_view_roll_direction_source,
solve_urdf_zero_offsets,
with_depth_free_axis_projection,
@@ -92,25 +96,12 @@ def _steady_records_in_feedback_domain(
records: Sequence[Mapping[str, Any]],
) -> list[dict[str, Any]]:
"""Mirror the live mechanical-backlash domain for offline replay."""
result: list[dict[str, Any]] = []
for source in records:
record = dict(source)
requested = int(
record.get("requested_command_u8", record.get("command_u8", -1))
)
record["command_u8"] = (
requested
if requested in {0, 255}
else int(
np.clip(
np.rint(float(record.get("feedback_u8", requested))),
0,
255,
)
)
)
result.append(record)
return result
return fitting_sample_records(
records,
domain="feedback",
allow_legacy_command=True,
snap_requested_endpoints=True,
)
def _latest_attempt_records(
@@ -195,19 +186,20 @@ def _load_raw_session(
for line in raw_path.read_text(encoding="utf-8").splitlines()
if line.strip()
]
# New sessions persist both domains explicitly. The fitting primitives
# retain their historical command_u8 input name internally, so normalise
# only after loading; raw_samples.jsonl itself never contains an ambiguous
# command_u8 field.
# Both online fitting and replay use the same canonical-to-fit projection.
# raw_samples.jsonl itself never contains the ambiguous compatibility key.
for row in rows:
if "command_u8" in row:
continue
if row.get("kind") in {
"sample", "baseline_hold_sample", "palm_axis_sample"
"sample",
"baseline_hold_sample",
"steady_command_sample",
"palm_axis_sample",
}:
row["command_u8"] = int(round(float(row["feedback_u8"])))
elif row.get("kind") == "steady_command_sample":
row["command_u8"] = int(row["requested_command_u8"])
projected = fitting_sample_record(
row, allow_legacy_command=True
)
row.clear()
row.update(projected)
starts = [row for row in rows if row.get("kind") == "session_start"]
if len(starts) != 1:
raise ValueError("raw session must contain exactly one session_start")
@@ -1247,8 +1239,20 @@ def replay_session(
)
command_fits = anchor_right_19_mechanical_endpoint_curves(
command_fits,
command_records_by_joint,
{
name: (
[
record
for record in curve_records_by_joint[name]
if int(record["cycle"]) in training_cycle_set
]
if name == "thumb_cmc_roll"
else command_records_by_joint[name]
)
for name in RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS
},
maximum_direction_difference_rad=command_gap_limit,
feedback_endpoint_joints=frozenset({"thumb_cmc_roll"}),
)
cross_view_roll_metrics: dict[str, dict[str, float]] = {}
for name, validation_name in (
@@ -1304,8 +1308,8 @@ def replay_session(
minimum_sources=int(
profile.minimum_palm_orientation_sources
),
minimum_arc_rad=math.radians(
float(parameters["trajectory_minimum_arc_deg"])
minimum_arc_rad=(
profile.palm_orientation_minimum_arc_rad
),
maximum_rotation_orthogonal_rms_rad=math.radians(
float(
@@ -1314,6 +1318,9 @@ def replay_session(
]
)
),
maximum_command_distance_u8=(
profile.palm_orientation_maximum_command_distance_u8
),
)
)
failures = _quality_failures(
@@ -1435,7 +1442,7 @@ def replay_session(
motor_by_joint = {
name: int(spec.motor_index) for name, spec in profile.joint_specs.items()
}
endpoint_zero_offsets = (
measured_endpoint_zero_offsets = (
derive_right_19_mechanical_endpoint_offsets(
source_urdf,
command_fits,
@@ -1450,6 +1457,16 @@ def replay_session(
if layout_id == G20_RIGHT_19_LAYOUT
else {}
)
endpoint_zero_offsets = {
name: value
for name, value in measured_endpoint_zero_offsets.items()
if name in RIGHT_19_MECHANICAL_ENDPOINT_JOINTS
}
post_solve_endpoint_offsets = {
name: value
for name, value in measured_endpoint_zero_offsets.items()
if name in RIGHT_19_POST_SOLVE_ENDPOINT_JOINTS
}
command_fits = clamp_runtime_fits_to_urdf_limits(
source_urdf,
command_fits,
@@ -1515,6 +1532,10 @@ def replay_session(
**zero_profile.fixed_direct_zero_offsets_rad,
**endpoint_zero_offsets,
},
"static_output_zero_offsets_rad": {
**zero_profile.static_output_zero_offsets_rad,
**post_solve_endpoint_offsets,
},
}
holdout_zero = solve_urdf_zero_offsets(curves=training_fits, **solve_arguments)
if not holdout_zero.passed:
@@ -1611,12 +1632,15 @@ def replay_session(
# differ slightly at settled checkpoints; using them here creates
# a fictitious residual zero after a structurally correct write.
curves=training_fits,
static_output_zero_offsets_rad={
name: 0.0
for name in zero_profile.static_output_zero_offsets_rad
},
**{
**solve_arguments,
"static_output_zero_offsets_rad": {
name: 0.0
for name in (
set(zero_profile.static_output_zero_offsets_rad)
| set(post_solve_endpoint_offsets)
)
},
"source_urdf": candidate,
"fixed_direct_zero_offsets_rad": {
name: 0.0
@@ -1762,7 +1786,10 @@ def replay_session(
},
"static_output_zero_offsets_deg": {
name: math.degrees(value)
for name, value in zero_profile.static_output_zero_offsets_rad.items()
for name, value in {
**zero_profile.static_output_zero_offsets_rad,
**post_solve_endpoint_offsets,
}.items()
},
"direct_offsets_deg": {
name: math.degrees(value)
@@ -153,6 +153,7 @@ def _launch_command(
session: Path,
*,
resume_from: Path | None = None,
recalibration_scope: str = "full",
) -> list[str]:
values = {
"model": config.model,
@@ -173,6 +174,7 @@ def _launch_command(
"start_sdk": "true",
"record_bag": "false",
"validation_enabled": "false",
"recalibration_scope": recalibration_scope,
}
if resume_from is not None:
values["resume_raw_samples_path"] = str(
@@ -235,6 +237,7 @@ def _run_hardware_session(
session: Path,
*,
resume_from: Path | None = None,
recalibration_scope: str = "full",
) -> tuple[dict[str, Any], int]:
session.mkdir(parents=True, exist_ok=False)
(session / "raw_samples.jsonl").touch()
@@ -254,7 +257,12 @@ def _run_hardware_session(
exit_code = EXIT_QUALITY
try:
process = subprocess.Popen(
_launch_command(config, session, resume_from=resume_from),
_launch_command(
config,
session,
resume_from=resume_from,
recalibration_scope=recalibration_scope,
),
cwd=config.workspace,
stdout=log_stream,
stderr=subprocess.STDOUT,
@@ -488,12 +496,66 @@ def _automatic_resume_candidate(config: ProductConfig) -> Path | None:
return None
def _resolve_partial_base_session(
config: ProductConfig, base_session: str | Path | None
) -> Path:
"""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 "
"complete G20 right session"
)
candidate = Path(base_session).expanduser().resolve(strict=True)
root = config.session_root.resolve()
if candidate.parent != root or not candidate.is_dir():
raise ValueError(
"base session must resolve to a direct session directory under "
f"{root}"
)
raw_path = candidate / "raw_samples.jsonl"
summary_path = candidate / "calibration_summary_zh.json"
payload_path = (
candidate
/ f"g20_right_{config.serial_number}_calibration.json"
)
for required in (raw_path, summary_path, payload_path):
if not required.is_file():
raise ValueError(f"base session is missing required artifact: {required}")
try:
summary = json.loads(summary_path.read_text(encoding="utf-8"))
except json.JSONDecodeError as error:
raise ValueError("base session summary is invalid JSON") from error
if (
not isinstance(summary, dict)
or summary.get("result") != "PASS"
or not bool(summary.get("quality", {}).get("passed"))
):
raise ValueError("base session is not a formally passed session")
hashes = summary.get("hashes", {})
expected_hashes = {
"source_urdf_sha256": config.source_urdf_sha256,
"camera_extrinsics_sha256": config.camera_extrinsics_sha256,
"calibration_config_sha256": config.calibration_config_sha256,
}
if not isinstance(hashes, Mapping) or any(
str(hashes.get(name, "")) != expected
for name, expected in expected_hashes.items()
):
raise ValueError(
"base session source URDF, camera extrinsics or calibration "
"configuration differs from the current product"
)
return candidate
def run(
config_path: str | Path,
*,
workspace: str | Path | None = None,
preflight_only: bool = False,
allow_resume: bool = True,
scope: str = "full",
base_session: str | Path | None = None,
) -> int:
path = Path(config_path).expanduser().resolve()
try:
@@ -501,6 +563,16 @@ def run(
# process to start. A second load enables the real CAN existence gate.
config = load_product_config(path, workspace=workspace, check_can=False)
load_product_config(path, workspace=workspace, check_can=True)
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 == "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)
)
except BaseException as error:
print(_startup_failure_block(path, error), flush=True)
return EXIT_QUALITY
@@ -510,14 +582,23 @@ def run(
config.session_root.mkdir(parents=True, exist_ok=True)
resume_candidate = (
_automatic_resume_candidate(config) if allow_resume else None
partial_base
if selected_scope == "thumb"
else (_automatic_resume_candidate(config) if allow_resume else None)
)
if resume_candidate is not None:
print(
"检测到兼容的失败会话,将恢复已完整通过的关节任务:"
f"{resume_candidate.name}。失败中的当前任务会从头重做。",
flush=True,
)
if selected_scope == "thumb":
print(
"拇指专项标定:四指任务继承自已通过会话 "
f"{resume_candidate.name}4项拇指任务将全部重新采集。",
flush=True,
)
else:
print(
"检测到兼容的失败会话,将恢复已完整通过的关节任务:"
f"{resume_candidate.name}。失败中的当前任务会从头重做。",
flush=True,
)
maximum_sessions = config.required_independent_passes
for pass_index in range(maximum_sessions):
stamp = datetime.now().strftime("%Y%m%d_%H%M%S")
@@ -531,7 +612,12 @@ def run(
status, code = _run_hardware_session(
config,
session,
resume_from=(resume_candidate if pass_index == 0 else None),
resume_from=(
resume_candidate
if selected_scope == "thumb" or pass_index == 0
else None
),
recalibration_scope=selected_scope,
)
if code != EXIT_PASS:
return code
@@ -569,6 +655,17 @@ def main(args: list[str] | None = None) -> None:
parser.add_argument("--config", default=str(_default_product_config()))
parser.add_argument("--workspace", default=None)
parser.add_argument("--preflight-only", action="store_true")
parser.add_argument(
"--scope",
choices=("full", "thumb"),
default="full",
help="full重新标定全手;thumb仅重采4项拇指任务",
)
parser.add_argument(
"--base-session",
default=None,
help="thumb模式继承四指数据的已通过完整会话目录",
)
parser.add_argument(
"--no-resume",
action="store_true",
@@ -587,6 +684,8 @@ def main(args: list[str] | None = None) -> None:
workspace=arguments.workspace,
preflight_only=arguments.preflight_only,
allow_resume=not arguments.no_resume,
scope=arguments.scope,
base_session=arguments.base_session,
)
finally:
if rclpy.ok():
@@ -161,8 +161,18 @@ def render_progress_zh(
+ "(正面+侧面同步)"
)
if not joint:
import_progress = status.get("base_import", {})
import_fraction = (
float(import_progress.get("fraction", 0.0))
if isinstance(import_progress, Mapping)
else 0.0
)
joint = str(active.get("label_zh", "")) or (
"等待设备" if state in {"PREFLIGHT", "WAIT_START"} else "全手"
f"导入基础会话 {100.0 * import_fraction:.1f}%"
if state in {"IMPORTING_BASE", "REVALIDATING_INHERITED"}
else (
"等待设备" if state in {"PREFLIGHT", "WAIT_START"} else "全手"
)
)
direction = {
"decreasing": "递减",
@@ -337,6 +347,13 @@ def classify_error(reason: str, status: Mapping[str, Any]) -> tuple[str, str, st
)
if value.startswith("CFG-"):
return value, "产品配置或文件预检失败", "不要移动相机;复制本诊断块给开发者。"
if value.startswith("DATA-CONTRACT-"):
return (
value.split(":", 1)[0],
"标定样本的请求命令与反馈值数据契约不完整",
"机械手无需重新采集;保留原始会话并用离线重放验证,"
"复制契约错误给开发者。",
)
if value.startswith("CAM-STATUS-202"):
return (
"CAM-STATUS-202",
@@ -579,6 +579,26 @@ def finalize_session_artifacts(
if not isinstance(combination, Mapping):
raise ValueError("node status is missing combination validation")
_verify_combination_validation(combination)
resume = node_status.get("resume", {})
if not isinstance(resume, Mapping):
raise ValueError("node status has invalid resume provenance")
calibration_scope = str(
resume.get("recalibration_scope", "full")
).strip().lower()
if calibration_scope not in {"full", "thumb"}:
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 (
not bool(resume.get("used"))
or not str(resume.get("source_session", ""))
or len(recalibration_tasks) != 4
or any("thumb_" not in name for name in recalibration_tasks)
):
raise ValueError(
"thumb recalibration is missing its passed base-session provenance"
)
offsets = active_offsets(payload)
endpoint_offsets = {
name: offsets[name]
@@ -610,6 +630,13 @@ def finalize_session_artifacts(
"schema_version": 1,
"serial_number": config.serial_number,
"session_id": f"{config.serial_number}_{directory.name}",
"calibration_scope": calibration_scope,
"inherited_base_session": (
None
if calibration_scope == "full"
else str(resume.get("source_session"))
),
"freshly_calibrated_task_keys": list(recalibration_tasks),
"result": "PASS" if release_ready else "PASS_AWAITING_SECOND_SESSION",
"quality": quality,
"runtime_limit_clipped_bins": clipped_runtime_joints,
@@ -0,0 +1,192 @@
"""Canonical command/feedback schema for calibration observations.
The hand command and its measured motor feedback are different physical
domains. Durable samples always retain both. Fitting code may still use the
historical ``command_u8`` key, but it is created only as an explicit projection
of a canonical record at the fitting boundary.
"""
from __future__ import annotations
import math
from typing import Any, Iterable, Literal, Mapping
import numpy as np
SAMPLE_KINDS = frozenset(
{
'sample',
'baseline_hold_sample',
'steady_command_sample',
'palm_axis_sample',
}
)
FitDomain = Literal['default', 'requested', 'feedback']
class SampleDataContractError(ValueError):
"""A calibration observation mixes or omits command domains."""
def _finite_u8(value: Any, field: str, *, integral: bool) -> int | float:
try:
number = float(value)
except (TypeError, ValueError) as error:
raise SampleDataContractError(
f'DATA-CONTRACT-701:{field} must be numeric'
) from error
if not math.isfinite(number) or not 0.0 <= number <= 255.0:
raise SampleDataContractError(
f'DATA-CONTRACT-701:{field} must be finite and in [0, 255]'
)
if integral:
rounded = int(round(number))
if not math.isclose(number, rounded, rel_tol=0.0, abs_tol=1.0e-9):
raise SampleDataContractError(
f'DATA-CONTRACT-701:{field} must be an integer command'
)
return rounded
return number
def explicit_domain_value(
source: Mapping[str, Any], domain: Literal['requested', 'feedback']
) -> int | float:
"""Read and validate one explicitly named domain from any observation."""
field = (
'requested_command_u8' if domain == 'requested' else 'feedback_u8'
)
if field not in source or source[field] is None:
raise SampleDataContractError(
f'DATA-CONTRACT-701:observation is missing explicit {field}'
)
return _finite_u8(source[field], field, integral=domain == 'requested')
def canonical_sample_record(
source: Mapping[str, Any],
*,
allow_legacy_command: bool = False,
) -> dict[str, Any]:
"""Return one durable, unambiguous calibration observation.
``allow_legacy_command`` is restricted to importing historical sessions
and unit fixtures. New online observations must provide both explicit
fields and therefore cannot silently reinterpret ``command_u8``.
"""
record = dict(source)
kind = str(record.get('kind', ''))
if not kind and allow_legacy_command:
# Old in-memory steady-curve fixtures predate durable sample kinds.
# This adapter is never enabled by the new online/import contract.
kind = 'steady_command_sample'
record['kind'] = kind
if kind not in SAMPLE_KINDS:
raise SampleDataContractError(
f'DATA-CONTRACT-701:unsupported calibration sample kind {kind!r}'
)
requested = record.get('requested_command_u8')
feedback = record.get('feedback_u8')
legacy = record.get('command_u8')
if requested is None or feedback is None:
if not allow_legacy_command or legacy is None:
missing = [
name
for name, value in (
('requested_command_u8', requested),
('feedback_u8', feedback),
)
if value is None
]
raise SampleDataContractError(
'DATA-CONTRACT-701:'
f'{kind} is missing explicit {",".join(missing)}'
)
# Historical in-memory records used requested commands for settled
# checkpoints and feedback bins for dense/baseline/palm observations.
if requested is None:
requested = legacy
if feedback is None:
feedback = legacy
record.pop('command_u8', None)
record['requested_command_u8'] = explicit_domain_value(
{'requested_command_u8': requested}, 'requested'
)
record['feedback_u8'] = explicit_domain_value(
{'feedback_u8': feedback}, 'feedback'
)
return record
def fitting_sample_record(
source: Mapping[str, Any],
*,
domain: FitDomain = 'default',
allow_legacy_command: bool = False,
snap_requested_endpoints: bool = False,
) -> dict[str, Any]:
"""Project a canonical sample into the legacy curve-fitter interface."""
record = canonical_sample_record(
source, allow_legacy_command=allow_legacy_command
)
kind = str(record['kind'])
selected = domain
if selected == 'default':
selected = (
'requested' if kind == 'steady_command_sample' else 'feedback'
)
if selected not in {'requested', 'feedback'}:
raise SampleDataContractError(
f'DATA-CONTRACT-701:unsupported fitting domain {domain!r}'
)
requested = int(record['requested_command_u8'])
if selected == 'requested' or (
snap_requested_endpoints and requested in {0, 255}
):
index = requested
else:
index = int(
np.clip(np.rint(float(record['feedback_u8'])), 0, 255)
)
record['command_u8'] = index
return record
def fitting_sample_records(
records: Iterable[Mapping[str, Any]],
*,
domain: FitDomain = 'default',
allow_legacy_command: bool = False,
snap_requested_endpoints: bool = False,
) -> list[dict[str, Any]]:
"""Project several canonical samples into one explicit fitting domain."""
return [
fitting_sample_record(
record,
domain=domain,
allow_legacy_command=allow_legacy_command,
snap_requested_endpoints=snap_requested_endpoints,
)
for record in records
]
def validate_sample_records(
records: Iterable[Mapping[str, Any]],
*,
allow_legacy_command: bool = False,
) -> None:
"""Validate a collection without changing its representation."""
for index, record in enumerate(records):
try:
canonical_sample_record(
record, allow_legacy_command=allow_legacy_command
)
except SampleDataContractError as error:
raise SampleDataContractError(
f'{error};record_index={index}'
) from error
@@ -9,6 +9,8 @@ from typing import Any, Mapping, Sequence
STATE_NAMES_ZH = {
"PREFLIGHT": "设备和标签预检",
"WAIT_START": "等待开始标定",
"IMPORTING_BASE": "正在读取基础标定会话",
"REVALIDATING_INHERITED": "正在复核继承的四指数据",
"RETURN_BASELINE": "正在恢复目标姿态",
"PREPARE_SWEEP": "正在到达扫描起点",
"SWEEP": "正在采集轨迹",
@@ -457,6 +459,13 @@ def three_camera_reason_zh(
if failures
else "方向观测不足"
)
if "thumb_cmc_" in detail:
return (
"拇指CMC yaw无法由顶部Tag 8/9的零位邻近短轨迹稳定确定:"
+ detail,
"保持Tag安装不变;确保顶部Tag 8/9在拇指CMC pitch和roll"
"从零位开始的前1/4行程持续可见后重新标定。",
)
return (
"掌部公共方向无法由至少三根手指的短时正面轨迹稳定确定:"
+ detail,
@@ -80,11 +80,20 @@ from .pnp import (
SquareTagPoseTracker,
)
from .product import get_product_calibration_contract
from .sample_schema import (
SampleDataContractError,
canonical_sample_record,
fitting_sample_record,
fitting_sample_records,
validate_sample_records,
)
from .storage import append_jsonl, append_jsonl_many, atomic_write_json
from .three_camera_diagnostics import render_three_camera_status_text_zh
from .urdf_zero import (
LEFT_ZERO_PROFILE,
RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS,
RIGHT_19_MECHANICAL_ENDPOINT_JOINTS,
RIGHT_19_POST_SOLVE_ENDPOINT_JOINTS,
JointAxisMeasurement,
PalmOrientationMeasurement,
UrdfKinematicModel,
@@ -114,6 +123,8 @@ from .urdf_zero import (
STATE_PREFLIGHT = "PREFLIGHT"
STATE_WAIT_START = "WAIT_START"
STATE_IMPORTING_BASE = "IMPORTING_BASE"
STATE_REVALIDATING_INHERITED = "REVALIDATING_INHERITED"
STATE_RETURN_BASELINE = "RETURN_BASELINE"
STATE_PREPARE_SWEEP = "PREPARE_SWEEP"
STATE_SWEEP = "SWEEP"
@@ -306,7 +317,7 @@ def _palm_axis_resume_policy(
session_start: Mapping[str, Any],
) -> tuple[bool, tuple[str, ...]]:
"""Validate optional palm-axis checkpoint data when a model uses it."""
capability = "palm_axis_side_channel_v1"
capability = "palm_axis_side_channel_v2"
previous = {str(value) for value in session_start.get("capabilities", [])}
if not profile.palm_axis_observers and capability not in profile.capabilities:
return True, ()
@@ -319,9 +330,11 @@ def _palm_axis_resume_policy(
):
raise ValueError("resume palm-axis observer schema differs")
return True, ()
return False, tuple(
observer.task_name for observer in profile.palm_axis_observers
)
# A new side-channel version changes the zero solver's observation model.
# Do not combine its fresh CMC axes with any trajectory captured under the
# previous PnP transaction semantics. This full invalidation happens only
# once; same-version checkpoints keep their normal task-level resume.
return False, tuple(spec.key for spec in profile.sweep_specs)
def _node_profile(node: Any) -> HandCalibrationProfile:
@@ -776,6 +789,39 @@ RESUMABLE_SAMPLE_KINDS = frozenset(
{"sample", "baseline_hold_sample", "steady_command_sample"}
)
RECALIBRATION_SCOPES = frozenset({"full", "thumb"})
def recalibration_task_keys(
profile: HandCalibrationProfile, scope: str
) -> tuple[str, ...]:
"""Return tasks that must be freshly acquired for a partial session."""
selected = str(scope).strip().lower()
if selected not in RECALIBRATION_SCOPES:
raise ValueError(
"recalibration_scope must be one of: "
+ ", ".join(sorted(RECALIBRATION_SCOPES))
)
if selected == "full":
return ()
if (
profile.side != "right"
or profile.layout_id != G20_RIGHT_19_LAYOUT
):
raise ValueError(
"thumb recalibration is supported only for the G20 right 19-Tag product"
)
tasks = tuple(
spec.key
for spec in profile.sweep_specs
if any(str(name).startswith("thumb_") for name in spec.joints)
)
if len(tasks) != 4:
raise ValueError(
"G20 right thumb recalibration must resolve exactly four tasks"
)
return tasks
def _normalise_legacy_split_roll_resume_rows(
profile: HandCalibrationProfile,
@@ -1140,6 +1186,13 @@ def _file_sha256(path: str | Path) -> str:
return digest.hexdigest()
def _publish_import_status(node: Any) -> None:
"""Publish synchronous import progress when running inside a ROS node."""
publisher = getattr(node, "_publish_status", None)
if callable(publisher):
publisher(time.monotonic())
def _steady_records_in_feedback_domain(
records: Sequence[Mapping[str, Any]],
) -> list[dict[str, Any]]:
@@ -1151,25 +1204,12 @@ def _steady_records_in_feedback_domain(
already passed their motor-specific reach gate, so they are snapped to
the exact endpoints required by the curve fitter.
"""
result: list[dict[str, Any]] = []
for source in records:
record = dict(source)
requested = int(
record.get("requested_command_u8", record.get("command_u8", -1))
)
if requested in {0, 255}:
feedback_index = requested
else:
feedback_index = int(
np.clip(
np.rint(float(record.get("feedback_u8", requested))),
0,
255,
)
)
record["command_u8"] = feedback_index
result.append(record)
return result
return fitting_sample_records(
records,
domain="feedback",
allow_legacy_command=True,
snap_requested_endpoints=True,
)
def _classify_cross_view_roll_hysteresis(
@@ -1822,6 +1862,8 @@ class G20ThreeCameraCalibrationNode(Node):
self.state = STATE_PREFLIGHT
self.reason = "waiting_for_devices_and_sdk"
self.sample_schema_version = 1
self.base_import_progress: dict[str, Any] = {}
self.paused_reason = ""
self.started = False
# Startup has two deliberately separate gates. Camera/SDK health is
@@ -1999,6 +2041,7 @@ class G20ThreeCameraCalibrationNode(Node):
self.declare_parameter("serial_number", "UNSET")
self.declare_parameter("session_dir", "calibration_output/session")
self.declare_parameter("resume_raw_samples_path", "")
self.declare_parameter("recalibration_scope", "full")
self.declare_parameter("camera_extrinsics_file", "")
self.declare_parameter("source_urdf_path", "")
self.declare_parameter("source_urdf_expected_sha256", "")
@@ -2206,6 +2249,20 @@ class G20ThreeCameraCalibrationNode(Node):
if not resume_value
else Path(resume_value).expanduser().resolve()
)
self.recalibration_scope = str(
value("recalibration_scope")
).strip().lower()
self.recalibration_task_keys = recalibration_task_keys(
self.profile, self.recalibration_scope
)
if (
self.recalibration_scope != "full"
and self.resume_raw_samples_path is None
):
raise ValueError(
"partial recalibration requires resume_raw_samples_path from "
"a passed complete session"
)
self.resumed_task_keys: tuple[str, ...] = ()
self.resume_source_session = ""
self.camera_extrinsics_file = Path(
@@ -3951,7 +4008,7 @@ class G20ThreeCameraCalibrationNode(Node):
* Rotation.from_matrix(child_matrix[:3, :3])
)
feedback = float(state_u8[observer.motor_index])
record = {
durable = canonical_sample_record({
"kind": "palm_axis_sample",
"attempt": self.sweep_attempts.get(
_sweep_storage_key(item.spec), 1
@@ -3963,7 +4020,8 @@ class G20ThreeCameraCalibrationNode(Node):
"motor_index": int(observer.motor_index),
"cycle": int(item.cycle),
"direction": item.direction,
"command_u8": int(np.clip(np.rint(feedback), 0, 255)),
"requested_command_u8": int(item.target_u8),
"feedback_u8": feedback,
"relative_quaternion_xyzw": [
float(value) for value in relative_rotation.as_quat()
],
@@ -3981,7 +4039,8 @@ class G20ThreeCameraCalibrationNode(Node):
6,
),
"image_stamp_ns": int(stamp_ns),
}
})
record = fitting_sample_record(durable)
self.palm_axis_records_by_source.setdefault(
observer.source_name, []
).append(record)
@@ -4012,18 +4071,7 @@ class G20ThreeCameraCalibrationNode(Node):
]
append_jsonl_many(
self.raw_path,
(
{
**{
key: value
for key, value in record.items()
if key != "command_u8"
},
"requested_command_u8": int(item.target_u8),
"feedback_u8": int(record["command_u8"]),
}
for record in rows
),
(canonical_sample_record(record) for record in rows),
)
def _accept_frame(self, observation: FrameObservation) -> None:
@@ -4432,18 +4480,32 @@ class G20ThreeCameraCalibrationNode(Node):
if source.resolve() == self.raw_path.resolve():
raise RuntimeError("resume raw samples must come from an older session")
rows: list[dict[str, Any]] = []
with source.open("r", encoding="utf-8") as stream:
for line_number, line in enumerate(stream, start=1):
if not line.strip():
source_size = max(1, int(source.stat().st_size))
bytes_read = 0
with source.open("rb") as stream:
for line_number, raw_line in enumerate(stream, start=1):
bytes_read += len(raw_line)
if not raw_line.strip():
continue
try:
line = raw_line.decode("utf-8")
value = json.loads(line)
except json.JSONDecodeError as error:
except (UnicodeDecodeError, json.JSONDecodeError) as error:
raise RuntimeError(
f"resume raw JSON is invalid at line {line_number}"
) from error
if isinstance(value, dict):
rows.append(value)
if line_number % 1000 == 0:
self.base_import_progress = {
"phase": "reading",
"records_read": len(rows),
"bytes_read": bytes_read,
"total_bytes": source_size,
"fraction": min(1.0, bytes_read / source_size),
}
self.reason = "reading_base_session_records"
_publish_import_status(self)
starts = [row for row in rows if row.get("kind") == "session_start"]
if len(starts) != 1:
raise RuntimeError("resume raw must contain exactly one session_start")
@@ -4494,6 +4556,10 @@ class G20ThreeCameraCalibrationNode(Node):
) from error
invalidated_task_set = set(size_invalidated_tasks)
invalidated_task_set.update(palm_axis_invalidated_tasks)
scope_invalidated_tasks = tuple(
getattr(self, "recalibration_task_keys", ()) or ()
)
invalidated_task_set.update(scope_invalidated_tasks)
if invalidated_task_set:
rows = [
row
@@ -4508,6 +4574,22 @@ class G20ThreeCameraCalibrationNode(Node):
minimum_sweep_bins=self.minimum_sweep_bins,
allow_sparse=True,
)
validate_sample_records(
row
for row in reusable
if str(row.get("kind", "")) in RESUMABLE_SAMPLE_KINDS
or row.get("kind") == "palm_axis_sample"
)
self.state = STATE_REVALIDATING_INHERITED
self.reason = "revalidating_inherited_tasks"
self.base_import_progress = {
"phase": "revalidating",
"records_read": len(rows),
"completed_tasks": 0,
"total_tasks": len(completed),
"fraction": 0.0,
}
_publish_import_status(self)
for durable in reusable:
kind = str(durable["kind"])
if kind == "synchronised_frame":
@@ -4519,10 +4601,7 @@ class G20ThreeCameraCalibrationNode(Node):
"resume checkpoint contains unknown palm-axis source "
+ source_name
)
record = dict(durable)
record["command_u8"] = int(
round(float(record["feedback_u8"]))
)
record = fitting_sample_record(durable)
self.palm_axis_records_by_source[source_name].append(record)
continue
joint_name = str(durable["joint"])
@@ -4530,15 +4609,11 @@ class G20ThreeCameraCalibrationNode(Node):
raise RuntimeError(
f"resume checkpoint contains unknown joint {joint_name}"
)
record = dict(durable)
if kind in {"sample", "baseline_hold_sample"}:
record["command_u8"] = int(
round(float(record["feedback_u8"]))
)
elif kind == "steady_command_sample":
record["command_u8"] = int(record["requested_command_u8"])
else:
if kind not in {
"sample", "baseline_hold_sample", "steady_command_sample"
}:
continue
record = fitting_sample_record(durable)
if kind == "sample":
self.records_by_joint[joint_name].append(record)
elif kind == "baseline_hold_sample":
@@ -4612,6 +4687,12 @@ class G20ThreeCameraCalibrationNode(Node):
"palm_axis_invalidated_task_keys": list(
palm_axis_invalidated_tasks
),
"recalibration_scope": str(
getattr(self, "recalibration_scope", "full")
),
"scope_invalidated_task_keys": list(
scope_invalidated_tasks
),
"imported_record_count": len(reusable),
"imported_attempt_floor_by_task": attempt_floor_by_task,
"source_raw_samples_sha256": _file_sha256(source),
@@ -4635,7 +4716,18 @@ class G20ThreeCameraCalibrationNode(Node):
"""
accepted: list[str] = []
dropped: list[dict[str, Any]] = []
for task_key in completed:
for task_index, task_key in enumerate(completed, start=1):
if getattr(self, "state", "") == STATE_REVALIDATING_INHERITED:
total = max(1, len(completed))
self.base_import_progress.update(
{
"current_task": task_key,
"completed_tasks": task_index - 1,
"total_tasks": len(completed),
"fraction": (task_index - 1) / total,
}
)
_publish_import_status(self)
spec = next(
(
item.spec
@@ -4669,6 +4761,16 @@ class G20ThreeCameraCalibrationNode(Node):
continue
break
accepted.append(task_key)
if getattr(self, "state", "") == STATE_REVALIDATING_INHERITED:
self.base_import_progress.update(
{
"current_task": "",
"completed_tasks": len(completed),
"total_tasks": len(completed),
"fraction": 1.0,
}
)
_publish_import_status(self)
if dropped:
accepted_set = set(accepted)
for task_key in completed:
@@ -4805,6 +4907,11 @@ class G20ThreeCameraCalibrationNode(Node):
"model": self.model,
"hand_type": self.hand_type,
"tag_layout": self.profile.layout_id,
"sample_schema_version": self.sample_schema_version,
"recalibration_scope": self.recalibration_scope,
"recalibration_task_keys": list(
self.recalibration_task_keys
),
"command_names": list(_command_names(self)),
"capabilities": sorted(self.profile.capabilities),
"reference_finger": self.profile.reference_finger,
@@ -4865,6 +4972,19 @@ class G20ThreeCameraCalibrationNode(Node):
},
)
try:
if self.resume_raw_samples_path is not None:
self.state = STATE_IMPORTING_BASE
self.reason = "reading_base_session_records"
self.base_import_progress = {
"phase": "reading",
"records_read": 0,
"bytes_read": 0,
"total_bytes": int(
self.resume_raw_samples_path.stat().st_size
),
"fraction": 0.0,
}
self._publish_status(time.monotonic())
restored_tasks = self._restore_durable_task_checkpoint()
except Exception as error:
self.started = False
@@ -6376,11 +6496,7 @@ class G20ThreeCameraCalibrationNode(Node):
),
axis=0,
)
record = {
# command_u8 is retained only in memory for the established
# pure fitting API. The durable record below uses explicit
# requested_command_u8 and feedback_u8 names.
"command_u8": int(requested_command_u8),
durable = canonical_sample_record({
"kind": "steady_command_sample",
"attempt": self.sweep_attempts.get(
_sweep_storage_key(item.spec), 1
@@ -6439,9 +6555,9 @@ class G20ThreeCameraCalibrationNode(Node):
),
"state_u8": [float(value) for value in state],
"valid_frames": len(selected),
}
})
record = fitting_sample_record(durable)
self.command_records_by_joint[joint_name].append(record)
durable = {key: value for key, value in record.items() if key != "command_u8"}
append_jsonl(self.raw_path, durable)
def _publish_next_checkpoint(self, now: float) -> bool:
@@ -8592,7 +8708,7 @@ class G20ThreeCameraCalibrationNode(Node):
),
axis=0,
)
record = {
durable = canonical_sample_record({
"kind": "baseline_hold_sample",
"attempt": attempt,
"task_name": item.spec.key,
@@ -8601,7 +8717,11 @@ class G20ThreeCameraCalibrationNode(Node):
"motor_index": motor,
"cycle": item.cycle,
"direction": item.direction,
"command_u8": command,
"requested_command_u8": command,
"feedback_u8": round(float(state[motor]), 6),
"image_stamp_ns": int(
np.median([frame.stamp_ns for frame in selected])
),
"relative_quaternion_xyzw": [
float(value) for value in quaternion
],
@@ -8651,18 +8771,11 @@ class G20ThreeCameraCalibrationNode(Node):
),
"valid_frames": len(selected),
"hold_seconds": float(self.baseline_hold_seconds),
}
})
record = fitting_sample_record(durable)
self.baseline_records_by_joint[joint_name].append(record)
if item.cycle == 0:
self.command_records_by_joint[joint_name].append(record)
durable = {
key: value for key, value in record.items() if key != "command_u8"
}
durable["requested_command_u8"] = command
durable["feedback_u8"] = round(float(state[motor]), 6)
durable["image_stamp_ns"] = int(
np.median([frame.stamp_ns for frame in selected])
)
append_jsonl(self.raw_path, durable)
def _baseline_hysteresis_records(
@@ -9005,7 +9118,7 @@ class G20ThreeCameraCalibrationNode(Node):
np.asarray([frame.state_u8 for frame in frames], dtype=float),
axis=0,
)
record = {
durable = canonical_sample_record({
"kind": "sample",
"attempt": self.sweep_attempts.get(
_sweep_storage_key(item.spec), 1
@@ -9016,7 +9129,15 @@ class G20ThreeCameraCalibrationNode(Node):
"motor_index": motor,
"cycle": item.cycle,
"direction": item.direction,
"command_u8": int(command),
"requested_command_u8": int(
self.sweep_checkpoint_target_u8
if self.sweep_checkpoint_target_u8 is not None
else item.target_u8
),
"feedback_u8": int(command),
"image_stamp_ns": int(
np.median([frame.stamp_ns for frame in frames])
),
"relative_translation_xyz_m": [
float(value) for value in vector
],
@@ -9055,20 +9176,9 @@ class G20ThreeCameraCalibrationNode(Node):
6,
),
"valid_frames": len(frames),
}
})
record = fitting_sample_record(durable)
self.records_by_joint[joint_name].append(record)
durable = {
key: value for key, value in record.items() if key != "command_u8"
}
durable["requested_command_u8"] = int(
self.sweep_checkpoint_target_u8
if self.sweep_checkpoint_target_u8 is not None
else item.target_u8
)
durable["feedback_u8"] = int(command)
durable["image_stamp_ns"] = int(
np.median([frame.stamp_ns for frame in frames])
)
append_jsonl(self.raw_path, durable)
G20ThreeCameraCalibrationNode._persist_palm_axis_samples(self, item)
@@ -9196,6 +9306,23 @@ class G20ThreeCameraCalibrationNode(Node):
fitting_started_at = time.monotonic()
self.last_status_publish = fitting_started_at
self._publish_status(fitting_started_at)
if int(getattr(self, "sample_schema_version", 0)) >= 1:
for store_name, store in (
("trajectory", self.records_by_joint),
("baseline", self.baseline_records_by_joint),
("steady", self.command_records_by_joint),
("palm_axis", self.palm_axis_records_by_source),
):
try:
validate_sample_records(
record
for records in store.values()
for record in records
)
except SampleDataContractError as error:
raise SampleDataContractError(
f"{error};store={store_name}"
) from error
# Provisional checks have a warning band so collection can continue,
# but the final fit always re-applies the unmodified hard thresholds.
for spec in self.profile.sweep_specs:
@@ -9452,10 +9579,16 @@ class G20ThreeCameraCalibrationNode(Node):
minimum_sources=int(
self.profile.minimum_palm_orientation_sources
),
minimum_arc_rad=self.trajectory_minimum_arc_rad,
minimum_arc_rad=(
self.profile.palm_orientation_minimum_arc_rad
),
maximum_rotation_orthogonal_rms_rad=(
self.active_maximum_rotation_orthogonal_rms_rad
),
maximum_command_distance_u8=(
self.profile
.palm_orientation_maximum_command_distance_u8
),
)
)
except ValueError as error:
@@ -9499,12 +9632,24 @@ class G20ThreeCameraCalibrationNode(Node):
if self.profile.layout_id == G20_RIGHT_19_LAYOUT:
command_fits = anchor_right_19_mechanical_endpoint_curves(
command_fits,
self.command_records_by_joint,
{
name: (
[
record
for record in self.records_by_joint[name]
if int(record["cycle"]) in training_cycle_set
]
if name == "thumb_cmc_roll"
else self.command_records_by_joint[name]
)
for name in RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS
},
maximum_direction_difference_rad=(
self.command_maximum_direction_gap_rad
),
feedback_endpoint_joints=frozenset({"thumb_cmc_roll"}),
)
endpoint_zero_offsets = (
measured_endpoint_zero_offsets = (
derive_right_19_mechanical_endpoint_offsets(
self.source_urdf_path,
command_fits,
@@ -9513,8 +9658,19 @@ class G20ThreeCameraCalibrationNode(Node):
),
)
)
endpoint_zero_offsets = {
name: value
for name, value in measured_endpoint_zero_offsets.items()
if name in RIGHT_19_MECHANICAL_ENDPOINT_JOINTS
}
post_solve_endpoint_offsets = {
name: value
for name, value in measured_endpoint_zero_offsets.items()
if name in RIGHT_19_POST_SOLVE_ENDPOINT_JOINTS
}
else:
endpoint_zero_offsets = {}
post_solve_endpoint_offsets = {}
holdout_zero_result = solve_urdf_zero_offsets(
source_urdf=self.source_urdf_path,
measurements=axes,
@@ -9559,6 +9715,10 @@ class G20ThreeCameraCalibrationNode(Node):
**self.zero_profile.fixed_direct_zero_offsets_rad,
**endpoint_zero_offsets,
},
static_output_zero_offsets_rad={
**self.zero_profile.static_output_zero_offsets_rad,
**post_solve_endpoint_offsets,
},
)
append_jsonl(
self.raw_path,
@@ -11680,10 +11840,17 @@ class G20ThreeCameraCalibrationNode(Node):
"resume": {
"used": bool(self.resumed_task_keys),
"source_session": self.resume_source_session,
"recalibration_scope": self.recalibration_scope,
"recalibration_task_keys": list(
self.recalibration_task_keys
),
"completed_task_count": len(self.resumed_task_keys),
"total_task_count": len(self.profile.sweep_specs),
"completed_task_keys": list(self.resumed_task_keys),
},
"base_import": dict(
getattr(self, "base_import_progress", {})
),
"quality": (
{} if self.completed_payload is None else self.completed_payload["quality"]
),
@@ -28,6 +28,7 @@ from .full_hand import (
JointCurveFit,
get_hand_calibration_profile,
)
from .sample_schema import explicit_domain_value
from .trajectory import (
_fit_circle_with_axis,
_fit_joint_curve,
@@ -55,6 +56,10 @@ class ZeroCalibrationProfile:
axis_parent_joint: Mapping[str, str]
phase_parent_joint: Mapping[str, str]
offset_observer_joint: Mapping[str, str]
# Some serial offsets can be observed from the angle between two axes
# captured by the same camera. The mapping is geometric topology only;
# it never contains a model- or serial-specific zero value.
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]
@@ -135,6 +140,7 @@ def _build_zero_profile(hand: HandCalibrationProfile) -> ZeroCalibrationProfile:
axis_parent_joint=axis_parent,
phase_parent_joint=phase_parent,
offset_observer_joint=observer,
same_view_axis_pair_by_offset={},
fixed_direct_zero_offsets_rad=fixed_direct_zero_offsets,
static_output_zero_offsets_rad={},
)
@@ -243,6 +249,9 @@ def _build_right_19_zero_profile(
axis_parent_joint=axis_parent,
phase_parent_joint=phase_parent,
offset_observer_joint=observer,
same_view_axis_pair_by_offset={
"thumb_cmc_yaw": ("thumb_cmc_roll", "thumb_cmc_pitch")
},
# Endpoint-observable offsets are supplied by the caller. CMC remains
# visually solved and no serial-specific zero is hidden in the shared
# profile.
@@ -262,6 +271,18 @@ RIGHT_19_MECHANICAL_ENDPOINT_JOINTS = frozenset(
}
)
# Thumb CMC roll has a directly observed actuator-to-actuator travel, but its
# electrical stop is not used as a runtime URDF limit. Keep it out of
# RIGHT_19_MECHANICAL_ENDPOINT_JOINTS (whose offsets also move upper limits),
# and use its endpoint estimate only as the final static roll origin. This
# makes roll independent of the fitted palm-frame phase and of all yaw logic.
RIGHT_19_ROLL_ENDPOINT_ZERO_JOINTS = frozenset({"thumb_cmc_roll"})
RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS = (
RIGHT_19_MECHANICAL_ENDPOINT_JOINTS
| RIGHT_19_ROLL_ENDPOINT_ZERO_JOINTS
)
RIGHT_19_POST_SOLVE_ENDPOINT_JOINTS = RIGHT_19_ROLL_ENDPOINT_ZERO_JOINTS
def anchor_right_19_mechanical_endpoint_curves(
curves: Mapping[str, JointCurveFit],
@@ -269,6 +290,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(),
) -> dict[str, JointCurveFit]:
"""Set mechanical-endpoint curve scale from direct SO(3) travel.
@@ -279,10 +301,11 @@ def anchor_right_19_mechanical_endpoint_curves(
both poses by a rigid hand/camera transform or either fixed Tag mounting
rotation cannot change it.
Use both sweep directions as independent endpoint measurements, reject
disagreement, and apply their robust mean as one scale correction to the
already validated curve. Curve shape, hysteresis, axis diagnostics and
all existing quality gates remain unchanged.
Use every supplied cycle and both sweep directions as independent endpoint
measurements, reject disagreement, and apply their robust median as one
scale correction to the already validated curve. Dense CMC-roll records
are selected in feedback coordinates; settled mechanical-contact records
remain selected in requested-command coordinates.
"""
if (
not math.isfinite(maximum_direction_difference_rad)
@@ -296,41 +319,74 @@ def anchor_right_19_mechanical_endpoint_curves(
raise ValueError("endpoint curve correction limit must be positive")
result = dict(curves)
for name in sorted(RIGHT_19_MECHANICAL_ENDPOINT_JOINTS):
unknown_feedback_joints = sorted(
set(feedback_endpoint_joints) - set(RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS)
)
if unknown_feedback_joints:
raise ValueError(
"feedback endpoint joints are not endpoint measurements: "
+ ",".join(unknown_feedback_joints)
)
for name in sorted(RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS):
fit = curves.get(name)
records = list(records_by_joint.get(name, ()))
if fit is None:
raise ValueError(f"missing measured endpoint curve for {name}")
direction_travel: dict[str, float] = {}
all_travel: list[float] = []
for direction in ("decreasing", "increasing"):
endpoint_rotations: dict[int, Rotation] = {}
for command in (0, 255):
quaternions = [
_relative_rotation(record)
cycles = sorted(
{
int(record.get("cycle", 0))
for record in records
if str(record.get("direction")) == direction
and int(
record.get(
"requested_command_u8",
record.get("command_u8", -1),
)
)
== command
]
if not quaternions:
raise ValueError(
f"{name} {direction} is missing settled endpoint {command}"
)
endpoint_rotations[command] = Rotation.from_quat(
robust_rotation_summary(quaternions)[0]
)
direction_travel[direction] = float(
(
endpoint_rotations[255].inv()
* endpoint_rotations[0]
).magnitude()
}
)
branch_travel: list[float] = []
for cycle in cycles:
endpoint_rotations: dict[int, Rotation] = {}
for command in (0, 255):
quaternions = []
for record in records:
if (
str(record.get("direction")) != direction
or int(record.get("cycle", 0)) != cycle
):
continue
endpoint_value = (
explicit_domain_value(record, "feedback")
if name in feedback_endpoint_joints
else record.get(
"requested_command_u8",
record.get("command_u8", -1),
)
)
if endpoint_value is None:
continue
if int(round(float(endpoint_value))) == command:
quaternions.append(_relative_rotation(record))
if not quaternions:
raise ValueError(
f"{name} {direction} cycle {cycle} is missing "
f"settled endpoint {command}"
)
endpoint_rotations[command] = Rotation.from_quat(
robust_rotation_summary(quaternions)[0]
)
branch_travel.append(
float(
(
endpoint_rotations[255].inv()
* endpoint_rotations[0]
).magnitude()
)
)
if not branch_travel:
raise ValueError(f"{name} is missing {direction} endpoint travel")
direction_travel[direction] = float(np.median(branch_travel))
all_travel.extend(branch_travel)
travel_values = np.asarray(list(direction_travel.values()), dtype=float)
direction_difference = float(np.ptp(travel_values))
@@ -339,7 +395,13 @@ def anchor_right_19_mechanical_endpoint_curves(
f"{name} settled endpoint directions disagree: "
f"{math.degrees(direction_difference):.3f}deg"
)
direct_travel = float(np.median(travel_values))
cycle_range = float(np.ptp(np.asarray(all_travel, dtype=float)))
if cycle_range > maximum_direction_difference_rad:
raise ValueError(
f"{name} settled endpoint cycles disagree: "
f"{math.degrees(cycle_range):.3f}deg"
)
direct_travel = float(np.median(np.asarray(all_travel, dtype=float)))
curve = np.asarray(fit.angle_rad, dtype=float)
if curve.shape != (256,) or not np.all(np.isfinite(curve)):
raise ValueError(f"{name} endpoint curve must contain 256 finite bins")
@@ -364,6 +426,8 @@ def anchor_right_19_mechanical_endpoint_curves(
"mechanical_endpoint_direction_difference_rad": (
direction_difference
),
"mechanical_endpoint_cycle_range_rad": cycle_range,
"mechanical_endpoint_sample_count": len(all_travel),
"mechanical_endpoint_raw_curve_travel_rad": projected_travel,
"mechanical_endpoint_curve_scale": scale,
}
@@ -400,10 +464,10 @@ def derive_right_19_mechanical_endpoint_offsets(
The source URDF upper limit describes those same physical endpoints.
Therefore ``origin_offset + measured_travel == CAD_upper``.
The coupled thumb-CMC axes are deliberately excluded. Physical
inspection of G20_RIGHT_001 proved that their electrical actuator
endpoints do not equal the source-CAD upper coordinates. Treating them
as equal produced repeatable but physically wrong zero offsets.
Thumb CMC pitch/yaw remain excluded because their endpoint-to-CAD contract
is not independently established. CMC roll is included as a post-solve
origin only: its measured travel determines roll without allowing the
palm common-direction fit to write that joint.
The returned corrections are measured, generally non-zero encoder zeros;
they do not retain the CAD origin. Using full relative rotation travel
@@ -415,7 +479,7 @@ 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_MECHANICAL_ENDPOINT_JOINTS):
for name in sorted(RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS):
fit = curves.get(name)
joint = joints.get(name)
limit = None if joint is None else joint.find("limit")
@@ -1079,14 +1143,15 @@ def fit_partial_palm_orientation_measurement(
zero_command_u8: int,
minimum_arc_rad: float = math.radians(15.0),
maximum_rotation_orthogonal_rms_rad: float = math.radians(2.5),
maximum_command_distance_u8: int = 255,
) -> PalmOrientationMeasurement:
"""Fit a physical axis direction without requiring full command travel."""
samples = [
"""Fit a physical axis within a configured window around encoder zero."""
cycle_samples = [
dict(record)
for record in records
if int(record.get("cycle", -1)) == int(cycle)
]
if len(samples) < 12:
if len(cycle_samples) < 12:
raise ValueError(
f"{source_joint} cycle {cycle + 1} has too few visible samples"
)
@@ -1096,6 +1161,22 @@ def fit_partial_palm_orientation_measurement(
raise ValueError(
"palm orientation rotation residual limit must be positive"
)
command_distance = int(maximum_command_distance_u8)
if command_distance < 1 or command_distance > 255:
raise ValueError(
"palm orientation command distance must be in [1, 255]"
)
zero = int(zero_command_u8)
samples = [
record
for record in cycle_samples
if abs(int(record["command_u8"]) - zero) <= command_distance
]
if len(samples) < 12:
raise ValueError(
f"{source_joint} cycle {cycle + 1} has too few zero-adjacent "
"visible samples"
)
reference = Rotation.from_quat(
_baseline_reference(samples, int(zero_command_u8))
)
@@ -1150,8 +1231,8 @@ def fit_partial_palm_orientation_measurement(
)
if orthogonal_rms > float(maximum_rotation_orthogonal_rms_rad):
raise ValueError(
f"{source_joint} cycle {cycle + 1} rotation residual "
f"{math.degrees(orthogonal_rms):.3f}deg exceeds "
f"{source_joint} cycle {cycle + 1} zero-adjacent rotation "
f"residual {math.degrees(orthogonal_rms):.3f}deg exceeds "
f"{math.degrees(maximum_rotation_orthogonal_rms_rad):.3f}deg"
)
zero_records = _near_zero_records(samples, int(zero_command_u8))
@@ -1198,8 +1279,9 @@ def fit_partial_palm_orientation_measurements(
minimum_sources: int,
minimum_arc_rad: float = math.radians(15.0),
maximum_rotation_orthogonal_rms_rad: float = math.radians(2.5),
maximum_command_distance_u8: int = 255,
) -> tuple[tuple[PalmOrientationMeasurement, ...], Mapping[str, str]]:
"""Fit every usable optional source and require redundant coverage."""
"""Fit every usable optional source and require configured coverage."""
source_map = {str(name): str(model) for name, model in sources.items()}
minimum = int(minimum_sources)
if not source_map:
@@ -1236,6 +1318,9 @@ def fit_partial_palm_orientation_measurements(
maximum_rotation_orthogonal_rms_rad=(
maximum_rotation_orthogonal_rms_rad
),
maximum_command_distance_u8=(
maximum_command_distance_u8
),
)
except Exception as error:
rejected[f"{source_joint}:cycle{cycle + 1}"] = str(error)
@@ -2280,6 +2365,17 @@ def solve_urdf_zero_offsets(
for item in orientation_measurements
if int(item.cycle) == int(validation_cycle)
)
orientation_by_model_cycle: dict[
tuple[str, int], PalmOrientationMeasurement
] = {}
for item in orientation_measurements:
key = (str(item.model_joint), int(item.cycle))
if key in orientation_by_model_cycle:
raise ValueError(
"palm orientation has multiple sources for "
f"{key[0]} cycle {key[1] + 1}"
)
orientation_by_model_cycle[key] = item
if not 0.0 < finger_maximum_offset_rad <= maximum_offset_rad:
raise ValueError("finger maximum offset must be positive and no larger than thumb")
joint_limits = {
@@ -2368,6 +2464,7 @@ def solve_urdf_zero_offsets(
joint_angles=angles,
)
return axis / np.linalg.norm(axis)
fixed_offsets = {
str(name): float(value)
for name, value in (
@@ -2898,6 +2995,64 @@ def solve_urdf_zero_offsets(
)
)
def same_view_axis_pair_errors(
offset_joint: str,
selected: Sequence[JointAxisMeasurement],
offsets: Mapping[str, float],
) -> tuple[float, ...]:
"""Return the camera-frame-invariant residual for a serial-axis pair.
This observation intentionally has two mirror roots. The optimiser
disambiguates them with the original cross-view estimate, but the
pair residual alone determines the final numerical zero.
"""
pair = profile.same_view_axis_pair_by_offset.get(offset_joint)
if pair is None:
return ()
anchor_joint, observer_joint = pair
def angle(left: np.ndarray, right: np.ndarray) -> float:
cosine = float(left @ right) / float(
np.linalg.norm(left) * np.linalg.norm(right)
)
return math.acos(abs(float(np.clip(cosine, -1.0, 1.0))))
errors: list[float] = []
for cycle in sorted({int(item.cycle) for item in selected}):
anchor = orientation_by_model_cycle.get((anchor_joint, cycle))
observer = orientation_by_model_cycle.get(
(observer_joint, cycle)
)
if anchor is None or observer is None:
return ()
angles = _angles_from_state(
observer.condition_state_u8,
curves=curves,
motor_by_joint=motor_by_joint,
inherited_zero_joints=profile.inherited_zero_joints,
)
predicted_anchor, _ = model.axis_line(
anchor_joint,
zero_offsets=offsets,
joint_angles=angles,
)
predicted_observer, _ = model.axis_line(
observer_joint,
zero_offsets=offsets,
joint_angles=angles,
)
observed_anchor = np.asarray(
anchor.axis_common_xyz, dtype=float
)
observed_observer = np.asarray(
observer.axis_common_xyz, dtype=float
)
errors.append(
angle(predicted_anchor, predicted_observer)
- angle(observed_anchor, observed_observer)
)
return tuple(errors)
def angular_error_samples(
offsets: Mapping[str, float],
selected: Sequence[JointAxisMeasurement],
@@ -2909,6 +3064,12 @@ def solve_urdf_zero_offsets(
name: [] for name in profile.direct_zero_joints
}
for offset_joint, observer_joint in profile.offset_observer_joint.items():
pair_errors = same_view_axis_pair_errors(
offset_joint, selected, offsets
)
if pair_errors:
errors[offset_joint].extend(pair_errors)
continue
observer_items = [
item for item in selected if item.joint == observer_joint
]
@@ -3061,11 +3222,18 @@ def solve_urdf_zero_offsets(
}
)
result.update(fixed_offsets)
for name, limit in zip(
profile.direct_zero_joints, diagnostic_offset_limits
for name, diagnostic_limit, configured_limit in zip(
profile.direct_zero_joints,
diagnostic_offset_limits,
offset_limits,
):
if name in fixed_offsets:
continue
has_axis_pair = name in profile.same_view_axis_pair_by_offset
limit = float(
configured_limit if has_axis_pair else diagnostic_limit
)
def residual(value: np.ndarray) -> np.ndarray:
candidate = dict(result)
candidate[name] = float(value[0])
@@ -3432,25 +3600,37 @@ def solve_urdf_zero_offsets(
improvement_by_joint: dict[str, float] = {}
improvement_confidence_lower: dict[str, float] = {}
improvement_passed = True
palm_orientation_validation_errors = tuple(
math.acos(
abs(
float(
np.clip(
base_rotation.apply(
predicted_palm_orientation_local(
item, zero_offsets
if profile.same_view_axis_pair_by_offset:
# Validate the same camera/Tag-mount-invariant axis-pair angle used by
# the configured offset. This keeps the side channel out of every
# other joint, including the already stable thumb CMC roll solve.
palm_orientation_validation_errors = tuple(
abs(float(error))
for offset_joint in profile.same_view_axis_pair_by_offset
for error in same_view_axis_pair_errors(
offset_joint, validation, applied_training
)
)
else:
palm_orientation_validation_errors = tuple(
math.acos(
abs(
float(
np.clip(
base_rotation.apply(
predicted_palm_orientation_local(
item, zero_offsets
)
)
@ np.asarray(item.axis_common_xyz, dtype=float),
-1.0,
1.0,
)
@ np.asarray(item.axis_common_xyz, dtype=float),
-1.0,
1.0,
)
)
)
for item in orientation_validation
)
for item in orientation_validation
)
palm_orientation_validation_limit = (
maximum_validation_error_rad
if maximum_validation_error_rad is not None
@@ -287,6 +287,9 @@ def _launch_stack(context):
"resume_raw_samples_path": LaunchConfiguration(
"resume_raw_samples_path"
),
"recalibration_scope": LaunchConfiguration(
"recalibration_scope"
),
# The SDK performs roughly 25 synchronous CAN queries whenever
# cb_<side>_hand_info has a subscriber. Calibration only used
# that topic to display a speed diagnostic, while those reads
@@ -483,6 +486,7 @@ def generate_launch_description() -> LaunchDescription:
),
DeclareLaunchArgument("session_dir", default_value=""),
DeclareLaunchArgument("resume_raw_samples_path", default_value=""),
DeclareLaunchArgument("recalibration_scope", default_value="full"),
DeclareLaunchArgument(
"calibration_config",
default_value=str(
@@ -99,8 +99,36 @@ def test_right_19_profile_has_exact_layout_tasks_and_measured_dips() -> None:
"pinky_mcp_roll",
"pinky_mcp_roll_side",
)
assert profile.palm_axis_observers == ()
assert profile.minimum_palm_orientation_sources == 0
assert {
(
observer.source_name,
observer.task_name,
observer.view,
observer.model_joint,
observer.motor_index,
)
for observer in profile.palm_axis_observers
} == {
(
"thumb_cmc_pitch_top_axis",
"thumb_cmc_pitch_front",
"top",
"thumb_cmc_pitch",
0,
),
(
"thumb_cmc_roll_top_axis",
"thumb_cmc_roll_front",
"top",
"thumb_cmc_roll",
5,
),
}
assert profile.minimum_palm_orientation_sources == 2
assert profile.palm_orientation_minimum_arc_rad == pytest.approx(
math.radians(10.0)
)
assert profile.palm_orientation_maximum_command_distance_u8 == 64
assert profile.sweep_specs[6].joints == ("pinky_pip", "pinky_dip")
assert {
name: spec.source_joint
@@ -31,6 +31,8 @@ from g20_thumb_apriltag_calibration.operator_report import (
from g20_thumb_apriltag_calibration.one_command import (
_automatic_resume_candidate,
_calibration_node_exited_before_status,
_launch_command,
_resolve_partial_base_session,
_status_timeout_seconds,
)
from g20_thumb_apriltag_calibration.product import load_product_config
@@ -56,6 +58,7 @@ from g20_thumb_apriltag_calibration.three_camera_node import (
_model_link_in_observer_base,
_palm_axis_observer_schema,
_palm_axis_resume_policy,
recalibration_task_keys,
_steady_checkpoint_commands,
_unresolved_fit_failure_tasks,
combination_target_coverage,
@@ -247,6 +250,9 @@ def _complete_resume_task_rows(profile, spec) -> list[dict]:
"joint": joint,
"cycle": cycle,
"direction": direction,
"requested_command_u8": (
0 if direction == "decreasing" else 255
),
"feedback_u8": command,
}
for command in (*range(32), 255)
@@ -339,18 +345,18 @@ def test_resume_preserves_palm_axis_side_channel_without_requiring_it() -> None:
assert completed_without_side_channel == (spec.key,)
def test_checkpoint_needs_no_palm_axis_reacquisition_when_disabled() -> 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_v1"
"palm_axis_side_channel_v2"
}
compatible, invalidated = _palm_axis_resume_policy(
profile, {"capabilities": sorted(old_capabilities)}
)
assert compatible is True
assert invalidated == ()
assert compatible is False
assert set(invalidated) == {spec.key for spec in profile.sweep_specs}
compatible, invalidated = _palm_axis_resume_policy(
profile,
@@ -759,6 +765,70 @@ def test_automatic_resume_requires_failed_matching_geometry(tmp_path: Path) -> N
assert _automatic_resume_candidate(config) is None
def test_thumb_scope_resolves_passed_base_and_launches_partial_mode(
tmp_path: Path,
) -> None:
config = _config(tmp_path)
session = config.session_root / "20260830_120000"
session.mkdir(parents=True)
(session / "raw_samples.jsonl").write_text("{}\n", encoding="utf-8")
(session / f"g20_right_{config.serial_number}_calibration.json").write_text(
"{}\n", encoding="utf-8"
)
atomic_write_json(
session / "calibration_summary_zh.json",
{
"result": "PASS",
"quality": {"passed": True},
"hashes": {
"source_urdf_sha256": config.source_urdf_sha256,
"camera_extrinsics_sha256": config.camera_extrinsics_sha256,
"calibration_config_sha256": config.calibration_config_sha256,
},
},
)
atomic_session_pointer(config.session_root, "latest_passed", session)
resolved = _resolve_partial_base_session(
config, config.session_root / "latest_passed"
)
command = _launch_command(
config,
config.session_root / "20260830_130000",
resume_from=resolved,
recalibration_scope="thumb",
)
assert resolved == session.resolve()
assert "recalibration_scope:=thumb" in command
assert f"resume_raw_samples_path:={session / 'raw_samples.jsonl'}" in command
def test_thumb_scope_rejects_base_with_different_geometry(tmp_path: Path) -> None:
config = _config(tmp_path)
session = config.session_root / "20260830_120000"
session.mkdir(parents=True)
(session / "raw_samples.jsonl").touch()
(session / f"g20_right_{config.serial_number}_calibration.json").write_text(
"{}\n", encoding="utf-8"
)
atomic_write_json(
session / "calibration_summary_zh.json",
{
"result": "PASS",
"quality": {"passed": True},
"hashes": {
"source_urdf_sha256": "0" * 64,
"camera_extrinsics_sha256": config.camera_extrinsics_sha256,
"calibration_config_sha256": config.calibration_config_sha256,
},
},
)
with pytest.raises(ValueError, match="differs from the current product"):
_resolve_partial_base_session(config, session)
def test_automatic_resume_accepts_ctrl_c_checkpoint_without_summary(
tmp_path: Path,
) -> None:
@@ -847,6 +917,8 @@ def test_node_restores_complete_prefix_into_new_self_contained_raw(
},
"baseline_command_u8": baseline,
"source_urdf_sha256": config.source_urdf_sha256,
"capabilities": sorted(profile.capabilities),
"palm_axis_observers": _palm_axis_observer_schema(profile),
},
*_complete_resume_task_rows(profile, profile.sweep_specs[0]),
]
@@ -882,6 +954,10 @@ def test_node_restores_complete_prefix_into_new_self_contained_raw(
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},
palm_axis_records_by_source={
observer.source_name: []
for observer in profile.palm_axis_observers
},
sweep_index=0,
sweep_items=sweep_items,
resumed_task_keys=(),
@@ -906,6 +982,99 @@ def test_node_restores_complete_prefix_into_new_self_contained_raw(
assert any(row["kind"] == "sample" for row in restored)
def test_thumb_recalibration_imports_fingers_but_invalidates_all_thumb_tasks(
tmp_path: Path, monkeypatch
) -> None:
monkeypatch.setattr(
G20ThreeCameraCalibrationNode,
"_revalidate_imported_tasks",
lambda self, completed, **_kwargs: (tuple(completed), []),
)
config = _config(tmp_path)
profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT)
baseline = [255] * 20
baseline[6:10] = [127] * 4
thumb_tasks = recalibration_task_keys(profile, "thumb")
thumb_spec = next(spec for spec in profile.sweep_specs if spec.key in thumb_tasks)
finger_spec = next(
spec
for spec in profile.sweep_specs
if spec.key == "pinky_pitch_side"
)
source_raw = tmp_path / "passed" / "raw_samples.jsonl"
source_raw.parent.mkdir()
rows = [
{
"kind": "session_start",
"model": "G20",
"hand_type": "right",
"tag_layout": G20_RIGHT_19_LAYOUT,
"view_tags": {
view: dict(tags) for view, tags in profile.view_tags.items()
},
"baseline_command_u8": baseline,
"source_urdf_sha256": config.source_urdf_sha256,
"capabilities": sorted(profile.capabilities),
"palm_axis_observers": _palm_axis_observer_schema(profile),
},
*_complete_resume_task_rows(profile, thumb_spec),
*_complete_resume_task_rows(profile, finger_spec),
]
source_raw.write_text(
"".join(json.dumps(row) + "\n" for row in rows),
encoding="utf-8",
)
current_raw = tmp_path / "new" / "raw_samples.jsonl"
current_raw.parent.mkdir()
current_raw.touch()
sweep_items = [
SweepItem(spec, cycle, direction)
for spec in profile.sweep_specs
for cycle in range(4)
for direction in ("decreasing", "increasing")
]
node = SimpleNamespace(
resume_raw_samples_path=source_raw,
raw_path=current_raw,
model="G20",
hand_type="right",
profile=profile,
baseline_command=tuple(baseline),
source_urdf_path=config.source_urdf,
repetitions=4,
minimum_sweep_bins=32,
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},
palm_axis_records_by_source={
observer.source_name: [] for observer in profile.palm_axis_observers
},
sweep_index=0,
sweep_items=sweep_items,
resumed_task_keys=(),
resume_source_session="",
recalibration_scope="thumb",
recalibration_task_keys=thumb_tasks,
)
count = G20ThreeCameraCalibrationNode._restore_durable_task_checkpoint(node)
assert count == 1
assert node.resumed_task_keys == (finger_spec.key,)
assert node.sweep_index == 0
assert all(
not node.records_by_joint[name]
for name in thumb_spec.joints
)
assert all(node.records_by_joint[name] for name in finger_spec.joints)
imported = [
json.loads(line)
for line in current_raw.read_text(encoding="utf-8").splitlines()
][0]
assert imported["recalibration_scope"] == "thumb"
assert imported["scope_invalidated_task_keys"] == list(thumb_tasks)
def test_runtime_json_is_minimal_v4_with_21_independent_midpoint_curves() -> None:
payload = _payload()
assert payload["schema_version"] == 4
@@ -955,6 +1124,33 @@ def test_publication_protects_passive_xml_and_requires_two_matching_sessions(
assert len(commands["poses"]) == 8
def test_publication_records_thumb_recalibration_provenance(tmp_path: Path) -> None:
config = _config(tmp_path, passes=1)
session = _make_passed_session(config, "20260830_140000")
status = _passed_node_status()
tasks = list(
recalibration_task_keys(
get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT),
"thumb",
)
)
status["resume"] = {
"used": True,
"source_session": "20260828_205005",
"recalibration_scope": "thumb",
"recalibration_task_keys": tasks,
}
summary, ready = finalize_session_artifacts(
config, session, node_status=status
)
assert ready is True
assert summary["calibration_scope"] == "thumb"
assert summary["inherited_base_session"] == "20260828_205005"
assert summary["freshly_calibrated_task_keys"] == tasks
def test_publication_numerically_binds_json_offsets_and_urdf_limits(
tmp_path: Path,
) -> None:
@@ -1351,6 +1547,16 @@ def test_missing_startup_status_has_specific_camera_status_code() -> None:
assert "尚未开始运动" in suggestion
def test_sample_domain_contract_has_specific_non_publication_code() -> None:
code, problem, suggestion = classify_error(
"DATA-CONTRACT-701:sample is missing explicit feedback_u8", {}
)
assert code == "DATA-CONTRACT-701"
assert "数据契约" in problem
assert "无需重新采集" in suggestion
def test_startup_watchdog_detects_only_calibration_child_exit(
tmp_path: Path,
) -> None:
@@ -1795,6 +2001,8 @@ def test_node_drops_imported_task_failing_hard_gates(
},
"baseline_command_u8": baseline,
"source_urdf_sha256": config.source_urdf_sha256,
"capabilities": sorted(profile.capabilities),
"palm_axis_observers": _palm_axis_observer_schema(profile),
},
*_complete_resume_task_rows(profile, profile.sweep_specs[0]),
]
@@ -1825,6 +2033,10 @@ def test_node_drops_imported_task_failing_hard_gates(
name: [] for name in profile.record_joints
},
command_records_by_joint={name: [] for name in profile.record_joints},
palm_axis_records_by_source={
observer.source_name: []
for observer in profile.palm_axis_observers
},
sweep_index=0,
sweep_items=sweep_items,
resumed_task_keys=(),
@@ -0,0 +1,69 @@
"""Tests for the explicit calibration sample command domains."""
from g20_thumb_apriltag_calibration.sample_schema import (
SampleDataContractError,
canonical_sample_record,
fitting_sample_record,
fitting_sample_records,
)
import pytest
def _sample(kind: str = 'sample') -> dict[str, object]:
return {
'kind': kind,
'joint': 'thumb_cmc_roll',
'cycle': 0,
'direction': 'decreasing',
'requested_command_u8': 0,
'feedback_u8': 17.6,
}
def test_online_and_serialized_replay_use_identical_fit_projection() -> None:
"""Serialization must not change the curve fitter's input record."""
durable = canonical_sample_record(_sample())
online = fitting_sample_record(durable)
serialized = canonical_sample_record(online)
replay = fitting_sample_record(serialized)
assert 'command_u8' not in durable
assert serialized == durable
assert replay == online
assert online['command_u8'] == 18
def test_default_domain_is_requested_only_for_steady_checkpoints() -> None:
"""Settled checkpoints and dense trajectories use declared domains."""
steady = _sample('steady_command_sample')
assert fitting_sample_record(steady)['command_u8'] == 0
assert fitting_sample_record(steady, domain='feedback')['command_u8'] == 18
def test_feedback_projection_can_snap_proven_requested_endpoints() -> None:
"""A settled endpoint remains exact after feedback-domain projection."""
records = fitting_sample_records(
[_sample()],
domain='feedback',
snap_requested_endpoints=True,
)
assert records[0]['command_u8'] == 0
assert records[0]['feedback_u8'] == pytest.approx(17.6)
def test_new_sample_rejects_missing_explicit_feedback_domain() -> None:
"""A new observation cannot rely on the compatibility command key."""
ambiguous = {
'kind': 'sample',
'requested_command_u8': 0,
'command_u8': 0,
}
with pytest.raises(
SampleDataContractError,
match='DATA-CONTRACT-701.*feedback_u8',
):
canonical_sample_record(ambiguous)
@@ -268,22 +268,25 @@ def test_joint_fit_failure_names_metric_and_selective_retry() -> None:
assert "运动采样:" not in text
def test_palm_orientation_failure_explains_direction_coverage() -> None:
def test_palm_orientation_failure_explains_thumb_top_coverage() -> None:
explanation, suggestion = three_camera_reason_zh(
"PAUSED",
"palm_orientation_quality_failed",
{
"failures": [
{
"reason": "palm orientation cycle 2 has 2/3 usable sources"
"reason": (
"palm orientation cycle 2 has 1/2 usable sources: "
"thumb_cmc_roll_top_axis:cycle2=too few samples"
)
}
]
},
)
assert "至少三根手指" in explanation
assert "2/3 usable sources" in explanation
assert "Tag 1013" in suggestion
assert "顶部Tag 8/9" in explanation
assert "1/2 usable sources" in explanation
assert "CMC pitch和roll" in suggestion
def test_cross_view_side_line_rms_names_side_source() -> None:
@@ -535,6 +535,38 @@ def test_right_roll_requires_both_views_task_local_tags() -> None:
) == ("side_base", "pinky_pip")
def test_thumb_yaw_top_axis_observer_does_not_change_front_sweep_contract() -> None:
spec = next(
item
for item in RIGHT_19_HAND_PROFILE.sweep_specs
if item.key == "thumb_cmc_pitch_front"
)
observer = _palm_axis_observer_for_sweep(
RIGHT_19_HAND_PROFILE, spec, "top"
)
assert observer is not None
assert observer.model_joint == "thumb_cmc_pitch"
assert _sweep_views(RIGHT_19_HAND_PROFILE, spec) == ("front",)
node = SimpleNamespace(
profile=RIGHT_19_HAND_PROFILE,
views={
view: SimpleNamespace(
roles=tuple(RIGHT_19_HAND_PROFILE.view_tags[view]),
preflight_roles=(f"{view}_base",),
)
for view in ("front", "side", "top")
},
active_sweep=SweepItem(spec, 0, DIRECTION_DECREASING),
active_validation=None,
retry_sweep_spec=None,
active_combination_validation=None,
)
assert G20ThreeCameraCalibrationNode._required_roles_for_view(
node, "top"
) == ("top_base",)
def test_pitch_task_does_not_add_a_front_direction_observer() -> None:
spec = next(
item
@@ -4422,6 +4454,10 @@ def _import_revalidation_node(records_by_joint: dict) -> SimpleNamespace:
]
for name, records in records_by_joint.items()
}
node.palm_axis_records_by_source = {
observer.source_name: []
for observer in RIGHT_19_HAND_PROFILE.palm_axis_observers
}
node.command_maximum_direction_gap_rad = math.radians(2.0)
node.sweep_items = [
SweepItem(spec, 0, DIRECTION_DECREASING)
@@ -617,10 +617,12 @@ def _solve_synthetic_offsets(
inject_secondary_root_axis_bias_degrees: float = 0.0,
inject_secondary_root_point_bias_m: float = 0.0,
inject_observer_cone_bias_degrees: float = 0.0,
palm_orientation_frame_bias_degrees: float = 0.0,
maximum_systematic_axis_cone_bias_degrees: float | None = None,
pose_axis_line_rms_by_joint_m: dict[str, float] | None = None,
joint_maximum_offset_degrees: dict[str, float] | None = None,
validation_offset_bias_degrees: dict[str, float] | None = None,
static_output_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),
):
@@ -782,12 +784,20 @@ def _solve_synthetic_offsets(
zero_offsets=cycle_offsets,
joint_angles=angles,
)
axis_common = base_rotation.apply(axis)
if palm_orientation_frame_bias_degrees:
bias_axis = np.asarray([0.3, -0.2, 0.4], dtype=float)
bias_axis /= np.linalg.norm(bias_axis)
axis_common = Rotation.from_rotvec(
math.radians(palm_orientation_frame_bias_degrees)
* bias_axis
).apply(axis_common)
palm_orientation_measurements.append(
PalmOrientationMeasurement(
source_joint=source_joint,
model_joint=model_joint,
cycle=cycle,
axis_common_xyz=tuple(base_rotation.apply(axis)),
axis_common_xyz=tuple(axis_common),
condition_state_u8=tuple(state),
observed_arc_rad=math.radians(45.0),
rotation_orthogonal_rms_rad=math.radians(0.1),
@@ -814,6 +824,10 @@ def _solve_synthetic_offsets(
maximum_systematic_axis_cone_bias_degrees
)
),
static_output_zero_offsets_rad={
name: math.radians(value)
for name, value in (static_output_offsets_degrees or {}).items()
},
)
return zero, result
@@ -870,6 +884,35 @@ 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_independent_roll_output_cannot_change_yaw_solution() -> 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,
]
_, visual = _solve_synthetic_offsets(
"right", injected, layout_id="g20_right_19"
)
_, independent = _solve_synthetic_offsets(
"right",
injected,
layout_id="g20_right_19",
static_output_offsets_degrees={"thumb_cmc_roll": 3.4},
)
assert math.degrees(
independent.direct_offsets_rad["thumb_cmc_roll"]
) == pytest.approx(3.4)
assert independent.direct_offsets_rad["thumb_cmc_yaw"] == pytest.approx(
visual.direct_offsets_rad["thumb_cmc_yaw"], abs=1.0e-10
)
assert independent.direct_offsets_rad["thumb_cmc_pitch"] == pytest.approx(
visual.direct_offsets_rad["thumb_cmc_pitch"], abs=1.0e-10
)
def _partial_orientation_records(
*,
tag_mount: Rotation,
@@ -995,6 +1038,66 @@ def test_partial_palm_direction_is_not_weighted_by_dwell_frame_count() -> None:
assert difference < math.radians(0.25)
def test_partial_palm_direction_ignores_far_stroke_pnp_bias() -> None:
records = _partial_orientation_records(
tag_mount=Rotation.from_euler("xyz", [0.2, -0.1, 0.3]),
common_rotation=Rotation.identity(),
maximum_angle_deg=30.0,
)
expected = fit_partial_palm_orientation_measurement(
"thumb_cmc_roll_top_axis",
"thumb_cmc_roll",
records,
cycle=0,
zero_command_u8=255,
minimum_arc_rad=math.radians(5.0),
maximum_command_distance_u8=64,
)
biased: list[dict[str, object]] = []
for source in records:
record = dict(source)
if abs(int(record["command_u8"]) - 255) > 64:
rotation = Rotation.from_quat(
record["relative_quaternion_xyzw"]
)
record["relative_quaternion_xyzw"] = list(
(
Rotation.from_rotvec([math.radians(12.0), 0.0, 0.0])
* rotation
).as_quat()
)
biased.append(record)
fitted = fit_partial_palm_orientation_measurement(
"thumb_cmc_roll_top_axis",
"thumb_cmc_roll",
biased,
cycle=0,
zero_command_u8=255,
minimum_arc_rad=math.radians(5.0),
maximum_command_distance_u8=64,
)
assert abs(
float(np.dot(expected.axis_common_xyz, fitted.axis_common_xyz))
) == pytest.approx(1.0, abs=1.0e-10)
def test_right_19_top_axis_pair_holdout_ignores_shared_frame_bias() -> None:
injected = [0.0] * 16
_, result = _solve_synthetic_offsets(
"right",
injected,
layout_id="g20_right_19",
palm_orientation_frame_bias_degrees=8.0,
)
assert result.passed is True
assert math.degrees(
result.direct_offsets_rad["thumb_cmc_yaw"]
) == pytest.approx(0.0, abs=0.05)
def test_partial_palm_direction_rejects_too_short_visible_arc() -> None:
records = _partial_orientation_records(
tag_mount=Rotation.identity(),
@@ -1066,7 +1169,7 @@ def test_right_19_offsets_are_invariant_to_rigid_hand_repositioning() -> None:
)
def test_right_19_only_verified_contact_zeros_use_mechanical_endpoints() -> None:
def test_right_19_roll_and_verified_contact_zeros_use_independent_endpoints() -> None:
hand = get_hand_calibration_profile("right", "g20_right_19")
curves = {
name: _synthetic_curve(255, math.radians(50.0))
@@ -1095,6 +1198,7 @@ def test_right_19_only_verified_contact_zeros_use_mechanical_endpoints() -> None
)
assert set(offsets) == {
"thumb_cmc_roll",
"thumb_mcp",
*(
f"{finger}_{suffix}"
@@ -1105,9 +1209,10 @@ def test_right_19_only_verified_contact_zeros_use_mechanical_endpoints() -> None
assert math.degrees(offsets["thumb_mcp"]) == pytest.approx(
math.degrees(1.25) - 70.0
)
assert not {
"thumb_cmc_roll", "thumb_cmc_yaw", "thumb_cmc_pitch"
}.intersection(offsets)
assert math.degrees(offsets["thumb_cmc_roll"]) == pytest.approx(
math.degrees(1.39) - 76.0
)
assert not {"thumb_cmc_yaw", "thumb_cmc_pitch"}.intersection(offsets)
assert math.degrees(offsets["index_mcp_pitch"]) == pytest.approx(
math.degrees(1.22) - 71.0
)
@@ -1141,8 +1246,10 @@ def _settled_endpoint_records(
for command, rotation in ((0, endpoint), (255, fixed_mounting)):
result.append(
{
"cycle": 0,
"direction": direction,
"requested_command_u8": command,
"feedback_u8": float(command),
"relative_quaternion_xyzw": rotation.as_quat().tolist(),
}
)
@@ -1152,6 +1259,7 @@ def _settled_endpoint_records(
def test_right_19_endpoint_curve_scale_uses_direct_rigid_rotation() -> None:
hand = get_hand_calibration_profile("right", "g20_right_19")
names = {
"thumb_cmc_roll",
"thumb_mcp",
*(
f"{finger}_{suffix}"
@@ -1169,7 +1277,11 @@ def test_right_19_endpoint_curve_scale_uses_direct_rigid_rotation() -> None:
name: _settled_endpoint_records(direct_travel) for name in names
}
anchored = anchor_right_19_mechanical_endpoint_curves(curves, records)
anchored = anchor_right_19_mechanical_endpoint_curves(
curves,
records,
feedback_endpoint_joints=frozenset({"thumb_cmc_roll"}),
)
assert curves["middle_pip"].angle_rad[0] == pytest.approx(
projected_travel
@@ -1186,6 +1298,7 @@ def test_right_19_endpoint_curve_scale_uses_direct_rigid_rotation() -> None:
def test_right_19_endpoint_curve_scale_rejects_direction_disagreement() -> None:
hand = get_hand_calibration_profile("right", "g20_right_19")
names = {
"thumb_cmc_roll",
"thumb_mcp",
*(
f"{finger}_{suffix}"
@@ -1207,7 +1320,11 @@ def test_right_19_endpoint_curve_scale_rejects_direction_disagreement() -> None:
)
with pytest.raises(ValueError, match="endpoint directions disagree"):
anchor_right_19_mechanical_endpoint_curves(curves, records)
anchor_right_19_mechanical_endpoint_curves(
curves,
records,
feedback_endpoint_joints=frozenset({"thumb_cmc_roll"}),
)
def test_right_15_finger_roll_limit_applies_to_independent_deviation() -> None:
@@ -1565,6 +1682,31 @@ def test_right_19_audits_stable_cross_view_cone_bias_without_retry() -> None:
] == "stable_cross_view_or_planar_pnp_bias"
def test_right_19_thumb_yaw_uses_top_axis_pair_not_front_cone_bias() -> None:
zero = get_zero_calibration_profile("right", "g20_right_19")
injected = [0.0] * len(zero.direct_zero_joints)
injected[zero.direct_zero_joints.index("thumb_cmc_roll")] = 3.611
injected[zero.direct_zero_joints.index("thumb_cmc_yaw")] = -1.319
zero, result = _solve_synthetic_offsets(
"right",
injected,
layout_id="g20_right_19",
inject_observer_cone_bias_degrees=3.803,
maximum_systematic_axis_cone_bias_degrees=15.0,
)
assert result.passed is True
assert math.degrees(
result.direct_offsets_rad["thumb_cmc_roll"]
) == pytest.approx(3.611, abs=0.05)
assert math.degrees(
result.direct_offsets_rad["thumb_cmc_yaw"]
) == pytest.approx(-1.319, abs=0.05)
assert math.degrees(
result.axis_cone_mismatch_by_joint_rad["thumb_cmc_yaw"]
) == pytest.approx(3.803, abs=0.01)
def test_right_19_still_rejects_gross_cross_view_cone_mismatch() -> None:
injected = [0.0] * 16
_, result = _solve_synthetic_offsets(
@@ -75,10 +75,10 @@ _HAND_CONFIGS: Dict[str, HandConfig] = {
"点赞": [255, 0, 0, 0, 0, 255, 162, 162, 144, 100, 210, 255, 255, 255, 255, 255, 0, 0, 0, 0],
"握拳": [96, 0, 0, 0, 0, 0, 193, 158, 128, 91, 132, 255, 255, 255, 255, 144, 0, 0, 0, 0],
"张开": [255, 255, 255, 255, 255, 255, 193, 148, 105, 42, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255],
"OK": [0, 0, 255, 255, 255, 151, 147, 148, 105, 42, 109, 255, 255, 255, 255, 255, 225, 255, 255, 255],
"拇指对中指": [0, 255, 0, 255, 255, 119, 149, 148, 105, 42, 109, 255, 255, 255, 255, 255, 225, 220, 255, 255],
"拇指对无名指": [0, 255, 255, 0, 255, 88, 149, 148, 105, 42, 109, 255, 255, 255, 255, 255, 255, 255, 229, 254],
"拇指对小指": [0, 255, 255, 255, 0, 49, 149, 148, 105, 42, 109, 255, 255, 255, 255, 255, 255, 255, 255, 215],
"OK": [0, 0, 255, 255, 255, 138, 147, 148, 105, 42, 109, 255, 255, 255, 255, 255, 211, 255, 255, 255],
"拇指对中指": [0, 255, 0, 255, 255, 107, 149, 148, 105, 42, 109, 255, 255, 255, 255, 255, 225, 202, 255, 255],
"拇指对无名指": [0, 255, 255, 0, 255, 88, 171, 148, 105, 42, 59, 255, 255, 255, 255, 255, 255, 255, 206, 254],
"拇指对小指": [0, 255, 255, 255, 0, 32, 170, 148, 105, 42, 109, 255, 255, 255, 255, 255, 255, 255, 255, 203],
"准备1": [255, 0, 0, 0, 0, 255, 162, 162, 144, 100, 210, 255, 255, 255, 255, 255, 0, 0, 0, 0],
"": [96, 255, 0, 0, 0, 0, 190, 161, 127, 80, 68, 255, 255, 255, 255, 144, 255, 0, 0, 0],
"": [96, 255, 255, 0, 0, 0, 190, 66, 127, 80, 68, 255, 255, 255, 255, 144, 255, 255, 0, 0],