O6gui预设动作修改、O12标定初始化提交

.gitignore

@@ -62,13 +62,16 @@ Thumbs.db
 # src/linkerhand_retarget/resource/linkerforce_v2/profiles/.
 /profiles/
 /calibration_output/
-/config/g20_three_camera_extrinsics.yaml
+/config/*_three_camera_extrinsics.yaml
 *.wear_check.json
 *.checkpoint.json
 *.verification.json
 *_mapping_quality.json
 
 # Device-specific robot descriptions derived from local calibration runs
+# Includes full/partial zero-calibration outputs and local copies.
+/src/linkerhand_calibration/urdf/*/*_zero_calibrated*.urdf
+/src/linkerhand_calibration/urdf/*/*_transferred_from_*.urdf
 /src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/linkerhand_g20_left_cmc_pitch_*.urdf
 /src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/linkerhand_g20_left_calibrated_*.urdf
 /src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/linkerhand_g20_left_zero_calibrated_*.urdf
@@ -92,3 +95,5 @@ candump-*
 # Local Codex/agent workspace metadata
 /.agents/
 /.codex/
+/.codebuddy/
+/.zcode/

src/g20_thumb_apriltag_calibration/config/three_camera_calibration.yaml

@@ -1,120 +0,0 @@
-g20_calibration:
-  ros__parameters:
-    command_topic: /g20/cb_left_hand_control_cmd
-    state_topic: /g20/cb_left_hand_state
-    info_topic: /g20/cb_left_hand_info
-    setting_topic: /g20/cb_hand_setting_cmd
-    front_camera_info_topic: /g20_calibration/front/camera/camera_info
-    front_detections_topic: /g20_calibration/front/apriltag/detections
-    side_camera_info_topic: /g20_calibration/side/camera/camera_info
-    side_detections_topic: /g20_calibration/side/apriltag/detections
-    top_camera_info_topic: /g20_calibration/top/camera/camera_info
-    top_detections_topic: /g20_calibration/top/apriltag/detections
-
-    # /start先下发并确认这个20通道基准姿态，稳定后才进入第一条扫描。
-    baseline_command_u8: [255, 255, 255, 255, 255, 255, 127, 127, 127, 127, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
-    normal_calibration_speed: 15
-    index_roll_calibration_speed: 5
-    index_flex_calibration_speed: 10
-    speed_setting_settle_seconds: 0.25
-
-    # tag36h11检测角点围成的黑色正方形实测为16 mm；不包含外围白边。
-    tag_size_m: 0.016
-    repetitions: 3
-    preflight_frames: 60
-    minimum_detection_rate: 0.95
-    minimum_detection_hz: 15.0
-    maximum_hamming: 0
-    minimum_decision_margin: 30.0
-    minimum_edge_pixels: 30.0
-
-    pnp_maximum_reprojection_error_px: 1.5
-    pnp_reprojection_tie_px: 1.5
-    pnp_maximum_pose_jump_deg: 35.0
-    pnp_maximum_translation_jump_m: 0.04
-    pnp_maximum_tag_tilt_deg: 75.0
-    pnp_tracker_reset_seconds: 5.0
-    # 标定任务不再用第一帧决定平面Tag的IPPE分支；静止端点联合8帧选择整组最稳定解。
-    pnp_group_initialization_frames: 8
-    # 侧面Tag 4/5/6/7在初始化端点的贴面法向应一致；用此先验消除静态IPPE镜像双解。
-    pnp_group_normal_alignment_scale_deg: 5.0
-    pnp_group_maximum_normal_alignment_deg: 15.0
-    top_pnp_invalid_reset_seconds: 1.0
-    # 三维位姿必须与实测20通道状态严格按时间戳配对。
-    maximum_state_image_skew_ms: 50.0
-
-    axis_maximum_plane_rms_m: 0.003
-    # 被动耦合轴只用轨迹确定轴线位置，允许更大的轴向深度噪声；径向和跨轮
-    # 轴线一致性仍沿用严格检查。
-    passive_axis_maximum_plane_rms_m: 0.004
-    axis_maximum_radial_rms_m: 0.003
-    # 整段相对SE(3)运动拟合轴线点；端视关节会投影掉单目PnP光轴深度。
-    axis_maximum_pose_line_rms_m: 0.001
-    # 仅用于运动平面在三维中可观测的斜视关节；近图像平面关节使用姿态轴
-    # 约束三维圆，不让单目平面Tag的深度噪声自由决定转轴方向。
-    axis_maximum_rotation_circle_difference_deg: 1.0
-    # 单轴模型残差与跨轮重复误差分开判定：主动刚性关节要求更严；被动耦合
-    # 关节允许可重复的非理想单轴分量，但仍须通过0.75°跨轮轴差及第三轮留出。
-    active_maximum_rotation_orthogonal_rms_deg: 2.5
-    passive_maximum_rotation_orthogonal_rms_deg: 7.5
-    zero_maximum_axis_cycle_difference_deg: 0.75
-    # 零位无法改变父子轴夹角；超过该值属于CAD/PnP几何错误，不能吸收到零位。
-    zero_maximum_axis_cone_mismatch_deg: 5.0
-    zero_maximum_offset_deg: 20.0
-    # 四指绝对静态零偏默认保护范围。MCP侧摆只保留实测动态曲线，静态零位固定为CAD 0。
-    zero_finger_maximum_offset_deg: 3.0
-
-    endpoint_tolerance_u8: 2.0
-    # 电机10在命令0时实测会稳定反馈为4；该0端使用±4。
-    thumb_yaw_zero_endpoint_tolerance_u8: 4.0
-    # 右手电机10在命令255时多次实测稳定反馈为250；仅右手该端点使用±5。
-    right_thumb_yaw_255_endpoint_tolerance_u8: 5.0
-    # 右手小指PIP电机19在命令0时固件反馈稳定饱和为5；仅其0端使用±5。
-    pinky_pip_zero_endpoint_tolerance_u8: 5.0
-    endpoint_hold_seconds: 0.5
-    baseline_hold_seconds: 0.5
-    position_timeout_seconds: 30.0
-    sweep_timeout_seconds: 90.0
-    # 反馈在远离目标时连续8秒没有至少1个u8的进展，按机械卡滞立即暂停；
-    # 这类故障不进入遮挡/超时的三次自动重扫。
-    motor_stall_timeout_seconds: 8.0
-    motor_stall_minimum_progress_u8: 1.0
-    invalid_timeout_seconds: 3.0
-    minimum_sweep_frames: 40
-    minimum_state_span_u8: 240.0
-    minimum_sweep_bins: 32
-    maximum_bin_gap: 16
-    # 可恢复的采样失败自动重扫当前方向；超过次数才暂停等待人工处理。
-    automatic_sweep_retry_limit: 3
-    # 轨迹拟合失败优先只重扫失败轮次；零位/URDF模型失败不重复运动。
-    automatic_fit_retry_limit: 2
-    automatic_motion_retry_limit: 2
-    # 过程检查允许25%的黄色预警带，最终验收仍使用下面的严格门限。
-    provisional_warning_ratio: 1.25
-    retry_minimum_speed: 3
-    retry_speed_scales: [0.8, 0.6, 0.5]
-    retry_endpoint_hold_seconds: [0.75, 1.0, 1.25]
-
-    trajectory_maximum_plane_rms_m: 0.004
-    trajectory_maximum_radial_rms_m: 0.004
-    trajectory_minimum_radius_m: 0.003
-    trajectory_minimum_arc_deg: 15.0
-    # 以下二维参数只供旧轨迹工具兼容，三机位v4零位不使用二维投影。
-    image_trajectory_maximum_radial_rms_px: 2.0
-    image_trajectory_maximum_radial_p95_px: 3.5
-    image_trajectory_minimum_radius_px: 20.0
-    trajectory_maximum_cycle_travel_difference_deg: 3.0
-    passive_maximum_cycle_travel_difference_deg: 10.0
-    maximum_monotonic_correction_deg: 2.0
-    maximum_hysteresis_deg: 5.0
-    passive_maximum_monotonic_correction_deg: 3.0
-    passive_maximum_hysteresis_deg: 7.5
-
-    # 默认无额外随机动作；第三轮扫描始终作为不可关闭的留出验证。
-    validation_enabled: false
-    validation_command_count: 3
-    validation_frames: 10
-    validation_seed: 20260804
-    validation_timeout_seconds: 20.0
-    maximum_validation_mae_deg: 1.0
-    maximum_validation_p95_deg: 2.0

src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/__init__.py

@@ -1,5 +0,0 @@
-"""Front-camera AprilTag calibration for the left LinkerHand G20 thumb."""
-
-from .core import BASELINE_COMMAND, COMMAND_NAMES
-
-__all__ = ["BASELINE_COMMAND", "COMMAND_NAMES"]

src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/calibrated_joint_state_bridge.py

@@ -1,218 +0,0 @@
-"""Map G20 u8 commands to URDF joint angles using one calibration JSON.
-
-The static encoder-zero corrections in ``zero_angles`` are already baked into
-the corrected URDF joint origins.  This bridge therefore publishes only the
-dynamic ``angle_rad`` values and never adds the static offsets a second time.
-"""
-
-from __future__ import annotations
-
-import json
-import math
-from pathlib import Path
-from typing import Any, Mapping, Sequence
-
-import rclpy
-from rclpy.node import Node
-from sensor_msgs.msg import JointState
-
-from .full_hand import get_hand_calibration_profile, validate_compact_payload
-
-
-G20_COMMAND_NAMES: tuple[str, ...] = (
-    "thumb_cmc_pitch",
-    "index_mcp_pitch",
-    "middle_mcp_pitch",
-    "ring_mcp_pitch",
-    "pinky_mcp_pitch",
-    "thumb_cmc_roll",
-    "index_mcp_roll",
-    "middle_mcp_roll",
-    "ring_mcp_roll",
-    "pinky_mcp_roll",
-    "thumb_cmc_yaw",
-    "reserved_11",
-    "reserved_12",
-    "reserved_13",
-    "reserved_14",
-    "thumb_mcp",
-    "index_pip",
-    "middle_pip",
-    "ring_pip",
-    "pinky_pip",
-)
-
-# Match the stable ordering used by the existing MuJoCo bridge.  JointState
-# consumers must use names, but retaining the ordering also keeps logs and
-# direct comparisons deterministic.
-G20_URDF_JOINT_NAMES: tuple[str, ...] = (
-    "index_dip",
-    "index_mcp_pitch",
-    "index_mcp_roll",
-    "index_pip",
-    "middle_dip",
-    "middle_mcp_pitch",
-    "middle_mcp_roll",
-    "middle_pip",
-    "pinky_dip",
-    "pinky_mcp_pitch",
-    "pinky_mcp_roll",
-    "pinky_pip",
-    "ring_dip",
-    "ring_mcp_pitch",
-    "ring_mcp_roll",
-    "ring_pip",
-    "thumb_cmc_pitch",
-    "thumb_cmc_roll",
-    "thumb_cmc_yaw",
-    "thumb_ip",
-    "thumb_mcp",
-)
-
-
-class CalibratedCommandMapper:
-    """Validated, side-specific lookup from a G20 command to URDF radians."""
-
-    def __init__(
-        self, payload: Mapping[str, Any], *, expected_side: str | None = None
-    ) -> None:
-        validate_compact_payload(payload)
-        side = str(payload["side"]).lower()
-        if expected_side is not None and side != str(expected_side).lower():
-            raise ValueError(
-                f"calibration side {side!r} does not match requested side "
-                f"{str(expected_side).lower()!r}"
-            )
-        quality = payload["quality"]
-        if quality.get("passed") is not True:
-            raise ValueError("calibration quality.passed must be true")
-        profile = get_hand_calibration_profile(side)
-        self.side = side
-        self.serial_number = str(payload["serial_number"])
-        self._motor_by_joint = {
-            name: int(profile.joint_specs[name].motor_index)
-            for name in G20_URDF_JOINT_NAMES
-        }
-        self._curves = {
-            name: tuple(
-                float(value)
-                for value in payload["joints"][name]["angle_rad"]
-            )
-            for name in G20_URDF_JOINT_NAMES
-        }
-
-    @staticmethod
-    def _command_index(value: float) -> int:
-        command = float(value)
-        if not math.isfinite(command):
-            raise ValueError("G20 command positions must be finite")
-        return max(0, min(255, int(math.floor(command + 0.5))))
-
-    def map_positions(
-        self, positions: Sequence[float], names: Sequence[str] = ()
-    ) -> tuple[float, ...]:
-        values = tuple(float(value) for value in positions)
-        if names:
-            if len(names) != len(values):
-                raise ValueError(
-                    "JointState names and positions must have equal length"
-                )
-            if len(set(names)) != len(names):
-                raise ValueError("JointState names must be unique")
-            by_name = dict(zip((str(name) for name in names), values))
-            missing = [name for name in G20_COMMAND_NAMES if name not in by_name]
-            if missing:
-                raise ValueError(
-                    "G20 command is missing named channels: " + ",".join(missing)
-                )
-            command = tuple(by_name[name] for name in G20_COMMAND_NAMES)
-        else:
-            if len(values) != len(G20_COMMAND_NAMES):
-                raise ValueError(
-                    "unnamed G20 command must contain exactly 20 positions"
-                )
-            command = values
-        indices = tuple(self._command_index(value) for value in command)
-        return tuple(
-            self._curves[name][indices[self._motor_by_joint[name]]]
-            for name in G20_URDF_JOINT_NAMES
-        )
-
-
-def load_calibrated_command_mapper(
-    calibration_file: str | Path, *, expected_side: str | None = None
-) -> CalibratedCommandMapper:
-    path = Path(calibration_file).expanduser().resolve()
-    if not path.is_file():
-        raise ValueError(f"calibration JSON does not exist: {path}")
-    payload = json.loads(path.read_text(encoding="utf-8"))
-    return CalibratedCommandMapper(payload, expected_side=expected_side)
-
-
-class CalibratedJointStateBridge(Node):
-    def __init__(self) -> None:
-        super().__init__("g20_calibrated_joint_state_bridge")
-        self.declare_parameter("hand_type", "right")
-        self.declare_parameter("calibration_file", "")
-        self.declare_parameter("input_topic", "")
-        self.declare_parameter("output_topic", "")
-
-        hand_type = str(self.get_parameter("hand_type").value).lower()
-        if hand_type not in {"left", "right"}:
-            raise ValueError("hand_type must be left or right")
-        calibration_file = str(self.get_parameter("calibration_file").value)
-        if not calibration_file:
-            raise ValueError("calibration_file is required")
-        self.mapper = load_calibrated_command_mapper(
-            calibration_file, expected_side=hand_type
-        )
-        input_topic = str(self.get_parameter("input_topic").value).strip()
-        output_topic = str(self.get_parameter("output_topic").value).strip()
-        self.input_topic = input_topic or f"/cb_{hand_type}_hand_control_cmd"
-        self.output_topic = (
-            output_topic or f"/sim/mujoco/g20/{hand_type}/joint_state"
-        )
-        self.publisher = self.create_publisher(JointState, self.output_topic, 10)
-        self.subscription = self.create_subscription(
-            JointState, self.input_topic, self._command_callback, 10
-        )
-        self._last_error = ""
-        self.get_logger().info(
-            f"loaded {hand_type} G20 calibration for {self.mapper.serial_number}: "
-            f"{self.input_topic} -> {self.output_topic}"
-        )
-
-    def _command_callback(self, command: JointState) -> None:
-        try:
-            positions = self.mapper.map_positions(command.position, command.name)
-        except ValueError as error:
-            message = str(error)
-            if message != self._last_error:
-                self.get_logger().error(message)
-                self._last_error = message
-            return
-        self._last_error = ""
-        result = JointState()
-        result.header = command.header
-        result.name = list(G20_URDF_JOINT_NAMES)
-        result.position = list(positions)
-        self.publisher.publish(result)
-
-
-def main(args: Sequence[str] | None = None) -> None:
-    rclpy.init(args=args)
-    node: CalibratedJointStateBridge | None = None
-    try:
-        node = CalibratedJointStateBridge()
-        rclpy.spin(node)
-    except KeyboardInterrupt:
-        pass
-    finally:
-        if node is not None:
-            node.destroy_node()
-        if rclpy.ok():
-            rclpy.shutdown()
-
-
-if __name__ == "__main__":
-    main()

src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/full_hand.py

@@ -1,878 +0,0 @@
-"""Pure three-camera G20 calibration model and compact runtime schema.
-
-The hardware node records one parent/child AprilTag trajectory for each
-directly observable joint.  This module deliberately contains no ROS imports:
-curve fitting, baseline centring, inheritance, and schema validation
-remain deterministic and unit-testable without connected cameras or a hand.
-"""
-
-from __future__ import annotations
-
-from dataclasses import dataclass, replace
-import math
-from typing import Any, Mapping, Sequence
-
-import numpy as np
-
-from .trajectory import (
-    _angle_for_circle,
-    _fit_circle_with_axis,
-    _fit_joint_curve,
-    _fit_plane_axis,
-    _orient_circle_positive,
-)
-from .zero_calibration import (
-    _fit_circle,
-    _trajectory_arc_rad,
-    circular_median_rad,
-    maximum_pairwise_angle_difference_rad,
-    wrap_angle_rad,
-)
-
-
-THREE_CAMERA_BASELINE_COMMAND: tuple[int, ...] = (
-    255, 255, 255, 255, 255, 255,
-    127, 127, 127, 127,
-    255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
-)
-
-
-@dataclass(frozen=True)
-class JointSpec:
-    name: str
-    motor_index: int
-    active: bool
-    view: str | None
-    parent_role: str | None
-    child_role: str | None
-    source_joint: str | None = None
-    zero_kind: str | None = None
-
-    @property
-    def measured(self) -> bool:
-        return self.source_joint is None
-
-
-@dataclass(frozen=True)
-class SweepSpec:
-    view: str
-    motor_index: int
-    joints: tuple[str, ...]
-
-
-@dataclass(frozen=True)
-class JointCurveFit:
-    angle_rad: tuple[float, ...]
-    decreasing_rad: tuple[float, ...]
-    increasing_rad: tuple[float, ...]
-    circle: Mapping[str, Any]
-    maximum_monotonic_correction_rad: float
-    maximum_hysteresis_rad: float
-    quality: Mapping[str, float]
-    zero_offset_rad: float = 0.0
-
-
-@dataclass(frozen=True)
-class HandCalibrationProfile:
-    side: str
-    reference_finger: str
-    view_tags: Mapping[str, Mapping[str, int]]
-    preflight_view_roles: Mapping[str, tuple[str, ...]]
-    joint_specs: Mapping[str, JointSpec]
-    sweep_specs: tuple[SweepSpec, ...]
-    image_trajectory_joints: frozenset[str]
-    roll_clearance_commands: Mapping[int, int]
-    thumb_pitch_clearance_commands: Mapping[int, int]
-
-    @property
-    def measured_joints(self) -> tuple[str, ...]:
-        return tuple(
-            name for name, spec in self.joint_specs.items() if spec.measured
-        )
-
-    @property
-    def active_joints(self) -> tuple[str, ...]:
-        return tuple(
-            name for name, spec in self.joint_specs.items() if spec.active
-        )
-
-    @property
-    def passive_joints(self) -> tuple[str, ...]:
-        return tuple(
-            name for name, spec in self.joint_specs.items() if not spec.active
-        )
-
-    @property
-    def reference_roll_motor(self) -> int:
-        return int(self.joint_specs[f"{self.reference_finger}_mcp_roll"].motor_index)
-
-    @property
-    def reference_pitch_motor(self) -> int:
-        return int(self.joint_specs[f"{self.reference_finger}_mcp_pitch"].motor_index)
-
-    @property
-    def reference_pip_motor(self) -> int:
-        return int(self.joint_specs[f"{self.reference_finger}_pip"].motor_index)
-
-    @property
-    def reference_speed_slot(self) -> int:
-        return {"index": 1, "middle": 2, "ring": 3, "pinky": 4}[
-            self.reference_finger
-        ]
-
-
-_FINGERS: tuple[str, ...] = ("index", "middle", "ring", "pinky")
-_MOTOR_BY_JOINT: dict[str, int] = {
-    **{f"{finger}_mcp_pitch": index + 1 for index, finger in enumerate(_FINGERS)},
-    **{f"{finger}_mcp_roll": index + 6 for index, finger in enumerate(_FINGERS)},
-    **{f"{finger}_pip": index + 16 for index, finger in enumerate(_FINGERS)},
-}
-
-
-def _build_hand_profile(side: str) -> HandCalibrationProfile:
-    hand_side = str(side).lower()
-    if hand_side not in {"left", "right"}:
-        raise ValueError("hand side must be left or right")
-    reference = "index" if hand_side == "left" else "pinky"
-    roll_role = f"{reference}_roll"
-    mcp_role = f"{reference}_mcp"
-    pip_role = f"{reference}_pip"
-    dip_role = f"{reference}_dip"
-    view_tags: dict[str, dict[str, int]] = {
-        "front": {
-            "front_base": 0,
-            "thumb_cmc": 1,
-            "thumb_mcp": 2,
-            "thumb_ip": 3,
-            roll_role: 10,
-        },
-        "side": {
-            "side_base": 4,
-            mcp_role: 5,
-            pip_role: 6,
-            dip_role: 7,
-        },
-        "top": {"top_base": 8, "thumb_yaw": 9},
-    }
-    preflight_view_roles = {
-        view: tuple(tags) for view, tags in view_tags.items()
-    }
-    joint_specs: dict[str, JointSpec] = {}
-    # Both hands use the front base/Tag 1 pair for motor 0.  Keeping the
-    # complete thumb root chain in one calibrated view avoids introducing a
-    # cross-camera axis-line phase into the URDF zero solve.
-    thumb_pitch_view = "front"
-    thumb_pitch_parent = "front_base"
-    joint_specs["thumb_cmc_pitch"] = JointSpec(
-        "thumb_cmc_pitch",
-        0,
-        True,
-        thumb_pitch_view,
-        thumb_pitch_parent,
-        "thumb_cmc",
-        zero_kind="urdf_axis_chain",
-    )
-    for finger in _FINGERS:
-        name = f"{finger}_mcp_pitch"
-        source = None if finger == reference else f"{reference}_mcp_pitch"
-        joint_specs[name] = JointSpec(
-            name,
-            _MOTOR_BY_JOINT[name],
-            True,
-            "side" if source is None else None,
-            "side_base" if source is None else None,
-            mcp_role if source is None else None,
-            source_joint=source,
-            zero_kind="urdf_axis_chain" if source is None else "inherited",
-        )
-    joint_specs["thumb_cmc_roll"] = JointSpec(
-        "thumb_cmc_roll", 5, True, "front", "front_base", "thumb_cmc",
-        zero_kind="urdf_axis_chain",
-    )
-    for finger in _FINGERS:
-        name = f"{finger}_mcp_roll"
-        source = None if finger == reference else f"{reference}_mcp_roll"
-        joint_specs[name] = JointSpec(
-            name,
-            _MOTOR_BY_JOINT[name],
-            True,
-            "front" if source is None else None,
-            "front_base" if source is None else None,
-            roll_role if source is None else None,
-            source_joint=source,
-            zero_kind="urdf_axis_chain" if source is None else "inherited",
-        )
-    joint_specs["thumb_cmc_yaw"] = JointSpec(
-        "thumb_cmc_yaw", 10, True, "top", "top_base", "thumb_yaw",
-        zero_kind="urdf_axis_chain",
-    )
-    joint_specs["thumb_mcp"] = JointSpec(
-        "thumb_mcp", 15, True, "front", "thumb_cmc", "thumb_mcp",
-        zero_kind="urdf_axis_chain",
-    )
-    for finger in _FINGERS:
-        name = f"{finger}_pip"
-        source = None if finger == reference else f"{reference}_pip"
-        joint_specs[name] = JointSpec(
-            name,
-            _MOTOR_BY_JOINT[name],
-            True,
-            "side" if source is None else None,
-            mcp_role if source is None else None,
-            pip_role if source is None else None,
-            source_joint=source,
-            zero_kind="urdf_axis_chain" if source is None else "inherited",
-        )
-    joint_specs["thumb_ip"] = JointSpec(
-        "thumb_ip", 15, False, "front", "thumb_mcp", "thumb_ip",
-    )
-    for finger in _FINGERS:
-        name = f"{finger}_dip"
-        source = None if finger == reference else f"{reference}_dip"
-        joint_specs[name] = JointSpec(
-            name,
-            _MOTOR_BY_JOINT[f"{finger}_pip"],
-            False,
-            "side" if source is None else None,
-            pip_role if source is None else None,
-            dip_role if source is None else None,
-            source_joint=source,
-        )
-    sweep_specs = (
-        SweepSpec(thumb_pitch_view, 0, ("thumb_cmc_pitch",)),
-        SweepSpec("front", 5, ("thumb_cmc_roll",)),
-        SweepSpec("front", 15, ("thumb_mcp", "thumb_ip")),
-        SweepSpec("front", _MOTOR_BY_JOINT[f"{reference}_mcp_roll"], (
-            f"{reference}_mcp_roll",
-        )),
-        SweepSpec("side", _MOTOR_BY_JOINT[f"{reference}_mcp_pitch"], (
-            f"{reference}_mcp_pitch",
-        )),
-        SweepSpec("side", _MOTOR_BY_JOINT[f"{reference}_pip"], (
-            f"{reference}_pip", f"{reference}_dip",
-        )),
-        SweepSpec("top", 10, ("thumb_cmc_yaw",)),
-    )
-    image_joints = frozenset(
-        {
-            "thumb_cmc_pitch",
-            "thumb_cmc_roll",
-            "thumb_mcp",
-            "thumb_ip",
-            f"{reference}_mcp_roll",
-            f"{reference}_mcp_pitch",
-            f"{reference}_pip",
-        }
-    )
-    measured_roll = _MOTOR_BY_JOINT[f"{reference}_mcp_roll"]
-    # The roll channels are mirrored mechanically between hands.  Command 0
-    # moves the three non-reference fingers to the camera-right clearance pose
-    # on the left hand, while the same physical pose is command 255 on the
-    # right hand.  Using 0 for both sides made the right index/middle/ring
-    # fingers lean left and could obstruct or load the pinky roll sweep.
-    roll_clearance_command = 0 if hand_side == "left" else 255
-    clearance = {
-        _MOTOR_BY_JOINT[f"{finger}_mcp_roll"]: roll_clearance_command
-        for finger in _FINGERS
-        if finger != reference
-    }
-    assert measured_roll not in clearance
-    # Motor 0 is observed from the front with both other thumb-root channels
-    # fully open.  Store the commands explicitly for the right hand so a
-    # caller-provided baseline cannot silently restore the obsolete 50/120
-    # pose and hide Tag 1 from the front camera.
-    thumb_pitch_clearance = {10: 255, 5: 255} if hand_side == "right" else {}
-    return HandCalibrationProfile(
-        side=hand_side,
-        reference_finger=reference,
-        view_tags=view_tags,
-        preflight_view_roles=preflight_view_roles,
-        joint_specs=joint_specs,
-        sweep_specs=sweep_specs,
-        image_trajectory_joints=image_joints,
-        roll_clearance_commands=clearance,
-        thumb_pitch_clearance_commands=thumb_pitch_clearance,
-    )
-
-
-LEFT_HAND_PROFILE = _build_hand_profile("left")
-RIGHT_HAND_PROFILE = _build_hand_profile("right")
-
-
-def get_hand_calibration_profile(side: str) -> HandCalibrationProfile:
-    value = str(side).lower()
-    if value == "left":
-        return LEFT_HAND_PROFILE
-    if value == "right":
-        return RIGHT_HAND_PROFILE
-    raise ValueError("hand side must be left or right")
-
-
-# Backwards-compatible aliases keep the existing left-hand API stable.
-VIEW_TAGS = LEFT_HAND_PROFILE.view_tags
-JOINT_SPECS = LEFT_HAND_PROFILE.joint_specs
-SWEEP_SPECS = LEFT_HAND_PROFILE.sweep_specs
-MEASURED_JOINTS = LEFT_HAND_PROFILE.measured_joints
-ACTIVE_JOINTS = LEFT_HAND_PROFILE.active_joints
-PASSIVE_JOINTS = LEFT_HAND_PROFILE.passive_joints
-SPLAY_JOINTS: tuple[str, ...] = (
-    "index_mcp_roll",
-    "middle_mcp_roll",
-    "ring_mcp_roll",
-    "pinky_mcp_roll",
-)
-
-# These directly actuated motions have a fixed parent link during their sweep
-# and a motion plane that is close to the active camera's image plane.  Their
-# projected circles are substantially more repeatable than the difference of
-# two independently estimated planar-Tag PnP depths.  thumb_ip remains projected
-# because its front-view PnP depth is not repeatable enough for a 3-D fit. The
-# side-view index_dip and oblique top-view yaw remain parent-relative full
-# SE(3) measurements; their circle directions are constrained by the more
-# repeatable relative-orientation screw axis in urdf_zero.py.
-IMAGE_TRAJECTORY_JOINTS = LEFT_HAND_PROFILE.image_trajectory_joints
-
-# Keep the three unmeasured finger-roll motors away from the front camera's
-# line of sight while index_mcp_roll is measured.
-INDEX_ROLL_CLEARANCE_COMMANDS = LEFT_HAND_PROFILE.roll_clearance_commands
-
-# Hold thumb CMC roll at a camera-friendly pose while thumb CMC yaw is
-# measured.  This keeps the moving top-view tag sufficiently front-facing.
-THUMB_YAW_CLEARANCE_COMMANDS: dict[int, int] = {
-    5: 145,
-}
-
-
-def calibration_auxiliary_commands(
-    spec: SweepSpec,
-    *,
-    profile: HandCalibrationProfile = LEFT_HAND_PROFILE,
-) -> dict[int, int]:
-    """Return motors that must remain fixed throughout one calibration task."""
-    if spec.motor_index == 0:
-        return dict(profile.thumb_pitch_clearance_commands)
-    if spec.motor_index == profile.reference_roll_motor:
-        return dict(profile.roll_clearance_commands)
-    if spec.motor_index == 10:
-        return dict(THUMB_YAW_CLEARANCE_COMMANDS)
-    return {}
-
-
-def build_full_hand_command(
-    motor_index: int,
-    command_u8: int,
-    baseline: Sequence[int] = THREE_CAMERA_BASELINE_COMMAND,
-    profile: HandCalibrationProfile = LEFT_HAND_PROFILE,
-) -> list[int]:
-    if len(baseline) != 20:
-        raise ValueError("baseline must contain exactly 20 values")
-    motor = int(motor_index)
-    if motor not in {spec.motor_index for spec in profile.joint_specs.values()}:
-        raise ValueError("motor_index is not a controlled G20 calibration channel")
-    command = int(command_u8)
-    if not 0 <= command <= 255:
-        raise ValueError("command_u8 must be in [0, 255]")
-    result = [int(value) for value in baseline]
-    if any(not 0 <= value <= 255 for value in result):
-        raise ValueError("baseline values must be in [0, 255]")
-    result[motor] = command
-    return result
-
-
-def build_calibration_motion_command(
-    spec: SweepSpec,
-    command_u8: int,
-    baseline: Sequence[int] = THREE_CAMERA_BASELINE_COMMAND,
-    profile: HandCalibrationProfile = LEFT_HAND_PROFILE,
-) -> list[int]:
-    """Build a sweep command, including any required clearance pose."""
-    result = build_full_hand_command(
-        spec.motor_index,
-        command_u8,
-        baseline=baseline,
-        profile=profile,
-    )
-    for motor_index, auxiliary_command in calibration_auxiliary_commands(
-        spec, profile=profile
-    ).items():
-        result[motor_index] = auxiliary_command
-    return result
-
-
-def build_calibration_speed_profile(
-    spec: SweepSpec,
-    *,
-    normal_speed: int,
-    index_roll_speed: int,
-    index_flex_speed: int,
-    profile: HandCalibrationProfile = LEFT_HAND_PROFILE,
-) -> list[int]:
-    """Return G20's five per-finger speeds for one calibration task."""
-    normal = int(normal_speed)
-    index_roll = int(index_roll_speed)
-    index_flex = int(index_flex_speed)
-    if not all(
-        0 <= speed <= 255 for speed in (normal, index_roll, index_flex)
-    ):
-        raise ValueError("calibration speeds must be in [0, 255]")
-    speeds = [normal] * 5
-    if spec.motor_index == profile.reference_roll_motor:
-        speeds[profile.reference_speed_slot] = index_roll
-    elif spec.motor_index in {
-        profile.reference_pitch_motor,
-        profile.reference_pip_motor,
-    }:
-        speeds[profile.reference_speed_slot] = index_flex
-    return speeds
-
-
-def _record_vector(record: Mapping[str, Any]) -> np.ndarray:
-    value = np.asarray(record.get("relative_translation_xyz_m"), dtype=float)
-    if value.shape != (3,) or not np.all(np.isfinite(value)):
-        raise ValueError("record relative_translation_xyz_m must contain 3 values")
-    return value
-
-
-def _record_image_vector(record: Mapping[str, Any]) -> np.ndarray:
-    value = np.asarray(record.get("image_relative_xy_px"), dtype=float)
-    if value.shape != (2,) or not np.all(np.isfinite(value)):
-        raise ValueError("record image_relative_xy_px must contain 2 values")
-    return value
-
-
-def fit_joint_center_curve(
-    records: Sequence[Mapping[str, Any]],
-    *,
-    maximum_plane_rms_m: float = 0.004,
-    maximum_radial_rms_m: float = 0.004,
-    minimum_radius_m: float = 0.003,
-    minimum_arc_rad: float = math.radians(15.0),
-) -> JointCurveFit:
-    """Fit one command-indexed curve from a parent-frame centre trajectory."""
-    samples = [dict(record) for record in records]
-    if len(samples) < 12:
-        raise ValueError("joint trajectory requires at least 12 samples")
-    points = np.asarray([_record_vector(record) for record in samples])
-    axis, common_plane_rms = _fit_plane_axis([points])
-    circle = _fit_circle_with_axis(points, axis)
-    circle = _orient_circle_positive(circle, samples, list(points))
-    values = [_angle_for_circle(point, circle) for point in points]
-    curves, correction, hysteresis = _fit_joint_curve(samples, values)
-    quality = {
-        "plane_rms_m": max(
-            float(common_plane_rms), float(circle["plane_rms_m"])
-        ),
-        "radial_rms_m": float(circle["radial_rms_m"]),
-        "radius_m": float(circle["radius_m"]),
-        "arc_rad": float(circle["observed_arc_rad"]),
-    }
-    failures: list[str] = []
-    if quality["plane_rms_m"] > float(maximum_plane_rms_m):
-        failures.append("plane_rms")
-    if quality["radial_rms_m"] > float(maximum_radial_rms_m):
-        failures.append("radial_rms")
-    if quality["radius_m"] < float(minimum_radius_m):
-        failures.append("radius")
-    if quality["arc_rad"] < float(minimum_arc_rad):
-        failures.append("arc")
-    if failures:
-        raise ValueError("joint_trajectory_quality_failed:" + ",".join(failures))
-    return JointCurveFit(
-        angle_rad=tuple(float(value) for value in curves["angle_rad"]),
-        decreasing_rad=tuple(
-            float(value) for value in curves["decreasing_rad"]
-        ),
-        increasing_rad=tuple(
-            float(value) for value in curves["increasing_rad"]
-        ),
-        circle=dict(circle),
-        maximum_monotonic_correction_rad=float(correction),
-        maximum_hysteresis_rad=float(hysteresis),
-        quality=quality,
-    )
-
-
-def _signed_image_circle_angle(
-    point_xy_px: Sequence[float], circle: Mapping[str, Any]
-) -> float:
-    point = np.asarray(point_xy_px, dtype=float)
-    centre = np.asarray(circle["center_xy_px"], dtype=float)
-    reference = np.asarray(circle["reference_xy_px"], dtype=float)
-    vector = point - centre
-    if point.shape != (2,) or not np.all(np.isfinite(point)):
-        raise ValueError("image point must contain 2 finite values")
-    if float(np.linalg.norm(vector)) < 1.0e-9:
-        raise ValueError("image point lies at the fitted circle centre")
-    return float(circle["orientation_sign"]) * math.atan2(
-        float(reference[0] * vector[1] - reference[1] * vector[0]),
-        float(np.dot(reference, vector)),
-    )
-
-
-def fit_joint_image_curve(
-    records: Sequence[Mapping[str, Any]],
-    *,
-    maximum_radial_rms_px: float = 2.0,
-    maximum_radial_p95_px: float = 3.5,
-    minimum_radius_px: float = 20.0,
-    minimum_arc_rad: float = math.radians(15.0),
-) -> JointCurveFit:
-    """Fit angular phase from a stable projected parent/child circle."""
-    samples = [dict(record) for record in records]
-    if len(samples) < 12:
-        raise ValueError("joint image trajectory requires at least 12 samples")
-    points = np.asarray([_record_image_vector(record) for record in samples])
-    centre, radius = _fit_circle(points)
-    radial_error = np.abs(np.linalg.norm(points - centre, axis=1) - radius)
-    quality = {
-        "radial_rms_px": float(np.sqrt(np.mean(np.square(radial_error)))),
-        "radial_p95_px": float(np.percentile(radial_error, 95.0)),
-        "radius_px": float(radius),
-        "arc_rad": float(_trajectory_arc_rad(points, centre)),
-    }
-    failures: list[str] = []
-    if quality["radial_rms_px"] > float(maximum_radial_rms_px):
-        failures.append("radial_rms")
-    if quality["radial_p95_px"] > float(maximum_radial_p95_px):
-        failures.append("radial_p95")
-    if quality["radius_px"] < float(minimum_radius_px):
-        failures.append("radius")
-    if quality["arc_rad"] < float(minimum_arc_rad):
-        failures.append("arc")
-    if failures:
-        raise ValueError(
-            "joint_image_trajectory_quality_failed:" + ",".join(failures)
-        )
-
-    endpoint_points = np.asarray(
-        [
-            point
-            for record, point in zip(samples, points)
-            if int(record["command_u8"]) == 255
-        ]
-    )
-    if endpoint_points.size == 0:
-        raise ValueError("joint image trajectory is missing command 255")
-    reference = np.median(endpoint_points, axis=0) - centre
-    reference_norm = float(np.linalg.norm(reference))
-    if reference_norm < 1.0e-9:
-        raise ValueError("joint image trajectory endpoint is degenerate")
-    reference /= reference_norm
-    circle: dict[str, Any] = {
-        "space": "image_2d",
-        "center_xy_px": [float(value) for value in centre],
-        "reference_xy_px": [float(value) for value in reference],
-        "orientation_sign": 1.0,
-        "radius_px": float(radius),
-    }
-    values = [_signed_image_circle_angle(point, circle) for point in points]
-    command_zero_values = [
-        value
-        for record, value in zip(samples, values)
-        if int(record["command_u8"]) == 0
-    ]
-    if not command_zero_values:
-        raise ValueError("joint image trajectory is missing command 0")
-    if float(np.median(command_zero_values)) < 0.0:
-        circle["orientation_sign"] = -1.0
-        values = [-value for value in values]
-    curves, correction, hysteresis = _fit_joint_curve(samples, values)
-    return JointCurveFit(
-        angle_rad=tuple(float(value) for value in curves["angle_rad"]),
-        decreasing_rad=tuple(
-            float(value) for value in curves["decreasing_rad"]
-        ),
-        increasing_rad=tuple(
-            float(value) for value in curves["increasing_rad"]
-        ),
-        circle=circle,
-        maximum_monotonic_correction_rad=float(correction),
-        maximum_hysteresis_rad=float(hysteresis),
-        quality=quality,
-    )
-
-
-def fit_measured_joint_curve(
-    joint_name: str,
-    records: Sequence[Mapping[str, Any]],
-    *,
-    maximum_plane_rms_m: float = 0.004,
-    maximum_radial_rms_m: float = 0.004,
-    minimum_radius_m: float = 0.003,
-    minimum_arc_rad: float = math.radians(15.0),
-    image_maximum_radial_rms_px: float = 2.0,
-    image_maximum_radial_p95_px: float = 3.5,
-    image_minimum_radius_px: float = 20.0,
-    profile: HandCalibrationProfile = LEFT_HAND_PROFILE,
-) -> JointCurveFit:
-    """Select the stable trajectory representation for one measured joint."""
-    if str(joint_name) in profile.image_trajectory_joints:
-        return fit_joint_image_curve(
-            records,
-            maximum_radial_rms_px=image_maximum_radial_rms_px,
-            maximum_radial_p95_px=image_maximum_radial_p95_px,
-            minimum_radius_px=image_minimum_radius_px,
-            minimum_arc_rad=minimum_arc_rad,
-        )
-    return fit_joint_center_curve(
-        records,
-        maximum_plane_rms_m=maximum_plane_rms_m,
-        maximum_radial_rms_m=maximum_radial_rms_m,
-        minimum_radius_m=minimum_radius_m,
-        minimum_arc_rad=minimum_arc_rad,
-    )
-
-
-def center_splay_curve(fit: JointCurveFit) -> tuple[JointCurveFit, int, float]:
-    """Centre a measured 255->0 splay travel on its angular midpoint."""
-    raw = np.asarray(fit.angle_rad, dtype=float)
-    if raw.shape != (256,) or not np.all(np.isfinite(raw)):
-        raise ValueError("splay curve must contain 256 finite values")
-    if np.any(np.diff(raw) > 1.0e-7):
-        raise ValueError("raw splay curve must be non-increasing")
-    travel_midpoint = 0.5 * float(raw[0] + raw[255])
-    if travel_midpoint <= 0.0:
-        raise ValueError("splay travel must be positive")
-    centred = raw - travel_midpoint
-    # Make the public endpoint symmetry exact after decimal rounding.
-    endpoint = round(0.5 * float(raw[0] - raw[255]), 8)
-    centred[0] = endpoint
-    centred[255] = -endpoint
-    centred = np.asarray([round(float(value), 8) for value in centred])
-    zero_command = int(np.argmin(np.abs(centred)))
-    return (
-        replace(
-            fit,
-            angle_rad=tuple(float(value) for value in centred),
-            zero_offset_rad=round(float(travel_midpoint), 8),
-        ),
-        zero_command,
-        round(float(travel_midpoint), 8),
-    )
-
-
-def measure_joint_vector(fit: JointCurveFit, vector_xyz_m: Sequence[float]) -> float:
-    """Measure one vector with a fitted model, including splay centring."""
-    return float(
-        _angle_for_circle(vector_xyz_m, fit.circle) - fit.zero_offset_rad
-    )
-
-
-def measure_joint_observation(
-    fit: JointCurveFit,
-    *,
-    vector_xyz_m: Sequence[float],
-    image_vector_xy_px: Sequence[float],
-) -> float:
-    """Measure an observation in the same space used to fit its curve."""
-    if fit.circle.get("space") == "image_2d":
-        return float(
-            _signed_image_circle_angle(image_vector_xy_px, fit.circle)
-            - fit.zero_offset_rad
-        )
-    return measure_joint_vector(fit, vector_xyz_m)
-
-
-def fit_projected_zero(
-    records: Sequence[Mapping[str, Any]],
-    *,
-    minimum_radius_px: float = 20.0,
-    minimum_arc_rad: float = math.radians(15.0),
-    maximum_radial_rms_px: float = 2.0,
-    maximum_radial_p95_px: float = 3.5,
-    maximum_round_difference_rad: float = math.radians(1.0),
-) -> float:
-    """Return the image/table-projected zero from three sweep rounds."""
-    samples = [dict(record) for record in records]
-    points = np.asarray(
-        [record.get("image_relative_xy_px") for record in samples], dtype=float
-    )
-    if points.ndim != 2 or points.shape[1] != 2 or len(points) < 12:
-        raise ValueError("projected zero requires 2-D trajectory samples")
-    if not np.all(np.isfinite(points)):
-        raise ValueError("projected zero samples must be finite")
-    centre, radius = _fit_circle(points)
-    radial_error = np.abs(np.linalg.norm(points - centre, axis=1) - radius)
-    radial_rms = float(np.sqrt(np.mean(np.square(radial_error))))
-    radial_p95 = float(np.percentile(radial_error, 95.0))
-    arc = _trajectory_arc_rad(points, centre)
-    if radius < minimum_radius_px:
-        raise ValueError("projected_zero_radius_too_small")
-    if arc < minimum_arc_rad:
-        raise ValueError("projected_zero_arc_too_small")
-    if radial_rms > maximum_radial_rms_px:
-        raise ValueError("projected_zero_radial_rms_too_large")
-    if radial_p95 > maximum_radial_p95_px:
-        raise ValueError("projected_zero_radial_p95_too_large")
-
-    cycles = sorted({int(record["cycle"]) for record in samples})
-    if len(cycles) < 3:
-        raise ValueError("projected zero requires three sweep rounds")
-    angles: list[float] = []
-    for cycle in cycles:
-        zero_points = np.asarray(
-            [
-                record["image_relative_xy_px"]
-                for record in samples
-                if int(record["cycle"]) == cycle
-                and int(record["command_u8"]) == 255
-            ],
-            dtype=float,
-        )
-        if zero_points.size == 0:
-            raise ValueError("projected zero is missing command 255")
-        point = np.median(zero_points, axis=0)
-        inward = centre - point
-        if float(np.linalg.norm(inward)) < 1.0e-9:
-            raise ValueError("projected zero point is degenerate")
-        angles.append(
-            wrap_angle_rad(math.atan2(-float(inward[1]), float(inward[0])))
-        )
-    if maximum_pairwise_angle_difference_rad(angles) > maximum_round_difference_rad:
-        raise ValueError("projected_zero_round_difference_too_large")
-    return round(float(circular_median_rad(angles)), 8)
-
-
-def build_compact_payload(
-    *,
-    serial_number: str,
-    measured_fits: Mapping[str, JointCurveFit],
-    urdf_zero_offsets_rad: Mapping[str, float],
-    validation_errors_rad: Sequence[float],
-    passed: bool,
-    baseline: Sequence[int] = THREE_CAMERA_BASELINE_COMMAND,
-    side: str = "left",
-) -> dict[str, Any]:
-    profile = get_hand_calibration_profile(side)
-    if set(measured_fits) != set(profile.measured_joints):
-        raise ValueError("measured_fits must contain all directly measured joints")
-    expected_active = {
-        name for name, spec in profile.joint_specs.items() if spec.active
-    }
-    if set(urdf_zero_offsets_rad) != expected_active:
-        raise ValueError("urdf_zero_offsets_rad has the wrong active-joint set")
-    if len(baseline) != 20:
-        raise ValueError("baseline must contain exactly 20 commands")
-
-    joints: dict[str, dict[str, Any]] = {}
-    for name, spec in profile.joint_specs.items():
-        source_name = spec.source_joint or name
-        fit = measured_fits[source_name]
-        joint: dict[str, Any] = {
-            "motor_index": int(spec.motor_index),
-            "angle_rad": [round(float(value), 8) for value in fit.angle_rad],
-        }
-        if spec.active:
-            joint["zero_command_u8"] = int(baseline[spec.motor_index])
-            joint["zero_angles"] = {
-                "urdf_zero_offset_rad": round(
-                    float(urdf_zero_offsets_rad[name]), 8
-                )
-            }
-        else:
-            joint["passive"] = True
-        if spec.source_joint is not None:
-            joint["source_joint"] = spec.source_joint
-        joints[name] = joint
-
-    errors = np.abs(np.asarray(validation_errors_rad, dtype=float))
-    mae = float(np.mean(errors)) if errors.size else float("nan")
-    p95 = float(np.percentile(errors, 95.0)) if errors.size else float("nan")
-    payload = {
-        "schema_version": 4,
-        "model": "G20",
-        "side": profile.side,
-        "serial_number": str(serial_number),
-        "angle_unit": "rad",
-        "command_range": [0, 255],
-        "baseline_command_u8": [int(value) for value in baseline],
-        "joints": joints,
-        "quality": {
-            "passed": bool(passed),
-            "validation_mae_rad": (
-                None if not math.isfinite(mae) else round(mae, 8)
-            ),
-            "validation_p95_rad": (
-                None if not math.isfinite(p95) else round(p95, 8)
-            ),
-        },
-    }
-    validate_compact_payload(payload)
-    return payload
-
-
-def validate_compact_payload(payload: Mapping[str, Any]) -> None:
-    expected_top = {
-        "schema_version", "model", "side", "serial_number", "angle_unit",
-        "command_range", "baseline_command_u8", "joints", "quality",
-    }
-    if set(payload) != expected_top:
-        raise ValueError("compact calibration has unexpected top-level fields")
-    if payload["schema_version"] != 4:
-        raise ValueError("schema_version must be 4")
-    if payload["model"] != "G20" or payload["side"] not in {"left", "right"}:
-        raise ValueError("payload must describe a left or right G20")
-    profile = get_hand_calibration_profile(str(payload["side"]))
-    if payload["angle_unit"] != "rad" or payload["command_range"] != [0, 255]:
-        raise ValueError("payload angle or command units are invalid")
-    baseline = payload["baseline_command_u8"]
-    if not isinstance(baseline, list) or len(baseline) != 20:
-        raise ValueError("baseline_command_u8 must contain 20 values")
-    joints = payload["joints"]
-    if not isinstance(joints, Mapping) or set(joints) != set(profile.joint_specs):
-        raise ValueError("payload must contain exactly 21 G20 joints")
-
-    for name, spec in profile.joint_specs.items():
-        joint = joints[name]
-        allowed = {"motor_index", "angle_rad"}
-        allowed.add("zero_command_u8" if spec.active else "passive")
-        if spec.active:
-            allowed.add("zero_angles")
-        if spec.source_joint is not None:
-            allowed.add("source_joint")
-        if set(joint) != allowed:
-            raise ValueError(f"{name} has unexpected fields")
-        if int(joint["motor_index"]) != spec.motor_index:
-            raise ValueError(f"{name} has the wrong motor_index")
-        curve = np.asarray(joint["angle_rad"], dtype=float)
-        if curve.shape != (256,) or not np.all(np.isfinite(curve)):
-            raise ValueError(f"{name}.angle_rad must contain 256 finite values")
-        if np.any(np.diff(curve) > 1.0e-7):
-            raise ValueError(f"{name}.angle_rad must be non-increasing")
-        if spec.active:
-            zero = joint["zero_command_u8"]
-            if not isinstance(zero, int) or not 0 <= zero <= 255:
-                raise ValueError(f"{name}.zero_command_u8 is invalid")
-            zero_angles = joint.get("zero_angles")
-            if not isinstance(zero_angles, Mapping) or set(zero_angles) != {
-                "urdf_zero_offset_rad"
-            }:
-                raise ValueError(
-                    f"{name}.zero_angles must contain urdf_zero_offset_rad"
-                )
-            offset = float(zero_angles["urdf_zero_offset_rad"])
-            if not math.isfinite(offset):
-                raise ValueError(f"{name}.urdf_zero_offset_rad is invalid")
-            if abs(float(curve[zero])) > 1.0e-6:
-                raise ValueError(
-                    f"{name}.angle_rad must be zero at zero_command_u8"
-                )
-        elif joint.get("passive") is not True:
-            raise ValueError(f"{name} must be marked passive")
-        if spec.source_joint is not None:
-            if joint.get("source_joint") != spec.source_joint:
-                raise ValueError(f"{name} has the wrong source_joint")
-            source = joints[spec.source_joint]
-            if joint["angle_rad"] != source["angle_rad"]:
-                raise ValueError(f"{name} must copy its source curve exactly")
-            if spec.active and joint["zero_command_u8"] != source["zero_command_u8"]:
-                raise ValueError(f"{name} must copy its source zero command")
-            # source_joint denotes a dynamic command-angle curve source.  The
-            # absolute URDF zero belongs to an independent motor/assembly and
-            # must remain per-joint unless it was independently observed.
-
-    quality = payload["quality"]
-    if set(quality) != {
-        "passed", "validation_mae_rad", "validation_p95_rad"
-    }:
-        raise ValueError("quality must contain only compact summary fields")

src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/offline_replay.py

@@ -1,846 +0,0 @@
-"""Safely replay a complete three-camera session without moving the hand."""
-
-from __future__ import annotations
-
-import argparse
-from collections import defaultdict
-from dataclasses import replace
-import hashlib
-import json
-import math
-import os
-from pathlib import Path
-import re
-import tempfile
-from typing import Any, Mapping, Sequence
-import xml.etree.ElementTree as ET
-
-import numpy as np
-from scipy.spatial.transform import Rotation
-import yaml
-
-from .extrinsics import load_three_camera_extrinsics
-from .full_hand import (
-    HandCalibrationProfile,
-    JointCurveFit,
-    build_calibration_motion_command,
-    build_compact_payload,
-    get_hand_calibration_profile,
-    validate_compact_payload,
-)
-from .storage import atomic_write_json
-from .urdf_zero import (
-    JointAxisMeasurement,
-    UrdfKinematicModel,
-    _angles_from_state,
-    fit_joint_axis_measurement,
-    fit_rotation_joint_curve,
-    get_zero_calibration_profile,
-    rotation_curve_holdout_errors,
-    solve_urdf_zero_offsets,
-    write_zero_corrected_urdf,
-)
-
-
-def _sha256(path: Path) -> str:
-    digest = hashlib.sha256()
-    with path.open("rb") as stream:
-        for chunk in iter(lambda: stream.read(1024 * 1024), b""):
-            digest.update(chunk)
-    return digest.hexdigest()
-
-
-def _output_suffix(output_tag: str | None) -> str:
-    """Return a filename-safe suffix for a non-destructive replay variant."""
-    if output_tag is None:
-        return ""
-    tag = str(output_tag)
-    if not re.fullmatch(r"[A-Za-z0-9][A-Za-z0-9_.-]{0,63}", tag):
-        raise ValueError(
-            "output tag must be 1-64 filename-safe characters, beginning "
-            "with a letter or digit"
-        )
-    return f"_{tag}"
-
-
-def _load_parameters(path: Path) -> dict[str, Any]:
-    payload = yaml.safe_load(path.read_text(encoding="utf-8"))
-    return dict(payload["g20_calibration"]["ros__parameters"])
-
-
-def _latest_attempt_records(
-    rows: Sequence[Mapping[str, Any]],
-) -> dict[str, list[dict[str, Any]]]:
-    """Reproduce the online retry buffer from append-only raw samples.
-
-    Online retry clears only the failed joint/cycle/direction from memory,
-    while JSONL deliberately retains every attempt for audit.  Offline replay
-    must therefore select the greatest attempt independently for each logical
-    trajectory rather than mixing rejected attempts into the final fit.
-    """
-    samples = [dict(row) for row in rows if row.get("kind") == "sample"]
-    latest_attempt: dict[tuple[str, int, str], int] = {}
-    for row in samples:
-        key = (
-            str(row["joint"]),
-            int(row["cycle"]),
-            str(row["direction"]),
-        )
-        latest_attempt[key] = max(
-            latest_attempt.get(key, 0), int(row.get("attempt", 1))
-        )
-    records: dict[str, list[dict[str, Any]]] = defaultdict(list)
-    for row in samples:
-        key = (
-            str(row["joint"]),
-            int(row["cycle"]),
-            str(row["direction"]),
-        )
-        if int(row.get("attempt", 1)) == latest_attempt[key]:
-            records[key[0]].append(row)
-    return dict(records)
-
-
-def _load_raw_session(
-    session_dir: Path,
-) -> tuple[dict[str, Any], dict[str, list[dict[str, Any]]], Path]:
-    raw_path = session_dir / "raw_samples.jsonl"
-    if not raw_path.is_file():
-        raise ValueError(f"raw session does not exist: {raw_path}")
-    rows = [
-        json.loads(line)
-        for line in raw_path.read_text(encoding="utf-8").splitlines()
-        if line.strip()
-    ]
-    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")
-    return starts[0], _latest_attempt_records(rows), raw_path
-
-
-def _fit_curve(
-    name: str,
-    records: Sequence[Mapping[str, Any]],
-    *,
-    profile: HandCalibrationProfile,
-    baseline: Sequence[int],
-) -> JointCurveFit:
-    motor = profile.joint_specs[name].motor_index
-    return fit_rotation_joint_curve(
-        records,
-        zero_command_u8=int(baseline[motor]),
-    )
-
-
-def _fit_axes(
-    *,
-    records_by_joint: Mapping[str, Sequence[Mapping[str, Any]]],
-    profile: HandCalibrationProfile,
-    baseline: Sequence[int],
-    extrinsics_file: Path,
-    repetitions: int,
-) -> list[JointAxisMeasurement]:
-    zero_profile = get_zero_calibration_profile(profile.side)
-    extrinsics = load_three_camera_extrinsics(extrinsics_file)
-    cache: dict[tuple[str, int], JointAxisMeasurement] = {}
-    reference = profile.reference_finger
-    upstream_by_joint = {
-        "thumb_mcp": "thumb_cmc_pitch",
-        "thumb_ip": "thumb_mcp",
-        f"{reference}_pip": f"{reference}_mcp_pitch",
-        f"{reference}_dip": f"{reference}_pip",
-    }
-
-    def fit_one(name: str, cycle: int) -> JointAxisMeasurement:
-        key = (name, cycle)
-        if key in cache:
-            return cache[key]
-        upstream = upstream_by_joint.get(name)
-        constraint = None if upstream is None else fit_one(upstream, cycle).axis_common_xyz
-        spec = profile.joint_specs[name]
-        view_normal = extrinsics.transform(spec.view)[:3, :3] @ np.asarray(
-            [0.0, 0.0, 1.0], dtype=float
-        )
-        result = fit_joint_axis_measurement(
-            name,
-            records_by_joint[name],
-            cycle=cycle,
-            zero_command_u8=int(baseline[spec.motor_index]),
-            axis_common_constraint=constraint,
-            constrained_circle_joints=zero_profile.constrained_circle_joints,
-            view_normal_common_xyz=view_normal,
-        )
-        condition = build_calibration_motion_command(
-            spec,
-            int(baseline[spec.motor_index]),
-            baseline=baseline,
-            profile=profile,
-        )
-        result = replace(
-            result,
-            condition_command_u8=tuple(float(value) for value in condition),
-            view_normal_common_xyz=tuple(float(value) for value in view_normal),
-        )
-        cache[key] = result
-        return result
-
-    return [
-        fit_one(name, cycle)
-        for name in zero_profile.axis_joints
-        for cycle in range(repetitions)
-    ]
-
-
-def _maximum_undirected_axis_difference(axes: Sequence[Sequence[float]]) -> float:
-    maximum = 0.0
-    for left in axes:
-        for right in axes:
-            maximum = max(
-                maximum,
-                math.acos(
-                    abs(
-                        float(
-                            np.clip(
-                                np.asarray(left, dtype=float)
-                                @ np.asarray(right, dtype=float),
-                                -1.0,
-                                1.0,
-                            )
-                        )
-                    )
-                ),
-            )
-    return maximum
-
-
-def _quality_failures(
-    *,
-    records_by_joint: Mapping[str, Sequence[Mapping[str, Any]]],
-    profile: HandCalibrationProfile,
-    baseline: Sequence[int],
-    fits: Mapping[str, JointCurveFit],
-    axes: Sequence[JointAxisMeasurement],
-    parameters: Mapping[str, Any],
-) -> list[str]:
-    failures: list[str] = []
-    repetitions = int(parameters["repetitions"])
-    zero_profile = get_zero_calibration_profile(profile.side)
-    axis_by_key = {(item.joint, item.cycle): item for item in axes}
-    expected_directions = {"decreasing", "increasing"}
-    for name in profile.measured_joints:
-        records = list(records_by_joint.get(name, ()))
-        if not records:
-            failures.append(f"{name}: no samples")
-            continue
-        attempts_by_direction: dict[tuple[int, str], set[int]] = defaultdict(set)
-        for record in records:
-            attempts_by_direction[
-                (int(record["cycle"]), str(record["direction"]))
-            ].add(int(record.get("attempt", 1)))
-        for cycle in range(repetitions):
-            for direction in expected_directions:
-                selected = [
-                    record
-                    for record in records
-                    if int(record["cycle"]) == cycle
-                    and str(record["direction"]) == direction
-                ]
-                attempts = attempts_by_direction.get((cycle, direction), set())
-                if len(attempts) != 1:
-                    failures.append(
-                        f"{name} cycle {cycle + 1} {direction}: ambiguous attempts {sorted(attempts)}"
-                    )
-                    continue
-                commands = sorted({int(record["command_u8"]) for record in selected})
-                if len(selected) < int(parameters["minimum_sweep_frames"]):
-                    failures.append(f"{name} cycle {cycle + 1} {direction}: too few frames")
-                if not commands or max(commands) - min(commands) < float(
-                    parameters["minimum_state_span_u8"]
-                ):
-                    failures.append(f"{name} cycle {cycle + 1} {direction}: insufficient span")
-                if len(commands) < int(parameters["minimum_sweep_bins"]):
-                    failures.append(f"{name} cycle {cycle + 1} {direction}: insufficient bins")
-                if 0 not in commands or 255 not in commands:
-                    failures.append(f"{name} cycle {cycle + 1} {direction}: endpoint missing")
-                if commands and max(np.diff(commands), default=0) > int(
-                    parameters["maximum_bin_gap"]
-                ):
-                    failures.append(f"{name} cycle {cycle + 1} {direction}: bin gap")
-
-        sync_p95 = float(
-            np.percentile(
-                [float(record.get("state_image_sync_error_ms", 0.0)) for record in records],
-                95.0,
-            )
-        )
-        if sync_p95 > float(parameters["maximum_state_image_skew_ms"]):
-            failures.append(f"{name}: state/image sync p95 {sync_p95:.3f}ms")
-        spec = profile.joint_specs[name]
-        fit = fits[name]
-        orthogonal_limit = math.radians(
-            float(
-                parameters[
-                    "active_maximum_rotation_orthogonal_rms_deg"
-                    if spec.active
-                    else "passive_maximum_rotation_orthogonal_rms_deg"
-                ]
-            )
-        )
-        if float(fit.quality["rotation_orthogonal_rms_rad"]) > orthogonal_limit:
-            failures.append(f"{name}: rotation orthogonal RMS")
-        if float(fit.quality["arc_rad"]) < math.radians(
-            float(parameters["trajectory_minimum_arc_deg"])
-        ):
-            failures.append(f"{name}: trajectory arc")
-        monotonic_limit = math.radians(
-            float(
-                parameters[
-                    "maximum_monotonic_correction_deg"
-                    if spec.active
-                    else "passive_maximum_monotonic_correction_deg"
-                ]
-            )
-        )
-        hysteresis_limit = math.radians(
-            float(
-                parameters[
-                    "maximum_hysteresis_deg"
-                    if spec.active
-                    else "passive_maximum_hysteresis_deg"
-                ]
-            )
-        )
-        if fit.maximum_monotonic_correction_rad > monotonic_limit:
-            failures.append(f"{name}: monotonic correction")
-        if fit.maximum_hysteresis_rad > hysteresis_limit:
-            failures.append(f"{name}: hysteresis")
-
-        cycle_travels: list[float] = []
-        cycle_axes: list[Sequence[float]] = []
-        cycle_axis_sources: list[str] = []
-        for cycle in range(repetitions):
-            cycle_fit = _fit_curve(
-                name,
-                [record for record in records if int(record["cycle"]) == cycle],
-                profile=profile,
-                baseline=baseline,
-            )
-            cycle_travels.append(
-                abs(float(cycle_fit.angle_rad[0]) - float(cycle_fit.angle_rad[255]))
-            )
-            axis = axis_by_key[(name, cycle)]
-            cycle_axes.append(axis.axis_common_xyz)
-            cycle_axis_sources.append(axis.axis_direction_source)
-            if axis.radial_rms_m > float(parameters["axis_maximum_radial_rms_m"]):
-                failures.append(f"{name} cycle {cycle + 1}: radial RMS")
-            if axis.pose_axis_line_rms_m > float(
-                parameters["axis_maximum_pose_line_rms_m"]
-            ):
-                failures.append(
-                    f"{name} cycle {cycle + 1}: pose axis-line RMS"
-                )
-            if name not in zero_profile.constrained_circle_joints:
-                plane_limit = float(
-                    parameters[
-                        "axis_maximum_plane_rms_m"
-                        if spec.active
-                        else "passive_axis_maximum_plane_rms_m"
-                    ]
-                )
-                if axis.plane_rms_m > plane_limit:
-                    failures.append(f"{name} cycle {cycle + 1}: plane RMS")
-                if (
-                    name not in zero_profile.constrained_circle_joints
-                    and axis.rotation_circle_axis_difference_rad > math.radians(
-                        float(parameters["axis_maximum_rotation_circle_difference_deg"])
-                    )
-                ):
-                    failures.append(f"{name} cycle {cycle + 1}: axis disagreement")
-        travel_limit = math.radians(
-            float(
-                parameters[
-                    "trajectory_maximum_cycle_travel_difference_deg"
-                    if spec.active
-                    else "passive_maximum_cycle_travel_difference_deg"
-                ]
-            )
-        )
-        if max(cycle_travels) - min(cycle_travels) > travel_limit:
-            failures.append(f"{name}: cycle travel difference")
-        if (
-            not all(
-                source == "upstream_constraint"
-                for source in cycle_axis_sources
-            )
-            and _maximum_undirected_axis_difference(cycle_axes) > math.radians(
-                float(parameters["zero_maximum_axis_cycle_difference_deg"])
-            )
-        ):
-            failures.append(f"{name}: cycle axis difference")
-    return failures
-
-
-def _joint_xml(path: Path) -> dict[str, ET.Element]:
-    return {
-        str(joint.get("name")): joint
-        for joint in ET.parse(path).getroot().findall("joint")
-    }
-
-
-def _triplet(value: str) -> np.ndarray:
-    return np.asarray([float(item) for item in value.split()], dtype=float)
-
-
-def _validate_corrected_urdf(
-    *,
-    source: Path,
-    corrected: Path,
-    offsets: Mapping[str, float],
-    axes: Sequence[JointAxisMeasurement],
-    curves: Mapping[str, JointCurveFit],
-    motor_by_joint: Mapping[str, int],
-    inherited_zero_joints: Mapping[str, str],
-) -> dict[str, float]:
-    source_joints = _joint_xml(source)
-    corrected_joints = _joint_xml(corrected)
-    if set(source_joints) != set(corrected_joints):
-        raise ValueError("corrected URDF changed the joint set")
-    maximum_origin_rotation_error = 0.0
-    maximum_origin_translation_error = 0.0
-    for name, original_joint in source_joints.items():
-        corrected_joint = corrected_joints[name]
-        original_origin = original_joint.find("origin")
-        corrected_origin = corrected_joint.find("origin")
-        if original_origin is None or corrected_origin is None:
-            continue
-        original_xyz = _triplet(original_origin.get("xyz", "0 0 0"))
-        corrected_xyz = _triplet(corrected_origin.get("xyz", "0 0 0"))
-        maximum_origin_translation_error = max(
-            maximum_origin_translation_error,
-            float(np.linalg.norm(corrected_xyz - original_xyz)),
-        )
-        original_rotation = Rotation.from_euler(
-            "xyz", _triplet(original_origin.get("rpy", "0 0 0"))
-        )
-        corrected_rotation = Rotation.from_euler(
-            "xyz", _triplet(corrected_origin.get("rpy", "0 0 0"))
-        )
-        expected = original_rotation
-        if name in offsets:
-            axis_node = original_joint.find("axis")
-            axis = _triplet(
-                "1 0 0" if axis_node is None else axis_node.get("xyz", "1 0 0")
-            )
-            axis /= np.linalg.norm(axis)
-            expected = original_rotation * Rotation.from_rotvec(
-                axis * float(offsets[name])
-            )
-        error = float((expected.inv() * corrected_rotation).magnitude())
-        maximum_origin_rotation_error = max(maximum_origin_rotation_error, error)
-        original_limit = original_joint.find("limit")
-        corrected_limit = corrected_joint.find("limit")
-        if original_limit is not None and corrected_limit is not None:
-            if (
-                original_limit.get("lower") != corrected_limit.get("lower")
-                or original_limit.get("upper") != corrected_limit.get("upper")
-            ):
-                raise ValueError(f"corrected URDF unexpectedly changed {name} limits")
-    if maximum_origin_translation_error > 1.0e-12:
-        raise ValueError("corrected URDF changed a joint origin translation")
-    if maximum_origin_rotation_error > 1.0e-10:
-        raise ValueError("corrected URDF does not implement T_original * Rot(axis, offset)")
-    original_model = UrdfKinematicModel(source)
-    corrected_model = UrdfKinematicModel(corrected)
-    maximum_axis_error = 0.0
-    maximum_point_error = 0.0
-    for measurement in axes:
-        state = (
-            measurement.condition_state_u8
-            if measurement.condition_command_u8 is None
-            else measurement.condition_command_u8
-        )
-        angles = _angles_from_state(
-            state,
-            curves=curves,
-            motor_by_joint=motor_by_joint,
-            inherited_zero_joints=inherited_zero_joints,
-        )
-        expected_axis, expected_point = original_model.axis_line(
-            measurement.joint,
-            zero_offsets=offsets,
-            joint_angles=angles,
-        )
-        actual_axis, actual_point = corrected_model.axis_line(
-            measurement.joint,
-            zero_offsets={},
-            joint_angles=angles,
-        )
-        maximum_axis_error = max(
-            maximum_axis_error,
-            math.acos(float(np.clip(expected_axis @ actual_axis, -1.0, 1.0))),
-        )
-        maximum_point_error = max(
-            maximum_point_error,
-            float(np.linalg.norm(expected_point - actual_point)),
-        )
-    if maximum_axis_error > 1.0e-7 or maximum_point_error > 1.0e-10:
-        raise ValueError("written URDF kinematics differ from the solved correction")
-    return {
-        "maximum_origin_rotation_error_rad": maximum_origin_rotation_error,
-        "maximum_origin_translation_error_m": maximum_origin_translation_error,
-        "maximum_axis_equivalence_error_rad": maximum_axis_error,
-        "maximum_axis_point_equivalence_error_m": maximum_point_error,
-    }
-
-
-def replay_session(
-    session_dir: str | Path,
-    *,
-    serial_number: str | None = None,
-    config_file: str | Path | None = None,
-    write_outputs: bool = False,
-    output_tag: str | None = None,
-) -> dict[str, Any]:
-    session = Path(session_dir).expanduser().resolve()
-    package_root = Path(__file__).resolve().parents[1]
-    config = (
-        package_root / "config" / "three_camera_calibration.yaml"
-        if config_file is None
-        else Path(config_file).expanduser().resolve()
-    )
-    parameters = _load_parameters(config)
-    start, records_by_joint, raw_path = _load_raw_session(session)
-    side = str(start["hand_type"]).lower()
-    profile = get_hand_calibration_profile(side)
-    zero_profile = get_zero_calibration_profile(side)
-    baseline = tuple(int(value) for value in start["baseline_command_u8"])
-    if len(baseline) != 20:
-        raise ValueError("session baseline must contain exactly 20 commands")
-    if set(records_by_joint) != set(profile.measured_joints):
-        raise ValueError("raw session does not contain exactly the measured joint set")
-    source_urdf = Path(start["source_urdf_path"]).expanduser().resolve()
-    extrinsics_file = Path(start["camera_extrinsics_file"]).expanduser().resolve()
-    if not source_urdf.is_file() or not extrinsics_file.is_file():
-        raise ValueError("session source URDF or camera extrinsics is missing")
-    if "zero_calibrated" in source_urdf.stem.lower():
-        raise ValueError("offline replay requires the original CAD URDF")
-    hand_serial = str(serial_number or session.parent.name)
-    output_suffix = _output_suffix(output_tag)
-    repetitions = int(parameters["repetitions"])
-
-    measured_fits = {
-        name: _fit_curve(
-            name,
-            records_by_joint[name],
-            profile=profile,
-            baseline=baseline,
-        )
-        for name in profile.measured_joints
-    }
-    training_fits = {
-        name: _fit_curve(
-            name,
-            [
-                record
-                for record in records_by_joint[name]
-                if int(record["cycle"]) in {0, 1}
-            ],
-            profile=profile,
-            baseline=baseline,
-        )
-        for name in profile.measured_joints
-    }
-    axes = _fit_axes(
-        records_by_joint=records_by_joint,
-        profile=profile,
-        baseline=baseline,
-        extrinsics_file=extrinsics_file,
-        repetitions=repetitions,
-    )
-    failures = _quality_failures(
-        records_by_joint=records_by_joint,
-        profile=profile,
-        baseline=baseline,
-        fits=measured_fits,
-        axes=axes,
-        parameters=parameters,
-    )
-    if failures:
-        raise ValueError("offline trajectory/axis validation failed: " + "; ".join(failures))
-
-    holdout_by_joint = {
-        name: rotation_curve_holdout_errors(
-            training_fits[name],
-            [
-                record
-                for record in records_by_joint[name]
-                if int(record["cycle"]) == 2
-            ],
-            zero_command_u8=int(
-                baseline[profile.joint_specs[name].motor_index]
-            ),
-        )
-        for name in profile.measured_joints
-    }
-    trajectory_errors = np.abs(
-        np.asarray(
-            [value for values in holdout_by_joint.values() for value in values],
-            dtype=float,
-        )
-    )
-    maximum_validation_mae = math.radians(
-        float(parameters["maximum_validation_mae_deg"])
-    )
-    maximum_validation_p95 = math.radians(
-        float(parameters["maximum_validation_p95_deg"])
-    )
-    trajectory_mae = float(np.mean(trajectory_errors))
-    trajectory_p95 = float(np.percentile(trajectory_errors, 95.0))
-    if (
-        trajectory_mae > maximum_validation_mae
-        or trajectory_p95 > maximum_validation_p95
-    ):
-        raise ValueError("third-cycle trajectory holdout failed")
-
-    motor_by_joint = {
-        name: int(spec.motor_index) for name, spec in profile.joint_specs.items()
-    }
-    joint_limits: dict[str, float] = {}
-    solve_arguments = {
-        "source_urdf": source_urdf,
-        "measurements": axes,
-        "motor_by_joint": motor_by_joint,
-        "maximum_offset_rad": math.radians(float(parameters["zero_maximum_offset_deg"])),
-        "finger_maximum_offset_rad": math.radians(
-            float(parameters.get("zero_finger_maximum_offset_deg", 3.0))
-        ),
-        "joint_maximum_offset_rad": joint_limits,
-        "maximum_cycle_difference_rad": math.radians(
-            float(parameters["zero_maximum_axis_cycle_difference_deg"])
-        ),
-        "maximum_axis_cone_mismatch_rad": math.radians(
-            float(parameters["zero_maximum_axis_cone_mismatch_deg"])
-        ),
-        "maximum_pose_axis_line_rms_m": float(
-            parameters["axis_maximum_pose_line_rms_m"]
-        ),
-        "maximum_validation_mae_rad": maximum_validation_mae,
-        "maximum_validation_p95_rad": maximum_validation_p95,
-        "hand_type": side,
-    }
-    holdout_zero = solve_urdf_zero_offsets(curves=training_fits, **solve_arguments)
-    if not holdout_zero.passed:
-        failure = {
-            "reasons": dict(holdout_zero.failure_reasons),
-            "fitted_offsets_deg": {
-                name: math.degrees(value)
-                for name, value in holdout_zero.direct_offsets_rad.items()
-            },
-            "cycle_offsets_deg": {
-                name: [math.degrees(value) for value in values]
-                for name, values in holdout_zero.cycle_offsets_rad.items()
-            },
-        }
-        raise ValueError(
-            "third-cycle zero/URDF holdout failed: "
-            + json.dumps(failure, ensure_ascii=False, sort_keys=True)
-        )
-    final_zero = solve_urdf_zero_offsets(curves=measured_fits, **solve_arguments)
-    if not final_zero.passed:
-        raise ValueError(
-            "all-cycle zero refit failed: "
-            + json.dumps(
-                {
-                    "reasons": dict(final_zero.failure_reasons),
-                    "fitted_offsets_deg": {
-                        name: math.degrees(value)
-                        for name, value in final_zero.direct_offsets_rad.items()
-                    },
-                },
-                ensure_ascii=False,
-                sort_keys=True,
-            )
-        )
-    for target, source_name in zero_profile.inherited_static_zero_joints.items():
-        if final_zero.all_active_offsets_rad[target] != final_zero.direct_offsets_rad[source_name]:
-            raise ValueError(f"inherited static zero mismatch: {target} <- {source_name}")
-    for target in (
-        set(zero_profile.inherited_zero_joints)
-        - set(zero_profile.inherited_static_zero_joints)
-    ):
-        if final_zero.all_active_offsets_rad[target] != 0.0:
-            raise ValueError(f"unobserved static zero must retain source CAD: {target}")
-
-    validation_errors = [
-        float(value) for values in holdout_by_joint.values() for value in values
-    ]
-    validation_errors.extend(float(value) for value in holdout_zero.validation_errors_rad)
-    payload = build_compact_payload(
-        serial_number=hand_serial,
-        measured_fits=measured_fits,
-        urdf_zero_offsets_rad=final_zero.all_active_offsets_rad,
-        validation_errors_rad=validation_errors,
-        passed=True,
-        baseline=baseline,
-        side=side,
-    )
-    validate_compact_payload(payload)
-
-    stamp = session.name
-    final_json = session / (
-        f"g20_{side}_{hand_serial}_calibration{output_suffix}.json"
-    )
-    expected_urdf_name = (
-        f"{source_urdf.stem}_zero_calibrated_{hand_serial}_{stamp}"
-        f"{output_suffix}.urdf"
-    )
-    final_urdf = source_urdf.parent / expected_urdf_name
-    report_path = session / (
-        f"g20_{side}_{hand_serial}_offline_validation{output_suffix}.json"
-    )
-    if write_outputs:
-        existing = [path for path in (final_json, final_urdf, report_path) if path.exists()]
-        if existing:
-            raise ValueError(
-                "refusing to overwrite replay outputs: "
-                + ", ".join(str(path) for path in existing)
-            )
-
-    source_hash_before = _sha256(source_urdf)
-    with tempfile.TemporaryDirectory(prefix="offline_replay_", dir=session) as temporary:
-        candidate = write_zero_corrected_urdf(
-            source_urdf=source_urdf,
-            output_directory=temporary,
-            serial_number=hand_serial,
-            offsets_rad=final_zero.all_active_offsets_rad,
-            timestamp=stamp,
-        )
-        urdf_checks = _validate_corrected_urdf(
-            source=source_urdf,
-            corrected=candidate,
-            offsets=final_zero.all_active_offsets_rad,
-            axes=axes,
-            curves=measured_fits,
-            motor_by_joint=motor_by_joint,
-            inherited_zero_joints=zero_profile.inherited_zero_joints,
-        )
-        residual_zero = solve_urdf_zero_offsets(
-            curves=measured_fits,
-            fixed_direct_zero_offsets_rad={
-                name: 0.0
-                for name in zero_profile.fixed_direct_zero_offsets_rad
-            },
-            static_output_zero_offsets_rad={
-                name: 0.0
-                for name in zero_profile.static_output_zero_offsets_rad
-            },
-            **{**solve_arguments, "source_urdf": candidate},
-        )
-        maximum_residual_offset = max(
-            abs(float(value)) for value in residual_zero.direct_offsets_rad.values()
-        )
-        if not residual_zero.passed or maximum_residual_offset > math.radians(0.3):
-            raise ValueError("written URDF retains a significant zero correction")
-        candidate_hash = _sha256(candidate)
-        if write_outputs:
-            os.replace(candidate, final_urdf)
-
-    if _sha256(source_urdf) != source_hash_before:
-        raise ValueError("source URDF changed during offline replay")
-
-    report: dict[str, Any] = {
-        "passed": True,
-        "session_dir": str(session),
-        "side": side,
-        "serial_number": hand_serial,
-        "output_tag": output_tag,
-        "raw_samples_sha256": _sha256(raw_path),
-        "source_urdf": str(source_urdf),
-        "source_urdf_sha256": source_hash_before,
-        "joint_limits_deg": {
-            "finger_default": float(
-                parameters.get("zero_finger_maximum_offset_deg", 3.0)
-            ),
-            "thumb_default": float(parameters["zero_maximum_offset_deg"]),
-        },
-        "static_zero_policy": "direct_measurements_only",
-        "fixed_zero_offsets_deg": {
-            name: math.degrees(value)
-            for name, value in zero_profile.fixed_direct_zero_offsets_rad.items()
-        },
-        "static_output_zero_offsets_deg": {
-            name: math.degrees(value)
-            for name, value in zero_profile.static_output_zero_offsets_rad.items()
-        },
-        "direct_offsets_deg": {
-            name: math.degrees(value)
-            for name, value in final_zero.direct_offsets_rad.items()
-        },
-        "cycle_offsets_deg": {
-            name: [math.degrees(value) for value in values]
-            for name, values in holdout_zero.cycle_offsets_rad.items()
-        },
-        "offset_uncertainty_deg": {
-            name: math.degrees(value)
-            for name, value in holdout_zero.offset_uncertainty_rad.items()
-        },
-        "trajectory_holdout_mae_deg": math.degrees(trajectory_mae),
-        "trajectory_holdout_p95_deg": math.degrees(trajectory_p95),
-        "zero_holdout_error_deg": {
-            name: math.degrees(value)
-            for name, value in holdout_zero.validation_error_by_joint_rad.items()
-        },
-        "zero_original_error_deg": {
-            name: math.degrees(value)
-            for name, value in holdout_zero.validation_original_error_by_joint_rad.items()
-        },
-        "zero_improvement_95pct_lower_deg": {
-            name: math.degrees(value)
-            for name, value in (
-                holdout_zero.validation_improvement_confidence_lower_rad.items()
-            )
-        },
-        "corrected_urdf_checks": {
-            **urdf_checks,
-            "maximum_residual_zero_offset_deg": math.degrees(maximum_residual_offset),
-        },
-        "corrected_urdf_sha256": candidate_hash,
-        "final_json": str(final_json) if write_outputs else None,
-        "corrected_urdf": str(final_urdf) if write_outputs else None,
-    }
-    if write_outputs:
-        atomic_write_json(final_json, payload)
-        report["final_json_sha256"] = _sha256(final_json)
-        if _sha256(final_urdf) != candidate_hash:
-            raise ValueError("formal corrected URDF differs from validated candidate")
-        atomic_write_json(report_path, report)
-        report["validation_report"] = str(report_path)
-    return report
-
-
-def main() -> None:
-    parser = argparse.ArgumentParser(
-        description="Replay and independently validate a complete G20 calibration session."
-    )
-    parser.add_argument("session_dir")
-    parser.add_argument("--serial-number", default=None)
-    parser.add_argument("--config-file", default=None)
-    parser.add_argument("--write", action="store_true")
-    parser.add_argument(
-        "--output-tag",
-        default=None,
-        help="safe suffix for a replay variant; existing outputs are never overwritten",
-    )
-    arguments = parser.parse_args()
-    result = replay_session(
-        arguments.session_dir,
-        serial_number=arguments.serial_number,
-        config_file=arguments.config_file,
-        write_outputs=arguments.write,
-        output_tag=arguments.output_tag,
-    )
-    print(json.dumps(result, ensure_ascii=False, indent=2, sort_keys=True))
-
-
-if __name__ == "__main__":
-    main()

src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/three_camera_diagnostics.py

@@ -1,481 +0,0 @@
-"""Chinese, operator-facing diagnostics for three-camera calibration."""
-
-from __future__ import annotations
-
-import re
-from typing import Any, Mapping
-
-
-STATE_NAMES_ZH = {
-    "PREFLIGHT": "设备和标签预检",
-    "WAIT_START": "等待开始标定",
-    "RETURN_BASELINE": "正在恢复目标姿态",
-    "PREPARE_SWEEP": "正在到达扫描起点",
-    "SWEEP": "正在采集轨迹",
-    "FITTING": "正在拟合轨迹和零位",
-    "VALIDATION_MOVE": "正在移动到随机复测位置",
-    "VALIDATION_CAPTURE": "正在采集随机复测数据",
-    "PAUSED": "标定已暂停",
-    "ABORTED": "标定已终止",
-    "COMPLETE": "标定已完成",
-}
-
-VIEW_NAMES_ZH = {
-    "front": "正面",
-    "side": "侧面",
-    "top": "上面",
-}
-
-JOINT_NAMES_ZH = {
-    "thumb_cmc_pitch": "拇指CMC俯仰",
-    "thumb_cmc_roll": "拇指CMC滚转",
-    "thumb_mcp": "拇指MCP",
-    "thumb_ip": "拇指IP（被动）",
-    "index_mcp_roll": "食指MCP侧摆",
-    "index_mcp_pitch": "食指MCP屈伸",
-    "index_pip": "食指PIP",
-    "index_dip": "食指DIP（被动）",
-    "middle_mcp_roll": "中指MCP侧摆",
-    "middle_mcp_pitch": "中指MCP屈伸",
-    "middle_pip": "中指PIP",
-    "middle_dip": "中指DIP（被动）",
-    "ring_mcp_roll": "无名指MCP侧摆",
-    "ring_mcp_pitch": "无名指MCP屈伸",
-    "ring_pip": "无名指PIP",
-    "ring_dip": "无名指DIP（被动）",
-    "pinky_mcp_roll": "小指MCP侧摆",
-    "pinky_mcp_pitch": "小指MCP屈伸",
-    "pinky_pip": "小指PIP",
-    "pinky_dip": "小指DIP（被动）",
-    "thumb_cmc_yaw": "拇指CMC侧摆",
-}
-
-
-def _format_u8(value: Any) -> str:
-    if value is None:
-        return "尚无反馈"
-    return f"{float(value):.1f}"
-
-
-def _task_text(active: Mapping[str, Any]) -> str:
-    if not active:
-        return "尚无活动任务"
-    view = VIEW_NAMES_ZH.get(str(active.get("view", "")), str(active.get("view", "")))
-    if active.get("kind") == "fit_failure":
-        joints = active.get("joints", [])
-        joint_text = "/".join(
-            JOINT_NAMES_ZH.get(str(joint), str(joint)) for joint in joints
-        )
-        return (
-            f"{view}机位，{joint_text}拟合检查失败，"
-            f"电机{active.get('motor_index')}，"
-            f"第{active.get('attempt', 1)}次尝试"
-        )
-    if active.get("kind") == "zero_model_failure":
-        joints = active.get("joints", [])
-        joint_text = "/".join(
-            JOINT_NAMES_ZH.get(str(joint), str(joint)) for joint in joints
-        )
-        return (
-            f"{view}机位，{joint_text}零位/URDF验证失败，"
-            f"电机{active.get('motor_index')}，不会自动重扫"
-        )
-    if active.get("kind") == "motion_stall":
-        return (
-            f"电机{active.get('motor_index', '?')}运动停滞，目标"
-            f"{_format_u8(active.get('target_u8'))}、实际"
-            f"{_format_u8(active.get('actual_u8'))}"
-        )
-    if active.get("kind") == "validation":
-        return (
-            f"{view}机位，随机复测，电机{active.get('motor_index')}，"
-            f"目标命令{active.get('command_u8')}"
-        )
-    joints = active.get("joints", [])
-    joint_text = "/".join(
-        JOINT_NAMES_ZH.get(str(joint), str(joint)) for joint in joints
-    )
-    start = active.get("start_u8")
-    target = active.get("target_u8")
-    cycle = active.get("cycle", "?")
-    repetitions = active.get("repetitions", "?")
-    task = (
-        f"{view}机位，{joint_text}，电机{active.get('motor_index')}，"
-        f"第{cycle}/{repetitions}轮，{start}→{target}"
-    )
-    fit_attempt = int(active.get("fit_attempt", 1))
-    if fit_attempt > 1:
-        task += (
-            f"（整关节自动重采第{fit_attempt}/"
-            f"{active.get('fit_attempt_limit', '?')}次）"
-        )
-    return task
-
-
-def three_camera_reason_zh(
-    state: str,
-    reason: str,
-    active: Mapping[str, Any],
-) -> tuple[str, str]:
-    """Translate a reason code and provide one concrete operator action."""
-    reason = str(reason)
-    sample = active.get("sample", {}) if active else {}
-    missing = [int(value) for value in sample.get("missing_endpoint_u8", [])]
-    sample_range = (
-        f"{_format_u8(sample.get('minimum_u8'))}～"
-        f"{_format_u8(sample.get('maximum_u8'))}"
-    )
-    tolerance = sample.get("endpoint_tolerance_u8", "?")
-
-    if reason.startswith("motor_state_stalled:"):
-        fields = reason.split(":")
-        context = fields[1] if len(fields) > 1 else "unknown"
-        error_match = re.search(r"error_u8=([0-9.]+)", reason)
-        error = error_match.group(1) if error_match else "未知"
-        motor = active.get("motor_index")
-        if motor is not None:
-            return (
-                f"电机{motor}反馈连续8秒没有向目标推进；目标"
-                f"{_format_u8(active.get('target_u8'))}、实际"
-                f"{_format_u8(active.get('actual_u8'))}、误差{error} u8，"
-                f"允许容差±{_format_u8(active.get('tolerance_u8'))} u8"
-                f"（阶段={context}）。程序已保持当前位置。",
-                "若实际反馈是稳定的固件端点，应只配置该电机该端点的专用容差后"
-                "重启；若仍在变化或有摩擦，则先排查机械问题，不要反复resume强推。",
-            )
-        return (
-            f"电机反馈连续8秒没有向目标推进；停止位置距目标{error}个u8"
-            f"（阶段={context}）。程序已保持当前位置，防止机械碰撞或摩擦加重。",
-            "检查该电机是否在机械端点稳定饱和或存在碰撞。若实际反馈已是该型号的"
-            "正常端点，应配置该电机专用端点容差后重启标定；不要反复调用resume强推。",
-        )
-
-    if "URDF zero offset reached the configured" in reason:
-        bound_match = re.search(
-            r"configured\s+([0-9.]+)\s+degree bound", reason
-        )
-        bound = bound_match.group(1) if bound_match else "配置的"
-        hit_text = ""
-        if "bound:" in reason:
-            hit_text = reason.split("bound:", 1)[1].split(
-                "; all_offsets:", 1
-            )[0]
-            for name, label in JOINT_NAMES_ZH.items():
-                hit_text = hit_text.replace(name, label)
-        hit_suffix = f"；触边关节：{hit_text}" if hit_text else ""
-        return (
-            f"联合URDF零位求解触及±{bound}°安全边界{hit_suffix}。这不是可靠的"
-            "零位结果，而是三机位米制位姿或固定关节轴链无法由纯零位旋转共同解释。",
-            "不要调用resume，也不要增大零位边界。先确认Tag有效黑框边长、三相机"
-            "内外参和原始CAD URDF；Tag尺寸修正后必须调用start重新采集，旧尺度"
-            "产生的轨迹不能直接生成修正URDF。",
-        )
-
-    if reason == "sweep_missing_endpoint_bin":
-        missing_text = "、".join(str(value) for value in missing) or "0或255"
-        return (
-            f"本方向已有{active.get('valid_frames', 0)}帧同步有效数据，但缺少"
-            f"电机端点{missing_text}附近的有效分箱；采样到的实际电机范围为"
-            f"{sample_range}，端点容差为±{tolerance}。这通常表示电机虽然运动到"
-            "端点，但该时刻没有同时取得有效Tag图像和电机状态。",
-            "确认当前机位所需Tag在整个行程（尤其缺失端点）均可见，然后调用"
-            "/g20_calibration/resume；程序会重新扫描当前方向，不要调用start。",
-        )
-    if reason == "sweep_bins_too_few":
-        return (
-            f"有效电机分箱只有{sample.get('bin_count', 0)}个，要求至少"
-            f"{sample.get('minimum_bin_count', '?')}个；当前采样范围{sample_range}。",
-            "检查Tag连续识别和电机状态频率，修正后调用resume重新扫描当前方向。",
-        )
-    if reason == "sweep_bin_gap_too_large":
-        return (
-            f"轨迹相邻有效电机分箱的最大空缺为{sample.get('maximum_bin_gap', '?')}，"
-            f"允许值不超过{sample.get('allowed_maximum_bin_gap', '?')}。",
-            "检查运动中Tag是否间歇丢失；修正遮挡、反光或对焦后调用resume。",
-        )
-    if reason == "synchronised_tag_state_timeout":
-        return (
-            "运动过程中连续超过允许时间没有取得“所需Tag全部有效且能与电机状态"
-            "按时间戳配对”的图像帧。",
-            "查看下面活动机位的缺失Tag，确认状态话题仍在更新；修正后调用resume，"
-            "程序会重扫当前方向。",
-        )
-    if reason == "sweep_start_position_timeout":
-        return (
-            f"电机{active.get('motor_index')}未在规定时间到达扫描起点"
-            f"{active.get('start_u8')}，当前实际值{_format_u8(active.get('actual_u8'))}。",
-            "检查CAN、机械手使能和是否存在机械卡阻，确认安全后调用resume。",
-        )
-    if reason == "sweep_timeout":
-        return (
-            "当前方向在规定时间内未完成端点到达、有效帧数和行程覆盖要求。",
-            "检查电机实际值、Tag连续识别和标定速度，修正后调用resume。",
-        )
-    if reason == "return_baseline_timeout":
-        return (
-            "一个或多个标定电机未在规定时间返回基准命令。",
-            "检查机械手状态、CAN和机械卡阻，确认安全后调用resume。",
-        )
-    if reason == "validation_move_timeout":
-        return (
-            "随机复测时电机未在规定时间到达目标命令。",
-            "检查机械手状态和机械卡阻，确认安全后调用resume。",
-        )
-    if reason == "validation_capture_timeout":
-        return (
-            "随机复测位置没有采集到足够的同步有效Tag帧。",
-            "检查当前机位Tag可见性后调用resume。",
-        )
-    if reason == "joint_fit_check_failed":
-        metric_names = {
-            "plane_rms_mm": "平面拟合RMS",
-            "radial_rms_mm": "圆半径拟合RMS",
-            "radius_mm": "拟合半径",
-            "image_radial_rms_px": "二维圆半径拟合RMS",
-            "image_radial_p95_px": "二维圆半径误差P95",
-            "image_radius_px": "二维拟合半径",
-            "arc_deg": "实测圆弧",
-            "monotonic_correction_deg": "最大单调修正",
-            "hysteresis_deg": "最大正反程差",
-            "cycle_travel_range_deg": "三轮行程差",
-            "rotation_orthogonal_rms_deg": "三维旋转轴外残差RMS",
-            "axis_plane_rms_mm": "三维圆轴向RMS",
-            "axis_radial_rms_mm": "三维圆半径RMS",
-            "rotation_circle_axis_difference_deg": "姿态轴与圆轨迹轴夹角",
-            "axis_cycle_difference_deg": "三轮转轴方向极差",
-            "third_cycle_axis_holdout_deg": "第三轮留出零位可观测轴向误差",
-            "third_cycle_axis_line_rms_mm": "第三轮留出轴线RMS",
-            "third_cycle_trajectory_p95_deg": "第三轮留出轨迹误差P95",
-            "state_image_sync_p95_ms": "图像与电机状态同步误差P95",
-            "tag_valid_rate_percent": "所需Tag同时有效率",
-        }
-        metric_units = {
-            "plane_rms_mm": "mm",
-            "radial_rms_mm": "mm",
-            "radius_mm": "mm",
-            "image_radial_rms_px": "px",
-            "image_radial_p95_px": "px",
-            "image_radius_px": "px",
-            "arc_deg": "°",
-            "monotonic_correction_deg": "°",
-            "hysteresis_deg": "°",
-            "cycle_travel_range_deg": "°",
-            "rotation_orthogonal_rms_deg": "°",
-            "axis_plane_rms_mm": "mm",
-            "axis_radial_rms_mm": "mm",
-            "rotation_circle_axis_difference_deg": "°",
-            "axis_cycle_difference_deg": "°",
-            "third_cycle_axis_holdout_deg": "°",
-            "third_cycle_axis_line_rms_mm": "mm",
-            "third_cycle_trajectory_p95_deg": "°",
-            "state_image_sync_p95_ms": "ms",
-            "tag_valid_rate_percent": "%",
-        }
-        details: list[str] = []
-        for failure in active.get("failures", []):
-            joint = JOINT_NAMES_ZH.get(
-                str(failure.get("joint")), str(failure.get("joint"))
-            )
-            metric = str(failure.get("metric", ""))
-            if metric in metric_names:
-                comparison = str(failure.get("comparison", ""))
-                requirement = "不超过" if comparison == "maximum" else "至少"
-                unit = metric_units[metric]
-                detail = (
-                    f"{joint}的{metric_names[metric]}为"
-                    f"{float(failure.get('actual', 0.0)):.2f}{unit}，"
-                    f"要求{requirement}{float(failure.get('limit', 0.0)):.2f}{unit}"
-                )
-                cycle_travel = failure.get("cycle_travel_deg", [])
-                if cycle_travel:
-                    detail += "（三轮=" + "/".join(
-                        f"{float(value):.2f}°" for value in cycle_travel
-                    ) + "）"
-                details.append(detail)
-            else:
-                cycle = failure.get("cycle")
-                cycle_text = "" if cycle is None else f"第{cycle}轮"
-                details.append(
-                    f"{joint}的{cycle_text}{metric or '轨迹'}拟合失败："
-                    f"{failure.get('reason', '未知原因')}"
-                )
-        detail_text = "；".join(details) or "当前关节的轨迹拟合未通过"
-        return (
-            detail_text + "。程序已在当前关节结束后立即停止后续步骤。",
-            "修正Tag位置、遮挡或机械行程后调用"
-            "/g20_calibration/resume；程序只清除当前失败关节的数据"
-            f"并重扫{active.get('directions_to_rescan', 6)}个方向，不要调用start。",
-        )
-    if reason == "zero_model_validation_failed":
-        reason_names = {
-            "zero_offset_reached_configured_bound": "零位解触及安全边界",
-            "zero_offset_exceeds_configured_limit": "零位估计超过安全范围",
-            "zero_offset_reached_diagnostic_bound": "零位估计仍触及诊断搜索边界",
-            "zero_offset_cycle_difference_too_large": "三轮零位离散过大",
-            "zero_offset_not_statistically_significant": "零位偏移未达到统计显著性",
-            "zero_axis_cone_mismatch_too_large": (
-                "父子轴夹角与原始URDF不一致，零位旋转无法解释"
-            ),
-            "zero_phase_axis_line_residual_too_large": (
-                "整段SE(3)运动无法稳定确定平行轴线相位"
-            ),
-            "zero_offset_did_not_improve_with_95pct_confidence": (
-                "第三轮留出验证未以95%置信度改善"
-            ),
-        }
-        details: list[str] = []
-        for failure in active.get("failures", []):
-            joint = JOINT_NAMES_ZH.get(
-                str(failure.get("joint")), str(failure.get("joint"))
-            )
-            if failure.get("metric") == "zero_guard":
-                reason_text = reason_names.get(
-                    str(failure.get("reason")), str(failure.get("reason"))
-                )
-                if "actual_deg" in failure and "limit_deg" in failure:
-                    reason_text += (
-                        f"（估计{float(failure['actual_deg']):+.2f}°，"
-                        f"允许±{float(failure['limit_deg']):.2f}°）"
-                    )
-                details.append(f"{joint}：{reason_text}")
-        return (
-            "轨迹采集已完成，但零位/URDF几何验证失败"
-            + ("（" + "；".join(details) + "）" if details else "")
-            + "。程序没有生成正式JSON或修正URDF。",
-            "该类稳定模型失败不能靠重复运动修复，程序不会自动重扫；"
-            "请检查Tag固定、相机外参和原始URDF后重新启动新标定。",
-        )
-    if reason in {"waiting_for_three_cameras_tags_and_sdk", "preflight_lost"}:
-        return (
-            "正在等待三台相机内参、外参身份匹配、帧率、全部必需Tag以及机械手SDK同时就绪。",
-            "根据下面每个机位的缺失Tag和有效率排查；全部就绪后程序会进入等待开始状态。",
-        )
-    if reason == "call_start":
-        return (
-            "三机位预检已经通过，等待操作员确认开始。",
-            "清空机械手运动范围后调用/g20_calibration/start。",
-        )
-    if reason == "operator_pause":
-        return "操作员主动暂停了标定。", "确认安全后调用/g20_calibration/resume。"
-    if reason == "operator_abort":
-        return "操作员终止了本次标定，程序保持终止时的当前姿态。", "需要重新启动一次新标定。"
-    if reason == "collecting_timestamp_synchronised_tag_centres":
-        return "正在按时间戳配对Tag图像和电机状态并采集当前轨迹。", "无需操作，保持相机、标签和底座不动。"
-    if reason == "capturing_random_validation_pose":
-        return "正在当前随机命令位置采集复测数据。", "无需操作，保持设备不动。"
-    if reason in {"calibration_passed", "calibration_complete"}:
-        return "三维轨迹、关节轴零位和第三轮留出验证已经完成。", "检查JSON、修正URDF路径和quality.passed。"
-    if reason == "quality_failed":
-        return "标定流程完成，但拟合或随机复测质量没有达到验收阈值。", "检查最终JSON的quality以及启动终端中的拟合日志。"
-    if reason.startswith("prepare_") or state == "PREPARE_SWEEP":
-        return "正在把当前电机移动到本方向的扫描起点并等待稳定。", "无需操作。"
-    if state == "RETURN_BASELINE":
-        return "正在把已使用的标定电机恢复到目标姿态。", "无需操作。"
-    if state == "FITTING":
-        return "所有扫描已经完成，正在联合拟合三维机械轴和URDF零位偏移。", "无需操作。"
-    return f"未分类原因码：{reason}", "保留该原因码和启动终端日志用于进一步定位。"
-
-
-def render_three_camera_status_text_zh(payload: Mapping[str, Any]) -> str:
-    """Render the complete operator status; the JSON topic remains unchanged."""
-    state = str(payload.get("state", ""))
-    active = payload.get("active", {})
-    reason_zh, action_zh = three_camera_reason_zh(
-        state, str(payload.get("reason", "")), active
-    )
-    progress = float(payload.get("progress", 0.0))
-    completed = payload.get("completed_sweeps", 0)
-    total = payload.get("total_sweeps", 0)
-    scan_progress = float(
-        payload.get(
-            "scan_progress",
-            0.0 if not total else float(completed) / float(total),
-        )
-    )
-    lines = [
-        f"状态：{STATE_NAMES_ZH.get(state, state)}（{state}）",
-        f"原因：{reason_zh}",
-        f"建议：{action_zh}",
-        f"总体进度：{progress:.1%}（计划扫描{completed}/{total}个方向，"
-        f"扫描进度{scan_progress:.1%}）",
-        f"当前任务：{_task_text(active)}",
-    ]
-    if state == "RETURN_BASELINE":
-        baseline_command = payload.get("baseline_command_u8", [])
-        return_command = payload.get("return_command_u8", baseline_command)
-        label = "恢复姿态" if return_command != baseline_command else "基准姿态"
-        lines.append(f"正在确认{label}：{return_command}")
-    if active and active.get("kind") not in {
-        "fit_failure",
-        "zero_model_failure",
-        "motion_stall",
-    }:
-        retry_count = int(active.get("automatic_retry_count", 0))
-        if retry_count:
-            lines.append(
-                "自动重试：当前方向已自动重扫"
-                f"{retry_count}/{active.get('automatic_retry_limit', '?')}次，"
-                f"速度比例{float(active.get('retry_speed_scale', 1.0)):.0%}，"
-                f"端点保持{float(active.get('endpoint_hold_seconds', 0.0)):.2f}s"
-            )
-        sample = active.get("sample", {})
-        motion_progress = active.get("motion_progress")
-        motion_text = (
-            "未知" if motion_progress is None else f"{float(motion_progress):.1%}"
-        )
-        lines.append(
-            "运动采样："
-            f"目标{active.get('target_u8', active.get('command_u8', '?'))}，"
-            f"实际{_format_u8(active.get('actual_u8'))}，"
-            f"本方向{motion_text}，有效帧{active.get('valid_frames', 0)}，"
-            f"实际采样范围{_format_u8(sample.get('minimum_u8'))}～"
-            f"{_format_u8(sample.get('maximum_u8'))}"
-        )
-        auxiliary = active.get("auxiliary_motors", [])
-        if auxiliary:
-            lines.append(
-                "避挡姿态："
-                + "，".join(
-                    f"电机{item.get('motor_index')}目标"
-                    f"{item.get('command_u8')}、实际"
-                    f"{_format_u8(item.get('actual_u8'))}"
-                    for item in auxiliary
-                )
-            )
-        speed = active.get("speed", {})
-        if speed:
-            lines.append(
-                "阶段速度：五指目标"
-                f"{speed.get('commanded_finger_speed')}，SDK报告"
-                f"{speed.get('reported_finger_speed')}"
-            )
-        if active.get("sweep_timeout_seconds") is not None:
-            lines.append(
-                "运动保护：扫描超时"
-                f"{float(active['sweep_timeout_seconds']):.1f}s，"
-                "连续"
-                f"{float(active.get('motor_stall_timeout_seconds', 0.0)):.1f}s"
-                "进展不足"
-                f"{float(active.get('motor_stall_minimum_progress_u8', 0.0)):.1f}"
-                "则立即暂停"
-            )
-    lines.append("机位：")
-    for name, view in payload.get("views", {}).items():
-        missing = view.get("missing_tag_ids", [])
-        missing_text = "无" if not missing else ",".join(map(str, missing))
-        lines.append(
-            f"- {VIEW_NAMES_ZH.get(str(name), str(name))}："
-            f"{'就绪' if view.get('ready') else '等待'}，"
-            f"外参{'匹配' if view.get('camera_extrinsics_valid') else '不匹配'}，"
-            f"{float(view.get('detection_hz', 0.0)):.1f}Hz，"
-            f"全部必需Tag同时有效率{float(view.get('valid_rate', 0.0)):.1%}，"
-            f"当前缺失Tag={missing_text}"
-        )
-    extrinsics_error = payload.get("camera_extrinsics_error")
-    if extrinsics_error:
-        lines.append(f"外参文件：{extrinsics_error}")
-    lines.append(f"JSON结果：{payload.get('result_path') or '尚未生成'}")
-    lines.append(
-        f"修正URDF：{payload.get('corrected_urdf_path') or '尚未生成'}"
-    )
-    return "\n".join(lines)

src/g20_thumb_apriltag_calibration/setup.cfg

@@ -1,4 +0,0 @@
-[develop]
-script_dir=$base/lib/g20_thumb_apriltag_calibration
-[install]
-install_scripts=$base/lib/g20_thumb_apriltag_calibration

src/g20_thumb_apriltag_calibration/setup.py

@@ -1,69 +0,0 @@
-from glob import glob
-
-from setuptools import find_packages, setup
-
-
-package_name = "g20_thumb_apriltag_calibration"
-
-setup(
-    name=package_name,
-    version="0.1.0",
-    packages=find_packages(),
-    data_files=[
-        (
-            "share/ament_index/resource_index/packages",
-            ["resource/" + package_name],
-        ),
-        ("share/" + package_name, ["package.xml", "README.md"]),
-        (
-            "share/" + package_name + "/config",
-            glob("config/*.yaml") + glob("config/*.xml"),
-        ),
-        ("share/" + package_name + "/launch", glob("launch/*.launch.py")),
-    ],
-    install_requires=["setuptools", "numpy", "scipy", "PyYAML"],
-    tests_require=["pytest"],
-    zip_safe=True,
-    maintainer="lxp",
-    maintainer_email="support@linker-robotics.com",
-    description="Three-view Hikrobot AprilTag calibration for the complete left G20 hand",
-    license="MIT",
-    entry_points={
-        "console_scripts": [
-            (
-                "hikrobot_camera_node = "
-                "g20_thumb_apriltag_calibration.hikrobot_camera:main"
-            ),
-            "calibration_node = g20_thumb_apriltag_calibration.node:main",
-            (
-                "cmc_pitch_zero_node = "
-                "g20_thumb_apriltag_calibration.zero_node:main"
-            ),
-            (
-                "cmc_roll_calibration_node = "
-                "g20_thumb_apriltag_calibration.zero_node:main"
-            ),
-            (
-                "three_camera_calibration_node = "
-                "g20_thumb_apriltag_calibration.three_camera_node:main"
-            ),
-            (
-                "three_camera_extrinsics_node = "
-                "g20_thumb_apriltag_calibration.extrinsics_node:main"
-            ),
-            (
-                "offline_replay = "
-                "g20_thumb_apriltag_calibration.offline_replay:main"
-            ),
-            (
-                "camera_alignment_view = "
-                "g20_thumb_apriltag_calibration.alignment_view:main"
-            ),
-            (
-                "calibrated_joint_state_bridge = "
-                "g20_thumb_apriltag_calibration."
-                "calibrated_joint_state_bridge:main"
-            ),
-        ],
-    },
-)

src/g20_thumb_apriltag_calibration/test/test_calibrated_joint_state_bridge.py

@@ -1,87 +0,0 @@
-import copy
-
-import pytest
-
-from g20_thumb_apriltag_calibration.calibrated_joint_state_bridge import (
-    G20_COMMAND_NAMES,
-    G20_URDF_JOINT_NAMES,
-    CalibratedCommandMapper,
-)
-from g20_thumb_apriltag_calibration.full_hand import (
-    get_hand_calibration_profile,
-)
-
-
-def _payload(side: str = "right") -> dict:
-    profile = get_hand_calibration_profile(side)
-    curve = [round((255 - command) * 0.001, 8) for command in range(256)]
-    joints = {}
-    for name, spec in profile.joint_specs.items():
-        joint = {
-            "motor_index": spec.motor_index,
-            "angle_rad": list(curve),
-        }
-        if spec.active:
-            joint["zero_command_u8"] = 255
-            joint["zero_angles"] = {"urdf_zero_offset_rad": 0.0}
-        else:
-            joint["passive"] = True
-        if spec.source_joint is not None:
-            joint["source_joint"] = spec.source_joint
-        joints[name] = joint
-    return {
-        "schema_version": 4,
-        "model": "G20",
-        "side": side,
-        "serial_number": "TEST_RIGHT" if side == "right" else "TEST_LEFT",
-        "angle_unit": "rad",
-        "command_range": [0, 255],
-        "baseline_command_u8": [255] * 20,
-        "joints": joints,
-        "quality": {
-            "passed": True,
-            "validation_mae_rad": 0.01,
-            "validation_p95_rad": 0.02,
-        },
-    }
-
-
-def test_mapper_uses_each_joint_motor_and_includes_passive_joints() -> None:
-    mapper = CalibratedCommandMapper(_payload(), expected_side="right")
-    command = list(range(20))
-    result = dict(
-        zip(G20_URDF_JOINT_NAMES, mapper.map_positions(command))
-    )
-
-    assert result["thumb_cmc_pitch"] == pytest.approx(0.255)
-    assert result["thumb_cmc_roll"] == pytest.approx(0.250)
-    assert result["thumb_cmc_yaw"] == pytest.approx(0.245)
-    assert result["thumb_mcp"] == pytest.approx(0.240)
-    assert result["thumb_ip"] == pytest.approx(0.240)
-    assert result["pinky_pip"] == pytest.approx(0.236)
-    assert result["pinky_dip"] == pytest.approx(0.236)
-
-
-def test_mapper_uses_names_instead_of_message_order() -> None:
-    mapper = CalibratedCommandMapper(_payload(), expected_side="right")
-    command = list(range(20))
-    names = list(reversed(G20_COMMAND_NAMES))
-    positions = list(reversed(command))
-
-    assert mapper.map_positions(positions, names) == mapper.map_positions(command)
-
-
-def test_mapper_rejects_wrong_side_and_unapproved_payload() -> None:
-    with pytest.raises(ValueError, match="does not match"):
-        CalibratedCommandMapper(_payload("left"), expected_side="right")
-
-    payload = copy.deepcopy(_payload())
-    payload["quality"]["passed"] = False
-    with pytest.raises(ValueError, match="quality.passed"):
-        CalibratedCommandMapper(payload, expected_side="right")
-
-
-def test_mapper_rejects_incomplete_named_command() -> None:
-    mapper = CalibratedCommandMapper(_payload(), expected_side="right")
-    with pytest.raises(ValueError, match="missing named channels"):
-        mapper.map_positions([255.0], ["thumb_cmc_pitch"])

src/g20_thumb_apriltag_calibration/test/test_full_hand.py

@@ -1,500 +0,0 @@
-import math
-from dataclasses import replace
-
-import numpy as np
-import pytest
-
-from g20_thumb_apriltag_calibration.full_hand import (
-    ACTIVE_JOINTS,
-    IMAGE_TRAJECTORY_JOINTS,
-    JOINT_SPECS,
-    LEFT_HAND_PROFILE,
-    MEASURED_JOINTS,
-    PASSIVE_JOINTS,
-    RIGHT_HAND_PROFILE,
-    SPLAY_JOINTS,
-    SWEEP_SPECS,
-    VIEW_TAGS,
-    build_calibration_motion_command,
-    build_calibration_speed_profile,
-    build_compact_payload,
-    build_full_hand_command,
-    center_splay_curve,
-    fit_joint_center_curve,
-    fit_joint_image_curve,
-    fit_measured_joint_curve,
-    fit_projected_zero,
-    get_hand_calibration_profile,
-    measure_joint_observation,
-    validate_compact_payload,
-)
-
-
-def _records() -> list[dict[str, object]]:
-    commands = list(range(0, 256, 16))
-    if commands[-1] != 255:
-        commands.append(255)
-    records: list[dict[str, object]] = []
-    centre = np.asarray([0.006, -0.004, 0.012])
-    radius = 0.025
-    image_centre = np.asarray([30.0, -12.0])
-    image_radius = 100.0
-    for cycle in range(3):
-        for direction, sequence in (
-            ("decreasing", reversed(commands)),
-            ("increasing", commands),
-        ):
-            for command in sequence:
-                angle = 0.70 * (255.0 - command) / 255.0
-                point = centre + np.asarray(
-                    [radius * math.cos(angle), radius * math.sin(angle), 0.0]
-                )
-                # At command 255 the inward vector points along image +x, so
-                # table_projected_zero_rad is exactly zero.
-                image_point = image_centre + np.asarray(
-                    [
-                        -image_radius * math.cos(angle),
-                        image_radius * math.sin(angle),
-                    ]
-                )
-                records.append(
-                    {
-                        "cycle": cycle,
-                        "direction": direction,
-                        "command_u8": command,
-                        "relative_translation_xyz_m": point.tolist(),
-                        "image_relative_xy_px": image_point.tolist(),
-                    }
-                )
-    return records
-
-
-def test_joint_layout_covers_16_active_and_5_passive_joints() -> None:
-    assert len(JOINT_SPECS) == 21
-    assert len(ACTIVE_JOINTS) == 16
-    assert len(PASSIVE_JOINTS) == 5
-
-
-def test_right_profile_measures_pinky_and_inherits_to_other_fingers() -> None:
-    profile = get_hand_calibration_profile("right")
-    assert profile is RIGHT_HAND_PROFILE
-    assert profile.reference_finger == "pinky"
-    assert [spec.motor_index for spec in profile.sweep_specs] == [
-        0, 5, 15, 9, 4, 19, 10
-    ]
-    assert profile.view_tags["front"]["pinky_roll"] == 10
-    assert profile.view_tags["side"] == {
-        "side_base": 4,
-        "pinky_mcp": 5,
-        "pinky_pip": 6,
-        "pinky_dip": 7,
-    }
-    assert profile.preflight_view_roles["side"] == (
-        "side_base",
-        "pinky_mcp",
-        "pinky_pip",
-        "pinky_dip",
-    )
-    thumb_pitch = profile.joint_specs["thumb_cmc_pitch"]
-    assert thumb_pitch.view == "front"
-    assert thumb_pitch.parent_role == "front_base"
-    assert thumb_pitch.child_role == "thumb_cmc"
-    assert profile.joint_specs["index_mcp_roll"].source_joint == (
-        "pinky_mcp_roll"
-    )
-    assert profile.joint_specs["middle_mcp_pitch"].source_joint == (
-        "pinky_mcp_pitch"
-    )
-    assert profile.joint_specs["ring_pip"].source_joint == "pinky_pip"
-    assert profile.joint_specs["index_dip"].source_joint == "pinky_dip"
-
-    roll = next(spec for spec in profile.sweep_specs if spec.motor_index == 9)
-    command = build_calibration_motion_command(
-        roll, 127, profile=profile
-    )
-    assert command[9] == 127
-    assert command[6:9] == [255, 255, 255]
-    speeds = build_calibration_speed_profile(
-        roll,
-        normal_speed=15,
-        index_roll_speed=5,
-        index_flex_speed=10,
-        profile=profile,
-    )
-    assert speeds == [15, 15, 15, 15, 5]
-    assert {tag for tags in VIEW_TAGS.values() for tag in tags.values()} == set(
-        range(11)
-    )
-    assert VIEW_TAGS["front"]["index_roll"] == 10
-    assert VIEW_TAGS["side"] == {
-        "side_base": 4,
-        "index_mcp": 5,
-        "index_pip": 6,
-        "index_dip": 7,
-    }
-    assert VIEW_TAGS["top"] == {"top_base": 8, "thumb_yaw": 9}
-    assert [spec.motor_index for spec in SWEEP_SPECS] == [0, 5, 15, 6, 1, 16, 10]
-
-
-def test_joint_trajectory_spaces_match_observation_geometry() -> None:
-    assert IMAGE_TRAJECTORY_JOINTS == {
-        "thumb_cmc_pitch",
-        "thumb_cmc_roll",
-        "thumb_mcp",
-        "thumb_ip",
-        "index_mcp_roll",
-        "index_mcp_pitch",
-        "index_pip",
-    }
-    records = _records()
-    for name in IMAGE_TRAJECTORY_JOINTS:
-        fit = fit_measured_joint_curve(name, records)
-        assert fit.circle["space"] == "image_2d"
-    for name in ("index_dip", "thumb_cmc_yaw"):
-        assert "space" not in fit_measured_joint_curve(name, records).circle
-
-
-def test_full_hand_command_changes_exactly_one_controlled_motor() -> None:
-    result = build_full_hand_command(6, 27)
-    assert result[6] == 27
-    assert result[:6] == [255] * 6
-    with pytest.raises(ValueError, match="controlled"):
-        build_full_hand_command(11, 27)
-
-
-def test_index_roll_motion_moves_other_three_roll_motors_out_of_view() -> None:
-    baseline = [255] * 20
-    baseline[6:10] = [127, 127, 127, 127]
-    index_roll = next(spec for spec in SWEEP_SPECS if spec.motor_index == 6)
-
-    result = build_calibration_motion_command(index_roll, 27, baseline)
-
-    assert result[6:10] == [27, 0, 0, 0]
-    assert result[:6] == baseline[:6]
-    assert result[10:] == baseline[10:]
-
-
-def test_right_pinky_roll_moves_other_three_fingers_camera_right() -> None:
-    profile = RIGHT_HAND_PROFILE
-    pinky_roll = next(
-        spec for spec in profile.sweep_specs if spec.motor_index == 9
-    )
-
-    result = build_calibration_motion_command(
-        pinky_roll, 27, profile=profile
-    )
-
-    assert result[6:10] == [255, 255, 255, 27]
-    assert profile.roll_clearance_commands == {6: 255, 7: 255, 8: 255}
-
-
-def test_non_index_roll_motion_keeps_clearance_motors_at_baseline() -> None:
-    baseline = [255] * 20
-    baseline[6:10] = [127, 127, 127, 127]
-    thumb_pitch = next(spec for spec in SWEEP_SPECS if spec.motor_index == 0)
-
-    result = build_calibration_motion_command(thumb_pitch, 17, baseline)
-
-    assert result[0] == 17
-    assert result[6:10] == [127, 127, 127, 127]
-
-
-def test_right_thumb_pitch_uses_front_visible_yaw_and_roll_pose() -> None:
-    profile = RIGHT_HAND_PROFILE
-    thumb_pitch = next(
-        spec for spec in profile.sweep_specs if spec.motor_index == 0
-    )
-
-    result = build_calibration_motion_command(
-        thumb_pitch, 17, profile=profile
-    )
-
-    assert result[0] == 17
-    assert result[10] == 255
-    assert result[5] == 255
-    assert profile.thumb_pitch_clearance_commands == {10: 255, 5: 255}
-
-
-def test_left_thumb_pitch_keeps_legacy_baseline_pose() -> None:
-    thumb_pitch = next(spec for spec in SWEEP_SPECS if spec.motor_index == 0)
-
-    result = build_calibration_motion_command(thumb_pitch, 17)
-
-    assert result[0] == 17
-    assert result[5] == 255
-    assert result[10] == 255
-    assert LEFT_HAND_PROFILE.thumb_pitch_clearance_commands == {}
-
-
-def test_thumb_yaw_motion_holds_thumb_roll_at_camera_clearance_pose() -> None:
-    baseline = [255] * 20
-    baseline[6:10] = [127, 127, 127, 127]
-    thumb_yaw = next(spec for spec in SWEEP_SPECS if spec.motor_index == 10)
-
-    result = build_calibration_motion_command(thumb_yaw, 27, baseline)
-
-    assert result[5] == 145
-    assert result[10] == 27
-    assert result[:5] == baseline[:5]
-    assert result[6:10] == baseline[6:10]
-    assert result[11:] == baseline[11:]
-
-
-def test_only_index_roll_uses_the_slow_index_finger_speed() -> None:
-    index_roll = next(spec for spec in SWEEP_SPECS if spec.motor_index == 6)
-    index_pitch = next(spec for spec in SWEEP_SPECS if spec.motor_index == 1)
-
-    assert build_calibration_speed_profile(
-        index_roll,
-        normal_speed=15,
-        index_roll_speed=5,
-        index_flex_speed=10,
-    ) == [15, 5, 15, 15, 15]
-    assert build_calibration_speed_profile(
-        index_pitch,
-        normal_speed=15,
-        index_roll_speed=5,
-        index_flex_speed=10,
-    ) == [15, 10, 15, 15, 15]
-
-    index_pip = next(spec for spec in SWEEP_SPECS if spec.motor_index == 16)
-    assert build_calibration_speed_profile(
-        index_pip,
-        normal_speed=15,
-        index_roll_speed=5,
-        index_flex_speed=10,
-    ) == [15, 10, 15, 15, 15]
-
-    thumb_pitch = next(spec for spec in SWEEP_SPECS if spec.motor_index == 0)
-    assert build_calibration_speed_profile(
-        thumb_pitch,
-        normal_speed=15,
-        index_roll_speed=5,
-        index_flex_speed=10,
-    ) == [15, 15, 15, 15, 15]
-
-
-def test_calibration_speed_profile_rejects_out_of_range_values() -> None:
-    index_roll = next(spec for spec in SWEEP_SPECS if spec.motor_index == 6)
-    with pytest.raises(ValueError, match="speeds"):
-        build_calibration_speed_profile(
-            index_roll,
-            normal_speed=15,
-            index_roll_speed=256,
-            index_flex_speed=10,
-        )
-
-
-def test_curve_fit_and_projected_zero_recover_synthetic_geometry() -> None:
-    records = _records()
-    fit = fit_joint_center_curve(records)
-    assert fit.angle_rad[255] == pytest.approx(0.0, abs=1.0e-8)
-    assert fit.angle_rad[0] == pytest.approx(0.70, abs=1.0e-4)
-    assert fit.maximum_hysteresis_rad == pytest.approx(0.0, abs=1.0e-8)
-    assert fit_projected_zero(records) == pytest.approx(0.0, abs=1.0e-6)
-
-
-def test_thumb_image_curve_avoids_corrupted_pnp_depth() -> None:
-    records = _records()
-    for record in records:
-        command = int(record["command_u8"])
-        record["relative_translation_xyz_m"] = [
-            0.001 * command,
-            0.0,
-            0.0,
-        ]
-
-    fit = fit_measured_joint_curve("thumb_mcp", records)
-
-    assert fit.circle["space"] == "image_2d"
-    assert fit.angle_rad[0] == pytest.approx(0.70, abs=0.02)
-    assert fit.angle_rad[255] == pytest.approx(0.0)
-    assert math.degrees(fit.maximum_monotonic_correction_rad) < 0.01
-    assert math.degrees(fit.maximum_hysteresis_rad) < 0.01
-
-    command_zero = next(
-        record for record in records if int(record["command_u8"]) == 0
-    )
-    observed = measure_joint_observation(
-        fit,
-        vector_xyz_m=command_zero["relative_translation_xyz_m"],
-        image_vector_xy_px=command_zero["image_relative_xy_px"],
-    )
-    assert observed == pytest.approx(0.70, abs=0.02)
-
-
-def test_image_curve_rejects_insufficient_projected_arc() -> None:
-    records = _records()
-    for record in records:
-        command = float(record["command_u8"])
-        angle = math.radians(2.0) * (255.0 - command) / 255.0
-        record["image_relative_xy_px"] = [
-            100.0 * math.cos(angle),
-            100.0 * math.sin(angle),
-        ]
-
-    with pytest.raises(
-        ValueError, match="joint_image_trajectory_quality_failed:arc"
-    ):
-        fit_joint_image_curve(records)
-
-
-def test_splay_uses_angular_midpoint_not_fixed_command_midpoint() -> None:
-    fit = fit_joint_center_curve(_records())
-    centred, zero_command, midpoint = center_splay_curve(fit)
-    assert midpoint == pytest.approx(0.35, abs=1.0e-4)
-    assert centred.angle_rad[0] == pytest.approx(0.35, abs=1.0e-4)
-    assert centred.angle_rad[255] == pytest.approx(-0.35, abs=1.0e-4)
-    assert zero_command in {127, 128}
-    assert abs(centred.angle_rad[zero_command]) <= 0.002
-
-
-def test_compact_payload_contains_only_runtime_fields_and_inheritance() -> None:
-    base = fit_joint_center_curve(_records())
-    splay, zero_command, _ = center_splay_curve(base)
-    splay = replace(
-        splay,
-        angle_rad=tuple(
-            value - splay.angle_rad[zero_command]
-            for value in splay.angle_rad
-        ),
-    )
-    measured = {
-        name: splay if name == "index_mcp_roll" else base
-        for name in MEASURED_JOINTS
-    }
-    offsets = {name: 0.01 for name in ACTIVE_JOINTS}
-    baseline = [255] * 20
-    baseline[6:10] = [zero_command] * 4
-    payload = build_compact_payload(
-        serial_number="G20_LEFT_001",
-        measured_fits=measured,
-        urdf_zero_offsets_rad=offsets,
-        validation_errors_rad=[0.01, -0.02],
-        passed=True,
-        baseline=baseline,
-    )
-    validate_compact_payload(payload)
-    assert set(payload) == {
-        "schema_version",
-        "model",
-        "side",
-        "serial_number",
-        "angle_unit",
-        "command_range",
-        "baseline_command_u8",
-        "joints",
-        "quality",
-    }
-    assert len(payload["joints"]) == 21
-    assert all(len(joint["angle_rad"]) == 256 for joint in payload["joints"].values())
-    assert sum("zero_command_u8" in joint for joint in payload["joints"].values()) == 16
-    assert sum(joint.get("passive") is True for joint in payload["joints"].values()) == 5
-    assert payload["joints"]["middle_mcp_roll"]["source_joint"] == "index_mcp_roll"
-    assert (
-        payload["joints"]["middle_mcp_roll"]["angle_rad"]
-        == payload["joints"]["index_mcp_roll"]["angle_rad"]
-    )
-    assert payload["joints"]["index_mcp_roll"]["zero_angles"] == {
-        "urdf_zero_offset_rad": pytest.approx(0.01)
-    }
-    for name in SPLAY_JOINTS:
-        assert payload["joints"][name]["zero_command_u8"] == zero_command
-
-
-def test_compact_payload_allows_skipped_random_validation() -> None:
-    base = fit_joint_center_curve(_records())
-    splay, zero_command, _ = center_splay_curve(base)
-    splay = replace(
-        splay,
-        angle_rad=tuple(
-            value - splay.angle_rad[zero_command]
-            for value in splay.angle_rad
-        ),
-    )
-    measured = {
-        name: splay if name == "index_mcp_roll" else base
-        for name in MEASURED_JOINTS
-    }
-    offsets = {name: 0.0 for name in ACTIVE_JOINTS}
-    baseline = [255] * 20
-    baseline[6:10] = [zero_command] * 4
-
-    payload = build_compact_payload(
-        serial_number="G20_LEFT_001",
-        measured_fits=measured,
-        urdf_zero_offsets_rad=offsets,
-        validation_errors_rad=[],
-        passed=True,
-        baseline=baseline,
-    )
-
-    validate_compact_payload(payload)
-    assert payload["quality"] == {
-        "passed": True,
-        "validation_mae_rad": None,
-        "validation_p95_rad": None,
-    }
-
-
-def test_right_compact_payload_keeps_v4_shape_and_uses_pinky_sources() -> None:
-    profile = RIGHT_HAND_PROFILE
-    base = fit_joint_center_curve(_records())
-    splay, zero_command, _ = center_splay_curve(base)
-    splay = replace(
-        splay,
-        angle_rad=tuple(
-            value - splay.angle_rad[zero_command]
-            for value in splay.angle_rad
-        ),
-    )
-    measured = {
-        name: splay if name == "pinky_mcp_roll" else base
-        for name in profile.measured_joints
-    }
-    offsets = {
-        name: (0.01 if profile.joint_specs[name].source_joint is None else 0.0)
-        for name in profile.active_joints
-    }
-    baseline = [255] * 20
-    baseline[6:10] = [zero_command] * 4
-    payload = build_compact_payload(
-        serial_number="G20_RIGHT_001",
-        measured_fits=measured,
-        urdf_zero_offsets_rad=offsets,
-        validation_errors_rad=[0.01],
-        passed=True,
-        baseline=baseline,
-        side="right",
-    )
-
-    validate_compact_payload(payload)
-    assert payload["schema_version"] == 4
-    assert payload["side"] == "right"
-    assert set(payload) == {
-        "schema_version",
-        "model",
-        "side",
-        "serial_number",
-        "angle_unit",
-        "command_range",
-        "baseline_command_u8",
-        "joints",
-        "quality",
-    }
-    for finger in ("index", "middle", "ring"):
-        assert payload["joints"][f"{finger}_mcp_roll"]["source_joint"] == (
-            "pinky_mcp_roll"
-        )
-        assert payload["joints"][f"{finger}_mcp_pitch"]["source_joint"] == (
-            "pinky_mcp_pitch"
-        )
-        assert payload["joints"][f"{finger}_pip"]["source_joint"] == (
-            "pinky_pip"
-        )
-        for suffix in ("mcp_roll", "mcp_pitch", "pip"):
-            joint = payload["joints"][f"{finger}_{suffix}"]
-            source = payload["joints"][joint["source_joint"]]
-            assert joint["angle_rad"] == source["angle_rad"]
-            assert joint["zero_angles"] == {"urdf_zero_offset_rad": 0.0}
-            assert source["zero_angles"] == {"urdf_zero_offset_rad": 0.01}

src/g20_thumb_apriltag_calibration/test/test_offline_replay.py

@@ -1,44 +0,0 @@
-import pytest
-
-from g20_thumb_apriltag_calibration.offline_replay import (
-    _latest_attempt_records,
-    _output_suffix,
-)
-
-
-def _sample(joint: str, cycle: int, direction: str, attempt: int) -> dict:
-    return {
-        "kind": "sample",
-        "joint": joint,
-        "cycle": cycle,
-        "direction": direction,
-        "attempt": attempt,
-    }
-
-
-def test_latest_attempt_is_selected_per_joint_cycle_and_direction() -> None:
-    rows = [
-        {"kind": "session_start"},
-        _sample("pinky_pip", 0, "decreasing", 1),
-        _sample("pinky_pip", 0, "decreasing", 3),
-        _sample("pinky_pip", 0, "increasing", 1),
-        _sample("pinky_pip", 1, "decreasing", 2),
-        _sample("thumb_cmc_yaw", 0, "decreasing", 1),
-    ]
-
-    selected = _latest_attempt_records(rows)
-
-    assert [
-        record["attempt"] for record in selected["pinky_pip"]
-    ] == [3, 1, 2]
-    assert [
-        record["attempt"] for record in selected["thumb_cmc_yaw"]
-    ] == [1]
-
-
-def test_output_suffix_is_safe_and_explicit() -> None:
-    assert _output_suffix(None) == ""
-    assert _output_suffix("MEASURED_ZERO_V2") == "_MEASURED_ZERO_V2"
-    for invalid in ("", "../escape", "/absolute", "contains space", "x" * 65):
-        with pytest.raises(ValueError, match="output tag"):
-            _output_suffix(invalid)

src/g20_thumb_apriltag_calibration/test/test_urdf_zero.py

@@ -1,977 +0,0 @@
-import math
-from pathlib import Path
-import xml.etree.ElementTree as ET
-
-import numpy as np
-import pytest
-from scipy.spatial.transform import Rotation
-
-from g20_thumb_apriltag_calibration.extrinsics import (
-    camera_info_fingerprint,
-    dump_three_camera_extrinsics,
-    load_three_camera_extrinsics,
-)
-from g20_thumb_apriltag_calibration.urdf_zero import (
-    AXIS_JOINTS,
-    DIRECT_ZERO_JOINTS,
-    INHERITED_ZERO_JOINTS,
-    JointAxisMeasurement,
-    UrdfKinematicModel,
-    _angles_from_state,
-    _zero_sensitive_axis_error_rad,
-    fit_joint_axis_measurement,
-    fit_rotation_joint_curve,
-    solve_urdf_zero_offsets,
-    get_zero_calibration_profile,
-    write_zero_corrected_urdf,
-)
-from g20_thumb_apriltag_calibration.full_hand import (
-    ACTIVE_JOINTS,
-    JOINT_SPECS,
-    MEASURED_JOINTS,
-    PASSIVE_JOINTS,
-    JointCurveFit,
-    get_hand_calibration_profile,
-)
-
-
-REPOSITORY = Path(__file__).resolve().parents[3]
-SOURCE_URDF = REPOSITORY / (
-    "src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/"
-    "linker_hand/g20_left/linkerhand_g20_left.urdf"
-)
-RIGHT_SOURCE_URDF = REPOSITORY / (
-    "src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/"
-    "linker_hand/g20_right/linkerhand_g20_right.urdf"
-)
-
-
-def test_zero_sensitive_axis_error_ignores_fixed_cone_angle_mismatch():
-    parent = np.asarray([0.0, 0.0, 1.0])
-    predicted = np.asarray([1.0, 0.0, 0.0])
-    cone_mismatch = np.asarray(
-        [math.cos(math.radians(10.0)), 0.0, math.sin(math.radians(10.0))]
-    )
-    zero_mismatch = np.asarray(
-        [math.cos(math.radians(3.0)), math.sin(math.radians(3.0)), 0.0]
-    )
-
-    assert _zero_sensitive_axis_error_rad(
-        predicted, cone_mismatch, parent
-    ) == pytest.approx(0.0, abs=1.0e-12)
-    assert math.degrees(
-        _zero_sensitive_axis_error_rad(predicted, zero_mismatch, parent)
-    ) == pytest.approx(3.0, abs=1.0e-9)
-
-
-def test_zero_sensitive_axis_error_is_exact_for_an_oblique_cone() -> None:
-    parent = np.asarray([0.0, 0.0, 1.0])
-    cone = math.radians(32.0)
-    phase = math.radians(7.0)
-    predicted = np.asarray([math.sin(cone), 0.0, math.cos(cone)])
-    observed = Rotation.from_rotvec(parent * phase).apply(predicted)
-
-    error = _zero_sensitive_axis_error_rad(predicted, observed, parent)
-
-    assert math.degrees(error) == pytest.approx(7.0, abs=1.0e-9)
-
-
-def _payload(transform: np.ndarray) -> dict[str, list[float]]:
-    return {
-        "translation_xyz_m": transform[:3, 3].tolist(),
-        "quaternion_xyzw": Rotation.from_matrix(
-            transform[:3, :3]
-        ).as_quat().tolist(),
-    }
-
-
-def _arbitrary_tag_records() -> tuple[list[dict], np.ndarray, np.ndarray]:
-    axis_parent = np.asarray([0.23, -0.31, 0.922], dtype=float)
-    axis_parent /= np.linalg.norm(axis_parent)
-    centre_parent = np.asarray([0.012, -0.008, 0.021])
-    radial = np.cross(axis_parent, np.asarray([0.7, 0.1, -0.2]))
-    radial = 0.035 * radial / np.linalg.norm(radial)
-    child_tag_mount = Rotation.from_euler(
-        "xyz", [1.1, -0.7, 0.45]
-    )
-    common_from_parent = np.eye(4)
-    common_from_parent[:3, :3] = Rotation.from_euler(
-        "xyz", [-0.8, 0.55, 1.3]
-    ).as_matrix()
-    common_from_parent[:3, 3] = [0.41, -0.12, 0.73]
-    expected_axis = common_from_parent[:3, :3] @ axis_parent
-    expected_point = (
-        common_from_parent[:3, :3] @ centre_parent
-        + common_from_parent[:3, 3]
-    )
-
-    commands = list(range(0, 256, 16)) + [255]
-    records = []
-    for cycle in range(3):
-        for direction in ("decreasing", "increasing"):
-            for command in commands:
-                angle = math.radians(62.0) * (255.0 - command) / 255.0
-                motion = Rotation.from_rotvec(axis_parent * angle)
-                relative_rotation = motion * child_tag_mount
-                relative_translation = centre_parent + motion.apply(radial)
-                child_common = common_from_parent.copy()
-                child_common[:3, :3] = (
-                    common_from_parent[:3, :3]
-                    @ relative_rotation.as_matrix()
-                )
-                child_common[:3, 3] = (
-                    common_from_parent[:3, :3] @ relative_translation
-                    + common_from_parent[:3, 3]
-                )
-                state = [255.0] * 20
-                state[5] = float(command)
-                records.append(
-                    {
-                        "cycle": cycle,
-                        "direction": direction,
-                        "command_u8": command,
-                        "relative_translation_xyz_m": relative_translation.tolist(),
-                        "relative_quaternion_xyzw": relative_rotation.as_quat().tolist(),
-                        "parent_pose_common": _payload(common_from_parent),
-                        "child_pose_common": _payload(child_common),
-                        "state_u8": state,
-                    }
-                )
-    return records, expected_axis, expected_point
-
-
-def test_axis_and_curve_ignore_camera_and_tag_mount_rotation() -> None:
-    records, expected_axis, expected_point = _arbitrary_tag_records()
-    curve = fit_rotation_joint_curve(records, zero_command_u8=255)
-    measurement = fit_joint_axis_measurement(
-        "thumb_cmc_roll", records, cycle=0, zero_command_u8=255
-    )
-
-    observed_axis = np.asarray(measurement.axis_common_xyz)
-    observed_point = np.asarray(measurement.point_common_xyz_m)
-    assert float(observed_axis @ expected_axis) > math.cos(math.radians(0.05))
-    assert np.linalg.norm(
-        np.cross(observed_point - expected_point, expected_axis)
-    ) < 1.0e-6
-    assert curve.angle_rad[255] == pytest.approx(0.0, abs=1.0e-9)
-    assert curve.angle_rad[0] == pytest.approx(math.radians(62.0), abs=1.0e-6)
-
-
-@pytest.mark.parametrize("joint", ["thumb_cmc_pitch", "index_mcp_pitch"])
-def test_image_plane_joint_uses_rotation_axis_to_constrain_noisy_depth(
-    joint: str,
-) -> None:
-    records, expected_axis, expected_point = _arbitrary_tag_records()
-    parent_rotation = Rotation.from_euler("xyz", [-0.8, 0.55, 1.3])
-    axis_parent = parent_rotation.inv().apply(expected_axis)
-    tangent = np.cross(axis_parent, np.asarray([0.4, -0.2, 0.7]))
-    tangent /= np.linalg.norm(tangent)
-    # Reproduce monocular planar-PnP depth bias: the centre trajectory remains
-    # precise in its dominant directions but receives a command-correlated
-    # component that makes a free 3-D plane normal substantially wrong.
-    biased_records = []
-    for record in records:
-        biased = dict(record)
-        point = np.asarray(record["relative_translation_xyz_m"], dtype=float)
-        depth_bias = 0.30 * float(point @ tangent)
-        biased["relative_translation_xyz_m"] = (
-            point + depth_bias * axis_parent
-        ).tolist()
-        biased_records.append(biased)
-
-    measurement = fit_joint_axis_measurement(
-        joint, biased_records, cycle=0, zero_command_u8=255
-    )
-
-    observed_axis = np.asarray(measurement.axis_common_xyz)
-    observed_point = np.asarray(measurement.point_common_xyz_m)
-    assert abs(float(observed_axis @ expected_axis)) > math.cos(
-        math.radians(0.05)
-    )
-    assert np.linalg.norm(
-        np.cross(observed_point - expected_point, expected_axis)
-    ) < 0.003
-    assert measurement.rotation_circle_axis_difference_rad > math.radians(5.0)
-    assert measurement.plane_rms_m < 0.003
-    assert measurement.radial_rms_m < 0.003
-
-
-def test_pose_axis_point_rejects_end_on_optical_depth_bias() -> None:
-    records, expected_axis, expected_point = _arbitrary_tag_records()
-    common_from_parent = Rotation.from_euler("xyz", [-0.8, 0.55, 1.3])
-    axis_parent = common_from_parent.inv().apply(expected_axis)
-    # Exact end-on depth is a gauge along the physical axis and therefore
-    # cannot alter the observable axis line.  An oblique camera has a small
-    # irreducible coupling between monocular depth and radial position; that
-    # case must be bounded by the residual/holdout gates, not asserted to be
-    # exactly recoverable from one view.
-    view_normal_parent = axis_parent
-    view_normal_common = common_from_parent.apply(view_normal_parent)
-    biased = []
-    for record in records:
-        changed = dict(record)
-        fraction = (255.0 - float(record["command_u8"])) / 255.0
-        depth_bias = 0.03 * (fraction - 0.5)
-        changed["relative_translation_xyz_m"] = (
-            np.asarray(record["relative_translation_xyz_m"], dtype=float)
-            + depth_bias * view_normal_parent
-        ).tolist()
-        biased.append(changed)
-
-    measurement = fit_joint_axis_measurement(
-        "thumb_cmc_pitch",
-        biased,
-        cycle=0,
-        zero_command_u8=255,
-        view_normal_common_xyz=view_normal_common,
-    )
-
-    observed_point = np.asarray(measurement.point_common_xyz_m)
-    assert measurement.axis_point_source == "pose_trajectory_image_plane"
-    assert measurement.pose_axis_line_rms_m < 1.0e-6
-    assert np.linalg.norm(
-        np.cross(observed_point - expected_point, expected_axis)
-    ) < 1.0e-6
-
-
-def test_splay_zero_interpolates_when_scan_does_not_hit_command_127() -> None:
-    records, expected_axis, _ = _arbitrary_tag_records()
-    assert not any(record["command_u8"] == 127 for record in records)
-
-    curve = fit_rotation_joint_curve(records, zero_command_u8=127)
-    measurement = fit_joint_axis_measurement(
-        "index_mcp_roll", records, cycle=0, zero_command_u8=127
-    )
-
-    observed_axis = np.asarray(measurement.axis_common_xyz)
-    assert abs(float(observed_axis @ expected_axis)) > math.cos(
-        math.radians(0.05)
-    )
-    assert curve.angle_rad[127] == pytest.approx(0.0, abs=1.0e-9)
-
-
-def test_passive_axis_can_use_trusted_upstream_direction_constraint() -> None:
-    records, expected_axis, expected_point = _arbitrary_tag_records()
-    common_from_parent = Rotation.from_euler("xyz", [-0.8, 0.55, 1.3])
-    physical_axis_parent = common_from_parent.inv().apply(expected_axis)
-    wrong_axis_parent = np.cross(
-        physical_axis_parent, np.asarray([0.2, 0.8, -0.1])
-    )
-    wrong_axis_parent /= np.linalg.norm(wrong_axis_parent)
-    mount = Rotation.from_quat(records[0]["relative_quaternion_xyzw"])
-    contradictory = []
-    for record in records:
-        changed = dict(record)
-        angle = math.radians(62.0) * (
-            255.0 - float(record["command_u8"])
-        ) / 255.0
-        changed["relative_quaternion_xyzw"] = (
-            Rotation.from_rotvec(wrong_axis_parent * angle) * mount
-        ).as_quat().tolist()
-        contradictory.append(changed)
-
-    measurement = fit_joint_axis_measurement(
-        "index_dip",
-        contradictory,
-        cycle=0,
-        zero_command_u8=255,
-        axis_common_constraint=expected_axis,
-    )
-
-    observed_axis = np.asarray(measurement.axis_common_xyz)
-    observed_point = np.asarray(measurement.point_common_xyz_m)
-    assert abs(float(observed_axis @ expected_axis)) > math.cos(
-        math.radians(0.05)
-    )
-    assert np.linalg.norm(
-        np.cross(observed_point - expected_point, expected_axis)
-    ) < 1.0e-6
-
-
-def test_extrinsics_round_trip_keeps_camera_identity(tmp_path: Path) -> None:
-    cameras = {
-        view: {
-            "serial_number": f"SERIAL_{view}",
-            "width": 1624,
-            "height": 1240,
-            "intrinsics_sha256": camera_info_fingerprint(
-                width=1624,
-                height=1240,
-                camera_matrix=np.asarray(
-                    [[1100.0, 0.0, 812.0], [0.0, 1099.0, 620.0], [0.0, 0.0, 1.0]]
-                ),
-            ),
-        }
-        for view in ("front", "side", "top")
-    }
-    transforms = {"front": np.eye(4), "side": np.eye(4), "top": np.eye(4)}
-    transforms["side"][:3, :3] = Rotation.from_euler("y", 0.7).as_matrix()
-    transforms["side"][:3, 3] = [0.2, 0.0, 0.1]
-    transforms["top"][:3, :3] = Rotation.from_euler("x", -0.9).as_matrix()
-    transforms["top"][:3, 3] = [-0.1, 0.3, 0.2]
-    destination = tmp_path / "extrinsics.yaml"
-
-    dump_three_camera_extrinsics(
-        destination,
-        cameras=cameras,
-        front_from_view=transforms,
-        quality={
-            "passed": True,
-            "reprojection_rms_px": 0.3,
-            "maximum_rotation_repeatability_deg": 0.2,
-            "maximum_translation_repeatability_m": 0.001,
-            "front_side_captures": 15,
-            "front_top_captures": 15,
-        },
-    )
-    loaded = load_three_camera_extrinsics(destination)
-
-    assert loaded.cameras["front"].serial_number == "SERIAL_front"
-    assert np.allclose(loaded.transform("side"), transforms["side"])
-    assert np.allclose(loaded.transform("top"), transforms["top"])
-    assert loaded.camera_matches(
-        "front",
-        serial_number="SERIAL_front",
-        width=1624,
-        height=1240,
-        intrinsics_sha256=cameras["front"]["intrinsics_sha256"],
-    )
-    assert not loaded.camera_matches(
-        "front",
-        serial_number="WRONG_SERIAL",
-        width=1624,
-        height=1240,
-        intrinsics_sha256=cameras["front"]["intrinsics_sha256"],
-    )
-
-
-def _joint_origin(path: Path, name: str) -> tuple[np.ndarray, np.ndarray]:
-    joint = next(
-        element
-        for element in ET.parse(path).getroot().findall("joint")
-        if element.get("name") == name
-    )
-    origin = joint.find("origin")
-    axis = joint.find("axis")
-    xyz = np.asarray([float(value) for value in origin.get("xyz").split()])
-    rpy = np.asarray([float(value) for value in origin.get("rpy").split()])
-    axis_xyz = np.asarray([float(value) for value in axis.get("xyz").split()])
-    return np.block(
-        [
-            [Rotation.from_euler("xyz", rpy).as_matrix(), xyz[:, None]],
-            [np.asarray([[0.0, 0.0, 0.0, 1.0]])],
-        ]
-    ), axis_xyz / np.linalg.norm(axis_xyz)
-
-
-def _joint_limit(path: Path, name: str) -> tuple[float, float]:
-    joint = next(
-        element
-        for element in ET.parse(path).getroot().findall("joint")
-        if element.get("name") == name
-    )
-    limit = joint.find("limit")
-    return float(limit.get("lower")), float(limit.get("upper"))
-
-
-def test_urdf_writer_postmultiplies_joint_axis_and_never_overwrites(tmp_path: Path) -> None:
-    offset = math.radians(7.3)
-    destination = write_zero_corrected_urdf(
-        source_urdf=SOURCE_URDF,
-        output_directory=tmp_path,
-        serial_number="G20_LEFT_001",
-        offsets_rad={"thumb_cmc_yaw": offset},
-        timestamp="20260806_120000",
-    )
-    original, axis = _joint_origin(SOURCE_URDF, "thumb_cmc_yaw")
-    corrected, _ = _joint_origin(destination, "thumb_cmc_yaw")
-    expected = original.copy()
-    expected[:3, :3] = original[:3, :3] @ Rotation.from_rotvec(
-        axis * offset
-    ).as_matrix()
-
-    assert destination != SOURCE_URDF
-    assert np.allclose(corrected, expected, atol=1.0e-12)
-    with pytest.raises(ValueError, match="refusing to overwrite"):
-        write_zero_corrected_urdf(
-            source_urdf=SOURCE_URDF,
-            output_directory=tmp_path,
-            serial_number="G20_LEFT_001",
-            offsets_rad={"thumb_cmc_yaw": offset},
-            timestamp="20260806_120000",
-        )
-    with pytest.raises(ValueError, match="original CAD URDF"):
-        write_zero_corrected_urdf(
-            source_urdf=destination,
-            output_directory=tmp_path,
-            serial_number="G20_LEFT_001",
-            offsets_rad={"thumb_cmc_yaw": offset},
-            timestamp="20260806_120001",
-        )
-    with pytest.raises(ValueError, match="finite and within"):
-        write_zero_corrected_urdf(
-            source_urdf=SOURCE_URDF,
-            output_directory=tmp_path,
-            serial_number="G20_LEFT_001",
-            offsets_rad={"thumb_cmc_yaw": math.nan},
-            timestamp="20260806_120002",
-        )
-
-
-def test_urdf_writer_changes_only_the_16_active_zero_origins(
-    tmp_path: Path,
-) -> None:
-    before = SOURCE_URDF.read_bytes()
-    offsets = {
-        name: math.radians(0.25 * (index + 1))
-        for index, name in enumerate(ACTIVE_JOINTS)
-    }
-    destination = write_zero_corrected_urdf(
-        source_urdf=SOURCE_URDF,
-        output_directory=tmp_path,
-        serial_number="G20_LEFT_001",
-        offsets_rad=offsets,
-        timestamp="20260807_180000",
-    )
-
-    assert len(offsets) == 16
-    assert SOURCE_URDF.read_bytes() == before
-    for name in ACTIVE_JOINTS:
-        original, axis = _joint_origin(SOURCE_URDF, name)
-        corrected, corrected_axis = _joint_origin(destination, name)
-        expected = original.copy()
-        expected[:3, :3] = original[:3, :3] @ Rotation.from_rotvec(
-            axis * offsets[name]
-        ).as_matrix()
-        assert np.allclose(corrected, expected, atol=1.0e-12)
-        assert np.allclose(corrected_axis, axis, atol=1.0e-12)
-    for name in PASSIVE_JOINTS:
-        original, axis = _joint_origin(SOURCE_URDF, name)
-        corrected, corrected_axis = _joint_origin(destination, name)
-        assert np.allclose(corrected, original, atol=1.0e-12)
-        assert np.allclose(corrected_axis, axis, atol=1.0e-12)
-    # A zero calibration must not silently expand mechanical/CAD safety
-    # limits.  Dynamic measured ranges remain in the calibration JSON.
-    for name in (*ACTIVE_JOINTS, *PASSIVE_JOINTS):
-        assert _joint_limit(destination, name) == pytest.approx(
-            _joint_limit(SOURCE_URDF, name)
-        )
-
-
-def _synthetic_curve(zero_command: int, travel: float) -> JointCurveFit:
-    values = np.asarray(
-        [travel * (255.0 - command) / 255.0 for command in range(256)]
-    )
-    values -= values[zero_command]
-    data = tuple(float(value) for value in values)
-    return JointCurveFit(
-        angle_rad=data,
-        decreasing_rad=data,
-        increasing_rad=data,
-        circle={},
-        maximum_monotonic_correction_rad=0.0,
-        maximum_hysteresis_rad=0.0,
-        quality={},
-    )
-
-
-def _solve_synthetic_offsets(
-    side: str,
-    offset_degrees: list[float],
-    *,
-    inject_oblique_optical_depth_bias: bool = False,
-    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,
-    pose_axis_line_rms_by_joint_m: dict[str, float] | None = None,
-    joint_maximum_offset_degrees: dict[str, float] | None = None,
-):
-    hand = get_hand_calibration_profile(side)
-    zero = get_zero_calibration_profile(side)
-    source = SOURCE_URDF if side == "left" else RIGHT_SOURCE_URDF
-    baseline = [255.0] * 20
-    baseline[6:10] = [127.0] * 4
-    curves = {
-        name: _synthetic_curve(
-            int(baseline[hand.joint_specs[name].motor_index]),
-            math.radians(50.0),
-        )
-        for name in hand.measured_joints
-    }
-    if inject_secondary_root_axis_bias_degrees:
-        # Make the thumb root the higher-travel, directly observed direction,
-        # matching the real right-hand data where the short pinky splay arc is
-        # the less reliable root-axis orientation estimate.
-        curves["thumb_cmc_roll"] = _synthetic_curve(
-            int(baseline[hand.joint_specs["thumb_cmc_roll"].motor_index]),
-            math.radians(70.0),
-        )
-    motor_by_joint = {
-        name: spec.motor_index for name, spec in hand.joint_specs.items()
-    }
-    offsets = {
-        name: math.radians(value)
-        for name, value in zip(zero.direct_zero_joints, offset_degrees)
-    }
-    model = UrdfKinematicModel(source)
-    base_rotation = Rotation.from_euler("xyz", [0.5, -0.4, 0.8])
-    base_translation = np.asarray([0.31, -0.19, 0.72])
-    measurements: list[JointAxisMeasurement] = []
-    for cycle in range(3):
-        for joint in zero.axis_joints:
-            state = list(baseline)
-            if joint == "thumb_cmc_yaw":
-                state[5] = 145.0
-            angles = _angles_from_state(
-                state,
-                curves=curves,
-                motor_by_joint=motor_by_joint,
-                inherited_zero_joints=zero.inherited_zero_joints,
-            )
-            axis, point = model.axis_line(
-                joint, zero_offsets=offsets, joint_angles=angles
-            )
-            point_common = base_rotation.apply(point) + base_translation
-            if (
-                inject_secondary_root_point_bias_m
-                and joint == f"{zero.reference_finger}_mcp_roll"
-            ):
-                # A repeatable monocular depth error on the second parallel
-                # root line must affect translation only, never palm rotation
-                # or the inferred thumb-roll zero.
-                point_common = point_common + base_rotation.apply(
-                    np.asarray([0.0, 0.0, inject_secondary_root_point_bias_m])
-                )
-            view_normal_common = None
-            if (
-                inject_oblique_optical_depth_bias
-                and joint in zero.phase_parent_joint
-            ):
-                parent_axis = model.axis_line(
-                    zero.phase_parent_joint[joint],
-                    zero_offsets=offsets,
-                    joint_angles=angles,
-                )[0]
-                helper = (
-                    np.asarray([1.0, 0.0, 0.0])
-                    if abs(float(parent_axis[0])) < 0.8
-                    else np.asarray([0.0, 1.0, 0.0])
-                )
-                tilt_axis = np.cross(parent_axis, helper)
-                tilt_axis /= np.linalg.norm(tilt_axis)
-                view_normal = Rotation.from_rotvec(
-                    math.radians(15.0) * tilt_axis
-                ).apply(parent_axis)
-                view_normal_common = tuple(base_rotation.apply(view_normal))
-                # Simulate an independent planar-PnP depth error on the child
-                # Tag. It is large enough to drive the old 3-D phase solve to
-                # a configured offset bound.
-                point_common = point_common + 0.03 * np.asarray(
-                    view_normal_common
-                )
-            axis_common = base_rotation.apply(axis)
-            if (
-                inject_observer_cone_bias_degrees
-                and joint == "thumb_cmc_pitch"
-            ):
-                parent_axis = model.axis_line(
-                    zero.axis_parent_joint[joint],
-                    zero_offsets=offsets,
-                    joint_angles=angles,
-                )[0]
-                cone_normal = np.cross(axis, parent_axis)
-                cone_normal /= np.linalg.norm(cone_normal)
-                axis_common = base_rotation.apply(
-                    Rotation.from_rotvec(
-                        math.radians(inject_observer_cone_bias_degrees)
-                        * cone_normal
-                    ).apply(axis)
-                )
-            if (
-                inject_secondary_root_axis_bias_degrees
-                and joint == f"{zero.reference_finger}_mcp_roll"
-            ):
-                helper = np.asarray([0.0, 0.0, 1.0])
-                if abs(float(axis_common @ helper)) > 0.8:
-                    helper = np.asarray([0.0, 1.0, 0.0])
-                bias_axis = np.cross(axis_common, helper)
-                bias_axis /= np.linalg.norm(bias_axis)
-                axis_common = Rotation.from_rotvec(
-                    math.radians(inject_secondary_root_axis_bias_degrees)
-                    * bias_axis
-                ).apply(axis_common)
-            measurements.append(
-                JointAxisMeasurement(
-                    joint=joint,
-                    cycle=cycle,
-                    axis_common_xyz=tuple(axis_common),
-                    point_common_xyz_m=tuple(point_common),
-                    condition_state_u8=tuple(state),
-                    plane_rms_m=0.0002,
-                    radial_rms_m=0.0002,
-                    rotation_circle_axis_difference_rad=math.radians(0.1),
-                    view_normal_common_xyz=view_normal_common,
-                    pose_axis_line_rms_m=(
-                        pose_axis_line_rms_by_joint_m or {}
-                    ).get(joint, 0.0),
-                )
-            )
-    result = solve_urdf_zero_offsets(
-        source_urdf=source,
-        measurements=measurements,
-        curves=curves,
-        motor_by_joint=motor_by_joint,
-        hand_type=side,
-        joint_maximum_offset_rad={
-            name: math.radians(value)
-            for name, value in (joint_maximum_offset_degrees or {}).items()
-        },
-    )
-    return zero, result
-
-
-def test_small_stable_offsets_are_validated_without_rewriting_urdf_zero() -> None:
-    zero, result = _solve_synthetic_offsets("right", [0.1] * 7)
-    assert result.passed is True
-    static_policy = {
-        **zero.fixed_direct_zero_offsets_rad,
-        **zero.static_output_zero_offsets_rad,
-    }
-    for name, value in result.direct_offsets_rad.items():
-        assert value == pytest.approx(static_policy.get(name, 0.0))
-
-
-def test_profiles_do_not_contain_hard_coded_thumb_zero_offsets() -> None:
-    right = get_zero_calibration_profile("right")
-    left = get_zero_calibration_profile("left")
-
-    assert "thumb_cmc_roll" not in right.fixed_direct_zero_offsets_rad
-    assert "thumb_cmc_roll" not in right.static_output_zero_offsets_rad
-    assert "thumb_cmc_roll" not in left.fixed_direct_zero_offsets_rad
-    assert "thumb_cmc_roll" not in left.static_output_zero_offsets_rad
-
-
-def test_reference_finger_roll_static_zero_is_fixed_to_upright_cad() -> None:
-    zero, result = _solve_synthetic_offsets(
-        "right", [2.0, -2.0, 2.0, 1.0, 4.0, 1.0, 1.0]
-    )
-    reference_roll = f"{zero.reference_finger}_mcp_roll"
-    assert result.passed is True
-    assert math.degrees(result.direct_offsets_rad[reference_roll]) == pytest.approx(
-        0.0, abs=1.0e-12
-    )
-
-
-def test_biased_short_root_axis_does_not_tilt_entire_zero_solution() -> None:
-    zero, result = _solve_synthetic_offsets(
-        "right",
-        [2.0, -3.0, 4.0, 1.5, 0.0, -1.0, 2.0],
-        inject_secondary_root_axis_bias_degrees=15.0,
-    )
-
-    assert result.passed is True
-    expected = dict(
-        zip(zero.direct_zero_joints, [2.0, -3.0, 4.0, 1.5, 0.0, -1.0, 2.0])
-    )
-    expected.update(
-        {
-            name: math.degrees(value)
-            for name, value in {
-                **zero.fixed_direct_zero_offsets_rad,
-                **zero.static_output_zero_offsets_rad,
-            }.items()
-        }
-    )
-    for name, value in expected.items():
-        assert math.degrees(result.direct_offsets_rad[name]) == pytest.approx(
-            value, abs=0.05
-        )
-
-
-def test_root_line_depth_bias_does_not_change_thumb_roll_zero() -> None:
-    zero, result = _solve_synthetic_offsets(
-        "right",
-        [2.0, -3.0, 4.0, 1.5, 0.0, -1.0, 2.0],
-        inject_secondary_root_point_bias_m=0.02,
-    )
-
-    assert result.passed is True
-    assert math.degrees(
-        result.direct_offsets_rad["thumb_cmc_roll"]
-    ) == pytest.approx(2.0, abs=0.05)
-    assert result.direct_offsets_rad[f"{zero.reference_finger}_mcp_roll"] == 0.0
-
-
-def test_thumb_mcp_static_phase_bias_cannot_override_original_cad_zero() -> None:
-    offsets = [2.0, -3.0, 4.0, -40.0, 1.0, -1.0, 2.0]
-    _, result = _solve_synthetic_offsets(
-        "right",
-        offsets,
-        joint_maximum_offset_degrees={"thumb_mcp": 45.0},
-    )
-
-    assert result.passed is True
-    assert result.direct_offsets_rad["thumb_mcp"] == pytest.approx(0.0)
-    assert result.cycle_offsets_rad["thumb_mcp"] == pytest.approx((0.0, 0.0, 0.0))
-    assert "thumb_ip" not in result.validation_error_by_joint_rad
-    for name in ("thumb_cmc_roll", "thumb_cmc_yaw", "thumb_cmc_pitch"):
-        assert abs(math.degrees(result.direct_offsets_rad[name])) <= 20.0
-    for name in ("pinky_mcp_pitch", "pinky_pip"):
-        assert abs(math.degrees(result.direct_offsets_rad[name])) <= 3.0
-    assert result.direct_offsets_rad["pinky_mcp_roll"] == pytest.approx(0.0)
-
-
-def test_end_on_phase_rejects_oblique_monocular_depth_bias() -> None:
-    zero, result = _solve_synthetic_offsets(
-        "right",
-        [2.0, -3.0, 4.0, 1.5, 0.0, -1.0, 2.0],
-        inject_oblique_optical_depth_bias=True,
-    )
-
-    assert result.passed is True
-    expected = dict(
-        zip(zero.direct_zero_joints, [2.0, -3.0, 4.0, 1.5, 0.0, -1.0, 2.0])
-    )
-    expected.update(
-        {
-            name: math.degrees(value)
-            for name, value in {
-                **zero.fixed_direct_zero_offsets_rad,
-                **zero.static_output_zero_offsets_rad,
-            }.items()
-        }
-    )
-    for name, value in expected.items():
-        assert math.degrees(result.direct_offsets_rad[name]) == pytest.approx(
-            value, abs=0.05
-        )
-
-
-def test_zero_solver_rejects_axis_cone_geometry_that_a_zero_cannot_fix() -> None:
-    _, result = _solve_synthetic_offsets(
-        "right",
-        [2.0, -3.0, 4.0, 1.5, 0.0, -1.0, 2.0],
-        inject_observer_cone_bias_degrees=8.0,
-    )
-
-    assert result.passed is False
-    assert result.failure_reasons["thumb_cmc_yaw"] == (
-        "zero_axis_cone_mismatch_too_large"
-    )
-
-
-def test_zero_solver_rejects_unreliable_parallel_axis_line_phase() -> None:
-    _, result = _solve_synthetic_offsets(
-        "right",
-        [2.0, -3.0, 4.0, 1.5, 0.0, -1.0, 2.0],
-        pose_axis_line_rms_by_joint_m={"thumb_mcp": 0.002},
-    )
-
-    assert result.passed is False
-    assert result.failure_reasons["thumb_cmc_pitch"] == (
-        "zero_phase_axis_line_residual_too_large"
-    )
-
-
-def test_joint_chain_solver_recovers_offsets_and_yaw_uses_roll_145() -> None:
-    zero = get_zero_calibration_profile("left")
-    baseline = [255.0] * 20
-    baseline[6:10] = [127.0] * 4
-    curves = {
-        name: _synthetic_curve(
-            int(baseline[JOINT_SPECS[name].motor_index]),
-            math.radians(50.0),
-        )
-        for name in MEASURED_JOINTS
-    }
-    motor_by_joint = {
-        name: spec.motor_index for name, spec in JOINT_SPECS.items()
-    }
-    true_offsets = {
-        name: math.radians(value)
-        for name, value in zip(
-            DIRECT_ZERO_JOINTS, [2.0, -3.0, 4.0, 1.5, 0.0, -1.0, 2.0]
-        )
-    }
-    model = UrdfKinematicModel(SOURCE_URDF)
-    base_rotation = Rotation.from_euler("xyz", [0.5, -0.4, 0.8])
-    base_translation = np.asarray([0.31, -0.19, 0.72])
-    measurements = []
-    yaw_axis_without_clearance = None
-    yaw_axis_with_clearance = None
-    for cycle in range(3):
-        for joint in AXIS_JOINTS:
-            state = list(baseline)
-            if joint == "thumb_cmc_yaw":
-                state[5] = 145.0
-            angles = _angles_from_state(
-                state, curves=curves, motor_by_joint=motor_by_joint
-            )
-            axis, point = model.axis_line(
-                joint,
-                zero_offsets=true_offsets,
-                joint_angles=angles,
-            )
-            if joint == "thumb_cmc_yaw":
-                yaw_axis_with_clearance = axis.copy()
-                baseline_angles = _angles_from_state(
-                    baseline, curves=curves, motor_by_joint=motor_by_joint
-                )
-                yaw_axis_without_clearance = model.axis_line(
-                    joint,
-                    zero_offsets=true_offsets,
-                    joint_angles=baseline_angles,
-                )[0]
-            measurements.append(
-                JointAxisMeasurement(
-                    joint=joint,
-                    cycle=cycle,
-                    axis_common_xyz=tuple(base_rotation.apply(axis)),
-                    point_common_xyz_m=tuple(
-                        base_rotation.apply(point) + base_translation
-                    ),
-                    condition_state_u8=tuple(state),
-                    plane_rms_m=0.0002,
-                    radial_rms_m=0.0002,
-                    rotation_circle_axis_difference_rad=math.radians(0.1),
-                )
-            )
-
-    result = solve_urdf_zero_offsets(
-        source_urdf=SOURCE_URDF,
-        measurements=measurements,
-        curves=curves,
-        motor_by_joint=motor_by_joint,
-    )
-
-    assert math.degrees(
-        math.acos(
-            np.clip(yaw_axis_with_clearance @ yaw_axis_without_clearance, -1.0, 1.0)
-        )
-    ) > 1.0
-    assert result.passed is True
-    static_policy = {
-        **zero.fixed_direct_zero_offsets_rad,
-        **zero.static_output_zero_offsets_rad,
-    }
-    for name, expected in true_offsets.items():
-        if name in static_policy:
-            expected = static_policy[name]
-        assert result.direct_offsets_rad[name] == pytest.approx(
-            expected, abs=math.radians(0.05)
-        )
-    assert set(result.all_active_offsets_rad) == set(ACTIVE_JOINTS)
-    assert result.all_active_offsets_rad["thumb_mcp"] == pytest.approx(
-        0.0, abs=math.radians(0.05)
-    )
-    for target in INHERITED_ZERO_JOINTS:
-        assert result.all_active_offsets_rad[target] == pytest.approx(
-            0.0, abs=1.0e-12
-        )
-    assert result.offset_uncertainty_rad.keys() == result.direct_offsets_rad.keys()
-
-
-def test_right_solver_uses_pinky_and_phase_ignores_length_and_depth_bias() -> None:
-    hand = get_hand_calibration_profile("right")
-    zero = get_zero_calibration_profile("right")
-    baseline = [255.0] * 20
-    baseline[6:10] = [127.0] * 4
-    curves = {
-        name: _synthetic_curve(
-            int(baseline[hand.joint_specs[name].motor_index]),
-            math.radians(50.0),
-        )
-        for name in hand.measured_joints
-    }
-    motor_by_joint = {
-        name: spec.motor_index for name, spec in hand.joint_specs.items()
-    }
-    true_offsets = {
-        name: math.radians(value)
-        for name, value in zip(
-            zero.direct_zero_joints,
-            [2.0, -3.0, 4.0, 1.5, 0.0, -1.0, 2.0],
-        )
-    }
-    model = UrdfKinematicModel(RIGHT_SOURCE_URDF)
-    base_rotation = Rotation.from_euler("xyz", [0.5, -0.4, 0.8])
-    base_translation = np.asarray([0.31, -0.19, 0.72])
-    measurements: list[JointAxisMeasurement] = []
-    for cycle in range(3):
-        states: dict[str, list[float]] = {}
-        lines: dict[str, tuple[np.ndarray, np.ndarray]] = {}
-        for joint in zero.axis_joints:
-            state = list(baseline)
-            if joint == "thumb_cmc_yaw":
-                state[5] = 145.0
-            states[joint] = state
-            angles = _angles_from_state(
-                state,
-                curves=curves,
-                motor_by_joint=motor_by_joint,
-                inherited_zero_joints=zero.inherited_zero_joints,
-            )
-            lines[joint] = model.axis_line(
-                joint,
-                zero_offsets=true_offsets,
-                joint_angles=angles,
-            )
-        original_lines = {
-            name: (axis.copy(), point.copy())
-            for name, (axis, point) in lines.items()
-        }
-        for observer, parent in zero.phase_parent_joint.items():
-            parent_axis, parent_point = lines[parent]
-            original_parent_axis, original_parent_point = original_lines[parent]
-            child_axis, original_child_point = original_lines[observer]
-            radial = original_child_point - original_parent_point
-            radial -= original_parent_axis * float(
-                radial @ original_parent_axis
-            )
-            # Preserve angular phase while deliberately corrupting link radius
-            # and along-axis depth. These components must not move a zero.
-            lines[observer] = (
-                child_axis,
-                parent_point + 1.25 * radial + 0.02 * parent_axis,
-            )
-        for joint in zero.axis_joints:
-            axis, point = lines[joint]
-            measurements.append(
-                JointAxisMeasurement(
-                    joint=joint,
-                    cycle=cycle,
-                    axis_common_xyz=tuple(base_rotation.apply(axis)),
-                    point_common_xyz_m=tuple(
-                        base_rotation.apply(point) + base_translation
-                    ),
-                    condition_state_u8=tuple(states[joint]),
-                    plane_rms_m=0.0002,
-                    radial_rms_m=0.0002,
-                    rotation_circle_axis_difference_rad=math.radians(0.1),
-                )
-            )
-
-    result = solve_urdf_zero_offsets(
-        source_urdf=RIGHT_SOURCE_URDF,
-        measurements=measurements,
-        curves=curves,
-        motor_by_joint=motor_by_joint,
-        hand_type="right",
-    )
-
-    assert result.passed is True
-    static_policy = {
-        **zero.fixed_direct_zero_offsets_rad,
-        **zero.static_output_zero_offsets_rad,
-    }
-    for name, expected in true_offsets.items():
-        if name in static_policy:
-            expected = static_policy[name]
-        assert result.direct_offsets_rad[name] == pytest.approx(
-            expected, abs=math.radians(0.05)
-        )
-    for target in zero.inherited_zero_joints:
-        assert result.all_active_offsets_rad[target] == pytest.approx(
-            0.0, abs=1.0e-12
-        )
-    assert set(result.all_active_offsets_rad) == set(hand.active_joints)

src/gui_control/gui_control/config/constants.py

@@ -9,6 +9,20 @@ class HandConfig:
     joint_names_en: Optional[List[str]] = None
     init_pos: List[int] = field(default_factory=list)
     preset_actions: Optional[Dict[str, List[int]]] = None
+    preset_action_overrides: Optional[Dict[str, Dict[str, List[int]]]] = None
+    # Integer GUI values are divided by this scale before publishing.
+    # Legacy hands use raw u8 values (scale=1); O12 uses milliradians.
+    position_scale: int = 1
+    position_unit: str = "u8"
+
+    def get_preset_actions(self, hand_type: str) -> Dict[str, List[int]]:
+        """Return preset actions with hand-specific values applied."""
+        actions = dict(self.preset_actions or {})
+        if self.preset_action_overrides:
+            actions.update(
+                self.preset_action_overrides.get(hand_type.lower(), {})
+            )
+        return actions
 
 # ------------------------------------------------------------------
 # 常量字典（仅构建一次）
@@ -75,10 +89,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],
+            "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, 119, 149, 148, 105, 42, 109, 255, 255, 255, 255, 255, 225, 220, 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, 149, 148, 105, 42, 109, 255, 255, 255, 255, 255, 255, 255, 229, 254],
+            "拇指对无名指": [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, 49, 149, 148, 105, 42, 109, 255, 255, 255, 255, 255, 255, 255, 255, 215],
+            "拇指对小指": [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],
@@ -224,13 +238,20 @@ _HAND_CONFIGS: Dict[str, HandConfig] = {
             "叁": [92, 87, 255, 255, 255, 0],
             "肆": [92, 87, 255, 255, 255, 255],
             "伍": [255, 255, 255, 255, 255, 255],
-            "OK": [139, 91, 103, 250, 250, 250],
+            "OK": [95, 75, 116, 255, 255, 255],
+            "拇指对中指": [88, 2, 255, 114, 255, 255],
             "点赞": [250, 79, 0, 0, 0, 0],
             "握拳": [102, 18, 0, 0, 0, 0],
-        }
+        },
+        preset_action_overrides={
+            "right": {
+                "OK": [95, 83, 122, 255, 255, 255],
+                "拇指对中指": [95, 8, 255, 114, 255, 255],
+            },
+        },
     ),
     "L6": HandConfig(
-        joint_names_en=["thumb_cmc_pitch", "thumb_cmc_yaw", "index_mcp_pitch", "middle_mcp_pitch", "pinky_mcp_pitch", "ring_mcp_pitch"],
+        joint_names_en=["thumb_cmc_pitch", "thumb_cmc_roll", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch"],
         joint_names=["大拇指弯曲", "大拇指横摆", "食指弯曲", "中指弯曲", "无名指弯曲", "小拇指弯曲"],
         init_pos=[250] * 6,
         preset_actions={
@@ -258,5 +279,33 @@ _HAND_CONFIGS: Dict[str, HandConfig] = {
             "拇指压感准备2": [139, 18, 130, 0, 0, 0]
         }
     ),
+    "O12": HandConfig(
+        # O12 active-angle API order.  Keep this independent from the
+        # physical motor order exposed by some SDK metadata.
+        joint_names_en=[
+            "thumb_roll", "thumb_abad", "thumb_mcp", "thumb_pip",
+            "index_abad", "index_mcp", "index_pip",
+            "middle_abad", "middle_mcp", "middle_pip",
+            "ring_mcp", "pinky_mcp",
+        ],
+        joint_names=[
+            "拇指旋转", "拇指侧摆", "拇指根部", "拇指中部",
+            "食指侧摆", "食指根部", "食指中部",
+            "中指侧摆", "中指根部", "中指中部",
+            "无名指根部", "小指根部",
+        ],
+        # Values are milliradians so QSlider can retain 0.001 rad resolution.
+        init_pos=[0] * 12,
+        preset_actions={
+            "展开": [0] * 12,
+            "食指轻弯": [0, 0, 0, 0, 0, 300, 400, 0, 0, 0, 0, 0],
+            "半握": [
+                300, -200, -200, -500,
+                0, 650, 700, 0, 650, 800, 700, 700,
+            ],
+        },
+        position_scale=1000,
+        position_unit="rad",
+    ),
 }
 HAND_CONFIGS = MappingProxyType(_HAND_CONFIGS)

src/gui_control/gui_control/gui_control.py

@@ -1,5 +1,6 @@
 import sys
 import time, json
+import math
 import threading
 from dataclasses import dataclass
 from typing import List, Dict
@@ -33,6 +34,16 @@ _CANONICAL_COMMAND_NAMES = {
         "thumb_cmc_pitch", "thumb_cmc_yaw", "index_mcp_pitch",
         "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch",
     ],
+    "L6": [
+        "thumb_cmc_pitch", "thumb_cmc_roll", "index_mcp_pitch",
+        "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch",
+    ],
+    "O12": [
+        "thumb_roll", "thumb_abad", "thumb_mcp", "thumb_pip",
+        "index_abad", "index_mcp", "index_pip",
+        "middle_abad", "middle_mcp", "middle_pip",
+        "ring_mcp", "pinky_mcp",
+    ],
 }
 
 _CANONICAL_COMMAND_BOUNDS = {
@@ -43,17 +54,38 @@ _CANONICAL_COMMAND_BOUNDS = {
         *[(0, 255)] * 5,
     ],
     "O6": [(0, 255)] * 6,
+    "L6": [(0, 255)] * 6,
+}
+
+# Integer slider bounds in milliradians, derived from the O12 active-angle
+# limits.  Right and left thumb signs differ; all other limits are shared.
+_O12_COMMAND_BOUNDS_MRAD = {
+    "right": [
+        (0, 942), (-1387, 0), (-827, 0), (-1291, 0),
+        (-261, 261), (0, 1352), (0, 1530), (-261, 261),
+        (0, 1357), (0, 1815), (0, 1535), (0, 1535),
+    ],
+    "left": [
+        (-942, 0), (0, 1387), (-827, 0), (-1291, 0),
+        (-261, 261), (0, 1352), (0, 1530), (-261, 261),
+        (0, 1357), (0, 1815), (0, 1535), (0, 1535),
+    ],
 }
 
 
 class ROS2NodeManager(QObject):
     """ROS2节点管理器，处理ROS通信"""
     status_updated = pyqtSignal(str, str)  # 状态类型, 消息内容
+    feedback_updated = pyqtSignal(object)
 
     def __init__(self, node_name: str = "hand_control_node"):
         super().__init__()
         self.node = None
         self.publisher = None
+        self.publisher_arc = None
+        self.state_subscription = None
+        self.speed_pub = None
+        self.torque_pub = None
         self.joint_state = JointState()
         self.joint_state.header = Header()
         
@@ -83,14 +115,26 @@ class ROS2NodeManager(QObject):
                 self.node.get_parameter('topic_prefix').value
             )
             
-            if self.is_arc == True:
+            if self.hand_joint == "O12":
+                topic_base = f'/o12/{self.hand_type}'
+                self.publisher = self.node.create_publisher(
+                    JointState, f'{topic_base}/joint_cmd', 10
+                )
+                self.state_subscription = self.node.create_subscription(
+                    JointState,
+                    f'{topic_base}/joint_states',
+                    self._o12_feedback_callback,
+                    10,
+                )
+            elif self.is_arc == True:
                 # 创建发布者
                 self.publisher_arc = self.node.create_publisher(
                     JointState,
                     self.topic(f'/cb_{self.hand_type}_hand_control_cmd_arc'),
                     10
                 )
-            # 创建发布者
+            # 创建旧型号发布者
+            if self.hand_joint != "O12":
                 self.publisher = self.node.create_publisher(
                     JointState,
                     self.topic(f'/cb_{self.hand_type}_hand_control_cmd'),
@@ -100,6 +144,7 @@ class ROS2NodeManager(QObject):
                 JointState, self.topic('/calibration_pose_snapshot'), 10
             )
                     # 新增 speed / torque 发布者
+            if self.hand_joint != "O12":
                 self.speed_pub = self.node.create_publisher(
                     String, self.topic('/cb_hand_setting_cmd'), 10)
                 self.torque_pub = self.node.create_publisher(
@@ -136,15 +181,36 @@ class ROS2NodeManager(QObject):
         while rclpy.ok() and self.node:
             rclpy.spin_once(self.node, timeout_sec=0.1)
 
-    def bound_positions(self, positions: List[int]) -> List[int]:
+    def _o12_feedback_callback(self, msg: JointState):
+        """Forward 12-channel O12 radian feedback safely into the Qt thread."""
+        if len(msg.position) != 12:
+            self.status_updated.emit(
+                "error", f"O12位置反馈长度错误: {len(msg.position)}，应为12"
+            )
+            return
+        self.feedback_updated.emit([float(value) for value in msg.position])
+
+    def command_bounds(self, count: int):
+        if self.hand_joint == "O12":
+            bounds = _O12_COMMAND_BOUNDS_MRAD.get(self.hand_type)
+        else:
             bounds = _CANONICAL_COMMAND_BOUNDS.get(self.hand_joint)
-        if not bounds or len(bounds) != len(positions):
+        if not bounds or len(bounds) != count:
-            return list(positions)
+            return [(0, 255)] * count
+        return bounds
+
+    def bound_positions(self, positions: List[int]) -> List[int]:
+        bounds = self.command_bounds(len(positions))
         return [
             max(minimum, min(maximum, int(value)))
             for value, (minimum, maximum) in zip(positions, bounds)
         ]
 
+    def wire_positions(self, positions: List[int]) -> List[float]:
+        positions = self.bound_positions(positions)
+        scale = _HAND_CONFIGS[self.hand_joint].position_scale
+        return [float(value) / float(scale) for value in positions]
+
     def publish_joint_state(self, positions: List[int]):
         """发布关节状态消息"""
         if not self.publisher or not self.node:
@@ -153,15 +219,21 @@ class ROS2NodeManager(QObject):
             
         try:
             positions = self.bound_positions(positions)
+            wire_positions = self.wire_positions(positions)
             self.joint_state.header.stamp = self.node.get_clock().now().to_msg()
-            self.joint_state.position = [float(pos) for pos in positions]
+            self.joint_state.position = wire_positions
             # self.joint_state.velocity = [0.1] * len(positions)
             # self.joint_state.effort = [0.01] * len(positions)
             # 如果有关节名称，添加到消息中
             #hand_config = HandConfig.from_hand_type(self.hand_joint)
             hand_config = _HAND_CONFIGS[self.hand_joint]
             canonical_names = _CANONICAL_COMMAND_NAMES.get(self.hand_joint)
-            if canonical_names and len(canonical_names) == len(positions):
+            # O12 SDK 1.1.8 may expose names in physical motor order while the
+            # position array uses active-angle order.  An empty name array makes
+            # the documented position order unambiguous to the driver.
+            if self.hand_joint == "O12":
+                self.joint_state.name = []
+            elif canonical_names and len(canonical_names) == len(positions):
                 self.joint_state.name = canonical_names
             elif len(hand_config.joint_names) == len(positions):
                 if hand_config.joint_names_en != None:
@@ -170,7 +242,7 @@ class ROS2NodeManager(QObject):
                     self.joint_state.name = hand_config.joint_names
                 
             self.publisher.publish(self.joint_state)
-            if self.is_arc == True:
+            if self.is_arc == True and self.hand_joint != "O12":
                 if self.hand_joint == "O6":
                     if self.hand_type == "left":
                         pose = range_to_arc_left(positions,self.hand_joint)
@@ -205,14 +277,21 @@ class ROS2NodeManager(QObject):
         positions = self.bound_positions(positions)
         self.publish_joint_state(positions)
         self.joint_state.header.stamp = self.node.get_clock().now().to_msg()
-        self.joint_state.position = [float(pos) for pos in positions]
+        self.joint_state.position = self.wire_positions(positions)
         canonical_names = _CANONICAL_COMMAND_NAMES.get(self.hand_joint)
-        if canonical_names:
+        if canonical_names and self.hand_joint != "O12":
             self.joint_state.name = canonical_names
+        elif self.hand_joint == "O12":
+            self.joint_state.name = []
         self.snapshot_publisher.publish(self.joint_state)
         self.status_updated.emit("info", "当前标定姿势快照已发布")
 
     def publish_speed(self, val: int):
+        if self.hand_joint == "O12":
+            self.status_updated.emit(
+                "warning", "O12不使用旧版u8速度接口；请通过角度轨迹限制速度"
+            )
+            return
         if self.hand_joint.upper() in ("O6", "L6"):
             joint_len = 6
         elif self.hand_joint == "L7":
@@ -232,6 +311,11 @@ class ROS2NodeManager(QObject):
         self.speed_pub.publish(msg)
 
     def publish_torque(self, val: int):
+        if self.hand_joint == "O12":
+            self.status_updated.emit(
+                "warning", "O12不使用旧版u8扭矩接口；位置GUI已禁用该设置"
+            )
+            return
         if self.hand_joint.upper() in ("O6", "L6"):
             joint_len = 6
         elif self.hand_joint == "L7":
@@ -273,11 +357,17 @@ class HandControlGUI(QWidget):
         # 设置ROS管理器
         self.ros_manager = ros_manager
         self.ros_manager.status_updated.connect(self.update_status)
+        self.ros_manager.feedback_updated.connect(self.on_feedback_updated)
         
         # 获取手部配置
         self.hand_joint = self.ros_manager.hand_joint
         self.hand_type = self.ros_manager.hand_type
         self.hand_config = _HAND_CONFIGS[self.hand_joint]
+        self.preset_actions = self.hand_config.get_preset_actions(self.hand_type)
+        self.feedback_positions = None
+        # O12 only publishes after an explicit slider/preset action.  This
+        # prevents opening or closing the real hand merely by launching GUI.
+        self.command_dirty = self.hand_joint != "O12"
         
         # 初始化UI
         self.init_ui()
@@ -288,6 +378,20 @@ class HandControlGUI(QWidget):
         self.publish_timer.timeout.connect(self.publish_joint_state)
         self.publish_timer.start()
 
+    def format_position(self, value: int) -> str:
+        """Format an integer GUI value in the hand's actual command unit."""
+        scale = self.hand_config.position_scale
+        if self.hand_config.position_unit == "rad":
+            radians = float(value) / float(scale)
+            return f"{radians:.3f} rad / {math.degrees(radians):.1f}°"
+        return str(value)
+
+    def command_positions(self) -> List[float]:
+        """Return current slider values converted to wire units."""
+        return self.ros_manager.wire_positions(
+            [slider.value() for slider in self.sliders]
+        )
+
     def init_ui(self):
         """初始化用户界面"""
         # 设置窗口属性
@@ -465,13 +569,11 @@ class HandControlGUI(QWidget):
         for i, (name, value) in enumerate(zip(
             self.hand_config.joint_names, self.hand_config.init_pos
         )):
-            bounds = _CANONICAL_COMMAND_BOUNDS.get(self.hand_joint)
+            bounds = self.ros_manager.command_bounds(len(self.hand_config.init_pos))
-            minimum, maximum = (
+            minimum, maximum = bounds[i]
-                bounds[i] if bounds and i < len(bounds) else (0, 255)
-            )
             value = max(minimum, min(maximum, int(value)))
             # 创建标签
-            label = QLabel(f"{name}: {value}")
+            label = QLabel(f"{name}: {self.format_position(value)}")
             label.setMinimumWidth(120)
             
             # 创建滑动条
@@ -535,9 +637,9 @@ class HandControlGUI(QWidget):
     def create_system_preset_buttons(self, parent_layout):
         """创建系统预设动作按钮"""
         self.preset_buttons = []  # 清空按钮列表
-        if self.hand_config.preset_actions:
+        if self.preset_actions:
             buttons = []
-            for idx, (name, positions) in enumerate(self.hand_config.preset_actions.items()):
+            for idx, (name, positions) in enumerate(self.preset_actions.items()):
                 button = QPushButton(name)
                 button.setProperty("category", "preset")
                 button.clicked.connect(
@@ -564,6 +666,9 @@ class HandControlGUI(QWidget):
 
         # —— 2. 新增：速度与扭矩设置（每行一个）——
         quick_set_gb = QGroupBox("快速设置")
+        if self.hand_joint == "O12":
+            quick_set_gb.setEnabled(False)
+            quick_set_gb.setToolTip("O12不使用旧版0-255速度/扭矩设置接口")
         qv_layout = QVBoxLayout(quick_set_gb)
 
         # 速度行
@@ -692,7 +797,10 @@ class HandControlGUI(QWidget):
         """滑动条值改变事件处理"""
         if 0 <= index < len(self.slider_labels):
             joint_name = self.hand_config.joint_names[index]
-            self.slider_labels[index].setText(f"{joint_name}: {value}")
+            self.slider_labels[index].setText(
+                f"{joint_name}: {self.format_position(value)}"
+            )
+        self.command_dirty = True
             
         # 更新数值显示
         self.update_value_display()
@@ -700,10 +808,23 @@ class HandControlGUI(QWidget):
     def update_value_display(self):
         """更新数值显示面板内容"""
         # 获取所有滑动条的当前值
-        values = [slider.value() for slider in self.sliders]
+        command = self.command_positions()
+        command_text = ", ".join(f"{value:.3f}" for value in command)
+        lines = [f"目标({self.hand_config.position_unit}): [{command_text}]"]
+        if self.feedback_positions is not None:
+            feedback_text = ", ".join(
+                f"{value:.3f}" for value in self.feedback_positions
+            )
+            lines.append(f"反馈(rad): [{feedback_text}]")
+        self.value_display.setText("\n".join(lines))
 
-        # 格式化显示为列表形式
+    def on_feedback_updated(self, positions):
-        self.value_display.setText(f"{values}")
+        """Display actual O12 joint feedback without moving command sliders."""
+        self.feedback_positions = list(positions)
+        self.update_value_display()
+        if hasattr(self, "connection_status"):
+            self.connection_status.setText("O12右手已连接并收到位置反馈")
+            self.connection_status.setObjectName("StatusInfo")
 
     def on_preset_action_clicked(self, positions: List[int]):
         """预设动作按钮点击事件处理"""
@@ -742,7 +863,25 @@ class HandControlGUI(QWidget):
             self.cycle_button.setText("循环运行预设动作")
             self.reset_preset_buttons_color()
             
-        self.status_updated.emit("warning", "已停止所有动作")
+        if self.hand_joint == "O12" and self.feedback_positions is not None:
+            # Hold the latest measured pose instead of merely stopping topic
+            # publication while a previous position target is still active.
+            scale = self.hand_config.position_scale
+            hold_values = [
+                int(round(value * scale)) for value in self.feedback_positions
+            ]
+            hold_values = self.ros_manager.bound_positions(hold_values)
+            for slider, value in zip(self.sliders, hold_values):
+                slider.blockSignals(True)
+                slider.setValue(value)
+                slider.blockSignals(False)
+            self.command_dirty = True
+            self.publish_joint_state()
+            self.update_value_display()
+            self.status_updated.emit("warning", "已下发当前反馈位置并保持")
+        else:
+            self.command_dirty = False
+            self.status_updated.emit("warning", "已停止连续动作和新的位置发布")
 
     def on_save_pose_clicked(self):
         """发布当前滑块姿势，供hand_pose_capture写入profile。"""
@@ -751,7 +890,7 @@ class HandControlGUI(QWidget):
 
     def on_cycle_clicked(self):
         """循环运行预设动作按钮点击事件处理"""
-        if not self.hand_config.preset_actions:
+        if not self.preset_actions:
             QMessageBox.warning(self, "无预设动作", "当前手部型号没有预设动作可循环运行")
             return
             
@@ -774,19 +913,19 @@ class HandControlGUI(QWidget):
 
     def run_next_action(self):
         """运行下一个预设动作"""
-        if not self.hand_config.preset_actions:
+        if not self.preset_actions:
             return
             
         # 重置所有按钮颜色
         self.reset_preset_buttons_color()
         
         # 计算下一个动作索引
-        self.current_action_index = (self.current_action_index + 1) % len(self.hand_config.preset_actions)
+        self.current_action_index = (self.current_action_index + 1) % len(self.preset_actions)
         
         # 获取下一个动作
-        action_names = list(self.hand_config.preset_actions.keys())
+        action_names = list(self.preset_actions.keys())
         action_name = action_names[self.current_action_index]
-        action_positions = self.hand_config.preset_actions[action_name]
+        action_positions = self.preset_actions[action_name]
         
         # 执行动作
         self.on_preset_action_clicked(action_positions)
@@ -811,6 +950,7 @@ class HandControlGUI(QWidget):
         """关节类型改变事件处理"""
         self.hand_joint = joint_type
         self.hand_config = _HAND_CONFIGS[self.hand_joint]
+        self.preset_actions = self.hand_config.get_preset_actions(self.hand_type)
         
         # 更新手部信息
         info_text = f"""手部类型: {self.hand_type}
@@ -829,8 +969,11 @@ class HandControlGUI(QWidget):
 
     def publish_joint_state(self):
         """发布当前关节状态"""
+        if self.hand_joint == "O12" and not self.command_dirty:
+            return
         positions = [slider.value() for slider in self.sliders]
         self.ros_manager.publish_joint_state(positions)
+        self.command_dirty = False
 
     def update_status(self, status_type: str, message: str):
         """更新状态显示"""

src/gui_control/launch/o12_right_gui.launch.py

@@ -0,0 +1,42 @@
+#!/usr/bin/env python3
+"""Start the HCAN-connected O12 right hand and its safe position GUI."""
+
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription, TimerAction
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+    o12_driver = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            PathJoinSubstitution([
+                FindPackageShare("omnihand_node"),
+                "launch",
+                "omnihand_pro_2025_node.launch.py",
+            ])
+        )
+    )
+
+    o12_gui = Node(
+        package="gui_control",
+        executable="gui_control",
+        name="o12_right_gui_control",
+        output="screen",
+        parameters=[{
+            "hand_type": "right",
+            "hand_joint": "O12",
+            # Commands are coalesced, so dragging a slider sends at most 10 Hz.
+            "topic_hz": 10,
+            "is_arc": False,
+            "topic_prefix": "",
+        }],
+    )
+
+    return LaunchDescription([
+        o12_driver,
+        # Give the CANFD driver a moment to claim and initialise the adapter.
+        TimerAction(period=1.5, actions=[o12_gui]),
+    ])

src/gui_control/package.xml

@@ -13,6 +13,7 @@
   <test_depend>python3-pytest</test_depend>
 
   <exec_depend>linker_hand_ros2_sdk</exec_depend>
+  <exec_depend>omnihand_node</exec_depend>
 
   <export>
     <build_type>ament_python</build_type>

src/gui_control/test/test_l6_config.py

@@ -0,0 +1,12 @@
+from gui_control.config.constants import HAND_CONFIGS
+
+
+def test_l6_gui_uses_the_sdk_channel_order() -> None:
+    assert HAND_CONFIGS["L6"].joint_names_en == [
+        "thumb_cmc_pitch",
+        "thumb_cmc_roll",
+        "index_mcp_pitch",
+        "middle_mcp_pitch",
+        "ring_mcp_pitch",
+        "pinky_mcp_pitch",
+    ]

src/gui_control/test/test_o12_config.py

@@ -0,0 +1,16 @@
+from gui_control.config.constants import HAND_CONFIGS
+
+
+def test_o12_gui_uses_active_angle_order_and_milliradians() -> None:
+    config = HAND_CONFIGS["O12"]
+
+    assert config.joint_names_en == [
+        "thumb_roll", "thumb_abad", "thumb_mcp", "thumb_pip",
+        "index_abad", "index_mcp", "index_pip",
+        "middle_abad", "middle_mcp", "middle_pip",
+        "ring_mcp", "pinky_mcp",
+    ]
+    assert config.position_scale == 1000
+    assert config.position_unit == "rad"
+    assert len(config.init_pos) == 12
+    assert all(len(pose) == 12 for pose in config.preset_actions.values())

src/gui_control/test/test_o6_config.py

@@ -0,0 +1,18 @@
+from gui_control.config.constants import HAND_CONFIGS
+
+
+def test_o6_ok_preset_is_hand_specific() -> None:
+    config = HAND_CONFIGS["O6"]
+
+    assert config.get_preset_actions("left")["OK"] == [
+        95, 75, 116, 255, 255, 255,
+    ]
+    assert config.get_preset_actions("right")["OK"] == [
+        95, 83, 122, 255, 255, 255,
+    ]
+    assert config.get_preset_actions("left")["拇指对中指"] == [
+        88, 2, 255, 114, 255, 255,
+    ]
+    assert config.get_preset_actions("right")["拇指对中指"] == [
+        95, 8, 255, 114, 255, 255,
+    ]

src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l6_can.py

@@ -1,3 +1,5 @@
+from collections import deque
+
 import can
 import time, sys
 import threading
@@ -57,6 +59,14 @@ class LinkerHandL6Can:
         self.normal_force, self.tangential_force, self.tangential_force_dir, self.approach_inc = [[-1] * 6 for _ in range(4)]
         self.is_lock = False
         self.version = None
+        # L6 replies to a six-byte 0x01 position command with an immediate
+        # byte-for-byte echo on the same CAN ID. A zero-payload 0x01 state
+        # query also replies on that ID, but with the measured positions.
+        # Keep the two transactions distinct so command echoes never enter
+        # the published feedback stream used by calibration.
+        self._position_echo_lock = threading.Lock()
+        self._pending_position_echoes = deque(maxlen=32)
+        self._position_echo_timeout_seconds = 0.02
         # Start the receiving thread
         self.running = True
         self.receive_thread = threading.Thread(target=self.receive_response)
@@ -111,6 +121,11 @@ class LinkerHandL6Can:
         frame_property_value = int(frame_property.value) if hasattr(frame_property, 'value') else frame_property
         data = [frame_property_value] + [int(val) for val in data_list]
         msg = can.Message(arbitration_id=self.can_id, data=data, is_extended_id=False)
+        if frame_property_value == 0x01 and len(data_list) == 6:
+            with self._position_echo_lock:
+                self._pending_position_echoes.append(
+                    (time.monotonic(), tuple(int(value) for value in data_list))
+                )
         try:
             self.bus.send(msg)
         except can.CanError as e:
@@ -201,7 +216,23 @@ class LinkerHandL6Can:
             except:
                 return
             if frame_type == 0x01:   # 0x01
-                self.x01 = list(response_data)
+                response = tuple(int(value) for value in response_data)
+                now = time.monotonic()
+                is_position_echo = False
+                with self._position_echo_lock:
+                    while (
+                        self._pending_position_echoes
+                        and now - self._pending_position_echoes[0][0]
+                        > self._position_echo_timeout_seconds
+                    ):
+                        self._pending_position_echoes.popleft()
+                    for pending in tuple(self._pending_position_echoes):
+                        if pending[1] == response:
+                            self._pending_position_echoes.remove(pending)
+                            is_position_echo = True
+                            break
+                if not is_position_echo:
+                    self.x01 = list(response)
             elif frame_type == 0x02:    # 0x02
                 self.x02 = list(response_data)
             elif frame_type == 0x05: # Set speed
@@ -391,7 +422,10 @@ class LinkerHandL6Can:
         return self.x35
     
     def get_finger_order(self):
-        return ["thumb_cmc_pitch", "thumb_cmc_yaw", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch"]
+        # L6 channel 1 is the physical CMC roll actuator.  Older SDK releases
+        # exposed the channel as ``thumb_cmc_yaw`` even though the wire order
+        # and mechanism have always been roll.
+        return ["thumb_cmc_pitch", "thumb_cmc_roll", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch"]
 
     def show_fun_table(self):
         pass

src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/rs485/linker_hand_l6_rs485.py

@@ -372,7 +372,7 @@ class LinkerHandL6RS485:
         return [0] * 6
 
     def get_finger_order(self):
-        return ["thumb_cmc_pitch", "thumb_cmc_yaw", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch"]
+        return ["thumb_cmc_pitch", "thumb_cmc_roll", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch"]
 
     # --------------------------------------------------
     # 便捷方法

src/linker_hand_ros2_sdk/test/test_l6_channel_order.py

@@ -0,0 +1,72 @@
+import ast
+from collections import deque
+from pathlib import Path
+import sys
+import threading
+import time
+from types import SimpleNamespace
+
+
+PACKAGE = Path(__file__).resolve().parents[1] / "linker_hand_ros2_sdk/LinkerHand/core"
+EXPECTED = [
+    "thumb_cmc_pitch",
+    "thumb_cmc_roll",
+    "index_mcp_pitch",
+    "middle_mcp_pitch",
+    "ring_mcp_pitch",
+    "pinky_mcp_pitch",
+]
+
+
+def _finger_order(path: Path, class_name: str) -> list[str]:
+    module = ast.parse(path.read_text(encoding="utf-8"))
+    selected = next(
+        item
+        for item in module.body
+        if isinstance(item, ast.ClassDef) and item.name == class_name
+    )
+    method = next(
+        item
+        for item in selected.body
+        if isinstance(item, ast.FunctionDef) and item.name == "get_finger_order"
+    )
+    returned = next(item for item in method.body if isinstance(item, ast.Return))
+    return ast.literal_eval(returned.value)
+
+
+def test_l6_can_and_rs485_publish_the_same_physical_channel_order() -> None:
+    assert _finger_order(PACKAGE / "can/linker_hand_l6_can.py", "LinkerHandL6Can") == EXPECTED
+    assert _finger_order(
+        PACKAGE / "rs485/linker_hand_l6_rs485.py", "LinkerHandL6RS485"
+    ) == EXPECTED
+
+
+def test_l6_can_position_echo_does_not_replace_measured_feedback() -> None:
+    linker_hand_root = PACKAGE.parent
+    sys.path.insert(0, str(linker_hand_root))
+    try:
+        from core.can.linker_hand_l6_can import LinkerHandL6Can
+    finally:
+        sys.path.remove(str(linker_hand_root))
+
+    hand = LinkerHandL6Can.__new__(LinkerHandL6Can)
+    hand.can_id = 0x27
+    hand.x01 = [10, 20, 30, 40, 50, 60]
+    hand._position_echo_lock = threading.Lock()
+    command = (255, 2, 253, 253, 253, 253)
+    hand._pending_position_echoes = deque(
+        [(time.monotonic(), command)], maxlen=32
+    )
+    hand._position_echo_timeout_seconds = 0.02
+
+    hand.process_response(
+        SimpleNamespace(arbitration_id=0x27, data=bytes((0x01, *command)))
+    )
+    assert hand.x01 == [10, 20, 30, 40, 50, 60]
+    assert not hand._pending_position_echoes
+
+    measured = (250, 3, 252, 252, 252, 252)
+    hand.process_response(
+        SimpleNamespace(arbitration_id=0x27, data=bytes((0x01, *measured)))
+    )
+    assert hand.x01 == list(measured)

src/linkerhand_calibration/README.md

@@ -1,4 +1,425 @@
-# G20 左右手 AprilTag 标定
+# LinkerHand 专业标定包
+
+## O12 右手 16-Tag 完整标定
+
+O12 使用 vendor `omnihand_pro_2025_node` 的标准弧度接口，固定订阅
+`/o12/right/joint_states`、发布 `/o12/right/joint_cmd`。标定程序不会使用
+0–2000 混合控制接口，也不会按可能错位的 `JointState.name[]` 重排数据；12 路
+`position[]` 顺序及 `thumb_roll → thumb_cmc_roll`、
+`thumb_abad → thumb_cmc_yaw` 映射由产品 Profile 固定。
+
+把 [o12_right_product.yaml](config/o12_right_product.yaml) 中的
+`serial_number` 改成实物串号后，只需运行：
+
+```bash
+ros2 run linkerhand_calibration calibrate_hand --config \
+  src/linkerhand_calibration/config/o12_right_product.yaml
+```
+
+runner 会自动加载仓库内 vendor Jazzy overlay，启动 HCAN device 0/channel 0
+节点、三相机、AprilTag 与标定节点；READY 后自动开始。启动前会确认 12 路
+POSITION 模式、错误码、温度和反馈，并以不超过 3° 的低速点动执行固定通道预检。
+正式扫描以 20 Hz 发布端点速度为零的弧度余弦轨迹；中止、堵转或质量失败时保持
+当前位置。结果发布到 `calibration_output/<O12串号>/latest_passed`，其中 JSON
+为 schema v7 弧度 knots，不生成 256 点 u8 表。
+
+只验证配置、16 张 16 mm Tag、相机/外参、源 URDF 和 SDK 配置哈希而不运动：
+
+```bash
+ros2 run linkerhand_calibration calibrate_hand --config \
+  src/linkerhand_calibration/config/o12_right_product.yaml --validate-only
+```
+
+## O6 右手局部标定（o6_right_8/v1）
+
+O6 使用与 L6 相同的三机位八 Tag 观测拓扑，但保留 O6 自己的六通道协议与
+URDF 关节名：正面 ID0/1/2 标定 `rh_thumb_cmc_pitch` 与
+`rh_thumb_ip`，侧面 ID3/4/5 标定 `rh_pinky_mcp_pitch` 与
+`rh_pinky_dip`，上面 ID6/7 标定 `rh_thumb_cmc_yaw`。六路 baseline 均为
+`255`；每次只扫描通道 0、1 或 5，其他通道保持 255。经硬件确认，食指、中指、
+无名指与小指同机构，因此小指 MCP/DIP 的实测结果会以迁移来源标记后用于其余
+三指。O6 实测 IP/DIP 均存在稳定的非线性，因此使用通过独立 holdout 的双向
+运行曲线和二次耦合模型。标准 URDF 无法表示二次 mimic，因此修正 URDF 与 L6
+一样保留端点对齐的线性 `<mimic>`，使普通 URDF/RViz 中五个被动关节能正常联动，
+同时把被动关节 limit 更新为实测范围；中间行程的精确双向非线性轨迹由下述标定桥
+发布。
+
+```bash
+ros2 run linkerhand_calibration calibrate_hand --config \
+  src/linkerhand_calibration/config/o6_right_product.yaml --validate-only
+
+ros2 run linkerhand_calibration calibrate_hand --config \
+  src/linkerhand_calibration/config/o6_right_product.yaml
+```
+
+由于 O6 的 FRONT/TOP 光轴接近正交，同一平面棋盘的同步视角天然更倾斜。O6 外参
+允许最终批次 RMS 不超过 1.5 px，但仍保持 0.3° 旋转、1.5 mm 平移稳定性门限，
+并在标定发布前额外用 20 mm 跨机位实体轴线 RMS 粗差门限拦截移动相机等明显错误：
+
+```bash
+ros2 launch linkerhand_calibration three_camera_extrinsics.launch.py \
+  output_file:=$PWD/config/o6_three_camera_extrinsics.yaml \
+  checkerboard_columns:=8 checkerboard_rows:=5 square_size_m:=0.027 \
+  maximum_reprojection_rms_px:=1.5
+```
+
+结果发布到 `calibration_output/<O6串号>/latest_partial_passed`，运行时 JSON、
+修正 URDF 与 correction-input JSON 均使用 `o6_right_` 前缀。产品 YAML 内的实物
+串号、相机身份、外参和四项输入哈希必须在启动硬件前通过校验。
+
+标定完成后，以修正 URDF 启动 robot state publisher，并用同一会话中的 JSON 把
+O6 六路反馈转换为 11 个 URDF 关节：
+
+```bash
+ros2 launch linkerhand_calibration calibrated_joint_state_bridge.launch.py \
+  hand_type:=right \
+  calibration_file:=$PWD/calibration_output/O6_RIGHT_001/latest_partial_passed/o6_right_O6_RIGHT_001_partial_calibration.json
+```
+
+## L6 右手局部标定（l6_right_8/v1）
+
+本版只发布 `rh_thumb_cmc_pitch`、`rh_thumb_cmc_roll`、
+`rh_pinky_mcp_pitch` 的静态零位与动态曲线，以及 `rh_thumb_dip`、
+`rh_pinky_dip` 的视觉动态曲线。拇指 DIP 通过线性 mimic；小指 DIP 使用实测
+双方向运行曲线和 MuJoCo 二次 equality，因为它的传动比会随屈曲角变化。生成的
+修正 URDF 保留 `rh_pinky_dip` 的 `<mimic>`，因此在普通 URDF/RViz 中仍会跟随
+MCP 运动；该线性回退精确对齐实测零位和闭合端点。中间行程的准确非线性轨迹由
+MuJoCo equality 或下述标定桥提供。根据 L6 四指同机构的实机确认，食指、中指、
+无名指的 MCP 零偏、行程、双向反馈曲线及 DIP 耦合从小指迁移；每指自己的
+`origin.xyz`、axis、mesh 和惯量保持 CAD，不把迁移结果标成 Tag 实测。四指 MCP
+以反馈 255 的展开端作为 CAD lower/zero 锚点；实测行程不会再被强制压回较短的
+CAD upper，因此不会向四指 origin 写入系统性的负零偏。
+结果指针为 `latest_partial_passed`，不会被当作六路主动关节的完整标定。
+
+拇指 pitch/roll 根据两组已记录六路反馈值的实机/仿真姿态对比，以反馈 255
+保持源 CAD joint zero，不叠加端点推断的静态偏置；小指及三指迁移仍以反馈 0
+机械闭合姿态对齐源 CAD upper。该策略只改变坐标锚点，不改变视觉实测的双方向
+行程曲线。现场姿态对比必须同时记录对应的六路反馈值。
+
+先启动 SDK 和 GUI 做手动检查时使用：
+
+```bash
+ros2 run linker_hand_ros2_sdk linker_hand_sdk --ros-args \
+  -p hand_type:=right -p hand_joint:=L6 -p can:=can0 -p topic_prefix:=/l6
+
+ros2 run gui_control gui_control
+```
+
+正式一键标定由 runner 自己启动 SDK、三相机、AprilTag 和标定节点，不要同时
+运行上面的 SDK/GUI 控制命令：
+
+```bash
+ros2 run linkerhand_calibration calibrate_hand --config \
+  src/linkerhand_calibration/config/l6_right_product.yaml
+```
+
+只检查 Profile、8 张 16 mm Tag、相机/外参哈希和只读源 URDF：
+
+```bash
+ros2 run linkerhand_calibration calibrate_hand --config \
+  src/linkerhand_calibration/config/l6_right_product.yaml --validate-only
+```
+
+离线回放与在线发布使用同一拟合/URDF写回路径：
+
+```bash
+ros2 run linkerhand_calibration calibrate_hand --config \
+  src/linkerhand_calibration/config/l6_right_product.yaml \
+  --offline-raw calibration_output/L6_RIGHT_001/<时间戳>/raw_samples.jsonl
+```
+
+拟合通过后，程序先原子写入并重新校验
+`l6_right_<序列号>_urdf_correction_input.json`，验证串号、Profile 和源 URDF
+SHA-256 后，才把其中的原精度零偏、行程和 DIP 耦合参数交给现有 URDF 写回器。
+面向运行时的 schema-v6 `*_partial_calibration.json` 及修正 URDF 的字段、数值和
+格式保持兼容；单独的 correction-input JSON 是可审计的 URDF 生成依据，不是运行桥
+的输入文件。
+
+运行前将产品 YAML 中的 `serial_number` 改成实物串号。通道顺序固定为 pitch、
+roll、index、middle、ring、pinky；旧 SDK 反馈中的 `thumb_cmc_yaw` 仅作为第二路
+兼容别名读取，新产物和运行桥始终输出 `thumb_cmc_roll` / `rh_*` URDF 名。
+预检和正式扫描均使用 L6 硬件速度 `1` 作为上限，并由 100 Hz 余弦缓入缓出
+轨迹把完整 `255↔0` 行程固定为 `6 s`；短行程按距离同比缩短。SDK 会过滤 L6
+在同一 CAN ID 回送的位置命令回显，标定只使用状态查询返回的真实反馈。标定
+启动时还会检查重复 SDK/GUI 发布者，避免两个进程同时访问同一只手。
+
+标定完成后，用生成的 JSON 将六路硬件反馈转换成包括小指 DIP 在内的 11 关节
+`JointState`：
+
+```bash
+ros2 launch linkerhand_calibration calibrated_joint_state_bridge.launch.py \
+  hand_type:=right \
+  calibration_file:=$PWD/calibration_output/L6_RIGHT_001/latest_partial_passed/l6_right_L6_RIGHT_001_partial_calibration.json
+```
+
+schema v6 默认订阅 `/l6/cb_right_hand_state`，发布
+`/sim/mujoco/l6/right/joint_state`。标准 URDF 的 `<mimic>` 本身只支持线性关系，
+所以只查看 URDF 时小指 DIP 中间行程是端点对齐的近似；需要实测轨迹时使用该桥
+或修正 URDF 内的 MuJoCo equality。
+
+## G20右手正式一键标定
+
+固定三相机和19张Tag安装完成后，用户只运行：
+
+```bash
+ros2 run linkerhand_calibration calibrate_hand
+```
+
+旧 executable `calibrate_g20_right` 在本发行版内保留为同一入口的别名；
+旧 ROS 包名前缀不再提供。新脚本和部署配置统一使用 `calibrate_hand`。
+
+构建和正式调用统一为：
+
+```bash
+colcon build --packages-select linkerhand_calibration
+ros2 run linkerhand_calibration calibrate_hand --config <产品配置.yaml>
+```
+
+## 代码边界
+
+- `core/`：无 ROS、无具体型号，包含领域类型、PnP/旋转数学、拟合接口、统一样本
+  契约、`TaskEvaluator/SessionSolver` 协议和 `UrdfCorrectionPlan`。其中
+  `core/urdf/patch.py` 是所有型号共用的声明式 URDF patch engine，统一负责属性级
+  文本修改、MuJoCo equality、mesh 安全复制、禁止覆盖和原子发布。
+- `runtime/`：通用会话状态机与注册 Profile 分发；ROS 消息和硬件适配只能位于
+  `runtime/nodes`、`runtime/adapters`。
+- `models/g20/`、`models/l6/`：只保留型号 Profile、拟合/零位策略以及把拟合结果
+  转成 `UrdfPatchSet` 的薄适配层，不再各自实现 XML/mesh 文件写入器。新增 O6 时
+  优先新增 Profile；只有测量链或传动模型不同的部分才增加小型拟合插件。
+  后续型号或左右手作为新的独立 Profile 加入 `models/`，不在通用层增加分支。
+- `compat/`：v1 配置、旧路径、旧会话与旧单相机逻辑。旧 Python 包名仅保留
+  一版最小转发 shim，不包含算法副本。
+
+产品配置在启动硬件前通过本地 `ProfileRegistry` 完成命令索引、任务、视角、
+Tag、零位目标、URDF关节和文件哈希校验。v1 配置原文不改；v2 配置使用
+`profile_id: MODEL/side/layout/vREVISION`。视角名和数量由 Profile 声明，通用层
+不要求 `front/side/top`，也不假设固定 20 个命令。
+
+仓库中存在已审定硬件会话时，可执行只读金标准检查（不会覆盖任何产物）：
+
+```bash
+ros2 run linkerhand_calibration validate_g20_goldens \
+  calibration_output/G20_RIGHT_001
+```
+
+它严格核对完整整手、合并拇指、独立拇指、拟合失败和零位失败五个会话；完整
+整手还会重新离线求解并要求 JSON、URDF 的 SHA-256 与正式产物一致。
+
+完全独立地只标定大拇指4项任务时，使用：
+
+```bash
+ros2 run linkerhand_calibration calibrate_hand \
+  --scope thumb
+```
+
+该模式不读取任何已标定四指数据。拇指零位求解只使用5条拇指轴、两条顶部同相机
+方向观测以及拇指自己的机械端点；输出URDF从原始CAD生成，只修改4个拇指主动
+关节，12个四指关节保持CAD零位。独立结果发布到`latest_thumb_passed`，其JSON是
+拇指标定/诊断产物，不冒充可直接运行的完整整手曲线JSON。
+
+如果确实需要把新的拇指结果合并到一份已经通过的完整整手标定，才额外使用：
+
+```bash
+ros2 run linkerhand_calibration calibrate_hand \
+  --scope thumb \
+  --base-session calibration_output/G20_RIGHT_001/latest_passed
+```
+
+合并模式会冻结基础会话中的12个四指主动零位；发布前再次读取基础会话JSON核对，
+任何四指零位变化都会拒绝发布。基础四指数据仍不参与4个拇指零位的数值求解。
+
+拇指专项结果重复性通过后，不必再做原来的16项整手扫描。以该拇指会话为基础，
+只重新采集12项四指任务并合成完整整手URDF：
+
+```bash
+ros2 run linkerhand_calibration calibrate_hand \
+  --scope fingers \
+  --base-session calibration_output/G20_RIGHT_001/<已通过的拇指会话时间戳>
+```
+
+`fingers`模式严格冻结基础会话中的4个拇指主动零位；最终完整整手URDF中的拇指
+零位与专项会话schema-v4数值完全一致。默认`full`也先调用与`thumb`完全相同的
+独立拇指内核，再冻结这4个结果求解12个四指零位，因此四指数据不能反向改写
+拇指结果。原来的默认`full`仍保留，用于需要16项全部重新采集的情况。
+
+采样文件中的运动域是显式且不可混用的：
+`requested_command_u8` 表示下发命令，`feedback_u8` 表示电机反馈。
+在线拟合和离线重放通过同一个数据契约投影到曲线索引；新会话不会把含糊的
+`command_u8` 写入 `raw_samples.jsonl`。基础会话导入期间状态会显示为
+`IMPORTING_BASE` 和 `REVALIDATING_INHERITED`，完成复核后才允许机械手运动。
+
+提供`--base-session`时，它必须解析到同一序列号目录下的完整PASS会话；启动前会校验源CAD
+URDF、相机外参和标定配置哈希。新会话从原始CAD重新生成完整URDF，不在旧校准
+URDF上叠加。两种thumb模式都只重采`thumb_cmc_pitch`、`thumb_cmc_roll`、
+`thumb_cmc_yaw`、`thumb_mcp/thumb_ip`四项物理任务，其余任务的原始记录导入后仍按
+数据契约和产物哈希检查，但不会以历史四指拟合结果否决本次拇指专项标定。
+
+开发阶段若上一次会话失败，同一命令会自动校验硬件/几何哈希，并恢复已经
+完整提交的关节任务；失败中的当前任务始终丢弃重做，位于它后面但已经完整通过的
+独立任务仍会复用，不再因“连续前缀”限制整段重采。导入的任务会立即用与
+最终验收相同的硬门限复检（不含视口实时有效率）：只以预警带余量通过的旧数据
+当场剔除并从其在扫掠顺序中的原始位置重采，避免全部任务采完后才在最终验收
+失败、把会话拉回靠前的关节。方向级自动重扫事件是追加日志中的持久失效标记；
+恢复时只读取该标记之后的替代采集，不能把同一尝试编号下重扫前后的稳态点合并。
+因此已经在线硬门限验收的任务保持已完成，暂停中的任务从任务开头重采，不会因
+日志中仍保留被自动重扫淘汰的旧点而倒退到更早任务。运行中的多视角任务按正面主测量和侧面校验测量
+独立保留；单轮转轴异常且其余三轮形成一致簇时只补扫异常轮的两个方向。侧面
+轴线位置若也能明确定位为单轮异常，同样只补扫该轮；补扫会保留任务预检和前次
+采集确定的PnP分支参考，不会因重新初始化切换到另一组平面Tag镜像解。侧面
+校验视角的任务级有效率只记录为诊断；G20右手预检若逐帧识别率低于标称值，
+但同步有效位姿已经完整覆盖端点、中点、最小分箱数和最大分箱空洞，也按完整
+轨迹通过。正式扫描仍逐方向执行相同的硬分箱覆盖检查，轴线、曲线和模型质量
+门限保持不变。每个任务的低速往返预检、首轮交接和四轮双向正式扫描属于同一
+采集事务：相邻方向共享已验证端点和任务级PnP参考。G20右手正式扫描固定使用
+产品审定速度，不再根据单次识别密度自动提速，确保不同会话测量的是同一动态
+过程。顶部单目`thumb_cmc_yaw`在最终求解后另做零偏轮次重复性检查：前三轮
+极差默认不得超过0.5°，95%置信半宽不得超过0.75°。若两轮形成不超过门限20%的
+紧密簇、仅另一轮越界，自动补采该轮两个方向；无法明确定位时只重采完整yaw任务，
+不会回退重采整手。与`latest_passed`中上一正式结果相差超过0.75°时另写入
+`thumb_yaw_cross_session_diagnostic`提示检查机械手位置和Tag安装，但该历史差值
+不直接否决当前会话，也不会用旧结果约束新零位。需要强制从第一个关节
+重新采集时使用 `--no-resume`。升级前已经分别完成的正面/侧面roll也会合并为
+一个完整同步任务断点；只有两边数据都完整时才复用。
+
+命令自动完成产品哈希预检、运动、当前任务补扫、前三轮训练、第四轮隔离留出、
+16个会话数据求解主动关节URDF零位修正、
+21条视觉实测命令曲线发布；四指PIP/DIP的动态曲线均实测，四指DIP静态零位保留CAD。
+终端只显示中文进度和问题；失败时复制“请复制以下内容给开发者”块即可。
+
+四指末端的16 mm Tag允许使用刚性延长杆避挡；软件不假设末端Tag平面与中节Tag
+平面平行。延长杆和Tag在一次标定期间必须完全刚性，不能晃动、扭转或重新调整。
+侧面掌部基准Tag（ID 4）与各活动指节Tag也不要求安装面平行：首次联合PnP使用
+静态多帧刚性、重投影误差和跨轮任务参考选择分支，不再用固定15°安装角门限阻断扫描。
+单Tag独立位姿仍保留75°倾角保护；对包含锁定掌部基准和完整父子链的任务，倾角保护只
+限制独立选择，不会在联合选择前删除正深度、低重投影的IPPE候选。联合跟踪继续用相邻帧
+绝对/相对位姿连续性约束这些斜视候选，最终轴线残差、四轮重复性和隔离留出门限不放宽。
+终端中的Tag计数表示“可见”；等待扫描起点时会另列PnP初始化进度和累计拒绝原因。
+若联合候选仍然失败，`raw_samples.jsonl` 会按8个反馈计数的区间保存
+`group_pnp_candidate_event`，其中包含缺失角色、逐Tag候选数/倾角/重投影、角点和相机内参
+哈希，可直接定位运动到哪个机械位置后开始失效，而不需要再次盲扫整条流程。
+
+正式结果位于 `calibration_output/G20_RIGHT_001/latest_passed`。该指针只在
+JSON、URDF数值等价、mesh完整性、21条曲线CAD限位、被动关节保护和隔离留出验证
+全部通过后更新。
+
+## G20右手19-Tag底层调试入口
+
+以下内容仅保留给旧会话回放和开发调试；正式一键命令只发布上面的精简
+schema v4 JSON，不再生成schema v5运行文件。
+
+新布局用独立参数启用，原有左右手11-Tag流程仍默认使用
+`tag_layout:=legacy_11`，两套配置和结果schema互不覆盖：
+
+```bash
+ros2 launch linkerhand_calibration three_camera_calibration.launch.py \
+  hand_type:=right \
+  tag_layout:=g20_right_19 \
+  serial_number:=G20_RIGHT_001 \
+  camera_extrinsics_file:=/home/lxp/projects/linkerhand_retarget_ros2/config/g20_three_camera_extrinsics.yaml \
+  source_urdf_expected_sha256:=<CAD负责人确认的G20右手源URDF_SHA256>
+```
+
+当前产品布局共19张`tag36h11`，所有Tag的黑色码区边长均为16 mm；四指末节ID为
+`7,14,16,18`。正面为`0,1,2,3,10,11,12,13`，侧面为
+`4,5,6,7,14,15,16,17,18`，上面为`8,9`。详细角色和逐ID尺寸以
+`config/three_camera_tags_g20_right_19.yaml`为唯一软件配置源。程序执行4项拇指任务，
+以及小指、无名指、中指、食指各自的正面+侧面同步roll、侧面pitch、
+侧面PIP/DIP联合任务，共16个物理运动任务。同步roll只驱动电机一次，但两台相机仍分别拟合并通过
+各自的观测质量门限。每个PIP任务只驱动一次对应电机，同时用“手掌→中节Tag”实测PIP、
+用“中节Tag→末节Tag”实测被动DIP；四个DIP不再由URDF mimic系数生成，并参加完整视觉
+拟合和质量门限。每项正式四轮之前自动低速往返预检
+0/127/255可见性；
+
+基准形态恢复完成后，程序先用至少30帧稳健锁定正面ID 0、侧面ID 4和顶部ID 8的
+固定掌部位姿。小指和无名指弯曲避让会遮住正面ID 0，因此四指正面+侧面同步roll中
+允许ID 0暂时不可见，并使用本会话基准锁定值；运动连杆Tag仍必须实时可见，门限不
+放宽。终端用`锁`表示该固定参考有效，例如`正面[0锁,12✓]`，`✗`才表示需要处理的
+实时Tag。锁定后本次标定运行中不得再移动相机、手掌底座或整只手，否则缓存参考
+失效，必须重新启动标定；两次独立会话之间轻微调整整只手的位置不会改变机械端点
+零位基准。任务级Tag有效率门限按"当前任务所需角色生效期间的采集帧"统计；
+任务结束后角色要求会切回预检全套标签，静止期帧不参与该门限，避免把
+采集质量良好的任务误判为可见性失败。
+
+恢复末端Tag后，程序能够独立实测四指PIP和DIP的转轴及动态命令曲线。但同一次
+相机外参和同一套Tag安装下的重复扫描无法排除固定安装相位偏差；实体手在反馈0端
+能够触掌是独立的机械端点约束。三个thumb CMC主动轴、`thumb_mcp`、四指MCP pitch
+和PIP的URDF静态零位均由本次实测全行程与CAD机械端点之差求出，不再把跨相机的
+平面PnP绝对相位直接当作编码器零位，也不写死为0。生成URDF时同步修正这些关节的
+坐标上限以及相关被动关节的mimic坐标偏置，
+保证非零零偏不会缩短最大闭合量。末节Tag继续用于DIP动态曲线、轴线质量、遮挡和
+第四轮留出检查。正式数据求解静态修正范围为拇指CMC三个主动关节、`thumb_mcp`、
+四指MCP roll、MCP pitch及PIP，共16个。`thumb_mcp`与四指屈伸关节一样使用
+实测全屈曲行程和CAD机械端点联合求解，不直接采用MCP/IP耦合运动的单目PnP相位。
+侧面累计避障按“PIP→MCP pitch→roll”的安全顺序分阶段进入，并按逆序分阶段退出；
+同类辅助电机（全部邻指滚转、全部PIP、全部MCP pitch）合并为同一个并行航点同时
+运动，被测通道最后单独进入。“滚转全部回中前不展开弯曲手指”“每指pitch先于PIP”
+等已评审不变量保持不变，过渡仍受类别限速、逐航点到位确认、停滞检测和超时保护。
+`parallel_pose_transitions`（默认true）置false可回退旧的逐电机顺序。
+同一任务的预检和四轮正式扫描会保持完整避障姿态连续执行，只在任务切换时退出，
+不再每轮重复展开/弯曲辅助手指。跨手指组切换时，下一组避障姿态仍然需要、且
+当前已经在位（含反馈容差）的辅助电机保持原位，只有下一组不再使用的避障电机
+退回基准，避免"先展开回基准、马上又折回"的多余动作；已评审的
+"滚转先回中再展开""先滚开再弯曲"顺序保持不变。预检正反方向若都保留至少64个电机分箱且最大空缺
+不超过8，只作为采集能力诊断。G20右手四轮正式速度始终使用产品配置的固定值，
+不会因本次预检帧率或识别密度而改变；旧11-Tag布局仍保留自适应速度兼容逻辑。
+四指roll不再把同一反馈127误当成方向无关的唯一机械姿态：以`255→127`为标准物理
+零位，反向到达127的实测偏差保留在`increasing_rad`中。方向分支间隙上限1.5°、
+四轮间隙极差上限0.3°；其他关节仍使用严格的0.5°baseline回差门限。
+预检、正式四轮和拟合重扫始终使用速度5。
+正式roll的每个方向会在经过127时先到位保持0.5秒，再独立保存至少10帧静止Tag/反馈；
+方向分支检查和动态曲线的127相位都使用这两组双向静止数据，运动中经过127的帧不再
+替代静态保持姿态。
+前三轮只用于训练，第四轮完全留出；留出轮不参与显著性、Student-t置信区间或最终重拟合。
+每个任务只在低速递减预检起点执行一次8帧PnP静态初始化；预检往返和四轮正式
+扫描连续复用同一帧间分支与任务参考，不再让每一轮独立选择平面Tag解。同一任务第1轮
+已确立的端点相对姿态作为后3轮的分支锚点，防止独立初始化选到相反的
+IPPE镜像解。baseline标准接近和全部质量门限保持不变。
+电机15任务会利用源URDF中已确认的`thumb_ip mimic=1.03`，只在逐帧IPPE双解中
+排除与MCP同步运动明显矛盾（残差超过7.5°）的ID3镜像候选。该先验不生成或缩放
+`thumb_ip`曲线；通过分支选择后的`ID2→ID3`姿态仍独立拟合并接受完整留出验证。
+
+当前19-Tag产品流程发布精简schema v4：21条运行时曲线全部来自当前会话的视觉实测。
+URDF零位字段覆盖拇指4个主动关节和四指各自的`mcp_roll/mcp_pitch/pip`，共16个，
+其中三个thumb CMC轴、`thumb_mcp`及四指`mcp_pitch/pip`共12个字段由实测旋转行程
+与机械端点联合求解；
+写出非零`thumb_mcp`零偏时同步平移其关节坐标上限，并更新被动`thumb_ip`的
+`mimic offset`，因此不会改变CAD定义的最大屈曲实体姿态；
+`thumb_ip`及四指DIP静态零位保留源CAD。旧schema v5文件仅作历史回放兼容，
+当前一键流程不再生成它。正面/侧面roll在同一次运动中独立拟合；方向、
+轴线和动态曲线均通过时做不确定度加权轴融合。侧面PIP连杆标签在滚转扫掠中
+相对侧相机视线倾斜约13°~20°，平面标签的单目IPPE姿态二义性会给侧视姿态引入
+数度的系统性"绕视线"偏差（亚像素重投影无法发现，会话20260820_105535实测
+前后轴向稳定相差11.4°），因此侧视PIP连杆姿态不再参与MCP轴向融合或角曲线验收，
+正侧姿态差只写入`cross_view_roll_axis_diagnostic`。四根MCP侧摆轴在产品URDF中
+严格平行：小指作为先采集的参考轴，其余三指复用该公共方向并各自独立拟合轴线位置，
+避免平面PnP分支在不同会话中改变轴向。前视侧摆连杆受丝杆平移影响，其纯旋转拟合得到的是
+随手指结构变化的伪轴线，不能与侧视PIP连杆的物理轴线使用统一距离门限；
+两者线距仅记录在诊断中。侧视校验通道
+（`*_mcp_roll_side`）的
+姿态分支间隙跨轮极差和独立姿态轴方向极差只作诊断，不触发重复采集；这两个量来自
+近掠射平面Tag的非发布姿态分量。绝对分支间隙1.5°上限保持不变，真正发布的正面主轴
+仍使用原跨轮严格门限。侧视逐帧`axis_pose_line_rms`同样只作诊断，组合轴线改用四轮
+位置RMS验收；径向、平面、圆一致性及可见性门限全部保留。
+侧面端视roll的圆轨迹方向已经受
+姿态轴约束，因此自由三维圆平面与姿态轴的夹角只保留诊断，不再被重复作为硬门限；
+径向残差和四轮轴线位置RMS仍是硬门限。正式MCP动态曲线统一使用正面Tag中心的
+二维投影圆角度，侧面姿态曲线仅保留为诊断；任一正式视角自身四轮不重复或第四轮
+留出失败仍会拒绝发布。任一静态目标、第四轮留出、
+遮挡、PnP或跨机位检查失败时，只保留原始轨迹和`passed:false`诊断，不发布正式URDF。
+8个组合姿态仅保留为开发诊断，正式产品默认不执行。轴线零位求解不提供适合绝对笛卡尔
+位置验收的手基座变换，因此不能用该诊断推翻已经通过的单关节隔离留出结果。三个CMC轴恢复使用
+`a609d521`验证过的完整四轮相对旋转曲线；全部实测关节均由隔离第四轮逐关节验收。
+现场需要区分某根手指的roll机构回差与单机位误差时，可设置
+`cross_view_roll_diagnostic_finger:=pinky|ring|middle|index`。该会话只执行目标手指的一次
+正面+侧面同步roll，共10个预检/正式方向；任一机位数据不足会重扫同一物理任务，
+双机位数据齐全后即使存在轴质量失败也不再自动重采，而是把失败项随双机位结果
+一起写入`cross_view_roll_diagnostic`并立即暂停。诊断会话永久
+锁定URDF发布，不能用`resume`转换成正式标定。
+schema v5明确声明曲线输入域为真实反馈u8；运行桥默认订阅
+`/g20/cb_right_hand_state`，并按反馈增减方向选择正程/反程曲线，停止时锁存最后运动
+方向。尚未观察到运动方向时使用`255→127`标准分支，不使用两个机械分支的平均值。
+schema v4继续兼容旧
+命令域。两者都只发布动态角度，不重复叠加已写入URDF的静态偏移。
 
 ## 三机位三维关节轴零位标定（schema v4）
 
@@ -47,7 +468,7 @@ ID 9 必须在拇指横摆的完整行程中持续可见。
 
 ```bash
 mkdir -p /home/lxp/projects/linkerhand_retarget_ros2/config
-ros2 launch g20_thumb_apriltag_calibration \
+ros2 launch linkerhand_calibration \
   three_camera_extrinsics.launch.py \
   output_file:=/home/lxp/projects/linkerhand_retarget_ros2/config/g20_three_camera_extrinsics.yaml \
   checkerboard_columns:=8 checkerboard_rows:=5 square_size_m:=0.027
@@ -63,11 +484,11 @@ PnP的最终外参偏差锁死采集。点击 `AUTO ON` 后，棋盘稳定1秒
 外参分两组采集。让棋盘静止且同时出现在正面/侧面画面中，每改变一次位置和倾角添加
 一个候选；随后以相同方法采集正面/上面。程序使用固定内参的
 `stereoCalibrate` 联合优化唯一旋转/平移。采集准入和最终验收分离：FRONT和
-配对相机的单帧RMS分别不得超过1.5 px，同时组合RMS不得超过1.2 px；界面中
+配对相机的单帧RMS分别不得超过1.5 px，候选组合RMS不得超过1.5 px；界面中
 单相机1.2 px以内显示绿色、1.2～1.5 px显示黄色且仍可采集、超过1.5 px显示红色。
 新姿态会与全部已采姿态比较，避免在少数姿态间反复采集。拟合先剔除粗大异常组，
-再在不低于15个内点的前提下有界裁剪联合误差最高的候选，最终1.2 px门限不会被
+再在不低于15个内点的前提下有界裁剪联合误差最高的候选，只有最终批次组合RMS
-放宽。两组均得到
+不超过1.2 px才允许保存。两组均得到
 至少15个内点且联合RMS、三折稳定性合格后 `SAVE` 才变绿。
 
 ```bash
@@ -88,12 +509,11 @@ ros2 service call /g20_camera_extrinsics/save std_srvs/srv/Trigger {}
 先使用禁止运动模式检查三个机位、外参、内参和标签：
 
 ```bash
-ros2 launch g20_thumb_apriltag_calibration \
+ros2 launch linkerhand_calibration \
   three_camera_calibration.launch.py \
   hand_type:=left \
   serial_number:=G20_LEFT_001 \
   camera_extrinsics_file:=/home/lxp/projects/linkerhand_retarget_ros2/config/g20_three_camera_extrinsics.yaml \
-  source_urdf_path:=/home/lxp/projects/linkerhand_retarget_ros2/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/linkerhand_g20_left.urdf \
   commands_enabled:=false
 ```
 
@@ -111,19 +531,20 @@ ros2 run image_view image_view --ros-args \
 确认全行程安全、MVS客户端已关闭且没有其他命令发布者后，重启正式流程：
 
 ```bash
-ros2 launch g20_thumb_apriltag_calibration \
+ros2 launch linkerhand_calibration \
   three_camera_calibration.launch.py \
   hand_type:=left \
   serial_number:=G20_LEFT_001 \
   camera_extrinsics_file:=/home/lxp/projects/linkerhand_retarget_ros2/config/g20_three_camera_extrinsics.yaml \
-  source_urdf_path:=/home/lxp/projects/linkerhand_retarget_ros2/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/linkerhand_g20_left.urdf \
   can_interface:=can0
 ```
 
-右手使用同一入口；默认自动选择右手SDK话题和原始URDF：
+原始URDF及其mesh随`linkerhand_calibration`安装，默认根据`hand_type`自动选择。
+如需调试其他CAD版本，仍可通过`source_urdf_path:=<绝对路径>`显式覆盖。
+右手使用同一入口，并自动选择右手SDK话题和原始URDF：
 
 ```bash
-ros2 launch g20_thumb_apriltag_calibration \
+ros2 launch linkerhand_calibration \
   three_camera_calibration.launch.py \
   hand_type:=right \
   serial_number:=G20_RIGHT_001 \
@@ -177,7 +598,7 @@ Tag位置，接近轴向观察时改用相机图像平面相位并丢弃单目Pn
 
 前两轮拟合，第三轮强制留出验证；轨迹与零位角度MAE必须≤1°、P95≤2°，三轮轴/零位
 差≤0.75°、径向RMS≤3 mm、轴线SE(3)残差≤1 mm。非零修正必须在第三轮优于原始URDF，并通过按三轮分组的
-95% bootstrap改善置信检查。最终门限不会因自动重试而放宽。
+训练周期Student-t 95%改善下界检查。最终门限不会因自动重试而放宽。
 
 单轮姿态相对理想固定轴的轴外RMS与跨轮重复性分别判定：主动关节上限2.5°，被动
 耦合关节上限7.5°。较宽的被动模型门限只容纳可重复的机构耦合和双Tag PnP系统误差，
@@ -194,13 +615,14 @@ Tag位置，接近轴向观察时改用相机图像平面相位并丢弃单目Pn
 约4°的整指倾斜；重复扫描与同源留出不能排除这种系统偏差，因此不得写入URDF。
 
 四指MCP屈伸和PIP采用同一静态策略：参考指轨迹仍参与动态曲线、轴质量和机构诊断，
-但拟合出的绝对相位不写入任何一根四指的 `origin.rpy`。拇指CMC roll/yaw/pitch的非零
+但拟合出的共同掌坐标相位不写入四指 `origin.rpy`；只发布各指相对四指中值的实测
-修正只能来自当前会话的三轮轨迹求解并通过第三轮留出验证；代码和配置中不保存任何
+装配偏差。拇指CMC roll/yaw/pitch的非零修正来自当前会话的完整相对旋转行程与机械
-按左右手或序列号写死的拇指零位角。电机5的256点动态曲线也使用本机三轮实测结果。
+端点，视觉轴链继续用于轴线、PnP和留出诊断；代码和配置中不保存任何按左右手或
+序列号写死的拇指零位角。电机5的256点动态曲线同样使用本机四轮实测结果。
 
-7个直接零位依赖链为：yaw轴约束拇指roll、pitch轴约束拇指yaw、MCP轴线相位约束
+视觉依赖链仍为：yaw轴检查拇指roll、pitch轴检查拇指yaw、MCP轴线相位检查拇指
-拇指pitch；IP轴线相位仅作诊断，不能覆盖拇指MCP的原始CAD零位。参考指MCP pitch轴
+pitch、IP轴线相位检查拇指MCP。该链用于几何和PnP诊断，不再决定四个具有机械端点
-约束roll，PIP/DIP轴线相位只用于参考指机构诊断，不再覆盖四指CAD静态零位。
+的拇指主动关节绝对零位；四指PIP/DIP轴线相位也继续用于机构诊断。
 原始URDF的 `origin.xyz`、`axis.xyz`、连杆长度、mesh和被动结构固定。yaw扫描时电机5
 保持145，求解器使用实测 `angle_rad[145]` 还原该条件，不会把145误当成baseline。
 偏移超过各关节专用上限时整次失败。数值求解会在更宽的诊断范围内继续估计，因此状态和原始JSONL
@@ -209,18 +631,22 @@ Tag位置，接近轴向观察时改用相机图像平面相位并丢弃单目Pn
 
 生成修正URDF时只修改通过验收的主动关节 `origin.rpy`，不会修改任何关节的
 `origin.xyz`、转轴、mimic关系或原始CAD/机械安全限位。256项实测轨迹只保存在最终
-JSON；实测曲线即使略微越过CAD限位，也不能自动扩大URDF限位。
+JSON；任一曲线点越过CAD限位都会阻止正式发布，程序不会自动扩大URDF限位。
 
 坏帧只丢弃。短时Tag丢失、同步帧中断、扫描超时、端点/分箱不足会自动保持当前位置、
 重置当前机位PnP、返回基准后重扫当前方向，最多3次；速度依次降为80%/60%/50%，
 端点保持延长到0.75/1.0/1.25秒，扫描超时按降速比例同步延长。若反馈在远离目标时
 连续8秒没有至少1个u8的进展，则按机械碰撞/摩擦或硬件故障立即保持当前反馈位置并
 暂停，不消耗三次采样重试预算。单轮拟合失败只重扫该轮两个方向，全局不一致才重扫
-完整关节，每关节最多自动重采2轮。过程指标在最终门限的1.25倍内只发黄色预警，最终
+完整关节，每关节最多自动重采2轮。过程指标落在最终门限的1.25倍内时会标记为黄色
-拟合仍按原硬门限验收。零位触边、稳定留出误差或URDF几何无法解释属于模型失败，程序
+预警，但只要仍超过硬门限，就在当前关节立即使用剩余重试预算
+（`provisional_fit_warning_rescan`）；第三次仍超限则当场暂停，不允许预警数据继续到
+后续关节。最终拟合仍按原硬门限验收，因此不会在全部任务采完后才回头重采靠前
+关节。零位触边、稳定留出误差或URDF几何无法解释属于模型失败，程序
 只暂停一次且不再自动重扫，防止重复运动；此时也拒绝`resume`形成死循环。其他可恢复
 失败在预算耗尽后才暂停，`resume`从最小失败单元继续，已通过数据保留。所有失败尝试
-仍保存在 `raw_samples.jsonl`。
+仍保存在 `raw_samples.jsonl`。若连续两次完整重扫出现轮次和数值都重复的
+PnP双簇行程，程序将它判为系统性分支失败并当场停止，不再浪费第3次全关节重扫。
 
 每个新机位/Tag组合开始运动前，不使用单个端点帧直接决定平面Tag的IPPE姿态分支。
 程序在静止端点联合8帧候选，按相邻Tag相对姿态的跨帧稳定性和重投影误差选择整组
@@ -273,27 +699,37 @@ ID 9的可见性和PnP稳定性。当前方向自动重试、失败轮次重试
 ```text
 calibration_output/G20_LEFT_001/<时间戳>/
   g20_left_G20_LEFT_001_calibration.json
-src/.../g20_left/
+  g20_left_G20_LEFT_001_calibration_urdf_correction_input.json
   linkerhand_g20_left_zero_calibrated_G20_LEFT_001_<时间戳>.urdf
+  meshes/*.STL
 
 calibration_output/G20_RIGHT_001/<时间戳>/
   g20_right_G20_RIGHT_001_calibration.json
-src/.../g20_right/
+  g20_right_G20_RIGHT_001_calibration_urdf_correction_input.json
   linkerhand_g20_right_zero_calibrated_G20_RIGHT_001_<时间戳>.urdf
+  meshes/*.STL
 ```
 
+`*_urdf_correction_input.json` 会在修正 URDF 之前落盘并重新读取，且绑定源 URDF
+哈希、型号、侧别、layout 和序列号。公开 schema-v4 运行 JSON 仍保持原格式，原有
+G20 运行桥、金标准哈希和 URDF 写回数值不因该交接层改变。
+
 文件包含21个关节的256项 `angle_rad`、16个主动关节的 `zero_command_u8`、
 `zero_angles.urdf_zero_offset_rad`、5个被动标记、模板来源和总体质量。新URDF每次从
 指定原始CAD文件生成，采用 `T_original × Rot(axis, offset)`，绝不叠加旧校准文件或
 覆盖原文件；只有通过独立求解验证或明确机械装配基准授权的主动关节 `origin.rpy` 可能
-改变，未观测关节和其他URDF文本保持不变。每帧Tag SE(3)、
+改变；当前19-Tag右手的16个主动静态零位字段全部由本会话数据求解；四指DIP和
+`thumb_ip`为被动关节，发布实测动态曲线并保留CAD静态零位，其mimic坐标偏置只随
+上游主动关节坐标系变换作等价调整。
+未观测关节和其他URDF文本保持不变。源URDF中的相对mesh资源会按原相对路径复制到
+同一会话，保证会话内URDF可独立加载，并在正式发布时逐文件记录SHA256。每帧Tag SE(3)、
 图像时间戳、20通道状态、同步误差和PnP误差只进入 `raw_samples.jsonl`。
 
 完整 `raw_samples.jsonl` 已存在时，可以按当前算法离线重放，不连接相机、不发送电机
 命令。`--output-tag` 为新产物增加安全后缀，已有JSON、URDF和验证报告不会被覆盖：
 
 ```bash
-python3 -m g20_thumb_apriltag_calibration.offline_replay \
+python3 -m linkerhand_calibration.offline_replay \
   calibration_output/G20_RIGHT_001/20260811_120146 \
   --output-tag AXIS_FRAME_V3 \
   --write
@@ -310,12 +746,13 @@ python3 -m g20_thumb_apriltag_calibration.offline_replay \
 `JointState`（包括5个被动关节）：
 
 ```bash
-ros2 launch g20_thumb_apriltag_calibration calibrated_joint_state_bridge.launch.py \
+ros2 launch linkerhand_calibration calibrated_joint_state_bridge.launch.py \
   hand_type:=right \
   calibration_file:=$PWD/calibration_output/G20_RIGHT_001/20260811_120146/g20_right_G20_RIGHT_001_calibration.json
 ```
 
-默认订阅 `/cb_right_hand_control_cmd`，发布
+schema v5默认订阅 `/g20/cb_right_hand_state`；schema v4默认订阅
+`/g20/cb_right_hand_control_cmd`。两者均发布
 `/sim/mujoco/g20/right/joint_state`。启动前必须停止任何旧的同名话题桥，避免两个
 发布者同时驱动仿真。节点会拒绝左右手不匹配、质量未通过、字段不完整或非有限命令，
 因此不会静默退回旧标定。
@@ -381,7 +818,7 @@ sudo apt-get install -y \
 cd /home/lxp/projects/linkerhand_retarget_ros2
 source /opt/ros/jazzy/setup.bash
 colcon build --symlink-install \
-  --packages-select linker_hand_ros2_sdk g20_thumb_apriltag_calibration
+  --packages-select linker_hand_ros2_sdk linkerhand_calibration
 source install/setup.bash
 ```
 
@@ -408,7 +845,7 @@ source install/setup.bash
 先单独启动相机（不会连接机械手，也不会发送关节命令）：
 
 ```bash
-ros2 run g20_thumb_apriltag_calibration hikrobot_camera_node --ros-args \
+ros2 run linkerhand_calibration hikrobot_camera_node --ros-args \
   --remap __ns:=/camera/camera/color \
   -p serial_number:=DB2163742 \
   -p camera_info_url:=$HOME/.ros/camera_info/hikrobot_DB2163742.yaml
@@ -446,7 +883,7 @@ ros2 run camera_calibration cameracalibrator \
 但标定节点不会发送位置运动命令，也不会允许解锁全行程扫描：
 
 ```bash
-ros2 launch g20_thumb_apriltag_calibration front_thumb_calibration.launch.py \
+ros2 launch linkerhand_calibration front_thumb_calibration.launch.py \
   serial_number:=G20_LEFT_001 \
   camera_serial_number:=DB2163742 \
   commands_enabled:=false
@@ -457,7 +894,7 @@ ros2 launch g20_thumb_apriltag_calibration front_thumb_calibration.launch.py \
 速度，并使用单终点连续运动：
 
 ```bash
-ros2 launch g20_thumb_apriltag_calibration front_thumb_calibration.launch.py \
+ros2 launch linkerhand_calibration front_thumb_calibration.launch.py \
   serial_number:=G20_LEFT_001 \
   camera_serial_number:=DB2163742 \
   can_interface:=can0 \
@@ -570,7 +1007,7 @@ calibration_output/<序列号>/<时间戳>/
 恢复时必须显式复用原目录，否则会创建新会话：
 
 ```bash
-ros2 launch g20_thumb_apriltag_calibration front_thumb_calibration.launch.py \
+ros2 launch linkerhand_calibration front_thumb_calibration.launch.py \
   serial_number:=G20_LEFT_001 \
   session_dir:=/绝对路径/calibration_output/G20_LEFT_001/20260727_120000
 ```
@@ -636,7 +1073,7 @@ for name, joint in data["joints"].items():
 不影响 AprilTag 的 ROI 输入。
 
 静态预检先在单 Tag 层拒绝高重投影误差，再检查三组相对中心的位置内点率和毫米级 RMS。
-当前 30～38 px 的 10 mm Tag 属于试标定尺寸，如果中心位置 RMS 持续不合格，应优先增加照明、缩短
+当前末节16 mm Tag如果中心位置 RMS 持续不合格，应优先增加照明、缩短
 相机距离或提高 Tag 有效像素，而不是放宽最终随机复测精度。
 
 启用 rosbag 后保存裁剪后的原始图像和配套 `CameraInfo`，避免新增一个全分辨率图像
@@ -650,7 +1087,7 @@ for name, joint in data["joints"].items():
 URDF：
 
 ```bash
-ros2 launch g20_thumb_apriltag_calibration \
+ros2 launch linkerhand_calibration \
   front_cmc_pitch_zero.launch.py \
   serial_number:=G20_LEFT_001
 ```
@@ -727,7 +1164,7 @@ zero_angles.table_projected_zero_rad
 Roll同样固定使用“T3中心→拟合圆心”的内向径向矢量，不读取T3标签朝向。
 
 ```bash
-ros2 launch g20_thumb_apriltag_calibration \
+ros2 launch linkerhand_calibration \
   front_cmc_roll_calibration.launch.py \
   serial_number:=G20_LEFT_001
 ```

src/linkerhand_calibration/config/calibration.yaml

@@ -44,7 +44,7 @@ g20_thumb_calibration:
     minimum_detection_hz: 15.0
     maximum_hamming: 0
     minimum_decision_margin: 30.0
-    # Trial threshold for the current 10 mm tags (observed at 32-38 px).
+    # Trial threshold for small/far tags (historically observed at 32-38 px).
     # Final acceptance is still guarded by static RMS and random validation.
     minimum_edge_pixels: 30.0
     # Current 30 px tags measure about 0.50-0.53 deg RMS while stationary.

src/linkerhand_calibration/config/g20_right_product.yaml

@@ -0,0 +1,37 @@
+schema_version: 1
+model: G20
+side: right
+tag_layout: g20_right_19
+serial_number: G20_RIGHT_001
+can_interface: can0
+output_root: calibration_output
+
+cameras:
+  front:
+    serial_number: DB2163742
+    camera_name: hikrobot_front_DB2163742
+    camera_info: ~/.ros/camera_info/hikrobot_DB2163742.yaml
+  side:
+    serial_number: DB2163749
+    camera_name: hikrobot_side_DB2163749
+    camera_info: ~/.ros/camera_info/hikrobot_DB2163749.yaml
+  top:
+    serial_number: DB2163739
+    camera_name: hikrobot_top_DB2163739
+    camera_info: ~/.ros/camera_info/hikrobot_DB2163739.yaml
+
+artifacts:
+  source_urdf: package://linkerhand_calibration/urdf/g20_right/linkerhand_g20_right.urdf
+  source_urdf_sha256: eeb6ffb0e95d2a6acd4c26331ae68062e0d74160de4b552b4f6d395cce5ca4e8
+  camera_extrinsics: config/g20_three_camera_extrinsics.yaml
+  camera_extrinsics_sha256: dd623572df3cb83fdefcbe92204dab54a60f2c68eb3a8c9bdb08407e8f0e5d80
+  calibration_config: src/g20_thumb_apriltag_calibration/config/three_camera_calibration.yaml
+  calibration_config_sha256: 0faaf891ebb616c4c8a3bb3052c48fa4b6c8aa0c5fdc5abaaa89f4fc29cca1c3
+  tag_config: src/g20_thumb_apriltag_calibration/config/three_camera_tags_g20_right_19.yaml
+  tag_config_sha256: b1ab45e97ae42d57b0a3a63c725107b5aa3828c06b2ced16f8222d6e9ebadc41
+
+release:
+  # Each task already contains three training cycles plus an isolated fourth
+  # holdout, so a second complete hardware session duplicates hours of motion.
+  required_independent_passes: 1
+  static_repeatability_deg: 1.0

src/linkerhand_calibration/config/l6_right_8_tags.yaml

@@ -0,0 +1,59 @@
+/l6_calibration/front/apriltag/apriltag:
+  ros__parameters:
+    image_transport: raw
+    qos_profile: sensor_data
+    family: 36h11
+    size: 0.016
+    max_hamming: 0
+    detector:
+      threads: 4
+      decimate: 1.0
+      blur: 0.0
+      refine: true
+      sharpening: 0.25
+      debug: false
+    pose_estimation_method: pnp
+    tag:
+      ids: [0, 1, 2]
+      frames: [front_base, thumb_pitch, thumb_dip]
+      sizes: [0.016, 0.016, 0.016]
+
+/l6_calibration/side/apriltag/apriltag:
+  ros__parameters:
+    image_transport: raw
+    qos_profile: sensor_data
+    family: 36h11
+    size: 0.016
+    max_hamming: 0
+    detector:
+      threads: 4
+      decimate: 1.0
+      blur: 0.0
+      refine: true
+      sharpening: 0.25
+      debug: false
+    pose_estimation_method: pnp
+    tag:
+      ids: [3, 4, 5]
+      frames: [side_base, pinky_pitch, pinky_dip]
+      sizes: [0.016, 0.016, 0.016]
+
+/l6_calibration/top/apriltag/apriltag:
+  ros__parameters:
+    image_transport: raw
+    qos_profile: sensor_data
+    family: 36h11
+    size: 0.016
+    max_hamming: 0
+    detector:
+      threads: 4
+      decimate: 1.0
+      blur: 0.0
+      refine: true
+      sharpening: 0.25
+      debug: false
+    pose_estimation_method: pnp
+    tag:
+      ids: [6, 7]
+      frames: [top_base, thumb_roll]
+      sizes: [0.016, 0.016]

src/linkerhand_calibration/config/l6_right_product.yaml

@@ -0,0 +1,37 @@
+schema_version: 2
+profile_id: L6/right/l6_right_8/v1
+model: L6
+side: right
+tag_layout: l6_right_8
+namespace: /l6_calibration
+serial_number: L6_RIGHT_001
+can_interface: can0
+output_root: calibration_output
+
+cameras:
+  front:
+    serial_number: DB2163742
+    camera_name: hikrobot_front_DB2163742
+    camera_info: ~/.ros/camera_info/hikrobot_DB2163742.yaml
+  side:
+    serial_number: DB2163749
+    camera_name: hikrobot_side_DB2163749
+    camera_info: ~/.ros/camera_info/hikrobot_DB2163749.yaml
+  top:
+    serial_number: DB2163739
+    camera_name: hikrobot_top_DB2163739
+    camera_info: ~/.ros/camera_info/hikrobot_DB2163739.yaml
+
+artifacts:
+  source_urdf: package://linkerhand_calibration/urdf/l6_right/linkerhand_l6v3.1_right.urdf
+  source_urdf_sha256: 298c1fbf5189648911426f530b50bdbeea4830cab9c54e20f46c532485df4666
+  camera_extrinsics: config/g20_three_camera_extrinsics.yaml
+  camera_extrinsics_sha256: dd623572df3cb83fdefcbe92204dab54a60f2c68eb3a8c9bdb08407e8f0e5d80
+  calibration_config: package://linkerhand_calibration/config/l6_three_camera_calibration.yaml
+  calibration_config_sha256: 0934699c8225891e748deefef6791eb28355821b89aeadd1f7ff0b7f7b4d265f
+  tag_config: package://linkerhand_calibration/config/l6_right_8_tags.yaml
+  tag_config_sha256: be1499eb947b61d2fe360ae2c92307a87710480fae8a9dd4cd171fc959fdcbf5
+
+release:
+  required_independent_passes: 1
+  static_repeatability_deg: 1.0

src/linkerhand_calibration/config/l6_three_camera_calibration.yaml

@@ -0,0 +1,67 @@
+l6_calibration:
+  ros__parameters:
+    command_topic: /l6/cb_right_hand_control_cmd
+    state_topic: /l6/cb_right_hand_state
+    setting_topic: /l6/cb_hand_setting_cmd
+    front_camera_info_topic: /l6_calibration/front/camera/camera_info
+    front_detections_topic: /l6_calibration/front/apriltag/detections
+    side_camera_info_topic: /l6_calibration/side/camera/camera_info
+    side_detections_topic: /l6_calibration/side/apriltag/detections
+    top_camera_info_topic: /l6_calibration/top/camera/camera_info
+    top_detections_topic: /l6_calibration/top/apriltag/detections
+
+    baseline_command_u8: [255, 255, 255, 255, 255, 255]
+    # L6_RIGHT_001 measured a 250->5 travel of only ~0.9 s at speed 10,
+    # which left fewer than 32 useful feedback bins. Speed 1 is still only a
+    # firmware ceiling: different L6 motors complete a full stroke in 0.7-1.3 s.
+    # A 100 Hz cosine trajectory therefore sets the actual, model-level pace.
+    preflight_speed_u8: 1
+    formal_speed_u8: 1
+    speed_settle_seconds: 0.2
+    command_trajectory_full_range_seconds: 6.0
+    torque_u8: 80
+    repetitions: 4
+    preflight_checkpoints_u8: [255, 127, 0]
+    tag_size_m: 0.016
+    tag_size_override_ids: [0, 1, 2, 3, 4, 5, 6, 7]
+    tag_size_overrides_m: [0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016]
+
+    minimum_detection_rate: 0.95
+    # Per-Tag quality remains >=95%. With three independently detected Tags,
+    # the fully joined frame rate may be 0.95^3 ~= 85.7%.
+    minimum_joint_frame_rate: 0.85
+    minimum_feedback_hz: 25.0
+    maximum_state_image_skew_ms: 50.0
+    maximum_hamming: 0
+    minimum_decision_margin: 30.0
+    minimum_edge_pixels: 30.0
+    pnp_maximum_reprojection_error_px: 1.5
+    pnp_reprojection_tie_px: 1.5
+    pnp_maximum_pose_jump_deg: 35.0
+    pnp_maximum_translation_jump_m: 0.04
+    pnp_maximum_tag_tilt_deg: 75.0
+    pnp_tracker_reset_seconds: 5.0
+
+    minimum_sweep_frames: 40
+    minimum_state_span_u8: 240.0
+    minimum_sweep_bins: 32
+    maximum_bin_gap: 16
+    maximum_monotonic_correction_deg: 2.0
+    passive_maximum_monotonic_correction_deg: 3.0
+    maximum_validation_mae_deg: 1.0
+    maximum_validation_p95_deg: 2.0
+    maximum_validation_error_deg: 3.0
+    mimic_minimum_multiplier: 0.5
+    mimic_maximum_multiplier: 1.5
+    mimic_maximum_cycle_range: 0.03
+    mimic_maximum_residual_p95_deg: 2.0
+
+    endpoint_tolerance_u8: 2.0
+    endpoint_hold_seconds: 1.0
+    motor_stall_timeout_seconds: 2.0
+    position_timeout_seconds: 30.0
+    sweep_timeout_seconds: 90.0
+    automatic_sweep_retry_limit: 2
+    non_target_motion_tolerance_u8: 3.0
+    fixed_base_maximum_corner_drift_px: 2.0
+    fixed_base_movement_confirmation_frames: 5

src/linkerhand_calibration/config/o12_right_16_tags.yaml

@@ -0,0 +1,41 @@
+/o12_calibration/front/apriltag/apriltag:
+  ros__parameters:
+    image_transport: raw
+    qos_profile: sensor_data
+    family: 36h11
+    size: 0.016
+    max_hamming: 0
+    detector: {threads: 4, decimate: 1.0, blur: 0.0, refine: true, sharpening: 0.25, debug: false}
+    pose_estimation_method: pnp
+    tag:
+      ids: [0, 1, 2, 3, 12, 13]
+      frames: [front_base, thumb_cmc, thumb_mcp, thumb_dip, middle_roll, index_roll]
+      sizes: [0.016, 0.016, 0.016, 0.016, 0.016, 0.016]
+
+/o12_calibration/side/apriltag/apriltag:
+  ros__parameters:
+    image_transport: raw
+    qos_profile: sensor_data
+    family: 36h11
+    size: 0.016
+    max_hamming: 0
+    detector: {threads: 4, decimate: 1.0, blur: 0.0, refine: true, sharpening: 0.25, debug: false}
+    pose_estimation_method: pnp
+    tag:
+      ids: [4, 5, 6, 7, 8, 9, 10, 11]
+      frames: [side_base, pinky_mcp, pinky_pip, pinky_dip, middle_pip, middle_dip, index_pip, index_dip]
+      sizes: [0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016]
+
+/o12_calibration/top/apriltag/apriltag:
+  ros__parameters:
+    image_transport: raw
+    qos_profile: sensor_data
+    family: 36h11
+    size: 0.016
+    max_hamming: 0
+    detector: {threads: 4, decimate: 1.0, blur: 0.0, refine: true, sharpening: 0.25, debug: false}
+    pose_estimation_method: pnp
+    tag:
+      ids: [14, 15]
+      frames: [top_base, thumb_yaw]
+      sizes: [0.016, 0.016]

src/linkerhand_calibration/config/o12_right_product.yaml

@@ -0,0 +1,43 @@
+schema_version: 3
+profile_id: O12/right/o12_right_16/v1
+model: O12
+side: right
+tag_layout: o12_right_16
+namespace: /o12_calibration
+serial_number: O12_RIGHT_001
+output_root: calibration_output
+
+sdk:
+  driver: omnihand_pro_2025_node
+  transport: hcan
+  setup: src/agillink_omnihand_sdk/linux/x64/ros2/jazzy/setup.bash
+  config: src/agillink_omnihand_sdk/linux/x64/ros2/jazzy/share/omnihand_node/config/omnihand_pro_2025_node.yaml
+  config_sha256: ca1791c822bf1e99f25db8806397c3f877100c9252fcce6c53a7a6b8cef7694a
+
+cameras:
+  front:
+    serial_number: DB2163742
+    camera_name: hikrobot_front_DB2163742
+    camera_info: ~/.ros/camera_info/hikrobot_DB2163742.yaml
+  side:
+    serial_number: DB2163749
+    camera_name: hikrobot_side_DB2163749
+    camera_info: ~/.ros/camera_info/hikrobot_DB2163749.yaml
+  top:
+    serial_number: DB2163739
+    camera_name: hikrobot_top_DB2163739
+    camera_info: ~/.ros/camera_info/hikrobot_DB2163739.yaml
+
+artifacts:
+  source_urdf: package://linkerhand_calibration/urdf/o12_right/linkerhand_o12_t3_right-0703.urdf
+  source_urdf_sha256: 75b3c18992a3640d2d7d36719da5ff75428ded477b97d9cf29adbe43a6eb27eb
+  camera_extrinsics: config/g20_three_camera_extrinsics.yaml
+  camera_extrinsics_sha256: dd623572df3cb83fdefcbe92204dab54a60f2c68eb3a8c9bdb08407e8f0e5d80
+  calibration_config: package://linkerhand_calibration/config/o12_three_camera_calibration.yaml
+  calibration_config_sha256: 9defe39a3f4b31c3764cbdcd9ee5b570da70e810215844515e8cc01e56414728
+  tag_config: package://linkerhand_calibration/config/o12_right_16_tags.yaml
+  tag_config_sha256: 41001c3afba74cc01eb524a75dc58561a37e9364fab029ea12d879156a008dab
+
+release:
+  required_independent_passes: 1
+  static_repeatability_deg: 1.0

src/linkerhand_calibration/config/o12_three_camera_calibration.yaml

@@ -0,0 +1,50 @@
+o12_calibration:
+  ros__parameters:
+    command_topic: /o12/right/joint_cmd
+    state_topic: /o12/right/joint_states
+    front_camera_info_topic: /o12_calibration/front/camera/camera_info
+    front_detections_topic: /o12_calibration/front/apriltag/detections
+    side_camera_info_topic: /o12_calibration/side/camera/camera_info
+    side_detections_topic: /o12_calibration/side/apriltag/detections
+    top_camera_info_topic: /o12_calibration/top/camera/camera_info
+    top_detections_topic: /o12_calibration/top/apriltag/detections
+
+    # O12 standard interface is radians. Raw 0..2000 mixed control is forbidden.
+    command_rate_hz: 20.0
+    repetitions: 4
+    tag_size_m: 0.016
+    tag_size_override_ids: [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15]
+    tag_size_overrides_m: [0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016]
+    minimum_detection_rate: 0.95
+    minimum_joint_frame_rate: 0.85
+    minimum_feedback_hz: 15.0
+    maximum_state_image_skew_ms: 50.0
+    minimum_sweep_frames: 40
+    minimum_state_span_fraction: 0.90
+    minimum_sweep_bins: 32
+    maximum_bin_gap: 2
+    endpoint_tolerance_rad: 0.01
+    endpoint_hold_seconds: 1.0
+    motor_stall_timeout_seconds: 2.0
+    position_timeout_seconds: 60.0
+    sweep_timeout_seconds: 180.0
+    automatic_sweep_retry_limit: 2
+    non_target_motion_tolerance_rad: 0.015
+    maximum_temperature_c: 70
+    # 当前 O12 固件返回空温度报告。仍发起查询；5秒无结果后依赖已验证的
+    # joint_error_states bit1 过热保护，并在会话记录中明确标注降级。
+    temperature_report_required: false
+    temperature_fallback_after_seconds: 5.0
+    # 现场确认初始保守速度过慢；正式/点动速度提高到2倍，避让另有硬上限。
+    motion_speed_scale: 2.0
+    maximum_hamming: 0
+    minimum_decision_margin: 30.0
+    minimum_edge_pixels: 30.0
+    fixed_base_maximum_corner_drift_px: 2.0
+    fixed_base_movement_confirmation_frames: 5
+    pnp_maximum_reprojection_error_px: 1.5
+    pnp_reprojection_tie_px: 1.5
+    pnp_maximum_pose_jump_deg: 35.0
+    pnp_maximum_translation_jump_m: 0.04
+    pnp_maximum_tag_tilt_deg: 75.0
+    pnp_tracker_reset_seconds: 5.0

src/linkerhand_calibration/config/o6_right_8_tags.yaml

@@ -0,0 +1,59 @@
+/o6_calibration/front/apriltag/apriltag:
+  ros__parameters:
+    image_transport: raw
+    qos_profile: sensor_data
+    family: 36h11
+    size: 0.016
+    max_hamming: 0
+    detector:
+      threads: 4
+      decimate: 1.0
+      blur: 0.0
+      refine: true
+      sharpening: 0.25
+      debug: false
+    pose_estimation_method: pnp
+    tag:
+      ids: [0, 1, 2]
+      frames: [front_base, thumb_pitch, thumb_ip]
+      sizes: [0.016, 0.016, 0.016]
+
+/o6_calibration/side/apriltag/apriltag:
+  ros__parameters:
+    image_transport: raw
+    qos_profile: sensor_data
+    family: 36h11
+    size: 0.016
+    max_hamming: 0
+    detector:
+      threads: 4
+      decimate: 1.0
+      blur: 0.0
+      refine: true
+      sharpening: 0.25
+      debug: false
+    pose_estimation_method: pnp
+    tag:
+      ids: [3, 4, 5]
+      frames: [side_base, pinky_pitch, pinky_dip]
+      sizes: [0.016, 0.016, 0.016]
+
+/o6_calibration/top/apriltag/apriltag:
+  ros__parameters:
+    image_transport: raw
+    qos_profile: sensor_data
+    family: 36h11
+    size: 0.016
+    max_hamming: 0
+    detector:
+      threads: 4
+      decimate: 1.0
+      blur: 0.0
+      refine: true
+      sharpening: 0.25
+      debug: false
+    pose_estimation_method: pnp
+    tag:
+      ids: [6, 7]
+      frames: [top_base, thumb_yaw]
+      sizes: [0.016, 0.016]

src/linkerhand_calibration/config/o6_right_product.yaml

@@ -0,0 +1,37 @@
+schema_version: 2
+profile_id: O6/right/o6_right_8/v1
+model: O6
+side: right
+tag_layout: o6_right_8
+namespace: /o6_calibration
+serial_number: O6_RIGHT_001
+can_interface: can0
+output_root: calibration_output
+
+cameras:
+  front:
+    serial_number: DB2163742
+    camera_name: hikrobot_front_DB2163742
+    camera_info: ~/.ros/camera_info/hikrobot_DB2163742.yaml
+  side:
+    serial_number: DB2163749
+    camera_name: hikrobot_side_DB2163749
+    camera_info: ~/.ros/camera_info/hikrobot_DB2163749.yaml
+  top:
+    serial_number: DB2163739
+    camera_name: hikrobot_top_DB2163739
+    camera_info: ~/.ros/camera_info/hikrobot_DB2163739.yaml
+
+artifacts:
+  source_urdf: package://linkerhand_calibration/urdf/o6_right/linkerhand_o6_right.urdf
+  source_urdf_sha256: 8f184faad699fbf771e388f109a4e8793b5cb190c33a87b2eba8491a3a37dd62
+  camera_extrinsics: config/o6_three_camera_extrinsics.yaml
+  camera_extrinsics_sha256: 29af61f7bf1bad6718cbbaa54b0536f0a471c83f5bb3554f264ab9d292e56ca4
+  calibration_config: package://linkerhand_calibration/config/o6_three_camera_calibration.yaml
+  calibration_config_sha256: ce20d998a4342dfaacb14568513aa9af5063df48566fabd42180acc8da47e4a6
+  tag_config: package://linkerhand_calibration/config/o6_right_8_tags.yaml
+  tag_config_sha256: 16abe7119b4764f86333dae8264247571d1e0bca45af959d558bef4fb5485f5e
+
+release:
+  required_independent_passes: 1
+  static_repeatability_deg: 1.0

src/linkerhand_calibration/config/o6_three_camera_calibration.yaml

@@ -0,0 +1,64 @@
+o6_calibration:
+  ros__parameters:
+    command_topic: /o6/cb_right_hand_control_cmd
+    state_topic: /o6/cb_right_hand_state
+    setting_topic: /o6/cb_hand_setting_cmd
+    front_camera_info_topic: /o6_calibration/front/camera/camera_info
+    front_detections_topic: /o6_calibration/front/apriltag/detections
+    side_camera_info_topic: /o6_calibration/side/camera/camera_info
+    side_detections_topic: /o6_calibration/side/apriltag/detections
+    top_camera_info_topic: /o6_calibration/top/camera/camera_info
+    top_detections_topic: /o6_calibration/top/apriltag/detections
+
+    baseline_command_u8: [255, 255, 255, 255, 255, 255]
+    # O6 has a different speed scale from L6. Motion is still bounded by the
+    # six-second cosine command trajectory; these values are firmware limits.
+    baseline_speed_u8: 80
+    preflight_speed_u8: 60
+    formal_speed_u8: 40
+    speed_settle_seconds: 0.2
+    command_trajectory_full_range_seconds: 6.0
+    torque_u8: 80
+    repetitions: 4
+    preflight_checkpoints_u8: [255, 127, 0]
+    tag_size_m: 0.016
+    tag_size_override_ids: [0, 1, 2, 3, 4, 5, 6, 7]
+    tag_size_overrides_m: [0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016]
+
+    minimum_detection_rate: 0.95
+    minimum_joint_frame_rate: 0.85
+    minimum_feedback_hz: 25.0
+    maximum_state_image_skew_ms: 50.0
+    maximum_hamming: 0
+    minimum_decision_margin: 30.0
+    minimum_edge_pixels: 30.0
+    pnp_maximum_reprojection_error_px: 1.5
+    pnp_reprojection_tie_px: 1.5
+    pnp_maximum_pose_jump_deg: 35.0
+    pnp_maximum_translation_jump_m: 0.04
+    pnp_maximum_tag_tilt_deg: 75.0
+    pnp_tracker_reset_seconds: 5.0
+
+    minimum_sweep_frames: 40
+    minimum_state_span_u8: 240.0
+    minimum_sweep_bins: 32
+    maximum_bin_gap: 16
+    maximum_monotonic_correction_deg: 2.0
+    passive_maximum_monotonic_correction_deg: 3.0
+    maximum_validation_mae_deg: 1.0
+    maximum_validation_p95_deg: 2.0
+    maximum_validation_error_deg: 3.0
+    mimic_minimum_multiplier: 0.5
+    mimic_maximum_multiplier: 2.2
+    mimic_maximum_cycle_range: 0.03
+    mimic_maximum_residual_p95_deg: 2.0
+
+    endpoint_tolerance_u8: 2.0
+    endpoint_hold_seconds: 1.0
+    motor_stall_timeout_seconds: 2.0
+    position_timeout_seconds: 30.0
+    sweep_timeout_seconds: 90.0
+    automatic_sweep_retry_limit: 2
+    non_target_motion_tolerance_u8: 3.0
+    fixed_base_maximum_corner_drift_px: 2.0
+    fixed_base_movement_confirmation_frames: 5

src/linkerhand_calibration/config/three_camera_calibration.yaml

@@ -0,0 +1,193 @@
+g20_calibration:
+  ros__parameters:
+    command_topic: /g20/cb_left_hand_control_cmd
+    state_topic: /g20/cb_left_hand_state
+    info_topic: /g20/cb_left_hand_info
+    setting_topic: /g20/cb_hand_setting_cmd
+    front_camera_info_topic: /g20_calibration/front/camera/camera_info
+    front_detections_topic: /g20_calibration/front/apriltag/detections
+    side_camera_info_topic: /g20_calibration/side/camera/camera_info
+    side_detections_topic: /g20_calibration/side/apriltag/detections
+    top_camera_info_topic: /g20_calibration/top/camera/camera_info
+    top_detections_topic: /g20_calibration/top/apriltag/detections
+
+    # /start先下发并确认这个20通道基准姿态，稳定后才进入第一条扫描。
+    baseline_command_u8: [255, 255, 255, 255, 255, 255, 127, 127, 127, 127, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
+    normal_calibration_speed: 15
+    index_roll_calibration_speed: 5
+    index_flex_calibration_speed: 10
+    # 19-Tag产品预检仍使用上面保守速度；只有正反预检都留出至少双倍正式分箱余量，
+    # 才把非roll任务正式扫描最多提速1.5倍。四指roll受0.5°回差门限约束，
+    # 始终保持速度5；任一方向采样余量不足也保持原速度。
+    adaptive_formal_speed_enabled: true
+    adaptive_formal_speed_max_scale: 1.5
+    adaptive_formal_speed_minimum_bins: 64
+    adaptive_formal_speed_maximum_bin_gap: 8
+    speed_setting_settle_seconds: 0.25
+
+    # tag36h11尺寸是检测角点围成的黑色正方形边长，不包含外围白边。
+    # 19张Tag的黑色码区外边长均为16 mm。自定义PnP必须与
+    # apriltag_ros逐ID尺寸一致，禁止用纸张/白边尺寸代替码区尺寸。
+    tag_size_m: 0.016
+    # ROS 2无法从YAML空数组推断整数/浮点数组类型。这四个
+    # 末端Tag仍显式写16 mm，防止节点启动时得到未初始化参数。
+    tag_size_override_ids: [7, 14, 16, 18]
+    tag_size_overrides_m: [0.016, 0.016, 0.016, 0.016]
+    repetitions: 3
+    # 19-Tag产品正式零位使用前三轮训练、最后一轮完全留出；旧11-Tag仍读取repetitions=3。
+    g20_right_19_repetitions: 4
+    preflight_frames: 60
+    minimum_detection_rate: 0.95
+    minimum_detection_hz: 15.0
+    minimum_feedback_hz: 25.0
+    maximum_hamming: 0
+    minimum_decision_margin: 30.0
+    minimum_edge_pixels: 30.0
+
+    pnp_maximum_reprojection_error_px: 1.5
+    pnp_reprojection_tie_px: 1.5
+    pnp_maximum_pose_jump_deg: 35.0
+    pnp_maximum_translation_jump_m: 0.04
+    pnp_maximum_tag_tilt_deg: 75.0
+    pnp_tracker_reset_seconds: 5.0
+    # 标定任务不再用第一帧决定平面Tag的IPPE分支；静止端点联合8帧选择整组最稳定解。
+    pnp_group_initialization_frames: 8
+    # 侧面当前任务所需Tag在初始化端点的贴面法向应一致；用此先验消除静态IPPE镜像双解。
+    pnp_group_normal_alignment_scale_deg: 5.0
+    pnp_group_maximum_normal_alignment_deg: 15.0
+    # 三个拇指顶部任务共用预检时冻结的Tag 8位姿。Tag 8仍须实时可见；
+    # 任一角点相对会话基准漂移超过2 px并连续5帧时，判定标定中基准被移动。
+    fixed_base_maximum_corner_drift_px: 2.0
+    fixed_base_movement_confirmation_frames: 5
+    # 仅在拇指MCP/IP同步运动且至少一个候选落入可信区间时，用源URDF mimic
+    # 关系辅助选择IPPE分支；若全部候选超限则退回纯视觉，绝不丢帧，也不生成、
+    # 缩放或替代被动IP的自身Tag实测曲线。
+    thumb_ip_pnp_coupling_multiplier: 1.03
+    thumb_ip_pnp_coupling_scale_deg: 3.0
+    thumb_ip_pnp_maximum_coupling_residual_deg: 7.5
+    top_pnp_invalid_reset_seconds: 1.0
+    # 三维位姿必须与实测20通道状态严格按时间戳配对。
+    maximum_state_image_skew_ms: 50.0
+
+    axis_maximum_plane_rms_m: 0.003
+    # 被动耦合轴只用轨迹确定轴线位置，允许更大的轴向深度噪声；径向和跨轮
+    # 轴线一致性仍沿用严格检查。
+    passive_axis_maximum_plane_rms_m: 0.004
+    axis_maximum_radial_rms_m: 0.003
+    # 整段相对SE(3)运动拟合轴线点；端视关节会投影掉单目PnP光轴深度。
+    axis_maximum_pose_line_rms_m: 0.001
+    # 仅用于运动平面在三维中可观测的斜视关节；近图像平面关节使用姿态轴
+    # 约束三维圆，不让单目平面Tag的深度噪声自由决定转轴方向。
+    axis_maximum_rotation_circle_difference_deg: 1.0
+    # 单轴模型残差与跨轮重复误差分开判定：主动刚性关节要求更严；被动耦合
+    # 关节允许可重复的非理想单轴分量，但仍须通过0.75°跨轮轴差及最终轮留出。
+    active_maximum_rotation_orthogonal_rms_deg: 2.5
+    passive_maximum_rotation_orthogonal_rms_deg: 7.5
+    zero_maximum_axis_cycle_difference_deg: 0.75
+    # 零位无法改变父子轴夹角；超过该值属于CAD/PnP几何错误，不能吸收到零位。
+    zero_maximum_axis_cone_mismatch_deg: 5.0
+    zero_maximum_observability_condition_number: 10000000000.0
+    zero_maximum_offset_deg: 20.0
+    # 四指MCP roll保留严格的装配保护范围。thumb CMC三轴由多轴视觉几何
+    # 求解且不假定电气端点等于CAD上限；thumb_mcp及四指MCP pitch/PIP
+    # 静态零位由实测全行程与CAD机械端点联合求解，不写死为0。
+    zero_finger_maximum_offset_deg: 3.0
+    # 只对实物已确认等同CAD端点的关节使用该限制；CMC电气端点不作此假设。
+    mechanical_endpoint_maximum_offset_deg: 5.0
+
+    endpoint_tolerance_u8: 2.0
+    # 请求命令与固件反馈是两个标定域。稳态检查点允许小幅死区，但反馈
+    # 必须已经稳定；大残差仍由机械卡滞保护处理。
+    steady_checkpoint_command_feedback_tolerance_u8: 8.0
+    steady_checkpoint_maximum_feedback_range_u8: 2.0
+    # 电机10在命令0时实测会稳定反馈为4；该0端使用±4。
+    thumb_yaw_zero_endpoint_tolerance_u8: 4.0
+    # 右手电机10在命令255时多次实测稳定反馈为250；仅右手该端点使用±5。
+    right_thumb_yaw_255_endpoint_tolerance_u8: 5.0
+    # 右手小指PIP电机19在命令0时固件反馈稳定饱和为5；仅其0端使用±5。
+    pinky_pip_zero_endpoint_tolerance_u8: 5.0
+    endpoint_hold_seconds: 0.5
+    # roll零位127必须从两个方向到位并静止采集，禁止用运动中经过127的帧判回差。
+    baseline_hold_seconds: 0.5
+    minimum_baseline_hold_frames: 10
+    # 19-Tag产品每项正式四轮前先做一次低速往返，端点Tag稳定至少2秒。
+    task_precheck_hold_seconds: 2.0
+    position_timeout_seconds: 30.0
+    sweep_timeout_seconds: 90.0
+    # 启动宽限1秒后，反馈连续2秒没有至少1个u8的进展，按机械卡滞立即暂停；
+    # 这类故障不进入遮挡/超时的三次自动重扫。
+    # 低速5也应持续产生反馈进展；5秒无进展即停，减少机构持续顶死时间。
+    motor_stall_timeout_seconds: 2.0
+    motor_stall_startup_grace_seconds: 1.0
+    motor_stall_minimum_progress_u8: 1.0
+    invalid_timeout_seconds: 3.0
+    minimum_sweep_frames: 40
+    minimum_state_span_u8: 240.0
+    minimum_sweep_bins: 32
+    maximum_bin_gap: 16
+    # 可恢复的采样失败自动重扫当前方向；超过次数才暂停等待人工处理。
+    automatic_sweep_retry_limit: 2
+    # 轨迹拟合失败优先只重扫失败轮次；零位/URDF模型失败不重复运动。
+    automatic_fit_retry_limit: 2
+    automatic_motion_retry_limit: 2
+    # 留空为正式标定；设为pinky/ring/middle/index时只采该指正面+侧面roll，
+    # 即使正面baseline回差失败也继续完成侧面对照，并永久锁定本会话URDF发布。
+    cross_view_roll_diagnostic_finger: ""
+    # 过程检查允许25%的黄色预警带，最终验收仍使用下面的严格门限。
+    provisional_warning_ratio: 1.25
+    retry_minimum_speed: 3
+    retry_speed_scales: [0.8, 0.6]
+    retry_endpoint_hold_seconds: [0.75, 1.0]
+
+    trajectory_maximum_plane_rms_m: 0.004
+    trajectory_maximum_radial_rms_m: 0.004
+    trajectory_minimum_radius_m: 0.003
+    trajectory_minimum_arc_deg: 15.0
+    # 以下二维参数只供旧轨迹工具兼容，三机位v4零位不使用二维投影。
+    image_trajectory_maximum_radial_rms_px: 2.0
+    image_trajectory_maximum_radial_p95_px: 3.5
+    image_trajectory_minimum_radius_px: 20.0
+    trajectory_maximum_cycle_travel_difference_deg: 3.0
+    passive_maximum_cycle_travel_difference_deg: 10.0
+    maximum_monotonic_correction_deg: 2.0
+    # 旧布局仍用连续扫描正反程差门限；19-Tag产品的连续运动包含速度相关滞后，
+    # 由方向曲线和最终留出验证建模，不再重复硬判。其绝对正反程门禁使用下面
+    # 的九点稳态command_maximum_direction_gap_deg。
+    maximum_hysteresis_deg: 2.0
+    # 19-Tag产品模式额外要求每轮正反方向在各自baseline处绕实测关节轴的角度差
+    # 不超过0.5°；四指roll例外：127以255→127为唯一物理零位，反向分支
+    # 保留实测偏差，并改为检查分支间隙上限及跨轮稳定性。
+    baseline_maximum_hysteresis_deg: 0.5
+    directional_zero_maximum_branch_gap_deg: 2.0
+    directional_zero_maximum_branch_gap_range_deg: 0.3
+    cross_view_roll_maximum_branch_gap_difference_deg: 0.3
+    # 正面roll是Tag中心的二维投影角，侧面roll是三维姿态角。允许一个有界的
+    # 固定比例吸收Tag安装倾角/偏置带来的投影缩放，再严格比较两条曲线形状；
+    # 比例过大、方向相反、形状RMS及两视角各自的四轮重复性仍会失败。
+    cross_view_roll_maximum_shape_rms_deg: 1.25
+    cross_view_roll_maximum_projection_scale_ratio: 1.5
+    passive_maximum_monotonic_correction_deg: 3.0
+    passive_maximum_hysteresis_deg: 2.0
+    # 九点姿态先按实测反馈重新对齐，再用此门限检查固有机械回差；请求命令域
+    # 的两条运行曲线仍原样保留固件方向死区，不能把command/feedback差算成回差。
+    command_maximum_direction_gap_deg: 2.0
+
+    # 默认无额外随机动作；19-Tag产品最终一轮始终作为不可关闭的留出验证。
+    validation_enabled: false
+    # 第四轮留出求解后必须再走8个固定安全组合姿态；三机位规定Tag全部可见
+    # 且实测20通道到位才允许发布。只保存Tag位姿，不保存原始图像。
+    # Developer diagnostic only. The formal fourth sweep cycle already gives
+    # every isolated PIP/DIP pair an independent holdout.
+    combination_validation_enabled: false
+    combination_validation_frames: 10
+    combination_maximum_position_p95_m: 0.003
+    combination_maximum_orientation_p95_deg: 2.0
+    validation_command_count: 3
+    validation_frames: 10
+    validation_seed: 20260804
+    validation_timeout_seconds: 20.0
+    maximum_validation_mae_deg: 1.0
+    maximum_validation_p95_deg: 2.0
+    # 19-Tag产品模式使用更严格的任一点及静态零偏95%置信区间门限。
+    maximum_validation_error_deg: 3.0
+    zero_maximum_confidence_half_width_deg: 1.5

src/linkerhand_calibration/config/three_camera_tags_g20_right_15.yaml

@@ -0,0 +1,62 @@
+/g20_calibration/front/apriltag/apriltag:
+  ros__parameters:
+    image_transport: raw
+    qos_profile: sensor_data
+    family: 36h11
+    size: 0.016
+    profile: false
+    max_hamming: 0
+    detector:
+      threads: 4
+      decimate: 1.5
+      blur: 0.0
+      refine: true
+      sharpening: 0.25
+      debug: false
+    pose_estimation_method: pnp
+    tag:
+      ids: [0, 1, 2, 3, 10, 11, 12, 13]
+      frames: [front_base, thumb_cmc, thumb_mcp, thumb_ip, pinky_roll, ring_roll, middle_roll, index_roll]
+      sizes: [0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016]
+
+/g20_calibration/side/apriltag/apriltag:
+  ros__parameters:
+    image_transport: raw
+    qos_profile: sensor_data
+    family: 36h11
+    size: 0.016
+    profile: false
+    max_hamming: 0
+    detector:
+      threads: 4
+      decimate: 1.5
+      blur: 0.0
+      refine: true
+      sharpening: 0.25
+      debug: false
+    pose_estimation_method: pnp
+    tag:
+      ids: [4, 5, 6, 15, 17]
+      frames: [side_base, ring_pip, pinky_pip, middle_pip, index_pip]
+      sizes: [0.016, 0.016, 0.016, 0.016, 0.016]
+
+/g20_calibration/top/apriltag/apriltag:
+  ros__parameters:
+    image_transport: raw
+    qos_profile: sensor_data
+    family: 36h11
+    size: 0.016
+    profile: false
+    max_hamming: 0
+    detector:
+      threads: 4
+      decimate: 1.5
+      blur: 0.0
+      refine: true
+      sharpening: 0.25
+      debug: false
+    pose_estimation_method: pnp
+    tag:
+      ids: [8, 9]
+      frames: [top_base, thumb_yaw]
+      sizes: [0.016, 0.016]

src/linkerhand_calibration/config/three_camera_tags_g20_right_19.yaml

@@ -0,0 +1,63 @@
+/g20_calibration/front/apriltag/apriltag:
+  ros__parameters:
+    image_transport: raw
+    qos_profile: sensor_data
+    family: 36h11
+    size: 0.016
+    profile: false
+    max_hamming: 0
+    detector:
+      threads: 4
+      decimate: 1.5
+      blur: 0.0
+      refine: true
+      sharpening: 0.25
+      debug: false
+    pose_estimation_method: pnp
+    tag:
+      ids: [0, 1, 2, 3, 10, 11, 12, 13]
+      frames: [front_base, thumb_cmc, thumb_mcp, thumb_ip, pinky_roll, ring_roll, middle_roll, index_roll]
+      sizes: [0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016]
+
+/g20_calibration/side/apriltag/apriltag:
+  ros__parameters:
+    image_transport: raw
+    qos_profile: sensor_data
+    family: 36h11
+    size: 0.016
+    profile: false
+    max_hamming: 0
+    detector:
+      threads: 4
+      # Distal Tags use the same measured 16 mm black-code edge as all others.
+      decimate: 1.0
+      blur: 0.0
+      refine: true
+      sharpening: 0.25
+      debug: false
+    pose_estimation_method: pnp
+    tag:
+      ids: [4, 5, 6, 7, 14, 15, 16, 17, 18]
+      frames: [side_base, ring_pip, pinky_pip, pinky_dip, ring_dip, middle_pip, middle_dip, index_pip, index_dip]
+      sizes: [0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016]
+
+/g20_calibration/top/apriltag/apriltag:
+  ros__parameters:
+    image_transport: raw
+    qos_profile: sensor_data
+    family: 36h11
+    size: 0.016
+    profile: false
+    max_hamming: 0
+    detector:
+      threads: 4
+      decimate: 1.5
+      blur: 0.0
+      refine: true
+      sharpening: 0.25
+      debug: false
+    pose_estimation_method: pnp
+    tag:
+      ids: [8, 9]
+      frames: [top_base, thumb_yaw]
+      sizes: [0.016, 0.016]

src/linkerhand_calibration/g20_thumb_apriltag_calibration/__init__.py

@@ -0,0 +1,21 @@
+"""One-release compatibility surface for the former Python package name.
+
+New code must import :mod:`linkerhand_calibration`.  Only the documented
+configuration loader is re-exported here; calibration algorithms continue to
+have a single implementation in the renamed package.
+"""
+
+from __future__ import annotations
+
+import warnings
+
+warnings.warn(
+    "g20_thumb_apriltag_calibration is deprecated; "
+    "import linkerhand_calibration instead",
+    DeprecationWarning,
+    stacklevel=2,
+)
+
+from linkerhand_calibration.product import ProductConfig, load_product_config
+
+__all__ = ["ProductConfig", "load_product_config"]

src/linkerhand_calibration/g20_thumb_apriltag_calibration/calibrated_joint_state_bridge.py

@@ -0,0 +1,9 @@
+"""Deprecated forwarding entry point for the runtime joint-state bridge."""
+
+from linkerhand_calibration.calibrated_joint_state_bridge import main
+
+__all__ = ["main"]
+
+
+if __name__ == "__main__":
+    main()

src/linkerhand_calibration/g20_thumb_apriltag_calibration/offline_replay.py

@@ -0,0 +1,9 @@
+"""Deprecated forwarding entry point for offline replay."""
+
+from linkerhand_calibration.offline_replay import main
+
+__all__ = ["main"]
+
+
+if __name__ == "__main__":
+    main()

src/linkerhand_calibration/g20_thumb_apriltag_calibration/one_command.py

@@ -0,0 +1,9 @@
+"""Deprecated forwarding entry point for the former Python package."""
+
+from linkerhand_calibration.one_command import main
+
+__all__ = ["main"]
+
+
+if __name__ == "__main__":
+    main()

src/linkerhand_calibration/launch/calibrated_joint_state_bridge.launch.py

@@ -1,4 +1,4 @@
-"""Publish calibrated G20 URDF joint angles from raw u8 commands."""
+"""Publish profile-calibrated URDF angles from raw command/feedback u8 values."""
 
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument
@@ -14,10 +14,10 @@ def generate_launch_description() -> LaunchDescription:
             DeclareLaunchArgument("input_topic", default_value=""),
             DeclareLaunchArgument("output_topic", default_value=""),
             Node(
-                package="g20_thumb_apriltag_calibration",
+                package="linkerhand_calibration",
                 executable="calibrated_joint_state_bridge",
                 name=[
-                    "g20_calibrated_joint_state_bridge_",
+                    "calibrated_joint_state_bridge_",
                     LaunchConfiguration("hand_type"),
                 ],
                 output="screen",

src/linkerhand_calibration/launch/front_cmc_pitch_zero.launch.py

@@ -48,7 +48,7 @@ def _launch_stack(context):
     tag_config = LaunchConfiguration("tag_config").perform(context)
     zero_config = LaunchConfiguration("zero_config").perform(context)
     camera = Node(
-        package="g20_thumb_apriltag_calibration",
+        package="linkerhand_calibration",
         executable="hikrobot_camera_node",
         name="hikrobot_camera",
         namespace="/camera/camera/color",
@@ -158,7 +158,7 @@ def _launch_stack(context):
     )
 
     zero_node = Node(
-        package="g20_thumb_apriltag_calibration",
+        package="linkerhand_calibration",
         executable="cmc_pitch_zero_node",
         name="g20_thumb_cmc_pitch_zero",
         output="screen",
@@ -203,7 +203,7 @@ def _launch_stack(context):
 
 def generate_launch_description() -> LaunchDescription:
     package_share = Path(
-        get_package_share_directory("g20_thumb_apriltag_calibration")
+        get_package_share_directory("linkerhand_calibration")
     )
     return LaunchDescription(
         [

src/linkerhand_calibration/launch/front_cmc_roll_calibration.launch.py

@@ -50,7 +50,7 @@ def _launch_stack(context):
         "calibration_config"
     ).perform(context)
     camera = Node(
-        package="g20_thumb_apriltag_calibration",
+        package="linkerhand_calibration",
         executable="hikrobot_camera_node",
         name="hikrobot_camera",
         namespace="/camera/camera/color",
@@ -160,7 +160,7 @@ def _launch_stack(context):
     )
 
     calibration_node = Node(
-        package="g20_thumb_apriltag_calibration",
+        package="linkerhand_calibration",
         executable="cmc_roll_calibration_node",
         name="g20_thumb_cmc_roll_calibration",
         output="screen",
@@ -205,7 +205,7 @@ def _launch_stack(context):
 
 def generate_launch_description() -> LaunchDescription:
     package_share = Path(
-        get_package_share_directory("g20_thumb_apriltag_calibration")
+        get_package_share_directory("linkerhand_calibration")
     )
     return LaunchDescription(
         [

src/linkerhand_calibration/launch/front_thumb_calibration.launch.py

@@ -82,7 +82,7 @@ def _launch_stack(context):
             )
 
     camera = Node(
-        package="g20_thumb_apriltag_calibration",
+        package="linkerhand_calibration",
         executable="hikrobot_camera_node",
         name="hikrobot_camera",
         namespace="/camera/camera/color",
@@ -234,7 +234,7 @@ def _launch_stack(context):
     )
 
     calibration = Node(
-        package="g20_thumb_apriltag_calibration",
+        package="linkerhand_calibration",
         executable="calibration_node",
         name="g20_thumb_calibration",
         output="screen",
@@ -315,7 +315,7 @@ def _launch_stack(context):
 
 def generate_launch_description() -> LaunchDescription:
     package_share = Path(
-        get_package_share_directory("g20_thumb_apriltag_calibration")
+        get_package_share_directory("linkerhand_calibration")
     )
     default_output = str(Path.cwd() / "calibration_output")
     return LaunchDescription(

src/linkerhand_calibration/launch/three_camera_calibration.launch.py

@@ -1,8 +1,9 @@
-"""Launch three Hikrobot views and one complete-G20 calibration owner."""
+"""Launch three Hikrobot views and one registered hand calibration owner."""
 
 from __future__ import annotations
 
 from datetime import datetime
+import hashlib
 from pathlib import Path
 import re
 
@@ -25,33 +26,99 @@ from launch_ros.parameter_descriptions import ParameterValue
 VIEWS = ("front", "side", "top")
 
 
-def _default_source_urdf(hand_type: str) -> Path:
+def _default_source_urdf(model: str, hand_type: str) -> Path:
-    relative = Path(
+    relative = (
-        "assets/robots/hands/linker_hand"
+        Path("urdf") / "l6_right" / "linkerhand_l6v3.1_right.urdf"
-    ) / f"g20_{hand_type}" / f"linkerhand_g20_{hand_type}.urdf"
+        if model.upper() == "L6" and hand_type == "right"
-    workspace = Path.cwd() / "src/linkerhand_retarget/linkerhand_retarget" / relative
+        else Path("urdf")
+        / f"{model.lower()}_{hand_type}"
+        / f"linkerhand_{model.lower()}_{hand_type}.urdf"
+    )
+    package_source_or_share = Path(__file__).resolve().parents[1] / relative
     try:
-        installed = Path(get_package_share_directory("linkerhand_retarget")) / relative
+        installed = (
+            Path(get_package_share_directory("linkerhand_calibration"))
+            / relative
+        )
     except Exception:
-        installed = workspace
+        installed = package_source_or_share
-    return workspace if workspace.is_file() else installed
+    return installed if installed.is_file() else package_source_or_share
 
 
 def _launch_stack(context):
+    from linkerhand_calibration.product import (
+        get_product_calibration_contract,
+    )
+
+    model = LaunchConfiguration("model").perform(context).strip().upper()
     hand_type = LaunchConfiguration("hand_type").perform(context).lower()
     if hand_type not in {"left", "right"}:
         raise RuntimeError("hand_type must be left or right")
-    command_topic = f"/g20/cb_{hand_type}_hand_control_cmd"
+    tag_layout = LaunchConfiguration("tag_layout").perform(context).lower()
-    state_topic = f"/g20/cb_{hand_type}_hand_state"
+    try:
-    info_topic = f"/g20/cb_{hand_type}_hand_info"
+        contract = get_product_calibration_contract(
+            model, hand_type, tag_layout
+        )
+    except ValueError as error:
+        raise RuntimeError(str(error)) from error
+    requested_tag_config = LaunchConfiguration("tag_config").perform(context)
+    package_share = Path(
+        get_package_share_directory("linkerhand_calibration")
+    )
+    tag_config = (
+        Path(requested_tag_config).expanduser().resolve()
+        if requested_tag_config
+        else package_share
+        / "config"
+        / (
+            "three_camera_tags_g20_right_19.yaml"
+            if tag_layout == "g20_right_19"
+            else "o12_right_16_tags.yaml"
+            if tag_layout == "o12_right_16"
+            else "o6_right_8_tags.yaml"
+            if tag_layout == "o6_right_8"
+            else "l6_right_8_tags.yaml"
+            if tag_layout == "l6_right_8"
+            else "three_camera_tags_g20_right_15.yaml"
+            if tag_layout == "g20_right_15"
+            else "three_camera_tags.yaml"
+        )
+    )
+    if not tag_config.is_file():
+        raise RuntimeError(f"tag config does not exist: {tag_config}")
+    topic_prefix = f"/{model.lower()}"
+    is_o12 = model == "O12"
+    command_topic = (
+        f"{topic_prefix}/{hand_type}/joint_cmd"
+        if is_o12 else f"{topic_prefix}/cb_{hand_type}_hand_control_cmd"
+    )
+    state_topic = (
+        f"{topic_prefix}/{hand_type}/joint_states"
+        if is_o12 else f"{topic_prefix}/cb_{hand_type}_hand_state"
+    )
+    info_topic = f"{topic_prefix}/cb_{hand_type}_hand_info"
     requested_source = LaunchConfiguration("source_urdf_path").perform(context)
     source_urdf = (
         Path(requested_source).expanduser().resolve()
         if requested_source
-        else _default_source_urdf(hand_type).resolve()
+        else _default_source_urdf(model, hand_type).resolve()
     )
     if not source_urdf.is_file():
         raise RuntimeError(f"source URDF does not exist: {source_urdf}")
+    expected_source_hash = LaunchConfiguration(
+        "source_urdf_expected_sha256"
+    ).perform(context).strip().lower()
+    if contract.typed_profile.artifacts.publish_corrected_urdf:
+        if re.fullmatch(r"[0-9a-f]{64}", expected_source_hash) is None:
+            raise RuntimeError(
+                "this profile requires source_urdf_expected_sha256 confirmed "
+                "by the CAD/hardware owner"
+            )
+        actual_source_hash = hashlib.sha256(source_urdf.read_bytes()).hexdigest()
+        if actual_source_hash != expected_source_hash:
+            raise RuntimeError(
+                "source_urdf_expected_sha256 does not match source_urdf_path"
+            )
 
     hand_serial = LaunchConfiguration("serial_number").perform(context)
     if (
@@ -90,19 +157,20 @@ def _launch_stack(context):
     raw_topics = []
     info_topics = []
     detection_topics = []
+    calibration_namespace = contract.typed_profile.namespace
     for view in VIEWS:
-        namespace = f"/g20_calibration/{view}/camera"
+        namespace = f"{calibration_namespace}/{view}/camera"
         raw_topic = f"{namespace}/image_raw"
         info_topic = f"{namespace}/camera_info"
         rect_topic = f"{namespace}/image_rect"
-        detector_namespace = f"/g20_calibration/{view}/apriltag"
+        detector_namespace = f"{calibration_namespace}/{view}/apriltag"
         detection_topic = f"{detector_namespace}/detections"
         raw_topics.append(raw_topic)
         info_topics.append(info_topic)
         detection_topics.append(detection_topic)
         cameras.append(
             Node(
-                package="g20_thumb_apriltag_calibration",
+                package="linkerhand_calibration",
                 executable="hikrobot_camera_node",
                 name="hikrobot_camera",
                 namespace=namespace,
@@ -118,7 +186,9 @@ def _launch_stack(context):
                         "camera_name": LaunchConfiguration(
                             f"{view}_camera_name"
                         ),
-                        "frame_id": f"g20_calibration_{view}_optical_frame",
+                        "frame_id": (
+                            f"{model.lower()}_calibration_{view}_optical_frame"
+                        ),
                         "image_width": 1624,
                         "image_height": 1240,
                         "frame_rate": ParameterValue(
@@ -163,7 +233,7 @@ def _launch_stack(context):
                     name="apriltag",
                     namespace=detector_namespace,
                     parameters=[
-                        LaunchConfiguration("tag_config"),
+                        str(tag_config),
                         {
                             "detector.decimate": ParameterValue(
                                 LaunchConfiguration("apriltag_decimate"),
@@ -181,7 +251,7 @@ def _launch_stack(context):
         )
 
     vision = ComposableNodeContainer(
-        name="g20_three_camera_vision",
+        name=f"{model.lower()}_three_camera_vision",
         namespace="/",
         package="rclcpp_components",
         executable="component_container_mt",
@@ -189,19 +259,29 @@ def _launch_stack(context):
         output="screen",
         emulate_tty=True,
     )
-    sdk = Node(
+    sdk = (
+        Node(
+            package="omnihand_node",
+            executable="omnihand_pro_2025_node",
+            name="omnihand_pro_2025_node",
+            namespace="o12",
+            output="screen",
+            condition=IfCondition(LaunchConfiguration("start_sdk")),
+            parameters=[LaunchConfiguration("vendor_sdk_config")],
+        )
+        if is_o12
+        else Node(
             package="linker_hand_ros2_sdk",
             executable="linker_hand_sdk",
             name="linker_hand_sdk",
             output="screen",
             condition=IfCondition(LaunchConfiguration("start_sdk")),
-        parameters=[
+            parameters=[{
-            {
                 "hand_type": hand_type,
-                "hand_joint": "G20",
+                "hand_joint": model,
                 "can": LaunchConfiguration("can_interface"),
                 "modbus": "None",
-                "topic_prefix": "/g20",
+                "topic_prefix": topic_prefix,
                 "move_on_startup": False,
                 "startup_speed": ParameterValue(
                     LaunchConfiguration("calibration_speed"), value_type=int
@@ -213,39 +293,66 @@ def _launch_stack(context):
                 # G20 velocity read sends another five synchronous CAN
                 # queries, so keep it off the trajectory-critical path.
                 "velocity_poll_rate": 1.0,
-                # Calibration sends one endpoint command per sweep.  Keep
+                # G20 sends an endpoint and L6 streams a bounded trajectory.
-                # polling the real motor state during the following motion;
+                # Keep polling the real motor state during either command path;
-                # otherwise the SDK republishes a stale state for 0.2 s and
+                # otherwise the SDK republishes stale state and creates large
-                # creates 17-27 command-unit holes in the trajectory bins.
+                # command-unit holes in the trajectory bins.
                 "defer_state_reads_while_commanding": False,
                 "repeat_position_commands": False,
                 "is_touch": False,
-            }
+            }],
-        ],
+        )
     )
     calibration = Node(
-        package="g20_thumb_apriltag_calibration",
+        package="linkerhand_calibration",
         executable="three_camera_calibration_node",
-        name="g20_calibration",
+        name=f"{model.lower()}_calibration",
         output="screen",
         emulate_tty=True,
+        arguments=[
+            "--profile-id",
+            contract.typed_profile.key.profile_id,
+        ],
         parameters=[
             LaunchConfiguration("calibration_config"),
             {
                 "serial_number": hand_serial,
+                "model": model,
                 "hand_type": hand_type,
+                "tag_layout": tag_layout,
                 "session_dir": str(session_dir),
+                "resume_raw_samples_path": LaunchConfiguration(
+                    "resume_raw_samples_path"
+                ),
+                "recalibration_scope": LaunchConfiguration(
+                    "recalibration_scope"
+                ),
                 # The SDK performs roughly 25 synchronous CAN queries whenever
                 # cb_<side>_hand_info has a subscriber. Calibration only used
                 # that topic to display a speed diagnostic, while those reads
                 # created 17-33 command-unit holes in position trajectories.
-                "info_topic": "/g20_calibration/disabled_hand_info",
+                "info_topic": f"{calibration_namespace}/disabled_hand_info",
                 "command_topic": command_topic,
                 "state_topic": state_topic,
                 "camera_extrinsics_file": LaunchConfiguration(
                     "camera_extrinsics_file"
                 ),
                 "source_urdf_path": str(source_urdf),
+                "source_urdf_expected_sha256": LaunchConfiguration(
+                    "source_urdf_expected_sha256"
+                ),
+                "camera_extrinsics_expected_sha256": LaunchConfiguration(
+                    "camera_extrinsics_expected_sha256"
+                ),
+                "calibration_config_expected_sha256": LaunchConfiguration(
+                    "calibration_config_expected_sha256"
+                ),
+                "tag_config_expected_sha256": LaunchConfiguration(
+                    "tag_config_expected_sha256"
+                ),
+                "sdk_config_expected_sha256": LaunchConfiguration(
+                    "sdk_config_expected_sha256"
+                ),
                 "corrected_urdf_output_dir": LaunchConfiguration(
                     "corrected_urdf_output_dir"
                 ),
@@ -267,6 +374,13 @@ def _launch_stack(context):
                     LaunchConfiguration("index_flex_calibration_speed"),
                     value_type=int,
                 ),
+                "adaptive_formal_speed_enabled": ParameterValue(
+                    LaunchConfiguration("adaptive_formal_speed_enabled"),
+                    value_type=bool,
+                ),
+                "cross_view_roll_diagnostic_finger": LaunchConfiguration(
+                    "cross_view_roll_diagnostic_finger"
+                ),
                 "validation_enabled": ParameterValue(
                     LaunchConfiguration("validation_enabled"), value_type=bool
                 ),
@@ -293,14 +407,22 @@ def _launch_stack(context):
             command_topic,
             state_topic,
             info_topic,
-            "/g20_calibration/status",
+            *(
+                [
+                    "/o12/right/joint_control_mode_states",
+                    "/o12/right/joint_error_states",
+                    "/o12/right/joint_temperature_states",
+                ]
+                if is_o12 else []
+            ),
+            f"{calibration_namespace}/status",
         ],
         output="screen",
     )
     return [
         LogInfo(
             msg=(
-                f"G20 {hand_type} three-camera session: {session_dir}; "
+                f"{model} {hand_type} {tag_layout} three-camera session: {session_dir}; "
                 f"source_urdf={source_urdf}"
             )
         ),
@@ -321,7 +443,7 @@ def _launch_stack(context):
 
 def generate_launch_description() -> LaunchDescription:
     package_share = Path(
-        get_package_share_directory("g20_thumb_apriltag_calibration")
+        get_package_share_directory("linkerhand_calibration")
     )
     info_root = Path.home() / ".ros" / "camera_info"
     return LaunchDescription(
@@ -341,7 +463,9 @@ def generate_launch_description() -> LaunchDescription:
                 name="FASTRTPS_DEFAULT_PROFILES_FILE",
                 value=str(package_share / "config" / "fastdds_large_images.xml"),
             ),
+            DeclareLaunchArgument("model", default_value="G20"),
             DeclareLaunchArgument("hand_type", default_value="left"),
+            DeclareLaunchArgument("tag_layout", default_value="legacy_11"),
             DeclareLaunchArgument("serial_number", default_value="UNSET"),
             DeclareLaunchArgument(
                 "front_camera_serial", default_value="DB2163742"
@@ -387,6 +511,12 @@ def generate_launch_description() -> LaunchDescription:
             DeclareLaunchArgument(
                 "index_flex_calibration_speed", default_value="10"
             ),
+            DeclareLaunchArgument(
+                "adaptive_formal_speed_enabled", default_value="true"
+            ),
+            DeclareLaunchArgument(
+                "cross_view_roll_diagnostic_finger", default_value=""
+            ),
             DeclareLaunchArgument("validation_enabled", default_value="false"),
             DeclareLaunchArgument(
                 "camera_extrinsics_file",
@@ -397,6 +527,22 @@ def generate_launch_description() -> LaunchDescription:
             DeclareLaunchArgument(
                 "source_urdf_path", default_value=""
             ),
+            DeclareLaunchArgument(
+                "source_urdf_expected_sha256", default_value=""
+            ),
+            DeclareLaunchArgument(
+                "camera_extrinsics_expected_sha256", default_value=""
+            ),
+            DeclareLaunchArgument(
+                "calibration_config_expected_sha256", default_value=""
+            ),
+            DeclareLaunchArgument(
+                "tag_config_expected_sha256", default_value=""
+            ),
+            DeclareLaunchArgument("vendor_sdk_config", default_value=""),
+            DeclareLaunchArgument(
+                "sdk_config_expected_sha256", default_value=""
+            ),
             DeclareLaunchArgument(
                 "corrected_urdf_output_dir", default_value=""
             ),
@@ -409,6 +555,8 @@ def generate_launch_description() -> LaunchDescription:
                 default_value=str(Path.cwd() / "calibration_output"),
             ),
             DeclareLaunchArgument("session_dir", default_value=""),
+            DeclareLaunchArgument("resume_raw_samples_path", default_value=""),
+            DeclareLaunchArgument("recalibration_scope", default_value="full"),
             DeclareLaunchArgument(
                 "calibration_config",
                 default_value=str(
@@ -417,9 +565,7 @@ def generate_launch_description() -> LaunchDescription:
             ),
             DeclareLaunchArgument(
                 "tag_config",
-                default_value=str(
+                default_value="",
-                    package_share / "config" / "three_camera_tags.yaml"
-                ),
             ),
             OpaqueFunction(function=_launch_stack),
         ]

src/linkerhand_calibration/launch/three_camera_extrinsics.launch.py

@@ -26,7 +26,7 @@ def _launch(context):
         namespace = f"/g20_extrinsics/{view}/camera"
         cameras.append(
             Node(
-                package="g20_thumb_apriltag_calibration",
+                package="linkerhand_calibration",
                 executable="hikrobot_camera_node",
                 name="hikrobot_camera",
                 namespace=namespace,
@@ -85,7 +85,7 @@ def _launch(context):
         output="screen",
     )
     solver = Node(
-        package="g20_thumb_apriltag_calibration",
+        package="linkerhand_calibration",
         executable="three_camera_extrinsics_node",
         name="g20_camera_extrinsics",
         output="screen",
@@ -112,6 +112,12 @@ def _launch(context):
                     LaunchConfiguration("maximum_reprojection_rms_px"),
                     value_type=float,
                 ),
+                "maximum_candidate_pair_reprojection_rms_px": ParameterValue(
+                    LaunchConfiguration(
+                        "maximum_candidate_pair_reprojection_rms_px"
+                    ),
+                    value_type=float,
+                ),
                 "maximum_single_camera_reprojection_rms_px": ParameterValue(
                     LaunchConfiguration(
                         "maximum_single_camera_reprojection_rms_px"
@@ -138,7 +144,7 @@ def _launch(context):
 
 def generate_launch_description() -> LaunchDescription:
     package_share = Path(
-        get_package_share_directory("g20_thumb_apriltag_calibration")
+        get_package_share_directory("linkerhand_calibration")
     )
     camera_info = Path.home() / ".ros" / "camera_info"
     return LaunchDescription(
@@ -182,6 +188,10 @@ def generate_launch_description() -> LaunchDescription:
             DeclareLaunchArgument(
                 "maximum_reprojection_rms_px", default_value="1.2"
             ),
+            DeclareLaunchArgument(
+                "maximum_candidate_pair_reprojection_rms_px",
+                default_value="1.5",
+            ),
             DeclareLaunchArgument(
                 "maximum_single_camera_reprojection_rms_px",
                 default_value="1.5",

src/linkerhand_calibration/linkerhand_calibration/__init__.py

@@ -0,0 +1,5 @@
+"""Profile-driven LinkerHand calibration and validated URDF correction."""
+
+from .core import CalibrationProfile, ProfileKey
+
+__all__ = ["CalibrationProfile", "ProfileKey"]

src/linkerhand_calibration/linkerhand_calibration/acquisition.py

@@ -9,7 +9,7 @@ from typing import Any, Mapping, Sequence
 
 import numpy as np
 
-from .core import PAIR_NAMES, robust_rotation_summary
+from .core import robust_rotation_summary
 from .pnp import SquareTagPose
 
 
@@ -18,6 +18,7 @@ TAG_PAIR_ROLES: dict[str, tuple[str, str]] = {
     "t3_t4": ("t3", "t4"),
     "t4_t5": ("t4", "t5"),
 }
+PAIR_NAMES: tuple[str, ...] = tuple(TAG_PAIR_ROLES)
 
 
 @dataclass(frozen=True)
@@ -116,7 +117,7 @@ def interpolate_state_u8(
     *,
     maximum_skew_ns: int,
 ) -> tuple[tuple[float, ...], int] | None:
-    """Interpolate the 20-D hand state at an image timestamp.
+    """Interpolate a profile-sized hand state at an image timestamp.
 
     The SDK publishes state independently from the camera.  Continuous
     calibration must therefore use the image timestamp instead of whichever
@@ -150,7 +151,11 @@ def interpolate_state_u8(
         fraction = before_gap / denominator
         before_values = np.asarray(before.position_u8, dtype=float)
         after_values = np.asarray(after.position_u8, dtype=float)
-        if before_values.shape != (20,) or after_values.shape != (20,):
+        if (
+            before_values.ndim != 1
+            or before_values.size == 0
+            or after_values.shape != before_values.shape
+        ):
             return None
         interpolated = before_values + fraction * (after_values - before_values)
         return (
@@ -162,11 +167,16 @@ def interpolate_state_u8(
     if nearest is None:
         return None
     gap = abs(int(stamp_ns) - int(nearest.stamp_ns))
-    if gap > maximum_skew_ns or len(nearest.position_u8) != 20:
+    if gap > maximum_skew_ns or not nearest.position_u8:
         return None
     return (tuple(float(value) for value in nearest.position_u8), gap)
 
 
+# Physical-angle profiles use the same timestamp interpolation.  Keep the old
+# public name for compatibility and offer a unit-neutral spelling to new code.
+interpolate_state = interpolate_state_u8
+
+
 class ContinuousSweepCollector:
     """Collect timestamp-synchronised observations during one end-to-end move."""
 

src/linkerhand_calibration/linkerhand_calibration/calibrated_joint_state_bridge.py

@@ -0,0 +1,371 @@
+"""Map model SDK u8 feedback to URDF joint angles using one calibration JSON.
+
+The static encoder-zero corrections in ``zero_angles`` are already baked into
+the corrected URDF joint origins.  This bridge therefore publishes only the
+dynamic ``angle_rad`` values and never adds the static offsets a second time.
+
+Schema-v5 trajectories are fitted against timestamp-synchronised hardware
+feedback, not controller set-points.  They must therefore be queried with the
+SDK ``hand_state`` topic.  The retained schema-v4 path is command-indexed for
+backwards compatibility only.
+"""
+
+from __future__ import annotations
+
+import json
+import math
+from pathlib import Path
+from typing import Any, Mapping, Sequence
+
+import numpy as np
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import JointState
+
+from .full_hand import (
+    get_hand_calibration_profile,
+    infer_compact_payload_layout,
+    validate_compact_payload,
+)
+from .models import get_default_registry, validate_schema_v6_runtime_payload
+from .models.o12.artifacts import validate_o12_runtime_payload
+from .core import ProfileKey
+
+
+G20_COMMAND_NAMES: tuple[str, ...] = (
+    "thumb_cmc_pitch",
+    "index_mcp_pitch",
+    "middle_mcp_pitch",
+    "ring_mcp_pitch",
+    "pinky_mcp_pitch",
+    "thumb_cmc_roll",
+    "index_mcp_roll",
+    "middle_mcp_roll",
+    "ring_mcp_roll",
+    "pinky_mcp_roll",
+    "thumb_cmc_yaw",
+    "reserved_11",
+    "reserved_12",
+    "reserved_13",
+    "reserved_14",
+    "thumb_mcp",
+    "index_pip",
+    "middle_pip",
+    "ring_pip",
+    "pinky_pip",
+)
+
+# Match the stable ordering used by the existing MuJoCo bridge.  JointState
+# consumers must use names, but retaining the ordering also keeps logs and
+# direct comparisons deterministic.
+G20_URDF_JOINT_NAMES: tuple[str, ...] = (
+    "index_dip",
+    "index_mcp_pitch",
+    "index_mcp_roll",
+    "index_pip",
+    "middle_dip",
+    "middle_mcp_pitch",
+    "middle_mcp_roll",
+    "middle_pip",
+    "pinky_dip",
+    "pinky_mcp_pitch",
+    "pinky_mcp_roll",
+    "pinky_pip",
+    "ring_dip",
+    "ring_mcp_pitch",
+    "ring_mcp_roll",
+    "ring_pip",
+    "thumb_cmc_pitch",
+    "thumb_cmc_roll",
+    "thumb_cmc_yaw",
+    "thumb_ip",
+    "thumb_mcp",
+)
+
+
+class CalibratedCommandMapper:
+    """Validated, profile-specific lookup from SDK u8 values to URDF radians."""
+
+    def __init__(
+        self, payload: Mapping[str, Any], *, expected_side: str | None = None
+    ) -> None:
+        schema_version = int(payload["schema_version"])
+        if schema_version == 7:
+            validate_o12_runtime_payload(payload)
+        elif schema_version == 6:
+            validate_schema_v6_runtime_payload(payload)
+        else:
+            validate_compact_payload(payload)
+        side = str(payload["side"]).lower()
+        if expected_side is not None and side != str(expected_side).lower():
+            raise ValueError(
+                f"calibration side {side!r} does not match requested side "
+                f"{str(expected_side).lower()!r}"
+            )
+        quality = payload["quality"]
+        if quality.get("passed") is not True:
+            raise ValueError("calibration quality.passed must be true")
+        layout_id = (
+            str(payload["layout_id"])
+            if schema_version in {6, 7}
+            else infer_compact_payload_layout(payload)
+        )
+        self.side = side
+        self.layout_id = layout_id
+        self.model = str(payload["model"]).upper()
+        self.profile_id = str(
+            payload.get("profile_id", f"G20/{side}/{layout_id}/v1")
+        )
+        self.serial_number = str(payload["serial_number"])
+        self.input_domain = str(
+            payload.get(
+                "curve_input_domain",
+                "command_u8" if schema_version == 4 else "",
+            )
+        )
+        if self.input_domain not in {"command_u8", "feedback_u8", "feedback_rad"}:
+            raise ValueError("calibration curve_input_domain is invalid")
+        if schema_version in {6, 7}:
+            self.command_names = tuple(str(value) for value in payload["command_names"])
+            self.urdf_joint_names = tuple(str(name) for name in payload["joints"])
+            self._motor_by_joint = {
+                name: int(payload["joints"][name]["motor_index"])
+                for name in self.urdf_joint_names
+            }
+            registered = get_default_registry().get(
+                ProfileKey.parse(self.profile_id)
+            )
+            self.feedback_name_aliases = dict(
+                registered.profile.command.feedback_name_aliases
+            )
+            self.feedback_by_index = bool(
+                registered.profile.command.feedback_by_index
+            )
+        else:
+            profile = get_hand_calibration_profile(side, layout_id)
+            self.command_names = G20_COMMAND_NAMES
+            self.urdf_joint_names = G20_URDF_JOINT_NAMES
+            self._motor_by_joint = {
+                name: int(profile.joint_specs[name].motor_index)
+                for name in self.urdf_joint_names
+            }
+            self.feedback_name_aliases = {}
+            self.feedback_by_index = False
+        self._curves = {
+            name: tuple(
+                float(value)
+                for value in payload["joints"][name]["angle_rad"]
+            )
+            for name in self.urdf_joint_names
+        }
+        self._decreasing_curves = {
+            name: tuple(
+                float(value)
+                for value in payload["joints"][name].get(
+                    "decreasing_rad", payload["joints"][name]["angle_rad"]
+                )
+            )
+            for name in self.urdf_joint_names
+        }
+        self._increasing_curves = {
+            name: tuple(
+                float(value)
+                for value in payload["joints"][name].get(
+                    "increasing_rad", payload["joints"][name]["angle_rad"]
+                )
+            )
+            for name in self.urdf_joint_names
+        }
+        self._previous_by_motor: dict[int, float] = {}
+        self._direction_by_motor: dict[int, str] = {}
+        self.direction_deadband_u8 = 0.002 if schema_version == 7 else 0.5
+        self._knots = {
+            name: tuple(float(value) for value in payload["joints"][name].get(
+                "curve_input_knots_rad", ()
+            ))
+            for name in self.urdf_joint_names
+        }
+        self._raw_increasing_branch = {
+            name: str(payload["joints"][name].get(
+                "raw_increasing_curve_branch", "increasing"
+            ))
+            for name in self.urdf_joint_names
+        }
+
+    @staticmethod
+    def _command_index(value: float) -> int:
+        command = float(value)
+        if not math.isfinite(command):
+            raise ValueError("calibrated command positions must be finite")
+        return max(0, min(255, int(math.floor(command + 0.5))))
+
+    def map_positions(
+        self, positions: Sequence[float], names: Sequence[str] = ()
+    ) -> tuple[float, ...]:
+        values = tuple(float(value) for value in positions)
+        if names and not self.feedback_by_index:
+            if len(names) != len(values):
+                raise ValueError(
+                    "JointState names and positions must have equal length"
+                )
+            if len(set(names)) != len(names):
+                raise ValueError("JointState names must be unique")
+            by_name = dict(zip((str(name) for name in names), values))
+            for alias, canonical in self.feedback_name_aliases.items():
+                if alias in by_name and canonical not in by_name:
+                    by_name[canonical] = by_name[alias]
+            missing = [name for name in self.command_names if name not in by_name]
+            if missing:
+                raise ValueError(
+                    f"{self.model} feedback is missing named channels: "
+                    + ",".join(missing)
+                )
+            command = tuple(by_name[name] for name in self.command_names)
+        else:
+            if len(values) != len(self.command_names):
+                raise ValueError(
+                    f"unnamed {self.model} feedback must contain exactly "
+                    f"{len(self.command_names)} positions"
+                )
+            command = values
+        indices = (
+            () if self.input_domain == "feedback_rad"
+            else tuple(self._command_index(value) for value in command)
+        )
+        direction_by_motor: dict[int, str | None] = {}
+        for motor, value in enumerate(command):
+            previous = self._previous_by_motor.get(motor)
+            direction = self._direction_by_motor.get(motor)
+            if previous is not None:
+                if value > previous + self.direction_deadband_u8:
+                    direction = "increasing"
+                elif value < previous - self.direction_deadband_u8:
+                    direction = "decreasing"
+            direction_by_motor[motor] = direction
+        result: list[float] = []
+        for name in self.urdf_joint_names:
+            motor = self._motor_by_joint[name]
+            direction = direction_by_motor[motor]
+            if self.input_domain == "feedback_rad" and direction is not None:
+                raw_increasing = self._raw_increasing_branch[name]
+                direction = (
+                    raw_increasing
+                    if direction == "increasing"
+                    else "increasing" if raw_increasing == "decreasing" else "decreasing"
+                )
+            curves = (
+                self._increasing_curves
+                if direction == "increasing"
+                else self._decreasing_curves
+                if direction == "decreasing"
+                else self._curves
+            )
+            if self.input_domain == "feedback_rad":
+                result.append(float(np.interp(command[motor], self._knots[name], curves[name])))
+            else:
+                result.append(curves[name][indices[motor]])
+        for motor, value in enumerate(command):
+            self._previous_by_motor[motor] = value
+            direction = direction_by_motor[motor]
+            if direction is not None:
+                self._direction_by_motor[motor] = direction
+        return tuple(result)
+
+
+def load_calibrated_command_mapper(
+    calibration_file: str | Path, *, expected_side: str | None = None
+) -> CalibratedCommandMapper:
+    path = Path(calibration_file).expanduser().resolve()
+    if not path.is_file():
+        raise ValueError(f"calibration JSON does not exist: {path}")
+    payload = json.loads(path.read_text(encoding="utf-8"))
+    return CalibratedCommandMapper(payload, expected_side=expected_side)
+
+
+def default_input_topic(
+    hand_type: str, input_domain: str, model: str = "G20"
+) -> str:
+    side = str(hand_type).lower()
+    if side not in {"left", "right"}:
+        raise ValueError("hand_type must be left or right")
+    if input_domain == "feedback_u8":
+        return f"/{str(model).lower()}/cb_{side}_hand_state"
+    if input_domain == "command_u8":
+        return f"/{str(model).lower()}/cb_{side}_hand_control_cmd"
+    if input_domain == "feedback_rad":
+        return f"/{str(model).lower()}/{side}/joint_states"
+    raise ValueError("calibration curve_input_domain is invalid")
+
+
+class CalibratedJointStateBridge(Node):
+    def __init__(self) -> None:
+        super().__init__("calibrated_joint_state_bridge")
+        self.declare_parameter("hand_type", "right")
+        self.declare_parameter("calibration_file", "")
+        self.declare_parameter("input_topic", "")
+        self.declare_parameter("output_topic", "")
+
+        hand_type = str(self.get_parameter("hand_type").value).lower()
+        if hand_type not in {"left", "right"}:
+            raise ValueError("hand_type must be left or right")
+        calibration_file = str(self.get_parameter("calibration_file").value)
+        if not calibration_file:
+            raise ValueError("calibration_file is required")
+        self.mapper = load_calibrated_command_mapper(
+            calibration_file, expected_side=hand_type
+        )
+        input_topic = str(self.get_parameter("input_topic").value).strip()
+        output_topic = str(self.get_parameter("output_topic").value).strip()
+        self.input_topic = input_topic or default_input_topic(
+            hand_type, self.mapper.input_domain, self.mapper.model
+        )
+        self.output_topic = (
+            output_topic
+            or f"/sim/mujoco/{self.mapper.model.lower()}/{hand_type}/joint_state"
+        )
+        self.publisher = self.create_publisher(JointState, self.output_topic, 10)
+        self.subscription = self.create_subscription(
+            JointState, self.input_topic, self._command_callback, 10
+        )
+        self._last_error = ""
+        self.get_logger().info(
+            f"loaded {self.mapper.profile_id} calibration for "
+            f"{self.mapper.serial_number}: "
+            f"{self.input_topic} ({self.mapper.input_domain}) -> "
+            f"{self.output_topic}"
+        )
+
+    def _command_callback(self, command: JointState) -> None:
+        try:
+            positions = self.mapper.map_positions(command.position, command.name)
+        except ValueError as error:
+            message = str(error)
+            if message != self._last_error:
+                self.get_logger().error(message)
+                self._last_error = message
+            return
+        self._last_error = ""
+        result = JointState()
+        result.header = command.header
+        result.name = list(self.mapper.urdf_joint_names)
+        result.position = list(positions)
+        self.publisher.publish(result)
+
+
+def main(args: Sequence[str] | None = None) -> None:
+    rclpy.init(args=args)
+    node: CalibratedJointStateBridge | None = None
+    try:
+        node = CalibratedJointStateBridge()
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        if node is not None:
+            node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

src/linkerhand_calibration/linkerhand_calibration/compat/__init__.py

@@ -0,0 +1,21 @@
+"""Compatibility adapters for one-release calibration migrations."""
+
+from .config_v1 import (
+    legacy_default_profile_key,
+    product_profile_key,
+    resolve_legacy_profile_alias,
+)
+from .defaults import (
+    default_product_config_path,
+    default_three_camera_config_path,
+)
+from .paths import resolve_renamed_package_path
+
+__all__ = [
+    "default_product_config_path",
+    "default_three_camera_config_path",
+    "legacy_default_profile_key",
+    "product_profile_key",
+    "resolve_legacy_profile_alias",
+    "resolve_renamed_package_path",
+]

src/linkerhand_calibration/linkerhand_calibration/compat/config_v1.py

@@ -0,0 +1,47 @@
+"""Identity migration for deployed product configuration schemas."""
+
+from __future__ import annotations
+
+from typing import Any, Mapping
+
+from ..core import ProfileKey
+
+
+def product_profile_key(raw: Mapping[str, Any]) -> ProfileKey:
+    version = int(raw.get("schema_version", -1))
+    if version in {2, 3}:
+        key = ProfileKey.parse(str(raw.get("profile_id", "")))
+        for field, actual in (
+            ("model", key.model),
+            ("side", key.side),
+            ("tag_layout", key.layout),
+        ):
+            configured = str(raw.get(field, "")).strip()
+            if configured and configured.lower() != actual.lower():
+                raise ValueError(f"{field} differs from profile_id")
+        return key
+    if version != 1:
+        raise ValueError("product config schema_version must be 1, 2 or 3")
+    model = str(raw.get("model", "")).strip().upper()
+    side = str(raw.get("side", "")).strip().lower()
+    layout = str(raw.get("tag_layout", "")).strip().lower()
+    if not layout and (model, side) == ("G20", "right"):
+        layout = "g20_right_19"
+    return ProfileKey(model, side, layout, 1)
+
+
+def legacy_default_profile_key() -> ProfileKey:
+    """Preserve the former no-argument executable for one release."""
+    return ProfileKey("G20", "right", "g20_right_19", 1)
+
+
+def resolve_legacy_profile_alias(key: ProfileKey) -> ProfileKey:
+    """Map retired layout identifiers to their reviewed physical profile."""
+    if (
+        key.model == "G20"
+        and key.side == "right"
+        and key.layout == "g20_right_15"
+        and key.revision == 1
+    ):
+        return ProfileKey("G20", "right", "g20_right_19", 1)
+    return key

src/linkerhand_calibration/linkerhand_calibration/compat/defaults.py

@@ -0,0 +1,16 @@
+"""One-release default selection for invocations without ``--config``."""
+
+from pathlib import Path
+
+from ament_index_python.packages import get_package_share_directory
+
+
+def default_product_config_path() -> Path:
+    share = Path(get_package_share_directory("linkerhand_calibration"))
+    return share / "config/g20_right_product.yaml"
+
+
+def default_three_camera_config_path() -> Path:
+    """Resolve the installed calibration defaults through the ROS index."""
+    share = Path(get_package_share_directory("linkerhand_calibration"))
+    return share / "config/three_camera_calibration.yaml"

src/linkerhand_calibration/linkerhand_calibration/compat/legacy/__init__.py

@@ -0,0 +1,4 @@
+"""Legacy single-camera algorithms retained for one compatibility release."""
+from .session_v1 import uses_coupled_full_hand_zero_solver
+
+__all__ = ["uses_coupled_full_hand_zero_solver"]

src/linkerhand_calibration/linkerhand_calibration/compat/legacy/session_v1.py

@@ -0,0 +1,24 @@
+"""Version selection for replaying durable pre-v3 hardware sessions."""
+
+from __future__ import annotations
+
+from typing import Any, Mapping
+
+
+def uses_coupled_full_hand_zero_solver(
+    session_start: Mapping[str, Any],
+) -> bool:
+    """Return the solver contract recorded by the legacy session header.
+
+    Capabilities are not consulted by the live runtime.  This adapter reads
+    the durable v1 header only so offline replay can reproduce an artifact
+    created before the independent thumb solver was introduced.
+    """
+    capabilities = {
+        str(value) for value in session_start.get("capabilities", ())
+    }
+    return (
+        int(session_start.get("sample_schema_version", 1)) == 1
+        and "palm_axis_side_channel_v2" in capabilities
+        and "palm_axis_relative_motion_v3" not in capabilities
+    )

src/linkerhand_calibration/linkerhand_calibration/compat/paths.py

@@ -0,0 +1,52 @@
+"""Path compatibility for immutable v1 product configurations."""
+
+from __future__ import annotations
+
+from pathlib import Path
+
+
+_LEGACY_SOURCE_PREFIX = Path("src/g20_thumb_apriltag_calibration")
+_CURRENT_SOURCE_PREFIX = Path("src/linkerhand_calibration")
+
+
+def _resolve_package_uri(value: str, workspace: Path) -> Path | None:
+    prefix = "package://"
+    if not value.startswith(prefix):
+        return None
+    package_name, separator, relative = value[len(prefix) :].partition("/")
+    if not separator or not package_name or not relative:
+        raise ValueError(f"invalid ROS package resource path: {value}")
+    workspace_candidate = (workspace / "src" / package_name / relative).resolve()
+    if workspace_candidate.exists():
+        return workspace_candidate
+    try:
+        from ament_index_python.packages import get_package_share_directory
+
+        package_share = Path(get_package_share_directory(package_name))
+    except Exception:
+        return workspace_candidate
+    return (package_share / relative).resolve()
+
+
+def resolve_renamed_package_path(value: str | Path, workspace: Path) -> Path:
+    """Resolve workspace paths, ROS package resources and the former prefix.
+
+    Deployed v1 product YAML files are kept byte-for-byte stable because the
+    artifact paths participate in operational review.  Existing paths always
+    win; package URIs prefer a source-workspace copy, and the rename mapping
+    is used only when the literal legacy path no longer exists.
+    """
+    text = str(value).strip()
+    package_resource = _resolve_package_uri(text, workspace)
+    if package_resource is not None:
+        return package_resource
+    raw = Path(text).expanduser()
+    candidate = raw if raw.is_absolute() else workspace / raw
+    candidate = candidate.resolve()
+    if candidate.exists() or raw.is_absolute():
+        return candidate
+    try:
+        suffix = raw.relative_to(_LEGACY_SOURCE_PREFIX)
+    except ValueError:
+        return candidate
+    return (workspace / _CURRENT_SOURCE_PREFIX / suffix).resolve()

src/linkerhand_calibration/linkerhand_calibration/core/__init__.py

@@ -0,0 +1,76 @@
+"""Hardware- and model-independent calibration kernel."""
+
+from .domain import (
+    ArtifactPolicy,
+    CalibrationProfile,
+    CommandLayout,
+    MeasurementPolicy,
+    MeasurementSpec,
+    MotionPolicy,
+    ProfileKey,
+    ProfileValidationError,
+    QualityPolicy,
+    SampleRecord,
+    ScopePolicy,
+    TagSpec,
+    TaskSpec,
+    ViewSpec,
+    VisionRigSpec,
+    ZeroSolvePolicy,
+    validate_profile,
+)
+from .domain.task import (
+    DIRECTION_DECREASING,
+    DIRECTION_INCREASING,
+    DIRECTIONS,
+    PHASE_ROOT,
+    PHASE_TIP,
+)
+from .fitting import FitResult, isotonic_nonincreasing
+from .geometry import (
+    delta_rotation_vector,
+    fit_rotation_axis,
+    image_plane_tag_quaternion_xyzw,
+    normalize_quaternion_xyzw,
+    relative_quaternion_xyzw,
+    robust_rotation_summary,
+    rotation_inlier_fraction,
+    rotation_rms_rad,
+    rotation_spread_rad,
+)
+
+__all__ = [
+    "ArtifactPolicy",
+    "CalibrationProfile",
+    "CommandLayout",
+    "DIRECTION_DECREASING",
+    "DIRECTION_INCREASING",
+    "DIRECTIONS",
+    "FitResult",
+    "MeasurementPolicy",
+    "MeasurementSpec",
+    "MotionPolicy",
+    "PHASE_ROOT",
+    "PHASE_TIP",
+    "ProfileKey",
+    "ProfileValidationError",
+    "QualityPolicy",
+    "SampleRecord",
+    "ScopePolicy",
+    "TagSpec",
+    "TaskSpec",
+    "ViewSpec",
+    "VisionRigSpec",
+    "ZeroSolvePolicy",
+    "delta_rotation_vector",
+    "fit_rotation_axis",
+    "image_plane_tag_quaternion_xyzw",
+    "isotonic_nonincreasing",
+    "normalize_quaternion_xyzw",
+    "relative_quaternion_xyzw",
+    "robust_rotation_summary",
+    "rotation_inlier_fraction",
+    "rotation_rms_rad",
+    "rotation_spread_rad",
+    "validate_profile",
+]

src/linkerhand_calibration/linkerhand_calibration/core/artifacts/__init__.py

@@ -0,0 +1,5 @@
+"""Artifact schema and release validation contracts."""
+
+from .release import ReleaseValidation, ReleaseValidator
+
+__all__ = ["ReleaseValidation", "ReleaseValidator"]

src/linkerhand_calibration/linkerhand_calibration/core/artifacts/release.py

@@ -0,0 +1,27 @@
+"""Release validation protocol used before atomic publication."""
+
+from __future__ import annotations
+
+from dataclasses import dataclass
+from pathlib import Path
+from typing import Mapping, Protocol
+
+from ..domain import CalibrationProfile
+from ..urdf import UrdfCorrectionPlan
+
+
+@dataclass(frozen=True)
+class ReleaseValidation:
+    passed: bool
+    errors: tuple[str, ...] = ()
+    verified_hashes: Mapping[str, str] | None = None
+
+
+class ReleaseValidator(Protocol):
+    def validate_release(
+        self,
+        profile: CalibrationProfile,
+        plan: UrdfCorrectionPlan,
+        calibration_json: Path,
+        corrected_urdf: Path,
+    ) -> ReleaseValidation: ...

src/linkerhand_calibration/linkerhand_calibration/core/domain/__init__.py

@@ -0,0 +1,41 @@
+"""Calibration domain types."""
+
+from .profile import (
+    ArtifactPolicy,
+    CalibrationProfile,
+    CommandLayout,
+    MeasurementPolicy,
+    MeasurementSpec,
+    MotionPolicy,
+    ProfileKey,
+    ProfileValidationError,
+    QualityPolicy,
+    ScopePolicy,
+    TagSpec,
+    TaskSpec,
+    ViewSpec,
+    VisionRigSpec,
+    ZeroSolvePolicy,
+    validate_profile,
+)
+from .sample import SampleRecord
+
+__all__ = [
+    "ArtifactPolicy",
+    "CalibrationProfile",
+    "CommandLayout",
+    "MeasurementPolicy",
+    "MeasurementSpec",
+    "MotionPolicy",
+    "ProfileKey",
+    "ProfileValidationError",
+    "QualityPolicy",
+    "SampleRecord",
+    "ScopePolicy",
+    "TagSpec",
+    "TaskSpec",
+    "ViewSpec",
+    "VisionRigSpec",
+    "ZeroSolvePolicy",
+    "validate_profile",
+]

src/linkerhand_calibration/linkerhand_calibration/core/domain/profile.py

@@ -0,0 +1,475 @@
+"""Typed, hardware-independent calibration profile contracts."""
+
+from __future__ import annotations
+
+from dataclasses import dataclass, field
+import math
+from pathlib import PurePath
+from typing import Mapping
+
+
+@dataclass(frozen=True, order=True)
+class ProfileKey:
+    """Stable identity for one independently reviewed hand profile."""
+
+    model: str
+    side: str
+    layout: str
+    revision: int = 1
+
+    def __post_init__(self) -> None:
+        object.__setattr__(self, "model", str(self.model).strip().upper())
+        object.__setattr__(self, "side", str(self.side).strip().lower())
+        object.__setattr__(self, "layout", str(self.layout).strip().lower())
+        object.__setattr__(self, "revision", int(self.revision))
+        if not self.model or not self.side or not self.layout:
+            raise ValueError("profile identity fields must be non-empty")
+        if self.side not in {"left", "right"}:
+            raise ValueError("profile side must be left or right")
+        if self.revision < 1:
+            raise ValueError("profile revision must be positive")
+
+    @property
+    def profile_id(self) -> str:
+        return f"{self.model}/{self.side}/{self.layout}/v{self.revision}"
+
+    @classmethod
+    def parse(cls, value: str) -> "ProfileKey":
+        parts = str(value).strip().split("/")
+        if len(parts) != 4 or not parts[3].startswith("v"):
+            raise ValueError(
+                "profile_id must be MODEL/side/layout/vREVISION"
+            )
+        return cls(parts[0], parts[1], parts[2], int(parts[3][1:]))
+
+
+@dataclass(frozen=True)
+class CommandLayout:
+    """Command channels, joint bindings, and the reviewed baseline pose."""
+
+    names: tuple[str, ...]
+    baseline_u8: tuple[int, ...]
+    command_index_by_joint: Mapping[str, int]
+    disabled_indices: frozenset[int] = frozenset()
+    # Calibration names are allowed to stay model-neutral while the source
+    # URDF keeps any vendor/side prefixes (for example ``rh_``).
+    urdf_joint_by_joint: Mapping[str, str] = field(default_factory=dict)
+    # Older SDKs occasionally published a wrong label for a physically stable
+    # channel.  Aliases are accepted only at the declared channel index.
+    feedback_name_aliases: Mapping[str, str] = field(default_factory=dict)
+    # SDK speed commands are not necessarily one value per position channel.
+    # This mapping makes that protocol detail explicit in a profile.
+    speed_slot_by_command_index: Mapping[int, int] = field(default_factory=dict)
+    # Physical-angle SDKs use these fields.  ``baseline_u8`` remains the
+    # deployed compatibility contract for legacy byte-command profiles.
+    unit: str = "u8"
+    baseline: tuple[float, ...] = ()
+    lower_bounds: tuple[float, ...] = ()
+    upper_bounds: tuple[float, ...] = ()
+    feedback_by_index: bool = False
+
+    @property
+    def command_count(self) -> int:
+        return len(self.names)
+
+    @property
+    def baseline_values(self) -> tuple[float, ...]:
+        source = self.baseline if self.baseline else self.baseline_u8
+        return tuple(float(value) for value in source)
+
+    @property
+    def minimum_values(self) -> tuple[float, ...]:
+        return (
+            tuple(float(value) for value in self.lower_bounds)
+            if self.lower_bounds
+            else (0.0,) * self.command_count
+        )
+
+    @property
+    def maximum_values(self) -> tuple[float, ...]:
+        return (
+            tuple(float(value) for value in self.upper_bounds)
+            if self.upper_bounds
+            else (255.0,) * self.command_count
+        )
+
+    def normalize(self, index: int, value: float) -> float:
+        lower = self.minimum_values[int(index)]
+        upper = self.maximum_values[int(index)]
+        if not math.isfinite(float(value)) or upper <= lower:
+            raise ValueError("command value or bounds are invalid")
+        return (float(value) - lower) / (upper - lower)
+
+    def denormalize(self, index: int, progress: float) -> float:
+        lower = self.minimum_values[int(index)]
+        upper = self.maximum_values[int(index)]
+        phase = float(progress)
+        if not math.isfinite(phase) or not 0.0 <= phase <= 1.0:
+            raise ValueError("normalized command progress must be in [0, 1]")
+        return lower + phase * (upper - lower)
+
+
+@dataclass(frozen=True)
+class TagSpec:
+    role: str
+    tag_id: int
+    fixed_reference: bool = False
+
+
+@dataclass(frozen=True)
+class ViewSpec:
+    name: str
+    tags: tuple[TagSpec, ...]
+
+
+@dataclass(frozen=True)
+class VisionRigSpec:
+    """Any number of named views and their Tag roles."""
+
+    views: tuple[ViewSpec, ...]
+    common_frame: str
+    extrinsic_reference_view: str
+    extrinsics_quality_limits: Mapping[str, float] = field(
+        default_factory=dict
+    )
+    minimum_capture_counts: Mapping[str, int] = field(default_factory=dict)
+
+    @property
+    def view_names(self) -> tuple[str, ...]:
+        return tuple(view.name for view in self.views)
+
+    @property
+    def tag_ids(self) -> frozenset[int]:
+        return frozenset(tag.tag_id for view in self.views for tag in view.tags)
+
+
+@dataclass(frozen=True)
+class TaskSpec:
+    key: str
+    view: str
+    command_index: int
+    joints: tuple[str, ...]
+    auxiliary_commands: tuple[tuple[int, float], ...] = ()
+    validation_only: bool = False
+    start_u8: int = 255
+    end_u8: int = 0
+    preflight_speed_u8: int | None = None
+    formal_speed_u8: int | None = None
+    start: float | None = None
+    end: float | None = None
+    preflight_speed: float | None = None
+    formal_speed: float | None = None
+
+    @property
+    def start_value(self) -> float:
+        return float(self.start_u8 if self.start is None else self.start)
+
+    @property
+    def end_value(self) -> float:
+        return float(self.end_u8 if self.end is None else self.end)
+
+
+@dataclass(frozen=True)
+class MotionPolicy:
+    """Reviewed motion tasks and optional safe waypoint sequences."""
+
+    tasks: tuple[TaskSpec, ...]
+    preparation_waypoints_u8: tuple[tuple[int, ...], ...] = ()
+    safe_return_waypoints_u8: tuple[tuple[int, ...], ...] = ()
+    speed_parameters: Mapping[str, float] = field(default_factory=dict)
+    precheck_sweeps: bool = False
+    steady_command_checkpoints: bool = False
+
+
+@dataclass(frozen=True)
+class MeasurementSpec:
+    joint: str
+    kind: str
+    view: str | None
+    parent_role: str | None
+    child_role: str | None
+    validation_source: str | None = None
+    # Some measured trajectories publish only a dynamic curve while their
+    # static URDF zero/axis remains CAD- or mimic-owned.  For those joints a
+    # monocular 3-D axis-line residual is useful diagnostic evidence, but it
+    # must not reject an otherwise clean image/SO(3) trajectory merely because
+    # the hand was placed at a different valid position in the camera view.
+    pose_axis_line_required: bool = True
+
+
+@dataclass(frozen=True)
+class MeasurementPolicy:
+    measurements: Mapping[str, MeasurementSpec]
+    cross_view_sources: Mapping[str, str] = field(default_factory=dict)
+    image_curve_joints: frozenset[str] = frozenset()
+    directional_zero: bool = False
+    cross_view_roll_curve: bool = False
+    stable_cross_view_cone_bias: bool = False
+
+
+@dataclass(frozen=True)
+class ZeroSolvePolicy:
+    active_joints: frozenset[str]
+    passive_joints: frozenset[str]
+    direct_zero_joints: tuple[str, ...]
+    axis_joints: tuple[str, ...]
+    mechanical_endpoint_joints: frozenset[str]
+    post_solve_endpoint_joints: frozenset[str]
+    mimic_source_by_joint: Mapping[str, str]
+    cad_frozen_joints: frozenset[str]
+    # ``upper_at_end`` means TaskSpec.end_u8 is the trusted source-URDF upper
+    # physical endpoint.  The measured travel then defines the electrical
+    # zero and corrected [0, travel] coordinate range.
+    endpoint_anchor_by_joint: Mapping[str, str] = field(default_factory=dict)
+    fitted_mimic_joints: frozenset[str] = frozenset()
+    # Passive coupling is not necessarily representable by the linear URDF
+    # ``mimic`` element.  Profiles must opt in explicitly before a nonlinear
+    # runtime/MuJoCo relation may be published.
+    coupling_model_by_joint: Mapping[str, str] = field(default_factory=dict)
+
+
+@dataclass(frozen=True)
+class QualityPolicy:
+    training_cycles: tuple[int, ...]
+    holdout_cycle: int | None
+    hard_threshold_keys: frozenset[str]
+    retry_metric_scope: Mapping[str, str] = field(default_factory=dict)
+    isolated_holdout: bool = False
+
+
+@dataclass(frozen=True)
+class ScopePolicy:
+    calibrate_joints: Mapping[str, frozenset[str]]
+    frozen_joints: Mapping[str, frozenset[str]]
+    default_scope: str = "full"
+
+    def selected_joints(self, scope: str) -> frozenset[str]:
+        try:
+            return self.calibrate_joints[str(scope)]
+        except KeyError as error:
+            raise ValueError(f"unsupported calibration scope: {scope}") from error
+
+
+@dataclass(frozen=True)
+class ArtifactPolicy:
+    output_schema_version: int
+    calibration_filename: str
+    corrected_urdf_filename: str
+    protected_input_fields: frozenset[str]
+    publication_pointer: str = "latest_passed"
+    session_compatibility_tokens: frozenset[str] = frozenset()
+    publish_corrected_urdf: bool = False
+
+
+@dataclass(frozen=True)
+class CalibrationProfile:
+    key: ProfileKey
+    namespace: str
+    command: CommandLayout
+    vision: VisionRigSpec
+    motion: MotionPolicy
+    measurement: MeasurementPolicy
+    zero: ZeroSolvePolicy
+    quality: QualityPolicy
+    scope: ScopePolicy
+    artifacts: ArtifactPolicy
+    # Per-URDF-joint provenance used by partial calibration artifacts.
+    # Known values are: measured_static_dynamic, measured_dynamic_cad_static,
+    # transferred_static_dynamic, transferred_dynamic_cad_static, cad_nominal,
+    # and mimic_nominal.
+    joint_coverage: Mapping[str, str] = field(default_factory=dict)
+
+
+class ProfileValidationError(ValueError):
+    """Raised before hardware startup when a profile is internally unsafe."""
+
+
+def validate_profile(profile: CalibrationProfile) -> None:
+    """Hard-check all cross-policy references before hardware is enabled."""
+    errors: list[str] = []
+    command = profile.command
+    baseline = command.baseline_values
+    if not command.names or len(command.names) != len(baseline):
+        errors.append("command names and baseline must be non-empty and aligned")
+    if len(set(command.names)) != len(command.names):
+        errors.append("command names must be unique")
+    if command.unit not in {"u8", "rad"}:
+        errors.append("command unit must be u8 or rad")
+    if command.unit == "u8" and any(value < 0 or value > 255 for value in baseline):
+        errors.append("baseline command values must be in [0, 255]")
+    if (
+        len(command.minimum_values) != command.command_count
+        or len(command.maximum_values) != command.command_count
+    ):
+        errors.append("command bounds must align with command names")
+    elif any(
+        not math.isfinite(lower)
+        or not math.isfinite(upper)
+        or lower >= upper
+        or not lower <= value <= upper
+        for lower, upper, value in zip(
+            command.minimum_values, command.maximum_values, baseline
+        )
+    ):
+        errors.append("command bounds or baseline values are invalid")
+    indices = set(range(command.command_count))
+    if not set(command.disabled_indices).issubset(indices):
+        errors.append("disabled command index is out of range")
+    if any(index not in indices for index in command.command_index_by_joint.values()):
+        errors.append("joint command index is out of range")
+    if command.urdf_joint_by_joint:
+        if not set(command.command_index_by_joint).issubset(
+            command.urdf_joint_by_joint
+        ):
+            errors.append("every commanded joint must map to a URDF joint")
+        urdf_names = tuple(command.urdf_joint_by_joint.values())
+        if len(set(urdf_names)) != len(urdf_names):
+            errors.append("URDF joint mappings must be unique")
+    if any(
+        index not in indices or slot < 0
+        for index, slot in command.speed_slot_by_command_index.items()
+    ):
+        errors.append("speed-slot mapping is invalid")
+    if any(
+        not str(alias).strip() or canonical not in command.names
+        for alias, canonical in command.feedback_name_aliases.items()
+    ):
+        errors.append("feedback name alias is not part of the command schema")
+
+    view_names = profile.vision.view_names
+    if not view_names or len(set(view_names)) != len(view_names):
+        errors.append("vision views must be non-empty and unique")
+    if profile.vision.extrinsic_reference_view not in view_names:
+        errors.append("extrinsic reference view is not declared")
+    tag_ids = [tag.tag_id for view in profile.vision.views for tag in view.tags]
+    tag_roles = [tag.role for view in profile.vision.views for tag in view.tags]
+    if len(set(tag_ids)) != len(tag_ids):
+        errors.append("Tag IDs must be unique across views")
+    if len(set(tag_roles)) != len(tag_roles):
+        errors.append("Tag roles must be unique across views")
+    if not any(
+        tag.fixed_reference for view in profile.vision.views for tag in view.tags
+    ):
+        errors.append("at least one fixed reference Tag is required")
+
+    task_keys = [task.key for task in profile.motion.tasks]
+    if not task_keys or len(set(task_keys)) != len(task_keys):
+        errors.append("motion task keys must be non-empty and unique")
+    measurement_names = set(profile.measurement.measurements)
+    for task in profile.motion.tasks:
+        if task.view not in view_names:
+            errors.append(f"task {task.key} uses an unknown view")
+        if task.command_index not in indices:
+            errors.append(f"task {task.key} command index is out of range")
+        if not task.joints or not set(task.joints).issubset(measurement_names):
+            errors.append(f"task {task.key} references unknown measurements")
+        if any(index not in indices for index, _ in task.auxiliary_commands):
+            errors.append(f"task {task.key} auxiliary index is out of range")
+        elif any(
+            not command.minimum_values[index] <= float(value) <= command.maximum_values[index]
+            for index, value in task.auxiliary_commands
+        ):
+            errors.append(f"task {task.key} auxiliary command is out of range")
+        lower = command.minimum_values[task.command_index]
+        upper = command.maximum_values[task.command_index]
+        if not lower <= task.start_value <= upper or not lower <= task.end_value <= upper:
+            errors.append(f"task {task.key} sweep endpoint is out of range")
+        if task.start_value == task.end_value:
+            errors.append(f"task {task.key} sweep endpoints must differ")
+        for speed in (task.preflight_speed_u8, task.formal_speed_u8):
+            if speed is not None and not 0 <= speed <= 255:
+                errors.append(f"task {task.key} speed is out of range")
+        if command.unit == "rad" and (
+            task.formal_speed is None or float(task.formal_speed) <= 0.0
+        ):
+            errors.append(f"task {task.key} physical speed must be positive")
+    for name, spec in profile.measurement.measurements.items():
+        if name != spec.joint:
+            errors.append(f"measurement mapping key differs for {name}")
+        if spec.view is not None and spec.view not in view_names:
+            errors.append(f"measurement {name} uses an unknown view")
+    for primary, validation in profile.measurement.cross_view_sources.items():
+        if primary not in measurement_names or validation not in measurement_names:
+            errors.append("cross-view measurement source is unknown")
+
+    zero = profile.zero
+    if zero.active_joints & zero.passive_joints:
+        errors.append("active and passive joints must be disjoint")
+    all_joints = zero.active_joints | zero.passive_joints
+    if not zero.active_joints.issubset(command.command_index_by_joint):
+        errors.append("every active joint must bind to a command channel")
+    if not set(zero.direct_zero_joints).issubset(zero.active_joints):
+        errors.append("direct zero targets must be active joints")
+    if not set(zero.axis_joints).issubset(all_joints):
+        errors.append("axis targets must be known joints")
+    if not zero.mechanical_endpoint_joints.issubset(zero.active_joints):
+        errors.append("mechanical endpoint targets must be active joints")
+    if not zero.post_solve_endpoint_joints.issubset(zero.active_joints):
+        errors.append("post-solve endpoint targets must be active joints")
+    if not set(zero.mimic_source_by_joint).issubset(zero.passive_joints):
+        errors.append("mimic targets must be passive joints")
+    if not set(zero.mimic_source_by_joint.values()).issubset(all_joints):
+        errors.append("mimic sources must be known joints")
+    if not set(zero.endpoint_anchor_by_joint).issubset(zero.active_joints):
+        errors.append("endpoint anchors must target active joints")
+    if not set(zero.endpoint_anchor_by_joint.values()).issubset(
+        {
+            "upper_at_end",
+            "lower_at_start",
+            "zero_at_start",
+            "cad_range_center",
+        }
+    ):
+        errors.append("endpoint anchor policy is unsupported")
+    if not zero.fitted_mimic_joints.issubset(zero.passive_joints):
+        errors.append("fitted mimic targets must be passive joints")
+    if not zero.fitted_mimic_joints.issubset(zero.mimic_source_by_joint):
+        errors.append("fitted mimic target has no source mapping")
+    if not set(zero.coupling_model_by_joint).issubset(
+        zero.mimic_source_by_joint
+    ):
+        errors.append("coupling model target has no source mapping")
+    if not set(zero.coupling_model_by_joint.values()).issubset(
+        {"linear_mimic", "quadratic_runtime"}
+    ):
+        errors.append("coupling model policy is unsupported")
+
+    scopes = set(profile.scope.calibrate_joints)
+    if profile.scope.default_scope not in scopes:
+        errors.append("default scope is not declared")
+    if scopes != set(profile.scope.frozen_joints):
+        errors.append("scope calibration and frozen mappings must align")
+    for name in scopes:
+        selected = profile.scope.calibrate_joints[name]
+        frozen = profile.scope.frozen_joints[name]
+        if selected & frozen or selected | frozen != zero.active_joints:
+            errors.append(f"scope {name} must partition all active joints")
+
+    artifacts = profile.artifacts
+    if artifacts.output_schema_version < 1:
+        errors.append("artifact schema version must be positive")
+    for label, filename in (
+        ("calibration", artifacts.calibration_filename),
+        ("corrected URDF", artifacts.corrected_urdf_filename),
+        ("publication pointer", artifacts.publication_pointer),
+    ):
+        if not filename or PurePath(filename).name != filename:
+            errors.append(f"{label} filename must not contain a directory")
+    if not profile.namespace.startswith("/"):
+        errors.append("runtime namespace must be absolute")
+    if profile.joint_coverage:
+        valid_coverage = {
+            "measured_static_dynamic",
+            "measured_dynamic_cad_static",
+            "transferred_static_dynamic",
+            "transferred_dynamic_cad_static",
+            "cad_nominal",
+            "mimic_nominal",
+        }
+        if set(profile.joint_coverage) != all_joints:
+            errors.append("joint coverage must describe every profile joint")
+        if not set(profile.joint_coverage.values()).issubset(valid_coverage):
+            errors.append("joint coverage contains an unsupported status")
+
+    if errors:
+        raise ProfileValidationError("; ".join(errors))

src/linkerhand_calibration/linkerhand_calibration/core/domain/sample.py

@@ -0,0 +1,39 @@
+"""Normalized records shared by online evaluation and offline replay."""
+
+from __future__ import annotations
+
+from dataclasses import dataclass, field
+import math
+from typing import Any, Mapping
+
+
+@dataclass(frozen=True)
+class SampleRecord:
+    task_key: str
+    measurement: str
+    view: str
+    cycle: int
+    direction: str
+    command_u8: int
+    timestamp_ns: int
+    values: Mapping[str, Any]
+    quality: Mapping[str, float] = field(default_factory=dict)
+    command: float | None = None
+    command_unit: str = "u8"
+    progress_01: float | None = None
+
+    def __post_init__(self) -> None:
+        if not self.task_key or not self.measurement or not self.view:
+            raise ValueError("sample task, measurement, and view are required")
+        if self.cycle < 0:
+            raise ValueError("sample cycle or command is out of range")
+        if self.command_unit == "u8" and not 0 <= self.command_u8 <= 255:
+            raise ValueError("sample cycle or command is out of range")
+        if self.command_unit not in {"u8", "rad"}:
+            raise ValueError("sample command unit must be u8 or rad")
+        if self.command is not None and not math.isfinite(float(self.command)):
+            raise ValueError("sample command must be finite")
+        if self.progress_01 is not None and not 0.0 <= float(self.progress_01) <= 1.0:
+            raise ValueError("sample progress must be in [0, 1]")
+        if self.timestamp_ns < 0:
+            raise ValueError("sample timestamp must be non-negative")

src/linkerhand_calibration/linkerhand_calibration/core/domain/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

src/linkerhand_calibration/linkerhand_calibration/core/domain/task.py

@@ -0,0 +1,19 @@
+"""Shared task direction vocabulary."""
+
+DIRECTION_DECREASING = "decreasing"
+DIRECTION_INCREASING = "increasing"
+DIRECTIONS: tuple[str, ...] = (
+    DIRECTION_DECREASING,
+    DIRECTION_INCREASING,
+)
+
+PHASE_ROOT = "root"
+PHASE_TIP = "tip"
+
+__all__ = [
+    "DIRECTION_DECREASING",
+    "DIRECTION_INCREASING",
+    "DIRECTIONS",
+    "PHASE_ROOT",
+    "PHASE_TIP",
+]

src/linkerhand_calibration/linkerhand_calibration/core/fitting/__init__.py

@@ -0,0 +1,5 @@
+"""Pure curve and axis fitting."""
+
+from .curve import FitResult, isotonic_nonincreasing
+
+__all__ = ["FitResult", "isotonic_nonincreasing"]

src/linkerhand_calibration/linkerhand_calibration/core/fitting/curve.py

@@ -0,0 +1,70 @@
+"""Model-independent curve fitting result and monotonic projection."""
+
+from __future__ import annotations
+
+from dataclasses import dataclass, field
+from typing import Any, Sequence
+
+import numpy as np
+
+from ..geometry import delta_rotation_vector
+
+
+def isotonic_nonincreasing(values: Sequence[float]) -> np.ndarray:
+    """Unweighted PAVA projection onto non-increasing values."""
+    original = np.asarray(values, dtype=float)
+    if original.ndim != 1 or not np.all(np.isfinite(original)):
+        raise ValueError("values must be a finite vector")
+    negated = -original
+    levels: list[float] = []
+    weights: list[int] = []
+    starts: list[int] = []
+    for index, value in enumerate(negated):
+        levels.append(float(value))
+        weights.append(1)
+        starts.append(index)
+        while len(levels) >= 2 and levels[-2] > levels[-1]:
+            total_weight = weights[-2] + weights[-1]
+            merged = (
+                levels[-2] * weights[-2] + levels[-1] * weights[-1]
+            ) / total_weight
+            levels[-2:] = [merged]
+            weights[-2:] = [total_weight]
+            starts.pop()
+    projected = np.empty_like(original)
+    for block_index, (level, start) in enumerate(zip(levels, starts)):
+        end = (
+            starts[block_index + 1]
+            if block_index + 1 < len(starts)
+            else len(original)
+        )
+        projected[start:end] = -level
+    return projected
+
+
+@dataclass(frozen=True)
+class FitResult:
+    joints: dict[str, dict[str, Any]]
+    axes: dict[str, tuple[float, float, float]]
+    references: dict[str, tuple[float, float, float, float]]
+    ip_coupling: dict[str, float]
+    max_monotonic_correction_rad: float
+    max_hysteresis_rad: float
+    measurement_mode: str = "rotation"
+    trajectory_models: dict[str, Any] = field(default_factory=dict)
+    trajectory_quality: dict[str, Any] = field(default_factory=dict)
+
+    def measure_from_reference(
+        self,
+        joint_name: str,
+        observed_quaternion_xyzw: Sequence[float],
+        reference_quaternion_xyzw: Sequence[float] | None = None,
+    ) -> float:
+        reference = (
+            reference_quaternion_xyzw
+            if reference_quaternion_xyzw is not None
+            else self.references[joint_name]
+        )
+        vector = delta_rotation_vector(reference, observed_quaternion_xyzw)
+        axis = np.asarray(self.axes[joint_name], dtype=float)
+        return float(vector @ axis)

src/linkerhand_calibration/linkerhand_calibration/core/geometry/__init__.py

@@ -0,0 +1,41 @@
+"""Pure geometry used by online and offline calibration."""
+
+from .extrinsics import (
+    CameraCalibrationIdentity,
+    CameraExtrinsics,
+    camera_info_fingerprint,
+    load_camera_extrinsics,
+    matrix_payload,
+    transform_matrix,
+    validate_camera_extrinsics_payload,
+)
+from .rotation import (
+    delta_rotation_vector,
+    fit_rotation_axis,
+    image_plane_tag_quaternion_xyzw,
+    normalize_quaternion_xyzw,
+    relative_quaternion_xyzw,
+    robust_rotation_summary,
+    rotation_inlier_fraction,
+    rotation_rms_rad,
+    rotation_spread_rad,
+)
+
+__all__ = [
+    "CameraCalibrationIdentity",
+    "CameraExtrinsics",
+    "camera_info_fingerprint",
+    "delta_rotation_vector",
+    "fit_rotation_axis",
+    "image_plane_tag_quaternion_xyzw",
+    "load_camera_extrinsics",
+    "matrix_payload",
+    "normalize_quaternion_xyzw",
+    "relative_quaternion_xyzw",
+    "robust_rotation_summary",
+    "rotation_inlier_fraction",
+    "rotation_rms_rad",
+    "rotation_spread_rad",
+    "transform_matrix",
+    "validate_camera_extrinsics_payload",
+]

src/linkerhand_calibration/linkerhand_calibration/core/geometry/extrinsics.py

@@ -13,9 +13,6 @@ import yaml
 from scipy.spatial.transform import Rotation
 
 
-VIEWS: tuple[str, ...] = ("front", "side", "top")
-
-
 def camera_info_fingerprint(
     *,
     width: int,
@@ -81,66 +78,61 @@ class CameraCalibrationIdentity:
 
 
 @dataclass(frozen=True)
-class ThreeCameraExtrinsics:
+class CameraExtrinsics:
-    """Transforms points from each camera optical frame into front optical."""
+    """Transforms every declared camera into one Profile-selected reference."""
 
     cameras: Mapping[str, CameraCalibrationIdentity]
-    front_from_view: Mapping[str, np.ndarray]
+    reference_view: str
+    reference_from_view: Mapping[str, np.ndarray]
     quality: Mapping[str, float]
 
     def transform(self, view: str) -> np.ndarray:
-        if view not in self.front_from_view:
+        if view not in self.reference_from_view:
             raise KeyError(f"extrinsics do not contain view {view}")
-        return np.asarray(self.front_from_view[view], dtype=float).copy()
+        return np.asarray(self.reference_from_view[view], dtype=float).copy()
 
-    def camera_matches(
+
-        self,
+def validate_camera_extrinsics_payload(
-        view: str,
+    payload: Mapping[str, Any],
     *,
-        serial_number: str,
+    required_views: Sequence[str],
-        width: int,
+    reference_view: str,
-        height: int,
+    quality_limits: Mapping[str, float] | None = None,
-        intrinsics_sha256: str,
+    minimum_capture_counts: Mapping[str, int] | None = None,
-    ) -> bool:
+) -> None:
-        expected = self.cameras.get(view)
-        return bool(
-            expected is not None
-            and expected.serial_number == str(serial_number)
-            and expected.width == int(width)
-            and expected.height == int(height)
-            and expected.intrinsics_sha256 == str(intrinsics_sha256)
-        )
-
-
-def validate_extrinsics_payload(payload: Mapping[str, Any]) -> None:
     if int(payload.get("schema_version", -1)) != 1:
         raise ValueError("camera extrinsics schema_version must be 1")
-    if payload.get("reference_view") != "front":
+    reference = str(reference_view)
-        raise ValueError("camera extrinsics reference_view must be front")
+    if payload.get("reference_view") != reference:
+        raise ValueError(
+            "camera extrinsics reference_view differs from the Profile"
+        )
     cameras = payload.get("cameras")
-    transforms = payload.get("front_from_view")
+    transforms = payload.get(f"{reference}_from_view")
     quality = payload.get("quality")
-    if not isinstance(cameras, Mapping) or set(cameras) != set(VIEWS):
+    views = tuple(str(view) for view in required_views)
-        raise ValueError("camera extrinsics must contain front/side/top cameras")
+    if not views or len(set(views)) != len(views):
-    if not isinstance(transforms, Mapping) or set(transforms) != set(VIEWS):
+        raise ValueError("required extrinsic views must be non-empty and unique")
-        raise ValueError("camera extrinsics must contain all three transforms")
+    if reference not in views:
+        raise ValueError("extrinsic reference view is not required")
+    if not isinstance(cameras, Mapping) or set(cameras) != set(views):
+        raise ValueError("camera extrinsics differ from the Profile views")
+    if not isinstance(transforms, Mapping) or set(transforms) != set(views):
+        raise ValueError("camera transforms differ from the Profile views")
     if not isinstance(quality, Mapping) or not bool(quality.get("passed")):
         raise ValueError("camera extrinsics quality is not passed")
-    quality_limits = {
+    for key, limit in dict(quality_limits or {}).items():
-        "reprojection_rms_px": 1.2,
-        "maximum_rotation_repeatability_deg": 0.3,
-        "maximum_translation_repeatability_m": 0.0015,
-    }
-    for key, limit in quality_limits.items():
         value = float(quality.get(key, float("inf")))
         if not np.isfinite(value) or value > limit:
             raise ValueError(
                 f"camera extrinsics {key}={value} exceeds {limit}"
             )
-    for key in ("front_side_captures", "front_top_captures"):
+    for key, minimum in dict(minimum_capture_counts or {}).items():
-        if int(quality.get(key, 0)) < 15:
+        if int(quality.get(key, 0)) < int(minimum):
-            raise ValueError(f"camera extrinsics {key} must be at least 15")
+            raise ValueError(
-    for view in VIEWS:
+                f"camera extrinsics {key} must be at least {minimum}"
+            )
+    for view in views:
         identity = cameras[view]
         if not isinstance(identity, Mapping):
             raise ValueError(f"{view} camera identity must be an object")
@@ -158,14 +150,23 @@ def validate_extrinsics_payload(payload: Mapping[str, Any]) -> None:
             transform.get("translation_xyz_m", ()),
             transform.get("quaternion_xyzw", ()),
         )
-        if view == "front" and not np.allclose(matrix, np.eye(4), atol=1.0e-9):
+        if view == reference and not np.allclose(
-            raise ValueError("front_from_view.front must be identity")
+            matrix, np.eye(4), atol=1.0e-9
-    serials = [str(cameras[view]["serial_number"]) for view in VIEWS]
+        ):
-    if len(set(serials)) != len(VIEWS):
+            raise ValueError("reference-view transform must be identity")
+    serials = [str(cameras[view]["serial_number"]) for view in views]
+    if len(set(serials)) != len(views):
         raise ValueError("camera extrinsics serial numbers must be unique")
 
 
-def load_three_camera_extrinsics(path: str | Path) -> ThreeCameraExtrinsics:
+def load_camera_extrinsics(
+    path: str | Path,
+    *,
+    required_views: Sequence[str],
+    reference_view: str,
+    quality_limits: Mapping[str, float] | None = None,
+    minimum_capture_counts: Mapping[str, int] | None = None,
+) -> CameraExtrinsics:
     source = Path(path).expanduser().resolve()
     if not source.is_file():
         raise ValueError(f"camera extrinsics file does not exist: {source}")
@@ -173,7 +174,15 @@ def load_three_camera_extrinsics(path: str | Path) -> ThreeCameraExtrinsics:
         payload = yaml.safe_load(stream)
     if not isinstance(payload, Mapping):
         raise ValueError("camera extrinsics file must contain an object")
-    validate_extrinsics_payload(payload)
+    validate_camera_extrinsics_payload(
+        payload,
+        required_views=required_views,
+        reference_view=reference_view,
+        quality_limits=quality_limits,
+        minimum_capture_counts=minimum_capture_counts,
+    )
+    views = tuple(str(view) for view in required_views)
+    transform_key = f"{reference_view}_from_view"
     cameras = {
         view: CameraCalibrationIdentity(
             serial_number=str(payload["cameras"][view]["serial_number"]),
@@ -183,45 +192,21 @@ def load_three_camera_extrinsics(path: str | Path) -> ThreeCameraExtrinsics:
                 payload["cameras"][view]["intrinsics_sha256"]
             ),
         )
-        for view in VIEWS
+        for view in views
     }
     transforms = {
         view: transform_matrix(
-            payload["front_from_view"][view]["translation_xyz_m"],
+            payload[transform_key][view]["translation_xyz_m"],
-            payload["front_from_view"][view]["quaternion_xyzw"],
+            payload[transform_key][view]["quaternion_xyzw"],
         )
-        for view in VIEWS
+        for view in views
     }
-    return ThreeCameraExtrinsics(
+    return CameraExtrinsics(
         cameras=cameras,
-        front_from_view=transforms,
+        reference_view=str(reference_view),
+        reference_from_view=transforms,
         quality={
             str(key): float(value) if isinstance(value, (int, float)) else value
             for key, value in payload["quality"].items()
         },
     )
-
-
-def dump_three_camera_extrinsics(
-    path: str | Path,
-    *,
-    cameras: Mapping[str, Mapping[str, Any]],
-    front_from_view: Mapping[str, Sequence[Sequence[float]]],
-    quality: Mapping[str, Any],
-) -> None:
-    payload = {
-        "schema_version": 1,
-        "reference_view": "front",
-        "cameras": {view: dict(cameras[view]) for view in VIEWS},
-        "front_from_view": {
-            view: matrix_payload(front_from_view[view]) for view in VIEWS
-        },
-        "quality": dict(quality),
-    }
-    validate_extrinsics_payload(payload)
-    destination = Path(path).expanduser().resolve()
-    destination.parent.mkdir(parents=True, exist_ok=True)
-    temporary = destination.with_suffix(destination.suffix + ".tmp")
-    with temporary.open("w", encoding="utf-8") as stream:
-        yaml.safe_dump(payload, stream, allow_unicode=True, sort_keys=False)
-    temporary.replace(destination)

src/linkerhand_calibration/linkerhand_calibration/core/geometry/pnp.py

@@ -477,6 +477,11 @@ def select_static_rigid_group_initialization(
     relative_translation_scale_m: float,
     normal_alignment_pairs: Sequence[tuple[str, str]] = (),
     normal_alignment_scale_rad: float = math.radians(5.0),
+    task_reference_pairs: Mapping[
+        tuple[str, str], tuple[Rotation, np.ndarray]
+    ] | None = None,
+    task_reference_rotation_scale_rad: float = math.radians(1.0),
+    task_reference_translation_scale_m: float = 0.01,
 ) -> tuple[list[dict[str, SquareTagPose]], dict[str, float | str]]:
     """Select a static multi-Tag IPPE branch path in bounded time.
 
@@ -497,13 +502,18 @@ def select_static_rigid_group_initialization(
     normal_pair_names = tuple(
         (str(first), str(second)) for first, second in normal_alignment_pairs
     )
+    task_references = dict(task_reference_pairs or {})
     if not frames:
         raise ValueError("at least one PnP frame is required")
     if not role_names or len(set(role_names)) != len(role_names):
         raise ValueError("roles must be non-empty and unique")
     if any(
         parent not in role_names or child not in role_names
-        for parent, child in (*pair_names, *normal_pair_names)
+        for parent, child in (
+            *pair_names,
+            *normal_pair_names,
+            *task_references,
+        )
     ):
         raise ValueError("geometry pairs must reference roles")
     scales = (
@@ -513,6 +523,8 @@ def select_static_rigid_group_initialization(
         float(relative_rotation_scale_rad),
         float(relative_translation_scale_m),
         float(normal_alignment_scale_rad),
+        float(task_reference_rotation_scale_rad),
+        float(task_reference_translation_scale_m),
     )
     if min(scales) <= 0.0:
         raise ValueError("static initialization scales must be positive")
@@ -523,6 +535,8 @@ def select_static_rigid_group_initialization(
         relative_rotation_scale,
         relative_translation_scale,
         normal_scale,
+        task_reference_rotation_scale,
+        task_reference_translation_scale,
     ) = scales
 
     combinations_by_frame: list[list[dict[str, SquareTagPose]]] = []
@@ -578,6 +592,16 @@ def select_static_rigid_group_initialization(
                 + float(np.linalg.norm(translation - reference_translation))
                 / relative_translation_scale
             )
+        for pair, (task_rotation, task_translation) in task_references.items():
+            rotation, translation = _relative_pose(
+                combination[pair[0]], combination[pair[1]]
+            )
+            score += (
+                float((task_rotation.inv() * rotation).magnitude())
+                / task_reference_rotation_scale
+                + float(np.linalg.norm(translation - task_translation))
+                / task_reference_translation_scale
+            )
         return float(score)
 
     best_total = float("inf")
@@ -628,6 +652,9 @@ def select_static_rigid_group_initialization(
     quality = dict(quality)
     quality["total_cost"] = float(best_total)
     quality["initialization_search"] = "static_reference"
+    quality["task_reference_used"] = (
+        "true" if task_references else "false"
+    )
     return selected_path, quality
 
 
@@ -886,11 +913,15 @@ class SquareTagPoseTracker:
         self.last_candidates_by_role: dict[
             str, tuple[SquareTagPose, ...]
         ] = {}
+        self.last_candidate_diagnostics_by_role: dict[
+            str, dict[str, float | int]
+        ] = {}
         self.branch_correction_counts: dict[str, int] = {}
 
     def reset(self) -> None:
         self._previous.clear()
         self.last_candidates_by_role.clear()
+        self.last_candidate_diagnostics_by_role.clear()
         self.branch_correction_counts.clear()
 
     def estimate(
@@ -911,28 +942,91 @@ class SquareTagPoseTracker:
             )
         except (ValueError, cv2.error):
             self.last_candidates_by_role[str(role)] = ()
+            self.last_candidate_diagnostics_by_role[str(role)] = {
+                "solved_candidate_count": 0,
+                "reprojection_candidate_count": 0,
+                "independent_tilt_candidate_count": 0,
+                "maximum_reprojection_error_px": float(
+                    self.maximum_reprojection_error_px
+                ),
+                "maximum_independent_tilt_deg": math.degrees(
+                    self.maximum_tag_tilt_rad
+                ),
+            }
             return None, "pnp_solve_failed"
         if not candidates:
             self.last_candidates_by_role[str(role)] = ()
+            self.last_candidate_diagnostics_by_role[str(role)] = {
+                "solved_candidate_count": 0,
+                "reprojection_candidate_count": 0,
+                "independent_tilt_candidate_count": 0,
+                "maximum_reprojection_error_px": float(
+                    self.maximum_reprojection_error_px
+                ),
+                "maximum_independent_tilt_deg": math.degrees(
+                    self.maximum_tag_tilt_rad
+                ),
+            }
             return None, "pnp_solve_failed"
-        usable_candidates: list[SquareTagPose] = []
+        reprojection_candidates = [
-        for candidate in candidates:
+            candidate
+            for candidate in candidates
+            if candidate.reprojection_error_px
+            <= self.maximum_reprojection_error_px
+        ]
+        candidate_tilts_rad: list[float] = []
+        independent_candidates: list[SquareTagPose] = []
+        for candidate in reprojection_candidates:
             normal = Rotation.from_quat(
                 candidate.quaternion_xyzw
             ).as_matrix()[:, 2]
             tilt = math.acos(
                 float(np.clip(abs(normal[2]), 0.0, 1.0))
             )
-            if (
+            candidate_tilts_rad.append(float(tilt))
-                candidate.reprojection_error_px
+            if tilt <= self.maximum_tag_tilt_rad:
-                <= self.maximum_reprojection_error_px
+                independent_candidates.append(candidate)
-                and tilt <= self.maximum_tag_tilt_rad
+
-            ):
+        # Candidate generation and candidate selection have different
-                usable_candidates.append(candidate)
+        # contracts.  The per-Tag tilt limit protects a pose used without any
+        # other geometry, but it must not erase a finite, low-reprojection
+        # IPPE solution before SquareTagGroupPoseTracker can evaluate it
+        # against the fixed palm reference, the articulated chain and the
+        # preceding group pose.  At a strongly oblique view the planar
+        # ambiguity is usually smaller, and rejecting both branches at a
+        # fixed angle caused deterministic mid-sweep holes despite continuous
+        # image detections.  Group tracking therefore receives every
+        # reprojection-valid candidate; independent tracking below retains the
+        # original tilt safety gate.
         self.last_candidates_by_role[str(role)] = tuple(
-            usable_candidates
+            reprojection_candidates
         )
-        if not usable_candidates:
+        diagnostics: dict[str, float | int] = {
+            "solved_candidate_count": len(candidates),
+            "reprojection_candidate_count": len(reprojection_candidates),
+            "independent_tilt_candidate_count": len(independent_candidates),
+            "minimum_reprojection_error_px": float(
+                min(
+                    candidate.reprojection_error_px
+                    for candidate in candidates
+                )
+            ),
+            "maximum_reprojection_error_px": float(
+                self.maximum_reprojection_error_px
+            ),
+            "maximum_independent_tilt_deg": math.degrees(
+                self.maximum_tag_tilt_rad
+            ),
+        }
+        if candidate_tilts_rad:
+            diagnostics["minimum_candidate_tilt_deg"] = math.degrees(
+                min(candidate_tilts_rad)
+            )
+            diagnostics["maximum_candidate_tilt_deg"] = math.degrees(
+                max(candidate_tilts_rad)
+            )
+        self.last_candidate_diagnostics_by_role[str(role)] = diagnostics
+        if not independent_candidates:
             return None, "no_pose_within_reprojection_or_tilt_limit"
 
         previous_record = self._previous.get(str(role))
@@ -944,7 +1038,7 @@ class SquareTagPoseTracker:
                 previous = previous_pose
 
         selected, reason = select_continuous_pose(
-            usable_candidates,
+            independent_candidates,
             previous=previous,
             maximum_reprojection_error_px=(
                 self.maximum_reprojection_error_px
@@ -1013,6 +1107,13 @@ class SquareTagGroupPoseTracker:
         normal_alignment_pairs: Sequence[tuple[str, str]] = (),
         normal_alignment_scale_rad: float = math.radians(5.0),
         maximum_normal_alignment_rad: float | None = None,
+        return_reference_rotation_scale_rad: float = math.radians(1.0),
+        return_reference_maximum_command_gap_u8: int = 8,
+        coupled_rotation_pairs: Sequence[
+            tuple[str, str, str, str, float]
+        ] = (),
+        coupled_rotation_scale_rad: float = math.radians(3.0),
+        maximum_coupled_rotation_residual_rad: float | None = None,
     ) -> None:
         self.roles = tuple(str(role) for role in roles)
         self.adjacent_pairs = tuple(
@@ -1023,6 +1124,22 @@ class SquareTagGroupPoseTracker:
             (str(first), str(second))
             for first, second in normal_alignment_pairs
         )
+        self.coupled_rotation_pairs = tuple(
+            (
+                str(driver_parent),
+                str(driver_child),
+                str(follower_parent),
+                str(follower_child),
+                float(multiplier),
+            )
+            for (
+                driver_parent,
+                driver_child,
+                follower_parent,
+                follower_child,
+                multiplier,
+            ) in coupled_rotation_pairs
+        )
         if not self.roles or len(set(self.roles)) != len(self.roles):
             raise ValueError("roles must be non-empty and unique")
         if any(
@@ -1054,6 +1171,20 @@ class SquareTagGroupPoseTracker:
             if maximum_normal_alignment_rad is None
             else float(maximum_normal_alignment_rad)
         )
+        self.return_reference_rotation_scale_rad = float(
+            return_reference_rotation_scale_rad
+        )
+        self.return_reference_maximum_command_gap_u8 = int(
+            return_reference_maximum_command_gap_u8
+        )
+        self.coupled_rotation_scale_rad = float(
+            coupled_rotation_scale_rad
+        )
+        self.maximum_coupled_rotation_residual_rad = (
+            None
+            if maximum_coupled_rotation_residual_rad is None
+            else float(maximum_coupled_rotation_residual_rad)
+        )
         reset_seconds = float(reset_after_seconds)
         if min(
             self.maximum_pose_jump_rad,
@@ -1062,6 +1193,8 @@ class SquareTagGroupPoseTracker:
             self.relative_translation_scale_m,
             self.reprojection_scale_px,
             self.normal_alignment_scale_rad,
+            self.return_reference_rotation_scale_rad,
+            self.coupled_rotation_scale_rad,
             reset_seconds,
         ) <= 0.0:
             raise ValueError("group tracking scales must be positive")
@@ -1069,11 +1202,33 @@ class SquareTagGroupPoseTracker:
             raise ValueError("reprojection_weight must be non-negative")
         if self.initialization_frames < 1:
             raise ValueError("initialization_frames must be positive")
+        if self.return_reference_maximum_command_gap_u8 < 0:
+            raise ValueError(
+                "return reference maximum command gap must be non-negative"
+            )
         if (
             self.maximum_normal_alignment_rad is not None
             and self.maximum_normal_alignment_rad <= 0.0
         ):
             raise ValueError("maximum normal alignment must be positive")
+        if any(
+            role not in self.roles
+            for coupling in self.coupled_rotation_pairs
+            for role in coupling[:4]
+        ):
+            raise ValueError("coupled rotation pairs must reference roles")
+        if any(
+            multiplier <= 0.0
+            for *_, multiplier in self.coupled_rotation_pairs
+        ):
+            raise ValueError("coupled rotation multipliers must be positive")
+        if (
+            self.maximum_coupled_rotation_residual_rad is not None
+            and self.maximum_coupled_rotation_residual_rad <= 0.0
+        ):
+            raise ValueError(
+                "maximum coupled rotation residual must be positive"
+            )
         self.reset_after_ns = int(reset_seconds * 1_000_000_000)
         self._previous: dict[str, SquareTagPose] = {}
         self._previous_stamp_ns: int | None = None
@@ -1083,28 +1238,214 @@ class SquareTagGroupPoseTracker:
         self._initial_stamps_ns: list[int] = []
         self.last_initialization_quality: dict[str, float | str] = {}
         self.branch_correction_counts: dict[str, int] = {}
+        self._decreasing_relative_rotations: dict[
+            int, dict[tuple[str, str], Rotation]
+        ] = {}
+        self._coupled_reference_rotations: dict[
+            tuple[str, str], Rotation
+        ] = {}
+        self._task_reference_relative_poses: dict[
+            tuple[str, str], tuple[Rotation, np.ndarray]
+        ] = {}
+        self.last_missing_roles: tuple[str, ...] = ()
 
-    def reset(self) -> None:
+    def reset(self, *, preserve_task_reference: bool = False) -> None:
         self._previous.clear()
         self._previous_stamp_ns = None
         self._initial_candidates.clear()
         self._initial_stamps_ns.clear()
         self.last_initialization_quality.clear()
         self.branch_correction_counts.clear()
+        self._decreasing_relative_rotations.clear()
+        self._coupled_reference_rotations.clear()
+        self.last_missing_roles = ()
+        if not preserve_task_reference:
+            self._task_reference_relative_poses.clear()
+
+    def _task_reference_cost(
+        self, combination: Mapping[str, SquareTagPose]
+    ) -> float:
+        residual = 0.0
+        for pair, (expected_rotation, expected_translation) in (
+            self._task_reference_relative_poses.items()
+        ):
+            rotation, translation = _relative_pose(
+                combination[pair[0]], combination[pair[1]]
+            )
+            residual += (
+                float((expected_rotation.inv() * rotation).magnitude())
+                / self.return_reference_rotation_scale_rad
+                + float(np.linalg.norm(translation - expected_translation))
+                / self.relative_translation_scale_m
+            )
+        return residual
+
+    def _coupled_rotation_residuals(
+        self, combination: Mapping[str, SquareTagPose]
+    ) -> tuple[float, ...]:
+        if not self.coupled_rotation_pairs:
+            return ()
+        residuals: list[float] = []
+        for (
+            driver_parent,
+            driver_child,
+            follower_parent,
+            follower_child,
+            multiplier,
+        ) in self.coupled_rotation_pairs:
+            driver_pair = (driver_parent, driver_child)
+            follower_pair = (follower_parent, follower_child)
+            if (
+                driver_pair not in self._coupled_reference_rotations
+                or follower_pair not in self._coupled_reference_rotations
+            ):
+                return ()
+            driver_rotation = _relative_pose(
+                combination[driver_parent], combination[driver_child]
+            )[0]
+            follower_rotation = _relative_pose(
+                combination[follower_parent], combination[follower_child]
+            )[0]
+            driver_travel = (
+                self._coupled_reference_rotations[driver_pair].inv()
+                * driver_rotation
+            ).magnitude()
+            follower_travel = (
+                self._coupled_reference_rotations[follower_pair].inv()
+                * follower_rotation
+            ).magnitude()
+            residuals.append(
+                abs(float(follower_travel) - multiplier * float(driver_travel))
+            )
+        return tuple(residuals)
+
+    def _informative_coupled_rotation_costs(
+        self,
+        combinations: Sequence[Mapping[str, SquareTagPose]],
+    ) -> tuple[float, ...]:
+        """Return branch costs only while the weak coupling prior is credible.
+
+        The URDF mimic ratio is useful for distinguishing two planar-IPPE
+        branches, but it is not measurement truth for a passive joint.  Once
+        every otherwise viable combination disagrees with that ratio, using
+        it would bias the measured curve (and previously rejected every
+        frame).  In that case fall back to visual continuity for this frame.
+        """
+        residuals = tuple(
+            self._coupled_rotation_residuals(combination)
+            for combination in combinations
+        )
+        if not residuals or not any(residuals):
+            return tuple(0.0 for _ in combinations)
+        if (
+            self.maximum_coupled_rotation_residual_rad is not None
+            and not any(
+                values
+                and max(values)
+                <= self.maximum_coupled_rotation_residual_rad
+                for values in residuals
+            )
+        ):
+            return tuple(0.0 for _ in combinations)
+        return tuple(
+            sum(values) / self.coupled_rotation_scale_rad
+            for values in residuals
+        )
+
+    def _return_reference(
+        self, command_u8: int | None
+    ) -> dict[tuple[str, str], tuple[Rotation, np.ndarray | None]]:
+        if command_u8 is None or not self._decreasing_relative_rotations:
+            return {}
+        command = int(command_u8)
+        nearest = min(
+            self._decreasing_relative_rotations,
+            key=lambda candidate: abs(candidate - command),
+        )
+        if (
+            abs(nearest - command)
+            > self.return_reference_maximum_command_gap_u8
+        ):
+            return {}
+        references = self._decreasing_relative_rotations[nearest]
+        commands = sorted(self._decreasing_relative_rotations)
+        axes: dict[tuple[str, str], np.ndarray | None] = {}
+        for pair in self.adjacent_pairs:
+            endpoint_delta = (
+                self._decreasing_relative_rotations[commands[-1]][pair].inv()
+                * self._decreasing_relative_rotations[commands[0]][pair]
+            ).as_rotvec()
+            norm = float(np.linalg.norm(endpoint_delta))
+            axes[pair] = (
+                None
+                if norm < math.radians(5.0)
+                else endpoint_delta / norm
+            )
+        return {
+            pair: (rotation, axes[pair])
+            for pair, rotation in references.items()
+        }
+
+    def _return_reference_cost(
+        self,
+        combination: Mapping[str, SquareTagPose],
+        reference: Mapping[
+            tuple[str, str], tuple[Rotation, np.ndarray | None]
+        ],
+    ) -> float:
+        residual = 0.0
+        for pair, (expected, motion_axis) in reference.items():
+            vector = (
+                expected.inv()
+                * _relative_pose(
+                    combination[pair[0]], combination[pair[1]]
+                )[0]
+            ).as_rotvec()
+            if motion_axis is not None:
+                # The outbound trajectory identifies the physical one-DOF
+                # motion axis.  Do not penalize return travel along that axis:
+                # it may contain real mechanical hysteresis that calibration
+                # must measure.  A planar-IPPE mirror branch appears primarily
+                # as a large orthogonal tilt and is rejected by this residual.
+                vector = vector - motion_axis * float(vector @ motion_axis)
+            residual += float(np.linalg.norm(vector))
+        return residual / self.return_reference_rotation_scale_rad
 
     def select(
         self,
         candidates_by_role: Mapping[str, Sequence[SquareTagPose]],
         *,
         stamp_ns: int,
+        trajectory_command_u8: int | None = None,
+        trajectory_direction: str | None = None,
     ) -> tuple[dict[str, SquareTagPose] | None, str]:
         """Return one mutually consistent pose for every configured role."""
+        direction = (
+            None
+            if trajectory_direction is None
+            else str(trajectory_direction)
+        )
+        if direction not in {None, "decreasing", "increasing"}:
+            raise ValueError(
+                "trajectory_direction must be decreasing or increasing"
+            )
+        return_reference = (
+            self._return_reference(trajectory_command_u8)
+            if direction == "increasing"
+            else {}
+        )
         candidate_lists = [
             tuple(candidates_by_role.get(role, ()))
             for role in self.roles
         ]
-        if any(not candidates for candidates in candidate_lists):
+        self.last_missing_roles = tuple(
+            role
+            for role, candidates in zip(self.roles, candidate_lists)
+            if not candidates
+        )
+        if self.last_missing_roles:
             return None, "group_missing_pose_candidates"
+        self.last_missing_roles = ()
 
         combinations = [
             dict(zip(self.roles, combination))
@@ -1175,6 +1516,15 @@ class SquareTagGroupPoseTracker:
                         normal_alignment_scale_rad=(
                             self.normal_alignment_scale_rad
                         ),
+                        task_reference_pairs=(
+                            self._task_reference_relative_poses
+                        ),
+                        task_reference_rotation_scale_rad=(
+                            self.return_reference_rotation_scale_rad
+                        ),
+                        task_reference_translation_scale_m=(
+                            self.relative_translation_scale_m
+                        ),
                     )
                 )
                 selected = selected_path[-1]
@@ -1195,23 +1545,31 @@ class SquareTagGroupPoseTracker:
                 ):
                     return None, "group_normal_alignment"
             else:
+                coupling_costs = self._informative_coupled_rotation_costs(
+                    combinations
+                )
                 selected = min(
-                    combinations,
+                    zip(combinations, coupling_costs),
-                    key=lambda combination: (
+                    key=lambda item: (
                         sum(
                             pose.reprojection_error_px
-                            for pose in combination.values()
+                            for pose in item[0].values()
                         )
                         / self.reprojection_scale_px
                         + sum(
                             _normal_alignment_rad(
-                                combination[first], combination[second]
+                                item[0][first], item[0][second]
                             )
                             for first, second in self.normal_alignment_pairs
                         )
                         / self.normal_alignment_scale_rad
-                    ),
+                        + self._return_reference_cost(
+                            item[0], return_reference
                         )
+                        + item[1]
+                        + self._task_reference_cost(item[0])
+                    ),
+                )[0]
                 maximum_alignment = max(
                     (
                         _normal_alignment_rad(
@@ -1238,7 +1596,7 @@ class SquareTagGroupPoseTracker:
                 )
                 for pair in self.adjacent_pairs
             }
-            scored: list[tuple[float, dict[str, SquareTagPose]]] = []
+            base_scored: list[tuple[float, dict[str, SquareTagPose]]] = []
             for combination in combinations:
                 absolute_rotation_motion = 0.0
                 absolute_translation_motion = 0.0
@@ -1305,11 +1663,23 @@ class SquareTagGroupPoseTracker:
                     + relative_translation_motion
                     / self.relative_translation_scale_m
                     + self.reprojection_weight * reprojection_penalty
+                    + self._return_reference_cost(
+                        combination, return_reference
                     )
-                scored.append((float(score), combination))
+                )
+                base_scored.append((float(score), combination))
 
-            if not scored:
+            if not base_scored:
                 return None, "group_pose_jump"
+            coupling_costs = self._informative_coupled_rotation_costs(
+                [combination for _, combination in base_scored]
+            )
+            scored = [
+                (base_score + coupling_cost, combination)
+                for (base_score, combination), coupling_cost in zip(
+                    base_scored, coupling_costs
+                )
+            ]
             selected = min(scored, key=lambda item: item[0])[1]
 
         aligned: dict[str, SquareTagPose] = {}
@@ -1343,4 +1713,39 @@ class SquareTagGroupPoseTracker:
 
         self._previous = aligned
         self._previous_stamp_ns = stamp
+        if (
+            direction == "decreasing"
+            and trajectory_command_u8 is not None
+            and not self._coupled_reference_rotations
+        ):
+            for (
+                driver_parent,
+                driver_child,
+                follower_parent,
+                follower_child,
+                _multiplier,
+            ) in self.coupled_rotation_pairs:
+                for pair in (
+                    (driver_parent, driver_child),
+                    (follower_parent, follower_child),
+                ):
+                    self._coupled_reference_rotations[pair] = _relative_pose(
+                        aligned[pair[0]], aligned[pair[1]]
+                    )[0]
+        if direction == "decreasing" and trajectory_command_u8 is not None:
+            self._decreasing_relative_rotations[
+                int(trajectory_command_u8)
+            ] = {
+                pair: _relative_pose(
+                    aligned[pair[0]], aligned[pair[1]]
+                )[0]
+                for pair in self.adjacent_pairs
+            }
+            if not self._task_reference_relative_poses:
+                self._task_reference_relative_poses = {
+                    pair: _relative_pose(
+                        aligned[pair[0]], aligned[pair[1]]
+                    )
+                    for pair in self.adjacent_pairs
+                }
         return dict(aligned), ""

src/linkerhand_calibration/linkerhand_calibration/core/geometry/rotation.py

@@ -0,0 +1,157 @@
+"""Pure quaternion summaries and rotation-axis fitting."""
+
+from __future__ import annotations
+
+import math
+from typing import Sequence
+
+import numpy as np
+from scipy.spatial.transform import Rotation
+
+
+def normalize_quaternion_xyzw(values: Sequence[float]) -> np.ndarray:
+    quaternion = np.asarray(values, dtype=float)
+    if quaternion.shape != (4,) or not np.all(np.isfinite(quaternion)):
+        raise ValueError("quaternion must contain four finite xyzw values")
+    norm = float(np.linalg.norm(quaternion))
+    if norm < 1e-12:
+        raise ValueError("quaternion norm is zero")
+    return quaternion / norm
+
+
+def relative_quaternion_xyzw(
+    parent_camera_quaternion: Sequence[float],
+    child_camera_quaternion: Sequence[float],
+) -> tuple[float, float, float, float]:
+    """Compute parent-to-child orientation from two camera-to-Tag rotations."""
+    parent = Rotation.from_quat(
+        normalize_quaternion_xyzw(parent_camera_quaternion)
+    )
+    child = Rotation.from_quat(
+        normalize_quaternion_xyzw(child_camera_quaternion)
+    )
+    quaternion = (parent.inv() * child).as_quat()
+    return tuple(float(value) for value in quaternion)
+
+
+def image_plane_tag_quaternion_xyzw(
+    corners_xy: Sequence[Sequence[float]],
+) -> tuple[float, float, float, float]:
+    """Estimate Tag orientation about the optical axis from ordered corners."""
+    corners = np.asarray(corners_xy, dtype=float)
+    if corners.shape != (4, 2) or not np.all(np.isfinite(corners)):
+        raise ValueError("corners_xy must contain four finite xy points")
+    x_axis = (corners[1] - corners[0]) + (corners[2] - corners[3])
+    if float(np.linalg.norm(x_axis)) < 1e-9:
+        raise ValueError("tag x-axis is degenerate")
+    angle = -math.atan2(float(x_axis[1]), float(x_axis[0]))
+    quaternion = Rotation.from_rotvec([0.0, 0.0, angle]).as_quat()
+    return tuple(float(value) for value in quaternion)
+
+
+def robust_rotation_summary(
+    quaternions_xyzw: Sequence[Sequence[float]],
+) -> tuple[tuple[float, float, float, float], float]:
+    """Return a robust orientation and maximum angular residual in radians."""
+    if not quaternions_xyzw:
+        raise ValueError("at least one quaternion is required")
+    rotations = Rotation.from_quat(
+        np.asarray(
+            [normalize_quaternion_xyzw(value) for value in quaternions_xyzw],
+            dtype=float,
+        )
+    )
+    reference = rotations[0]
+    delta_vectors = (reference.inv() * rotations).as_rotvec()
+    median_delta = np.median(delta_vectors, axis=0)
+    robust = reference * Rotation.from_rotvec(median_delta)
+    residuals = (robust.inv() * rotations).magnitude()
+    maximum = float(np.max(residuals)) if residuals.size else 0.0
+    return tuple(float(value) for value in robust.as_quat()), maximum
+
+
+def rotation_spread_rad(
+    quaternions_xyzw: Sequence[Sequence[float]],
+) -> float:
+    """Return the maximum geodesic residual around a robust orientation."""
+    _, spread = robust_rotation_summary(quaternions_xyzw)
+    return spread
+
+
+def rotation_rms_rad(
+    quaternions_xyzw: Sequence[Sequence[float]],
+    *,
+    outlier_threshold_rad: float | None = None,
+) -> float:
+    """Return RMS geodesic noise around a robust orientation."""
+    robust, _ = robust_rotation_summary(quaternions_xyzw)
+    reference = Rotation.from_quat(robust)
+    rotations = Rotation.from_quat(
+        np.asarray(
+            [normalize_quaternion_xyzw(value) for value in quaternions_xyzw],
+            dtype=float,
+        )
+    )
+    residuals = (reference.inv() * rotations).magnitude()
+    if outlier_threshold_rad is not None:
+        threshold = float(outlier_threshold_rad)
+        if threshold <= 0.0:
+            raise ValueError("outlier_threshold_rad must be positive")
+        residuals = residuals[residuals <= threshold]
+        if residuals.size == 0:
+            return float("inf")
+    return float(np.sqrt(np.mean(np.square(residuals))))
+
+
+def rotation_inlier_fraction(
+    quaternions_xyzw: Sequence[Sequence[float]],
+    *,
+    outlier_threshold_rad: float,
+) -> float:
+    """Return the fraction close to the robust orientation."""
+    threshold = float(outlier_threshold_rad)
+    if threshold <= 0.0:
+        raise ValueError("outlier_threshold_rad must be positive")
+    robust, _ = robust_rotation_summary(quaternions_xyzw)
+    reference = Rotation.from_quat(robust)
+    rotations = Rotation.from_quat(
+        np.asarray(
+            [normalize_quaternion_xyzw(value) for value in quaternions_xyzw],
+            dtype=float,
+        )
+    )
+    residuals = (reference.inv() * rotations).magnitude()
+    return float(np.mean(residuals <= threshold))
+
+
+def delta_rotation_vector(
+    reference_xyzw: Sequence[float],
+    observed_xyzw: Sequence[float],
+) -> np.ndarray:
+    reference = Rotation.from_quat(normalize_quaternion_xyzw(reference_xyzw))
+    observed = Rotation.from_quat(normalize_quaternion_xyzw(observed_xyzw))
+    return (reference.inv() * observed).as_rotvec()
+
+
+def fit_rotation_axis(
+    vectors: Sequence[Sequence[float]],
+    commands: Sequence[int],
+) -> np.ndarray:
+    """Fit and orient the single rotational axis used by one command sweep."""
+    matrix = np.asarray(vectors, dtype=float)
+    command_values = np.asarray(commands, dtype=int)
+    if matrix.ndim != 2 or matrix.shape[1] != 3:
+        raise ValueError("vectors must have shape (N, 3)")
+    if command_values.shape != (matrix.shape[0],):
+        raise ValueError("commands must match vectors")
+    useful = np.linalg.norm(matrix, axis=1) > 1e-6
+    if int(np.count_nonzero(useful)) < 3:
+        raise ValueError("insufficient non-zero rotations to fit an axis")
+    _, _, vh = np.linalg.svd(matrix[useful], full_matrices=False)
+    axis = vh[0]
+    projections = matrix @ axis
+    low = projections[command_values <= 16]
+    high = projections[command_values >= 239]
+    if low.size and high.size and float(np.median(low)) < float(np.median(high)):
+        axis = -axis
+    return axis / np.linalg.norm(axis)

src/linkerhand_calibration/linkerhand_calibration/core/solver/__init__.py

@@ -0,0 +1,15 @@
+"""Task acceptance and final-session solver contracts."""
+
+from .interfaces import (
+    SessionSolution,
+    SessionSolver,
+    TaskEvaluation,
+    TaskEvaluator,
+)
+
+__all__ = [
+    "SessionSolution",
+    "SessionSolver",
+    "TaskEvaluation",
+    "TaskEvaluator",
+]

src/linkerhand_calibration/linkerhand_calibration/core/solver/interfaces.py

@@ -0,0 +1,42 @@
+"""Shared evaluator and final-solver interfaces."""
+
+from __future__ import annotations
+
+from dataclasses import dataclass, field
+from typing import Any, Mapping, Protocol, Sequence
+
+from ..domain import CalibrationProfile, SampleRecord, TaskSpec
+
+
+@dataclass(frozen=True)
+class TaskEvaluation:
+    accepted: bool
+    failures: tuple[Mapping[str, Any], ...] = ()
+    rescan_measurements: frozenset[str] = frozenset()
+    rescan_cycles: frozenset[int] = frozenset()
+
+
+@dataclass(frozen=True)
+class SessionSolution:
+    passed: bool
+    calibration: Mapping[str, Any]
+    zero_offsets_rad: Mapping[str, float]
+    failures: tuple[Mapping[str, Any], ...] = ()
+    diagnostics: Mapping[str, Any] = field(default_factory=dict)
+
+
+class TaskEvaluator(Protocol):
+    def evaluate_task(
+        self,
+        profile: CalibrationProfile,
+        task: TaskSpec,
+        samples: Sequence[SampleRecord],
+    ) -> TaskEvaluation: ...
+
+
+class SessionSolver(Protocol):
+    def solve_session(
+        self,
+        profile: CalibrationProfile,
+        samples: Sequence[SampleRecord],
+    ) -> SessionSolution: ...

src/linkerhand_calibration/linkerhand_calibration/core/urdf/__init__.py

@@ -0,0 +1,22 @@
+"""URDF correction authorization and validation types."""
+
+from .plan import UrdfCorrectionPlan, build_correction_plan
+from .patch import (
+    MujocoEqualityPatch,
+    UrdfJointPatch,
+    UrdfPatchSet,
+    apply_urdf_patch_text,
+    materialize_relative_mesh_assets,
+    write_urdf_patches,
+)
+
+__all__ = [
+    "MujocoEqualityPatch",
+    "UrdfCorrectionPlan",
+    "UrdfJointPatch",
+    "UrdfPatchSet",
+    "apply_urdf_patch_text",
+    "build_correction_plan",
+    "materialize_relative_mesh_assets",
+    "write_urdf_patches",
+]

src/linkerhand_calibration/linkerhand_calibration/core/urdf/patch.py

@@ -0,0 +1,315 @@
+"""Byte-preserving, declarative URDF patch application.
+
+Model profiles decide *what* values are authorized.  This module owns the
+shared mechanics of locating those fields in the original XML text, changing
+only the declared attributes, materializing mesh resources and atomically
+publishing a new file.
+"""
+
+from __future__ import annotations
+
+from dataclasses import dataclass, field
+import os
+from pathlib import Path
+import re
+import shutil
+from typing import Mapping, Sequence
+import xml.etree.ElementTree as ET
+
+
+@dataclass(frozen=True)
+class UrdfJointPatch:
+    """Authorized attribute replacements inside one top-level URDF joint."""
+
+    origin_rpy: str | None = None
+    limit_lower: str | None = None
+    limit_upper: str | None = None
+    mimic_multiplier: str | None = None
+    mimic_offset: str | None = None
+
+    def replacements(self) -> tuple[tuple[str, str, str], ...]:
+        values = (
+            ("origin", "rpy", self.origin_rpy),
+            ("limit", "lower", self.limit_lower),
+            ("limit", "upper", self.limit_upper),
+            ("mimic", "multiplier", self.mimic_multiplier),
+            ("mimic", "offset", self.mimic_offset),
+        )
+        return tuple(
+            (element, attribute, str(value))
+            for element, attribute, value in values
+            if value is not None
+        )
+
+
+
+@dataclass(frozen=True)
+class MujocoEqualityPatch:
+    """Replacement and optional topology assertion for one equality joint."""
+
+    polycoef: str
+    expected_joint1: str | None = None
+    expected_joint2: str | None = None
+
+
+@dataclass(frozen=True)
+class UrdfPatchSet:
+    """Complete declarative edit set for one generated URDF."""
+
+    joints: Mapping[str, UrdfJointPatch]
+    mujoco_equalities: Mapping[str, MujocoEqualityPatch] = field(
+        default_factory=dict
+    )
+
+    def __post_init__(self) -> None:
+        empty = [
+            name for name, patch in self.joints.items() if not patch.replacements()
+        ]
+        if empty:
+            raise ValueError(
+                "URDF joint patch contains no replacements: "
+                + ",".join(sorted(empty))
+            )
+
+
+def _replace_attribute(
+    block: str, element: str, attribute: str, value: str
+) -> str:
+    pattern = re.compile(
+        rf"(<{element}\b[^>]*\b{attribute}\s*=\s*)([\"'])"
+        rf"(?P<value>[^\"']*)\2",
+        re.DOTALL,
+    )
+    match = pattern.search(block)
+    if match is None:
+        raise ValueError(f"{element} has no {attribute} attribute")
+    start, end = match.span("value")
+    return block[:start] + str(value) + block[end:]
+
+
+def apply_urdf_patch_text(
+    original_text: str,
+    root: ET.Element,
+    patches: UrdfPatchSet,
+) -> str:
+    """Apply declared patches without serializing unaffected XML."""
+    top_level_joints = {
+        str(joint.get("name")): joint for joint in root.findall("joint")
+    }
+    missing_joints = set(patches.joints) - set(top_level_joints)
+    if missing_joints:
+        raise ValueError(
+            "source URDF is missing target joints: "
+            + ",".join(sorted(missing_joints))
+        )
+
+    equality_nodes = {
+        str(joint.get("name")): joint
+        for joint in root.findall("./mujoco/equality/joint")
+    }
+    missing_equalities = set(patches.mujoco_equalities) - set(equality_nodes)
+    if missing_equalities:
+        raise ValueError(
+            "source URDF is missing MuJoCo equalities: "
+            + ",".join(sorted(missing_equalities))
+        )
+    for name, patch in patches.mujoco_equalities.items():
+        node = equality_nodes[name]
+        if (
+            patch.expected_joint1 is not None
+            and node.get("joint1") != patch.expected_joint1
+        ):
+            raise ValueError(f"MuJoCo equality joint1 differs for {name}")
+        if (
+            patch.expected_joint2 is not None
+            and node.get("joint2") != patch.expected_joint2
+        ):
+            raise ValueError(f"MuJoCo equality joint2 differs for {name}")
+
+    # Requiring the URDF ``type`` attribute excludes transmission and MuJoCo
+    # elements which also use the tag name ``joint``.
+    joint_pattern = re.compile(
+        r"<joint\b(?=[^>]*\btype\s*=)[^>]*\bname\s*=\s*"
+        r"([\"'])(?P<name>[^\"']+)\1[^>]*>"
+        r".*?</joint>",
+        re.DOTALL,
+    )
+    applied_joints: set[str] = set()
+
+    def replace_joint(match: re.Match[str]) -> str:
+        name = match.group("name")
+        patch = patches.joints.get(name)
+        if patch is None:
+            return match.group(0)
+        if name in applied_joints:
+            raise ValueError(f"duplicate top-level URDF joint text: {name}")
+        block = match.group(0)
+        for element, attribute, value in patch.replacements():
+            block = _replace_attribute(block, element, attribute, value)
+        applied_joints.add(name)
+        return block
+
+    corrected = joint_pattern.sub(replace_joint, original_text)
+    if applied_joints != set(patches.joints):
+        missing = set(patches.joints) - applied_joints
+        raise ValueError(
+            "could not locate every target joint in source URDF text: "
+            + ",".join(sorted(missing))
+        )
+
+    applied_equalities: set[str] = set()
+    for name, patch in patches.mujoco_equalities.items():
+        equality_pattern = re.compile(
+            rf"(<joint\b[^>]*\bname\s*=\s*([\"']))"
+            rf"{re.escape(name)}\2[^>]*>",
+            re.DOTALL,
+        )
+        matches = list(equality_pattern.finditer(corrected))
+        if len(matches) != 1:
+            raise ValueError(f"could not uniquely locate MuJoCo equality {name}")
+        match = matches[0]
+        replacement = _replace_attribute(
+            match.group(0), "joint", "polycoef", patch.polycoef
+        )
+        corrected = corrected[: match.start()] + replacement + corrected[match.end() :]
+        applied_equalities.add(name)
+    if applied_equalities != set(patches.mujoco_equalities):
+        raise ValueError("could not apply every MuJoCo equality patch")
+    return corrected
+
+
+def _files_have_identical_contents(left: Path, right: Path) -> bool:
+    if left.stat().st_size != right.stat().st_size:
+        return False
+    with left.open("rb") as left_stream, right.open("rb") as right_stream:
+        while True:
+            left_chunk = left_stream.read(1024 * 1024)
+            right_chunk = right_stream.read(1024 * 1024)
+            if left_chunk != right_chunk:
+                return False
+            if not left_chunk:
+                return True
+
+
+def materialize_relative_mesh_assets(
+    *, source: Path, output: Path, urdf_root: ET.Element
+) -> tuple[Path, ...]:
+    """Copy safe relative mesh resources beside the generated URDF."""
+    filenames = sorted(
+        {
+            str(mesh.get("filename", "")).strip()
+            for mesh in urdf_root.findall(".//mesh")
+            if str(mesh.get("filename", "")).strip()
+        }
+    )
+    materialized: list[Path] = []
+    for filename in filenames:
+        if "://" in filename or filename.startswith("package:"):
+            continue
+        relative = Path(filename)
+        if relative.is_absolute() or ".." in relative.parts:
+            raise ValueError(
+                f"URDF mesh path must be a safe relative path or URI: {filename}"
+            )
+        source_asset = (source.parent / relative).resolve()
+        if not source_asset.is_file():
+            raise ValueError(f"URDF mesh resource does not exist: {source_asset}")
+        destination_asset = (output / relative).resolve()
+        try:
+            destination_asset.relative_to(output)
+        except ValueError as error:
+            raise ValueError(
+                f"URDF mesh destination escapes output directory: {filename}"
+            ) from error
+        if destination_asset == source_asset:
+            materialized.append(destination_asset)
+            continue
+        destination_asset.parent.mkdir(parents=True, exist_ok=True)
+        if destination_asset.exists():
+            if not destination_asset.is_file() or not _files_have_identical_contents(
+                source_asset, destination_asset
+            ):
+                raise ValueError(
+                    "refusing to overwrite a different mesh resource: "
+                    f"{destination_asset}"
+                )
+            materialized.append(destination_asset)
+            continue
+        temporary_asset = destination_asset.with_name(
+            f".{destination_asset.name}.{os.getpid()}.tmp"
+        )
+        if temporary_asset.exists():
+            raise ValueError(f"temporary mesh path is occupied: {temporary_asset}")
+        try:
+            shutil.copy2(source_asset, temporary_asset)
+            os.replace(temporary_asset, destination_asset)
+        finally:
+            if temporary_asset.exists():
+                temporary_asset.unlink()
+        materialized.append(destination_asset)
+    return tuple(materialized)
+
+
+def _copy_complete_mesh_directory(source: Path, output: Path) -> None:
+    source_meshes = source.parent / "meshes"
+    if not source_meshes.is_dir():
+        return
+    destination_meshes = output / "meshes"
+    destination_meshes.mkdir(parents=True, exist_ok=True)
+    for mesh in source_meshes.iterdir():
+        if mesh.is_file():
+            shutil.copy2(mesh, destination_meshes / mesh.name)
+
+
+def write_urdf_patches(
+    *,
+    source_urdf: str | Path,
+    destination_urdf: str | Path,
+    patches: UrdfPatchSet,
+    forbidden_source_stem_patterns: Sequence[str] = (),
+    copy_complete_mesh_directory: bool = False,
+) -> Path:
+    """Validate and atomically materialize one patched URDF."""
+    source = Path(source_urdf).expanduser().resolve()
+    destination = Path(destination_urdf).expanduser().resolve()
+    if not source.is_file():
+        raise ValueError(f"source URDF does not exist: {source}")
+    for pattern in forbidden_source_stem_patterns:
+        if re.search(str(pattern), source.stem, re.IGNORECASE):
+            raise ValueError(
+                "source URDF must be the immutable original CAD URDF"
+            )
+    if destination == source or destination.exists():
+        raise ValueError(f"refusing to overwrite URDF: {destination}")
+    destination.parent.mkdir(parents=True, exist_ok=True)
+    tree = ET.parse(source)
+    root = tree.getroot()
+    corrected = apply_urdf_patch_text(
+        source.read_text(encoding="utf-8"), root, patches
+    )
+    materialize_relative_mesh_assets(
+        source=source, output=destination.parent, urdf_root=root
+    )
+    if copy_complete_mesh_directory:
+        _copy_complete_mesh_directory(source, destination.parent)
+    temporary = destination.with_suffix(destination.suffix + ".tmp")
+    try:
+        with temporary.open("w", encoding="utf-8") as stream:
+            stream.write(corrected)
+            stream.flush()
+            os.fsync(stream.fileno())
+        os.replace(temporary, destination)
+    finally:
+        if temporary.exists():
+            temporary.unlink()
+    return destination
+
+
+__all__ = [
+    "MujocoEqualityPatch",
+    "UrdfJointPatch",
+    "UrdfPatchSet",
+    "apply_urdf_patch_text",
+    "materialize_relative_mesh_assets",
+    "write_urdf_patches",
+]

src/linkerhand_calibration/linkerhand_calibration/core/urdf/plan.py

@@ -0,0 +1,104 @@
+"""One authorization plan shared by URDF writers and validators."""
+
+from __future__ import annotations
+
+from dataclasses import dataclass, field
+import hashlib
+from pathlib import Path
+from typing import Mapping
+
+from ..domain import CalibrationProfile
+
+
+@dataclass(frozen=True)
+class UrdfCorrectionPlan:
+    source_sha256: str
+    allowed_active_joints: frozenset[str]
+    endpoint_limit_joints: frozenset[str]
+    mimic_source_by_joint: Mapping[str, str]
+    frozen_joints: frozenset[str]
+    frozen_offsets_rad: Mapping[str, float] = field(default_factory=dict)
+    forbid_calibrated_source: bool = True
+    forbid_overwrite: bool = True
+    preserve_passive_joints: bool = True
+
+    def __post_init__(self) -> None:
+        if len(self.source_sha256) != 64 or any(
+            character not in "0123456789abcdef"
+            for character in self.source_sha256.lower()
+        ):
+            raise ValueError("source URDF SHA-256 is invalid")
+        if self.allowed_active_joints & self.frozen_joints:
+            raise ValueError("allowed and frozen URDF joints overlap")
+        applied = self.allowed_active_joints | set(self.frozen_offsets_rad)
+        if not set(self.frozen_offsets_rad).issubset(self.frozen_joints):
+            raise ValueError("frozen offsets must belong to frozen joints")
+        if not self.endpoint_limit_joints.issubset(applied):
+            raise ValueError("endpoint limit joint is not an applied active joint")
+        if set(self.mimic_source_by_joint) & self.allowed_active_joints:
+            raise ValueError("dependent mimic joints cannot be active edit targets")
+
+    def authorize_offsets(self, offsets_rad: Mapping[str, float]) -> None:
+        required = self.allowed_active_joints | set(self.frozen_offsets_rad)
+        unexpected = set(offsets_rad) - required
+        if unexpected:
+            raise ValueError(
+                "URDF correction contains unauthorized joints: "
+                + ", ".join(sorted(unexpected))
+            )
+        missing = required - set(offsets_rad)
+        if missing:
+            raise ValueError(
+                "URDF correction is missing active joints: "
+                + ", ".join(sorted(missing))
+            )
+        changed_frozen = {
+            name
+            for name, expected in self.frozen_offsets_rad.items()
+            if abs(float(offsets_rad[name]) - float(expected)) > 1.0e-12
+        }
+        if changed_frozen:
+            raise ValueError(
+                "URDF correction changed frozen offsets: "
+                + ", ".join(sorted(changed_frozen))
+            )
+
+    def verify_source(self, source_urdf: str | Path) -> None:
+        digest = hashlib.sha256(Path(source_urdf).read_bytes()).hexdigest()
+        if digest != self.source_sha256.lower():
+            raise ValueError("source URDF SHA-256 differs from correction plan")
+
+
+def build_correction_plan(
+    profile: CalibrationProfile,
+    *,
+    source_sha256: str,
+    scope: str,
+    frozen_offsets_rad: Mapping[str, float] | None = None,
+) -> UrdfCorrectionPlan:
+    """Build one scope-aware edit authorization from typed policies."""
+    selected = profile.scope.selected_joints(scope)
+    frozen = profile.scope.frozen_joints[str(scope)]
+    expected_frozen = {
+        str(name): float(value)
+        for name, value in dict(frozen_offsets_rad or {}).items()
+    }
+    if set(expected_frozen) != set(frozen):
+        missing = set(frozen) - set(expected_frozen)
+        extra = set(expected_frozen) - set(frozen)
+        raise ValueError(
+            "frozen URDF offset state differs from scope policy: "
+            f"missing={','.join(sorted(missing)) or '-'};"
+            f"extra={','.join(sorted(extra)) or '-'}"
+        )
+    applied = selected | frozen
+    return UrdfCorrectionPlan(
+        source_sha256=source_sha256,
+        allowed_active_joints=selected,
+        endpoint_limit_joints=(
+            profile.zero.mechanical_endpoint_joints & applied
+        ),
+        mimic_source_by_joint=profile.zero.mimic_source_by_joint,
+        frozen_joints=frozen | profile.zero.cad_frozen_joints,
+        frozen_offsets_rad=expected_frozen,
+    )

src/linkerhand_calibration/linkerhand_calibration/extrinsics.py

@@ -0,0 +1,150 @@
+"""Three-view compatibility policy over generic camera extrinsics."""
+
+from __future__ import annotations
+
+from dataclasses import dataclass
+from pathlib import Path
+from typing import Any, Mapping, Sequence
+
+import numpy as np
+import yaml
+
+from .core.geometry.extrinsics import (
+    CameraCalibrationIdentity,
+    CameraExtrinsics,
+    camera_info_fingerprint,
+    load_camera_extrinsics,
+    matrix_payload,
+    transform_matrix,
+    validate_camera_extrinsics_payload,
+)
+
+
+VIEWS: tuple[str, ...] = ("front", "side", "top")
+_QUALITY_LIMITS = {
+    "reprojection_rms_px": 1.2,
+    "maximum_rotation_repeatability_deg": 0.3,
+    "maximum_translation_repeatability_m": 0.0015,
+}
+_MINIMUM_CAPTURE_COUNTS = {
+    "front_side_captures": 15,
+    "front_top_captures": 15,
+}
+
+
+@dataclass(frozen=True)
+class ThreeCameraExtrinsics:
+    """Transforms points from each camera optical frame into front optical."""
+
+    cameras: Mapping[str, CameraCalibrationIdentity]
+    front_from_view: Mapping[str, np.ndarray]
+    quality: Mapping[str, float]
+
+    def transform(self, view: str) -> np.ndarray:
+        if view not in self.front_from_view:
+            raise KeyError(f"extrinsics do not contain view {view}")
+        return np.asarray(self.front_from_view[view], dtype=float).copy()
+
+    def camera_matches(
+        self,
+        view: str,
+        *,
+        serial_number: str,
+        width: int,
+        height: int,
+        intrinsics_sha256: str,
+    ) -> bool:
+        expected = self.cameras.get(view)
+        return bool(
+            expected is not None
+            and expected.serial_number == str(serial_number)
+            and expected.width == int(width)
+            and expected.height == int(height)
+            and expected.intrinsics_sha256 == str(intrinsics_sha256)
+        )
+
+
+def validate_extrinsics_payload(payload: Mapping[str, Any]) -> None:
+    validate_camera_extrinsics_payload(
+        payload,
+        required_views=VIEWS,
+        reference_view="front",
+        quality_limits=_QUALITY_LIMITS,
+        minimum_capture_counts=_MINIMUM_CAPTURE_COUNTS,
+    )
+
+
+def load_three_camera_extrinsics(
+    path: str | Path,
+    *,
+    quality_limits: Mapping[str, float] | None = None,
+    minimum_capture_counts: Mapping[str, int] | None = None,
+) -> ThreeCameraExtrinsics:
+    generic = load_camera_extrinsics(
+        path,
+        required_views=VIEWS,
+        reference_view="front",
+        quality_limits=(
+            _QUALITY_LIMITS if quality_limits is None else quality_limits
+        ),
+        minimum_capture_counts=(
+            _MINIMUM_CAPTURE_COUNTS
+            if minimum_capture_counts is None
+            else minimum_capture_counts
+        ),
+    )
+    return ThreeCameraExtrinsics(
+        cameras=generic.cameras,
+        front_from_view=generic.reference_from_view,
+        quality=generic.quality,
+    )
+
+
+def dump_three_camera_extrinsics(
+    path: str | Path,
+    *,
+    cameras: Mapping[str, Mapping[str, Any]],
+    front_from_view: Mapping[str, Sequence[Sequence[float]]],
+    quality: Mapping[str, Any],
+    quality_limits: Mapping[str, float] | None = None,
+) -> None:
+    payload = {
+        "schema_version": 1,
+        "reference_view": "front",
+        "cameras": {view: dict(cameras[view]) for view in VIEWS},
+        "front_from_view": {
+            view: matrix_payload(front_from_view[view]) for view in VIEWS
+        },
+        "quality": dict(quality),
+    }
+    validate_camera_extrinsics_payload(
+        payload,
+        required_views=VIEWS,
+        reference_view="front",
+        quality_limits=(
+            _QUALITY_LIMITS if quality_limits is None else quality_limits
+        ),
+        minimum_capture_counts=_MINIMUM_CAPTURE_COUNTS,
+    )
+    destination = Path(path).expanduser().resolve()
+    destination.parent.mkdir(parents=True, exist_ok=True)
+    temporary = destination.with_suffix(destination.suffix + ".tmp")
+    with temporary.open("w", encoding="utf-8") as stream:
+        yaml.safe_dump(payload, stream, allow_unicode=True, sort_keys=False)
+    temporary.replace(destination)
+
+
+__all__ = [
+    "CameraCalibrationIdentity",
+    "CameraExtrinsics",
+    "ThreeCameraExtrinsics",
+    "VIEWS",
+    "camera_info_fingerprint",
+    "dump_three_camera_extrinsics",
+    "load_camera_extrinsics",
+    "load_three_camera_extrinsics",
+    "matrix_payload",
+    "transform_matrix",
+    "validate_camera_extrinsics_payload",
+    "validate_extrinsics_payload",
+]

src/linkerhand_calibration/linkerhand_calibration/extrinsics_node.py

@@ -605,6 +605,9 @@ class ThreeCameraExtrinsicsNode(Node):
         self.declare_parameter("minimum_captures_per_pair", 15)
         self.declare_parameter("maximum_pair_skew_ms", 100.0)
         self.declare_parameter("maximum_reprojection_rms_px", 1.2)
+        self.declare_parameter(
+            "maximum_candidate_pair_reprojection_rms_px", 1.5
+        )
         self.declare_parameter(
             "maximum_single_camera_reprojection_rms_px", 1.5
         )
@@ -640,6 +643,9 @@ class ThreeCameraExtrinsicsNode(Node):
         self.maximum_reprojection_rms_px = float(
             value("maximum_reprojection_rms_px")
         )
+        self.maximum_candidate_pair_reprojection_rms_px = float(
+            value("maximum_candidate_pair_reprojection_rms_px")
+        )
         self.maximum_single_camera_reprojection_rms_px = float(
             value("maximum_single_camera_reprojection_rms_px")
         )
@@ -671,13 +677,21 @@ class ThreeCameraExtrinsicsNode(Node):
         if self.maximum_reprojection_rms_px <= 0.0:
             raise ValueError("maximum_reprojection_rms_px must be positive")
         if (
-            self.maximum_single_camera_reprojection_rms_px
+            self.maximum_candidate_pair_reprojection_rms_px
             < self.maximum_reprojection_rms_px
         ):
             raise ValueError(
-                "maximum_single_camera_reprojection_rms_px must be at "
+                "maximum_candidate_pair_reprojection_rms_px must be at "
                 "least maximum_reprojection_rms_px"
             )
+        if (
+            self.maximum_single_camera_reprojection_rms_px
+            < self.maximum_candidate_pair_reprojection_rms_px
+        ):
+            raise ValueError(
+                "maximum_single_camera_reprojection_rms_px must be at "
+                "least maximum_candidate_pair_reprojection_rms_px"
+            )
         if self.gui_refresh_hz <= 0.0 or self.gui_refresh_hz > 10.0:
             raise ValueError("gui_refresh_hz must be in (0, 10]")
         if self.auto_capture_stable_seconds < 0.5:
@@ -869,14 +883,20 @@ class ThreeCameraExtrinsicsNode(Node):
             selected.reprojection_rms_px,
             self.maximum_single_camera_reprojection_rms_px,
         )
-        if camera_rms_failed or pair_rms > self.maximum_reprojection_rms_px:
+        if (
+            camera_rms_failed
+            or pair_rms
+            > self.maximum_candidate_pair_reprojection_rms_px
+        ):
             return PairAssessment(
                 ready=False,
                 reason_code="reprojection_rms_too_large",
                 message=(
                     f"当前组未通过采集门限，未计入。单相机上限="
                     f"{self.maximum_single_camera_reprojection_rms_px:.3f}px，"
-                    f"组合上限={self.maximum_reprojection_rms_px:.3f}px；"
+                    "候选组合上限="
+                    f"{self.maximum_candidate_pair_reprojection_rms_px:.3f}px，"
+                    f"最终批次上限={self.maximum_reprojection_rms_px:.3f}px；"
                     f"组合RMS={pair_rms:.3f}px，"
                     f"front={front.reprojection_rms_px:.3f}px，"
                     f"{other}={selected.reprojection_rms_px:.3f}px；"
@@ -1352,7 +1372,9 @@ class ThreeCameraExtrinsicsNode(Node):
             )
             cv2.putText(
                 canvas,
-                f"Pair RMS {pair_rms}/{self.maximum_reprojection_rms_px:.3f}px  "
+                "Pair RMS "
+                f"{pair_rms}/"
+                f"{self.maximum_candidate_pair_reprojection_rms_px:.3f}px  "
                 f"Skew {skew}/{self.maximum_pair_skew_ns / 1e6:.1f}ms  "
                 f"Batch RMS {batch_rms}/{self.maximum_reprojection_rms_px:.3f}px  "
                 f"stability {rotation}/"
@@ -1526,6 +1548,15 @@ class ThreeCameraExtrinsicsNode(Node):
             cameras=self.camera_identities,
             front_from_view=transforms,
             quality=quality,
+            quality_limits={
+                "reprojection_rms_px": self.maximum_reprojection_rms_px,
+                "maximum_rotation_repeatability_deg": math.degrees(
+                    self.maximum_rotation_repeatability_rad
+                ),
+                "maximum_translation_repeatability_m": (
+                    self.maximum_translation_repeatability_m
+                ),
+            },
         )
         response.success = True
         response.message = f"外参已保存：{self.output_file}；{quality}"

src/linkerhand_calibration/linkerhand_calibration/full_hand.py

@@ -0,0 +1,7 @@
+"""One-release module alias for the relocated model profile implementation."""
+
+import sys
+
+from .models.g20 import profile as _implementation
+
+sys.modules[__name__] = _implementation

src/linkerhand_calibration/linkerhand_calibration/hikrobot_camera.py

@@ -203,7 +203,7 @@ def configure_fastdds_large_image_transport() -> Path:
     from ament_index_python.packages import get_package_share_directory
 
     profile = (
-        Path(get_package_share_directory("g20_thumb_apriltag_calibration"))
+        Path(get_package_share_directory("linkerhand_calibration"))
         / "config"
         / "fastdds_large_images.xml"
     )

src/linkerhand_calibration/linkerhand_calibration/models/__init__.py

@@ -0,0 +1,17 @@
+"""Model- and side-specific calibration policies."""
+
+from .registry import (
+    EngineBindings,
+    ProfileRegistry,
+    RegisteredProfile,
+    get_default_registry,
+)
+from .runtime_schema import validate_schema_v6_runtime_payload
+
+__all__ = [
+    "EngineBindings",
+    "ProfileRegistry",
+    "RegisteredProfile",
+    "get_default_registry",
+    "validate_schema_v6_runtime_payload",
+]

src/linkerhand_calibration/linkerhand_calibration/models/g20/__init__.py

@@ -0,0 +1,15 @@
+"""Registered profiles for this hand family."""
+
+from ..registry import ProfileRegistry
+
+
+def register_profiles(registry: ProfileRegistry) -> None:
+    from .legacy_11 import build_left_profile, build_right_profile
+    from .right_19 import build_profile
+
+    registry.register(build_profile())
+    registry.register(build_left_profile())
+    registry.register(build_right_profile())
+
+
+__all__ = ["register_profiles"]

src/linkerhand_calibration/linkerhand_calibration/models/g20/_adapter.py

@@ -0,0 +1,219 @@
+"""Adapt reviewed family profiles to the shared typed contract."""
+
+from __future__ import annotations
+
+from ...core import (
+    CalibrationProfile,
+    CommandLayout,
+    MeasurementPolicy,
+    MeasurementSpec,
+    MotionPolicy,
+    ProfileKey,
+    QualityPolicy,
+    ScopePolicy,
+    TagSpec,
+    TaskSpec,
+    ViewSpec,
+    VisionRigSpec,
+    ZeroSolvePolicy,
+)
+from .profile import MIMIC_DERIVED_FINGER_DIPS
+from ..registry import EngineBindings, RegisteredProfile
+from .artifacts import build_artifact_policy
+from .motion import (
+    build_calibration_motion_command,
+    build_calibration_preparation_waypoints,
+    build_calibration_return_waypoints,
+)
+
+
+_HARD_THRESHOLD_KEYS = frozenset(
+    {
+        "minimum_detection_rate",
+        "maximum_reprojection_error_px",
+        "maximum_axis_cycle_difference_rad",
+        "maximum_pose_line_rms_m",
+        "maximum_hysteresis_rad",
+        "maximum_validation_error_rad",
+    }
+)
+
+
+def _run_cli(args: list[str] | None = None) -> None:
+    from .runner import main
+
+    main(args)
+
+
+def _run_node(args: list[str] | None = None) -> None:
+    from .node import main
+
+    main(args)
+
+
+def adapt_profile(
+    *,
+    key: ProfileKey,
+    namespace: str,
+    hand_profile,
+    zero_profile,
+    mechanical_endpoint_joints: frozenset[str] = frozenset(),
+    post_solve_endpoint_joints: frozenset[str] = frozenset(),
+) -> RegisteredProfile:
+    fixed_by_view = {
+        view: frozenset(roles)
+        for view, roles in hand_profile.preflight_view_roles.items()
+    }
+    views = tuple(
+        ViewSpec(
+            name=view,
+            tags=tuple(
+                TagSpec(
+                    role=role,
+                    tag_id=int(tag_id),
+                    fixed_reference=role in fixed_by_view.get(view, frozenset()),
+                )
+                for role, tag_id in roles.items()
+            ),
+        )
+        for view, roles in hand_profile.view_tags.items()
+    )
+    record_specs = hand_profile.record_specs
+    command_index_by_joint = {
+        name: int(spec.motor_index) for name, spec in record_specs.items()
+    }
+    command_index_by_joint.update(
+        {
+            name: int(spec.motor_index)
+            for name, spec in hand_profile.joint_specs.items()
+        }
+    )
+    measurements = {
+        name: MeasurementSpec(
+            joint=name,
+            kind=str(spec.zero_kind or "curve"),
+            view=spec.view,
+            parent_role=spec.parent_role,
+            child_role=spec.child_role,
+            validation_source=(hand_profile.axis_validation_sources or {}).get(
+                name
+            ),
+            pose_axis_line_required=bool(
+                spec.pose_axis_line_required
+            ),
+        )
+        for name, spec in record_specs.items()
+    }
+    tasks = tuple(
+        TaskSpec(
+            key=spec.key,
+            view=spec.view,
+            command_index=int(spec.motor_index),
+            joints=tuple(spec.joints),
+            auxiliary_commands=tuple(spec.auxiliary_commands),
+            validation_only=bool(spec.validation_only),
+        )
+        for spec in hand_profile.sweep_specs
+    )
+    active = frozenset(hand_profile.active_joints)
+    passive = frozenset(hand_profile.passive_joints)
+    thumb = frozenset(name for name in active if name.startswith("thumb_"))
+    fingers = active - thumb
+    typed = CalibrationProfile(
+        key=key,
+        namespace=namespace,
+        command=CommandLayout(
+            names=tuple(hand_profile.command_names),
+            baseline_u8=tuple(int(value) for value in hand_profile.baseline_command),
+            command_index_by_joint=command_index_by_joint,
+            disabled_indices=frozenset(
+                index
+                for index, name in enumerate(hand_profile.command_names)
+                if name.startswith("reserved_")
+            ),
+        ),
+        vision=VisionRigSpec(
+            views=views,
+            common_frame="calibration_common",
+            extrinsic_reference_view=views[0].name,
+            extrinsics_quality_limits={
+                "reprojection_rms_px": 1.2,
+                "maximum_rotation_repeatability_deg": 0.3,
+                "maximum_translation_repeatability_m": 0.0015,
+            },
+            minimum_capture_counts={
+                "front_side_captures": 15,
+                "front_top_captures": 15,
+            },
+        ),
+        motion=MotionPolicy(
+            tasks=tasks,
+            precheck_sweeps=bool(hand_profile.precheck_sweeps),
+            steady_command_checkpoints=bool(
+                hand_profile.steady_command_checkpoints
+            ),
+        ),
+        measurement=MeasurementPolicy(
+            measurements=measurements,
+            cross_view_sources=dict(
+                hand_profile.axis_validation_sources or {}
+            ),
+            image_curve_joints=frozenset(
+                hand_profile.image_trajectory_joints
+            ),
+            directional_zero=bool(hand_profile.directional_zero),
+            cross_view_roll_curve=bool(hand_profile.cross_view_roll_curve),
+            stable_cross_view_cone_bias=bool(
+                hand_profile.stable_cross_view_cone_bias
+            ),
+        ),
+        zero=ZeroSolvePolicy(
+            active_joints=active,
+            passive_joints=passive,
+            direct_zero_joints=tuple(zero_profile.direct_zero_joints),
+            axis_joints=tuple(zero_profile.axis_joints),
+            mechanical_endpoint_joints=mechanical_endpoint_joints,
+            post_solve_endpoint_joints=post_solve_endpoint_joints,
+            mimic_source_by_joint={
+                target: source
+                for target, source in MIMIC_DERIVED_FINGER_DIPS.items()
+                if target in passive and source in active
+            },
+            cad_frozen_joints=frozenset(
+                passive - set(zero_profile.static_output_zero_offsets_rad)
+            ),
+        ),
+        quality=QualityPolicy(
+            training_cycles=(0, 1, 2),
+            holdout_cycle=3 if key.layout != "legacy_11" else None,
+            hard_threshold_keys=_HARD_THRESHOLD_KEYS,
+            isolated_holdout=bool(hand_profile.isolated_holdout),
+        ),
+        scope=ScopePolicy(
+            calibrate_joints={
+                "full": active,
+                "thumb": thumb,
+                "fingers": fingers,
+            },
+            frozen_joints={
+                "full": frozenset(),
+                "thumb": fingers,
+                "fingers": thumb,
+            },
+        ),
+        artifacts=build_artifact_policy(
+            frozenset(hand_profile.capabilities)
+        ),
+    )
+    return RegisteredProfile(
+        profile=typed,
+        engine=EngineBindings(
+            hand_profile=hand_profile,
+            zero_profile=zero_profile,
+            motion_command=build_calibration_motion_command,
+            preparation_waypoints=build_calibration_preparation_waypoints,
+            return_waypoints=build_calibration_return_waypoints,
+            cli_main=_run_cli,
+            node_main=_run_node,
+        ),
+    )

src/linkerhand_calibration/linkerhand_calibration/models/g20/artifacts.py

@@ -0,0 +1,30 @@
+"""Runtime JSON and corrected-URDF naming policy."""
+
+from ...core import ArtifactPolicy
+
+
+def build_artifact_policy(
+    compatibility_tokens: frozenset[str],
+) -> ArtifactPolicy:
+    return ArtifactPolicy(
+        output_schema_version=4,
+        calibration_filename="g20_{side}_{serial_number}_calibration.json",
+        corrected_urdf_filename=(
+            "linkerhand_g20_{side}_{serial_number}_zero_calibrated.urdf"
+        ),
+        protected_input_fields=frozenset(
+            {
+                "source_urdf_sha256",
+                "camera_extrinsics_sha256",
+                "calibration_config_sha256",
+                "tag_config_sha256",
+            }
+        ),
+        session_compatibility_tokens=frozenset(compatibility_tokens),
+        publish_corrected_urdf=(
+            "urdf_zero_publication" in compatibility_tokens
+        ),
+    )
+
+
+__all__ = ["build_artifact_policy"]

src/linkerhand_calibration/linkerhand_calibration/models/g20/command_layout.py

@@ -0,0 +1,26 @@
+"""Reviewed command-channel layouts for this hand family."""
+
+G20_COMMAND_NAMES: tuple[str, ...] = (
+    "thumb_cmc_pitch",
+    "index_mcp_pitch",
+    "middle_mcp_pitch",
+    "ring_mcp_pitch",
+    "pinky_mcp_pitch",
+    "thumb_cmc_roll",
+    "index_mcp_roll",
+    "middle_mcp_roll",
+    "ring_mcp_roll",
+    "pinky_mcp_roll",
+    "thumb_cmc_yaw",
+    "reserved_11",
+    "reserved_12",
+    "reserved_13",
+    "reserved_14",
+    "thumb_mcp",
+    "index_pip",
+    "middle_pip",
+    "ring_pip",
+    "pinky_pip",
+)
+
+__all__ = ["G20_COMMAND_NAMES"]

src/linkerhand_calibration/linkerhand_calibration/models/g20/golden_regression.py

@@ -0,0 +1,144 @@
+"""Read-only regression checks for the reviewed hardware sessions."""
+
+from __future__ import annotations
+
+import argparse
+import hashlib
+import json
+from pathlib import Path
+from typing import Any
+
+_PASS_SESSIONS = {
+    "20260830_181154": "full",
+    "20260831_141123": "thumb",
+    "20260831_163843": "thumb",
+}
+_FAIL_SESSIONS = {
+    "20260831_111837": ("FIT-MODEL-401", "joint_fit_check_failed"),
+    "20260831_142322": ("VAL-QUALITY-501", "zero_model_validation_failed"),
+}
+
+
+def _sha256_file(path: Path) -> str:
+    digest = hashlib.sha256()
+    with path.open("rb") as stream:
+        for chunk in iter(lambda: stream.read(1024 * 1024), b""):
+            digest.update(chunk)
+    return digest.hexdigest()
+
+
+def _summary(session: Path) -> dict[str, Any]:
+    value = json.loads(
+        (session / "calibration_summary_zh.json").read_text(encoding="utf-8")
+    )
+    if not isinstance(value, dict):
+        raise ValueError(f"invalid golden summary: {session}")
+    return value
+
+
+def validate_golden_sessions(
+    serial_root: str | Path,
+    *,
+    replay_full_session: bool = True,
+) -> dict[str, Any]:
+    """Prove PASS/FAIL, hashes, partial freezes, and failure non-publication."""
+    root = Path(serial_root).expanduser().resolve()
+    results: dict[str, Any] = {}
+    for session_id, scope in _PASS_SESSIONS.items():
+        session = root / session_id
+        summary = _summary(session)
+        if summary.get("result") != "PASS":
+            raise ValueError(f"golden PASS changed: {session_id}")
+        if summary.get("calibration_scope") != scope:
+            raise ValueError(f"golden scope changed: {session_id}")
+        artifacts = summary["artifacts"]
+        hashes = summary["hashes"]
+        calibration_json = session / artifacts["json"]
+        corrected_urdf = session / artifacts["urdf"]
+        actual_json_hash = _sha256_file(calibration_json)
+        actual_urdf_hash = _sha256_file(corrected_urdf)
+        if actual_json_hash != hashes["calibration_json_sha256"]:
+            raise ValueError(f"golden JSON hash changed: {session_id}")
+        if actual_urdf_hash != hashes["corrected_urdf_sha256"]:
+            raise ValueError(f"golden URDF hash changed: {session_id}")
+        if session_id == "20260831_141123" and len(
+            summary.get("preserved_certified_zero_joints", ())
+        ) != 12:
+            raise ValueError("merged thumb no longer freezes all finger zeros")
+        if session_id == "20260831_163843":
+            payload = json.loads(calibration_json.read_text(encoding="utf-8"))
+            if payload.get("artifact_type") != (
+                "g20_right_standalone_thumb_calibration"
+            ):
+                raise ValueError("standalone thumb artifact type changed")
+        results[session_id] = {
+            "result": "PASS",
+            "json_sha256": actual_json_hash,
+            "urdf_sha256": actual_urdf_hash,
+        }
+
+    for session_id, (error_code, reason) in _FAIL_SESSIONS.items():
+        session = root / session_id
+        summary = _summary(session)
+        if (
+            summary.get("result") != "FAIL"
+            or summary.get("error_code") != error_code
+            or summary.get("reason") != reason
+        ):
+            raise ValueError(f"golden failure decision changed: {session_id}")
+        results[session_id] = {
+            "result": "FAIL",
+            "error_code": error_code,
+            "reason": reason,
+        }
+
+    failure_ids = set(_FAIL_SESSIONS)
+    for pointer_name in ("latest_passed", "latest_thumb_passed"):
+        pointer = root / pointer_name
+        if pointer.exists() and pointer.resolve().name in failure_ids:
+            raise ValueError(f"failure session was published through {pointer_name}")
+
+    if replay_full_session:
+        from .offline_replay import replay_session
+
+        session_id = "20260830_181154"
+        replay = replay_session(root / session_id, write_outputs=False)
+        expected = results[session_id]
+        if replay["computed_final_json_sha256"] != expected["json_sha256"]:
+            raise ValueError(
+                "full-session replay JSON is not byte-identical: "
+                f"computed={replay['computed_final_json_sha256']} "
+                f"expected={expected['json_sha256']}"
+            )
+        if replay["corrected_urdf_sha256"] != expected["urdf_sha256"]:
+            raise ValueError(
+                "full-session replay URDF is not byte-identical: "
+                f"computed={replay['corrected_urdf_sha256']} "
+                f"expected={expected['urdf_sha256']}"
+            )
+        results[session_id]["offline_replay"] = "byte_identical"
+    return results
+
+
+def main(args: list[str] | None = None) -> None:
+    parser = argparse.ArgumentParser(
+        description="Validate the five reviewed calibration sessions"
+    )
+    parser.add_argument("serial_root")
+    parser.add_argument("--no-replay", action="store_true")
+    selected = parser.parse_args(args)
+    print(
+        json.dumps(
+            validate_golden_sessions(
+                selected.serial_root,
+                replay_full_session=not selected.no_replay,
+            ),
+            ensure_ascii=False,
+            indent=2,
+            sort_keys=True,
+        )
+    )
+
+
+if __name__ == "__main__":
+    main()