拇指零位标定
This commit is contained in:
@@ -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/
|
||||
|
||||
@@ -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 中心抵消平移；
|
||||
它仍不是真实三维桌面检测，因此会随相机滚转和机械手摆放改变。
|
||||
|
||||
@@ -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
|
||||
+512
@@ -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")
|
||||
File diff suppressed because it is too large
Load Diff
@@ -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),
|
||||
]
|
||||
)
|
||||
@@ -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"
|
||||
),
|
||||
],
|
||||
},
|
||||
)
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
Reference in New Issue
Block a user