diff --git a/src/g20_thumb_apriltag_calibration/README.md b/src/g20_thumb_apriltag_calibration/README.md index 61e267c..46d1c32 100644 --- a/src/g20_thumb_apriltag_calibration/README.md +++ b/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`。 diff --git a/src/g20_thumb_apriltag_calibration/config/cmc_pitch_zero.yaml b/src/g20_thumb_apriltag_calibration/config/cmc_pitch_zero.yaml index f72df8d..7b354fd 100644 --- a/src/g20_thumb_apriltag_calibration/config/cmc_pitch_zero.yaml +++ b/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 diff --git a/src/g20_thumb_apriltag_calibration/config/cmc_roll_zero_travel.yaml b/src/g20_thumb_apriltag_calibration/config/cmc_roll_zero_travel.yaml new file mode 100644 index 0000000..c0847bf --- /dev/null +++ b/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 diff --git a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/core.py b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/core.py index a8cd01a..fd19fd5 100644 --- a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/core.py +++ b/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]") diff --git a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/zero_calibration.py b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/zero_calibration.py index f3aa5e3..31aee58 100644 --- a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/zero_calibration.py +++ b/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: diff --git a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/zero_node.py b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/zero_node.py index 42a0506..ba3885b 100644 --- a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/zero_node.py +++ b/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 diff --git a/src/g20_thumb_apriltag_calibration/launch/front_cmc_roll_calibration.launch.py b/src/g20_thumb_apriltag_calibration/launch/front_cmc_roll_calibration.launch.py new file mode 100644 index 0000000..a396a4f --- /dev/null +++ b/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), + ] + ) diff --git a/src/g20_thumb_apriltag_calibration/setup.py b/src/g20_thumb_apriltag_calibration/setup.py index e408f5f..a615a18 100644 --- a/src/g20_thumb_apriltag_calibration/setup.py +++ b/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" + ), ], }, ) diff --git a/src/g20_thumb_apriltag_calibration/test/test_config.py b/src/g20_thumb_apriltag_calibration/test/test_config.py index bed9bc0..01a4477 100644 --- a/src/g20_thumb_apriltag_calibration/test/test_config.py +++ b/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 diff --git a/src/g20_thumb_apriltag_calibration/test/test_core.py b/src/g20_thumb_apriltag_calibration/test/test_core.py index adf543b..a134640 100644 --- a/src/g20_thumb_apriltag_calibration/test/test_core.py +++ b/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: diff --git a/src/g20_thumb_apriltag_calibration/test/test_zero_calibration.py b/src/g20_thumb_apriltag_calibration/test/test_zero_calibration.py index 63e72c6..0a6496b 100644 --- a/src/g20_thumb_apriltag_calibration/test/test_zero_calibration.py +++ b/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