拇指侧摆零位标定

src/g20_thumb_apriltag_calibration/README.md

@@ -267,11 +267,11 @@ D405 的彩色流来自 `depth_module`，启动文件会同时设置
 订阅者；同时使用 MCAP `zstd_fast` 压缩并按 10 GiB 分卷。快速标定通常不需要录制；
 若用于正式可追溯验收，再启用并检查磁盘空间。
 
-## 5. CMC Pitch 双零位角测量
+## 5. CMC Pitch 零位角测量
 
-只测量命令 255 时 `thumb_cmc_pitch` 的掌心相对零位角和画面水平投影零位角时，
-使用独立启动文件。它只拟合 CMC 的二维零位轨迹圆，不运行完整 0～255 角度映射，
-也不会生成或修改 URDF：
+只测量命令 255 时 `thumb_cmc_pitch` 的画面水平投影零位角时，使用独立启动文件。
+它只拟合 CMC 的二维零位轨迹圆，不运行完整 0～255 角度映射，也不会生成或修改
+URDF：
 
 ```bash
 ros2 launch g20_thumb_apriltag_calibration \
@@ -296,7 +296,7 @@ ros2 service call \
   std_srvs/srv/Trigger {}
 ```
 
-查看带红色画面水平线、T0/T3 方向箭头、青色轨迹点、紫色拟合圆心和径向零位线
+查看带红色画面水平线、T0/T3标签中心、青色轨迹点、紫色拟合圆心和径向零位线
 的调试画面：
 
 ```bash
@@ -304,15 +304,26 @@ ros2 run image_view image_view --ros-args \
   --remap image:=/g20_thumb_cmc_pitch_zero/debug_image
 ```
 
+画面底部红线是固定的相机水平与构图目标。程序会在画面下部自动寻找一条足够长、
+接近水平的物理桌边或高对比参考直线，并画成蓝线。开始前调整相机，使蓝线与红线
+重合；画面和 `status_text` 会实时显示红蓝线夹角及垂直偏差。`±0.5°` 和
+`±12 px` 只用于显示 `ALIGNED/ADJUST`，完全不参与预检或 `start` 服务判断。
+由操作人员确认相机位置后手动开始标定。参考直线应清晰、连续并尽量横跨画面；
+该检测不使用 T0/T3 标签朝向。
+
+一条二维直线只能确认相机滚转角和上下构图位置，不能单独证明相机的距离、俯仰、
+偏航或完整三维位置。若需要严格复现这些量，还应使用固定相机支架或专用标定板。
+
 每轮只控制电机 0 执行一次 `255→64` 端点运动和一次 `64→255` 返回运动。运动期间
 连续采集 `T3中心−T0中心`，按机械手状态分箱后拟合图像平面圆；返回 255 后使用
-“拟合圆心→T3零位中心”的径向线计算角度。运动前和返回后各采集 30 帧静态零位，
+“T3零位中心→拟合圆心”的固定内向径向矢量计算角度。运动前和返回后各采集30帧静态零位，
 三轮轨迹合并后得到最终圆心。其他 19 个命令保持固定基准。任何其他节点同时发布
 `/g20/cb_left_hand_control_cmd` 时，`start` 服务会拒绝启动。
 
-T0用于消除相机或整只手的平移抖动。T3标签自身的箭头只负责在径向线的两个相反
-方向中选择与旧版一致的正方向，不再决定角度大小。若轨迹跨度、圆弧、半径、径向
-RMS/P95或回零误差不合格，节点暂停或写出 `quality.passed=false`。
+T0中心用于消除相机或整只手的平移抖动。T0和T3标签自身的朝向与角点 `+x`
+都不参与零位或行程计算；标签可以任意平面内旋转或反贴180°，只需标签平整、
+固定且中心始终可见。若轨迹跨度、圆弧、半径、径向RMS/P95或回零误差不合格，
+节点暂停或写出 `quality.passed=false`。
 
 完成后只生成：
 
@@ -321,13 +332,67 @@ calibration_output/G20_LEFT_001/<时间戳>/
   g20_left_G20_LEFT_001_thumb_cmc_pitch_zero.json
 ```
 
-其中两个核心字段是：
+核心字段是：
 
 ```text
-zero_angles.palm_relative_zero_rad
 zero_angles.table_projected_zero_rad
 ```
 
-前者是圆心径向零位线相对 T0 方向的角度，后者是圆心径向零位线相对相机画面
-水平向右方向的角度。桌面投影角不使用 T0 的方向，但会用 T0 中心抵消平移；
-它仍不是真实三维桌面检测，因此会随相机滚转和机械手摆放改变。
+该值是内向径向零位矢量相对相机画面水平向右方向的角度。它不使用 T0 的方向，
+但会用 T0 中心抵消平移；它仍不是真实三维桌面检测，因此会随相机滚转和机械手
+摆放改变。
+
+## 6. CMC Roll 零位与行程标定
+
+`thumb_cmc_roll` 复用上节的 T0 平移补偿、T3 中心轨迹分箱和稳健圆拟合，
+但控制的是电机 5。每轮执行 `255→0→255`：在 255 零位、0 行程端点和返回
+255 后各静态采集 30 帧，因此可以同时测量零位角和完整 `0～255` 实际角行程。
+命令 0 是完整行程端点，开始前必须确认拇指没有机械碰撞或硬限位顶死风险。
+Roll同样固定使用“T3中心→拟合圆心”的内向径向矢量，不读取T3标签朝向。
+
+```bash
+ros2 launch g20_thumb_apriltag_calibration \
+  front_cmc_roll_calibration.launch.py \
+  serial_number:=G20_LEFT_001
+```
+
+预检通过后启动三轮标定：
+
+```bash
+ros2 topic echo --once --full-length \
+  /g20_thumb_cmc_roll_calibration/status_text \
+  --field data
+
+ros2 service call \
+  /g20_thumb_cmc_roll_calibration/start \
+  std_srvs/srv/Trigger {}
+```
+
+调试画面：
+
+```bash
+ros2 run image_view image_view --ros-args \
+  --remap image:=/g20_thumb_cmc_roll_calibration/debug_image
+```
+
+Roll 使用与 Pitch 相同的红蓝参考线显示，但是否对齐由操作人员确认，程序不会用
+蓝线状态阻止 `start` 进入电机运动。
+
+完成后生成：
+
+```text
+calibration_output/G20_LEFT_001/<时间戳>/
+  g20_left_G20_LEFT_001_thumb_cmc_roll_zero_travel.json
+```
+
+核心输出字段：
+
+```text
+zero_angles.table_projected_zero_rad
+travel.signed_rad
+travel.range_rad
+```
+
+`travel.signed_rad` 是从命令 255 到 0 的有符号转角，`travel.range_rad` 是三轮
+行程大小的中值。只有轨迹圆质量、T0/T3 检出率、三轮零位/行程一致性、端点径向
+误差和回零误差全部通过时，`quality.passed` 才为 `true`。

src/g20_thumb_apriltag_calibration/config/cmc_pitch_zero.yaml

@@ -2,6 +2,10 @@ g20_thumb_cmc_pitch_zero:
   ros__parameters:
     t0_id: 0
     t3_id: 1
+    joint_name: thumb_cmc_pitch
+    motor_index: 0
+    zero_command_u8: 255
+    measure_travel: false
     baseline_command_u8:
       [255, 255, 255, 255, 255, 255, 193, 148, 105, 42,
        245, 255, 255, 255, 255, 255, 255, 255, 255, 255]
@@ -30,16 +34,26 @@ g20_thumb_cmc_pitch_zero:
     maximum_hamming: 0
     minimum_decision_margin: 30.0
     minimum_edge_pixels: 40.0
-    maximum_static_std_deg: 0.5
     maximum_static_position_rms_px: 1.5
     maximum_round_difference_deg: 1.0
     maximum_return_error_deg: 1.0
-    maximum_anchor_drift_deg: 1.0
-    # T3 +x is used only to choose which of the two radial-line directions is
-    # positive.  A large mismatch means the tag was mounted nearly orthogonal
-    # to the physical CMC radial direction.
-    maximum_axis_alignment_deg: 45.0
+    # Zero direction is always T3 centre -> fitted circle centre. T3's printed
+    # orientation and corner +x direction are deliberately not used.
     maximum_zero_radial_error_px: 4.0
+    # Detect a long physical table/reference edge in the lower image. The red
+    # target and blue detected line are display-only aids for manual alignment;
+    # they never block preflight or the start service.
+    camera_alignment_enabled: true
+    camera_alignment_reference_y_ratio: 0.90
+    camera_alignment_roi_y_min_ratio: 0.55
+    camera_alignment_roi_y_max_ratio: 0.98
+    camera_alignment_minimum_line_length_ratio: 0.30
+    camera_alignment_max_candidate_angle_deg: 15.0
+    camera_alignment_max_angle_deg: 0.5
+    camera_alignment_max_vertical_offset_px: 12.0
+    camera_alignment_required_frames: 10
+    camera_alignment_minimum_detection_rate: 0.8
+    camera_alignment_max_age_seconds: 1.0
     publish_debug_image: true
     debug_max_rate_hz: 10.0
     debug_scale: 0.75

src/g20_thumb_apriltag_calibration/config/cmc_roll_zero_travel.yaml

@@ -0,0 +1,60 @@
+g20_thumb_cmc_roll_calibration:
+  ros__parameters:
+    t0_id: 0
+    t3_id: 1
+    joint_name: thumb_cmc_roll
+    motor_index: 5
+    zero_command_u8: 255
+    measure_travel: true
+    baseline_command_u8:
+      [255, 255, 255, 255, 255, 255, 193, 148, 105, 42,
+       245, 255, 255, 255, 255, 255, 255, 255, 255, 255]
+    repetitions: 3
+    zero_capture_frames: 30
+    # Measure the complete motor-5 range. Each round captures both static
+    # endpoints around one 255->0->255 circle trajectory.
+    trajectory_command_u8: 0
+    trajectory_bin_size_u8: 8.0
+    trajectory_minimum_frames: 65
+    trajectory_minimum_bins: 30
+    trajectory_minimum_state_span_u8: 240.0
+    trajectory_minimum_radius_px: 20.0
+    trajectory_minimum_arc_deg: 20.0
+    trajectory_maximum_radial_rms_px: 2.0
+    trajectory_maximum_p95_radial_error_px: 3.5
+    trajectory_endpoint_settle_seconds: 0.3
+    trajectory_timeout_seconds: 35.0
+    settle_seconds: 0.5
+    move_timeout_seconds: 25.0
+    capture_timeout_seconds: 15.0
+    state_tolerance_u8: 2.0
+    preflight_frames: 60
+    minimum_detection_rate: 0.95
+    maximum_hamming: 0
+    minimum_decision_margin: 30.0
+    minimum_edge_pixels: 40.0
+    maximum_static_position_rms_px: 1.5
+    maximum_round_difference_deg: 1.0
+    maximum_travel_difference_deg: 1.0
+    minimum_travel_deg: 20.0
+    maximum_return_error_deg: 1.0
+    # Zero direction is always T3 centre -> fitted circle centre. T3's printed
+    # orientation and corner +x direction are deliberately not used.
+    maximum_zero_radial_error_px: 4.0
+    # Detect a long physical table/reference edge in the lower image. The red
+    # target and blue detected line are display-only aids for manual alignment;
+    # they never block preflight or the start service.
+    camera_alignment_enabled: true
+    camera_alignment_reference_y_ratio: 0.90
+    camera_alignment_roi_y_min_ratio: 0.55
+    camera_alignment_roi_y_max_ratio: 0.98
+    camera_alignment_minimum_line_length_ratio: 0.30
+    camera_alignment_max_candidate_angle_deg: 15.0
+    camera_alignment_max_angle_deg: 0.5
+    camera_alignment_max_vertical_offset_px: 12.0
+    camera_alignment_required_frames: 10
+    camera_alignment_minimum_detection_rate: 0.8
+    camera_alignment_max_age_seconds: 1.0
+    publish_debug_image: true
+    debug_max_rate_hz: 10.0
+    debug_scale: 0.75

src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/core.py

@@ -93,8 +93,10 @@ def build_command(
     values = [int(value) for value in baseline]
     if any(value < 0 or value > 255 for value in values):
         raise ValueError("baseline values must be in [0, 255]")
-    if motor_index not in (0, 15):
-        raise ValueError("front thumb calibration only permits motor 0 or 15")
+    if motor_index not in (0, 5, 15):
+        raise ValueError(
+            "front thumb calibration only permits motor 0, 5 or 15"
+        )
     command_u8 = int(command_u8)
     if command_u8 < 0 or command_u8 > 255:
         raise ValueError("command_u8 must be in [0, 255]")

src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/zero_calibration.py

@@ -1,10 +1,11 @@
-"""Pure helpers for image-trajectory CMC pitch zero-angle measurement."""
+"""Pure helpers for image-trajectory CMC zero and travel measurement."""
 
 from __future__ import annotations
 
 import math
 from typing import Any, Mapping, Sequence
 
+import cv2
 import numpy as np
 from scipy.optimize import least_squares
 
@@ -94,6 +95,120 @@ def maximum_pairwise_angle_difference_rad(
     )
 
 
+def detect_reference_alignment_line(
+    image: np.ndarray,
+    *,
+    reference_y_px: float,
+    roi_y_min_ratio: float,
+    roi_y_max_ratio: float,
+    minimum_length_ratio: float,
+    maximum_candidate_angle_rad: float,
+) -> dict[str, Any] | None:
+    """Detect the best near-horizontal physical reference edge.
+
+    The returned angle uses image +x with y pointing up, matching the public
+    table-projected angle convention.  AprilTag corners and orientation are
+    deliberately not used.  Among sufficiently long Hough segments, the
+    edge nearest the fixed red reference line is preferred.
+    """
+    pixels = np.asarray(image)
+    if pixels.ndim not in {2, 3} or pixels.shape[0] < 8 or pixels.shape[1] < 8:
+        raise ValueError("image must be a non-empty grayscale or BGR array")
+    if pixels.ndim == 3 and pixels.shape[2] not in {3, 4}:
+        raise ValueError("image must have 3 BGR or 4 BGRA channels")
+    height, width = pixels.shape[:2]
+    if not math.isfinite(reference_y_px) or not 0.0 <= reference_y_px < height:
+        raise ValueError("reference_y_px must be inside the image")
+    if not 0.0 <= roi_y_min_ratio < roi_y_max_ratio <= 1.0:
+        raise ValueError("alignment ROI ratios must satisfy 0 <= min < max <= 1")
+    if not 0.0 < minimum_length_ratio <= 1.0:
+        raise ValueError("minimum_length_ratio must be in (0, 1]")
+    if not 0.0 < maximum_candidate_angle_rad < math.pi / 2.0:
+        raise ValueError("maximum_candidate_angle_rad must be in (0, pi/2)")
+
+    if pixels.ndim == 2:
+        gray = pixels.astype(np.uint8, copy=False)
+    elif pixels.shape[2] == 4:
+        gray = cv2.cvtColor(pixels, cv2.COLOR_BGRA2GRAY)
+    else:
+        gray = cv2.cvtColor(pixels, cv2.COLOR_BGR2GRAY)
+    gray = cv2.GaussianBlur(gray, (5, 5), 0.0)
+    edges = cv2.Canny(gray, 50, 150)
+
+    roi_top = max(0, min(height - 1, int(round(height * roi_y_min_ratio))))
+    roi_bottom = max(
+        roi_top + 1,
+        min(height, int(round(height * roi_y_max_ratio))),
+    )
+    roi_edges = edges[roi_top:roi_bottom, :]
+    minimum_length_px = max(8, int(round(width * minimum_length_ratio)))
+    hough_threshold = max(25, int(round(minimum_length_px * 0.20)))
+    lines = cv2.HoughLinesP(
+        roi_edges,
+        1.0,
+        math.pi / 720.0,
+        threshold=hough_threshold,
+        minLineLength=minimum_length_px,
+        maxLineGap=max(10, int(round(width * 0.025))),
+    )
+    if lines is None:
+        return None
+
+    candidates: list[tuple[float, dict[str, Any]]] = []
+    image_centre_x = 0.5 * float(width - 1)
+    for raw_line in lines[:, 0, :]:
+        x1, y1, x2, y2 = (float(value) for value in raw_line)
+        y1 += roi_top
+        y2 += roi_top
+        if x2 < x1:
+            x1, x2 = x2, x1
+            y1, y2 = y2, y1
+        delta_x = x2 - x1
+        delta_y = y2 - y1
+        if delta_x < 1.0:
+            continue
+        length = math.hypot(delta_x, delta_y)
+        if length < minimum_length_px:
+            continue
+        angle_rad = math.atan2(-delta_y, delta_x)
+        if abs(angle_rad) > maximum_candidate_angle_rad:
+            continue
+        slope = delta_y / delta_x
+        centre_y = y1 + slope * (image_centre_x - x1)
+        offset_px = centre_y - float(reference_y_px)
+        full_y_left = y1 - slope * x1
+        full_y_right = y1 + slope * (float(width - 1) - x1)
+        normalized_angle = abs(angle_rad) / maximum_candidate_angle_rad
+        normalized_offset = abs(offset_px) / float(height)
+        normalized_length = min(1.5, length / float(width))
+        score = (
+            normalized_offset
+            + 0.02 * normalized_angle
+            - 0.10 * normalized_length
+        )
+        candidates.append(
+            (
+                score,
+                {
+                    "line_xyxy_px": [
+                        0.0,
+                        float(full_y_left),
+                        float(width - 1),
+                        float(full_y_right),
+                    ],
+                    "angle_rad": float(angle_rad),
+                    "vertical_offset_px": float(offset_px),
+                    "segment_length_px": float(length),
+                    "reference_y_px": float(reference_y_px),
+                    "roi_y_px": [int(roi_top), int(roi_bottom)],
+                },
+            )
+        )
+    if not candidates:
+        return None
+    return min(candidates, key=lambda item: item[0])[1]
+
+
 def summarize_zero_frames(
     frames: Sequence[Mapping[str, float]],
 ) -> dict[str, float]:
@@ -109,8 +224,6 @@ def summarize_zero_frames(
         "t0_y_px",
         "t3_x_px",
         "t3_y_px",
-        "t0_rad",
-        "t3_rad",
     )
     values: dict[str, list[float]] = {name: [] for name in required}
     for frame in frames:
@@ -134,10 +247,6 @@ def summarize_zero_frames(
         "t0_y_px": float(t0_median[1]),
         "t3_x_px": float(t3_median[0]),
         "t3_y_px": float(t3_median[1]),
-        "t0_rad": circular_median_rad(values["t0_rad"]),
-        "t3_rad": circular_median_rad(values["t3_rad"]),
-        "t0_std_rad": circular_std_rad(values["t0_rad"]),
-        "t3_std_rad": circular_std_rad(values["t3_rad"]),
         "relative_position_rms_px": float(
             math.sqrt(float(np.mean(np.square(relative_error))))
         ),
@@ -314,11 +423,11 @@ def measure_zero_from_circle(
     circle: Mapping[str, Any],
     zero_summary: Mapping[str, float],
 ) -> dict[str, float]:
-    """Measure command-255 radial angle from a fitted trajectory circle.
+    """Measure the fixed inward radial angle from a fitted trajectory circle.
 
-    The radius has two equivalent line directions.  T3's ordered +x axis is
-    used only to choose the direction closest to the historical convention;
-    it does not set the radial angle itself.
+    The public zero convention is always T3 centre -> trajectory centre.
+    Neither AprilTag corner ordering nor the printed T3 +x direction is used,
+    so rotating or mounting T3 upside down cannot flip the zero by pi.
     """
     centre = np.asarray(circle["centre_relative_xy_px"], dtype=float)
     point = np.asarray(
@@ -328,25 +437,16 @@ def measure_zero_from_circle(
         ],
         dtype=float,
     )
-    vector = point - centre
-    distance = float(np.linalg.norm(vector))
+    inward = centre - point
+    distance = float(np.linalg.norm(inward))
     if not math.isfinite(distance) or distance < 1.0e-9:
         raise ValueError("zero point is at the fitted circle centre")
-    first = wrap_angle_rad(math.atan2(-float(vector[1]), float(vector[0])))
-    second = wrap_angle_rad(first + math.pi)
-    t3_rad = float(zero_summary["t3_rad"])
-    first_error = abs(signed_angle_difference_rad(first, t3_rad))
-    second_error = abs(signed_angle_difference_rad(second, t3_rad))
-    table_rad = first if first_error <= second_error else second
-    alignment_error = min(first_error, second_error)
-    t0_rad = float(zero_summary["t0_rad"])
+    table_rad = wrap_angle_rad(
+        math.atan2(-float(inward[1]), float(inward[0]))
+    )
     radius = float(circle["radius_px"])
     return {
-        "palm_rad": signed_angle_difference_rad(table_rad, t0_rad),
         "table_rad": table_rad,
-        "t0_rad": t0_rad,
-        "t3_rad": t3_rad,
-        "axis_alignment_error_rad": alignment_error,
         "zero_radial_error_px": abs(distance - radius),
     }
 
@@ -358,8 +458,6 @@ def build_trajectory_zero_angle_payload(
     trajectory_quality: Mapping[str, Any],
     maximum_round_difference_rad: float,
     maximum_return_error_rad: float,
-    maximum_anchor_drift_rad: float,
-    maximum_axis_alignment_rad: float,
     maximum_zero_radial_error_px: float,
     detection_rate: float,
     minimum_detection_rate: float,
@@ -370,20 +468,12 @@ def build_trajectory_zero_angle_payload(
     if not 0.0 <= detection_rate <= 1.0:
         raise ValueError("detection_rate must be in [0, 1]")
 
-    palm_rounds: list[float] = []
     table_rounds: list[float] = []
     return_errors: list[float] = []
-    anchor_drift: list[float] = []
-    alignment_errors: list[float] = []
     zero_radial_errors: list[float] = []
     for round_value in rounds:
         before = round_value["zero_before"]
         after = round_value["zero_after"]
-        palm_rounds.append(
-            circular_mean_rad(
-                [float(before["palm_rad"]), float(after["palm_rad"])]
-            )
-        )
         table_rounds.append(
             circular_mean_rad(
                 [float(before["table_rad"]), float(after["table_rad"])]
@@ -397,20 +487,6 @@ def build_trajectory_zero_angle_payload(
                 )
             )
         )
-        anchor_drift.append(
-            abs(
-                signed_angle_difference_rad(
-                    float(after["t0_rad"]),
-                    float(before["t0_rad"]),
-                )
-            )
-        )
-        alignment_errors.extend(
-            [
-                float(before["axis_alignment_error_rad"]),
-                float(after["axis_alignment_error_rad"]),
-            ]
-        )
         zero_radial_errors.extend(
             [
                 float(before["zero_radial_error_px"]),
@@ -418,21 +494,17 @@ def build_trajectory_zero_angle_payload(
             ]
         )
 
-    palm_difference = maximum_pairwise_angle_difference_rad(palm_rounds)
     table_difference = maximum_pairwise_angle_difference_rad(table_rounds)
     passed = bool(
         bool(trajectory_quality.get("passed", False))
         and detection_rate >= minimum_detection_rate
-        and palm_difference <= maximum_round_difference_rad
         and table_difference <= maximum_round_difference_rad
         and max(return_errors) <= maximum_return_error_rad
-        and max(anchor_drift) <= maximum_anchor_drift_rad
-        and max(alignment_errors) <= maximum_axis_alignment_rad
         and max(zero_radial_errors) <= maximum_zero_radial_error_px
     )
 
     payload: dict[str, Any] = {
-        "schema_version": 1,
+        "schema_version": 2,
         "model": "G20",
         "side": "left",
         "serial_number": str(serial_number),
@@ -440,24 +512,21 @@ def build_trajectory_zero_angle_payload(
         "angle_unit": "rad",
         "zero_command_u8": 255,
         "zero_angles": {
-            "palm_relative_zero_rad": circular_median_rad(palm_rounds),
             "table_projected_zero_rad": circular_median_rad(table_rounds),
         },
         "quality": {
             "passed": passed,
-            "three_run_palm_max_difference_rad": palm_difference,
             "three_run_table_max_difference_rad": table_difference,
         },
     }
     report: dict[str, Any] = {
-        "measurement_method": "t3_center_image_trajectory_circle",
+        "measurement_method": (
+            "t3_center_to_circle_centre_image_trajectory"
+        ),
         "detection_rate": float(detection_rate),
         "trajectory_circle": dict(trajectory_quality),
-        "palm_rounds_rad": palm_rounds,
         "table_rounds_rad": table_rounds,
         "return_errors_rad": return_errors,
-        "anchor_drift_rad": anchor_drift,
-        "axis_alignment_errors_rad": alignment_errors,
         "zero_radial_errors_px": zero_radial_errors,
         "passed": passed,
     }
@@ -478,8 +547,8 @@ def validate_zero_angle_payload(payload: Mapping[str, Any]) -> None:
         "quality",
     }:
         raise ValueError("unexpected top-level zero calibration fields")
-    if payload["schema_version"] != 1:
-        raise ValueError("schema_version must be 1")
+    if payload["schema_version"] != 2:
+        raise ValueError("schema_version must be 2")
     if payload["model"] != "G20" or payload["side"] != "left":
         raise ValueError("payload must describe the left G20")
     if payload["joint"] != "thumb_cmc_pitch":
@@ -487,25 +556,198 @@ def validate_zero_angle_payload(payload: Mapping[str, Any]) -> None:
     if payload["angle_unit"] != "rad" or payload["zero_command_u8"] != 255:
         raise ValueError("zero calibration must use radians and command 255")
     zero_angles = payload["zero_angles"]
-    if set(zero_angles) != {
-        "palm_relative_zero_rad",
-        "table_projected_zero_rad",
-    }:
+    if set(zero_angles) != {"table_projected_zero_rad"}:
         raise ValueError("unexpected zero angle fields")
     if not all(math.isfinite(float(value)) for value in zero_angles.values()):
         raise ValueError("zero angles must be finite")
     quality = payload["quality"]
     if set(quality) != {
         "passed",
-        "three_run_palm_max_difference_rad",
         "three_run_table_max_difference_rad",
     }:
         raise ValueError("unexpected quality fields")
     if not isinstance(quality["passed"], bool):
         raise ValueError("quality.passed must be boolean")
-    for name in (
-        "three_run_palm_max_difference_rad",
+    for name in ("three_run_table_max_difference_rad",):
+        value = float(quality[name])
+        if not math.isfinite(value) or value < 0.0:
+            raise ValueError(f"quality.{name} must be finite and non-negative")
+
+
+def build_trajectory_zero_travel_payload(
+    *,
+    serial_number: str,
+    joint_name: str,
+    zero_command_u8: int,
+    travel_endpoint_command_u8: int,
+    rounds: Sequence[Mapping[str, Mapping[str, float]]],
+    trajectory_quality: Mapping[str, Any],
+    maximum_round_difference_rad: float,
+    maximum_travel_difference_rad: float,
+    minimum_travel_rad: float,
+    maximum_return_error_rad: float,
+    maximum_zero_radial_error_px: float,
+    detection_rate: float,
+    minimum_detection_rate: float,
+) -> tuple[dict[str, Any], dict[str, Any]]:
+    """Create a compact zero-and-travel payload from three circle sweeps."""
+    if joint_name != "thumb_cmc_roll":
+        raise ValueError("zero-and-travel payload only supports thumb_cmc_roll")
+    if len(rounds) != 3:
+        raise ValueError("exactly three rounds are required")
+    if not 0.0 <= detection_rate <= 1.0:
+        raise ValueError("detection_rate must be in [0, 1]")
+    if not 0 <= zero_command_u8 <= 255:
+        raise ValueError("zero_command_u8 must be in [0, 255]")
+    if not 0 <= travel_endpoint_command_u8 <= 255:
+        raise ValueError("travel_endpoint_command_u8 must be in [0, 255]")
+    if zero_command_u8 == travel_endpoint_command_u8:
+        raise ValueError("zero and travel endpoint commands must differ")
+
+    table_rounds: list[float] = []
+    endpoint_table_rounds: list[float] = []
+    signed_travel_rounds: list[float] = []
+    return_errors: list[float] = []
+    radial_errors: list[float] = []
+    for round_value in rounds:
+        before = round_value["zero_before"]
+        endpoint = round_value["travel_endpoint"]
+        after = round_value["zero_after"]
+        zero_table = circular_mean_rad(
+            [float(before["table_rad"]), float(after["table_rad"])]
+        )
+        endpoint_table = float(endpoint["table_rad"])
+        table_rounds.append(zero_table)
+        endpoint_table_rounds.append(endpoint_table)
+        signed_travel_rounds.append(
+            signed_angle_difference_rad(endpoint_table, zero_table)
+        )
+        return_errors.append(
+            abs(
+                signed_angle_difference_rad(
+                    float(after["table_rad"]),
+                    float(before["table_rad"]),
+                )
+            )
+        )
+        radial_errors.extend(
+            float(value["zero_radial_error_px"])
+            for value in (before, endpoint, after)
+        )
+
+    table_difference = maximum_pairwise_angle_difference_rad(table_rounds)
+    travel_difference = maximum_pairwise_angle_difference_rad(
+        signed_travel_rounds
+    )
+    signed_travel = circular_median_rad(signed_travel_rounds)
+    travel_range = float(np.median(np.abs(signed_travel_rounds)))
+    passed = bool(
+        bool(trajectory_quality.get("passed", False))
+        and detection_rate >= minimum_detection_rate
+        and table_difference <= maximum_round_difference_rad
+        and travel_difference <= maximum_travel_difference_rad
+        and travel_range >= minimum_travel_rad
+        and max(return_errors) <= maximum_return_error_rad
+        and max(radial_errors) <= maximum_zero_radial_error_px
+    )
+
+    payload: dict[str, Any] = {
+        "schema_version": 2,
+        "model": "G20",
+        "side": "left",
+        "serial_number": str(serial_number),
+        "joint": joint_name,
+        "angle_unit": "rad",
+        "zero_command_u8": int(zero_command_u8),
+        "travel_endpoint_command_u8": int(travel_endpoint_command_u8),
+        "zero_angles": {
+            "table_projected_zero_rad": circular_median_rad(table_rounds),
+        },
+        "travel": {
+            "signed_rad": signed_travel,
+            "range_rad": travel_range,
+        },
+        "quality": {
+            "passed": passed,
+            "three_run_table_max_difference_rad": table_difference,
+            "three_run_travel_max_difference_rad": travel_difference,
+        },
+    }
+    report: dict[str, Any] = {
+        "measurement_method": (
+            "t3_center_to_circle_centre_image_trajectory"
+        ),
+        "detection_rate": float(detection_rate),
+        "trajectory_circle": dict(trajectory_quality),
+        "table_zero_rounds_rad": table_rounds,
+        "endpoint_table_rounds_rad": endpoint_table_rounds,
+        "signed_travel_rounds_rad": signed_travel_rounds,
+        "return_errors_rad": return_errors,
+        "radial_errors_px": radial_errors,
+        "minimum_travel_rad": float(minimum_travel_rad),
+        "passed": passed,
+    }
+    return payload, report
+
+
+def validate_zero_travel_payload(payload: Mapping[str, Any]) -> None:
+    """Validate the public left-G20 CMC-roll zero-and-travel schema."""
+    if set(payload) != {
+        "schema_version",
+        "model",
+        "side",
+        "serial_number",
+        "joint",
+        "angle_unit",
+        "zero_command_u8",
+        "travel_endpoint_command_u8",
+        "zero_angles",
+        "travel",
+        "quality",
+    }:
+        raise ValueError("unexpected top-level zero-and-travel fields")
+    if payload["schema_version"] != 2:
+        raise ValueError("schema_version must be 2")
+    if payload["model"] != "G20" or payload["side"] != "left":
+        raise ValueError("payload must describe the left G20")
+    if payload["joint"] != "thumb_cmc_roll":
+        raise ValueError("payload must describe thumb_cmc_roll")
+    if payload["angle_unit"] != "rad":
+        raise ValueError("zero-and-travel calibration must use radians")
+    for name in ("zero_command_u8", "travel_endpoint_command_u8"):
+        value = payload[name]
+        if not isinstance(value, int) or not 0 <= value <= 255:
+            raise ValueError(f"{name} must be an integer in [0, 255]")
+    if payload["zero_command_u8"] == payload["travel_endpoint_command_u8"]:
+        raise ValueError("zero and travel endpoint commands must differ")
+
+    zero_angles = payload["zero_angles"]
+    if set(zero_angles) != {"table_projected_zero_rad"}:
+        raise ValueError("unexpected zero angle fields")
+    if not all(math.isfinite(float(value)) for value in zero_angles.values()):
+        raise ValueError("zero angles must be finite")
+
+    travel = payload["travel"]
+    if set(travel) != {"signed_rad", "range_rad"}:
+        raise ValueError("unexpected travel fields")
+    if not math.isfinite(float(travel["signed_rad"])):
+        raise ValueError("travel.signed_rad must be finite")
+    travel_range = float(travel["range_rad"])
+    if not math.isfinite(travel_range) or travel_range < 0.0:
+        raise ValueError("travel.range_rad must be finite and non-negative")
+
+    quality = payload["quality"]
+    if set(quality) != {
+        "passed",
         "three_run_table_max_difference_rad",
+        "three_run_travel_max_difference_rad",
+    }:
+        raise ValueError("unexpected quality fields")
+    if not isinstance(quality["passed"], bool):
+        raise ValueError("quality.passed must be boolean")
+    for name in (
+        "three_run_table_max_difference_rad",
+        "three_run_travel_max_difference_rad",
     ):
         value = float(quality[name])
         if not math.isfinite(value) or value < 0.0:

src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/zero_node.py

@@ -1,4 +1,4 @@
-"""ROS 2 node for trajectory-circle G20 CMC pitch zero measurement."""
+"""ROS 2 node for trajectory-circle G20 CMC zero/travel measurement."""
 
 from __future__ import annotations
 
@@ -25,11 +25,13 @@ from .core import BASELINE_COMMAND, build_command, COMMAND_NAMES
 from .storage import atomic_write_json
 from .zero_calibration import (
     build_trajectory_zero_angle_payload,
+    build_trajectory_zero_travel_payload,
+    detect_reference_alignment_line,
     fit_image_circle_trajectory,
     measure_zero_from_circle,
     summarize_zero_frames,
-    tag_x_axis_angle_rad,
     validate_zero_angle_payload,
+    validate_zero_travel_payload,
 )
 
 
@@ -56,6 +58,7 @@ STATE_ZH = {
 STAGE_ZH = {
     "zero_before": "255零位（运动前）",
     "sweep_out": "圆心轨迹（255到轨迹端点）",
+    "travel_endpoint": "行程端点",
     "sweep_return": "圆心轨迹（返回255）",
     "zero_after": "255零位（返回后）",
 }
@@ -66,18 +69,19 @@ def _safe_name(value: str) -> str:
     return safe or "UNSET"
 
 
-class G20ThumbCmcPitchZeroNode(Node):
-    """Measure CMC zero from the T3-centre image trajectory circle."""
+class G20ThumbCmcTrajectoryNode(Node):
+    """Measure a CMC zero and optional travel from the T3-centre circle."""
 
     def __init__(self) -> None:
-        super().__init__("g20_thumb_cmc_pitch_zero")
+        super().__init__("g20_thumb_cmc_trajectory_calibration")
         self._declare_parameters()
         self._load_parameters()
 
         self.session_dir.mkdir(parents=True, exist_ok=True)
+        result_kind = "zero_travel" if self.measure_travel else "zero"
         self.result_path = self.session_dir / (
             f"g20_left_{_safe_name(self.serial_number)}"
-            "_thumb_cmc_pitch_zero.json"
+            f"_{self.joint_name}_{result_kind}.json"
         )
 
         self.state = STATE_PREFLIGHT
@@ -87,6 +91,11 @@ class G20ThumbCmcPitchZeroNode(Node):
         self.latest_quality: dict[str, dict[str, Any]] = {}
         self.latest_angles: dict[str, float] = {}
         self.preflight_flags: deque[bool] = deque(maxlen=self.preflight_frames)
+        self.camera_alignment_measurements: deque[dict[str, Any] | None] = deque(
+            maxlen=self.camera_alignment_required_frames
+        )
+        self.latest_camera_alignment: dict[str, Any] = {}
+        self.last_camera_alignment_update = 0.0
         self.session_detection_flags: list[bool] = []
 
         self.round_index = 0
@@ -102,6 +111,7 @@ class G20ThumbCmcPitchZeroNode(Node):
         self.trajectory_frames: list[dict[str, float]] = []
         self.latest_trajectory_frames: list[dict[str, float]] = []
         self.latest_circle: dict[str, Any] = {}
+        self.latest_travel: dict[str, float] = {}
         self.final_payload: dict[str, Any] | None = None
         self.final_report: dict[str, Any] = {}
 
@@ -129,7 +139,7 @@ class G20ThumbCmcPitchZeroNode(Node):
             self._detections_callback,
             qos_profile_sensor_data,
         )
-        if self.publish_debug_image:
+        if self.publish_debug_image or self.camera_alignment_enabled:
             self.create_subscription(
                 Image,
                 self.image_topic,
@@ -142,7 +152,7 @@ class G20ThumbCmcPitchZeroNode(Node):
         self.timer = self.create_timer(0.05, self._timer_callback)
         self.status_timer = self.create_timer(0.5, self._publish_status)
         self.get_logger().info(
-            "CMC pitch zero session: "
+            f"{self.joint_name} calibration session: "
             f"{self.session_dir}; result={self.result_path}"
         )
 
@@ -163,8 +173,23 @@ class G20ThumbCmcPitchZeroNode(Node):
         self.declare_parameter("publish_debug_image", True)
         self.declare_parameter("debug_max_rate_hz", 10.0)
         self.declare_parameter("debug_scale", 0.75)
+        self.declare_parameter("camera_alignment_enabled", False)
+        self.declare_parameter("camera_alignment_reference_y_ratio", 0.90)
+        self.declare_parameter("camera_alignment_roi_y_min_ratio", 0.55)
+        self.declare_parameter("camera_alignment_roi_y_max_ratio", 0.98)
+        self.declare_parameter("camera_alignment_minimum_line_length_ratio", 0.30)
+        self.declare_parameter("camera_alignment_max_candidate_angle_deg", 15.0)
+        self.declare_parameter("camera_alignment_max_angle_deg", 0.5)
+        self.declare_parameter("camera_alignment_max_vertical_offset_px", 12.0)
+        self.declare_parameter("camera_alignment_required_frames", 10)
+        self.declare_parameter("camera_alignment_minimum_detection_rate", 0.8)
+        self.declare_parameter("camera_alignment_max_age_seconds", 1.0)
         self.declare_parameter("t0_id", 0)
         self.declare_parameter("t3_id", 1)
+        self.declare_parameter("joint_name", "thumb_cmc_pitch")
+        self.declare_parameter("motor_index", 0)
+        self.declare_parameter("zero_command_u8", 255)
+        self.declare_parameter("measure_travel", False)
         self.declare_parameter(
             "baseline_command_u8", list(BASELINE_COMMAND)
         )
@@ -192,11 +217,10 @@ class G20ThumbCmcPitchZeroNode(Node):
         self.declare_parameter("maximum_hamming", 0)
         self.declare_parameter("minimum_decision_margin", 30.0)
         self.declare_parameter("minimum_edge_pixels", 40.0)
-        self.declare_parameter("maximum_static_std_deg", 0.5)
         self.declare_parameter("maximum_round_difference_deg", 1.0)
+        self.declare_parameter("maximum_travel_difference_deg", 1.0)
+        self.declare_parameter("minimum_travel_deg", 10.0)
         self.declare_parameter("maximum_return_error_deg", 1.0)
-        self.declare_parameter("maximum_anchor_drift_deg", 1.0)
-        self.declare_parameter("maximum_axis_alignment_deg", 45.0)
         self.declare_parameter("maximum_zero_radial_error_px", 4.0)
         self.declare_parameter("maximum_static_position_rms_px", 1.5)
 
@@ -220,11 +244,48 @@ class G20ThumbCmcPitchZeroNode(Node):
         self.publish_debug_image = bool(value("publish_debug_image"))
         self.debug_max_rate_hz = float(value("debug_max_rate_hz"))
         self.debug_scale = float(value("debug_scale"))
+        self.camera_alignment_enabled = bool(
+            value("camera_alignment_enabled")
+        )
+        self.camera_alignment_reference_y_ratio = float(
+            value("camera_alignment_reference_y_ratio")
+        )
+        self.camera_alignment_roi_y_min_ratio = float(
+            value("camera_alignment_roi_y_min_ratio")
+        )
+        self.camera_alignment_roi_y_max_ratio = float(
+            value("camera_alignment_roi_y_max_ratio")
+        )
+        self.camera_alignment_minimum_line_length_ratio = float(
+            value("camera_alignment_minimum_line_length_ratio")
+        )
+        self.camera_alignment_max_candidate_angle_rad = math.radians(
+            float(value("camera_alignment_max_candidate_angle_deg"))
+        )
+        self.camera_alignment_max_angle_rad = math.radians(
+            float(value("camera_alignment_max_angle_deg"))
+        )
+        self.camera_alignment_max_vertical_offset_px = float(
+            value("camera_alignment_max_vertical_offset_px")
+        )
+        self.camera_alignment_required_frames = int(
+            value("camera_alignment_required_frames")
+        )
+        self.camera_alignment_minimum_detection_rate = float(
+            value("camera_alignment_minimum_detection_rate")
+        )
+        self.camera_alignment_max_age_seconds = float(
+            value("camera_alignment_max_age_seconds")
+        )
         self.t0_id = int(value("t0_id"))
         self.t3_id = int(value("t3_id"))
         self.role_by_id = {self.t0_id: "t0", self.t3_id: "t3"}
         if self.t0_id == self.t3_id:
             raise ValueError("t0_id and t3_id must differ")
+        self.joint_name = str(value("joint_name"))
+        self.motor_index = int(value("motor_index"))
+        self.zero_command_u8 = int(value("zero_command_u8"))
+        self.measure_travel = bool(value("measure_travel"))
         self.baseline_command = tuple(
             int(item) for item in value("baseline_command_u8")
         )
@@ -268,21 +329,18 @@ class G20ThumbCmcPitchZeroNode(Node):
             value("minimum_decision_margin")
         )
         self.minimum_edge_pixels = float(value("minimum_edge_pixels"))
-        self.maximum_static_std_rad = math.radians(
-            float(value("maximum_static_std_deg"))
-        )
         self.maximum_round_difference_rad = math.radians(
             float(value("maximum_round_difference_deg"))
         )
+        self.maximum_travel_difference_rad = math.radians(
+            float(value("maximum_travel_difference_deg"))
+        )
+        self.minimum_travel_rad = math.radians(
+            float(value("minimum_travel_deg"))
+        )
         self.maximum_return_error_rad = math.radians(
             float(value("maximum_return_error_deg"))
         )
-        self.maximum_anchor_drift_rad = math.radians(
-            float(value("maximum_anchor_drift_deg"))
-        )
-        self.maximum_axis_alignment_rad = math.radians(
-            float(value("maximum_axis_alignment_deg"))
-        )
         self.maximum_zero_radial_error_px = float(
             value("maximum_zero_radial_error_px")
         )
@@ -292,19 +350,81 @@ class G20ThumbCmcPitchZeroNode(Node):
         self._validate_parameters()
 
     def _validate_parameters(self) -> None:
+        expected_motor_indices = {
+            "thumb_cmc_pitch": 0,
+            "thumb_cmc_roll": 5,
+        }
+        if self.joint_name not in expected_motor_indices:
+            raise ValueError(
+                "joint_name must be thumb_cmc_pitch or thumb_cmc_roll"
+            )
+        if self.motor_index != expected_motor_indices[self.joint_name]:
+            raise ValueError(
+                f"{self.joint_name} requires motor_index "
+                f"{expected_motor_indices[self.joint_name]}"
+            )
+        if self.measure_travel and self.joint_name != "thumb_cmc_roll":
+            raise ValueError(
+                "measure_travel is currently supported only for thumb_cmc_roll"
+            )
+        if not (
+            0.0
+            <= self.camera_alignment_roi_y_min_ratio
+            < self.camera_alignment_reference_y_ratio
+            < self.camera_alignment_roi_y_max_ratio
+            <= 1.0
+        ):
+            raise ValueError(
+                "camera alignment ratios must satisfy "
+                "0 <= roi_min < reference < roi_max <= 1"
+            )
+        if not (
+            0.0 < self.camera_alignment_minimum_line_length_ratio <= 1.0
+        ):
+            raise ValueError(
+                "camera_alignment_minimum_line_length_ratio must be in (0, 1]"
+            )
+        if not (
+            self.camera_alignment_max_angle_rad
+            < self.camera_alignment_max_candidate_angle_rad
+            < math.pi / 2.0
+        ):
+            raise ValueError(
+                "camera alignment acceptance angle must be smaller than "
+                "the candidate angle"
+            )
+        if self.camera_alignment_required_frames < 3:
+            raise ValueError(
+                "camera_alignment_required_frames must be at least 3"
+            )
+        if not 0.0 < self.camera_alignment_minimum_detection_rate <= 1.0:
+            raise ValueError(
+                "camera_alignment_minimum_detection_rate must be in (0, 1]"
+            )
         if len(self.baseline_command) != 20:
             raise ValueError("baseline_command_u8 must contain 20 values")
         if any(value < 0 or value > 255 for value in self.baseline_command):
             raise ValueError("baseline command values must be in [0, 255]")
-        if self.baseline_command[0] != 255:
-            raise ValueError("baseline motor 0 command must be 255")
+        if not 0 <= self.zero_command_u8 <= 255:
+            raise ValueError("zero_command_u8 must be in [0, 255]")
+        if self.baseline_command[self.motor_index] != self.zero_command_u8:
+            raise ValueError(
+                "baseline command for the calibrated motor must equal "
+                "zero_command_u8"
+            )
         if self.repetitions != 3:
             raise ValueError("CMC zero calibration requires exactly 3 repetitions")
         if self.zero_capture_frames != 30:
             raise ValueError("zero_capture_frames must be 30")
-        if not 0 <= self.trajectory_command_u8 < 255:
-            raise ValueError("trajectory_command_u8 must be in [0, 254]")
-        available_span = 255 - self.trajectory_command_u8
+        if not 0 <= self.trajectory_command_u8 <= 255:
+            raise ValueError("trajectory_command_u8 must be in [0, 255]")
+        if self.trajectory_command_u8 == self.zero_command_u8:
+            raise ValueError(
+                "trajectory_command_u8 must differ from zero_command_u8"
+            )
+        available_span = abs(
+            self.zero_command_u8 - self.trajectory_command_u8
+        )
         if self.trajectory_minimum_state_span_u8 > available_span:
             raise ValueError(
                 "trajectory_minimum_state_span_u8 exceeds commanded span"
@@ -316,6 +436,9 @@ class G20ThumbCmcPitchZeroNode(Node):
         if min(
             self.debug_max_rate_hz,
             self.debug_scale,
+            self.camera_alignment_max_angle_rad,
+            self.camera_alignment_max_vertical_offset_px,
+            self.camera_alignment_max_age_seconds,
             self.trajectory_bin_size_u8,
             self.trajectory_minimum_state_span_u8,
             self.trajectory_minimum_radius_px,
@@ -330,11 +453,10 @@ class G20ThumbCmcPitchZeroNode(Node):
             self.state_tolerance_u8,
             self.minimum_decision_margin,
             self.minimum_edge_pixels,
-            self.maximum_static_std_rad,
             self.maximum_round_difference_rad,
+            self.maximum_travel_difference_rad,
+            self.minimum_travel_rad,
             self.maximum_return_error_rad,
-            self.maximum_anchor_drift_rad,
-            self.maximum_axis_alignment_rad,
             self.maximum_zero_radial_error_px,
             self.maximum_static_position_rms_px,
         ) <= 0.0:
@@ -403,22 +525,15 @@ class G20ThumbCmcPitchZeroNode(Node):
         if not good:
             return
 
-        try:
-            t0_rad = tag_x_axis_angle_rad(corners_by_role["t0"])
-            t3_rad = tag_x_axis_angle_rad(corners_by_role["t3"])
-        except ValueError:
-            return
         t0_centre = np.mean(corners_by_role["t0"], axis=0)
         t3_centre = np.mean(corners_by_role["t3"], axis=0)
         frame = {
-            "t0_rad": t0_rad,
-            "t3_rad": t3_rad,
             "t0_x_px": float(t0_centre[0]),
             "t0_y_px": float(t0_centre[1]),
             "t3_x_px": float(t3_centre[0]),
             "t3_y_px": float(t3_centre[1]),
             "state_u8": (
-                float(self.latest_state_u8[0])
+                float(self.latest_state_u8[self.motor_index])
                 if len(self.latest_state_u8) == 20
                 else float("nan")
             ),
@@ -446,13 +561,6 @@ class G20ThumbCmcPitchZeroNode(Node):
         if len(self.capture_frames) < self.required_frames:
             return
         summary = summarize_zero_frames(list(self.capture_frames))
-        maximum_std = max(summary["t0_std_rad"], summary["t3_std_rad"])
-        if maximum_std > self.maximum_static_std_rad:
-            self.reason = (
-                f"{self.active_stage}_not_stable:"
-                f"{math.degrees(maximum_std):.2f}deg"
-            )
-            return
         if (
             summary["relative_position_rms_px"]
             > self.maximum_static_position_rms_px
@@ -472,6 +580,127 @@ class G20ThumbCmcPitchZeroNode(Node):
             and float(quality["edge_pixels"]) >= self.minimum_edge_pixels
         )
 
+    def _update_camera_alignment(
+        self, image: np.ndarray, now: float
+    ) -> None:
+        if not self.camera_alignment_enabled:
+            return
+        height = int(image.shape[0])
+        reference_y = self.camera_alignment_reference_y_ratio * float(
+            height - 1
+        )
+        detected = detect_reference_alignment_line(
+            image,
+            reference_y_px=reference_y,
+            roi_y_min_ratio=self.camera_alignment_roi_y_min_ratio,
+            roi_y_max_ratio=self.camera_alignment_roi_y_max_ratio,
+            minimum_length_ratio=(
+                self.camera_alignment_minimum_line_length_ratio
+            ),
+            maximum_candidate_angle_rad=(
+                self.camera_alignment_max_candidate_angle_rad
+            ),
+        )
+        self.last_camera_alignment_update = now
+        self.camera_alignment_measurements.append(detected)
+        valid_measurements = [
+            measurement
+            for measurement in self.camera_alignment_measurements
+            if measurement is not None
+        ]
+        window_frames = len(self.camera_alignment_measurements)
+        detected_frames = len(valid_measurements)
+        detection_rate = (
+            float(detected_frames) / float(window_frames)
+            if window_frames
+            else 0.0
+        )
+        if not valid_measurements:
+            self.latest_camera_alignment = {
+                "enabled": True,
+                "detected": False,
+                "sample_passed": False,
+                "window_frames": window_frames,
+                "detected_frames": 0,
+                "detection_rate": 0.0,
+            }
+        else:
+            latest_valid = valid_measurements[-1]
+            line_xyxy = np.median(
+                np.asarray(
+                    [
+                        measurement["line_xyxy_px"]
+                        for measurement in valid_measurements
+                    ],
+                    dtype=float,
+                ),
+                axis=0,
+            )
+            angle_rad = float(
+                np.median(
+                    [
+                        float(measurement["angle_rad"])
+                        for measurement in valid_measurements
+                    ]
+                )
+            )
+            vertical_offset_px = float(
+                np.median(
+                    [
+                        float(measurement["vertical_offset_px"])
+                        for measurement in valid_measurements
+                    ]
+                )
+            )
+            sample_passed = bool(
+                abs(angle_rad) <= self.camera_alignment_max_angle_rad
+                and abs(vertical_offset_px)
+                <= self.camera_alignment_max_vertical_offset_px
+            )
+            self.latest_camera_alignment = {
+                "enabled": True,
+                "detected": True,
+                "line_xyxy_px": [float(value) for value in line_xyxy],
+                "angle_rad": angle_rad,
+                "vertical_offset_px": vertical_offset_px,
+                "reference_y_px": float(latest_valid["reference_y_px"]),
+                "roi_y_px": list(latest_valid["roi_y_px"]),
+                "sample_passed": sample_passed,
+                "window_frames": window_frames,
+                "detected_frames": detected_frames,
+                "detection_rate": detection_rate,
+            }
+        self.latest_camera_alignment.update(
+            {
+                "required_frames": self.camera_alignment_required_frames,
+                "minimum_detection_rate": (
+                    self.camera_alignment_minimum_detection_rate
+                ),
+                "maximum_angle_rad": self.camera_alignment_max_angle_rad,
+                "maximum_vertical_offset_px": (
+                    self.camera_alignment_max_vertical_offset_px
+                ),
+                "ready": self._camera_alignment_ready(now),
+            }
+        )
+
+    def _camera_alignment_ready(self, now: float | None = None) -> bool:
+        if not self.camera_alignment_enabled:
+            return True
+        current_time = time.monotonic() if now is None else float(now)
+        return bool(
+            self.latest_camera_alignment.get("detected", False)
+            and current_time - self.last_camera_alignment_update
+            <= self.camera_alignment_max_age_seconds
+            and int(self.latest_camera_alignment.get("window_frames", 0))
+            == self.camera_alignment_required_frames
+            and float(self.latest_camera_alignment.get("detection_rate", 0.0))
+            >= self.camera_alignment_minimum_detection_rate
+            and bool(
+                self.latest_camera_alignment.get("sample_passed", False)
+            )
+        )
+
     def _start_callback(
         self, request: Trigger.Request, response: Trigger.Response
     ) -> Trigger.Response:
@@ -503,7 +732,7 @@ class G20ThumbCmcPitchZeroNode(Node):
         self._reset_run()
         self._begin_stage("zero_before")
         response.success = True
-        response.message = "CMC pitch zero calibration started"
+        response.message = f"{self.joint_name} calibration started"
         return response
 
     def _abort_callback(
@@ -532,23 +761,40 @@ class G20ThumbCmcPitchZeroNode(Node):
         self.latest_trajectory_frames = []
         self.latest_circle = {}
         self.latest_angles = {}
+        self.latest_travel = {}
         self.session_detection_flags = []
         self.final_payload = None
         self.final_report = {}
 
     def _begin_stage(self, stage: str) -> None:
-        if stage not in {"zero_before", "zero_after"}:
+        allowed_stages = {"zero_before", "zero_after"}
+        if self.measure_travel:
+            allowed_stages.add("travel_endpoint")
+        if stage not in allowed_stages:
             raise ValueError(f"unknown stage {stage}")
         self.active_stage = stage
-        self.active_target_u8 = 255
+        self.active_target_u8 = (
+            self.trajectory_command_u8
+            if stage == "travel_endpoint"
+            else self.zero_command_u8
+        )
         self.required_frames = self.zero_capture_frames
         self.capture_frames = deque(maxlen=self.required_frames)
         self.stage_started_at = time.monotonic()
         self.target_reached_at = None
         self.capture_started_at = 0.0
         self.state = STATE_MOVING
-        self.reason = f"round_{self.round_index + 1}:{stage}:target=255"
-        self._publish_hand_command(build_command(0, 255, self.baseline_command))
+        self.reason = (
+            f"round_{self.round_index + 1}:{stage}:"
+            f"target={self.active_target_u8}"
+        )
+        self._publish_hand_command(
+            build_command(
+                self.motor_index,
+                self.active_target_u8,
+                self.baseline_command,
+            )
+        )
 
     def _begin_sweep(self, stage: str) -> None:
         if stage not in {"sweep_out", "sweep_return"}:
@@ -557,7 +803,7 @@ class G20ThumbCmcPitchZeroNode(Node):
             self.trajectory_frames = []
             target = self.trajectory_command_u8
         else:
-            target = 255
+            target = self.zero_command_u8
         self.active_stage = stage
         self.active_target_u8 = target
         self.required_frames = self.trajectory_minimum_frames
@@ -569,7 +815,7 @@ class G20ThumbCmcPitchZeroNode(Node):
             f"target={target}:collecting_trajectory"
         )
         self._publish_hand_command(
-            build_command(0, target, self.baseline_command)
+            build_command(self.motor_index, target, self.baseline_command)
         )
 
     def _fit_trajectory(
@@ -601,7 +847,7 @@ class G20ThumbCmcPitchZeroNode(Node):
             self.active_target_u8 is not None
             and len(self.latest_state_u8) == 20
             and abs(
-                float(self.latest_state_u8[0])
+                float(self.latest_state_u8[self.motor_index])
                 - float(self.active_target_u8)
             )
             <= self.state_tolerance_u8
@@ -617,7 +863,10 @@ class G20ThumbCmcPitchZeroNode(Node):
             ):
                 self.state = STATE_WAIT_START
                 self.reason = "call_start_service"
-            elif len(self.preflight_flags) == self.preflight_frames:
+            elif (
+                len(self.preflight_flags) == self.preflight_frames
+                and self._preflight_rate() < self.minimum_detection_rate
+            ):
                 self.reason = (
                     "T0/T3 detection rate too low:"
                     f"{self._preflight_rate():.3f}"
@@ -703,6 +952,9 @@ class G20ThumbCmcPitchZeroNode(Node):
         if stage == "zero_before":
             self._begin_sweep("sweep_out")
             return
+        if stage == "travel_endpoint":
+            self._begin_sweep("sweep_return")
+            return
 
         self.rounds.append(dict(self.current_round))
         self.current_round = {}
@@ -715,7 +967,10 @@ class G20ThumbCmcPitchZeroNode(Node):
     def _finish_sweep(self) -> None:
         stage = self.active_stage
         if stage == "sweep_out":
-            self._begin_sweep("sweep_return")
+            if self.measure_travel:
+                self._begin_stage("travel_endpoint")
+            else:
+                self._begin_sweep("sweep_return")
             return
         if stage != "sweep_return":
             self._pause(f"unexpected_sweep_stage:{stage}")
@@ -749,6 +1004,15 @@ class G20ThumbCmcPitchZeroNode(Node):
                     "zero_before": measure_zero_from_circle(
                         circle, round_value["zero_before"]
                     ),
+                    **(
+                        {
+                            "travel_endpoint": measure_zero_from_circle(
+                                circle, round_value["travel_endpoint"]
+                            )
+                        }
+                        if self.measure_travel
+                        else {}
+                    ),
                     "zero_after": measure_zero_from_circle(
                         circle, round_value["zero_after"]
                     ),
@@ -758,28 +1022,56 @@ class G20ThumbCmcPitchZeroNode(Node):
         except ValueError as error:
             self._pause(f"final_trajectory_fit_failed:{error}")
             return
-        payload, report = build_trajectory_zero_angle_payload(
-            serial_number=self.serial_number,
-            rounds=measured_rounds,
-            trajectory_quality=circle,
-            maximum_round_difference_rad=self.maximum_round_difference_rad,
-            maximum_return_error_rad=self.maximum_return_error_rad,
-            maximum_anchor_drift_rad=self.maximum_anchor_drift_rad,
-            maximum_axis_alignment_rad=self.maximum_axis_alignment_rad,
-            maximum_zero_radial_error_px=self.maximum_zero_radial_error_px,
-            detection_rate=detection_rate,
-            minimum_detection_rate=self.minimum_detection_rate,
-        )
-        validate_zero_angle_payload(payload)
+        if self.measure_travel:
+            payload, report = build_trajectory_zero_travel_payload(
+                serial_number=self.serial_number,
+                joint_name=self.joint_name,
+                zero_command_u8=self.zero_command_u8,
+                travel_endpoint_command_u8=self.trajectory_command_u8,
+                rounds=measured_rounds,
+                trajectory_quality=circle,
+                maximum_round_difference_rad=(
+                    self.maximum_round_difference_rad
+                ),
+                maximum_travel_difference_rad=(
+                    self.maximum_travel_difference_rad
+                ),
+                minimum_travel_rad=self.minimum_travel_rad,
+                maximum_return_error_rad=self.maximum_return_error_rad,
+                maximum_zero_radial_error_px=(
+                    self.maximum_zero_radial_error_px
+                ),
+                detection_rate=detection_rate,
+                minimum_detection_rate=self.minimum_detection_rate,
+            )
+            validate_zero_travel_payload(payload)
+        else:
+            payload, report = build_trajectory_zero_angle_payload(
+                serial_number=self.serial_number,
+                rounds=measured_rounds,
+                trajectory_quality=circle,
+                maximum_round_difference_rad=(
+                    self.maximum_round_difference_rad
+                ),
+                maximum_return_error_rad=self.maximum_return_error_rad,
+                maximum_zero_radial_error_px=(
+                    self.maximum_zero_radial_error_px
+                ),
+                detection_rate=detection_rate,
+                minimum_detection_rate=self.minimum_detection_rate,
+            )
+            validate_zero_angle_payload(payload)
         atomic_write_json(self.result_path, payload)
         self.final_payload = payload
         self.final_report = report
         self.latest_circle = circle
         self.latest_trajectory_frames = all_trajectory_frames
         self.latest_angles = {
-            "palm_rad": payload["zero_angles"]["palm_relative_zero_rad"],
             "table_rad": payload["zero_angles"]["table_projected_zero_rad"],
         }
+        self.latest_travel = (
+            dict(payload["travel"]) if self.measure_travel else {}
+        )
         self.state = STATE_COMPLETE
         self.reason = (
             "result_passed"
@@ -787,9 +1079,9 @@ class G20ThumbCmcPitchZeroNode(Node):
             else "result_written_but_quality_failed"
         )
         self.active_stage = None
-        self.active_target_u8 = 255
+        self.active_target_u8 = self.zero_command_u8
         self.get_logger().info(
-            f"CMC pitch zero result written: {self.result_path}; "
+            f"{self.joint_name} result written: {self.result_path}; "
             f"passed={payload['quality']['passed']}"
         )
 
@@ -797,7 +1089,9 @@ class G20ThumbCmcPitchZeroNode(Node):
         self.state = STATE_PAUSED
         self.reason = reason
         self.capture_frames.clear()
-        self.get_logger().error(f"CMC pitch zero calibration paused: {reason}")
+        self.get_logger().error(
+            f"{self.joint_name} calibration paused: {reason}"
+        )
 
     def _preflight_rate(self) -> float:
         return (
@@ -809,12 +1103,15 @@ class G20ThumbCmcPitchZeroNode(Node):
     def _motor_value_text(self) -> str:
         if len(self.latest_state_u8) != 20:
             return "unavailable"
-        return f"{self.latest_state_u8[0]:.1f}"
+        return f"{self.latest_state_u8[self.motor_index]:.1f}"
 
     def _image_callback(self, message: Image) -> None:
         if (
-            self.debug_publisher is None
-            or self.debug_publisher.get_subscription_count() < 1
+            not self.camera_alignment_enabled
+            and (
+                self.debug_publisher is None
+                or self.debug_publisher.get_subscription_count() < 1
+            )
         ):
             return
         now = time.monotonic()
@@ -828,6 +1125,12 @@ class G20ThumbCmcPitchZeroNode(Node):
         except Exception as error:
             self.get_logger().warning(f"debug image conversion failed: {error}")
             return
+        self._update_camera_alignment(image, now)
+        if (
+            self.debug_publisher is None
+            or self.debug_publisher.get_subscription_count() < 1
+        ):
+            return
         if self.debug_scale != 1.0:
             image = cv2.resize(
                 image,
@@ -838,23 +1141,48 @@ class G20ThumbCmcPitchZeroNode(Node):
             )
 
         height, width = image.shape[:2]
-        reference_y = max(0, height - 55)
+        reference_y = int(
+            round(self.camera_alignment_reference_y_ratio * (height - 1))
+        )
         cv2.line(
             image,
             (20, reference_y),
             (max(20, width - 20), reference_y),
             (0, 0, 255),
-            3,
+            5,
         )
         cv2.putText(
             image,
-            "image +x / table projection",
+            "RED target / image +x",
             (25, max(25, reference_y - 8)),
             cv2.FONT_HERSHEY_SIMPLEX,
             0.55,
             (0, 0, 255),
             2,
         )
+        if self.latest_camera_alignment.get("detected", False):
+            line = np.asarray(
+                self.latest_camera_alignment["line_xyxy_px"], dtype=float
+            ).reshape(2, 2)
+            line *= self.debug_scale
+            left = tuple(np.rint(line[0]).astype(int))
+            right = tuple(np.rint(line[1]).astype(int))
+            clipped, left, right = cv2.clipLine(
+                (0, 0, width, height), left, right
+            )
+            if clipped:
+                cv2.line(image, left, right, (255, 0, 0), 2)
+                label_x = max(20, min(width - 260, left[0] + 10))
+                label_y = max(25, min(height - 15, left[1] - 8))
+                cv2.putText(
+                    image,
+                    "BLUE detected reference",
+                    (label_x, label_y),
+                    cv2.FONT_HERSHEY_SIMPLEX,
+                    0.55,
+                    (255, 0, 0),
+                    2,
+                )
         colors = {"t0": (0, 210, 0), "t3": (0, 170, 255)}
         for role in ("t0", "t3"):
             corners = self.latest_corners.get(role)
@@ -863,17 +1191,7 @@ class G20ThumbCmcPitchZeroNode(Node):
             points = np.rint(corners * self.debug_scale).astype(np.int32)
             cv2.polylines(image, [points], True, colors[role], 2)
             centre = np.mean(points, axis=0)
-            try:
-                angle = tag_x_axis_angle_rad(corners)
-            except ValueError:
-                continue
-            vector = np.asarray(
-                [math.cos(angle), -math.sin(angle)],
-                dtype=float,
-            )
             start = tuple(np.rint(centre).astype(int))
-            end = tuple(np.rint(centre + 65.0 * vector).astype(int))
-            cv2.arrowedLine(image, start, end, colors[role], 3, tipLength=0.2)
             cv2.putText(
                 image,
                 role.upper(),
@@ -971,22 +1289,40 @@ class G20ThumbCmcPitchZeroNode(Node):
             else (0, 0, 255),
             2,
         )
-        if self.latest_angles:
+        angle_text_y = 58
+        if self.camera_alignment_enabled:
+            alignment_ready = self._camera_alignment_ready(now)
+            if self.latest_camera_alignment.get("detected", False):
+                angle_deg = math.degrees(
+                    float(self.latest_camera_alignment["angle_rad"])
+                )
+                offset_px = float(
+                    self.latest_camera_alignment["vertical_offset_px"]
+                )
+                alignment_text = (
+                    f"red-blue angle {angle_deg:+.2f} deg  "
+                    f"dy {offset_px:+.1f}px  "
+                    f"hit {self.latest_camera_alignment.get('detected_frames', 0)}/"
+                    f"{self.camera_alignment_required_frames}  "
+                    f"{'ALIGNED' if alignment_ready else 'ADJUST'}"
+                )
+            else:
+                alignment_text = "BLUE reference line not detected"
             cv2.putText(
                 image,
-                "palm zero: "
-                f"{math.degrees(self.latest_angles['palm_rad']):.2f} deg",
-                (20, 58),
+                alignment_text,
+                (20, angle_text_y),
                 cv2.FONT_HERSHEY_SIMPLEX,
-                0.62,
-                (0, 255, 255),
+                0.60,
+                (0, 255, 0) if alignment_ready else (0, 165, 255),
                 2,
             )
+        if self.latest_angles:
             cv2.putText(
                 image,
                 "table zero: "
                 f"{math.degrees(self.latest_angles['table_rad']):.2f} deg",
-                (20, 86),
+                (20, 86 if self.camera_alignment_enabled else 58),
                 cv2.FONT_HERSHEY_SIMPLEX,
                 0.62,
                 (0, 255, 255),
@@ -1000,7 +1336,7 @@ class G20ThumbCmcPitchZeroNode(Node):
             cv2.putText(
                 image,
                 "zero angle: waiting for trajectory circle",
-                (20, 58),
+                (20, 86 if self.camera_alignment_enabled else 58),
                 cv2.FONT_HERSHEY_SIMPLEX,
                 0.58,
                 (0, 255, 255),
@@ -1021,24 +1357,35 @@ class G20ThumbCmcPitchZeroNode(Node):
                     **quality,
                     "valid": self._quality_valid(quality),
                 }
+        camera_alignment = {
+            "enabled": self.camera_alignment_enabled,
+            **self.latest_camera_alignment,
+            "ready": self._camera_alignment_ready(),
+        }
         payload = {
             "state": self.state,
             "state_zh": STATE_ZH[self.state],
             "reason": self.reason,
-            "measurement_method": "t3_center_image_trajectory_circle",
+            "measurement_method": (
+                "t3_center_to_circle_centre_image_trajectory"
+            ),
+            "joint": self.joint_name,
+            "motor_index": self.motor_index,
+            "zero_command_u8": self.zero_command_u8,
+            "measure_travel": self.measure_travel,
             "cycle": min(self.round_index + 1, self.repetitions),
             "cycles_total": self.repetitions,
             "stage": self.active_stage,
             "stage_zh": (
                 None
                 if self.active_stage is None
-                else STAGE_ZH[self.active_stage]
+                else self._stage_zh(self.active_stage)
             ),
             "active_command_u8": self.active_target_u8,
             "actual_command_u8": (
                 None
                 if len(self.latest_state_u8) != 20
-                else self.latest_state_u8[0]
+                else self.latest_state_u8[self.motor_index]
             ),
             "capture_frames_seen": len(self.capture_frames),
             "capture_frames_required": self.required_frames,
@@ -1047,7 +1394,9 @@ class G20ThumbCmcPitchZeroNode(Node):
             "trajectory_circle": self.latest_circle,
             "preflight_detection_rate": self._preflight_rate(),
             "tag_quality": tag_status,
+            "camera_alignment": camera_alignment,
             "latest_angles_rad": self.latest_angles,
+            "latest_travel_rad": self.latest_travel,
             "result_path": str(self.result_path),
             "result": self.final_payload,
             "quality_report": self.final_report,
@@ -1059,6 +1408,22 @@ class G20ThumbCmcPitchZeroNode(Node):
         text.data = self._status_text(payload)
         self.status_text_publisher.publish(text)
 
+    def _stage_zh(self, stage: str) -> str:
+        if stage == "zero_before":
+            return f"{self.zero_command_u8}零位（运动前）"
+        if stage == "sweep_out":
+            return (
+                f"圆心轨迹（{self.zero_command_u8}到"
+                f"{self.trajectory_command_u8}）"
+            )
+        if stage == "travel_endpoint":
+            return f"{self.trajectory_command_u8}行程端点"
+        if stage == "sweep_return":
+            return f"圆心轨迹（返回{self.zero_command_u8}）"
+        if stage == "zero_after":
+            return f"{self.zero_command_u8}零位（返回后）"
+        return STAGE_ZH.get(stage, stage)
+
     def _status_text(self, payload: dict[str, Any]) -> str:
         reason_zh = self._reason_zh()
         if self.state == STATE_CAPTURING:
@@ -1075,6 +1440,10 @@ class G20ThumbCmcPitchZeroNode(Node):
         lines = [
             f"状态：{payload['state_zh']}",
             f"原因：{reason_zh}",
+            (
+                f"关节：{self.joint_name}，电机索引"
+                f"{self.motor_index}"
+            ),
             (
                 f"进度：第{payload['cycle']}/{self.repetitions}轮，"
                 f"{payload['stage_zh'] or '无活动阶段'}"
@@ -1095,15 +1464,42 @@ class G20ThumbCmcPitchZeroNode(Node):
                 f"置信度{status['decision_margin']:.1f}，"
                 f"hamming={status['hamming']}"
             )
+        alignment = payload["camera_alignment"]
+        if alignment["enabled"]:
+            if alignment.get("detected", False):
+                angle_deg = math.degrees(float(alignment["angle_rad"]))
+                offset_px = float(alignment["vertical_offset_px"])
+                lines.extend(
+                    [
+                        "相机二维对齐：",
+                        (
+                            f"- 红蓝线夹角 {angle_deg:+.3f}°，要求 "
+                            "±"
+                            f"{math.degrees(self.camera_alignment_max_angle_rad):.3f}°"
+                        ),
+                        (
+                            f"- 垂直偏差 {offset_px:+.1f}px，要求 "
+                            f"±{self.camera_alignment_max_vertical_offset_px:.1f}px"
+                        ),
+                        (
+                            "- 状态 "
+                            f"{'已对齐' if alignment['ready'] else '请调整相机'}"
+                            "，"
+                            f"窗口 {alignment.get('window_frames', 0)}/"
+                            f"{self.camera_alignment_required_frames}，"
+                            f"有效帧 {alignment.get('detected_frames', 0)}/"
+                            f"{self.camera_alignment_required_frames}"
+                        ),
+                    ]
+                )
+            else:
+                lines.append(
+                    "相机二维对齐：未识别到下方物理参考直线"
+                )
         if self.latest_angles:
             lines.extend(
                 [
-                    "圆心径向零位角：",
-                    (
-                        "- 掌心相对零位 "
-                        f"{self.latest_angles['palm_rad']:.6f} rad "
-                        f"({math.degrees(self.latest_angles['palm_rad']):.2f}°)"
-                    ),
+                    "内向径向零位角：",
                     (
                         "- 桌面投影零位 "
                         f"{self.latest_angles['table_rad']:.6f} rad "
@@ -1131,9 +1527,23 @@ class G20ThumbCmcPitchZeroNode(Node):
                     ),
                 ]
             )
+        if self.latest_travel:
+            lines.extend(
+                [
+                    "角行程：",
+                    (
+                        f"- 有符号 {self.latest_travel['signed_rad']:.6f} rad "
+                        f"({math.degrees(self.latest_travel['signed_rad']):.2f}°)"
+                    ),
+                    (
+                        f"- 行程大小 {self.latest_travel['range_rad']:.6f} rad "
+                        f"({math.degrees(self.latest_travel['range_rad']):.2f}°)"
+                    ),
+                ]
+            )
         if self.state == STATE_WAIT_START:
             lines.append(
-                "下一步：调用 /g20_thumb_cmc_pitch_zero/start 开始标定"
+                f"下一步：调用 /{self.get_name()}/start 开始标定"
             )
         elif self.state == STATE_PAUSED:
             lines.append("下一步：排除问题后再次调用 start，从第一轮重做")
@@ -1181,9 +1591,9 @@ class G20ThumbCmcPitchZeroNode(Node):
 
 def main(args: list[str] | None = None) -> None:
     rclpy.init(args=args)
-    node: G20ThumbCmcPitchZeroNode | None = None
+    node: G20ThumbCmcTrajectoryNode | None = None
     try:
-        node = G20ThumbCmcPitchZeroNode()
+        node = G20ThumbCmcTrajectoryNode()
         rclpy.spin(node)
     except KeyboardInterrupt:
         pass

src/g20_thumb_apriltag_calibration/launch/front_cmc_roll_calibration.launch.py

@@ -0,0 +1,242 @@
+"""Launch front-camera trajectory-circle CMC roll zero/travel calibration."""
+
+from __future__ import annotations
+
+from datetime import datetime
+from pathlib import Path
+import re
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import (
+    DeclareLaunchArgument,
+    IncludeLaunchDescription,
+    LogInfo,
+    OpaqueFunction,
+)
+from launch.conditions import IfCondition
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import ComposableNodeContainer, Node
+from launch_ros.descriptions import ComposableNode
+from launch_ros.parameter_descriptions import ParameterValue
+
+
+def _launch_stack(context):
+    serial_number = LaunchConfiguration("serial_number").perform(context)
+    if (
+        not serial_number
+        or serial_number == "UNSET"
+        or re.fullmatch(r"[A-Za-z0-9_.-]+", serial_number) is None
+        or serial_number in {".", ".."}
+    ):
+        raise RuntimeError(
+            "serial_number is required and may contain only letters, "
+            "digits, dot, underscore and dash"
+        )
+
+    requested_session = LaunchConfiguration("session_dir").perform(context)
+    output_root = Path(
+        LaunchConfiguration("output_root").perform(context)
+    ).expanduser().resolve()
+    if requested_session:
+        session_dir = Path(requested_session).expanduser().resolve()
+    else:
+        timestamp = datetime.now().strftime("%Y%m%d_%H%M%S")
+        session_dir = output_root / serial_number / timestamp
+    session_dir.mkdir(parents=True, exist_ok=True)
+
+    tag_config = LaunchConfiguration("tag_config").perform(context)
+    calibration_config = LaunchConfiguration(
+        "calibration_config"
+    ).perform(context)
+    realsense_config = LaunchConfiguration("realsense_config").perform(context)
+    realsense_launch = (
+        Path(get_package_share_directory("realsense2_camera"))
+        / "launch"
+        / "rs_launch.py"
+    )
+
+    camera = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(str(realsense_launch)),
+        condition=IfCondition(LaunchConfiguration("start_camera")),
+        launch_arguments={
+            "camera_name": "camera",
+            "camera_namespace": "camera",
+            "serial_no": LaunchConfiguration("camera_serial_number"),
+            "config_file": realsense_config,
+            "enable_color": "true",
+            "rgb_camera.color_profile": LaunchConfiguration("color_profile"),
+            "depth_module.color_profile": LaunchConfiguration("color_profile"),
+            "enable_depth": "false",
+            "enable_sync": "false",
+            "align_depth.enable": "false",
+            "diagnostics_period": "1.0",
+        }.items(),
+    )
+
+    raw_topic = "/camera/camera/color/image_raw"
+    camera_info_topic = "/camera/camera/color/camera_info"
+    rect_topic = "/camera/camera/color/image_rect"
+    vision_container = ComposableNodeContainer(
+        name="g20_thumb_roll_vision_container",
+        namespace="/",
+        package="rclcpp_components",
+        executable="component_container_mt",
+        composable_node_descriptions=[
+            ComposableNode(
+                package="image_proc",
+                plugin="image_proc::RectifyNode",
+                name="rectify_color",
+                namespace="/camera/camera/color",
+                remappings=[
+                    ("image", raw_topic),
+                    ("camera_info", camera_info_topic),
+                    ("image_rect", rect_topic),
+                ],
+                parameters=[{"queue_size": 1}],
+                extra_arguments=[{"use_intra_process_comms": True}],
+            ),
+            ComposableNode(
+                package="apriltag_ros",
+                plugin="AprilTagNode",
+                name="apriltag",
+                namespace="/apriltag",
+                parameters=[
+                    tag_config,
+                    {
+                        "detector.decimate": ParameterValue(
+                            LaunchConfiguration("apriltag_decimate"),
+                            value_type=float,
+                        )
+                    },
+                ],
+                remappings=[
+                    ("image_rect", rect_topic),
+                    ("camera_info", camera_info_topic),
+                ],
+                extra_arguments=[{"use_intra_process_comms": True}],
+            ),
+        ],
+        output="screen",
+        emulate_tty=True,
+    )
+
+    sdk = Node(
+        package="linker_hand_ros2_sdk",
+        executable="linker_hand_sdk",
+        name="linker_hand_sdk",
+        output="screen",
+        condition=IfCondition(LaunchConfiguration("start_sdk")),
+        parameters=[
+            {
+                "hand_type": "left",
+                "hand_joint": "G20",
+                "can": LaunchConfiguration("can_interface"),
+                "modbus": "None",
+                "topic_prefix": "/g20",
+                "move_on_startup": False,
+                "startup_speed": ParameterValue(
+                    LaunchConfiguration("calibration_speed"),
+                    value_type=int,
+                ),
+                "startup_torque": 80,
+                "state_poll_rate": 10.0,
+                "repeat_position_commands": False,
+                "is_touch": False,
+            }
+        ],
+    )
+
+    calibration_node = Node(
+        package="g20_thumb_apriltag_calibration",
+        executable="cmc_roll_calibration_node",
+        name="g20_thumb_cmc_roll_calibration",
+        output="screen",
+        parameters=[
+            calibration_config,
+            {
+                "serial_number": serial_number,
+                "session_dir": str(session_dir),
+                "commands_enabled": ParameterValue(
+                    LaunchConfiguration("commands_enabled"),
+                    value_type=bool,
+                ),
+                "image_topic": rect_topic,
+                "publish_debug_image": ParameterValue(
+                    LaunchConfiguration("publish_debug_image"),
+                    value_type=bool,
+                ),
+            },
+        ],
+    )
+
+    return [
+        LogInfo(msg=f"G20 CMC roll calibration session: {session_dir}"),
+        LogInfo(
+            msg=(
+                "Only T0(ID 0) and T3(ID 1) are required; "
+                "T4/T5 detections are ignored"
+            )
+        ),
+        LogInfo(
+            msg=(
+                "Motor 5 performs three 255->0->255 sweeps; "
+                "static captures measure both zero and angular travel"
+            )
+        ),
+        camera,
+        vision_container,
+        sdk,
+        calibration_node,
+    ]
+
+
+def generate_launch_description() -> LaunchDescription:
+    package_share = Path(
+        get_package_share_directory("g20_thumb_apriltag_calibration")
+    )
+    return LaunchDescription(
+        [
+            DeclareLaunchArgument("serial_number", default_value="UNSET"),
+            DeclareLaunchArgument("camera_serial_number", default_value=""),
+            DeclareLaunchArgument(
+                "color_profile", default_value="1280x720x30"
+            ),
+            DeclareLaunchArgument("can_interface", default_value="can0"),
+            DeclareLaunchArgument("calibration_speed", default_value="15"),
+            DeclareLaunchArgument("apriltag_decimate", default_value="1.5"),
+            DeclareLaunchArgument("commands_enabled", default_value="true"),
+            DeclareLaunchArgument(
+                "publish_debug_image", default_value="true"
+            ),
+            DeclareLaunchArgument("start_camera", default_value="true"),
+            DeclareLaunchArgument("start_sdk", default_value="true"),
+            DeclareLaunchArgument(
+                "output_root",
+                default_value=str(Path.cwd() / "calibration_output"),
+            ),
+            DeclareLaunchArgument("session_dir", default_value=""),
+            DeclareLaunchArgument(
+                "calibration_config",
+                default_value=str(
+                    package_share
+                    / "config"
+                    / "cmc_roll_zero_travel.yaml"
+                ),
+            ),
+            DeclareLaunchArgument(
+                "realsense_config",
+                default_value=str(
+                    package_share / "config" / "realsense_color_qos.yaml"
+                ),
+            ),
+            DeclareLaunchArgument(
+                "tag_config",
+                default_value=str(
+                    package_share / "config" / "front_tags.yaml"
+                ),
+            ),
+            OpaqueFunction(function=_launch_stack),
+        ]
+    )

src/g20_thumb_apriltag_calibration/setup.py

@@ -32,6 +32,10 @@ setup(
                 "cmc_pitch_zero_node = "
                 "g20_thumb_apriltag_calibration.zero_node:main"
             ),
+            (
+                "cmc_roll_calibration_node = "
+                "g20_thumb_apriltag_calibration.zero_node:main"
+            ),
         ],
     },
 )

src/g20_thumb_apriltag_calibration/test/test_config.py

@@ -104,6 +104,10 @@ def test_cmc_pitch_zero_config_uses_three_trajectory_circle_rounds() -> None:
 
     assert parameters["t0_id"] == 0
     assert parameters["t3_id"] == 1
+    assert parameters["joint_name"] == "thumb_cmc_pitch"
+    assert parameters["motor_index"] == 0
+    assert parameters["zero_command_u8"] == 255
+    assert parameters["measure_travel"] is False
     assert parameters["baseline_command_u8"] == [
         255,
         255,
@@ -136,6 +140,45 @@ def test_cmc_pitch_zero_config_uses_three_trajectory_circle_rounds() -> None:
     assert parameters["trajectory_maximum_p95_radial_error_px"] <= 3.5
     assert parameters["minimum_detection_rate"] == 0.95
     assert parameters["minimum_edge_pixels"] == 40.0
-    assert parameters["maximum_static_std_deg"] == 0.5
     assert parameters["maximum_static_position_rms_px"] <= 1.5
+    assert "maximum_axis_alignment_deg" not in parameters
+    assert "maximum_static_std_deg" not in parameters
+    assert "maximum_anchor_drift_deg" not in parameters
+    assert parameters["camera_alignment_enabled"] is True
+    assert parameters["camera_alignment_max_angle_deg"] <= 0.5
+    assert parameters["camera_alignment_max_vertical_offset_px"] <= 12.0
+    assert parameters["camera_alignment_required_frames"] >= 10
+    assert parameters["camera_alignment_minimum_detection_rate"] <= 0.8
+
+
+def test_cmc_roll_config_measures_full_endpoint_travel() -> None:
+    config = yaml.safe_load(
+        (PACKAGE_ROOT / "config" / "cmc_roll_zero_travel.yaml").read_text()
+    )
+    parameters = config[
+        "g20_thumb_cmc_roll_calibration"
+    ]["ros__parameters"]
+
+    assert parameters["t0_id"] == 0
+    assert parameters["t3_id"] == 1
+    assert parameters["joint_name"] == "thumb_cmc_roll"
+    assert parameters["motor_index"] == 5
+    assert parameters["zero_command_u8"] == 255
+    assert parameters["trajectory_command_u8"] == 0
+    assert parameters["measure_travel"] is True
+    assert parameters["baseline_command_u8"][5] == 255
+    assert parameters["repetitions"] == 3
+    assert parameters["zero_capture_frames"] == 30
+    assert parameters["trajectory_minimum_state_span_u8"] >= 240.0
+    assert parameters["trajectory_minimum_bins"] >= 30
+    assert parameters["minimum_travel_deg"] >= 20.0
+    assert parameters["maximum_travel_difference_deg"] <= 1.0
     assert parameters["maximum_round_difference_deg"] == 1.0
+    assert "maximum_axis_alignment_deg" not in parameters
+    assert "maximum_static_std_deg" not in parameters
+    assert "maximum_anchor_drift_deg" not in parameters
+    assert parameters["camera_alignment_enabled"] is True
+    assert parameters["camera_alignment_max_angle_deg"] <= 0.5
+    assert parameters["camera_alignment_max_vertical_offset_px"] <= 12.0
+    assert parameters["camera_alignment_required_frames"] >= 10
+    assert parameters["camera_alignment_minimum_detection_rate"] <= 0.8

src/g20_thumb_apriltag_calibration/test/test_core.py

@@ -86,8 +86,12 @@ def test_build_command_changes_only_selected_channel() -> None:
     assert result[15] == 37
     assert result[:15] == list(BASELINE_COMMAND[:15])
     assert result[16:] == list(BASELINE_COMMAND[16:])
+    roll_result = build_command(5, 20)
+    assert roll_result[5] == 20
+    assert roll_result[:5] == list(BASELINE_COMMAND[:5])
+    assert roll_result[6:] == list(BASELINE_COMMAND[6:])
     with pytest.raises(ValueError):
-        build_command(5, 10)
+        build_command(6, 10)
 
 
 def test_three_cycle_scan_has_every_integer_in_both_directions() -> None:

src/g20_thumb_apriltag_calibration/test/test_zero_calibration.py

@@ -2,31 +2,31 @@ from __future__ import annotations
 
 import math
 
+import cv2
 from g20_thumb_apriltag_calibration.zero_calibration import (
     build_trajectory_zero_angle_payload,
+    build_trajectory_zero_travel_payload,
     circular_median_rad,
+    detect_reference_alignment_line,
     fit_image_circle_trajectory,
     measure_zero_from_circle,
     signed_angle_difference_rad,
     summarize_zero_frames,
-    tag_x_axis_angle_rad,
     validate_zero_angle_payload,
+    validate_zero_travel_payload,
 )
 import numpy as np
 import pytest
 
 
-def _corners(angle_rad: float) -> np.ndarray:
-    x_axis = np.asarray([math.cos(angle_rad), -math.sin(angle_rad)])
-    y_axis = np.asarray([math.sin(angle_rad), math.cos(angle_rad)])
-    centre = np.asarray([400.0, 300.0])
-    return np.asarray(
-        [
-            centre - 20.0 * x_axis - 20.0 * y_axis,
-            centre + 20.0 * x_axis - 20.0 * y_axis,
-            centre + 20.0 * x_axis + 20.0 * y_axis,
-            centre - 20.0 * x_axis + 20.0 * y_axis,
-        ]
+def _detect_reference_line(image: np.ndarray, reference_y: float) -> dict | None:
+    return detect_reference_alignment_line(
+        image,
+        reference_y_px=reference_y,
+        roi_y_min_ratio=0.55,
+        roi_y_max_ratio=0.98,
+        minimum_length_ratio=0.30,
+        maximum_candidate_angle_rad=math.radians(15.0),
     )
 
 
@@ -34,8 +34,6 @@ def _frame(
     table_angle_rad: float,
     *,
     state_u8: float = 255.0,
-    t0_angle_rad: float = math.radians(3.0),
-    t3_angle_rad: float | None = None,
     t0_xy: tuple[float, float] = (500.0, 300.0),
     circle_xy: tuple[float, float] = (-120.0, 80.0),
     radius_px: float = 90.0,
@@ -52,10 +50,6 @@ def _frame(
         "t0_y_px": float(t0[1]),
         "t3_x_px": float(t3[0]),
         "t3_y_px": float(t3[1]),
-        "t0_rad": t0_angle_rad,
-        "t3_rad": (
-            table_angle_rad if t3_angle_rad is None else t3_angle_rad
-        ),
     }
 
 
@@ -95,17 +89,10 @@ def _fit(observations: list[dict[str, float]] | None = None) -> dict:
     )
 
 
-def _zero_summary(
-    angle_rad: float,
-    *,
-    t0_rad: float = math.radians(3.0),
-    t3_rad: float | None = None,
-) -> dict[str, float]:
+def _zero_summary(angle_rad: float) -> dict[str, float]:
     frames = [
         _frame(
             angle_rad + math.radians(index - 14.5) * 1.0e-4,
-            t0_angle_rad=t0_rad,
-            t3_angle_rad=t3_rad,
         )
         for index in range(30)
     ]
@@ -113,44 +100,43 @@ def _zero_summary(
 
 
 def _measurement(
-    palm_rad: float,
     table_rad: float,
     *,
-    t0_rad: float,
     radial_error_px: float = 0.2,
 ) -> dict[str, float]:
     return {
-        "palm_rad": palm_rad,
         "table_rad": table_rad,
-        "t0_rad": t0_rad,
-        "t3_rad": table_rad,
-        "axis_alignment_error_rad": math.radians(2.0),
         "zero_radial_error_px": radial_error_px,
     }
 
 
 def _round(
-    palm_rad: float,
     table_rad: float,
     *,
     return_delta: float = 0.002,
 ) -> dict[str, dict[str, float]]:
-    t0_rad = table_rad - palm_rad
     return {
-        "zero_before": _measurement(
-            palm_rad, table_rad, t0_rad=t0_rad
-        ),
+        "zero_before": _measurement(table_rad),
         "zero_after": _measurement(
-            palm_rad + return_delta,
             table_rad + return_delta,
-            t0_rad=t0_rad,
         ),
     }
 
 
-def test_tag_angle_uses_ordered_opposite_edges() -> None:
-    expected = math.radians(27.0)
-    assert tag_x_axis_angle_rad(_corners(expected)) == pytest.approx(expected)
+def _travel_round(
+    zero_table_rad: float,
+    travel_rad: float,
+    *,
+    return_delta: float = 0.002,
+) -> dict[str, dict[str, float]]:
+    round_value = _round(
+        zero_table_rad,
+        return_delta=return_delta,
+    )
+    round_value["travel_endpoint"] = _measurement(
+        zero_table_rad + travel_rad,
+    )
+    return round_value
 
 
 def test_circular_statistics_cross_pi_without_jumping() -> None:
@@ -159,6 +145,35 @@ def test_circular_statistics_cross_pi_without_jumping() -> None:
     assert abs(signed_angle_difference_rad(result, math.pi)) < math.radians(1.1)
 
 
+def test_detect_reference_line_reports_signed_angle_and_offset() -> None:
+    height, width = 720, 1280
+    reference_y = 0.90 * (height - 1)
+    expected_angle = math.radians(2.0)
+    image = np.zeros((height, width, 3), dtype=np.uint8)
+    half_span = 560.0
+    vertical_change = math.tan(expected_angle) * half_span
+    cv2.line(
+        image,
+        (80, int(round(reference_y + vertical_change))),
+        (1200, int(round(reference_y - vertical_change))),
+        (255, 255, 255),
+        5,
+    )
+
+    result = _detect_reference_line(image, reference_y)
+
+    assert result is not None
+    assert result["angle_rad"] == pytest.approx(expected_angle, abs=0.004)
+    assert abs(result["vertical_offset_px"]) <= 5.0
+
+
+def test_detect_reference_line_rejects_non_horizontal_scene() -> None:
+    image = np.zeros((720, 1280, 3), dtype=np.uint8)
+    cv2.line(image, (640, 420), (640, 700), (255, 255, 255), 5)
+
+    assert _detect_reference_line(image, 0.90 * 719.0) is None
+
+
 def test_circle_fit_recovers_center_radius_and_rejects_anchor_translation() -> None:
     fit = _fit()
     assert fit["centre_relative_xy_px"] == pytest.approx(
@@ -182,22 +197,22 @@ def test_circle_fit_is_robust_to_sparse_bad_tag_centres() -> None:
     assert fit["radius_px"] == pytest.approx(90.0, abs=1.5)
 
 
-def test_zero_angle_comes_from_circle_radius_not_tag_rotation() -> None:
+def test_zero_uses_fixed_inward_radius_and_ignores_t3_rotation() -> None:
     circle = _fit()
-    expected_table = math.radians(10.0)
-    summary = _zero_summary(
-        expected_table,
-        t0_rad=math.radians(3.0),
-        t3_rad=math.radians(31.0),
-    )
+    trajectory_angle = math.radians(10.0)
+    expected_table = math.radians(-170.0)
+    summary = _zero_summary(trajectory_angle)
+    summary_with_t3_forward = {**summary, "t3_rad": math.radians(31.0)}
+    summary_with_t3_flipped = {
+        **summary,
+        "t3_rad": math.radians(-149.0),
+    }
     result = measure_zero_from_circle(circle, summary)
+    forward = measure_zero_from_circle(circle, summary_with_t3_forward)
+    flipped = measure_zero_from_circle(circle, summary_with_t3_flipped)
     assert result["table_rad"] == pytest.approx(expected_table, abs=0.01)
-    assert result["palm_rad"] == pytest.approx(
-        math.radians(7.0), abs=0.01
-    )
-    assert result["axis_alignment_error_rad"] == pytest.approx(
-        math.radians(21.0), abs=0.01
-    )
+    assert forward == result
+    assert flipped == result
 
 
 def test_circle_fit_rejects_short_state_span() -> None:
@@ -211,9 +226,9 @@ def test_circle_fit_rejects_short_state_span() -> None:
 
 def test_payload_remains_small_and_contains_circle_derived_angles() -> None:
     rounds = [
-        _round(-0.50, -0.30),
-        _round(-0.498, -0.299),
-        _round(-0.501, -0.302),
+        _round(-0.30),
+        _round(-0.299),
+        _round(-0.302),
     ]
     payload, report = build_trajectory_zero_angle_payload(
         serial_number="G20_LEFT_TEST",
@@ -221,25 +236,20 @@ def test_payload_remains_small_and_contains_circle_derived_angles() -> None:
         trajectory_quality=_fit(),
         maximum_round_difference_rad=math.radians(1.0),
         maximum_return_error_rad=math.radians(1.0),
-        maximum_anchor_drift_rad=math.radians(1.0),
-        maximum_axis_alignment_rad=math.radians(45.0),
         maximum_zero_radial_error_px=4.0,
         detection_rate=0.99,
         minimum_detection_rate=0.95,
     )
     validate_zero_angle_payload(payload)
-    assert set(payload["zero_angles"]) == {
-        "palm_relative_zero_rad",
-        "table_projected_zero_rad",
-    }
-    assert payload["zero_angles"]["palm_relative_zero_rad"] == pytest.approx(
-        -0.499
-    )
+    assert payload["schema_version"] == 2
+    assert set(payload["zero_angles"]) == {"table_projected_zero_rad"}
     assert payload["zero_angles"]["table_projected_zero_rad"] == pytest.approx(
         -0.299
     )
     assert payload["quality"]["passed"] is True
-    assert report["measurement_method"] == "t3_center_image_trajectory_circle"
+    assert report["measurement_method"] == (
+        "t3_center_to_circle_centre_image_trajectory"
+    )
 
 
 @pytest.mark.parametrize(("detection_rate", "radial_error"), [(0.8, 0.2), (0.99, 8.0)])
@@ -249,10 +259,10 @@ def test_quality_fails_for_detection_or_zero_circle_error(
     rounds = [
         {
             "zero_before": _measurement(
-                -0.50, -0.30, t0_rad=0.20, radial_error_px=radial_error
+                -0.30, radial_error_px=radial_error
             ),
             "zero_after": _measurement(
-                -0.499, -0.299, t0_rad=0.20, radial_error_px=radial_error
+                -0.299, radial_error_px=radial_error
             ),
         }
         for _ in range(3)
@@ -263,10 +273,68 @@ def test_quality_fails_for_detection_or_zero_circle_error(
         trajectory_quality=_fit(),
         maximum_round_difference_rad=math.radians(1.0),
         maximum_return_error_rad=math.radians(1.0),
-        maximum_anchor_drift_rad=math.radians(1.0),
-        maximum_axis_alignment_rad=math.radians(45.0),
         maximum_zero_radial_error_px=4.0,
         detection_rate=detection_rate,
         minimum_detection_rate=0.95,
     )
     assert payload["quality"]["passed"] is False
+
+
+def test_roll_payload_contains_zero_and_measured_travel() -> None:
+    rounds = [
+        _travel_round(-0.30, 1.201),
+        _travel_round(-0.299, 1.199),
+        _travel_round(-0.302, 1.200),
+    ]
+    payload, report = build_trajectory_zero_travel_payload(
+        serial_number="G20_LEFT_TEST",
+        joint_name="thumb_cmc_roll",
+        zero_command_u8=255,
+        travel_endpoint_command_u8=0,
+        rounds=rounds,
+        trajectory_quality=_fit(),
+        maximum_round_difference_rad=math.radians(1.0),
+        maximum_travel_difference_rad=math.radians(1.0),
+        minimum_travel_rad=math.radians(20.0),
+        maximum_return_error_rad=math.radians(1.0),
+        maximum_zero_radial_error_px=4.0,
+        detection_rate=0.99,
+        minimum_detection_rate=0.95,
+    )
+
+    validate_zero_travel_payload(payload)
+    assert payload["joint"] == "thumb_cmc_roll"
+    assert payload["schema_version"] == 2
+    assert payload["zero_command_u8"] == 255
+    assert payload["travel_endpoint_command_u8"] == 0
+    assert set(payload["zero_angles"]) == {"table_projected_zero_rad"}
+    assert payload["travel"]["signed_rad"] == pytest.approx(1.2, abs=0.003)
+    assert payload["travel"]["range_rad"] == pytest.approx(1.2, abs=0.003)
+    assert payload["quality"]["passed"] is True
+    assert report["signed_travel_rounds_rad"] == pytest.approx(
+        [1.2, 1.198, 1.199], abs=0.003
+    )
+
+
+def test_roll_quality_rejects_inconsistent_or_short_travel() -> None:
+    rounds = [
+        _travel_round(-0.30, travel)
+        for travel in (0.10, 0.12, 0.14)
+    ]
+    payload, _ = build_trajectory_zero_travel_payload(
+        serial_number="G20_LEFT_TEST",
+        joint_name="thumb_cmc_roll",
+        zero_command_u8=255,
+        travel_endpoint_command_u8=0,
+        rounds=rounds,
+        trajectory_quality=_fit(),
+        maximum_round_difference_rad=math.radians(1.0),
+        maximum_travel_difference_rad=math.radians(1.0),
+        minimum_travel_rad=math.radians(20.0),
+        maximum_return_error_rad=math.radians(1.0),
+        maximum_zero_radial_error_px=4.0,
+        detection_rate=0.99,
+        minimum_detection_rate=0.95,
+    )
+
+    assert payload["quality"]["passed"] is False