diff --git a/src/g20_thumb_apriltag_calibration/README.md b/src/g20_thumb_apriltag_calibration/README.md index 55aade3..5021394 100644 --- a/src/g20_thumb_apriltag_calibration/README.md +++ b/src/g20_thumb_apriltag_calibration/README.md @@ -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`四项物理任务，其余任务的原始记录导入后仍按 +当前硬门限重新验证。 + 开发阶段若上一次会话失败，同一命令会自动校验硬件/几何哈希，并恢复已经 完整提交的关节任务；失败中的当前任务始终丢弃重做，位于它后面但已经完整通过的 独立任务仍会复用，不再因“连续前缀”限制整段重采。导入的任务会立即用与 diff --git a/src/g20_thumb_apriltag_calibration/config/g20_right_product.yaml b/src/g20_thumb_apriltag_calibration/config/g20_right_product.yaml index 07f3f5a..5f47411 100644 --- a/src/g20_thumb_apriltag_calibration/config/g20_right_product.yaml +++ b/src/g20_thumb_apriltag_calibration/config/g20_right_product.yaml @@ -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 diff --git a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/full_hand.py b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/full_hand.py index 952da99..736f537 100644 --- a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/full_hand.py +++ b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/full_hand.py @@ -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", } ), ) diff --git a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/offline_replay.py b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/offline_replay.py index 9b8eca2..f2a99f5 100644 --- a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/offline_replay.py +++ b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/offline_replay.py @@ -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) diff --git a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/one_command.py b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/one_command.py index a9fcc4d..41c9717 100644 --- a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/one_command.py +++ b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/one_command.py @@ -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(): diff --git a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/operator_report.py b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/operator_report.py index df512ba..f62169a 100644 --- a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/operator_report.py +++ b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/operator_report.py @@ -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", diff --git a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/publication.py b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/publication.py index 0f9b8df..60bb1da 100644 --- a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/publication.py +++ b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/publication.py @@ -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, diff --git a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/sample_schema.py b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/sample_schema.py new file mode 100644 index 0000000..a39bddd --- /dev/null +++ b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/sample_schema.py @@ -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 diff --git a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/three_camera_diagnostics.py b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/three_camera_diagnostics.py index 9761db9..0e9e619 100644 --- a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/three_camera_diagnostics.py +++ b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/three_camera_diagnostics.py @@ -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, diff --git a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/three_camera_node.py b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/three_camera_node.py index f0404e2..1f0e9f8 100644 --- a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/three_camera_node.py +++ b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/three_camera_node.py @@ -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"] ), diff --git a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/urdf_zero.py b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/urdf_zero.py index 56b94d1..f1d6b20 100644 --- a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/urdf_zero.py +++ b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/urdf_zero.py @@ -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 diff --git a/src/g20_thumb_apriltag_calibration/launch/three_camera_calibration.launch.py b/src/g20_thumb_apriltag_calibration/launch/three_camera_calibration.launch.py index e0b0661..4187281 100644 --- a/src/g20_thumb_apriltag_calibration/launch/three_camera_calibration.launch.py +++ b/src/g20_thumb_apriltag_calibration/launch/three_camera_calibration.launch.py @@ -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__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( diff --git a/src/g20_thumb_apriltag_calibration/test/test_full_hand.py b/src/g20_thumb_apriltag_calibration/test/test_full_hand.py index e06b927..14c0fba 100644 --- a/src/g20_thumb_apriltag_calibration/test/test_full_hand.py +++ b/src/g20_thumb_apriltag_calibration/test/test_full_hand.py @@ -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 diff --git a/src/g20_thumb_apriltag_calibration/test/test_g20_right_product.py b/src/g20_thumb_apriltag_calibration/test/test_g20_right_product.py index e1fe3dc..be4421c 100644 --- a/src/g20_thumb_apriltag_calibration/test/test_g20_right_product.py +++ b/src/g20_thumb_apriltag_calibration/test/test_g20_right_product.py @@ -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=(), diff --git a/src/g20_thumb_apriltag_calibration/test/test_sample_schema.py b/src/g20_thumb_apriltag_calibration/test/test_sample_schema.py new file mode 100644 index 0000000..8267b70 --- /dev/null +++ b/src/g20_thumb_apriltag_calibration/test/test_sample_schema.py @@ -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) diff --git a/src/g20_thumb_apriltag_calibration/test/test_three_camera_diagnostics.py b/src/g20_thumb_apriltag_calibration/test/test_three_camera_diagnostics.py index c17a504..a2c545d 100644 --- a/src/g20_thumb_apriltag_calibration/test/test_three_camera_diagnostics.py +++ b/src/g20_thumb_apriltag_calibration/test/test_three_camera_diagnostics.py @@ -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 10–13" 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: diff --git a/src/g20_thumb_apriltag_calibration/test/test_three_camera_retry.py b/src/g20_thumb_apriltag_calibration/test/test_three_camera_retry.py index 296b52e..4443e10 100644 --- a/src/g20_thumb_apriltag_calibration/test/test_three_camera_retry.py +++ b/src/g20_thumb_apriltag_calibration/test/test_three_camera_retry.py @@ -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) diff --git a/src/g20_thumb_apriltag_calibration/test/test_urdf_zero.py b/src/g20_thumb_apriltag_calibration/test/test_urdf_zero.py index d27c6cc..4cc57c5 100644 --- a/src/g20_thumb_apriltag_calibration/test/test_urdf_zero.py +++ b/src/g20_thumb_apriltag_calibration/test/test_urdf_zero.py @@ -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( diff --git a/src/gui_control/gui_control/config/constants.py b/src/gui_control/gui_control/config/constants.py index 95d297c..a80e5ae 100644 --- a/src/gui_control/gui_control/config/constants.py +++ b/src/gui_control/gui_control/config/constants.py @@ -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],