diff --git a/.gitignore b/.gitignore index eae41ab..88691e7 100644 --- a/.gitignore +++ b/.gitignore @@ -66,6 +66,9 @@ Thumbs.db *.verification.json *_mapping_quality.json +# Device-specific robot descriptions derived from local CMC pitch calibration +/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/linkerhand_g20_left_cmc_pitch_*.urdf + # ROS bag / MCAP recordings and CAN captures rosbag2_*/ /bags/ diff --git a/src/g20_thumb_apriltag_calibration/README.md b/src/g20_thumb_apriltag_calibration/README.md index b4b7460..61e267c 100644 --- a/src/g20_thumb_apriltag_calibration/README.md +++ b/src/g20_thumb_apriltag_calibration/README.md @@ -266,3 +266,68 @@ D405 的彩色流来自 `depth_module`，启动文件会同时设置 启用 rosbag 后保存裁剪后的原始图像和配套 `CameraInfo`，避免新增一个全分辨率图像 订阅者；同时使用 MCAP `zstd_fast` 压缩并按 10 GiB 分卷。快速标定通常不需要录制； 若用于正式可追溯验收，再启用并检查磁盘空间。 + +## 5. CMC Pitch 双零位角测量 + +只测量命令 255 时 `thumb_cmc_pitch` 的掌心相对零位角和画面水平投影零位角时， +使用独立启动文件。它只拟合 CMC 的二维零位轨迹圆，不运行完整 0～255 角度映射， +也不会生成或修改 URDF： + +```bash +ros2 launch g20_thumb_apriltag_calibration \ + front_cmc_pitch_zero.launch.py \ + serial_number:=G20_LEFT_001 +``` + +该流程只要求 T0（ID 0）和 T3（ID 1）有效。T4/T5 可以留在手上，但丢失不会阻塞。 +预检完成后查看中文状态： + +```bash +ros2 topic echo --once --full-length \ + /g20_thumb_cmc_pitch_zero/status_text \ + --field data +``` + +状态显示“等待开始”后启动三轮测量： + +```bash +ros2 service call \ + /g20_thumb_cmc_pitch_zero/start \ + std_srvs/srv/Trigger {} +``` + +查看带红色画面水平线、T0/T3 方向箭头、青色轨迹点、紫色拟合圆心和径向零位线 +的调试画面： + +```bash +ros2 run image_view image_view --ros-args \ + --remap image:=/g20_thumb_cmc_pitch_zero/debug_image +``` + +每轮只控制电机 0 执行一次 `255→64` 端点运动和一次 `64→255` 返回运动。运动期间 +连续采集 `T3中心−T0中心`，按机械手状态分箱后拟合图像平面圆；返回 255 后使用 +“拟合圆心→T3零位中心”的径向线计算角度。运动前和返回后各采集 30 帧静态零位， +三轮轨迹合并后得到最终圆心。其他 19 个命令保持固定基准。任何其他节点同时发布 +`/g20/cb_left_hand_control_cmd` 时，`start` 服务会拒绝启动。 + +T0用于消除相机或整只手的平移抖动。T3标签自身的箭头只负责在径向线的两个相反 +方向中选择与旧版一致的正方向，不再决定角度大小。若轨迹跨度、圆弧、半径、径向 +RMS/P95或回零误差不合格，节点暂停或写出 `quality.passed=false`。 + +完成后只生成： + +```text +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 中心抵消平移； +它仍不是真实三维桌面检测，因此会随相机滚转和机械手摆放改变。 diff --git a/src/g20_thumb_apriltag_calibration/config/cmc_pitch_zero.yaml b/src/g20_thumb_apriltag_calibration/config/cmc_pitch_zero.yaml new file mode 100644 index 0000000..f72df8d --- /dev/null +++ b/src/g20_thumb_apriltag_calibration/config/cmc_pitch_zero.yaml @@ -0,0 +1,45 @@ +g20_thumb_cmc_pitch_zero: + ros__parameters: + t0_id: 0 + t3_id: 1 + 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 + # Each round sends one 255->64 endpoint command and one 64->255 return + # command. All valid T3-minus-T0 centres observed during both motions are + # state-binned and fitted to one image-plane circle. + trajectory_command_u8: 64 + trajectory_bin_size_u8: 8.0 + trajectory_minimum_frames: 45 + trajectory_minimum_bins: 18 + trajectory_minimum_state_span_u8: 160.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: 30.0 + settle_seconds: 0.5 + move_timeout_seconds: 20.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_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 + maximum_zero_radial_error_px: 4.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/zero_calibration.py b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/zero_calibration.py new file mode 100644 index 0000000..f3aa5e3 --- /dev/null +++ b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/zero_calibration.py @@ -0,0 +1,512 @@ +"""Pure helpers for image-trajectory CMC pitch zero-angle measurement.""" + +from __future__ import annotations + +import math +from typing import Any, Mapping, Sequence + +import numpy as np +from scipy.optimize import least_squares + + +def wrap_angle_rad(angle_rad: float) -> float: + """Wrap one finite angle to [-pi, pi).""" + angle = float(angle_rad) + if not math.isfinite(angle): + raise ValueError("angle must be finite") + return (angle + math.pi) % (2.0 * math.pi) - math.pi + + +def signed_angle_difference_rad(angle_rad: float, reference_rad: float) -> float: + """Return the wrapped signed rotation from reference to angle.""" + return wrap_angle_rad(float(angle_rad) - float(reference_rad)) + + +def tag_x_axis_angle_rad( + corners_xy: Sequence[Sequence[float]], +) -> float: + """Return the ordered AprilTag +x direction in a y-up image frame.""" + corners = np.asarray(corners_xy, dtype=float) + if corners.shape != (4, 2) or not np.all(np.isfinite(corners)): + raise ValueError("corners_xy must contain four finite xy points") + x_axis = (corners[1] - corners[0]) + (corners[2] - corners[3]) + if float(np.linalg.norm(x_axis)) < 1.0e-9: + raise ValueError("tag x-axis is degenerate") + return wrap_angle_rad( + math.atan2(-float(x_axis[1]), float(x_axis[0])) + ) + + +def circular_mean_rad(angles_rad: Sequence[float]) -> float: + """Return the circular mean of at least one finite angle.""" + angles = np.asarray(angles_rad, dtype=float) + if angles.ndim != 1 or angles.size == 0 or not np.all(np.isfinite(angles)): + raise ValueError("angles_rad must contain finite angles") + vector = complex( + float(np.mean(np.cos(angles))), + float(np.mean(np.sin(angles))), + ) + if abs(vector) < 1.0e-12: + raise ValueError("angles do not have a unique circular mean") + return wrap_angle_rad(math.atan2(vector.imag, vector.real)) + + +def circular_median_rad(angles_rad: Sequence[float]) -> float: + """Return a robust circular median around the circular mean.""" + angles = np.asarray(angles_rad, dtype=float) + reference = circular_mean_rad(angles) + deltas = np.asarray( + [signed_angle_difference_rad(value, reference) for value in angles], + dtype=float, + ) + return wrap_angle_rad(reference + float(np.median(deltas))) + + +def circular_std_rad(angles_rad: Sequence[float]) -> float: + """Return circular standard deviation in radians.""" + angles = np.asarray(angles_rad, dtype=float) + if angles.ndim != 1 or angles.size == 0 or not np.all(np.isfinite(angles)): + raise ValueError("angles_rad must contain finite angles") + resultant = float( + math.hypot( + float(np.mean(np.cos(angles))), + float(np.mean(np.sin(angles))), + ) + ) + resultant = min(1.0, max(1.0e-15, resultant)) + return math.sqrt(max(0.0, -2.0 * math.log(resultant))) + + +def maximum_pairwise_angle_difference_rad( + angles_rad: Sequence[float], +) -> float: + """Return the largest wrapped distance between any two angles.""" + angles = [float(value) for value in angles_rad] + if not angles or not all(math.isfinite(value) for value in angles): + raise ValueError("angles_rad must contain finite angles") + return max( + ( + abs(signed_angle_difference_rad(left, right)) + for index, left in enumerate(angles) + for right in angles[index + 1:] + ), + default=0.0, + ) + + +def summarize_zero_frames( + frames: Sequence[Mapping[str, float]], +) -> dict[str, float]: + """Summarize one stable command-255 capture window. + + Image coordinates use x-right/y-down. The T3-minus-T0 centre is retained + so a later trajectory-circle fit is insensitive to camera/hand translation. + """ + if not frames: + raise ValueError("at least one frame is required") + required = ( + "t0_x_px", + "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: + for name in required: + value = float(frame[name]) + if not math.isfinite(value): + raise ValueError(f"{name} must be finite") + values[name].append(value) + + t0_xy = np.column_stack((values["t0_x_px"], values["t0_y_px"])) + t3_xy = np.column_stack((values["t3_x_px"], values["t3_y_px"])) + relative_xy = t3_xy - t0_xy + relative_median = np.median(relative_xy, axis=0) + relative_error = np.linalg.norm(relative_xy - relative_median, axis=1) + t0_median = np.median(t0_xy, axis=0) + t3_median = np.median(t3_xy, axis=0) + return { + "relative_x_px": float(relative_median[0]), + "relative_y_px": float(relative_median[1]), + "t0_x_px": float(t0_median[0]), + "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)))) + ), + } + + +def _fit_circle(points_xy: np.ndarray) -> tuple[np.ndarray, float]: + """Fit a geometric circle with robust nonlinear least squares.""" + points = np.asarray(points_xy, dtype=float) + if points.ndim != 2 or points.shape[1] != 2 or len(points) < 3: + raise ValueError("circle fit requires at least three 2D points") + origin = np.mean(points, axis=0) + centered = points - origin + matrix = np.column_stack( + (2.0 * centered[:, 0], 2.0 * centered[:, 1], np.ones(len(points))) + ) + rhs = np.sum(np.square(centered), axis=1) + solution, _, rank, _ = np.linalg.lstsq(matrix, rhs, rcond=None) + if rank < 3: + raise ValueError("trajectory points are degenerate") + centre0 = solution[:2] + origin + radius0_squared = ( + float(solution[2]) + float(np.dot(solution[:2], solution[:2])) + ) + if radius0_squared <= 0.0: + raise ValueError("trajectory circle radius is invalid") + radius0 = math.sqrt(radius0_squared) + + def residual(parameters: np.ndarray) -> np.ndarray: + centre = parameters[:2] + radius = parameters[2] + return np.linalg.norm(points - centre, axis=1) - radius + + initial_error = residual(np.asarray([*centre0, radius0], dtype=float)) + robust_scale = max( + 0.25, + 1.4826 + * float( + np.median( + np.abs(initial_error - float(np.median(initial_error))) + ) + ), + ) + result = least_squares( + residual, + np.asarray([*centre0, radius0], dtype=float), + loss="soft_l1", + f_scale=robust_scale, + max_nfev=1000, + ) + centre = np.asarray(result.x[:2], dtype=float) + radius = float(abs(result.x[2])) + if not result.success or not np.all(np.isfinite(centre)): + raise ValueError("trajectory circle optimization failed") + if not math.isfinite(radius) or radius <= 0.0: + raise ValueError("trajectory circle radius is invalid") + return centre, radius + + +def _trajectory_arc_rad(points_xy: np.ndarray, centre_xy: np.ndarray) -> float: + angles = np.mod( + np.arctan2( + points_xy[:, 1] - centre_xy[1], + points_xy[:, 0] - centre_xy[0], + ), + 2.0 * math.pi, + ) + if len(angles) < 2: + return 0.0 + angles = np.sort(angles) + gaps = np.diff(np.r_[angles, angles[0] + 2.0 * math.pi]) + return float(2.0 * math.pi - np.max(gaps)) + + +def fit_image_circle_trajectory( + observations: Sequence[Mapping[str, float]], + *, + bin_size_u8: float, + minimum_frames: int, + minimum_bins: int, + minimum_state_span_u8: float, + minimum_radius_px: float, + minimum_arc_rad: float, + maximum_radial_rms_px: float, + maximum_p95_radial_error_px: float, +) -> dict[str, Any]: + """Fit the T3-minus-T0 image trajectory to a robust circle. + + Repeated detector frames at the same hand state are collapsed into state + bins before fitting so slow endpoints cannot dominate the circle. + """ + if len(observations) < minimum_frames: + raise ValueError( + f"trajectory_frames_too_few:{len(observations)}/{minimum_frames}" + ) + required = ("state_u8", "t0_x_px", "t0_y_px", "t3_x_px", "t3_y_px") + rows: list[tuple[float, float, float]] = [] + for observation in observations: + values = [float(observation[name]) for name in required] + if not all(math.isfinite(value) for value in values): + continue + state, t0_x, t0_y, t3_x, t3_y = values + rows.append((state, t3_x - t0_x, t3_y - t0_y)) + if len(rows) < minimum_frames: + raise ValueError( + f"trajectory_valid_frames_too_few:{len(rows)}/{minimum_frames}" + ) + + samples = np.asarray(rows, dtype=float) + state_span = float(np.ptp(samples[:, 0])) + if state_span < minimum_state_span_u8: + raise ValueError( + "trajectory_state_span_too_small:" + f"{state_span:.1f}/{minimum_state_span_u8:.1f}" + ) + + bins: dict[int, list[np.ndarray]] = {} + for state, x_value, y_value in samples: + key = int(round(float(state) / bin_size_u8)) + bins.setdefault(key, []).append(np.asarray([x_value, y_value])) + binned_points = np.asarray( + [np.median(np.asarray(values), axis=0) for _, values in sorted(bins.items())], + dtype=float, + ) + if len(binned_points) < minimum_bins: + raise ValueError( + f"trajectory_bins_too_few:{len(binned_points)}/{minimum_bins}" + ) + + centre, radius = _fit_circle(binned_points) + radial_error = np.abs( + np.linalg.norm(binned_points - centre, axis=1) - radius + ) + median_error = float(np.median(radial_error)) + mad = 1.4826 * float(np.median(np.abs(radial_error - median_error))) + inlier_limit = max(0.75, median_error + 3.5 * max(mad, 0.1)) + inliers = radial_error <= inlier_limit + if int(np.count_nonzero(inliers)) >= max(6, minimum_bins // 2): + centre, radius = _fit_circle(binned_points[inliers]) + radial_error = np.abs( + np.linalg.norm(binned_points - centre, axis=1) - radius + ) + radial_rms = math.sqrt(float(np.mean(np.square(radial_error)))) + radial_p95 = float(np.percentile(radial_error, 95.0)) + arc_rad = _trajectory_arc_rad(binned_points, centre) + + failures: list[str] = [] + if radius < minimum_radius_px: + failures.append(f"radius={radius:.2f}px") + if arc_rad < minimum_arc_rad: + failures.append(f"arc={math.degrees(arc_rad):.2f}deg") + if radial_rms > maximum_radial_rms_px: + failures.append(f"rms={radial_rms:.2f}px") + if radial_p95 > maximum_p95_radial_error_px: + failures.append(f"p95={radial_p95:.2f}px") + if failures: + raise ValueError("trajectory_circle_quality_failed:" + ",".join(failures)) + + return { + "method": "t3_minus_t0_image_trajectory_circle", + "centre_relative_xy_px": [float(centre[0]), float(centre[1])], + "radius_px": radius, + "radial_rms_px": radial_rms, + "radial_p95_px": radial_p95, + "arc_rad": arc_rad, + "state_span_u8": state_span, + "frames": len(rows), + "bins": len(binned_points), + "passed": True, + } + + +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. + + 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. + """ + centre = np.asarray(circle["centre_relative_xy_px"], dtype=float) + point = np.asarray( + [ + float(zero_summary["relative_x_px"]), + float(zero_summary["relative_y_px"]), + ], + dtype=float, + ) + vector = point - centre + distance = float(np.linalg.norm(vector)) + 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"]) + 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), + } + + +def build_trajectory_zero_angle_payload( + *, + serial_number: str, + rounds: Sequence[Mapping[str, Mapping[str, float]]], + 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, +) -> tuple[dict[str, Any], dict[str, Any]]: + """Create the compact JSON and detailed circle-fit status report.""" + 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]") + + 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"])] + ) + ) + return_errors.append( + abs( + signed_angle_difference_rad( + float(after["table_rad"]), + float(before["table_rad"]), + ) + ) + ) + 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"]), + float(after["zero_radial_error_px"]), + ] + ) + + 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, + "model": "G20", + "side": "left", + "serial_number": str(serial_number), + "joint": "thumb_cmc_pitch", + "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", + "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, + } + return payload, report + + +def validate_zero_angle_payload(payload: Mapping[str, Any]) -> None: + """Validate the deliberately small public JSON schema.""" + if set(payload) != { + "schema_version", + "model", + "side", + "serial_number", + "joint", + "angle_unit", + "zero_command_u8", + "zero_angles", + "quality", + }: + raise ValueError("unexpected top-level zero calibration fields") + if payload["schema_version"] != 1: + raise ValueError("schema_version must be 1") + if payload["model"] != "G20" or payload["side"] != "left": + raise ValueError("payload must describe the left G20") + if payload["joint"] != "thumb_cmc_pitch": + raise ValueError("payload must describe thumb_cmc_pitch") + 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", + }: + 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", + "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") 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 new file mode 100644 index 0000000..42a0506 --- /dev/null +++ b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/zero_node.py @@ -0,0 +1,1194 @@ +"""ROS 2 node for trajectory-circle G20 CMC pitch zero measurement.""" + +from __future__ import annotations + +from collections import deque +import json +import math +from pathlib import Path +import re +import time +from typing import Any + +from apriltag_msgs.msg import AprilTagDetectionArray +import cv2 +from cv_bridge import CvBridge +import numpy as np +import rclpy +from rclpy.node import Node +from rclpy.qos import qos_profile_sensor_data +from sensor_msgs.msg import Image, JointState +from std_msgs.msg import String +from std_srvs.srv import Trigger + +from .core import BASELINE_COMMAND, build_command, COMMAND_NAMES +from .storage import atomic_write_json +from .zero_calibration import ( + build_trajectory_zero_angle_payload, + fit_image_circle_trajectory, + measure_zero_from_circle, + summarize_zero_frames, + tag_x_axis_angle_rad, + validate_zero_angle_payload, +) + + +STATE_PREFLIGHT = "PREFLIGHT" +STATE_WAIT_START = "WAIT_START" +STATE_MOVING = "MOVING" +STATE_CAPTURING = "CAPTURING" +STATE_SWEEPING = "SWEEPING" +STATE_PAUSED = "PAUSED" +STATE_ABORTED = "ABORTED" +STATE_COMPLETE = "COMPLETE" + +STATE_ZH = { + STATE_PREFLIGHT: "标定前检查", + STATE_WAIT_START: "等待开始", + STATE_MOVING: "机械手运动中", + STATE_CAPTURING: "采集零位角", + STATE_SWEEPING: "采集圆心轨迹", + STATE_PAUSED: "标定已暂停", + STATE_ABORTED: "标定已终止", + STATE_COMPLETE: "标定完成", +} + +STAGE_ZH = { + "zero_before": "255零位（运动前）", + "sweep_out": "圆心轨迹（255到轨迹端点）", + "sweep_return": "圆心轨迹（返回255）", + "zero_after": "255零位（返回后）", +} + + +def _safe_name(value: str) -> str: + safe = re.sub(r"[^A-Za-z0-9_.-]+", "_", str(value).strip()) + return safe or "UNSET" + + +class G20ThumbCmcPitchZeroNode(Node): + """Measure CMC zero from the T3-centre image trajectory circle.""" + + def __init__(self) -> None: + super().__init__("g20_thumb_cmc_pitch_zero") + self._declare_parameters() + self._load_parameters() + + self.session_dir.mkdir(parents=True, exist_ok=True) + self.result_path = self.session_dir / ( + f"g20_left_{_safe_name(self.serial_number)}" + "_thumb_cmc_pitch_zero.json" + ) + + self.state = STATE_PREFLIGHT + self.reason = "waiting_for_t0_t3_and_hand_state" + self.latest_state_u8: tuple[float, ...] = () + self.latest_corners: dict[str, np.ndarray] = {} + 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.session_detection_flags: list[bool] = [] + + self.round_index = 0 + self.rounds: list[dict[str, Any]] = [] + self.current_round: dict[str, Any] = {} + self.active_stage: str | None = None + self.active_target_u8: int | None = None + self.required_frames = 0 + self.stage_started_at = 0.0 + self.target_reached_at: float | None = None + self.capture_started_at = 0.0 + self.capture_frames: deque[dict[str, float]] = deque() + self.trajectory_frames: list[dict[str, float]] = [] + self.latest_trajectory_frames: list[dict[str, float]] = [] + self.latest_circle: dict[str, Any] = {} + self.final_payload: dict[str, Any] | None = None + self.final_report: dict[str, Any] = {} + + self.command_publisher = self.create_publisher( + JointState, self.command_topic, 1 + ) + self.status_publisher = self.create_publisher(String, "~/status", 10) + self.status_text_publisher = self.create_publisher( + String, "~/status_text", 10 + ) + self.debug_publisher = None + self.bridge = CvBridge() + self.last_debug_publish = 0.0 + if self.publish_debug_image: + self.debug_publisher = self.create_publisher( + Image, "~/debug_image", qos_profile_sensor_data + ) + + self.create_subscription( + JointState, self.state_topic, self._state_callback, 10 + ) + self.create_subscription( + AprilTagDetectionArray, + self.detections_topic, + self._detections_callback, + qos_profile_sensor_data, + ) + if self.publish_debug_image: + self.create_subscription( + Image, + self.image_topic, + self._image_callback, + qos_profile_sensor_data, + ) + + self.create_service(Trigger, "~/start", self._start_callback) + self.create_service(Trigger, "~/abort", self._abort_callback) + 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.session_dir}; result={self.result_path}" + ) + + def _declare_parameters(self) -> None: + self.declare_parameter("serial_number", "UNSET") + self.declare_parameter("session_dir", "calibration_output/session") + self.declare_parameter("commands_enabled", True) + self.declare_parameter( + "command_topic", "/g20/cb_left_hand_control_cmd" + ) + self.declare_parameter("state_topic", "/g20/cb_left_hand_state") + self.declare_parameter( + "detections_topic", "/apriltag/detections" + ) + self.declare_parameter( + "image_topic", "/camera/camera/color/image_rect" + ) + 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("t0_id", 0) + self.declare_parameter("t3_id", 1) + self.declare_parameter( + "baseline_command_u8", list(BASELINE_COMMAND) + ) + self.declare_parameter("repetitions", 3) + self.declare_parameter("zero_capture_frames", 30) + self.declare_parameter("trajectory_command_u8", 64) + self.declare_parameter("trajectory_bin_size_u8", 8.0) + self.declare_parameter("trajectory_minimum_frames", 45) + self.declare_parameter("trajectory_minimum_bins", 18) + self.declare_parameter("trajectory_minimum_state_span_u8", 160.0) + self.declare_parameter("trajectory_minimum_radius_px", 20.0) + self.declare_parameter("trajectory_minimum_arc_deg", 20.0) + self.declare_parameter("trajectory_maximum_radial_rms_px", 2.0) + self.declare_parameter( + "trajectory_maximum_p95_radial_error_px", 3.5 + ) + self.declare_parameter("trajectory_endpoint_settle_seconds", 0.3) + self.declare_parameter("trajectory_timeout_seconds", 30.0) + self.declare_parameter("settle_seconds", 0.5) + self.declare_parameter("move_timeout_seconds", 20.0) + self.declare_parameter("capture_timeout_seconds", 15.0) + self.declare_parameter("state_tolerance_u8", 2.0) + self.declare_parameter("preflight_frames", 60) + self.declare_parameter("minimum_detection_rate", 0.95) + 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_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) + + def _load_parameters(self) -> None: + def value(name: str) -> Any: + return self.get_parameter(name).value + + self.serial_number = str(value("serial_number")) + if ( + not self.serial_number + or self.serial_number == "UNSET" + or re.fullmatch(r"[A-Za-z0-9_.-]+", self.serial_number) is None + ): + raise ValueError("serial_number is required and must be path-safe") + self.session_dir = Path(str(value("session_dir"))).expanduser().resolve() + self.commands_enabled = bool(value("commands_enabled")) + self.command_topic = str(value("command_topic")) + self.state_topic = str(value("state_topic")) + self.detections_topic = str(value("detections_topic")) + self.image_topic = str(value("image_topic")) + 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.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.baseline_command = tuple( + int(item) for item in value("baseline_command_u8") + ) + self.repetitions = int(value("repetitions")) + self.zero_capture_frames = int(value("zero_capture_frames")) + self.trajectory_command_u8 = int(value("trajectory_command_u8")) + self.trajectory_bin_size_u8 = float(value("trajectory_bin_size_u8")) + self.trajectory_minimum_frames = int( + value("trajectory_minimum_frames") + ) + self.trajectory_minimum_bins = int(value("trajectory_minimum_bins")) + self.trajectory_minimum_state_span_u8 = float( + value("trajectory_minimum_state_span_u8") + ) + self.trajectory_minimum_radius_px = float( + value("trajectory_minimum_radius_px") + ) + self.trajectory_minimum_arc_rad = math.radians( + float(value("trajectory_minimum_arc_deg")) + ) + self.trajectory_maximum_radial_rms_px = float( + value("trajectory_maximum_radial_rms_px") + ) + self.trajectory_maximum_p95_radial_error_px = float( + value("trajectory_maximum_p95_radial_error_px") + ) + self.trajectory_endpoint_settle_seconds = float( + value("trajectory_endpoint_settle_seconds") + ) + self.trajectory_timeout_seconds = float( + value("trajectory_timeout_seconds") + ) + self.settle_seconds = float(value("settle_seconds")) + self.move_timeout_seconds = float(value("move_timeout_seconds")) + self.capture_timeout_seconds = float(value("capture_timeout_seconds")) + self.state_tolerance_u8 = float(value("state_tolerance_u8")) + self.preflight_frames = int(value("preflight_frames")) + self.minimum_detection_rate = float(value("minimum_detection_rate")) + self.maximum_hamming = int(value("maximum_hamming")) + self.minimum_decision_margin = float( + 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_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") + ) + self.maximum_static_position_rms_px = float( + value("maximum_static_position_rms_px") + ) + self._validate_parameters() + + def _validate_parameters(self) -> None: + 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 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 self.trajectory_minimum_state_span_u8 > available_span: + raise ValueError( + "trajectory_minimum_state_span_u8 exceeds commanded span" + ) + if self.trajectory_minimum_frames < 12: + raise ValueError("trajectory_minimum_frames must be at least 12") + if self.trajectory_minimum_bins < 6: + raise ValueError("trajectory_minimum_bins must be at least 6") + if min( + self.debug_max_rate_hz, + self.debug_scale, + self.trajectory_bin_size_u8, + self.trajectory_minimum_state_span_u8, + self.trajectory_minimum_radius_px, + self.trajectory_minimum_arc_rad, + self.trajectory_maximum_radial_rms_px, + self.trajectory_maximum_p95_radial_error_px, + self.trajectory_endpoint_settle_seconds, + self.trajectory_timeout_seconds, + self.settle_seconds, + self.move_timeout_seconds, + self.capture_timeout_seconds, + self.state_tolerance_u8, + self.minimum_decision_margin, + self.minimum_edge_pixels, + self.maximum_static_std_rad, + self.maximum_round_difference_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: + raise ValueError("timing and quality thresholds must be positive") + if self.preflight_frames < 10: + raise ValueError("preflight_frames must be at least 10") + if not 0.0 < self.minimum_detection_rate <= 1.0: + raise ValueError("minimum_detection_rate must be in (0, 1]") + + def _state_callback(self, message: JointState) -> None: + if len(message.position) != 20: + return + if len(message.name) == 20 and set(message.name) == set(COMMAND_NAMES): + lookup = dict(zip(message.name, message.position)) + self.latest_state_u8 = tuple( + float(lookup[name]) for name in COMMAND_NAMES + ) + else: + self.latest_state_u8 = tuple( + float(item) for item in message.position + ) + + def _detections_callback( + self, message: AprilTagDetectionArray + ) -> None: + corners_by_role: dict[str, np.ndarray] = {} + quality_by_role: dict[str, dict[str, Any]] = {} + for detection in message.detections: + role = self.role_by_id.get(int(detection.id)) + if role is None: + continue + corners = np.asarray( + [ + [float(point.x), float(point.y)] + for point in detection.corners + ], + dtype=float, + ) + if corners.shape != (4, 2): + continue + edges = np.linalg.norm( + corners - np.roll(corners, -1, axis=0), + axis=1, + ) + quality_by_role[role] = { + "hamming": int(detection.hamming), + "decision_margin": float(detection.decision_margin), + "edge_pixels": float(np.mean(edges)), + } + corners_by_role[role] = corners + + good = set(corners_by_role) == {"t0", "t3"} and all( + self._quality_valid(quality_by_role[role]) + for role in ("t0", "t3") + ) + self.preflight_flags.append(good) + if self.state not in { + STATE_PREFLIGHT, + STATE_WAIT_START, + STATE_COMPLETE, + STATE_ABORTED, + }: + self.session_detection_flags.append(good) + self.latest_corners = corners_by_role + self.latest_quality = quality_by_role + 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]) + if len(self.latest_state_u8) == 20 + else float("nan") + ), + } + if self.latest_circle: + try: + live_summary = summarize_zero_frames([frame]) + self.latest_angles = measure_zero_from_circle( + self.latest_circle, live_summary + ) + except ValueError: + self.latest_angles = {} + + if self.state == STATE_SWEEPING: + if len(self.latest_state_u8) == 20: + self.trajectory_frames.append(frame) + return + if self.state != STATE_CAPTURING: + return + if not self._motor_at_target(): + return + self.capture_frames.append(frame) + while len(self.capture_frames) > self.required_frames: + self.capture_frames.popleft() + 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 + ): + self.reason = ( + f"{self.active_stage}_position_not_stable:" + f"{summary['relative_position_rms_px']:.2f}px" + ) + return + self._finish_capture(summary) + + def _quality_valid(self, quality: dict[str, Any]) -> bool: + return bool( + int(quality["hamming"]) <= self.maximum_hamming + and float(quality["decision_margin"]) + >= self.minimum_decision_margin + and float(quality["edge_pixels"]) >= self.minimum_edge_pixels + ) + + def _start_callback( + self, request: Trigger.Request, response: Trigger.Response + ) -> Trigger.Response: + del request + if self.state not in {STATE_WAIT_START, STATE_PAUSED}: + response.success = False + response.message = f"current state is {self.state}" + return response + if not self.commands_enabled: + response.success = False + response.message = "commands_enabled is false" + return response + if len(self.latest_state_u8) != 20: + response.success = False + response.message = "hand state is not available" + return response + if self.command_publisher.get_subscription_count() < 1: + response.success = False + response.message = "hand SDK is not subscribed to command topic" + return response + if self.count_publishers(self.command_topic) > 1: + response.success = False + response.message = "another node is publishing hand commands" + return response + if self._preflight_rate() < self.minimum_detection_rate: + response.success = False + response.message = "T0/T3 detection rate is below threshold" + return response + self._reset_run() + self._begin_stage("zero_before") + response.success = True + response.message = "CMC pitch zero calibration started" + return response + + def _abort_callback( + self, request: Trigger.Request, response: Trigger.Response + ) -> Trigger.Response: + del request + if self.state in {STATE_COMPLETE, STATE_ABORTED}: + response.success = False + response.message = f"current state is {self.state}" + return response + self.state = STATE_ABORTED + self.reason = "aborted_by_operator_no_motion_command_sent" + self.capture_frames.clear() + response.success = True + response.message = "aborted; no automatic hand motion was sent" + return response + + def _reset_run(self) -> None: + self.round_index = 0 + self.rounds = [] + self.current_round = {} + self.active_stage = None + self.active_target_u8 = None + self.capture_frames.clear() + self.trajectory_frames = [] + self.latest_trajectory_frames = [] + self.latest_circle = {} + self.latest_angles = {} + 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"}: + raise ValueError(f"unknown stage {stage}") + self.active_stage = stage + self.active_target_u8 = 255 + 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)) + + def _begin_sweep(self, stage: str) -> None: + if stage not in {"sweep_out", "sweep_return"}: + raise ValueError(f"unknown sweep stage {stage}") + if stage == "sweep_out": + self.trajectory_frames = [] + target = self.trajectory_command_u8 + else: + target = 255 + self.active_stage = stage + self.active_target_u8 = target + self.required_frames = self.trajectory_minimum_frames + self.stage_started_at = time.monotonic() + self.target_reached_at = None + self.state = STATE_SWEEPING + self.reason = ( + f"round_{self.round_index + 1}:{stage}:" + f"target={target}:collecting_trajectory" + ) + self._publish_hand_command( + build_command(0, target, self.baseline_command) + ) + + def _fit_trajectory( + self, observations: list[dict[str, float]] + ) -> dict[str, Any]: + return fit_image_circle_trajectory( + observations, + bin_size_u8=self.trajectory_bin_size_u8, + minimum_frames=self.trajectory_minimum_frames, + minimum_bins=self.trajectory_minimum_bins, + minimum_state_span_u8=self.trajectory_minimum_state_span_u8, + minimum_radius_px=self.trajectory_minimum_radius_px, + minimum_arc_rad=self.trajectory_minimum_arc_rad, + maximum_radial_rms_px=self.trajectory_maximum_radial_rms_px, + maximum_p95_radial_error_px=( + self.trajectory_maximum_p95_radial_error_px + ), + ) + + def _publish_hand_command(self, values: list[int]) -> None: + message = JointState() + message.header.stamp = self.get_clock().now().to_msg() + message.name = list(COMMAND_NAMES) + message.position = [float(value) for value in values] + self.command_publisher.publish(message) + + def _motor_at_target(self) -> bool: + return bool( + self.active_target_u8 is not None + and len(self.latest_state_u8) == 20 + and abs( + float(self.latest_state_u8[0]) + - float(self.active_target_u8) + ) + <= self.state_tolerance_u8 + ) + + def _timer_callback(self) -> None: + now = time.monotonic() + if self.state == STATE_PREFLIGHT: + if ( + len(self.preflight_flags) == self.preflight_frames + and len(self.latest_state_u8) == 20 + and self._preflight_rate() >= self.minimum_detection_rate + ): + self.state = STATE_WAIT_START + self.reason = "call_start_service" + elif len(self.preflight_flags) == self.preflight_frames: + self.reason = ( + "T0/T3 detection rate too low:" + f"{self._preflight_rate():.3f}" + ) + return + if self.state == STATE_MOVING: + if now - self.stage_started_at > self.move_timeout_seconds: + self._pause( + f"move_timeout:target={self.active_target_u8}:" + f"actual={self._motor_value_text()}" + ) + return + if not self._motor_at_target(): + self.target_reached_at = None + return + if self.target_reached_at is None: + self.target_reached_at = now + self.reason = ( + f"{self.active_stage}:settling:" + f"{self.settle_seconds:.1f}s" + ) + return + if now - self.target_reached_at >= self.settle_seconds: + self.state = STATE_CAPTURING + self.capture_started_at = now + self.capture_frames.clear() + self.reason = ( + f"{self.active_stage}:collecting:" + f"0/{self.required_frames}" + ) + return + if self.state == STATE_SWEEPING: + self.reason = ( + f"{self.active_stage}:trajectory_frames=" + f"{len(self.trajectory_frames)}:" + f"actual={self._motor_value_text()}:" + f"target={self.active_target_u8}" + ) + if now - self.stage_started_at > self.trajectory_timeout_seconds: + self._pause( + f"trajectory_timeout:{self.active_stage}:" + f"target={self.active_target_u8}:" + f"actual={self._motor_value_text()}:" + f"frames={len(self.trajectory_frames)}" + ) + return + if not self._motor_at_target(): + self.target_reached_at = None + return + if self.target_reached_at is None: + self.target_reached_at = now + return + if ( + now - self.target_reached_at + >= self.trajectory_endpoint_settle_seconds + ): + self._finish_sweep() + return + if self.state == STATE_CAPTURING: + self.reason = ( + f"{self.active_stage}:collecting:" + f"{len(self.capture_frames)}/{self.required_frames}" + ) + if not self._motor_at_target(): + self._pause( + f"motor_left_target_during_capture:" + f"target={self.active_target_u8}:" + f"actual={self._motor_value_text()}" + ) + return + if now - self.capture_started_at > self.capture_timeout_seconds: + self._pause( + f"capture_timeout:{self.active_stage}:" + f"{len(self.capture_frames)}/{self.required_frames}" + ) + + def _finish_capture(self, summary: dict[str, float]) -> None: + if self.active_stage is None: + return + stage = self.active_stage + self.current_round[stage] = summary + self.capture_frames.clear() + if stage == "zero_before": + self._begin_sweep("sweep_out") + return + + self.rounds.append(dict(self.current_round)) + self.current_round = {} + self.round_index += 1 + if self.round_index < self.repetitions: + self._begin_stage("zero_before") + return + self._complete_run() + + def _finish_sweep(self) -> None: + stage = self.active_stage + if stage == "sweep_out": + self._begin_sweep("sweep_return") + return + if stage != "sweep_return": + self._pause(f"unexpected_sweep_stage:{stage}") + return + try: + circle = self._fit_trajectory(self.trajectory_frames) + except ValueError as error: + self._pause(str(error)) + return + self.current_round["trajectory_circle"] = circle + self.current_round["trajectory_frames"] = list(self.trajectory_frames) + self.latest_trajectory_frames = list(self.trajectory_frames) + self.latest_circle = circle + self._begin_stage("zero_after") + + def _complete_run(self) -> None: + detection_rate = ( + float(np.mean(self.session_detection_flags)) + if self.session_detection_flags + else 0.0 + ) + all_trajectory_frames = [ + frame + for round_value in self.rounds + for frame in round_value["trajectory_frames"] + ] + try: + circle = self._fit_trajectory(all_trajectory_frames) + measured_rounds = [ + { + "zero_before": measure_zero_from_circle( + circle, round_value["zero_before"] + ), + "zero_after": measure_zero_from_circle( + circle, round_value["zero_after"] + ), + } + for round_value in self.rounds + ] + 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) + 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.state = STATE_COMPLETE + self.reason = ( + "result_passed" + if payload["quality"]["passed"] + else "result_written_but_quality_failed" + ) + self.active_stage = None + self.active_target_u8 = 255 + self.get_logger().info( + f"CMC pitch zero result written: {self.result_path}; " + f"passed={payload['quality']['passed']}" + ) + + def _pause(self, reason: str) -> None: + self.state = STATE_PAUSED + self.reason = reason + self.capture_frames.clear() + self.get_logger().error(f"CMC pitch zero calibration paused: {reason}") + + def _preflight_rate(self) -> float: + return ( + float(np.mean(self.preflight_flags)) + if self.preflight_flags + else 0.0 + ) + + def _motor_value_text(self) -> str: + if len(self.latest_state_u8) != 20: + return "unavailable" + return f"{self.latest_state_u8[0]:.1f}" + + def _image_callback(self, message: Image) -> None: + if ( + self.debug_publisher is None + or self.debug_publisher.get_subscription_count() < 1 + ): + return + now = time.monotonic() + if now - self.last_debug_publish < 1.0 / self.debug_max_rate_hz: + return + self.last_debug_publish = now + try: + image = self.bridge.imgmsg_to_cv2( + message, desired_encoding="bgr8" + ) + except Exception as error: + self.get_logger().warning(f"debug image conversion failed: {error}") + return + if self.debug_scale != 1.0: + image = cv2.resize( + image, + None, + fx=self.debug_scale, + fy=self.debug_scale, + interpolation=cv2.INTER_AREA, + ) + + height, width = image.shape[:2] + reference_y = max(0, height - 55) + cv2.line( + image, + (20, reference_y), + (max(20, width - 20), reference_y), + (0, 0, 255), + 3, + ) + cv2.putText( + image, + "image +x / table projection", + (25, max(25, reference_y - 8)), + cv2.FONT_HERSHEY_SIMPLEX, + 0.55, + (0, 0, 255), + 2, + ) + colors = {"t0": (0, 210, 0), "t3": (0, 170, 255)} + for role in ("t0", "t3"): + corners = self.latest_corners.get(role) + if corners is None: + continue + 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(), + (start[0] + 5, start[1] - 8), + cv2.FONT_HERSHEY_SIMPLEX, + 0.65, + colors[role], + 2, + ) + + if "t0" in self.latest_corners: + anchor = np.mean(self.latest_corners["t0"], axis=0) + trajectory = ( + self.trajectory_frames + if self.state == STATE_SWEEPING + else self.latest_trajectory_frames + ) + if trajectory: + stride = max(1, len(trajectory) // 300) + for frame in trajectory[::stride]: + relative = np.asarray( + [ + frame["t3_x_px"] - frame["t0_x_px"], + frame["t3_y_px"] - frame["t0_y_px"], + ], + dtype=float, + ) + point = tuple( + np.rint( + (anchor + relative) * self.debug_scale + ).astype(int) + ) + cv2.circle(image, point, 2, (255, 180, 0), -1) + if self.latest_circle: + centre_relative = np.asarray( + self.latest_circle["centre_relative_xy_px"], dtype=float + ) + centre = (anchor + centre_relative) * self.debug_scale + centre_point = tuple(np.rint(centre).astype(int)) + radius = max( + 1, + int( + round( + float(self.latest_circle["radius_px"]) + * self.debug_scale + ) + ), + ) + cv2.circle(image, centre_point, radius, (255, 0, 255), 2) + cv2.drawMarker( + image, + centre_point, + (255, 0, 255), + cv2.MARKER_CROSS, + 18, + 2, + ) + cv2.putText( + image, + "CMC circle centre", + (centre_point[0] + 8, centre_point[1] - 8), + cv2.FONT_HERSHEY_SIMPLEX, + 0.52, + (255, 0, 255), + 2, + ) + cv2.line( + image, + centre_point, + (centre_point[0] + 85, centre_point[1]), + (0, 0, 255), + 2, + ) + if "t3" in self.latest_corners: + t3_centre = ( + np.mean(self.latest_corners["t3"], axis=0) + * self.debug_scale + ) + cv2.line( + image, + centre_point, + tuple(np.rint(t3_centre).astype(int)), + (255, 0, 255), + 3, + ) + + cv2.putText( + image, + f"{self.state} round {min(self.round_index + 1, 3)}/3", + (20, 30), + cv2.FONT_HERSHEY_SIMPLEX, + 0.65, + (0, 255, 0) + if self.state not in {STATE_PAUSED, STATE_ABORTED} + else (0, 0, 255), + 2, + ) + if self.latest_angles: + cv2.putText( + image, + "palm zero: " + f"{math.degrees(self.latest_angles['palm_rad']):.2f} deg", + (20, 58), + cv2.FONT_HERSHEY_SIMPLEX, + 0.62, + (0, 255, 255), + 2, + ) + cv2.putText( + image, + "table zero: " + f"{math.degrees(self.latest_angles['table_rad']):.2f} deg", + (20, 86), + cv2.FONT_HERSHEY_SIMPLEX, + 0.62, + (0, 255, 255), + 2, + ) + elif self.state in { + STATE_MOVING, + STATE_CAPTURING, + STATE_SWEEPING, + }: + cv2.putText( + image, + "zero angle: waiting for trajectory circle", + (20, 58), + cv2.FONT_HERSHEY_SIMPLEX, + 0.58, + (0, 255, 255), + 2, + ) + output = self.bridge.cv2_to_imgmsg(image, encoding="bgr8") + output.header = message.header + self.debug_publisher.publish(output) + + def _publish_status(self) -> None: + tag_status: dict[str, dict[str, Any]] = {} + for role in ("t0", "t3"): + quality = self.latest_quality.get(role) + if quality is None: + tag_status[role] = {"valid": False, "reason": "未检测到"} + else: + tag_status[role] = { + **quality, + "valid": self._quality_valid(quality), + } + payload = { + "state": self.state, + "state_zh": STATE_ZH[self.state], + "reason": self.reason, + "measurement_method": "t3_center_image_trajectory_circle", + "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] + ), + "active_command_u8": self.active_target_u8, + "actual_command_u8": ( + None + if len(self.latest_state_u8) != 20 + else self.latest_state_u8[0] + ), + "capture_frames_seen": len(self.capture_frames), + "capture_frames_required": self.required_frames, + "trajectory_frames_seen": len(self.trajectory_frames), + "trajectory_command_u8": self.trajectory_command_u8, + "trajectory_circle": self.latest_circle, + "preflight_detection_rate": self._preflight_rate(), + "tag_quality": tag_status, + "latest_angles_rad": self.latest_angles, + "result_path": str(self.result_path), + "result": self.final_payload, + "quality_report": self.final_report, + } + message = String() + message.data = json.dumps(payload, ensure_ascii=False) + self.status_publisher.publish(message) + text = String() + text.data = self._status_text(payload) + self.status_text_publisher.publish(text) + + def _status_text(self, payload: dict[str, Any]) -> str: + reason_zh = self._reason_zh() + if self.state == STATE_CAPTURING: + frame_text = ( + f"帧数：{len(self.capture_frames)}/{self.required_frames}" + ) + elif self.state == STATE_SWEEPING: + frame_text = ( + f"轨迹帧数：{len(self.trajectory_frames)}/" + f"至少{self.trajectory_minimum_frames}" + ) + else: + frame_text = "帧数：当前未采集" + lines = [ + f"状态：{payload['state_zh']}", + f"原因：{reason_zh}", + ( + f"进度：第{payload['cycle']}/{self.repetitions}轮，" + f"{payload['stage_zh'] or '无活动阶段'}" + ), + frame_text, + ] + if reason_zh != self.reason: + lines.append(f"诊断代码：{self.reason}") + for role, label in (("t0", "掌心T0(ID 0)"), ("t3", "根部T3(ID 1)")): + status = payload["tag_quality"][role] + if "edge_pixels" not in status: + lines.append(f"标签：{label} 未检测到") + continue + lines.append( + f"标签：{label} " + f"{'正常' if status['valid'] else '不合格'}，" + f"边长{status['edge_pixels']:.1f}px，" + f"置信度{status['decision_margin']:.1f}，" + f"hamming={status['hamming']}" + ) + 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 " + f"({math.degrees(self.latest_angles['table_rad']):.2f}°)" + ), + ] + ) + elif self.state in { + STATE_MOVING, + STATE_CAPTURING, + STATE_SWEEPING, + }: + lines.append("当前角度：等待完成T3中心轨迹圆拟合") + if self.latest_circle: + lines.extend( + [ + "轨迹圆质量：", + ( + f"- 半径 {self.latest_circle['radius_px']:.2f}px，" + f"圆弧 {math.degrees(self.latest_circle['arc_rad']):.2f}°" + ), + ( + f"- 径向RMS {self.latest_circle['radial_rms_px']:.2f}px，" + f"P95 {self.latest_circle['radial_p95_px']:.2f}px" + ), + ] + ) + if self.state == STATE_WAIT_START: + lines.append( + "下一步：调用 /g20_thumb_cmc_pitch_zero/start 开始标定" + ) + elif self.state == STATE_PAUSED: + lines.append("下一步：排除问题后再次调用 start，从第一轮重做") + elif self.state == STATE_COMPLETE: + lines.append(f"结果文件：{self.result_path}") + return "\n".join(lines) + + def _reason_zh(self) -> str: + reason = str(self.reason) + exact = { + "waiting_for_t0_t3_and_hand_state": "等待T0、T3和机械手状态", + "call_start_service": "预检通过，可以调用start开始", + "result_passed": "轨迹圆与三轮回零质量均通过", + "result_written_but_quality_failed": ( + "结果已保存，但三轮一致性或回零质量未通过" + ), + "aborted_by_operator_no_motion_command_sent": "已由用户终止", + } + if reason in exact: + return exact[reason] + prefixes = ( + ("trajectory_frames_too_few", "轨迹有效帧不足"), + ("trajectory_valid_frames_too_few", "轨迹有效数据不足"), + ("trajectory_state_span_too_small", "电机轨迹跨度不足"), + ("trajectory_bins_too_few", "轨迹覆盖的电机区间不足"), + ("trajectory_circle_quality_failed", "轨迹圆拟合质量不合格"), + ("trajectory_timeout", "轨迹运动或采集超时"), + ("final_trajectory_fit_failed", "三轮合并轨迹圆拟合失败"), + ("move_timeout", "机械手未在规定时间到达目标"), + ("capture_timeout", "静态零位帧采集超时"), + ("motor_left_target_during_capture", "静态采集时电机离开目标"), + ("unexpected_sweep_stage", "内部轨迹阶段异常"), + ) + for prefix, text in prefixes: + if reason.startswith(prefix): + return text + if "_position_not_stable" in reason: + return "T0与T3相对位置仍在抖动" + if "_not_stable" in reason: + return "标签方向仍在抖动" + if reason.startswith("round_"): + return "正在执行本轮零位或圆心轨迹阶段" + return reason + + +def main(args: list[str] | None = None) -> None: + rclpy.init(args=args) + node: G20ThumbCmcPitchZeroNode | None = None + try: + node = G20ThumbCmcPitchZeroNode() + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + if node is not None: + node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() diff --git a/src/g20_thumb_apriltag_calibration/launch/front_cmc_pitch_zero.launch.py b/src/g20_thumb_apriltag_calibration/launch/front_cmc_pitch_zero.launch.py new file mode 100644 index 0000000..50c8aa8 --- /dev/null +++ b/src/g20_thumb_apriltag_calibration/launch/front_cmc_pitch_zero.launch.py @@ -0,0 +1,238 @@ +"""Launch front-camera trajectory-circle CMC pitch zero measurement.""" + +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) + zero_config = LaunchConfiguration("zero_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_zero_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, + } + ], + ) + + zero_node = Node( + package="g20_thumb_apriltag_calibration", + executable="cmc_pitch_zero_node", + name="g20_thumb_cmc_pitch_zero", + output="screen", + parameters=[ + zero_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 pitch zero session: {session_dir}"), + LogInfo( + msg=( + "Only T0(ID 0) and T3(ID 1) are required; " + "T4/T5 detections are ignored" + ) + ), + LogInfo( + msg=( + "Motor 0 performs three 255->64->255 sweeps; " + "zero angles come from the fitted T3-centre trajectory radius" + ) + ), + camera, + vision_container, + sdk, + zero_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( + "zero_config", + default_value=str( + package_share / "config" / "cmc_pitch_zero.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 b6d0fce..e408f5f 100644 --- a/src/g20_thumb_apriltag_calibration/setup.py +++ b/src/g20_thumb_apriltag_calibration/setup.py @@ -28,6 +28,10 @@ setup( entry_points={ "console_scripts": [ "calibration_node = g20_thumb_apriltag_calibration.node:main", + ( + "cmc_pitch_zero_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 76d95bc..bed9bc0 100644 --- a/src/g20_thumb_apriltag_calibration/test/test_config.py +++ b/src/g20_thumb_apriltag_calibration/test/test_config.py @@ -94,3 +94,48 @@ def test_realsense_color_and_camera_info_use_matching_qos() -> None: assert config["color_qos"] == "DEFAULT" assert config["color_info_qos"] == "DEFAULT" + + +def test_cmc_pitch_zero_config_uses_three_trajectory_circle_rounds() -> None: + config = yaml.safe_load( + (PACKAGE_ROOT / "config" / "cmc_pitch_zero.yaml").read_text() + ) + parameters = config["g20_thumb_cmc_pitch_zero"]["ros__parameters"] + + assert parameters["t0_id"] == 0 + assert parameters["t3_id"] == 1 + assert parameters["baseline_command_u8"] == [ + 255, + 255, + 255, + 255, + 255, + 255, + 193, + 148, + 105, + 42, + 245, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + ] + assert parameters["repetitions"] == 3 + assert parameters["zero_capture_frames"] == 30 + assert parameters["trajectory_command_u8"] <= 64 + assert parameters["trajectory_minimum_state_span_u8"] >= 160.0 + assert parameters["trajectory_minimum_bins"] >= 18 + assert parameters["trajectory_minimum_arc_deg"] >= 20.0 + assert parameters["trajectory_maximum_radial_rms_px"] <= 2.0 + 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 parameters["maximum_round_difference_deg"] == 1.0 diff --git a/src/g20_thumb_apriltag_calibration/test/test_zero_calibration.py b/src/g20_thumb_apriltag_calibration/test/test_zero_calibration.py new file mode 100644 index 0000000..63e72c6 --- /dev/null +++ b/src/g20_thumb_apriltag_calibration/test/test_zero_calibration.py @@ -0,0 +1,272 @@ +from __future__ import annotations + +import math + +from g20_thumb_apriltag_calibration.zero_calibration import ( + build_trajectory_zero_angle_payload, + circular_median_rad, + fit_image_circle_trajectory, + measure_zero_from_circle, + signed_angle_difference_rad, + summarize_zero_frames, + tag_x_axis_angle_rad, + validate_zero_angle_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 _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, +) -> dict[str, float]: + # table_angle_rad uses a y-up convention while image y grows downwards. + relative = np.asarray(circle_xy) + radius_px * np.asarray( + [math.cos(table_angle_rad), -math.sin(table_angle_rad)] + ) + t0 = np.asarray(t0_xy) + t3 = t0 + relative + return { + "state_u8": state_u8, + "t0_x_px": float(t0[0]), + "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 + ), + } + + +def _trajectory(noise_px: float = 0.15) -> list[dict[str, float]]: + rng = np.random.default_rng(7) + observations: list[dict[str, float]] = [] + for states in ( + np.linspace(255.0, 64.0, 70), + np.linspace(64.0, 255.0, 70), + ): + for state in states: + fraction = (255.0 - state) / (255.0 - 64.0) + angle = math.radians(10.0 + 65.0 * fraction) + shift = rng.normal(0.0, 0.35, size=2) + frame = _frame( + angle, + state_u8=float(state), + t0_xy=(500.0 + shift[0], 300.0 + shift[1]), + ) + frame["t3_x_px"] += float(rng.normal(0.0, noise_px)) + frame["t3_y_px"] += float(rng.normal(0.0, noise_px)) + observations.append(frame) + return observations + + +def _fit(observations: list[dict[str, float]] | None = None) -> dict: + return fit_image_circle_trajectory( + _trajectory() if observations is None else observations, + bin_size_u8=8.0, + minimum_frames=45, + minimum_bins=18, + minimum_state_span_u8=160.0, + minimum_radius_px=20.0, + minimum_arc_rad=math.radians(20.0), + maximum_radial_rms_px=2.0, + maximum_p95_radial_error_px=3.5, + ) + + +def _zero_summary( + angle_rad: float, + *, + t0_rad: float = math.radians(3.0), + t3_rad: float | None = None, +) -> 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) + ] + return summarize_zero_frames(frames) + + +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_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 test_circular_statistics_cross_pi_without_jumping() -> None: + values = [math.radians(179.0), math.radians(-179.0), math.pi] + result = circular_median_rad(values) + assert abs(signed_angle_difference_rad(result, math.pi)) < math.radians(1.1) + + +def test_circle_fit_recovers_center_radius_and_rejects_anchor_translation() -> None: + fit = _fit() + assert fit["centre_relative_xy_px"] == pytest.approx( + [-120.0, 80.0], abs=0.8 + ) + assert fit["radius_px"] == pytest.approx(90.0, abs=0.8) + assert fit["arc_rad"] >= math.radians(60.0) + assert fit["radial_rms_px"] < 0.5 + assert fit["passed"] is True + + +def test_circle_fit_is_robust_to_sparse_bad_tag_centres() -> None: + observations = _trajectory() + for index in (9, 31, 57, 92, 121): + observations[index]["t3_x_px"] += 18.0 + observations[index]["t3_y_px"] -= 15.0 + fit = _fit(observations) + assert fit["centre_relative_xy_px"] == pytest.approx( + [-120.0, 80.0], abs=1.5 + ) + assert fit["radius_px"] == pytest.approx(90.0, abs=1.5) + + +def test_zero_angle_comes_from_circle_radius_not_tag_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), + ) + result = measure_zero_from_circle(circle, summary) + 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 + ) + + +def test_circle_fit_rejects_short_state_span() -> None: + observations = [ + _frame(math.radians(10.0 + index * 0.1), state_u8=255.0 - index) + for index in range(50) + ] + with pytest.raises(ValueError, match="trajectory_state_span_too_small"): + _fit(observations) + + +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), + ] + payload, report = build_trajectory_zero_angle_payload( + serial_number="G20_LEFT_TEST", + rounds=rounds, + 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["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" + + +@pytest.mark.parametrize(("detection_rate", "radial_error"), [(0.8, 0.2), (0.99, 8.0)]) +def test_quality_fails_for_detection_or_zero_circle_error( + detection_rate: float, radial_error: float +) -> None: + rounds = [ + { + "zero_before": _measurement( + -0.50, -0.30, t0_rad=0.20, radial_error_px=radial_error + ), + "zero_after": _measurement( + -0.499, -0.299, t0_rad=0.20, radial_error_px=radial_error + ), + } + for _ in range(3) + ] + payload, _ = build_trajectory_zero_angle_payload( + serial_number="G20_LEFT_TEST", + rounds=rounds, + 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