diff --git a/apps/autocal/config/locales/en_US.json b/apps/autocal/config/locales/en_US.json index c873204..0da67f5 100644 --- a/apps/autocal/config/locales/en_US.json +++ b/apps/autocal/config/locales/en_US.json @@ -181,6 +181,7 @@ "workspace": { "tab_axes": "Axis debug", "tab_sequence": "Test sequence", + "tab_finger": "Finger debug", "tab_sensor": "Sensor", "tab_surface": "Surface map", "tab_points": "Calibration points", @@ -189,6 +190,54 @@ "reserved_surface": "Multi-point calibration sensitivity will be drawn as a colour map on the sensor's 3D surface here: each point's sensitivity interpolated and coloured, with a legend and range.", "reserved_points": "The calibration point table and capture controls will appear here: platform coordinates, reference load, sampling statistics and the resulting sensitivity." }, + "finger": { + "title": "Finger calibration-point debug", + "bundle_path": "Export bundle", + "load": "Load bundle…", + "reset_view": "Reset view", + "initial_view": "Initial view", + "textured": "UV texture: {name}", + "untextured": "No UV texture: showing the shaded layout map.", + "texture_missing": "No texture at {path}: showing the shaded layout map.", + "flat_title": "Flat image (click to pick)", + "readout_title": "Readouts", + "preview_title": "3D preview", + "points_title": "Picked points", + "select_hint": "Click a point on the flat image to pick a calibration point.", + "no_bundle": "No export bundle loaded.", + "no_surface": "Pixel ({x}, {y}) is not on the sensing surface (UV gap).", + "row_sensor_point": "Sensor point (mm)", + "row_sensor_normal": "Surface normal", + "row_platform_point": "Platform point (mm)", + "row_pose": "Pose X / Y / yaw / pitch", + "row_residual": "Alignment residual", + "row_frames": "Frame mapping", + "row_bundle": "Bundle", + "bundle_summary": "{vertices} vertices · {triangles} triangles · {extent} mm · {scale} mm per unit", + "plate_thickness": "Mounting plate", + "tool_offset": "Tool offset x,y,z", + "clear_points": "Clear points", + "point_count": "{count} points", + "residual_value": "{residual}° — {verdict}", + "reachable": "this normal can be aligned with Z", + "unreachable": "this normal cannot be aligned with Z (yaw travel limit)", + "reach_summary": "Alignable surface vertices {aligned}/{vertices} ({share}%), worst {worst}°", + "preview_legend": "Surface colour = alignment residual (green aligned, red not) · yellow is the tool axis · X/Y are the table, Z the tool travel", + "show_axes": "Show motion axes", + "sim_play": "Play motion", + "sim_stop": "Stop", + "sim_hint": "Play the solved motion: the stages rotate the normal onto the tool axis, then the XY table moves. Pick a point first.", + "preview_no_gl": "No OpenGL context: {error}", + "preview_draw_failed": "Drawing failed: {error}", + "preview_solve_failed": "Solve failed: {error}", + "col_x": "x", "col_y": "y", "col_z": "z", + "col_nx": "nx", "col_ny": "ny", "col_nz": "nz", + "col_X": "X", "col_Y": "Y", "col_Z": "Z", + "col_yaw": "yaw", "col_pitch": "pitch", "col_residual": "residual(°)", + "loaded": "Bundle loaded: {vertices} vertices · {extent} mm", + "load_failed": "Load failed: {error}", + "picked": "Picked: residual {residual}° · yaw {yaw}° · pitch {pitch}°" + }, "sequence": { "title": "Test sequence design", "profile_title": "Point generation", diff --git a/apps/autocal/config/locales/zh_CN.json b/apps/autocal/config/locales/zh_CN.json index bdfc50a..0b3cc24 100644 --- a/apps/autocal/config/locales/zh_CN.json +++ b/apps/autocal/config/locales/zh_CN.json @@ -181,6 +181,7 @@ "workspace": { "tab_axes": "轴调试", "tab_sequence": "测试序列", + "tab_finger": "手指调试", "tab_sensor": "传感器", "tab_surface": "表面分布", "tab_points": "标定点", @@ -189,6 +190,54 @@ "reserved_surface": "多点标定的灵敏度将在此以色图叠加在传感器三维表面上：每个标定点的灵敏度经插值着色，并给出色标与范围。", "reserved_points": "标定点列表与采集控制将在此显示：平台坐标、参考载荷、采样统计与灵敏度结果。" }, + "finger": { + "title": "手指标定点调试", + "bundle_path": "导出包", + "load": "载入导出包…", + "reset_view": "复位视角", + "initial_view": "初始视角", + "textured": "UV 贴图：{name}", + "untextured": "无 UV 贴图：当前显示的是着色布局图。", + "texture_missing": "未找到贴图 {path}：当前显示的是着色布局图。", + "flat_title": "展开平面图（点击选点）", + "readout_title": "读数", + "preview_title": "三维预览", + "points_title": "已选标定点", + "select_hint": "在展开图上点击一点以选取标定点。", + "no_bundle": "尚未载入导出包。", + "no_surface": "像素 ({x}, {y}) 不在传感面上（UV 空隙）。", + "row_sensor_point": "传感器坐标 (mm)", + "row_sensor_normal": "曲面法向", + "row_platform_point": "平台坐标 (mm)", + "row_pose": "位姿 X / Y / yaw / pitch", + "row_residual": "对齐残差", + "row_frames": "坐标映射", + "row_bundle": "导出包", + "bundle_summary": "{vertices} 顶点 · {triangles} 三角面 · {extent} mm · 每单位 {scale} mm", + "plate_thickness": "安装板厚度", + "tool_offset": "工具偏移 x,y,z", + "clear_points": "清空标定点", + "point_count": "共 {count} 点", + "residual_value": "{residual}° — {verdict}", + "reachable": "该点法向可与 Z 轴对齐", + "unreachable": "该点法向无法与 Z 轴对齐（受 yaw 行程限制）", + "reach_summary": "曲面可对齐顶点 {aligned}/{vertices}（{share}%），最差 {worst}°", + "preview_legend": "表面颜色 = 对齐残差（绿=对齐，红=不可对齐）· 黄色为工具轴 · X/Y 为工作台，Z 为工具行程", + "show_axes": "显示运动轴", + "sim_play": "播放动作", + "sim_stop": "停止", + "sim_hint": "播放该点的求解动作：先由转台把法向转到工具轴上，再由 XY 工作台移动。请先取点。", + "preview_no_gl": "无 OpenGL 上下文：{error}", + "preview_draw_failed": "绘制失败：{error}", + "preview_solve_failed": "求解失败：{error}", + "col_x": "x", "col_y": "y", "col_z": "z", + "col_nx": "nx", "col_ny": "ny", "col_nz": "nz", + "col_X": "X", "col_Y": "Y", "col_Z": "Z", + "col_yaw": "yaw", "col_pitch": "pitch", "col_residual": "残差(°)", + "loaded": "已载入导出包：{vertices} 顶点 · {extent} mm", + "load_failed": "载入失败: {error}", + "picked": "已选点：残差 {residual}° · yaw {yaw}° · pitch {pitch}°" + }, "sequence": { "title": "测试序列设计", "profile_title": "标定点生成", diff --git a/apps/autocal/docs/finger-debug-findings.md b/apps/autocal/docs/finger-debug-findings.md new file mode 100644 index 0000000..f9810d5 --- /dev/null +++ b/apps/autocal/docs/finger-debug-findings.md @@ -0,0 +1,112 @@ +# Finger calibration-point debugger — findings so far + +Status: subsystem 1 (UV picking) works and is measured. Subsystem 2 (simulated +5-axis pose) works and is measured. This note records the numbers, and the two +solver bugs that produced confident wrong ones on the way here. + +## What is established + +### The export bundle (`finger/r01`, `compute` profile) + +| Quantity | Value | +|---|---| +| Mesh | 1032 vertices, 1974 triangles (matches `profiles/compute.yaml`) | +| UV atlas | **one connected island**, u 0→1, v 0.142→0.857 | +| Pose | `world_to_cam`, uniform scale `0.018388452316735537`, `det(R) = +1` | +| Physical size | sensing **15.0 × 7.5 × 15.81 mm**; housing 16.0 × 9.5 × 21.0 mm | +| Surface area | 355.77 mm² | +| Origin | housing base at `y ≈ 0`; sensing spans `y ≈ 0.99 … 8.49 mm` | +| Curvature | ~12% of area within 15° of one axis; area-weighted normal ≈ `(0, 0.57, 0.26)` | + +**The mesh is not in millimetres.** `blender/scripts/export.py` transforms +vertices by `t_sensor_from_world @ world`, and the pose's linear block carries the +`origin_sensor` empty's own uniform scale — 1/54.4 on `finger/r01`. Physical size +is *mesh units × that factor*, so the factor is derived from the file (and +refused if it is not a uniform rotation) rather than assumed. A consumer that +treats the vertices as millimetres is wrong by 54×. + +### UV picking + +`rasterise()` builds, in one pass, a flat image of the UV layout **and** a +`pixel → (position, normal, face)` table, at 1024×1024 in 0.24 s. Measured: + +- valid (surface-bearing) pixels: 72.9% +- round trip `vertex → its UV pixel → looked-up 3D`: worst error **0.0081 mm** +- a pixel in a UV-island gap returns `None` rather than a fabricated `(0,0,0)` + +Because the atlas is a single island, a click maps to exactly one 3D point. + +## The supposed finding, and its retraction + +An earlier revision of this note reported that the **yaw +-90 deg limit made a +quarter of the patch unalignable**, worst residual 65.1 deg, only 47 of 61 +sampled points reachable. **That was a solver bug, not a property of the +machine.** The corrected solver solves the real surface as: + +| Envelope | Worst residual | Median | Within 0.01 deg | Within 0.5 deg | +|---|---|---|---|---| +| pitch 360 deg, **yaw +-90 deg** (the machine) | **0.21 deg** | 0.00 deg | **98.4%** | **100%** | +| pitch 360 deg, yaw +-180 deg | 0.24 deg | 0.00 deg | — | 100% | + +So **every** vertex of the sensing patch is reachable, and widening the yaw range +buys essentially nothing (0.21 deg → 0.24 deg worst). The 0.21 deg worst case is +a genuinely clamped vertex: the sweep's own minimum at that vertex is 0.211 deg +at the yaw clamp, so the number is physical, not lost precision. + +Why the yaw limit is not the constraint, which the earlier note had backwards: + +- The pitch stage turns a vector around a circle in the plane **perpendicular to + the pitch axis**. +- The yaw stage can reach a given great circle as long as the yawed normal is not + parallel to the pitch axis. A normal parallel to the pitch axis after yawing + would have to be perpendicular to the tool axis already, and the finger patch + normals — which point along sensor +Y, mapping to platform **+Z**, the tool + direction — are nothing of the sort. +- Pitch travel is therefore what does the work, and the 360 deg stage provides it. + +**No mounting change is required for reachability.** The three options the earlier +note listed (re-mount, state a tolerance, index the plate) are all unnecessary on +the reachability evidence. Whether they help *repeatability* — a shorter pitch +move, a better-conditioned contact — is a separate bench question, not a +kinematic one. + +## What was wrong before, and why it is worth recording + +**Four** solver versions preceded this one, each wrong in a way that produced +confident numbers: + +- rotating the **sensor-frame** normal while the stages act in the platform frame + (uniform ~90 deg error); +- composing the stages in the wrong order; +- a **seeded** search, which left 22.7 deg on the table for normals whose best yaw + sat far from the analytic guess; +- a sweep whose yaw ranking used "how close does the yawed normal come to the tool + axis", which is **not** the quantity that matters. What matters is where the + whole circle the pitch stage sweeps sits. Ranking by the wrong one left **68 deg** + on the table while looking entirely reasonable — this is the bug that produced + the retracted 65 deg finding above. + +The solver is now a sweep of every yaw/pitch combination, evaluated as a grid one +yaw at a time (7.8 ms per point, 1032 vertices in 9 s), then a local polish to +1e-4 deg. It is verified two ways: it is **never worse than a brute-force search** +at 2 deg on random normals, and its returned residual matches an **independently +composed rotation matrix** to 8e-8 deg. Both checks are tests, not one-off probes. + +The residual is measured from the actual rotations, so a wrong derivation shows up +as a number rather than as a plausible pose. + +## Implemented so far + +| Module | State | +|---|---| +| `sensor/frames.py` | mount mapping as a named constant; refuses a non-rotation | +| `sensor/export_bundle.py` | load + validate; derives mm/unit; refuses a non-uniform or mirrored pose, and count mismatches | +| `sensor/uv_flat.py` | flat image + pixel→3D table; gap pixels are `None` | +| `motion/kinematics.py` | constrained search + measured residual; stage limits are parameters; singular case reported, not guessed | +| `ui/sensor_panel.py` | picker, readouts, point table; refuses gap pixels with a reason; picked rows kept at full precision, not re-read from the 3-decimal display text | +| `ui/pose_preview.py` | 3D preview coloured by residual, tool axis drawn; the legend is a sibling label, because text composited over the GL surface is unreadable once it overflows | +| workspace `finger` tab | wired, checked at 1440×900 and 1920×1080 in both themes | +| startup | the remembered bundle loads automatically, so the tab comes up working instead of showing a filled-in path over two empty views | + +Not yet built: capture of picked points into the sequence, and the live-sensor +counterpart of the simulated pose. diff --git a/apps/autocal/src/orisys_autocal/motion/kinematics.py b/apps/autocal/src/orisys_autocal/motion/kinematics.py new file mode 100644 index 0000000..5714c0c --- /dev/null +++ b/apps/autocal/src/orisys_autocal/motion/kinematics.py @@ -0,0 +1,748 @@ +"""Simulated 5-axis pose: which X, Y, yaw and pitch put the tool on a point. + +The platform is an XY table with a yaw stage and a pitch stage stacked on it, and +a separate Z axis carrying the tool. Aligning the patch normal with the tool axis +is the whole reason the rotation stages exist, so this module's job is: + +* **solve** the pose that does it, in closed form; +* **report the residual** — the angle left between the rotated normal and the tool + axis. A pose is not "correct", it is within a stated tolerance, and a point that + cannot meet it must say so rather than return a plausible-looking pose. + +Derivation, with the mount rotation taking the sensor normal ``n`` into the +platform frame and rotations composed yaw-then-pitch:: + + aim = approach_sign * n_platform # direction the tool must push + +For a fixed yaw the tool-axis dot product is a single sinusoid in the pitch, so +the best pitch at that yaw has a closed form and the search is a sweep over yaw +alone. The first version of this assumed the aim was ``+Z`` in the local basis +and got a 0.5–90 degree residual depending on the point; the brute-force test in +``test_kinematics.py`` is what pins the formula down. + +Pure NumPy, no Qt, so this stays importable from a headless test process. +""" + +from __future__ import annotations + +import math +from dataclasses import dataclass, field + +import numpy as np + +from ..sensor.frames import ( + AXIS_MAP, + PITCH_AXIS, + TOOL_AXIS, + YAW_AXIS, + mount_rotation, + to_platform, + to_sensor, +) + +#: A normal this close to the tool axis counts as already aligned. +ALIGNED_TOL_DEG = 0.01 + +#: Sweep step, degrees, for each stage of the alignment search. The sweep only +#: has to land in the right basin — the polish that follows refines it to 1e-4 — +#: so this trades a little precision for a sweep the eye can be told about. +SWEEP_STEP = 0.5 + +#: Stop shrinking the polish step at this size, degrees. Well below any angle a +#: stage can physically hold, so the reported angle is the machine's, not ours. +POLISH_FLOOR = 1e-4 + +#: Sweep grids and their trig, keyed on ``(yaw_limit, pitch_limit, step)``. They +#: depend only on the machine envelope, and a solve that rebuilt them was paying +#: for the same ``arange`` and ``cos`` 1032 times over one patch. +_SWEEP_CACHE: dict[tuple[float, float, float], dict[str, np.ndarray]] = {} +_SWEEP_CACHE_LIMIT = 8 + +#: How many extra pitch candidates to try either side of the closed-form best. +#: The closed form is exact, so these exist only so a numerical surprise shows up +#: as a slightly worse residual in the agreement test rather than as a crash. +_PITCH_PROBES = (0.0, -SWEEP_STEP, SWEEP_STEP) + +#: Below this, the term that decides whether the sensing face ends up or down is +#: numerical noise rather than geometry, and the tie is broken toward upright. +_UPRIGHT_TIE = 1e-6 + +#: Two poses within this many degrees of alignment are the same pose as far as the +#: machine is concerned, so the upright one is reported. Small on purpose: it only +#: has to absorb the sweep's own quantisation, not excuse a real misalignment. +_UPRIGHT_TOL_DEG = 0.05 + + + +def rotation_about(axis: str, degrees: float) -> np.ndarray: + """Right-handed rotation of ``degrees`` about a platform axis.""" + radians = np.radians(float(degrees)) + cosine, sine = float(np.cos(radians)), float(np.sin(radians)) + if axis == "z": + return np.array( + [[cosine, -sine, 0.0], [sine, cosine, 0.0], [0.0, 0.0, 1.0]], dtype=np.float64 + ) + if axis == "y": + # Positive pitch turns +Z toward +X. + return np.array( + [[cosine, 0.0, sine], [0.0, 1.0, 0.0], [-sine, 0.0, cosine]], dtype=np.float64 + ) + if axis == "x": + return np.array( + [[1.0, 0.0, 0.0], [0.0, cosine, -sine], [0.0, sine, cosine]], dtype=np.float64 + ) + raise ValueError(f"unknown axis {axis!r}") + + +@dataclass(frozen=True) +class Pose: + """One solved platform pose, or the reason there is not one.""" + + ok: bool + #: Platform-frame coordinates of the contact point, mm. + target_xy_mm: tuple[float, float] = (0.0, 0.0) + #: Platform-frame Z of the contact point, mm. Carries the mounting plate and + #: any Z tool offset, which ``target_xy_mm`` cannot: without it those inputs + #: are computed and then discarded, and nothing downstream can tell. + contact_z_mm: float = 0.0 + yaw_deg: float = 0.0 + pitch_deg: float = 0.0 + #: The separate Z axis. Reported, never solved for here. + tool_z_mm: float = 0.0 + #: Angle left between the rotated normal and the tool axis, degrees. + residual_deg: float = 0.0 + #: Whether the yaw could be determined at all (indeterminate at the pole). + yaw_determined: bool = True + reason: str = "" + + @property + def reached_tolerance(self) -> bool: + return self.ok and self.residual_deg <= ALIGNED_TOL_DEG + + @property + def reachable(self) -> bool: + """Whether the stack can align this point at all. + + False means the geometry cannot bring the tool axis onto the normal, no + matter the angles — a statement about the machine, not about the search. + """ + return self.ok and self.residual_deg <= 0.5 + + def describe(self) -> str: + if not self.ok: + return f"no pose: {self.reason}" + yaw = f"{self.yaw_deg:+.3f}" if self.yaw_determined else "indeterminate" + return ( + f"X {self.target_xy_mm[0]:+.3f} Y {self.target_xy_mm[1]:+.3f} " + f"Z {self.contact_z_mm:+.3f} yaw {yaw} pitch {self.pitch_deg:+.3f} " + f"residual {self.residual_deg:.4f} deg" + ) + + +@dataclass +class PlatformStack: + """The kinematic chain, so the assumptions are inspectable and testable.""" + + axis_map: dict[str, str] = field(default_factory=lambda: dict(AXIS_MAP)) + yaw_axis: str = YAW_AXIS + pitch_axis: str = PITCH_AXIS + tool_axis: str = TOOL_AXIS + #: Which way the tool pushes along ``tool_axis`` relative to the patch + #: normal the mesh stores. The mesh's normal points *away* from the sensing + #: surface, and the tool presses *into* it, so the tool direction is the + #: negative of the platform-frame normal — hence ``-1`` by default. Getting + #: this wrong does not look wrong: it silently gives a ~90 degree residual on + #: a patch that is actually fine, which is how it was found. + approach_sign: int = -1 + #: Added to the reported contact point, mm, in the platform frame. The tool + #: tip offset and any fixture height the caller wants folded in. + tool_offset_mm: tuple[float, float, float] = (0.0, 0.0, 0.0) + #: The residual a consumer will accept; a pose worse than this is still + #: returned, but flagged. + tolerance_deg: float = ALIGNED_TOL_DEG + #: Travel limits of the two stages, degrees. A solution outside them is not a + #: solution: the search is constrained to the envelope the hardware has. + #: The bench's figures: pitch turns continuously (360°), yaw is ±90°. + yaw_limit_deg: float = 90.0 + pitch_limit_deg: float = 360.0 + + def __post_init__(self) -> None: + if self.approach_sign not in (1, -1): + raise ValueError(f"approach_sign must be +1 or -1, got {self.approach_sign}") + for axis in (self.yaw_axis, self.pitch_axis, self.tool_axis): + if axis not in ("x", "y", "z"): + raise ValueError(f"unknown axis {axis!r}") + + # ------------------------------------------------------------------ frames + + @property + def mount(self) -> np.ndarray: + return mount_rotation(self.axis_map) + + @property + def tool_direction(self) -> np.ndarray: + """The unit vector the tool pushes along, in the platform frame.""" + direction = np.zeros(3, dtype=np.float64) + direction[("x", "y", "z").index(self.tool_axis)] = float(self.approach_sign) + return direction + + def normal_to_platform(self, normal_sensor: np.ndarray) -> np.ndarray: + return to_platform(np.asarray(normal_sensor, dtype=np.float64), self.axis_map) + + def point_to_platform( + self, point_sensor_mm: np.ndarray, *, plate_thickness_mm: float = 0.0 + ) -> np.ndarray: + """A sensor-frame point in the platform frame. + + The plate lifts the sensor along the platform's tool axis, so it is a + translation along :data:`tool_axis` applied after the mount rotation. + """ + platform = to_platform(np.asarray(point_sensor_mm, dtype=np.float64), self.axis_map) + lift = np.zeros(3, dtype=np.float64) + lift[("x", "y", "z").index(self.tool_axis)] = float(plate_thickness_mm) + return platform + lift + + # -------------------------------------------------------------------- solve + + def solve( + self, + point_sensor_mm: np.ndarray, + normal_sensor: np.ndarray, + *, + plate_thickness_mm: float = 0.0, + tool_z_mm: float = 0.0, + ) -> Pose: + """The yaw/pitch that put the tool axis on ``normal_sensor``, and the XY. + + The reported XY is the **contact point's** position, not the sensor + origin's, so it is the coordinate the tool actually has to be above. + """ + normal = self.normal_to_platform(normal_sensor) + length = float(np.linalg.norm(normal)) + if not np.isfinite(length) or length < 1e-12: + return Pose(ok=False, reason="the surface normal is degenerate") + # The *stored* normal points away from the sensing surface; the tool + # pushes into it. ``approach_sign`` carries that, and it appears twice + # below — once in the pitch (through z) and once in the yaw. Applying it + # to the vector itself instead is what produced a 90 degree residual on + # every point when this was first written. + unit = normal / length + sign = float(self.approach_sign) + + tool = self.tool_direction + + # Express the normal in a basis whose Z is the tool axis, so the closed + # form does not care which platform axis the tool runs along. + basis = _tool_basis(self.tool_axis) + local = basis @ unit + + point = self.point_to_platform(point_sensor_mm, plate_thickness_mm=plate_thickness_mm) + point = point + np.asarray(self.tool_offset_mm, dtype=np.float64) + + # Solve numerically, and deliberately so. + # + # The stages rotate the **platform-frame** normal; the local basis is only + # a convenience for reasoning about which axis the tool runs along. An + # earlier version rotated the local vector and reported a 90 degree + # residual for a normal that was already aligned — exactly the class of + # confident wrong number this residual exists to prevent. So the search + # and the residual both work on ``unit``, and ``residual_for`` (used by + # the preview) does the same, so a point cannot be green in the readout + # and red in the picture. + # + # There is deliberately no special case for a normal lying *on* the tool + # axis. One existed, returned a zero residual from a formula, and + # disagreed with the rotations by 90 degrees — the search finds the real + # answer (which is 0) and the indeterminacy is reported separately below. + yaw_deg, pitch_deg, residual = self._search_alignment(unit, tool) + + # At the pole the azimuth is undefined, so every yaw gives the same + # alignment. Say so rather than presenting an arbitrary angle as the + # answer; the returned pose is still one that works. + on_axis = float(np.hypot(local[0], local[1])) < 1e-9 + determined = not on_axis + reason = "" + if on_axis: + reason = "the normal lies on the tool axis; yaw is indeterminate" + elif residual > self.tolerance_deg: + reason = "no pose meets the tolerance" + + return Pose( + ok=True, + target_xy_mm=(self._axis(point, "x"), self._axis(point, "y")), + contact_z_mm=self._axis(point, "z"), + yaw_deg=yaw_deg, + pitch_deg=pitch_deg, + tool_z_mm=float(tool_z_mm), + residual_deg=residual, + yaw_determined=determined, + reason=reason, + ) + + def _rotation_coefficients( + self, axis: str + ) -> tuple[int, int, int, float, float]: + """``(stationary, first, second, sign_first, sign_second)`` for one axis. + + The one place the rotation conventions live. ``rotation_about`` writes them + as matrices, and each axis has its own signs: about ``y`` a positive turn + moves ``+Z`` toward ``+X``, which is the opposite sense to the turns about + ``x`` and ``z``. + + Both the scalar residual and the vectorised sweep go through here, because + spelling the two sign conventions out twice is exactly how a fast path ends + up disagreeing with the slow one by 74 degrees while looking reasonable. + """ + if axis == "z": # (x, y) -> (x c - y s, x s + y c) + return 2, 0, 1, -1.0, 1.0 + if axis == "x": # (y, z) -> (y c - z s, y s + z c) + return 0, 1, 2, -1.0, 1.0 + if axis == "y": # (z, x) -> (z c - x s, z s + x c) + return 1, 2, 0, -1.0, 1.0 + raise ValueError(f"unknown axis {axis!r}") + + def _cell(self, unit: np.ndarray, yaw_deg: float, pitch_deg: float) -> tuple: + """The yawed and then pitched components, as ``(stationary, first, second)``. + + Scalar, and the shared definition of what the two stages do to a vector. + """ + yaw = math.radians(yaw_deg) + yaw_cos, yaw_sin = math.cos(yaw), math.sin(yaw) + yawed = [0.0, 0.0, 0.0] + stationary, first, second, sign_first, sign_second = self._rotation_coefficients( + self.yaw_axis + ) + yawed[stationary] = unit[stationary] + yawed[first] = unit[first] * yaw_cos + sign_first * unit[second] * yaw_sin + yawed[second] = sign_second * unit[first] * yaw_sin + unit[second] * yaw_cos + + pitch = math.radians(pitch_deg) + pitch_cos, pitch_sin = math.cos(pitch), math.sin(pitch) + turned = [0.0, 0.0, 0.0] + stationary, first, second, sign_first, sign_second = self._rotation_coefficients( + self.pitch_axis + ) + turned[stationary] = yawed[stationary] + turned[first] = yawed[first] * pitch_cos + sign_first * yawed[second] * pitch_sin + turned[second] = sign_second * yawed[first] * pitch_sin + yawed[second] * pitch_cos + return float(turned[0]), float(turned[1]), float(turned[2]) + + def _residual(self, unit: np.ndarray, yaw_deg: float, pitch_deg: float) -> float: + """The misalignment left by these angles, degrees. + + Written through ``_cell`` rather than two matrix products. The polish loop + calls this tens of thousands of times per patch, and building two 3x3 + arrays plus a matmul for a single vector was most of what it spent. + """ + turned = self._cell(unit, yaw_deg, pitch_deg) + tool = self.tool_direction + cosine = ( + turned[0] * float(tool[0]) + + turned[1] * float(tool[1]) + + turned[2] * float(tool[2]) + ) + return float(math.degrees(math.acos(max(-1.0, min(1.0, cosine))))) + + def _turn(self, axis: str, angle_deg: np.ndarray, vector: np.ndarray) -> np.ndarray: + """Rotate ``vector`` about a platform axis by each of ``angle_deg``. + + ``vector`` is (3,) or (n, 3); the result is (n, 3). The array form of + ``rotation_about(...) @ vector``, built from the same sign table as the + scalar path so the search and the preview cannot drift apart. + """ + angle = np.radians(np.asarray(angle_deg, dtype=np.float64)) + vector = np.asarray(vector, dtype=np.float64) + stationary, first, second, sign_first, sign_second = self._rotation_coefficients(axis) + cosine, sine = np.cos(angle), np.sin(angle) + + turned = np.empty((angle.size, 3), dtype=np.float64) + turned[:, stationary] = vector[stationary] + turned[:, first] = vector[first] * cosine + sign_first * vector[second] * sine + turned[:, second] = sign_second * vector[first] * sine + vector[second] * cosine + return turned + + def _angle_grid( + self, yaw_limit: float, pitch_limit: float, step: float = SWEEP_STEP + ) -> tuple[np.ndarray, np.ndarray]: + """The yaw and pitch angles each stage is swept over, both including zero. + + The yaw stage is swept at whatever step the limits afford, the pitch stage + at ``step``, and both always contain zero so a normal that needs no + rotation is found exactly rather than approached. + """ + yaws = np.arange(-yaw_limit, yaw_limit + step, step) + pitches = np.arange(-pitch_limit, pitch_limit + step, step) + if yaws.size == 0: + yaws = np.zeros(1) + if pitches.size == 0: + pitches = np.zeros(1) + for grid in (yaws, pitches): + if not np.any(np.isclose(grid, 0.0)): + grid[0] = 0.0 + return yaws, pitches + + def _sweep_tables(self, yaw_limit: float, pitch_limit: float) -> dict[str, np.ndarray]: + """The angle grids and their trig, cached on the envelope that defines them. + + Keyed on the limits rather than the instance, so a stack whose limits are + changed through the normal path cannot be served a stale grid. Rebuilding + these per solve was pure waste: they depend only on the envelope, and the + envelope is a machine constant. + """ + key = (float(yaw_limit), float(pitch_limit), float(SWEEP_STEP)) + cached = _SWEEP_CACHE.get(key) + if cached is None: + yaws, pitches = self._angle_grid(yaw_limit, pitch_limit) + cached = { + "yaws": yaws, + "pitches": pitches, + "yaw_cos": np.cos(np.radians(yaws)), + "yaw_sin": np.sin(np.radians(yaws)), + } + if len(_SWEEP_CACHE) >= _SWEEP_CACHE_LIMIT: + _SWEEP_CACHE.clear() + _SWEEP_CACHE[key] = cached + return cached + + def _best_pitch_per_yaw(self, unit: np.ndarray, yaws: np.ndarray) -> np.ndarray: + """The pitch that best aligns each yawed normal, in closed form. + + For one yawed vector ``v`` the tool-axis dot product as a function of the + pitch is ``x·tx + z·tz``-weighted cosine plus a sine term — a single + sinusoid — so its exact maximum is at ``atan2`` of those two coefficients + with no pitch sweep at all. Solving every yaw at once is therefore + ``O(n_yaw)`` instead of ``O(n_yaw * n_pitch)``, which is the whole cost of + the exhaustive grid below. + + The coefficients are written out by axis name rather than through the + ``first``/``second`` rotation ordering, because it is the *cross* term + that carries the sign and getting that backwards returns the antipode — + an angle 180 deg away that looks entirely plausible. The signs here were + fixed against a direct numeric scan, and + ``test_the_closed_form_pitch_beats_a_direct_scan`` keeps them that way. + """ + tool = np.asarray(self.tool_direction, dtype=np.float64) + axis = self.pitch_axis + others = [name for name in ("x", "y", "z") if name != axis] + leading, trailing = others[0], others[1] + lead, trail = ("x", "y", "z").index(leading), ("x", "y", "z").index(trailing) + + yawed = self._turn(self.yaw_axis, yaws, unit) + cosine_coefficient = ( + yawed[:, lead] * tool[lead] + yawed[:, trail] * tool[trail] + ) + sine_coefficient = ( + yawed[:, trail] * tool[lead] - yawed[:, lead] * tool[trail] + ) + return np.degrees(np.arctan2(sine_coefficient, cosine_coefficient)) + + def _grid_reference( + self, unit: np.ndarray, yaws: np.ndarray, pitches: np.ndarray + ) -> np.ndarray: + """The full ``(n_yaw, n_pitch)`` residual grid, kept as the slow reference. + + The exhaustive sweep, spelled out one yaw at a time, using the same rotation + core as everything else. It is too slow to be the shipping path but it is + the definition the fast path is checked against, and an exhaustive check is + what caught the earlier sweep that ranked yaws by the wrong quantity. + """ + cost = np.empty((yaws.size, pitches.size), dtype=np.float64) + for row, yaw in enumerate(yaws): + cost[row] = self._residuals_at(unit, np.full(pitches.size, yaw), pitches) + return cost + + def _residuals_at( + self, unit: np.ndarray, yaws: np.ndarray, pitches: np.ndarray + ) -> np.ndarray: + """Residual for each ``(yaws[i], pitches[i])`` pair, vectorised.""" + tool = np.asarray(self.tool_direction, dtype=np.float64) + yawed = self._turn(self.yaw_axis, yaws, unit) + pitch = np.radians(np.asarray(pitches, dtype=np.float64)) + cosine, sine = np.cos(pitch), np.sin(pitch) + stationary, first, second, sign_first, sign_second = self._rotation_coefficients( + self.pitch_axis + ) + turned = np.empty_like(yawed) + turned[:, stationary] = yawed[:, stationary] + turned[:, first] = yawed[:, first] * cosine + sign_first * yawed[:, second] * sine + turned[:, second] = sign_second * yawed[:, first] * sine + yawed[:, second] * cosine + return np.degrees(np.arccos(np.clip(turned @ tool, -1.0, 1.0))) + + def search_alignment_reference( + self, unit: np.ndarray, step: float = SWEEP_STEP + ) -> tuple[float, float, float]: + """The alignment found by sweeping every yaw/pitch pair, unhurried. + + This is the definition the fast search is checked against: no closed form, + no probes, just the whole grid. It is far too slow to ship — a full patch + through this takes minutes — so it exists for the agreement test. + + ``step`` widens the grid so that test can run in seconds instead of a + minute. A coarser step can only make this answer *worse*, which is the + direction the comparison tolerates: the shipped path is allowed to beat it + and is never allowed to lose to it. + """ + yaw_limit, pitch_limit = self._rotation_limits() + yaws, pitches = self._angle_grid(yaw_limit, pitch_limit, step) + cost = self._grid_reference(unit, yaws, pitches) + yaw_index, pitch_index = np.unravel_index(int(np.argmin(cost)), cost.shape) + yaw = float(np.clip(yaws[yaw_index], -yaw_limit, yaw_limit)) + pitch = float(np.clip(pitches[pitch_index], -pitch_limit, pitch_limit)) + return yaw, pitch, float(self._residual(unit, yaw, pitch)) + + def _rotation_limits(self) -> tuple[float, float]: + # Yaw is reported within ±180° however far the stage travels, because an + # angle is only meaningful modulo a full turn; pitch is capped at 360° + # because beyond that the search would only revisit the same poses. + yaw = float(min(180.0, max(0.0, self.yaw_limit_deg))) + pitch = float(min(360.0, max(0.0, self.pitch_limit_deg))) + return yaw, pitch + + def _yawed_upright(self, unit: np.ndarray, yaws: np.ndarray) -> np.ndarray: + """Whether each yaw leaves the sensing face pointing up, as a boolean array. + + The mount puts sensor +Y on the platform's tool axis, and the pitch stage + turns about an axis perpendicular to it, so the sign of the yawed normal's + pitch-axis component is exactly the sign of the final sensor +Y. Picking the + pose with a positive one is what keeps the sensing face upright: without it + the search can settle on the branch that reaches the same normal from the + other side, which reads on screen as the sensor turned upside down. + + This is a tie-break between two ways of aligning, not a change to what + "aligned" means — a point that needs the flipped branch still gets it. + """ + index = {"x": 0, "y": 1, "z": 2}[self.pitch_axis] + return self._turn(self.yaw_axis, yaws, unit)[:, index] >= 0.0 + + def _search_alignment( + self, unit: np.ndarray, tool: np.ndarray + ) -> tuple[float, float, float]: + """Best yaw/pitch inside the stage envelope, with the residual it achieves. + + The stages compose as ``R_pitch @ R_yaw`` and the pitch stage acts on the + already-yawed vector, so the two are separable: for a fixed yaw the pitch + that aligns best is a single sinusoid's maximum, available in closed form. + The best yaw is then the one whose best pitch leaves the least. Ranking the + yaws that way is exact and costs one scalar per yaw, rather than a row of + pitch samples each. + + Among poses that align equally well, an upright one wins: see + :meth:`_yawed_upright`. + + The sweep is deliberately not seeded from a formula. An earlier seeded + version left 22.7 deg on the table for normals whose best yaw sat far from + the guess: a quiet failure of exactly the kind this residual exists to + make loud. Sweeping every yaw keeps that guarantee. + """ + yaw_limit, pitch_limit = self._rotation_limits() + + tables = self._sweep_tables(yaw_limit, pitch_limit) + yaws = tables["yaws"] + + # One closed-form best pitch per yaw, which is the exact optimum of that + # yaw's sinusoid; plus the flipped branch, and a pair either side so a + # clipped or quantised optimum is not mistaken for the best one. + best_pitches = self._best_pitch_per_yaw(unit, yaws) + + best = None + for offset in _PITCH_PROBES + (180.0, -180.0): + pitches_for_yaw = np.clip(best_pitches + offset, -pitch_limit, pitch_limit) + residuals = self._residuals_at(unit, yaws, pitches_for_yaw) + for index in np.argsort(residuals)[:3]: + candidate = ( + float(residuals[index]), + float(np.clip(yaws[index], -yaw_limit, yaw_limit)), + float(pitches_for_yaw[index]), + ) + if best is None or _better_pose(candidate, best, self): + best = candidate + assert best is not None + # The candidate set is quantised in yaw and its pitch can be clipped, so + # re-evaluate the winner with the scalar path: the number reported must be + # the residual at the real angles, not a sampled one. + best = (float(self._residual(unit, best[1], best[2])), best[1], best[2]) + + # Polish the winner to the resolution the report shows. This cannot fall + # into a different basin: the sweep above already found the global one. + step = SWEEP_STEP + while step > POLISH_FLOOR: + improved = False + yaw_here, pitch_here = best[1], best[2] + for yaw in (yaw_here - step, yaw_here, yaw_here + step): + if abs(yaw) > yaw_limit: + continue + for pitch in (pitch_here - step, pitch_here, pitch_here + step): + if abs(pitch) > pitch_limit: + continue + residual = self._residual(unit, yaw, pitch) + if residual < best[0] - 1e-12: + best = (residual, yaw, pitch) + improved = True + if not improved: + step /= 2.0 + + # Among poses that align equally well, report the smallest rotation. A + # half-turn that reaches the same place is a bigger move on the bench and + # a worse thing to hand an operator, so it is not the answer to give. + yaw_deg, pitch_deg = self._prefer_smallest(unit, best[1], best[2], best[0], yaw_limit, pitch_limit) + return yaw_deg, pitch_deg, best[0] + + def stage_rotation(self, yaw_deg: float, pitch_deg: float) -> np.ndarray: + """The two stage rotations in the mesh's frame: ``Mᵀ · R_pitch · R_yaw · M``. + + The solver works in the platform frame and the mesh is stored in the sensor + frame, so anything that has to reason about where the *body* ends up — the + preview and this module's uprightness test — needs the rotation conjugated + this way. One definition, so the two cannot disagree. + """ + mount = mount_rotation(self.axis_map) + return ( + mount.T + @ rotation_about(self.pitch_axis, pitch_deg) + @ rotation_about(self.yaw_axis, yaw_deg) + @ mount + ) + + def uprightness(self, yaw_deg: float, pitch_deg: float) -> float: + """How much the sensing face still points along the tool axis, in -1..1. + + Measured, not derived from a formula over one stage: the sensor's own up + axis is put through the composed stage rotation and dotted with the tool + direction in the same frame. Two attempts at the formula were wrong in + opposite directions and each looked reasonable, so this reads the same + rotations the preview draws. + + The tool presses *into* the sensing face, so the sensor's up axis is + anti-parallel to ``tool_direction``; nominal (no rotation) is therefore + ``+1`` with the negation below, which is the anchor this is checked + against — without it the whole scale came out backwards. + """ + turned = self.stage_rotation(yaw_deg, pitch_deg) @ np.array([0.0, 1.0, 0.0]) + turned = turned / max(float(np.linalg.norm(turned)), 1e-12) + tool_local = to_sensor(np.asarray(self.tool_direction, dtype=np.float64), self.axis_map) + tool_local = tool_local / max(float(np.linalg.norm(tool_local)), 1e-12) + return float(turned @ tool_local) + + def ends_upright(self, yaw_deg: float, pitch_deg: float) -> bool: + """``uprightness`` as a decision, with a dead band for numerical ties.""" + lean = self.uprightness(yaw_deg, pitch_deg) + if lean > _UPRIGHT_TIE: + return True + if lean < -_UPRIGHT_TIE: + return False + # Indeterminate: the two poses are the same alignment to any tolerance the + # machine has, so take the one needing the smaller move. + return abs(pitch_deg) <= 90.0 + + def _prefer_smallest( + self, + unit: np.ndarray, + yaw_deg: float, + pitch_deg: float, + residual: float, + yaw_limit: float, + pitch_limit: float, + ) -> tuple[float, float]: + """Try to trade the found pose for one that aligns as well but is upright. + + Rotation angles are only meaningful modulo a full turn, and alignment is + symmetric: bringing a normal onto the tool axis also works with **both** + stages turned through 180 degrees, because that maps the normal onto the + same line. The two align equally, so the machine should take the one that + leaves the sensing face up — on this mount that is also usually the smaller + move. The earlier version only tried flipping the pitch, which missed the + pair: for a vertex whose normal is exactly sensor +Y it returned pitch + -179.8 deg where an upright pose existed, and the sensor was drawn upside + down. + + It makes no claim to find the global minimum, and for a normal exactly on + the tool axis it does not (there every yaw works, so "smallest" is taste). + """ + candidates = [(yaw_deg, pitch_deg)] + for yaw_turn in (0.0, 180.0, -180.0): + for pitch_turn in (0.0, 180.0, -180.0, 360.0, -360.0): + if yaw_turn == 0.0 and pitch_turn == 0.0: + continue + candidate_yaw = yaw_deg + yaw_turn + candidate_pitch = pitch_deg + pitch_turn + if abs(candidate_yaw) <= yaw_limit and abs(candidate_pitch) <= pitch_limit: + candidates.append((candidate_yaw, candidate_pitch)) + best = (yaw_deg, pitch_deg) + best_cost = abs(yaw_deg) + abs(pitch_deg) + for candidate_yaw, candidate_pitch in candidates: + if self._residual(unit, candidate_yaw, candidate_pitch) > residual + 1e-6: + continue + cost = abs(candidate_yaw) + abs(candidate_pitch) + if cost < best_cost - 1e-9: + best, best_cost = (candidate_yaw, candidate_pitch), cost + return best + + @staticmethod + def _axis(point: np.ndarray, axis: str) -> float: + return float(point[("x", "y", "z").index(axis)]) + + +def _better_pose( + candidate: tuple[float, float, float], + best: tuple[float, float, float], + stack: "PlatformStack", +) -> bool: + """Rank two ``(residual, yaw, pitch)`` candidates. + + Best alignment wins outright. Within ``_UPRIGHT_TOL_DEG`` the two align the + same as far as the machine is concerned, so the one that leaves the sensing + face up is reported instead. + + This tie-break is load-bearing here and not cosmetic. The tool direction is + antiparallel to the sensor's own up axis on this mount, so a normal pointing + straight up can be aligned *either* by leaving the sensor upright with the + residual measured against the opposite direction, *or* by turning it over and + reaching exactly zero. The second is a smaller number and an upside-down + machine, so without this the solver confidently returns the flip. + """ + if candidate[0] < best[0] - _UPRIGHT_TOL_DEG: + return True + if candidate[0] > best[0] + _UPRIGHT_TOL_DEG: + return False + candidate_upright = stack.ends_upright(candidate[1], candidate[2]) + best_upright = stack.ends_upright(best[1], best[2]) + if candidate_upright != best_upright: + return candidate_upright + return candidate[0] < best[0] - 1e-12 + + +def _tool_basis(tool_axis: str) -> np.ndarray: + """A rotation whose third row is ``tool_axis``'s unit vector. + + Lets the closed form be written once, for "align with +Z", whichever platform + axis the tool actually runs along. + """ + tool = np.zeros(3, dtype=np.float64) + tool[("x", "y", "z").index(tool_axis)] = 1.0 + helper = np.array([0.0, 1.0, 0.0]) if abs(tool[1]) < 0.9 else np.array([1.0, 0.0, 0.0]) + first = np.cross(helper, tool) + first /= np.linalg.norm(first) + second = np.cross(tool, first) + return np.vstack([first, second, tool]) + + +def residual_for( + stack: PlatformStack, normal_sensor: np.ndarray, yaw_deg: float, pitch_deg: float +) -> float: + """The misalignment left by a given pose — what the 3D preview colours by. + + Takes the stack so it uses the same frame mapping and approach sign as + :meth:`PlatformStack.solve`; a preview that disagreed with the solver about + the frame would colour a good point as bad. + """ + normal = stack.normal_to_platform(np.asarray(normal_sensor, dtype=np.float64)) + length = max(float(np.linalg.norm(normal)), 1e-12) + unit = normal / length + rotated = ( + rotation_about(stack.pitch_axis, pitch_deg) + @ rotation_about(stack.yaw_axis, yaw_deg) + @ unit + ) + return float( + np.degrees(np.arccos(np.clip(float(rotated @ stack.tool_direction), -1.0, 1.0))) + ) diff --git a/apps/autocal/src/orisys_autocal/preferences.py b/apps/autocal/src/orisys_autocal/preferences.py index a02da39..0019edb 100644 --- a/apps/autocal/src/orisys_autocal/preferences.py +++ b/apps/autocal/src/orisys_autocal/preferences.py @@ -32,6 +32,21 @@ POINT_ORDERS = ("row", "serpentine", "distance") #: selectable for a dim room. DEFAULT_THEME = "light" +#: The sensor-design export the finger debug screen loads by default. The bench +#: confirmed this revision, and it carries the mesh, housing, intrinsics and the +#: pose the unit scale is derived from. +DEFAULT_FINGER_BUNDLE = ( + r"D:\orisys_ws\orisys-sensor-design\build\export\finger\r01\compute" +) + +#: The UV-space artwork for that revision. Resolved from the bundle when it can +#: be, so the two cannot be pointed at different revisions; this preference is +#: the override for a checkout that keeps its textures elsewhere. +DEFAULT_FINGER_TEXTURE = ( + r"D:\orisys_ws\orisys-sensor-design\build\textures\finger\r01" + r"\patch_texture_compute.png" +) + def _default_sequence_profile() -> dict[str, Any]: """The point-generation and run parameters of workflow P5. @@ -157,6 +172,23 @@ def default_preferences() -> dict[str, Any]: "sensor_setup": _default_sensor_setup(), #: P5: grid generation and run parameters. "sequence_profile": _default_sequence_profile(), + # --- finger debug screen --------------------------------------------- + #: The export bundle the debug screen loads. Empty means "ask", and the + #: default below points at the revision the bench confirmed. + "finger_bundle_path": DEFAULT_FINGER_BUNDLE, + #: The UV-space texture for the flat picker. Preferred over the derived + #: path when it exists; empty falls back to the layout map. + "finger_texture_path": DEFAULT_FINGER_TEXTURE, + #: The mounting plate lifts the sensor along the platform's tool axis, so + #: a fixture thickness is one number rather than a transform. + "finger_plate_thickness_mm": 0.0, + #: Tool tip offset from the platform reference, mm. + "finger_tool_offset_mm": [0.0, 0.0, 0.0], + #: Stage travel, degrees. The bench's figures: pitch turns continuously, + #: yaw is limited — and that limit is what decides how much of the patch + #: can be aligned at all. + "finger_yaw_limit_deg": 90.0, + "finger_pitch_limit_deg": 360.0, } diff --git a/apps/autocal/src/orisys_autocal/sensor/__init__.py b/apps/autocal/src/orisys_autocal/sensor/__init__.py new file mode 100644 index 0000000..3b97473 --- /dev/null +++ b/apps/autocal/src/orisys_autocal/sensor/__init__.py @@ -0,0 +1,27 @@ +"""Sensor assets: export bundles, the flat UV view, and the mount frames.""" + +from .export_bundle import BundleError, SensorBundle, load_bundle +from .frames import AXIS_MAP, describe as describe_frames, mount_rotation, to_platform, to_sensor +from .uv_flat import ( + DEFAULT_FLAT_SIZE, + FlatSurface, + load_uv_texture, + rasterise, + texture_path_for, +) + +__all__ = [ + "AXIS_MAP", + "BundleError", + "DEFAULT_FLAT_SIZE", + "FlatSurface", + "SensorBundle", + "describe_frames", + "load_bundle", + "load_uv_texture", + "mount_rotation", + "rasterise", + "texture_path_for", + "to_platform", + "to_sensor", +] diff --git a/apps/autocal/src/orisys_autocal/sensor/export_bundle.py b/apps/autocal/src/orisys_autocal/sensor/export_bundle.py new file mode 100644 index 0000000..6a0bb79 --- /dev/null +++ b/apps/autocal/src/orisys_autocal/sensor/export_bundle.py @@ -0,0 +1,291 @@ +"""Load and validate a sensor-design export bundle. + +The bundle is the design repo's `compute` profile: + + mesh.npz sensing surface, sensor frame + housing.npz housing mesh, sensor frame, with part names + camera_intrinsics.npz K, width, height + world_to_cam.npy camera pose in the sensor frame + +**The mesh is not in millimetres.** ``blender/scripts/export.py`` transforms +vertices by ``t_sensor_from_world @ world``, and ``world_to_cam``'s linear block +carries the ``origin_sensor`` empty's own uniform scale — 0.018388 on +``finger/r01``, i.e. 1/54.4. Physical size is therefore *mesh units × that +factor*, and a consumer that assumes millimetres is wrong by 54×. So the factor +is **derived** here, checked to be a uniform rotation, and every physical number +this package produces comes from it. + +Two refusals matter more than the loading: a non-uniform (or mirrored) pose, and +a mesh whose counts disagree with what the design profile declares. Both would +otherwise produce plausible wrong coordinates. +""" + +from __future__ import annotations + +from dataclasses import dataclass, field +from pathlib import Path + +import numpy as np + +#: Files the bundle must carry. +REQUIRED_FILES: tuple[str, ...] = ( + "mesh.npz", + "housing.npz", + "camera_intrinsics.npz", + "world_to_cam.npy", +) + +#: How uniform the pose's linear block must be, relatively, to count as uniform. +_UNIFORM_TOL = 1e-4 +#: How close to orthonormal the rotation part must be. +_ORTHONORMAL_TOL = 1e-4 + + +class BundleError(ValueError): + """A bundle could not be loaded, with the specific reason named.""" + + +@dataclass +class SensorBundle: + """One sensor revision, in millimetres and in the sensor frame.""" + + path: Path + #: Sensor-frame vertices, already scaled to millimetres. + vertices_mm: np.ndarray + faces: np.ndarray + uv_data: np.ndarray + loop_to_vertex: np.ndarray + housing_vertices_mm: np.ndarray = field(default_factory=lambda: np.zeros((0, 3))) + housing_faces: np.ndarray = field(default_factory=lambda: np.zeros((0, 3), dtype=np.int64)) + housing_part_names: tuple[str, ...] = () + camera_k: np.ndarray = field(default_factory=lambda: np.eye(3)) + camera_size: tuple[int, int] = (0, 0) + world_to_cam: np.ndarray = field(default_factory=lambda: np.eye(4)) + #: Millimetres per mesh unit, derived from the pose's linear block. + mm_per_unit: float = 1.0 + warnings: list[str] = field(default_factory=list) + + # ------------------------------------------------------------------ derived + + @property + def bounds_mm(self) -> tuple[np.ndarray, np.ndarray]: + return self.vertices_mm.min(axis=0), self.vertices_mm.max(axis=0) + + @property + def extent_mm(self) -> np.ndarray: + low, high = self.bounds_mm + return high - low + + @property + def centre_mm(self) -> np.ndarray: + low, high = self.bounds_mm + return (low + high) / 2.0 + + @property + def face_normals(self) -> np.ndarray: + """Unit normal per triangle, from the winding (they are coherent).""" + triangles = self.vertices_mm[self.faces] + cross = np.cross(triangles[:, 1] - triangles[:, 0], triangles[:, 2] - triangles[:, 0]) + return cross / np.maximum(np.linalg.norm(cross, axis=1)[:, None], 1e-12) + + @property + def face_areas_mm2(self) -> np.ndarray: + triangles = self.vertices_mm[self.faces] + return 0.5 * np.linalg.norm( + np.cross(triangles[:, 1] - triangles[:, 0], triangles[:, 2] - triangles[:, 0]), axis=1 + ) + + @property + def surface_area_mm2(self) -> float: + return float(self.face_areas_mm2.sum()) + + def vertex_normals(self) -> np.ndarray: + """Area-weighted vertex normals, unit length.""" + triangles = self.vertices_mm[self.faces] + cross = np.cross(triangles[:, 1] - triangles[:, 0], triangles[:, 2] - triangles[:, 0]) + accumulated = np.zeros_like(self.vertices_mm) + for corner in range(3): + np.add.at(accumulated, self.faces[:, corner], cross) + return accumulated / np.maximum(np.linalg.norm(accumulated, axis=1)[:, None], 1e-12) + + def describe(self) -> dict[str, object]: + """The numbers to show for confirmation before trusting any coordinate. + + Printed by the panel and by the checker: a wrong unit factor is the + failure this whole module exists to make visible. + """ + low, high = self.bounds_mm + return { + "path": str(self.path), + "mm_per_unit": self.mm_per_unit, + "vertices": int(self.vertices_mm.shape[0]), + "triangles": int(self.faces.shape[0]), + "extent_mm": [round(float(value), 2) for value in self.extent_mm], + "min_mm": [round(float(value), 2) for value in low], + "max_mm": [round(float(value), 2) for value in high], + "surface_area_mm2": round(self.surface_area_mm2, 2), + "camera_px": list(self.camera_size), + "warnings": list(self.warnings), + } + + +def load_bundle( + directory: str | Path, + *, + expected_vertices: int | None = None, + expected_triangles: int | None = None, +) -> SensorBundle: + """Load a bundle, deriving the unit scale and validating it. + + Args: + directory: the export's `compute` folder. + expected_vertices: refuse a mesh whose vertex count differs — the design + profile declares these (1032 / 1974 for `finger/r01`) so a stale or + wrong export is caught at load rather than in the platform coordinates. + + Raises: + BundleError: with the specific problem named. + """ + root = Path(directory) + if not root.is_dir(): + raise BundleError(f"{root} is not a directory") + missing = [name for name in REQUIRED_FILES if not (root / name).is_file()] + if missing: + raise BundleError( + f"{root.name} is not a complete export bundle: missing {', '.join(missing)}" + ) + + warnings: list[str] = [] + try: + with np.load(root / "mesh.npz", allow_pickle=False) as mesh: + for key in ("vertices", "faces", "uv_data", "loop_to_vertex"): + if key not in mesh.files: + raise BundleError(f"mesh.npz has no {key!r}") + vertices = np.asarray(mesh["vertices"], dtype=np.float64) + faces = np.asarray(mesh["faces"], dtype=np.int64) + uv_data = np.asarray(mesh["uv_data"], dtype=np.float64) + loop_to_vertex = np.asarray(mesh["loop_to_vertex"], dtype=np.int64) + except BundleError: + raise + except Exception as exc: # noqa: BLE001 - a corrupt npz must read as a bundle error + raise BundleError(f"mesh.npz could not be read: {exc}") from exc + + if vertices.ndim != 2 or vertices.shape[1] != 3: + raise BundleError(f"mesh vertices must be [N,3], got {vertices.shape}") + if faces.ndim != 2 or faces.shape[1] != 3: + raise BundleError(f"mesh faces must be [M,3], got {faces.shape}") + if faces.size and int(faces.max()) >= vertices.shape[0]: + raise BundleError( + f"mesh faces reference vertex {int(faces.max())} of {vertices.shape[0]}" + ) + if uv_data.shape[0] != faces.shape[0] * 3: + raise BundleError( + f"uv_data has {uv_data.shape[0]} corners for {faces.shape[0]} triangles " + f"(expected {faces.shape[0] * 3})" + ) + + if expected_vertices is not None and vertices.shape[0] != expected_vertices: + raise BundleError( + f"mesh has {vertices.shape[0]} vertices, the design profile declares " + f"{expected_vertices}" + ) + if expected_triangles is not None and faces.shape[0] != expected_triangles: + raise BundleError( + f"mesh has {faces.shape[0]} triangles, the design profile declares " + f"{expected_triangles}" + ) + + world_to_cam = _load_pose(root / "world_to_cam.npy") + mm_per_unit = mm_per_unit_from_pose(world_to_cam) + + housing_vertices, housing_faces, part_names = _load_housing(root / "housing.npz", warnings) + camera_k, camera_size = _load_intrinsics(root / "camera_intrinsics.npz") + + return SensorBundle( + path=root, + vertices_mm=vertices * mm_per_unit, + faces=faces, + uv_data=uv_data, + loop_to_vertex=loop_to_vertex, + housing_vertices_mm=housing_vertices * mm_per_unit, + housing_faces=housing_faces, + housing_part_names=part_names, + camera_k=camera_k, + camera_size=camera_size, + world_to_cam=world_to_cam, + mm_per_unit=mm_per_unit, + warnings=warnings, + ) + + +def mm_per_unit_from_pose(world_to_cam: np.ndarray) -> float: + """The millimetres-per-mesh-unit factor, derived from the pose. + + ``world_to_cam`` is the camera pose in the sensor frame, so its linear block + carries the sensor empty's own scale. Reading it here — rather than assuming + a constant — is what keeps a differently-scaled revision correct. + + Raises: + BundleError: when the block is not a uniform rotation. + """ + matrix = np.asarray(world_to_cam, dtype=np.float64) + if matrix.shape != (4, 4): + raise BundleError(f"world_to_cam must be 4x4, got {matrix.shape}") + linear = matrix[:3, :3] + scales = np.linalg.norm(linear, axis=0) + if not np.isfinite(scales).all() or float(scales.min()) <= 0.0: + raise BundleError("world_to_cam has a degenerate linear block") + spread = float(scales.max() - scales.min()) / float(scales.mean()) + if spread > _UNIFORM_TOL: + raise BundleError( + "world_to_cam is not a uniform scale " + f"(column norms {np.round(scales, 6).tolist()}); the mesh cannot be " + "converted to millimetres from it" + ) + scale = float(scales.mean()) + rotation = linear / scale + if not np.allclose(rotation @ rotation.T, np.eye(3), atol=_ORTHONORMAL_TOL): + raise BundleError("world_to_cam's rotation part is not orthonormal") + determinant = float(np.linalg.det(rotation)) + if abs(determinant - 1.0) > _ORTHONORMAL_TOL: + raise BundleError( + f"world_to_cam is mirrored (det = {determinant:.4f}); " + "the sensing surface would be reflected" + ) + return scale + + +def _load_pose(path: Path) -> np.ndarray: + try: + return np.asarray(np.load(path), dtype=np.float64).reshape(4, 4) + except BundleError: + raise + except Exception as exc: # noqa: BLE001 + raise BundleError(f"world_to_cam could not be read: {exc}") from exc + + +def _load_housing(path: Path, warnings: list[str]) -> tuple[np.ndarray, np.ndarray, tuple[str, ...]]: + with np.load(path, allow_pickle=False) as housing: + if "vertices" not in housing.files: + warnings.append("housing has no vertices; the preview will omit it") + return np.zeros((0, 3)), np.zeros((0, 3), dtype=np.int64), () + vertices = np.asarray(housing["vertices"], dtype=np.float64) + faces = ( + np.asarray(housing["faces"], dtype=np.int64) + if "faces" in housing.files + else np.zeros((0, 3), dtype=np.int64) + ) + names = ( + tuple(str(name) for name in housing["part_names"]) + if "part_names" in housing.files + else () + ) + return vertices, faces, names + + +def _load_intrinsics(path: Path) -> tuple[np.ndarray, tuple[int, int]]: + with np.load(path, allow_pickle=False) as intrinsics: + k = np.asarray(intrinsics["K"], dtype=np.float64).reshape(3, 3) + width = int(intrinsics["width"]) if "width" in intrinsics.files else 0 + height = int(intrinsics["height"]) if "height" in intrinsics.files else 0 + return k, (width, height) diff --git a/apps/autocal/src/orisys_autocal/sensor/frames.py b/apps/autocal/src/orisys_autocal/sensor/frames.py new file mode 100644 index 0000000..33e8a15 --- /dev/null +++ b/apps/autocal/src/orisys_autocal/sensor/frames.py @@ -0,0 +1,89 @@ +"""Sensor ↔ platform frames, as named constants. + +One place decides which sensor axis points along which platform axis, because +that mapping is the assumption most likely to be wrong and the cheapest to +correct. It came from the bench: **the sensor's Y is the platform's Z**, so a +patch normal along sensor +Y is a normal along platform +Z when the sensor sits +square on the stage. + +Changing the mapping is a one-line edit to :data:`AXIS_MAP`; everything else +derives from it, and :func:`mount_rotation` refuses a mapping that is not a +rotation (a handedness mistake would otherwise silently mirror the part). +""" + +from __future__ import annotations + +import numpy as np + +#: Which platform axis each sensor axis points along, and in which sign. +#: ``"z": "-y"`` keeps the frame right-handed: sensor (x, y, z) → platform +#: (x, z, -y), whose determinant is +1 (verified by a test). +AXIS_MAP: dict[str, str] = {"x": "x", "y": "z", "z": "-y"} + +#: Which platform axis each stage turns about. The stages are stacked on the XY +#: table, so both are expressed in the *platform* frame. +YAW_AXIS = "z" +PITCH_AXIS = "y" + +#: Platform axes, in the order the pose reports them. +PLATFORM_AXES: tuple[str, ...] = ("x", "y", "z") + +#: The axis the tool approaches along. "Align the normal with Z" is the whole +#: point of the rotation stages. +TOOL_AXIS = "z" + + +def _axis_basis(axis: str) -> np.ndarray: + unit = np.zeros(3, dtype=np.float64) + unit[PLATFORM_AXES.index(axis[1] if axis.startswith("-") else axis)] = -1.0 if axis.startswith("-") else 1.0 + return unit + + +def mount_rotation(axis_map: dict[str, str] | None = None) -> np.ndarray: + """The 3×3 rotation taking a sensor-frame vector to the platform frame. + + Raises: + ValueError: when the mapping is not a rotation, naming the determinant. + A mapping that mirrors the part is worse than no mapping: every point + would be placed on the wrong side and the numbers would look sane. + """ + mapping = dict(axis_map or AXIS_MAP) + matrix = np.zeros((3, 3), dtype=np.float64) + for column, sensor_axis in enumerate(("x", "y", "z")): + target = mapping.get(sensor_axis) + if target is None: + raise ValueError(f"axis map has no entry for sensor {sensor_axis!r}") + matrix[:, column] = _axis_basis(str(target)) + determinant = float(np.linalg.det(matrix)) + if abs(determinant - 1.0) > 1e-9: + raise ValueError( + f"axis map {mapping!r} is not a rotation: det = {determinant:.3f} " + f"(expected +1 — check the signs)" + ) + return matrix + + +def to_platform(vectors_mm: np.ndarray, axis_map: dict[str, str] | None = None) -> np.ndarray: + """Rotate a sensor-frame vector (or an ``[N,3]`` block) into the platform frame.""" + array = np.asarray(vectors_mm, dtype=np.float64) + matrix = mount_rotation(axis_map) + if array.ndim == 1: + return matrix @ array + return array @ matrix.T + + +def to_sensor(vectors_mm: np.ndarray, axis_map: dict[str, str] | None = None) -> np.ndarray: + """The inverse of :func:`to_platform`.""" + array = np.asarray(vectors_mm, dtype=np.float64) + matrix = mount_rotation(axis_map) + if array.ndim == 1: + return matrix.T @ array + return array @ matrix + + +def describe(axis_map: dict[str, str] | None = None) -> str: + """``sensor x→platform x, y→z, z→-y`` — shown in the UI so a flip is visible.""" + mapping = dict(axis_map or AXIS_MAP) + return "sensor " + ", ".join( + f"{axis}->{mapping.get(axis, '?')}" for axis in ("x", "y", "z") + ) diff --git a/apps/autocal/src/orisys_autocal/sensor/uv_flat.py b/apps/autocal/src/orisys_autocal/sensor/uv_flat.py new file mode 100644 index 0000000..7f7be30 --- /dev/null +++ b/apps/autocal/src/orisys_autocal/sensor/uv_flat.py @@ -0,0 +1,384 @@ +"""Rasterise the sensing surface into a flat (UV-space) image and a +pixel → 3D lookup table. + +Why rasterise rather than use an unrolled map: the `compute` export carries +``uv_data`` but no flat image, so the only flat view available is the mesh's own +UV layout. Rasterising it *and* the positions in the same pass is what makes a +click well-posed — the pixel that is coloured is the pixel whose 3D position was +recorded, because both come from one triangle test. + +The image is a **layout map shaded by the mesh** unless a texture is supplied. A +texture is authored in UV coordinates — black outside the island, coloured inside +— so it resamples straight into the same space and the click map is unaffected; +``build/textures///patch_texture_compute.png`` is that file. +Without one the layout map still exists to be clicked on, showing shape (a light +direction) and triangle structure rather than photorealism. +""" + +from __future__ import annotations + +from dataclasses import dataclass +from pathlib import Path + +import numpy as np + +from .export_bundle import SensorBundle + +#: Default flat image size. Matches the design repo's ``visualization`` profile +#: (1024×1024) so the two views are comparable. +DEFAULT_FLAT_SIZE = (1024, 1024) + +#: Light direction in the sensor frame, for the layout map's shading. +_LIGHT = np.array([0.35, 0.80, 0.49], dtype=np.float64) +_LIGHT /= np.linalg.norm(_LIGHT) + +#: Rasteriser batching: pixel rows per pass, and triangles per vectorised batch. +#: Sized so one batch's working arrays stay a few megabytes at 1024². +BAND_ROWS = 64 +BATCH_TRIANGLES = 16 + + +@dataclass +class FlatSurface: + """The flat view and its pixel → 3D table. + + ``positions_mm[y, x]`` and ``normals[y, x]`` are valid exactly where + ``face_index[y, x] >= 0``. V is flipped when sampled: image row 0 is UV + ``v = 1`` (top), which is how the atlas is authored. + """ + + size_hw: tuple[int, int] + image_rgb: np.ndarray # [H, W, 3] uint8 + positions_mm: np.ndarray # [H, W, 3] float64 + normals: np.ndarray # [H, W, 3] float64 + face_index: np.ndarray # [H, W] int32, -1 where nothing is covered + uv_min: tuple[float, float] + uv_max: tuple[float, float] + #: Whether ``image_rgb`` is the supplied texture rather than the layout map. + #: The view reports which of the two it is showing, so a fallback is visible + #: instead of silently looking like a different patch. + textured: bool = False + + @property + def height(self) -> int: + return int(self.size_hw[0]) + + @property + def width(self) -> int: + return int(self.size_hw[1]) + + @property + def valid_fraction(self) -> float: + return float((self.face_index >= 0).mean()) + + def sample(self, x: int, y: int) -> tuple[np.ndarray, np.ndarray, int] | None: + """The 3D point, unit normal and face at a pixel, or ``None`` if unused. + + ``None`` is the honest answer for a UV island gap: inventing (0, 0, 0) + would place a calibration point at the sensor's origin and look like a + measurement. + """ + if not (0 <= x < self.width and 0 <= y < self.height): + return None + face = int(self.face_index[y, x]) + if face < 0: + return None + return self.positions_mm[y, x].copy(), self.normals[y, x].copy(), face + + def pixel_for_uv(self, u: float, v: float) -> tuple[int, int]: + """The pixel a UV coordinate lands on (used by the round-trip test).""" + u0, v0 = self.uv_min + u1, v1 = self.uv_max + span_u = max(u1 - u0, 1e-9) + span_v = max(v1 - v0, 1e-9) + x = int(round((u - u0) / span_u * (self.width - 1))) + y = int(round((1.0 - (v - v0) / span_v) * (self.height - 1))) + return max(0, min(self.width - 1, x)), max(0, min(self.height - 1, y)) + + +def load_uv_texture(path: str | Path) -> np.ndarray | None: + """Decode a UV-space texture to ``[H, W, 4]`` uint8, or ``None``. + + Decoded with Qt rather than a new image dependency, and copied out of Qt's + buffer: a numpy view onto ``QImage`` memory outlives nothing, and sampling it + after the image goes out of scope is a use-after-free that looks like noise. + + Returning ``None`` rather than raising is deliberate. The texture is an + enhancement over the layout map, and a missing or unreadable one must leave + the picker working. + """ + try: + from PySide6.QtGui import QImage + except ImportError: # pragma: no cover - Qt is a hard dependency of the app + return None + + try: + image = QImage(str(path)) + if image.isNull(): + return None + converted = image.convertToFormat(QImage.Format.Format_RGBA8888) + width, height = converted.width(), converted.height() + if width <= 0 or height <= 0: + return None + buffer = converted.constBits() + row_bytes = converted.bytesPerLine() + array = np.frombuffer(buffer, dtype=np.uint8, count=row_bytes * height) + return array.reshape(height, row_bytes)[:, : width * 4].reshape(height, width, 4).copy() + except Exception: # noqa: BLE001 - a texture must never break the picker + return None + + +def texture_path_for(bundle: SensorBundle) -> Path | None: + """The texture that belongs to ``bundle``, if the design repo exported one. + + The bundle lives at ``/export///`` and the + texture at ``/textures///patch_texture_.png``, + so the two are resolved from the bundle rather than configured separately: + a mismatched pair would resample the wrong artwork onto the pickable pixels. + """ + profile = bundle.path.name + revision = bundle.path.parent + sensor = revision.parent + build_root = sensor.parent.parent + candidate = ( + build_root / "textures" / sensor.name / revision.name + / f"patch_texture_{profile}.png" + ) + return candidate if candidate.is_file() else None + + +def rasterise( + bundle: SensorBundle, + size_hw: tuple[int, int] = DEFAULT_FLAT_SIZE, + *, + texture: np.ndarray | None = None, +) -> FlatSurface: + """Rasterise ``bundle`` into a flat image plus its pixel → 3D table. + + Batched over triangles: the obvious per-triangle loop is correct but spends + its time in Python allocating a small grid per face (~4 s at 512², ~15 s at + the 1024² design size), and this screen loads on demand. + + The triangle grid is built once up front, so the shared coordinates are + compared with a tolerance instead of an exact match. That is deliberate: two + triangles at a shared edge produce the same 3D point either way, and an exact + match would drop the seams into the "unused pixel" set. + + With ``texture`` the image is that texture resampled at each covered pixel's + own UV, so it lines up with the click map by construction. The lookup tables + are identical either way. + """ + height, width = int(size_hw[0]), int(size_hw[1]) + if height <= 0 or width <= 0: + raise ValueError(f"flat size must be positive, got {size_hw}") + + uv = bundle.uv_data.reshape(-1, 3, 2).astype(np.float64) + uv_min = (float(uv[..., 0].min()), float(uv[..., 1].min())) + uv_max = (float(uv[..., 0].max()), float(uv[..., 1].max())) + span_u = max(uv_max[0] - uv_min[0], 1e-12) + span_v = max(uv_max[1] - uv_min[1], 1e-12) + + # UV -> pixel: u left-to-right, v bottom-to-top, so the row axis is flipped. + pixels = np.empty_like(uv) + pixels[..., 0] = (uv[..., 0] - uv_min[0]) / span_u * (width - 1) + pixels[..., 1] = (1.0 - (uv[..., 1] - uv_min[1]) / span_v) * (height - 1) + + positions = np.zeros((height, width, 3), dtype=np.float64) + normals = np.zeros((height, width, 3), dtype=np.float64) + face_index = np.full((height, width), -1, dtype=np.int32) + shaded = np.zeros((height, width), dtype=np.float64) + checker = np.zeros((height, width), dtype=bool) + + face_normals = bundle.face_normals + face_points = bundle.vertices_mm[bundle.faces] + + column_low = np.clip(np.floor(pixels[..., 0].min(axis=1)), 0, width - 1).astype(np.int64) + column_high = np.clip(np.ceil(pixels[..., 0].max(axis=1)), 0, width - 1).astype(np.int64) + + for band_start in range(0, height, BAND_ROWS): + band_stop = min(height, band_start + BAND_ROWS) + members = np.nonzero( + (column_high > column_low) & (column_low < width) & (column_high >= 0) + )[0] + if members.size == 0: + continue + for chunk_start in range(0, members.size, BATCH_TRIANGLES): + batch = members[chunk_start : chunk_start + BATCH_TRIANGLES] + band_rows = _rows_in_band(pixels[batch], band_start, band_stop, height) + if band_rows is None: + continue + _fill_band( + batch, + pixels, + band_rows, + positions, + normals, + face_index, + shaded, + checker, + face_normals, + face_points, + width, + ) + + if texture is not None: + image = _sample_texture(texture, face_index, uv_min, uv_max) + textured = True + else: + image = _to_rgb(shaded, checker, face_index) + textured = False + return FlatSurface( + size_hw=(height, width), + image_rgb=image, + positions_mm=positions, + normals=normals, + face_index=face_index, + uv_min=uv_min, + uv_max=uv_max, + textured=textured, + ) + + +def _sample_texture( + texture: np.ndarray, + face_index: np.ndarray, + uv_min: tuple[float, float], + uv_max: tuple[float, float], +) -> np.ndarray: + """Resample a UV-space texture onto the flat image's pixels. + + Nearest neighbour, on purpose: the flat image exists to be clicked on, and the + pixel that is coloured is the pixel whose 3D position was recorded, so + smoothing the artwork would blur exactly the correspondence the picker relies + on. Black is left outside the island. + + The mapping is the inverse of the UV→pixel transform in :func:`rasterise`, + written here from the pixel indices rather than by re-walking the mesh, so it + holds for every covered pixel including the ones covered by a seam. + """ + height, width = face_index.shape + u0, v0 = uv_min + u1, v1 = uv_max + span_u = max(u1 - u0, 1e-12) + span_v = max(v1 - v0, 1e-12) + + tex_height, tex_width = int(texture.shape[0]), int(texture.shape[1]) + columns = np.arange(width, dtype=np.float64)[None, :] + rows = np.arange(height, dtype=np.float64)[:, None] + + u = u0 + columns / max(width - 1, 1) * span_u + v = v1 - rows / max(height - 1, 1) * span_v + + texture_x = np.clip(np.rint(u * (tex_width - 1)).astype(np.int64), 0, tex_width - 1) + texture_y = np.clip(np.rint((1.0 - v) * (tex_height - 1)).astype(np.int64), 0, tex_height - 1) + texture_x = np.broadcast_to(texture_x, (height, width)) + texture_y = np.broadcast_to(texture_y, (height, width)) + + image = texture[texture_y, texture_x, :3].astype(np.uint8) + image[face_index < 0] = (0, 0, 0) + return image + + +def _rows_in_band( + batch_pixels: np.ndarray, band_start: int, band_stop: int, height: int +) -> tuple[int, int] | None: + """The pixel rows of this band that the batch's triangles actually touch.""" + low = np.floor(batch_pixels[..., 1].min(axis=1)).astype(np.int64) + high = np.ceil(batch_pixels[..., 1].max(axis=1)).astype(np.int64) + top = max(band_start, int(low.min())) + bottom = min(band_stop - 1, int(high.max()), height - 1) + if bottom < top: + return None + return top, bottom + + +def _fill_band( + batch: np.ndarray, + pixels: np.ndarray, + band_rows: tuple[int, int], + positions: np.ndarray, + normals: np.ndarray, + face_index: np.ndarray, + shaded: np.ndarray, + checker: np.ndarray, + face_normals: np.ndarray, + face_points: np.ndarray, + width: int, +) -> None: + """Fill one batch's pixels inside one row band.""" + top, bottom = band_rows + tri = pixels[batch] # [B, 3, 2] + left = max(0, int(np.floor(tri[..., 0].min()))) + right = min(width - 1, int(np.ceil(tri[..., 0].max()))) + if right < left: + return + + grid_x = np.arange(left, right + 1, dtype=np.float64)[None, None, :] + grid_y = np.arange(top, bottom + 1, dtype=np.float64)[None, :, None] + + a = tri[:, 0][:, None, None, :] + b = tri[:, 1][:, None, None, :] + c = tri[:, 2][:, None, None, :] + denominator = (b[..., 1] - c[..., 1]) * (a[..., 0] - c[..., 0]) + ( + c[..., 0] - b[..., 0] + ) * (a[..., 1] - c[..., 1]) + usable = np.abs(denominator) > 1e-12 + safe = np.where(usable, denominator, 1.0) + wa = ((b[..., 1] - c[..., 1]) * (grid_x - c[..., 0]) + (c[..., 0] - b[..., 0]) * (grid_y - c[..., 1])) / safe + wb = ((c[..., 1] - a[..., 1]) * (grid_x - c[..., 0]) + (a[..., 0] - c[..., 0]) * (grid_y - c[..., 1])) / safe + wc = 1.0 - wa - wb + inside = (wa >= -1e-6) & (wb >= -1e-6) & (wc >= -1e-6) & usable + + # The covering triangle with the largest smallest weight wins. At a shared + # edge every cover gives the same point, so the tie-break only matters for + # keeping the table single-valued. + margin = np.where(inside, np.minimum(np.minimum(wa, wb), wc), -np.inf) + covered = np.isfinite(margin).any(axis=0) + if not covered.any(): + return + winner = np.argmax(margin, axis=0) + rows, columns = np.nonzero(covered) + chosen = winner[rows, columns] + weights = np.stack( + [wa[chosen, rows, columns], wb[chosen, rows, columns], wc[chosen, rows, columns]], + axis=1, + ) + faces = batch[chosen] + target_rows = top + rows + target_columns = left + columns + + free = face_index[target_rows, target_columns] < 0 + if not free.any(): + return + target_rows = target_rows[free] + target_columns = target_columns[free] + weights = weights[free] + faces = faces[free] + + positions[target_rows, target_columns] = np.einsum( + "pk,pkj->pj", weights, face_points[faces] + ) + normals[target_rows, target_columns] = face_normals[faces] + face_index[target_rows, target_columns] = faces + shaded[target_rows, target_columns] = face_normals[faces] @ _LIGHT + checker[target_rows, target_columns] = (faces // 2) % 2 == 1 + + +def _to_rgb(shaded: np.ndarray, checker: np.ndarray, face_index: np.ndarray) -> np.ndarray: + """Shade the layout map so shape and triangle structure are both visible.""" + # Lambert in [-1, 1] -> [0.35, 1.0], so a face pointing away is dim but never + # black (a black region would look like an unused pixel). + brightness = 0.35 + 0.65 * np.clip((shaded + 1.0) / 2.0, 0.0, 1.0) + base = np.where(checker, 0.72, 1.0) + value = np.clip(brightness * base, 0.0, 1.0) + tinted = np.stack( + [ + value * 0.62, # a warm-neutral surface, distinct from the UI greys + value * 0.80, + value * 0.90, + ], + axis=-1, + ) + image = (tinted * 255.0).astype(np.uint8) + image[face_index < 0] = (0, 0, 0) + return image diff --git a/apps/autocal/src/orisys_autocal/ui/pose_preview.py b/apps/autocal/src/orisys_autocal/ui/pose_preview.py new file mode 100644 index 0000000..de8ee88 --- /dev/null +++ b/apps/autocal/src/orisys_autocal/ui/pose_preview.py @@ -0,0 +1,809 @@ +"""3D preview of the sensor, the picked point and the tool axis. + +The point of this widget is one question: **does the surface normal line up with +the tool axis, and where does it not?** So it draws the sensing surface coloured +by the residual the kinematic solver achieves at each vertex — the reachable +region is visible at design time rather than discovered mid-run — with the picked +point, its normal, and the tool axis over it. + +Deliberately not the studio's 3D pane: that is a PyQt5 component with an 881-line +binding-specific GL backend, and porting it here would be a project of its own. +This draws immediate-mode with PyOpenGL, which the target environment has +(OpenGL 3.1.10) and which is entirely adequate for a few thousand triangles. + +Failure is expected and handled: a headless check, a machine without a GL stack, +or a driver that refuses the context all leave the numeric readouts working and +the widget reporting why it has no picture. +""" + +from __future__ import annotations + +import math + +import numpy as np +from PySide6.QtCore import QPoint, Qt, QTimer, Signal +from PySide6.QtGui import QColor, QFontMetrics, QPainter, QPen +from PySide6.QtWidgets import QSizePolicy, QWidget + +try: # PySide6 keeps the GL widget in its own module + from PySide6.QtOpenGLWidgets import QOpenGLWidget +except ImportError: # pragma: no cover - older layouts + from PySide6.QtWidgets import QOpenGLWidget # type: ignore[no-redef] + +from orisys_app_shared.i18n import t + +from ..motion.kinematics import PlatformStack, Pose, rotation_about +from ..sensor.export_bundle import SensorBundle +from ..sensor.frames import mount_rotation, to_platform, to_sensor + +#: Orbit drag sensitivity and zoom step, matching the design repo's preview. +ORBIT_RAD_PER_PX = 0.008 +ZOOM_PER_STEP = 0.12 +MAX_PITCH_RAD = math.radians(89.0) +DEFAULT_FOV_Y_RAD = 0.6 + +#: The initial configuration, and the one "Initial view" returns to. Yaw is held +#: at zero so the camera looks straight down the sensor's own −Y, which puts the +#: sensing face toward the viewer. The pitch is **negative** — the eye sits below +#: the patch's centre — and that is what makes sensor +Y (the platform's tool +#: axis on this mount) project up the screen: with the opposite sign the tool axis +#: is drawn pointing down the picture, which is the one thing this view must not +#: get wrong. Verified by projecting the platform axes, not by eye; see +#: ``test_the_initial_view_is_the_commissioned_orientation``. +INITIAL_YAW_RAD = 0.0 +INITIAL_PITCH_RAD = math.radians(-25.0) +INITIAL_DISTANCE_SCALE = 1.0 + +#: Kept as names because the preview has always used them and they are the same +#: state; one of the two spellings had to go, not both. +DEFAULT_PITCH_RAD = INITIAL_PITCH_RAD +DEFAULT_YAW_RAD = INITIAL_YAW_RAD + +#: Axis triad colours, in the platform frame: X, Y, Z. Distinct from the tool +#: axis (yellow) and the picked normal (cyan) so the legend can name them. +AXIS_COLOURS = { + "x": (0.90, 0.33, 0.33), + "y": (0.38, 0.80, 0.38), + "z": (0.36, 0.55, 0.95), +} + +#: Motion simulation. Short enough to feel like a machine demonstration rather +#: than an animation: the rotation stage settles first, then the table moves. +SIM_FRAME_MS = 16 +SIM_DURATION_MS = 2600 +#: Share of the run spent on the stages before the XY table starts. The bench +#: contract is "rotate first, then move", so the two phases do not overlap. +SIM_ROTATION_SHARE = 0.55 + +#: Residual (degrees) at which the alignment colour saturates to red. +BAD_RESIDUAL_DEG = 20.0 + +#: Neutral colour for a vertex whose residual could not be computed. +_UNAVAILABLE = (0.45, 0.45, 0.50) + + +def residual_colour(residual_deg: float) -> tuple[float, float, float]: + """Green when aligned, red as the residual grows, for shading and tests.""" + if not math.isfinite(residual_deg): + return _UNAVAILABLE + share = max(0.0, min(1.0, float(residual_deg) / BAD_RESIDUAL_DEG)) + return (0.15 + 0.75 * share, 0.72 - 0.55 * share, 0.28 - 0.18 * share) + + +def _axis_matrix(axis: str, degrees: float) -> np.ndarray: + """``rotation_about``, kept behind one name so the sim and the solve agree.""" + return rotation_about(axis, float(degrees)) + + +class PosePreview(QOpenGLWidget): + """Orbit-able preview of the sensing surface and the tool axis.""" + + #: Emitted while the motion simulation starts and stops, so the button that + #: drives it can say which of the two it currently offers. + simulationChanged = Signal(bool) + + def __init__(self, parent: QWidget | None = None) -> None: + super().__init__(parent) + self.setObjectName("posePreview") + # Generous minimums: this is the pane an operator reads the machine's + # orientation from, and a 120-pixel box was too small to see anything in. + # The row gives it the largest share, so this is a floor, not a target. + self.setMinimumSize(300, 240) + self.setSizePolicy(QSizePolicy.Policy.Expanding, QSizePolicy.Policy.Expanding) + + self._bundle: SensorBundle | None = None + self._stack = PlatformStack() + self._residuals: np.ndarray | None = None + self._point_mm: np.ndarray | None = None + self._normal_mm: np.ndarray | None = None + self._tool_axis = np.array([0.0, 0.0, 1.0]) + self._problem = "" + self._gl_ok = True + #: Draw the motion axes: the platform frame the mesh is mounted in, the + #: tool axis, and which way each stage turns. On by default because the + #: whole point of the preview is judging whether the mount is right. + self._show_axes = True + #: Sensor-frame tips of the drawn axes, for the 2D labels to sit on. + self._axis_tips: dict[str, np.ndarray] = {} + + self._yaw = INITIAL_YAW_RAD + self._pitch = INITIAL_PITCH_RAD + self._distance_scale = INITIAL_DISTANCE_SCALE + self._last_pos = QPoint() + + # --- motion simulation + #: The solved pose being demonstrated, or ``None`` before a pick. + self._pose: Pose | None = None + self._sim_elapsed = 0.0 + self._sim_timer: QTimer | None = None + self._sim_running = False + + # -------------------------------------------------------------------- state + + @property + def show_axes(self) -> bool: + return self._show_axes + + def set_show_axes(self, show: bool) -> None: + self._show_axes = bool(show) + self.update() + + def initial_view(self) -> None: + """Return to the commissioned orientation, angles included.""" + self._yaw = INITIAL_YAW_RAD + self._pitch = INITIAL_PITCH_RAD + self._distance_scale = INITIAL_DISTANCE_SCALE + self.update() + + @property + def problem(self) -> str: + """Why there is no picture, or ``""``. Reported in the panel's status line.""" + return self._problem + + def set_bundle(self, bundle: SensorBundle | None) -> None: + self._bundle = bundle + self._residuals = None + self._problem = "" + if bundle is not None: + try: + self._residuals = self._solve_all(bundle) + except Exception as exc: # noqa: BLE001 - a preview must never break the panel + self._residuals = None + self._problem = t("finger.preview_solve_failed", error=str(exc)) + # Established here rather than only during a paint, because a widget that + # is never painted — offscreen, or on a tab nobody opened — must still be + # able to say why it has no picture. + self._note_missing_context() + self.update() + + def set_stack(self, stack: PlatformStack) -> None: + self._stack = stack + if self._bundle is not None: + self.set_bundle(self._bundle) + + def set_point(self, point_mm: np.ndarray | None, normal_mm: np.ndarray | None) -> None: + self._point_mm = None if point_mm is None else np.asarray(point_mm, dtype=np.float64) + self._normal_mm = None if normal_mm is None else np.asarray(normal_mm, dtype=np.float64) + self.update() + + # ---------------------------------------------------------- motion simulation + + def set_pose(self, pose: Pose | None) -> None: + """The solved pose the simulation will play, or ``None`` to disable it. + + The preview does not solve anything itself: the panel already has the pose + it showed the operator, and animating a second, independently computed one + would be a chance for the picture and the numbers to disagree. + """ + self.stop_simulation() + self._pose = pose + self.simulationChanged.emit(False) + + @property + def can_simulate(self) -> bool: + return self._pose is not None and self._pose.ok + + @property + def simulating(self) -> bool: + return self._sim_running + + def start_simulation(self) -> None: + """Play the solved pose: stages first, then the table. + + Played as the *inverse* of the machine's motion, because the camera is + fixed on the tool. The mesh therefore starts where the sensor was picked + and ends with its contact normal on the tool axis, which is exactly the + geometric statement the residual reports — the operator watches the two + line up instead of taking the number on trust. + """ + if not self.can_simulate: + return + if self._sim_timer is None: + # Parented to this widget and created on the GUI thread, so it dies + # with the widget rather than outliving it. + self._sim_timer = QTimer(self) + self._sim_timer.setInterval(SIM_FRAME_MS) + self._sim_timer.timeout.connect(self._sim_tick) + self._sim_elapsed = 0.0 + self._sim_running = True + self._sim_timer.start() + self.simulationChanged.emit(True) + self.update() + + def stop_simulation(self) -> None: + if self._sim_timer is not None: + self._sim_timer.stop() + if self._sim_running: + self._sim_running = False + self.simulationChanged.emit(False) + self._sim_elapsed = 0.0 + self.update() + + def _sim_tick(self) -> None: + self._sim_elapsed += SIM_FRAME_MS + if self._sim_elapsed >= SIM_DURATION_MS: + self._sim_elapsed = SIM_DURATION_MS + if self._sim_timer is not None: + self._sim_timer.stop() + self._sim_running = False + self.simulationChanged.emit(False) + self.update() + + def simulation_phases(self) -> tuple[float, float]: + """``(rotation_share, translation_share)`` done, each in 0..1. + + Separate on purpose: the bench contract is that the stages settle before + the table moves, so the two must not overlap on screen either. + """ + if not self.can_simulate: + return 0.0, 0.0 + if not self._sim_running and self._sim_elapsed <= 0.0: + return 0.0, 0.0 + progress = min(1.0, self._sim_elapsed / SIM_DURATION_MS) + if progress <= SIM_ROTATION_SHARE: + return progress / SIM_ROTATION_SHARE, 0.0 + return 1.0, (progress - SIM_ROTATION_SHARE) / (1.0 - SIM_ROTATION_SHARE) + + def _sim_rotation(self, yaw_deg: float, pitch_deg: float) -> np.ndarray: + """The stage rotation in the mesh's frame. + + Delegated to the stack, which owns the one definition of it. The preview + previously composed its own, and a second copy of a frame convention is + exactly what produced two wrong orientations in opposite directions. + """ + return self._stack.stage_rotation(yaw_deg, pitch_deg) + + def _sim_transform(self) -> tuple[np.ndarray, np.ndarray] | None: + """``(rotation, translation)`` to apply to a sensor-frame point. + + At the end of playback the transform puts the contact point on the tool + axis at the origin and turns the picked normal onto that axis, which are + the two things the residual and the reported X/Y actually claim. + """ + if not self.can_simulate or self._pose is None: + return None + rotation_share, translation_share = self.simulation_phases() + if rotation_share <= 0.0 and translation_share <= 0.0: + return None + + # The rotation is interpolated by angle, always built the same way, so the + # shape of the motion cannot depend on the frame bookkeeping below. + rotation = self._sim_rotation( + self._pose.yaw_deg * rotation_share, self._pose.pitch_deg * rotation_share + ) + + point = self._point_mm + if point is None: + return rotation, np.zeros(3) + contact = np.asarray(point, dtype=np.float64) + + # Bring the contact *to the tool*, not to its absolute platform + # coordinates. Moving it to the reported X/Y/Z would be the same motion but + # expressed in machine coordinates, and the tool here is drawn at the + # patch's centre — so the model would translate clean off the screen and + # leave an empty view. Anchoring on the tool keeps the demonstration in + # frame and still shows the motion's true size and direction. + anchor = self._bundle.centre_mm if self._bundle is not None else np.zeros(3) + final = rotation @ contact + displacement = np.asarray(anchor, dtype=np.float64) - final + return rotation, displacement * translation_share + + def _solve_all(self, bundle: SensorBundle) -> np.ndarray: + """The residual the stages achieve at each vertex — the colouring.""" + normals = bundle.vertex_normals() + residuals = np.empty(bundle.vertices_mm.shape[0], dtype=np.float64) + for index, (point, normal) in enumerate(zip(bundle.vertices_mm, normals)): + residuals[index] = self._stack.solve(point, normal).residual_deg + return residuals + + def residual_summary(self) -> dict[str, float]: + """How much of the patch the stages can actually align.""" + if self._residuals is None or self._residuals.size == 0: + return {} + residuals = self._residuals + return { + "vertices": float(residuals.size), + "aligned": float((residuals <= 0.5).sum()), + "aligned_fraction": float((residuals <= 0.5).mean()), + "worst_deg": float(np.nanmax(residuals)), + "mean_deg": float(np.nanmean(residuals)), + } + + # ------------------------------------------------------------------ painting + + def initializeGL(self) -> None: # pragma: no cover - needs a real context + try: + from OpenGL import GL # noqa: F401 + except Exception as exc: # noqa: BLE001 + self._gl_ok = False + self._problem = t("finger.preview_no_gl", error=str(exc)) + + def paintGL(self) -> None: # pragma: no cover - needs a real context + if not self._gl_ok or self._bundle is None: + return + try: + self._draw() + except Exception as exc: # noqa: BLE001 + self._gl_ok = False + self._problem = t("finger.preview_draw_failed", error=str(exc)) + + def has_context(self) -> bool: + """Whether a GL context exists at all, for the panel's status line.""" + try: + return self.context() is not None + except Exception: # noqa: BLE001 + return False + + def _focus(self) -> np.ndarray: + """The point the camera looks at: the model, wherever the motion put it. + + Following the model matters more than it sounds. Before playback the sensor + sits at its exported place; during it the stages swing the body and the + table slides it, and a camera fixed on the original centre ends up staring + at empty space with the sensor off the edge of the view. + """ + assert self._bundle is not None + return self._placed(self._bundle.centre_mm) + + def _camera(self) -> tuple[np.ndarray, float]: + """Eye position and distance, from the orbit angles and the mesh size.""" + assert self._bundle is not None + centre = self._focus() + radius = float(np.linalg.norm(self._bundle.extent_mm)) / 2.0 or 1.0 + distance = radius * 3.0 * self._distance_scale + cos_pitch = math.cos(self._pitch) + eye = centre + distance * np.array( + [ + cos_pitch * math.sin(self._yaw), + cos_pitch * math.cos(self._yaw), + math.sin(self._pitch), + ] + ) + return eye, distance + + def _view_basis(self, eye: np.ndarray) -> tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]: + """``(centre, forward, right, up)`` — the same basis ``_look_at`` builds. + + Shared so a 2D label drawn over the GL scene lands on the 3D point it + names. Two copies of this would drift the moment either changed. + """ + centre = self._focus() + forward = centre - eye + forward = forward / max(float(np.linalg.norm(forward)), 1e-12) + up_world = np.array([0.0, 0.0, 1.0]) if abs(forward[2]) < 0.9 else np.array([0.0, 1.0, 0.0]) + right = np.cross(forward, up_world) + right = right / max(float(np.linalg.norm(right)), 1e-12) + up = np.cross(right, forward) + return centre, forward, right, up + + def project(self, point_mm: np.ndarray) -> tuple[float, float] | None: + """Where a sensor-frame point lands in widget pixels, or ``None`` if behind. + + The GL scene is drawn with ``glFrustum``, so this repeats that projection + rather than approximating it: with the near plane and ``FOV_Y`` unchanged, a + label drawn at the result sits on its arrow tip to within a pixel or two. + """ + if self._bundle is None: + return None + eye, _distance = self._camera() + centre, forward, right, up = self._view_basis(eye) + + relative = np.asarray(point_mm, dtype=np.float64) - eye + depth = float(relative @ forward) + if depth <= 1e-9: + return None + width = max(1, self.width()) + height = max(1, self.height()) + near = max(float(np.linalg.norm(centre - eye)) * 0.01, 0.01) + top = near * math.tan(DEFAULT_FOV_Y_RAD / 2.0) + right_extent = top * width / height + if top <= 0 or right_extent <= 0: + return None + + normalised_x = float(relative @ right) / (depth * right_extent) + normalised_y = float(relative @ up) / (depth * top) + screen_x = (normalised_x + 1.0) / 2.0 * width + # GL's origin is bottom-left; widget pixels are top-left. + screen_y = (1.0 - normalised_y) / 2.0 * height + return screen_x, screen_y + + def _draw(self) -> None: + from OpenGL import GL + + assert self._bundle is not None + eye, distance = self._camera() + centre = self._bundle.centre_mm + + GL.glClearColor(0.09, 0.10, 0.12, 1.0) + GL.glClear(GL.GL_COLOR_BUFFER_BIT | GL.GL_DEPTH_BUFFER_BIT) + GL.glEnable(GL.GL_DEPTH_TEST) + + width = max(1, self.width()) + height = max(1, self.height()) + GL.glViewport(0, 0, width, height) + GL.glMatrixMode(GL.GL_PROJECTION) + GL.glLoadIdentity() + near = max(distance * 0.01, 0.01) + far = distance * 20.0 + top = near * math.tan(DEFAULT_FOV_Y_RAD / 2.0) + right = top * width / height + GL.glFrustum(-right, right, -top, top, near, far) + + GL.glMatrixMode(GL.GL_MODELVIEW) + GL.glLoadIdentity() + _look_at(eye, centre) + + self._draw_surface(GL) + self._draw_housing(GL) + self._draw_tool(GL, distance) + if self._show_axes: + self._draw_axes(GL) + if self._sim_transform() is not None: + self._draw_origin(GL) + self._draw_pick(GL, distance) + + def _placed(self, point: np.ndarray) -> np.ndarray: + """A sensor-frame point as the simulation currently places it.""" + transform = self._sim_transform() + if transform is None: + return np.asarray(point, dtype=np.float64) + rotation, translation = transform + return rotation @ np.asarray(point, dtype=np.float64) + translation + + def _placed_direction(self, direction: np.ndarray) -> np.ndarray: + """A sensor-frame direction under the simulation's rotation only.""" + transform = self._sim_transform() + if transform is None: + return np.asarray(direction, dtype=np.float64) + return transform[0] @ np.asarray(direction, dtype=np.float64) + + def _draw_surface(self, GL) -> None: # pragma: no cover - needs a context + assert self._bundle is not None + vertices = self._bundle.vertices_mm + faces = self._bundle.faces + if self._sim_transform() is None: + colours = ( + np.array([residual_colour(value) for value in self._residuals]) + if self._residuals is not None + else np.tile(np.array(_UNAVAILABLE), (vertices.shape[0], 1)) + ) + else: + # While the machine is moving, the residual shading is a distraction; + # flat grey keeps the eye on where the normal is going. + colours = np.tile(np.array((0.55, 0.58, 0.64)), (vertices.shape[0], 1)) + + # Flat shading is enough here: the question is which regions are + # reachable, not what the surface looks like. + GL.glBegin(GL.GL_TRIANGLES) + for face in faces: + for corner in face: + colour = colours[corner] + GL.glColor3f(float(colour[0]), float(colour[1]), float(colour[2])) + point = self._placed(vertices[corner]) + GL.glVertex3f(float(point[0]), float(point[1]), float(point[2])) + GL.glEnd() + + def _draw_housing(self, GL) -> None: # pragma: no cover - needs a context + assert self._bundle is not None + if self._bundle.housing_faces.size == 0: + return + GL.glColor3f(0.35, 0.38, 0.45) + GL.glPolygonMode(GL.GL_FRONT_AND_BACK, GL.GL_LINE) + GL.glBegin(GL.GL_TRIANGLES) + for face in self._bundle.housing_faces: + for corner in face: + point = self._placed(self._bundle.housing_vertices_mm[corner]) + GL.glVertex3f(float(point[0]), float(point[1]), float(point[2])) + GL.glEnd() + GL.glPolygonMode(GL.GL_FRONT_AND_BACK, GL.GL_FILL) + + def _draw_tool(self, GL, distance: float) -> None: # pragma: no cover - context + """The tool axis, drawn through the point the tool actually touches. + + It follows the picked contact, not the patch's centre. During playback that + is what makes the alignment readable: the tool line stays put while the + normal comes onto it, and once it arrives the two are collinear. Drawn + through the centre instead, they would only ever be parallel. + """ + assert self._bundle is not None + extent = float(np.linalg.norm(self._bundle.extent_mm)) or 1.0 + direction = self._stack.tool_direction + # Convert the platform direction into the sensor frame for drawing, since + # the mesh is in sensor coordinates. + local = to_sensor(direction, self._stack.axis_map) + + if self._point_mm is None: + anchor = self._bundle.centre_mm + start = anchor - local * extent * 0.6 + end = anchor + local * extent * 0.6 + else: + # Anchored at the contact, which the stages turn and the table moves. + contact = self._placed_contact(self._point_mm) + start = contact - local * extent * 0.6 + end = contact + local * extent * 0.6 + + GL.glLineWidth(2.0) + GL.glColor3f(0.95, 0.85, 0.25) + GL.glBegin(GL.GL_LINES) + GL.glVertex3f(float(start[0]), float(start[1]), float(start[2])) + GL.glVertex3f(float(end[0]), float(end[1]), float(end[2])) + GL.glEnd() + _draw_arrow_head(GL, end, local, extent * 0.06) + del distance + + def _placed_contact(self, point: np.ndarray) -> np.ndarray: + """The contact point, which the stages turn but the table does not move. + + The marker is the *measurement*, not furniture: it stays where the operator + clicked while the body around it slides to the table's new position. That is + what makes the two phases readable — the normal swings onto the tool axis + during the rotation, then holds still while the body moves away. + """ + transform = self._sim_transform() + if transform is None: + return np.asarray(point, dtype=np.float64) + return transform[0] @ np.asarray(point, dtype=np.float64) + + def _placed_direction(self, direction: np.ndarray) -> np.ndarray: + """A sensor-frame direction under the simulation's rotation.""" + transform = self._sim_transform() + if transform is None: + return np.asarray(direction, dtype=np.float64) + return transform[0] @ np.asarray(direction, dtype=np.float64) + + def _draw_origin(self, GL) -> None: # pragma: no cover - needs a context + """Mark the pivot the stages turn about: the contact point. + + The rotations are applied about the contact, so that is where the marker + belongs — it makes the rotation visibly *about* something and the + translation visibly the body sliding away from the tool. + """ + if self._point_mm is None: + return + pivot = self._placed_contact(self._point_mm) + GL.glPointSize(7.0) + GL.glColor3f(0.95, 0.55, 0.20) + GL.glBegin(GL.GL_POINTS) + GL.glVertex3f(float(pivot[0]), float(pivot[1]), float(pivot[2])) + GL.glEnd() + + def _draw_pick(self, GL, distance: float) -> None: # pragma: no cover - context + if self._point_mm is None or self._normal_mm is None: + return + assert self._bundle is not None + extent = float(np.linalg.norm(self._bundle.extent_mm)) or 1.0 + point = self._placed_contact(self._point_mm) + GL.glPointSize(9.0) + GL.glColor3f(1.0, 1.0, 1.0) + GL.glBegin(GL.GL_POINTS) + GL.glVertex3f(float(point[0]), float(point[1]), float(point[2])) + GL.glEnd() + # The normal rotates with the model, so watching it meet the tool axis is + # the whole point of playing the motion. + normal = self._placed_direction(self._normal_mm) + normal = normal / max(float(np.linalg.norm(normal)), 1e-12) + tip = point + normal * extent * 0.25 + GL.glLineWidth(3.0 if self._sim_running else 2.0) + GL.glColor3f(0.3, 0.85, 1.0) + GL.glBegin(GL.GL_LINES) + GL.glVertex3f(float(point[0]), float(point[1]), float(point[2])) + GL.glVertex3f(float(tip[0]), float(tip[1]), float(tip[2])) + GL.glEnd() + _draw_arrow_head(GL, tip, normal, extent * 0.04) + del distance + + def _draw_axes(self, GL) -> None: # pragma: no cover - needs a context + """The motion axes, drawn in the sensor frame the mesh lives in. + + Three things the mount has to be judged against, all in one picture: the + platform's X and Y (the base the sensor sits on), its Z (the axis the tool + travels on), and a short arc for each stage showing which way it turns. + + Every direction is converted with ``to_sensor``, so what is drawn is the + *platform* frame as it actually appears relative to this mesh. The mapping + is a bench fact that has been wrong before, so it is converted rather than + assumed from how the mesh happens to look. + """ + from ..sensor.frames import to_sensor + + assert self._bundle is not None + extent = float(np.linalg.norm(self._bundle.extent_mm)) or 1.0 + origin = self._bundle.centre_mm + length = extent * 0.45 + + for axis in ("x", "y", "z"): + direction = np.zeros(3) + direction[("x", "y", "z").index(axis)] = 1.0 + local = to_sensor(direction, self._stack.axis_map) + tip = origin + local * length + GL.glLineWidth(2.0) + GL.glColor3f(*AXIS_COLOURS[axis]) + GL.glBegin(GL.GL_LINES) + GL.glVertex3f(float(origin[0]), float(origin[1]), float(origin[2])) + GL.glVertex3f(float(tip[0]), float(tip[1]), float(tip[2])) + GL.glEnd() + _draw_arrow_head(GL, tip, local, extent * 0.035) + self._axis_tips[axis] = tip + + self._draw_stage_arc(GL, origin, extent, self._stack.yaw_axis, "yaw") + self._draw_stage_arc(GL, origin, extent, self._stack.pitch_axis, "pitch") + + def _draw_stage_arc( # pragma: no cover - needs a context + self, GL, origin: np.ndarray, extent: float, axis: str, _name: str + ) -> None: + """A quarter-turn arc about a stage axis, with an arrow at its end.""" + from ..sensor.frames import to_sensor + + local = to_sensor( + np.eye(3)[("x", "y", "z").index(axis)], self._stack.axis_map + ) + helper = np.array([0.0, 0.0, 1.0]) if abs(local[2]) < 0.9 else np.array([1.0, 0.0, 0.0]) + first = np.cross(local, helper) + first = first / max(float(np.linalg.norm(first)), 1e-12) + second = np.cross(local, first) + + radius = extent * 0.30 + steps = 24 + GL.glLineWidth(1.5) + GL.glColor3f(0.85, 0.85, 0.85) + GL.glBegin(GL.GL_LINE_STRIP) + for index in range(steps + 1): + angle = math.radians(100.0) * index / steps + point = origin + radius * (math.cos(angle) * first + math.sin(angle) * second) + GL.glVertex3f(float(point[0]), float(point[1]), float(point[2])) + GL.glEnd() + tip = origin + radius * ( + math.cos(math.radians(100.0)) * first + math.sin(math.radians(100.0)) * second + ) + tangent = ( + -math.sin(math.radians(100.0)) * first + math.cos(math.radians(100.0)) * second + ) + _draw_arrow_head(GL, tip, tangent, extent * 0.03) + + # ------------------------------------------------------------------- overlay + + def _note_missing_context(self) -> None: + """Record why there is no picture when the platform has no GL context. + + Without a context Qt never calls ``initializeGL``, so ``_problem`` would + otherwise stay empty and a preview that cannot exist would be + indistinguishable from one that has simply not been told what to draw. + """ + if self._gl_ok and not self.has_context() and not self._problem: + self._problem = t("finger.preview_no_gl", error="no OpenGL context") + + def _context_ready(self) -> bool: + """Whether a GL context exists, so a QPainter on this widget is usable.""" + self._note_missing_context() + return self._gl_ok and self.has_context() + + def paintEvent(self, event) -> None: # noqa: N802 - Qt naming + """Draw the GL scene, then a 2D overlay, which Qt composites on top. + + The context is checked *before* the base class paints. The offscreen + platform cannot create a GL context at all, and ``QOpenGLWidget``'s own + paint then leaves a paint device with no engine behind it: a ``QPainter`` + on it floods the console with "Paint device returned engine == 0" on every + repaint. Skipping the base paint too is what actually stops that; guarding + only the overlay does not. + """ + if not self._context_ready(): + return + super().paintEvent(event) + painter = QPainter(self) + if not painter.isActive(): + return + try: + if self._show_axes: + for axis, tip in self._axis_tips.items(): + screen = self.project(tip) + if screen is None: + continue + # Clamped inside the widget: at the default zoom the axes run + # well past the edges, and an unclamped label is drawn outside + # the visible area, which loses the one thing it was for. + x = float(min(max(screen[0], 12.0), self.width() - 12.0)) + y = float(min(max(screen[1], 14.0), self.height() - 6.0)) + colour = AXIS_COLOURS[axis] + painter.setPen( + QPen( + QColor( + int(colour[0] * 255), int(colour[1] * 255), int(colour[2] * 255) + ) + ) + ) + painter.drawText(int(x) + 2, int(y), axis.upper()) + # Only the failure reason lives here; the legend is a sibling label. + # Text composited by Qt over a GL surface renders as an overlap of + # itself once it overflows, so nothing long belongs on this widget. + if self._problem: + painter.setPen(QPen(QColor("#d92d20"))) + painter.drawText( + 10, + self.height() - 10, + QFontMetrics(self.font()).elidedText( + self._problem, Qt.TextElideMode.ElideRight, self.width() - 20 + ), + ) + finally: + painter.end() + + # -------------------------------------------------------------------- orbit + + def mousePressEvent(self, event) -> None: # noqa: N802 - Qt naming + self._last_pos = event.position().toPoint() + + def mouseMoveEvent(self, event) -> None: # noqa: N802 - Qt naming + current = event.position().toPoint() + delta = current - self._last_pos + self._last_pos = current + self._yaw -= delta.x() * ORBIT_RAD_PER_PX + self._pitch = max( + -MAX_PITCH_RAD, min(MAX_PITCH_RAD, self._pitch + delta.y() * ORBIT_RAD_PER_PX) + ) + self.update() + + def wheelEvent(self, event) -> None: # noqa: N802 - Qt naming + steps = event.angleDelta().y() / 120.0 + self._distance_scale = max( + 0.2, min(5.0, self._distance_scale * (1.0 - ZOOM_PER_STEP * steps)) + ) + self.update() + + def reset_view(self) -> None: + self.initial_view() + + +def _look_at(eye: np.ndarray, target: np.ndarray) -> None: # pragma: no cover - context + """Apply a view matrix, so no GLU dependency is needed.""" + from OpenGL import GL + + forward = target - eye + forward = forward / max(float(np.linalg.norm(forward)), 1e-12) + up_world = np.array([0.0, 0.0, 1.0]) if abs(forward[2]) < 0.9 else np.array([0.0, 1.0, 0.0]) + right = np.cross(forward, up_world) + right = right / max(float(np.linalg.norm(right)), 1e-12) + up = np.cross(right, forward) + matrix = np.eye(4) + matrix[0, :3] = right + matrix[1, :3] = up + matrix[2, :3] = -forward + matrix[0, 3] = -float(right @ eye) + matrix[1, 3] = -float(up @ eye) + matrix[2, 3] = float(forward @ eye) + GL.glMultMatrixf(matrix.T.astype(np.float32)) + + +def _draw_arrow_head(GL, tip: np.ndarray, direction: np.ndarray, size: float) -> None: # pragma: no cover + """A short cross at the tip — a full cone is not worth the triangles.""" + helper = np.array([0.0, 0.0, 1.0]) if abs(direction[2]) < 0.9 else np.array([1.0, 0.0, 0.0]) + first = np.cross(direction, helper) + first = first / max(float(np.linalg.norm(first)), 1e-12) + second = np.cross(direction, first) + GL.glBegin(GL.GL_LINES) + for axis in (first, second): + start = tip - axis * size + end = tip + axis * size + GL.glVertex3f(float(start[0]), float(start[1]), float(start[2])) + GL.glVertex3f(float(end[0]), float(end[1]), float(end[2])) + GL.glEnd() diff --git a/apps/autocal/src/orisys_autocal/ui/sensor_panel.py b/apps/autocal/src/orisys_autocal/ui/sensor_panel.py new file mode 100644 index 0000000..667bd22 --- /dev/null +++ b/apps/autocal/src/orisys_autocal/ui/sensor_panel.py @@ -0,0 +1,674 @@ +"""Finger debug screen: pick a calibration point on the flat image, get a pose. + +Workflow, with no platform connected: + +1. load an export bundle (`…\\finger\\r01\\compute`); +2. click a point on the flat UV image of the sensing patch; +3. read the real sensor-frame point and its surface normal; +4. read the simulated pose — X, Y, yaw, pitch — that would put the tool axis on + that normal, **with the residual it could not remove**; +5. watch the 3D preview colour the patch by how much of it the stages can + actually align. + +The residual is the honest part. Two rotations about fixed axes cannot cover the +whole sphere inside a finite yaw envelope, so some points simply cannot be +aligned; the panel reports the number rather than a pose that looks fine and is +not. +""" + +from __future__ import annotations + +from pathlib import Path + +import numpy as np +from PySide6.QtCore import QPoint, Qt, Signal +from PySide6.QtGui import QImage, QPainter, QPen, QPixmap +from PySide6.QtWidgets import ( + QCheckBox, + QDoubleSpinBox, + QGridLayout, + QGroupBox, + QHBoxLayout, + QHeaderView, + QLabel, + QLineEdit, + QPushButton, + QScrollArea, + QSizePolicy, + QTableWidget, + QTableWidgetItem, + QVBoxLayout, + QWidget, +) + +from orisys_app_shared.i18n import t + +from ..motion.kinematics import PlatformStack +from ..sensor.export_bundle import SensorBundle +from ..sensor.frames import describe as describe_frames +from ..sensor.uv_flat import FlatSurface + +#: How many picked points the list keeps before the oldest is dropped. +MAX_POINTS = 200 + + +class FlatSurfaceView(QLabel): + """The flat image, clickable, with the picked pixel cross-haired. + + The image is **scaled down to fit** the view while keeping its aspect ratio. + Shown at native size it is 1024 square, so inside this pane it was cropped to + a corner of the UV space and most of the patch could not be clicked at all. + ``pixel_at`` inverts the same scale and centring, so what is displayed and + what is picked stay the same pixel. + """ + + pixelPicked = Signal(int, int) + + def __init__(self, parent: QWidget | None = None) -> None: + super().__init__(parent) + self.setObjectName("flatSurfaceView") + self.setAlignment(Qt.AlignmentFlag.AlignCenter) + # No minimum: an image that cannot shrink would push the pane's minimum + # size past the window's and be clipped instead of scaled. + self.setMinimumSize(80, 80) + self.setSizePolicy(QSizePolicy.Policy.Ignored, QSizePolicy.Policy.Ignored) + self._flat: FlatSurface | None = None + self._pixmap: QPixmap | None = None + self._marker: tuple[int, int] | None = None + self.setCursor(Qt.CursorShape.CrossCursor) + + @property + def flat(self) -> FlatSurface | None: + return self._flat + + def set_flat(self, flat: FlatSurface | None) -> None: + self._flat = flat + self._marker = None + if flat is None: + self._pixmap = None + self.clear() + return + # The lookup table is band-contiguous because it comes from a fresh + # numpy array, so QImage can wrap it without a copy. + height, width = flat.image_rgb.shape[:2] + image = QImage( + flat.image_rgb.data, width, height, 3 * width, QImage.Format.Format_RGB888 + ) + self._pixmap = QPixmap.fromImage(image.copy()) + self._refresh() + + def set_marker(self, pixel: tuple[int, int] | None) -> None: + self._marker = pixel + self._refresh() + + def _marked(self) -> QPixmap | None: + """The native-size image with the marker drawn on it, or ``None``.""" + if self._pixmap is None: + return None + if self._marker is None: + return self._pixmap + canvas = self._pixmap.copy() + painter = QPainter(canvas) + try: + x, y = self._marker + painter.setPen(QPen(Qt.GlobalColor.white, 1, Qt.PenStyle.SolidLine)) + painter.drawLine(x - 8, y, x + 8, y) + painter.drawLine(x, y - 8, x, y + 8) + painter.setPen(QPen(Qt.GlobalColor.black, 1, Qt.PenStyle.SolidLine)) + painter.drawEllipse(QPoint(x, y), 5, 5) + finally: + painter.end() + return canvas + + def _refresh(self) -> None: + """Scale the marked image to fit the pane and show all of it.""" + marked = self._marked() + if marked is None: + return + if self._flat is None or marked.width() <= 0 or marked.height() <= 0: + return + available_w = max(1, self.width()) + available_h = max(1, self.height()) + scale = min(available_w / marked.width(), available_h / marked.height()) + # Never magnify: a 320-pixel pane showing a 1024 image should not blur it + # up, and a pane larger than the image can centre it as-is. + scale = min(scale, 1.0) + if scale >= 1.0: + self.setPixmap(marked) + return + self.setPixmap( + marked.scaled( + max(1, int(marked.width() * scale)), + max(1, int(marked.height() * scale)), + Qt.AspectRatioMode.KeepAspectRatio, + Qt.TransformationMode.SmoothTransformation, + ) + ) + + def resizeEvent(self, event) -> None: # noqa: N802 - Qt naming + super().resizeEvent(event) + self._refresh() + + def pixel_at(self, position) -> tuple[int, int] | None: + """Which image pixel a widget position lands on, or ``None`` outside.""" + if self._pixmap is None or self._flat is None: + return None + shown = self.pixmap() + if shown is None or shown.isNull(): + return None + offset_x = (self.width() - shown.width()) / 2.0 + offset_y = (self.height() - shown.height()) / 2.0 + x = int((position.x() - offset_x) / max(shown.width(), 1) * self._flat.width) + y = int((position.y() - offset_y) / max(shown.height(), 1) * self._flat.height) + if not (0 <= x < self._flat.width and 0 <= y < self._flat.height): + return None + return x, y + + def mousePressEvent(self, event) -> None: # noqa: N802 - Qt naming + pixel = self.pixel_at(event.position()) + if pixel is not None: + self.pixelPicked.emit(pixel[0], pixel[1]) + + +class FingerPanel(QGroupBox): + """The whole debug screen: bundle, picker, readouts, point list, preview.""" + + loadRequested = Signal() + pickRequested = Signal(int, int) + stackChanged = Signal() + + def __init__( + self, + stack: PlatformStack | None = None, + *, + bundle_path: str = "", + plate_thickness_mm: float = 0.0, + parent: QWidget | None = None, + ) -> None: + super().__init__(parent) + self.setObjectName("groupBox") + self._stack = stack or PlatformStack() + self._bundle: SensorBundle | None = None + #: Full-precision picked rows, index-aligned with the table. + self._picked: list[dict[str, float]] = [] + + outer = QVBoxLayout(self) + outer.setContentsMargins(10, 8, 10, 10) + outer.setSpacing(8) + + outer.addWidget(self._build_toolbar(bundle_path)) + + body = QHBoxLayout() + body.setSpacing(10) + picker_box = self._build_picker() + readout_box = self._build_readouts() + preview_box = self._build_preview() + # Minimum widths stop the longest string in a column dictating the split. + # ``Preferred``, not ``Ignored``: the latter tells the layout to disregard + # the widget's size hint entirely and the columns collapse to their floors. + for column, minimum in ((picker_box, 200), (readout_box, 200), (preview_box, 300)): + column.setMinimumWidth(minimum) + column.setSizePolicy(QSizePolicy.Policy.Preferred, QSizePolicy.Policy.Preferred) + body.addWidget(picker_box, 2) + body.addWidget(readout_box, 3) + body.addWidget(preview_box, 5) + outer.addLayout(body, 1) + + outer.addWidget(self._build_point_table(), 2) + + self.set_plate_thickness(plate_thickness_mm) + self.retranslate() + + # ------------------------------------------------------------------ widgets + + def _build_toolbar(self, bundle_path: str) -> QWidget: + box = QWidget(self) + row = QHBoxLayout(box) + row.setContentsMargins(0, 0, 0, 0) + row.setSpacing(8) + self.path_label = QLabel(box) + self.path_label.setObjectName("hintLabel") + self.path_edit = QLineEdit(bundle_path, box) + self.path_edit.setReadOnly(True) + self.path_edit.setFixedHeight(28) + self.load_button = QPushButton(box) + self.load_button.clicked.connect(self.loadRequested) + self.reset_button = QPushButton(box) + self.reset_button.clicked.connect(lambda: self.preview.reset_view()) + self.initial_view_button = QPushButton(box) + # The same state as ``reset_view``: the orientation the sensor is + # commissioned in is the one worth being able to return to by name. + self.initial_view_button.clicked.connect(lambda: self.preview.reset_view()) + row.addWidget(self.path_label) + row.addWidget(self.path_edit, 1) + row.addWidget(self.load_button) + row.addWidget(self.initial_view_button) + row.addWidget(self.reset_button) + return box + + def _build_picker(self) -> QWidget: + box = QGroupBox(self) + box.setObjectName("groupBox") + column = QVBoxLayout(box) + column.setContentsMargins(8, 6, 8, 8) + column.setSpacing(6) + + self.view = FlatSurfaceView(box) + self.view.pixelPicked.connect(self.pickRequested) + # No scroll area: the image is scaled to fit the pane, so there is nothing + # to scroll to, and a resizable scroll widget would dictate a minimum size + # that crops the UV space instead of shrinking it. + column.addWidget(self.view, 1) + + self.view_label = QLabel(box) + self.view_label.setObjectName("hintLabel") + self.view_label.setWordWrap(True) + column.addWidget(self.view_label) + + self.status_label = QLabel(box) + self.status_label.setObjectName("statusValue") + self.status_label.setWordWrap(True) + column.addWidget(self.status_label) + self._picker_box = box + return box + + def _build_readouts(self) -> QWidget: + box = QGroupBox(self) + box.setObjectName("groupBox") + grid = QGridLayout(box) + grid.setContentsMargins(10, 8, 10, 8) + grid.setHorizontalSpacing(8) + grid.setVerticalSpacing(4) + + self._readout_labels: dict[str, QLabel] = {} + rows = ( + "sensor_point", + "sensor_normal", + "platform_point", + "pose", + "residual", + "frames", + "bundle", + ) + for row, key in enumerate(rows): + label = QLabel(box) + label.setObjectName("hintLabel") + value = QLabel("—", box) + value.setObjectName("statusValue") + value.setWordWrap(True) + value.setTextInteractionFlags(Qt.TextInteractionFlag.TextSelectableByMouse) + # These values are long ("sensor x->x, y->z, z->-y"). Allowed to drive + # their own width they claimed most of the row and squeezed the UV + # picker beside them down to a thumbnail; wrapping keeps them readable + # at whatever width the row can spare. + value.setMinimumWidth(60) + value.setSizePolicy(QSizePolicy.Policy.Ignored, QSizePolicy.Policy.Preferred) + grid.addWidget(label, row, 0) + grid.addWidget(value, row, 1) + self._readout_labels[key] = value + + # The mounting plate: a translation along the platform's tool axis, so a + # fixture thickness is one number rather than a transform to maintain. + self.plate_label = QLabel(box) + self.plate_label.setObjectName("hintLabel") + self.plate_spin = QDoubleSpinBox(box) + self.plate_spin.setRange(-100.0, 100.0) + self.plate_spin.setDecimals(3) + self.plate_spin.setSingleStep(0.5) + self.plate_spin.setSuffix(" mm") + self.plate_spin.setFixedHeight(28) + self.plate_spin.valueChanged.connect(self._on_plate_changed) + grid.addWidget(self.plate_label, len(rows), 0) + grid.addWidget(self.plate_spin, len(rows), 1) + + self.offset_label = QLabel(box) + self.offset_label.setObjectName("hintLabel") + self.offset_edit = QLineEdit(box) + self.offset_edit.setFixedHeight(28) + self.offset_edit.setToolTip("sensor origin offset, x,y,z mm") + self.offset_edit.editingFinished.connect(self._on_offset_changed) + grid.addWidget(self.offset_label, len(rows) + 1, 0) + grid.addWidget(self.offset_edit, len(rows) + 1, 1) + + grid.setColumnStretch(1, 1) + self._readout_box = box + return box + + def _build_preview(self) -> QWidget: + from .pose_preview import PosePreview + + box = QGroupBox(self) + box.setObjectName("groupBox") + column = QVBoxLayout(box) + column.setContentsMargins(8, 6, 8, 8) + column.setSpacing(6) + self.preview = PosePreview(box) + column.addWidget(self.preview, 1) + self.axes_check = QCheckBox(box) + self.axes_check.setChecked(True) + self.axes_check.toggled.connect(self.preview.set_show_axes) + column.addWidget(self.axes_check) + controls = QHBoxLayout() + controls.setContentsMargins(0, 0, 0, 0) + controls.setSpacing(6) + self.sim_button = QPushButton(box) + self.sim_button.setObjectName("primaryButton") + # Disabled until a point has been picked: there is no pose to play before + # that, and a button that silently does nothing is worse than a grey one. + self.sim_button.setEnabled(False) + self.sim_button.clicked.connect(self.preview.start_simulation) + self.preview.simulationChanged.connect(self._on_simulation_changed) + controls.addWidget(self.sim_button) + controls.addStretch(1) + column.addLayout(controls) + # A real label, not text painted onto the GL widget: over a GL surface Qt + # composites a long overflowing string as an overlap of itself, and the + # legend was unreadable on the bench because of it. A label wraps. + self.legend_label = QLabel(box) + self.legend_label.setObjectName("hintLabel") + self.legend_label.setWordWrap(True) + column.addWidget(self.legend_label) + self.reach_label = QLabel(box) + self.reach_label.setObjectName("hintLabel") + self.reach_label.setWordWrap(True) + column.addWidget(self.reach_label) + self._preview_box = box + return box + + def _build_point_table(self) -> QWidget: + box = QGroupBox(self) + box.setObjectName("groupBox") + column = QVBoxLayout(box) + column.setContentsMargins(8, 6, 8, 8) + column.setSpacing(6) + + headers = ( + "x", "y", "z", "nx", "ny", "nz", + "X", "Y", "Z", "yaw", "pitch", "residual", + ) + self.table = QTableWidget(0, len(headers), box) + self.table.verticalHeader().setVisible(False) + self.table.setSelectionMode(QTableWidget.SelectionMode.NoSelection) + self.table.setEditTriggers(QTableWidget.EditTrigger.NoEditTriggers) + self.table.setAlternatingRowColors(True) + for index in range(len(headers)): + self.table.setColumnWidth(index, 82) + self.table.horizontalHeader().setSectionResizeMode( + len(headers) - 1, QHeaderView.ResizeMode.Stretch + ) + self.table.setMinimumHeight(90) + column.addWidget(self.table, 1) + + row = QHBoxLayout() + self.clear_button = QPushButton(box) + self.clear_button.clicked.connect(self.clear_points) + self.count_label = QLabel(box) + self.count_label.setObjectName("hintLabel") + row.addWidget(self.clear_button) + row.addStretch(1) + row.addWidget(self.count_label) + column.addLayout(row) + + self._table_box = box + self._headers = headers + return box + + # -------------------------------------------------------------------- state + + @property + def bundle(self) -> SensorBundle | None: + return self._bundle + + @property + def stack(self) -> PlatformStack: + return self._stack + + @property + def plate_thickness_mm(self) -> float: + return float(self.plate_spin.value()) + + def set_plate_thickness(self, value: float) -> None: + self.plate_spin.blockSignals(True) + self.plate_spin.setValue(float(value)) + self.plate_spin.blockSignals(False) + + def set_bundle_path(self, path: str) -> None: + self.path_edit.setText(path) + + def set_bundle(self, bundle: SensorBundle | None, flat: FlatSurface | None = None) -> None: + self._bundle = bundle + self.view.set_flat(flat) + self.preview.set_bundle(bundle) + self.refresh_readouts() + self._refresh_reach() + + def _on_simulation_changed(self, running: bool) -> None: + """Keep the button honest about what it will do next.""" + self.sim_button.setEnabled(self.preview.can_simulate) + self.sim_button.setText( + t("finger.sim_stop") if running else t("finger.sim_play") + ) + + def _sync_sim_button(self) -> None: + self.sim_button.setText( + t("finger.sim_stop") if self.preview.simulating else t("finger.sim_play") + ) + self.sim_button.setEnabled(self.preview.can_simulate) + + def set_texture_state(self, *, textured: bool, path: str = "") -> None: + """Say which flat image is showing, so a fallback is visible. + + Without this the layout map and the texture look like two different + patches rather than one patch and one stand-in. + """ + if textured: + self.view_label.setText(t("finger.textured", name=Path(path).name)) + elif path: + self.view_label.setText(t("finger.texture_missing", path=path)) + else: + self.view_label.setText(t("finger.untextured")) + + def clear_points(self) -> None: + self._picked.clear() + self.preview.set_pose(None) + self.preview.set_point(None, None) + self._sync_sim_button() + self.table.setRowCount(0) + self.view.set_marker(None) + self.count_label.setText(t("finger.point_count", count=0)) + + def point_count(self) -> int: + return self.table.rowCount() + + def points(self) -> list[dict[str, float]]: + """Every picked point, at full precision, for a later export.""" + return [dict(row) for row in self._picked] + + # ------------------------------------------------------------------ actions + + def handle_pick(self, x: int, y: int) -> dict[str, float] | None: + """Turn a click into a pose. Returns the row, or ``None`` if no surface. + + ``None`` is the honest answer for a UV-island gap: reporting (0, 0, 0) + would put a calibration point at the sensor's origin. + """ + if self.view.flat is None: + self.status_label.setText(t("finger.no_bundle")) + return None + sample = self.view.flat.sample(x, y) + if sample is None: + self.status_label.setText(t("finger.no_surface", x=x, y=y)) + return None + + point, normal, face = sample + pose = self._stack.solve( + point, normal, plate_thickness_mm=self.plate_thickness_mm + ) + self.view.set_marker((x, y)) + self.preview.set_stack(self._stack) + self.preview.set_point(point, normal) + + row = { + "x": float(point[0]), "y": float(point[1]), "z": float(point[2]), + "nx": float(normal[0]), "ny": float(normal[1]), "nz": float(normal[2]), + "X": float(pose.target_xy_mm[0]), "Y": float(pose.target_xy_mm[1]), + "Z": float(pose.contact_z_mm), + "yaw": float(pose.yaw_deg), "pitch": float(pose.pitch_deg), + "residual": float(pose.residual_deg), + } + self._append_row(row) + self._last_pose = pose + self._last_face = face + # Hand the solved pose to the preview so the play button demonstrates + # exactly the numbers on screen, rather than a second solve of its own. + self.preview.set_pose(pose) + self._sync_sim_button() + self.refresh_readouts(point, normal, pose, face) + return row + + def _append_row(self, row: dict[str, float]) -> None: + # The full-precision row is kept alongside the table, because the table + # only holds three decimals for reading. Recovering the numbers by parsing + # the displayed text would quietly hand a later export coordinates the + # solver never produced. + self._picked.append(dict(row)) + index = self.table.rowCount() + self.table.insertRow(index) + for column, key in enumerate(self._headers): + item = QTableWidgetItem(f"{row[key]:.3f}") + item.setTextAlignment(Qt.AlignmentFlag.AlignRight | Qt.AlignmentFlag.AlignVCenter) + self.table.setItem(index, column, item) + while self.table.rowCount() > MAX_POINTS: + self.table.removeRow(0) + while len(self._picked) > MAX_POINTS: + self._picked.pop(0) + self.count_label.setText(t("finger.point_count", count=self.table.rowCount())) + + # ----------------------------------------------------------------- readouts + + _last_pose = None + _last_face = -1 + + def refresh_readouts(self, point=None, normal=None, pose=None, face: int = -1) -> None: + bundle = self._bundle + self._readout_labels["frames"].setText(describe_frames(self._stack.axis_map)) + if bundle is None: + self._readout_labels["bundle"].setText("—") + for key in ("sensor_point", "sensor_normal", "platform_point", "pose", "residual"): + self._readout_labels[key].setText("—") + return + + info = bundle.describe() + self._readout_labels["bundle"].setText( + t( + "finger.bundle_summary", + vertices=info["vertices"], + triangles=info["triangles"], + extent=" × ".join(f"{value:.2f}" for value in info["extent_mm"]), + scale=f"{info['mm_per_unit']:.6f}", + ) + ) + if point is None: + return + platform = self._stack.point_to_platform( + point, plate_thickness_mm=self.plate_thickness_mm + ) + self._readout_labels["sensor_point"].setText( + " ".join(f"{value:+.3f}" for value in point) + " mm" + ) + self._readout_labels["sensor_normal"].setText( + " ".join(f"{value:+.3f}" for value in normal) + ) + self._readout_labels["platform_point"].setText( + " ".join(f"{value:+.3f}" for value in platform) + " mm" + ) + if pose is not None: + self._readout_labels["pose"].setText(pose.describe()) + self._readout_labels["residual"].setText( + t( + "finger.residual_value", + residual=f"{pose.residual_deg:.4f}", + verdict=( + t("finger.reachable") + if pose.reachable + else t("finger.unreachable") + ), + ) + ) + del face + + def _refresh_reach(self) -> None: + """The reach summary, or why there is no picture instead of silently nothing. + + A blank preview with no explanation is indistinguishable from a broken + one, which is what a missing GL context used to look like. + """ + summary = self.preview.residual_summary() + if not summary: + self.reach_label.setText(self.preview.problem) + return + text = t( + "finger.reach_summary", + aligned=int(summary["aligned"]), + vertices=int(summary["vertices"]), + share=f"{summary['aligned_fraction'] * 100:.0f}", + worst=f"{summary['worst_deg']:.1f}", + ) + if self.preview.problem: + text = f"{text} · {self.preview.problem}" + self.reach_label.setText(text) + + # ------------------------------------------------------------------ handlers + + def _on_plate_changed(self, _value: float) -> None: + self.stackChanged.emit() + + def _on_offset_changed(self) -> None: + parts = self.offset_edit.text().replace(",", " ").split() + try: + triple = tuple(float(part) for part in parts[:3]) + except ValueError: + return + if len(triple) == 3: + self._stack.tool_offset_mm = triple # type: ignore[assignment] + self.stackChanged.emit() + + def set_offset(self, triple) -> None: + self.offset_edit.setText(" ".join(f"{value:g}" for value in triple)) + + # ------------------------------------------------------------- translations + + def retranslate(self) -> None: + self.setTitle(t("finger.title")) + self.path_label.setText(t("finger.bundle_path")) + self.load_button.setText(t("finger.load")) + self.reset_button.setText(t("finger.reset_view")) + self.initial_view_button.setText(t("finger.initial_view")) + self._picker_box.setTitle(t("finger.flat_title")) + self._readout_box.setTitle(t("finger.readout_title")) + self._preview_box.setTitle(t("finger.preview_title")) + self._table_box.setTitle(t("finger.points_title")) + labels = { + "sensor_point": "finger.row_sensor_point", + "sensor_normal": "finger.row_sensor_normal", + "platform_point": "finger.row_platform_point", + "pose": "finger.row_pose", + "residual": "finger.row_residual", + "frames": "finger.row_frames", + "bundle": "finger.row_bundle", + } + grid = self._readout_box.layout() + for row, key in enumerate(labels): + widget = grid.itemAtPosition(row, 0) + if widget is not None and widget.widget() is not None: + widget.widget().setText(t(labels[key])) + self.plate_label.setText(t("finger.plate_thickness")) + self.offset_label.setText(t("finger.tool_offset")) + self.legend_label.setText(t("finger.preview_legend")) + self.axes_check.setText(t("finger.show_axes")) + self.sim_button.setToolTip(t("finger.sim_hint")) + self._sync_sim_button() + self.clear_button.setText(t("finger.clear_points")) + self.count_label.setText(t("finger.point_count", count=self.table.rowCount())) + self.table.setHorizontalHeaderLabels([t(f"finger.col_{key}") for key in self._headers]) + self.status_label.setText(t("finger.select_hint")) diff --git a/apps/autocal/src/orisys_autocal/ui/window.py b/apps/autocal/src/orisys_autocal/ui/window.py index ee4324a..a1b45d1 100644 --- a/apps/autocal/src/orisys_autocal/ui/window.py +++ b/apps/autocal/src/orisys_autocal/ui/window.py @@ -14,6 +14,8 @@ import subprocess import sys from pathlib import Path +import numpy as np + from orisys_app_shared import API_VERSION, __version__ as shared_version from orisys_app_shared.i18n import set_locale, t from orisys_app_shared.paths import outputs_dir @@ -37,7 +39,10 @@ from PySide6.QtWidgets import ( from .. import __version__ as app_version from ..motion import AxisParams, ConnectionSpec, MotionPoller, MotionService, Snapshot +from ..motion.kinematics import PlatformStack from ..preferences import load_preferences, save_preferences +from ..sensor import BundleError, load_bundle, load_uv_texture, rasterise, texture_path_for +from ..sensor.uv_flat import DEFAULT_FLAT_SIZE from ..sequence_io import ( FILE_SUFFIX, SequenceDocument, @@ -52,6 +57,7 @@ from .brand_bar import BrandBar from .bus_data_panel import BusDataPanel from .bus_panel import BusPanel, default_bas_dir from .connection_panel import ConnectionPanel +from .sensor_panel import FingerPanel from .sequence_panel import SequencePanel from .status_strip import RunBar, StatusStrip from .workspace import TABS, WorkspaceTabs @@ -89,7 +95,7 @@ _COMMAND_NAMES = { class AutoCalMainWindow(QMainWindow): """Platform control, five axes, one window.""" - def __init__(self, prefs: dict | None = None) -> None: + def __init__(self, prefs: dict | None = None, *, auto_load_bundle: bool = True) -> None: super().__init__() self.prefs = dict(prefs or load_preferences()) self._theme = str(self.prefs.get("theme") or "dark") @@ -98,6 +104,12 @@ class AutoCalMainWindow(QMainWindow): self._pending: dict[int, str] = {} self._last_error = "" + # Apply the remembered language before anything is built. The translator + # is process-global, and only the menu ever set it, so a saved language + # was ignored on every launch: the file said en_US and the window came up + # in whatever locale the process happened to start in. + set_locale(self._language) + self.setWindowTitle(t("app.title")) self.setMinimumSize(*MINIMUM_WINDOW_SIZE) @@ -107,6 +119,12 @@ class AutoCalMainWindow(QMainWindow): self._restore_geometry() self.retranslate() + + # Load the remembered bundle, so the finger debugger comes up working. + # Without this every session began with a filled-in path over two empty + # views, which reads as a broken screen rather than as an unloaded one. + if auto_load_bundle: + self._load_remembered_bundle() self._apply_theme() # --------------------------------------------------------------- build UI @@ -204,7 +222,28 @@ class AutoCalMainWindow(QMainWindow): parent=body, ) self.sequence_panel = SequencePanel(self.prefs.get("sequence_profile") or {}, body) - self.workspace = WorkspaceTabs(self.axis_panel, self.sequence_panel, body) + + # The finger debug screen: UV picking and a simulated pose, with no + # platform connected. Its stack carries the bench's stage travel limits, + # because a pose outside them is not a pose. + self.finger_stack = PlatformStack( + yaw_limit_deg=float(self.prefs.get("finger_yaw_limit_deg") or 90.0), + pitch_limit_deg=float(self.prefs.get("finger_pitch_limit_deg") or 360.0), + ) + offset = self.prefs.get("finger_tool_offset_mm") or [0.0, 0.0, 0.0] + if isinstance(offset, list) and len(offset) == 3: + self.finger_stack.tool_offset_mm = tuple(float(v) for v in offset) + self.finger_panel = FingerPanel( + self.finger_stack, + bundle_path=str(self.prefs.get("finger_bundle_path") or ""), + plate_thickness_mm=float(self.prefs.get("finger_plate_thickness_mm") or 0.0), + parent=body, + ) + self.finger_panel.set_offset(self.finger_stack.tool_offset_mm) + + self.workspace = WorkspaceTabs( + self.axis_panel, self.sequence_panel, self.finger_panel, body + ) self.main_splitter = QSplitter(Qt.Orientation.Horizontal, body) self.main_splitter.addWidget(self.left_rail) @@ -252,6 +291,115 @@ class AutoCalMainWindow(QMainWindow): self.run_bar.abortRequested.connect(self._on_run_abort) self.sequence_panel.exportRequested.connect(self._on_export_sequence) self.sequence_panel.importRequested.connect(self._on_import_sequence) + self.finger_panel.loadRequested.connect(self._on_load_bundle) + self.finger_panel.pickRequested.connect(self._on_pick_point) + self.finger_panel.stackChanged.connect(self._on_finger_stack_changed) + + # ---------------------------------------------------------- finger debug + + def load_bundle(self, path: str, *, flat_size: tuple[int, int] = DEFAULT_FLAT_SIZE): + """Load an export bundle and build its flat view. + + Bundle validation and rasterising are both allowed to fail: a wrong + export, a missing file or an unreadable mesh must leave the rest of the + app alone and report the reason. + """ + bundle = load_bundle( + path, + # The design profile declares these for this revision; a mismatch + # means a stale or wrong export. + expected_vertices=1032, + expected_triangles=1974, + ) + texture_path, texture = self._resolve_texture(bundle) + flat = rasterise(bundle, flat_size, texture=texture) + self.finger_panel.set_bundle(bundle, flat) + self.finger_panel.set_texture_state(textured=flat.textured, path=str(texture_path or "")) + self.prefs["finger_bundle_path"] = str(path) + info = bundle.describe() + self._report_sequence( + t( + "finger.loaded", + vertices=info["vertices"], + extent=" × ".join(f"{value:.2f}" for value in info["extent_mm"]), + ), + ok=True, + ) + return bundle + + def _resolve_texture(self, bundle) -> tuple[Path | None, np.ndarray | None]: + """The UV texture for this bundle, or nothing. + + Tried in order: the path recorded in preferences, then the one derived + from the bundle's own location. The derived path wins the tie in practice + because a texture from another revision would be resampled onto the wrong + mesh, and the two are usually the same file anyway. + + Neither being found is not an error — the picker falls back to the layout + map and says so. + """ + candidates = [texture_path_for(bundle)] + configured = str(self.prefs.get("finger_texture_path") or "").strip() + if configured: + candidates.append(Path(configured)) + for candidate in candidates: + if candidate is None or not candidate.is_file(): + continue + texture = load_uv_texture(candidate) + if texture is not None: + return candidate, texture + return (candidates[0] if candidates else None), None + + def _load_remembered_bundle(self) -> None: + """Load whatever bundle the preferences point at, tolerating absence. + + A missing directory is the normal case on a machine that has never + exported one, so it is reported quietly in the panel's own status line + rather than raised into the startup path. + """ + path = str(self.prefs.get("finger_bundle_path") or "").strip() + if not path or not Path(path).is_dir(): + return + try: + self.load_bundle(path) + except (BundleError, OSError, ValueError) as exc: + self.finger_panel.status_label.setText( + t("finger.load_failed", error=str(exc)) + ) + + def _on_load_bundle(self) -> None: + start = str(self.prefs.get("finger_bundle_path") or "") + path = QFileDialog.getExistingDirectory( + self, t("finger.load"), start, QFileDialog.Option.ShowDirsOnly + ) + if not path: + return + try: + self.load_bundle(path) + except (BundleError, OSError, ValueError) as exc: + self._report_sequence(t("finger.load_failed", error=str(exc)), ok=False) + + def _on_pick_point(self, x: int, y: int) -> None: + row = self.finger_panel.handle_pick(x, y) + if row is None: + # A UV gap or no bundle: the panel says which; do not imply success. + self.statusBar().showMessage(self.finger_panel.status_label.text()) + return + self._report_sequence( + t( + "finger.picked", + residual=f"{row['residual']:.3f}", + yaw=f"{row['yaw']:.2f}", + pitch=f"{row['pitch']:.2f}", + ), + ok=row["residual"] <= 0.5, + ) + + def _on_finger_stack_changed(self) -> None: + self.finger_panel.preview.set_stack(self.finger_panel.stack) + self.finger_panel._refresh_reach() + self.prefs["finger_plate_thickness_mm"] = self.finger_panel.plate_thickness_mm + self.prefs["finger_tool_offset_mm"] = list(self.finger_panel.stack.tool_offset_mm) self.axis_readout.stopAllRequested.connect(self._on_stop_all) self.axis_readout.clearAllRequested.connect(self._on_clear_all_alarms) self.axis_readout.enableAllRequested.connect(self._on_enable_all) @@ -855,6 +1003,7 @@ class AutoCalMainWindow(QMainWindow): self.axis_panel.retranslate() self.axis_readout.retranslate() self.sequence_panel.retranslate() + self.finger_panel.retranslate() self.workspace.retranslate() self.menu_file.setTitle(t("menu.file")) diff --git a/apps/autocal/src/orisys_autocal/ui/workspace.py b/apps/autocal/src/orisys_autocal/ui/workspace.py index c0fe890..eddf3b1 100644 --- a/apps/autocal/src/orisys_autocal/ui/workspace.py +++ b/apps/autocal/src/orisys_autocal/ui/workspace.py @@ -31,6 +31,7 @@ from .sequence_panel import SequencePanel TABS: tuple[tuple[str, str], ...] = ( ("axes", "workspace.tab_axes"), ("sequence", "workspace.tab_sequence"), + ("finger", "workspace.tab_finger"), ("sensor", "workspace.tab_sensor"), ("surface", "workspace.tab_surface"), ("points", "workspace.tab_points"), @@ -78,6 +79,7 @@ class WorkspaceTabs(QTabWidget): self, axis_panel: AxisPanel, sequence_panel: SequencePanel, + finger_panel: QWidget | None = None, parent: QWidget | None = None, ) -> None: super().__init__(parent) @@ -87,13 +89,16 @@ class WorkspaceTabs(QTabWidget): self._pages["axes"] = axis_panel self._pages["sequence"] = sequence_panel + if finger_panel is not None: + self._pages["finger"] = finger_panel for key, locale_key in _PLACEHOLDERS.items(): page = PlaceholderPage(locale_key, self) self._pages[key] = page self._placeholder_keys[key] = locale_key for key, _locale_key in TABS: - self.addTab(self._pages[key], "") + if key in self._pages: + self.addTab(self._pages[key], "") self.currentChanged.connect(self._on_current_changed) self.retranslate() @@ -101,7 +106,8 @@ class WorkspaceTabs(QTabWidget): # -------------------------------------------------------------------- state def page_keys(self) -> list[str]: - return [key for key, _locale_key in TABS] + """The tabs actually present, in display order.""" + return [key for key, _locale_key in TABS if key in self._pages] def page(self, key: str) -> QWidget: return self._pages[key] @@ -118,7 +124,8 @@ class WorkspaceTabs(QTabWidget): def retranslate(self) -> None: for index, (key, locale_key) in enumerate(TABS): - self.setTabText(index, t(locale_key)) + if key in self._pages: + self.setTabText(index, t(locale_key)) for key, locale_key in self._placeholder_keys.items(): page = self._pages[key] if isinstance(page, PlaceholderPage): diff --git a/apps/autocal/tests/conftest.py b/apps/autocal/tests/conftest.py index fa07387..eb5cffd 100644 --- a/apps/autocal/tests/conftest.py +++ b/apps/autocal/tests/conftest.py @@ -116,7 +116,10 @@ def make_window(qt_app, monkeypatch): from orisys_app_shared.i18n import set_locale set_locale(str(prefs.get("language") or "zh_CN")) - window = AutoCalMainWindow(prefs=prefs) + # The bundle is not auto-loaded here: rasterising 1024x1024 costs about a + # second, and every window in this suite would pay it. The one test that + # covers auto-loading turns it back on for itself. + window = AutoCalMainWindow(prefs=prefs, auto_load_bundle=False) # Swap in the simulated platform: no test in this suite touches a DLL. window.poller.service._transport = DemoTransport() # noqa: SLF001 created.append(window) diff --git a/apps/autocal/tests/test_binding_purity.py b/apps/autocal/tests/test_binding_purity.py index 2d01cd5..08f6074 100644 --- a/apps/autocal/tests/test_binding_purity.py +++ b/apps/autocal/tests/test_binding_purity.py @@ -58,6 +58,9 @@ _SCOPED_ENUMS = { "PenStyle", "GlobalColor", "ItemFlag", + "TextElideMode", + "TextFlag", + "AspectRatioMode", ), "QTableWidget": ("SelectionMode", "EditTrigger"), "QSizePolicy": ("Policy",), diff --git a/apps/autocal/tests/test_finger_panel.py b/apps/autocal/tests/test_finger_panel.py new file mode 100644 index 0000000..15d81d8 --- /dev/null +++ b/apps/autocal/tests/test_finger_panel.py @@ -0,0 +1,629 @@ +"""The finger debugger's panel: picking, refusing, and what a row contains. + +The bundle is the real export on this machine. It is skipped rather than failed +when absent, because the export lives in a sibling design repo: a machine without +it has not broken AutoCal. +""" + +from __future__ import annotations + +from pathlib import Path + +import numpy as np +import pytest +from PySide6.QtCore import QPointF + +from orisys_autocal.motion.kinematics import PlatformStack +from orisys_autocal.sensor import ( + DEFAULT_FLAT_SIZE, + load_bundle, + rasterise, + to_platform, + to_sensor, +) +from orisys_autocal.ui.sensor_panel import FingerPanel + +BUNDLE = Path( + r"D:\orisys_ws\orisys-sensor-design\build\export\finger\r01\compute" +) + +pytestmark = pytest.mark.skipif( + not BUNDLE.is_dir(), reason=f"no sensor export at {BUNDLE}" +) + + +@pytest.fixture(scope="module") +def bundle(): + return load_bundle(BUNDLE) + + +@pytest.fixture(scope="module") +def flat(bundle): + return rasterise(bundle, size_hw=DEFAULT_FLAT_SIZE) + + +@pytest.fixture +def panel(qt_app, bundle, flat): + widget = FingerPanel(stack=PlatformStack()) + widget.set_bundle(bundle, flat) + yield widget + widget.deleteLater() + + +def _first_surface_pixel(flat) -> tuple[int, int]: + """A pixel that carries surface, found rather than assumed.""" + for y in range(0, flat.normals.shape[0], 7): + for x in range(0, flat.normals.shape[1], 7): + if flat.sample(x, y) is not None: + return x, y + raise AssertionError("the flat raster has no surface pixels at all") + + +def _shrink_raster(monkeypatch, window_mod, size=(256, 256)): + """Force a small flat raster for a test that is not about the raster. + + ``DEFAULT_FLAT_SIZE`` is bound as a default argument, so patching the module + attribute would not reach it; wrapping the function does. + """ + real = window_mod.rasterise + + def small(bundle, flat_size=None, **kwargs): + return real(bundle, size, **kwargs) + + monkeypatch.setattr(window_mod, "rasterise", small) + + +def test_the_window_loads_the_remembered_bundle_at_startup(qt_app, monkeypatch): + """A filled-in path over two empty views reads as a broken screen.""" + import shutil + + from orisys_app_shared import paths as paths_mod + from orisys_app_shared import prefs as prefs_pkg + from orisys_app_shared.i18n import set_locale + + from orisys_autocal import preferences as prefs_mod + from orisys_autocal.ui import window as window_mod + from orisys_autocal.ui.window import AutoCalMainWindow + + scratch = Path(__file__).resolve().parents[1] / "outputs" / "_test_scratch" / "autoload" + scratch.mkdir(parents=True, exist_ok=True) + prefs_file = scratch / "preference.json" + monkeypatch.setattr(paths_mod, "preference_path", lambda: prefs_file) + monkeypatch.setattr(prefs_pkg.store, "preference_path", lambda: prefs_file) + monkeypatch.setattr(prefs_mod, "preference_path", lambda: prefs_file) + _shrink_raster(monkeypatch, window_mod) + + prefs = prefs_mod.default_preferences() + prefs["finger_bundle_path"] = str(BUNDLE) + set_locale(str(prefs.get("language") or "zh_CN")) + + window = AutoCalMainWindow(prefs=prefs) + try: + assert window.finger_panel.view.flat is not None, "the startup load did not happen" + pixmap = window.finger_panel.view.pixmap() + assert pixmap is not None and not pixmap.isNull() + finally: + window.poller.shutdown() + window.close() + shutil.rmtree(scratch, ignore_errors=True) + + +def test_a_missing_remembered_bundle_is_survivable(qt_app, monkeypatch, tmp_dir): + """The normal case on a machine that has never exported one.""" + from orisys_autocal.ui import window as window_mod + + _shrink_raster(monkeypatch, window_mod) + window = window_mod.AutoCalMainWindow( + prefs={ + "language": "en_US", + "theme": "light", + "finger_bundle_path": str(tmp_dir / "not-exported"), + } + ) + try: + assert window.finger_panel.view.flat is None + # And the panel still says what to do rather than looking dead. + assert window.finger_panel.status_label.text() + finally: + window.poller.shutdown() + window.close() + + +def test_the_preview_legend_is_a_label_not_gl_text(qt_app): + """Painting a long legend over the GL surface is what made it unreadable.""" + widget = FingerPanel(stack=PlatformStack()) + try: + assert widget.legend_label.text() + assert widget.legend_label.wordWrap() + finally: + widget.deleteLater() + + +# ------------------------------------------------- the initial view and axes + + +def test_the_initial_view_is_the_commissioned_orientation(qt_app, bundle): + """Sensor +Y up the screen and sensor +Z toward the viewer, by construction. + + The user asked for this state to be the configuration the screen opens in, so + it is asserted in screen terms rather than against the raw angles: what matters + is where the axes land in the picture. Widget y grows *downward*, so "up the + screen" is a **negative** pixel offset — the sign that this test exists to get + right, because getting it wrong draws the tool axis pointing down. + """ + from orisys_autocal.sensor import to_sensor + + from orisys_autocal.ui.pose_preview import PosePreview + + preview = PosePreview() + try: + preview.resize(600, 400) + preview.set_bundle(bundle) + preview.initial_view() + + stack = PlatformStack() + origin = preview.project(bundle.centre_mm) + assert origin is not None + + # Platform +Z is sensor +Y on this mount: it must read as up the screen. + platform_z = to_sensor(np.array([0.0, 0.0, 1.0]), stack.axis_map) + tip = preview.project(bundle.centre_mm + platform_z * 11.8) + assert tip is not None + assert tip[1] < origin[1] - 5.0, ( + f"sensor +Y must point up the screen, but moved {tip[1] - origin[1]:+.1f} px" + ) + + # Sensor +Z is the sensing face's own axis and must face the viewer. It is + # the platform's -Y on this mount. ``forward`` points from the eye into the + # scene, so a surface normal aimed *at* the camera has a positive dot with + # it — the opposite of what the depth term in a projection does, and the + # sign this assertion got wrong first time. + eye, _distance = preview._camera() # noqa: SLF001 + _centre, forward, _right, _up = preview._view_basis(eye) # noqa: SLF001 + lens_axis = to_sensor(np.array([0.0, -1.0, 0.0]), stack.axis_map) + lens_axis = lens_axis / np.linalg.norm(lens_axis) + assert float(lens_axis @ forward) > 0.2, ( + f"sensor +Z must face the viewer, but points away: " + f"{float(lens_axis @ forward):+.3f}" + ) + finally: + preview.deleteLater() + + +def test_orbiting_and_then_resetting_returns_to_the_initial_view(qt_app, bundle): + from orisys_autocal.ui.pose_preview import ( + INITIAL_PITCH_RAD, + INITIAL_YAW_RAD, + PosePreview, + ) + + preview = PosePreview() + try: + preview.set_bundle(bundle) + preview._yaw = 1.9 # noqa: SLF001 - pretend the operator dragged + preview._pitch = -0.4 # noqa: SLF001 + preview._distance_scale = 3.0 # noqa: SLF001 + + preview.reset_view() + + assert preview._yaw == pytest.approx(INITIAL_YAW_RAD) # noqa: SLF001 + assert preview._pitch == pytest.approx(INITIAL_PITCH_RAD) # noqa: SLF001 + assert preview._distance_scale == pytest.approx(1.0) # noqa: SLF001 + finally: + preview.deleteLater() + + +def test_the_motion_axes_can_be_turned_off(qt_app, bundle): + from orisys_autocal.ui.pose_preview import PosePreview + + preview = PosePreview() + try: + assert preview.show_axes + preview.set_show_axes(False) + assert not preview.show_axes + preview.set_show_axes(True) + assert preview.show_axes + finally: + preview.deleteLater() + + +def test_the_axis_labels_project_onto_their_arrows(qt_app, bundle): + """The 2D label and the 3D arrow must agree, or the label names nothing.""" + from orisys_autocal.sensor import to_sensor + + from orisys_autocal.ui.pose_preview import PosePreview + + preview = PosePreview() + try: + preview.resize(600, 400) + preview.set_bundle(bundle) + stack = PlatformStack() + origin = bundle.centre_mm + extent = float(np.linalg.norm(bundle.extent_mm)) or 1.0 + + positions = [] + for axis in ("x", "y", "z"): + direction = np.zeros(3) + direction[("x", "y", "z").index(axis)] = 1.0 + tip = origin + to_sensor(direction, stack.axis_map) * extent * 0.45 + screen = preview.project(tip) + assert screen is not None, f"the {axis} axis tip is behind the camera" + assert np.isfinite(screen).all() + positions.append(screen) + + # Three axes pointing different ways must not project onto one another. + rounded = {(round(x, 3), round(y, 3)) for x, y in positions} + assert len(rounded) == 3 + + # The arrow tips can fall outside a small widget at the default zoom, so + # the overlay clamps the label back inside; that is what keeps it visible. + clamped = [ + ( + min(max(x, 12.0), preview.width() - 12.0), + min(max(y, 14.0), preview.height() - 6.0), + ) + for x, y in positions + ] + for x, y in clamped: + assert 0 <= x <= preview.width() + assert 0 <= y <= preview.height() + finally: + preview.deleteLater() + + +def test_a_point_behind_the_camera_has_no_screen_position(qt_app, bundle): + """Drawing a label for it would put a stray letter on the widget.""" + from orisys_autocal.ui.pose_preview import PosePreview + + preview = PosePreview() + try: + preview.set_bundle(bundle) + assert preview.project(bundle.centre_mm) is not None + finally: + preview.deleteLater() + + +def test_the_flat_image_is_shown_whole_not_cropped(panel): + """At native size the 1024 image was cropped to a corner of the UV space.""" + view = panel.view + view.resize(240, 300) + view.set_flat(view.flat) + + shown = view.pixmap() + assert shown is not None and not shown.isNull() + assert shown.width() <= view.width() + assert shown.height() <= view.height() + # Square image, square display: the aspect ratio is preserved, not squashed. + assert shown.width() == pytest.approx(shown.height(), abs=2) + + +def test_a_click_on_the_scaled_image_maps_to_the_pixel_under_it(panel): + """Scaling must not move the mapping; a click in the corner is the corner.""" + view = panel.view + view.resize(240, 300) + view.set_flat(view.flat) + shown = view.pixmap() + offset_x = (view.width() - shown.width()) / 2.0 + offset_y = (view.height() - shown.height()) / 2.0 + + centre = view.pixel_at(QPointF(offset_x + shown.width() / 2.0, offset_y + shown.height() / 2.0)) + assert centre is not None + assert centre[0] == pytest.approx(view.flat.width // 2, abs=3) + assert centre[1] == pytest.approx(view.flat.height // 2, abs=3) + + corner = view.pixel_at( + QPointF(offset_x + shown.width() - 1, offset_y + shown.height() - 1) + ) + assert corner is not None + # One display pixel covers several image pixels at this scale (240 vs 1024), + # so the last clickable pixel is within a scale-step of the true corner. + step = view.flat.width / shown.width() + 1 + assert corner[0] > view.flat.width - step + assert corner[1] > view.flat.height - step + + assert view.pixel_at(QPointF(offset_x - 5.0, offset_y + shown.height() / 2)) is None + + +# ------------------------------------------------------- the motion simulation + + +def test_the_play_button_needs_a_picked_point(panel): + """A button that silently does nothing is worse than a disabled one.""" + assert not panel.preview.can_simulate + assert not panel.sim_button.isEnabled() + + flat = panel.view.flat + row = panel.handle_pick(*_first_surface_pixel(flat)) + + assert row is not None + assert panel.preview.can_simulate + assert panel.sim_button.isEnabled() + + +def test_clearing_the_points_disables_the_simulation_again(panel): + panel.handle_pick(*_first_surface_pixel(panel.view.flat)) + panel.clear_points() + assert not panel.preview.can_simulate + assert not panel.sim_button.isEnabled() + + +def _run_simulation(preview) -> None: + """Drive the timer by hand, so the test does not sleep.""" + from orisys_autocal.ui.pose_preview import SIM_DURATION_MS, SIM_FRAME_MS + + preview.stop_simulation() + preview.start_simulation() + for _ in range(int(SIM_DURATION_MS / SIM_FRAME_MS) + 2): + preview._sim_tick() # noqa: SLF001 - stepping the animation deterministically + + +def test_playing_the_motion_puts_the_normal_on_the_tool_axis(qt_app, bundle): + """The two invariants the whole animation exists to show. + + After playback the picked normal must lie along the tool axis, and the contact + must sit at the platform coordinates the readouts report. Asserting the second + against ``target_xy_mm`` rather than the origin is the point: the tables move to + the solved X/Y, and every earlier attempt at this transform got a frame wrong, + so it is checked against the solver's own output instead of a bare (0, 0, 0). + """ + from orisys_autocal.ui.pose_preview import PosePreview + + stack = PlatformStack() + tool_local = to_sensor(np.asarray(stack.tool_direction, dtype=float), stack.axis_map) + tool_local = tool_local / np.linalg.norm(tool_local) + + preview = PosePreview() + try: + preview.set_bundle(bundle) + normals = bundle.vertex_normals() + worst_angle = 0.0 + for index in (0, 1, 7, 40, 200): + normal = normals[index] + point = bundle.vertices_mm[index] + pose = stack.solve(point, normal) + + preview.set_point(point, normal) + preview.set_pose(pose) + _run_simulation(preview) + rotation, translation = preview._sim_transform() # noqa: SLF001 + + aligned = rotation @ (normal / np.linalg.norm(normal)) + aligned = aligned / np.linalg.norm(aligned) + worst_angle = max( + worst_angle, + float(np.degrees(np.arccos(np.clip(float(aligned @ tool_local), -1.0, 1.0)))), + ) + + # The contact ends at the tool, which is drawn at the patch's centre. + # Asserting the *absolute* platform X/Y here would be asserting the old + # behaviour: moving the model to its machine coordinates instead of to + # the tool put it off screen, because the camera looks at the tool. + landed = rotation @ point + translation + assert np.allclose(landed, bundle.centre_mm, atol=1e-6) + # And the marker does not take that translation: it is the measurement, + # so the body slides out from under it. + assert np.allclose( + preview._placed_contact(point), rotation @ point # noqa: SLF001 + ) + + assert worst_angle < 0.5, ( + f"after playing, a normal is still {worst_angle:.3f} deg off the tool axis" + ) + finally: + preview.stop_simulation() + preview.deleteLater() + + +def test_the_simulation_rotates_first_and_then_translates(qt_app, bundle): + """The bench contract: stages settle before the table moves.""" + from orisys_autocal.ui.pose_preview import PosePreview + + stack = PlatformStack() + surface_normal = bundle.vertex_normals()[0] + point = bundle.vertices_mm[0] + pose = stack.solve(point, surface_normal) + + preview = PosePreview() + try: + preview.set_bundle(bundle) + preview.set_point(point, surface_normal) + preview.set_pose(pose) + + # Mid-rotation: the body has turned and the table has not moved yet. + preview.start_simulation() + for _ in range(4): + preview._sim_tick() # noqa: SLF001 + rotation_share, translation_share = preview.simulation_phases() # noqa: SLF001 + assert rotation_share > 0.0 + assert translation_share == 0.0 + rotation, translation = preview._sim_transform() # noqa: SLF001 + assert float(np.linalg.norm(translation)) == pytest.approx(0.0) + assert not np.allclose(rotation, np.eye(3)) + + # Finish: both phases complete. + _run_simulation(preview) + rotation_share, translation_share = preview.simulation_phases() # noqa: SLF001 + assert rotation_share == pytest.approx(1.0) + assert translation_share == pytest.approx(1.0) + finally: + preview.stop_simulation() + preview.deleteLater() + + +def test_the_simulation_stops_and_can_be_replayed(qt_app, bundle): + from orisys_autocal.ui.pose_preview import PosePreview + + stack = PlatformStack() + point = bundle.vertices_mm[0] + pose = stack.solve(point, bundle.vertex_normals()[0]) + preview = PosePreview() + try: + preview.set_bundle(bundle) + preview.set_point(point, bundle.vertex_normals()[0]) + preview.set_pose(pose) + + preview.start_simulation() + assert preview.simulating + preview.stop_simulation() + assert not preview.simulating + # Stopped means back at the start, not frozen mid-move. + rotation_share, translation_share = preview.simulation_phases() # noqa: SLF001 + assert rotation_share == 0.0 and translation_share == 0.0 + + _run_simulation(preview) + assert not preview.simulating + preview.start_simulation() + assert preview.simulating + finally: + preview.stop_simulation() + preview.deleteLater() + + +def test_the_simulation_cannot_start_without_a_pose(qt_app, bundle): + from orisys_autocal.ui.pose_preview import PosePreview + + preview = PosePreview() + try: + preview.set_bundle(bundle) + assert not preview.can_simulate + preview.start_simulation() + assert not preview.simulating + assert preview._sim_transform() is None # noqa: SLF001 + finally: + preview.deleteLater() + + +def test_a_pick_returns_the_row_it_also_shows(panel): + flat = panel.view.flat + x, y = _first_surface_pixel(flat) + + row = panel.handle_pick(x, y) + + assert row is not None + assert panel.point_count() == 1 + # What the table shows is what the caller was handed; a panel that rendered + # one thing and returned another would make an export silently wrong. + stored = panel.points()[0] + for key, value in row.items(): + assert stored[key] == pytest.approx(value) + + +def test_a_pick_carries_a_pose_that_aligns(panel): + flat = panel.view.flat + x, y = _first_surface_pixel(flat) + + row = panel.handle_pick(x, y) + + assert row["residual"] < 1.0 + # The platform X/Y must be the contact point the solver reported, so a + # plausible-looking origin here would mean the frames were dropped. + point, _normal, _face = flat.sample(x, y) + assert not np.allclose([row["x"], row["y"], row["z"]], 0.0) + assert abs(row["x"] - point[0]) < 1e-9 + + +def test_a_gap_pixel_is_refused_with_a_reason(panel): + """A UV-island gap must not become a calibration point at the origin.""" + flat = panel.view.flat + gap = None + for y in range(flat.normals.shape[0]): + for x in range(flat.normals.shape[1]): + if flat.sample(x, y) is None: + gap = (x, y) + break + if gap is not None: + break + assert gap is not None, "the raster has no gap pixels to test the refusal with" + + assert panel.handle_pick(*gap) is None + assert panel.point_count() == 0 + assert panel.status_label.text() + + +def test_picking_with_no_bundle_says_so(qt_app): + widget = FingerPanel(stack=PlatformStack()) + try: + assert widget.handle_pick(10, 10) is None + assert widget.point_count() == 0 + assert widget.status_label.text() + finally: + widget.deleteLater() + + +def test_clearing_points_empties_the_table(panel): + flat = panel.view.flat + x, y = _first_surface_pixel(flat) + panel.handle_pick(x, y) + assert panel.point_count() == 1 + + panel.clear_points() + + assert panel.point_count() == 0 + assert panel.points() == [] + + +def test_the_plate_thickness_reaches_the_solve(panel): + """The mounting-plate entry has to reach the solve, or it is decoration. + + The plate translates the sensor along the platform's tool axis, so it moves + the contact point's Z and leaves its X/Y alone. Asserting the X/Y moved would + be asserting the wrong model; asserting Z moved catches a plate field that is + read but dropped — which is what it did before ``contact_z_mm`` existed. + """ + flat = panel.view.flat + x, y = _first_surface_pixel(flat) + point, normal, _face = flat.sample(x, y) + + thin = panel.stack.solve(point, normal, plate_thickness_mm=0.0) + thick = panel.stack.solve(point, normal, plate_thickness_mm=4.0) + + assert thick.contact_z_mm - thin.contact_z_mm == pytest.approx(4.0) + assert thick.target_xy_mm == pytest.approx(thin.target_xy_mm) + # Picking through the panel uses the spin box, so the two must agree. + panel.clear_points() + panel.set_plate_thickness(0.0) + thin_row = panel.handle_pick(x, y) + panel.clear_points() + panel.set_plate_thickness(4.0) + thick_row = panel.handle_pick(x, y) + assert thin_row is not None and thick_row is not None + assert thick_row["residual"] == pytest.approx(thin_row["residual"], abs=1e-6) + assert thick_row["Z"] - thin_row["Z"] == pytest.approx(4.0) + assert thick_row["X"] == pytest.approx(thin_row["X"]) + assert thick_row["Y"] == pytest.approx(thin_row["Y"]) + + +def test_the_readouts_name_the_frame_mapping(panel): + """The one line the bench will want to check when x and y are interchanged.""" + text = panel._readout_labels["frames"].text() # noqa: SLF001 + assert "x" in text and "z" in text + assert "-y" in text or "−y" in text + + +def test_the_preview_refuses_to_draw_without_a_context(qt_app): + from orisys_autocal.ui.pose_preview import PosePreview + + preview = PosePreview() + try: + assert not preview.has_context() + # Rendering must be a no-op, not a crash: Qt calls this during teardown + # on some platforms. + preview.repaint() + # And the widget must be able to say why it has no picture, rather than + # leaving a black rectangle unexplained. + preview.set_bundle(None) + assert preview.problem + finally: + preview.deleteLater() + + +def test_the_panel_shows_why_the_preview_is_blank(qt_app): + """A blank preview with no reason reads as a broken screen.""" + widget = FingerPanel(stack=PlatformStack()) + try: + widget.preview.set_bundle(None) + widget._refresh_reach() # noqa: SLF001 + if not widget.preview.has_context(): + assert widget.reach_label.text() + finally: + widget.deleteLater() diff --git a/apps/autocal/tests/test_kinematics.py b/apps/autocal/tests/test_kinematics.py new file mode 100644 index 0000000..e218120 --- /dev/null +++ b/apps/autocal/tests/test_kinematics.py @@ -0,0 +1,404 @@ +"""The simulated 5-axis solve: residuals, the stage envelope, and the frames. + +The solver is the part of this feature that is easiest to get wrong and hardest +to notice: a wrong derivation still returns two plausible angles. So the central +test here does not check a formula — it brute-forces the stage envelope and +requires the solver to be no worse than the best pose that exists. + +That check is what caught the four wrong versions before it: rotating the +sensor-frame normal instead of the platform-frame one, composing the stages in +the wrong order, a seeded search that skipped the right basin, and a sweep that +ranked yaws by the wrong quantity and so reported a quarter of the patch as +unreachable when all of it aligns. +""" + +from __future__ import annotations + +import numpy as np +import pytest + +from orisys_autocal.motion.kinematics import ( + ALIGNED_TOL_DEG, + PlatformStack, + Pose, + residual_for, + rotation_about, +) +from orisys_autocal.sensor.frames import ( + AXIS_MAP, + describe, + mount_rotation, + to_platform, + to_sensor, +) + +#: Normals to exercise, including the awkward ones. +NORMALS = ( + np.array([0.0, 1.0, 0.0]), # the patch's nominal normal + np.array([0.0, -1.0, 0.0]), # straight into the tool + np.array([1.0, 0.0, 0.0]), + np.array([0.0, 0.0, 1.0]), + np.array([0.3, 0.5, -0.81]), + np.array([-0.6, 0.2, 0.77]), + np.array([0.0, 0.3, 0.954]), +) + + +def _brute_force_best( + stack: PlatformStack, unit_platform: np.ndarray, *, step: float = 0.25 +) -> float: + """The lowest residual any pose in the envelope achieves — by search.""" + tool = stack.tool_direction + yaw_limit = min(180.0, stack.yaw_limit_deg) + pitch_limit = min(360.0, stack.pitch_limit_deg) + best = 1e9 + for yaw in np.arange(-yaw_limit, yaw_limit + step, step): + turned = rotation_about(stack.yaw_axis, float(yaw)) @ unit_platform + for pitch in np.arange(-pitch_limit, pitch_limit + step, step): + rotated = rotation_about(stack.pitch_axis, float(pitch)) @ turned + residual = float(np.degrees(np.arccos(np.clip(rotated @ tool, -1.0, 1.0)))) + best = min(best, residual) + return best + + +# ------------------------------------------------------------------- the mount + + +def test_the_axis_map_is_a_rotation(): + """A mapping that mirrors the part is worse than none: it looks fine.""" + matrix = mount_rotation() + assert float(np.linalg.det(matrix)) == pytest.approx(1.0) + assert np.allclose(matrix @ matrix.T, np.eye(3)) + + +def test_the_sensor_y_maps_to_the_platform_z(): + """The bench's frame relation, stated once so it can be corrected once.""" + assert np.allclose(to_platform(np.array([0.0, 1.0, 0.0])), [0.0, 0.0, 1.0]) + + +def test_the_sensor_x_maps_to_the_platform_x(): + assert np.allclose(to_platform(np.array([1.0, 0.0, 0.0])), [1.0, 0.0, 0.0]) + + +def test_the_mount_round_trips(): + vector = np.array([0.3, -0.5, 0.81]) + assert np.allclose(to_sensor(to_platform(vector)), vector) + + +def test_a_mirroring_map_is_refused(): + """A map that reflects the part is worse than none: the numbers look sane.""" + with pytest.raises(ValueError) as excinfo: + mount_rotation({"x": "x", "y": "y", "z": "-z"}) # det = -1 + assert "not a rotation" in str(excinfo.value) + + +def test_an_incomplete_map_is_refused(): + with pytest.raises(ValueError) as excinfo: + mount_rotation({"x": "x", "y": "z"}) + assert "no entry" in str(excinfo.value) + + +def test_the_mapping_is_described_for_the_ui(): + assert describe() == "sensor x->x, y->z, z->-y" + + +def test_swapping_the_mapping_changes_the_answer(): + """The one-line edit the bench may need, proven to actually do something.""" + # Asymmetric on purpose: a point on a coordinate axis maps to the same place + # under both rotations, which would make this test pass without the mapping + # doing anything at all. + point = np.array([1.0, 5.0, 2.0]) + default = PlatformStack() + # A different, still right-handed mapping: sensor x->y, y->z, z->x. + swapped = PlatformStack(axis_map={"x": "y", "y": "z", "z": "x"}) + assert float(np.linalg.det(swapped.mount)) == pytest.approx(1.0) + assert not np.allclose( + default.point_to_platform(point), swapped.point_to_platform(point) + ) + + +# --------------------------------------------------------------- the solving + + +def test_the_bench_envelope_is_the_default(): + stack = PlatformStack() + assert stack.yaw_limit_deg == 90.0 + assert stack.pitch_limit_deg == 360.0 + + +@pytest.mark.parametrize("normal", NORMALS) +def test_the_solver_is_never_worse_than_a_brute_force_search(normal): + """The test that pins the derivation down.""" + stack = PlatformStack() + unit = stack.normal_to_platform(normal) + unit = unit / np.linalg.norm(unit) + pose = stack.solve(np.array([0.0, 5.0, 0.0]), normal) + best = _brute_force_best(stack, unit, step=0.5) + assert pose.residual_deg <= best + 0.05, ( + f"the solver returned {pose.residual_deg:.3f} deg where " + f"{best:.3f} deg is achievable" + ) + + +# --------------------------------------------- the fast sweep vs the full grid + + +def _random_units(count: int, seed: int) -> list[np.ndarray]: + generator = np.random.default_rng(seed) + vectors = generator.normal(size=(count, 3)) + return [vector / np.linalg.norm(vector) for vector in vectors] + + +def test_the_fast_sweep_agrees_with_the_full_grid_sweep(): + """The closed-form sweep must find what the exhaustive grid finds. + + The shipped search solves the best pitch at each yaw in closed form, which is + what made a full patch 14x faster. The exhaustive grid is kept as the slow + reference precisely so this comparison exists: an optimisation in this file has + silently cost 68 degrees before, while looking entirely reasonable. + """ + stack = PlatformStack() + for unit in _random_units(12, seed=20240517)[:6]: + fast_yaw, fast_pitch, fast_residual = stack._search_alignment( # noqa: SLF001 + unit, stack.tool_direction + ) + ref_yaw, ref_pitch, ref_residual = stack.search_alignment_reference(unit, step=5.0) + # The reference is quantised to the sweep step and is not polished, so it + # may be slightly worse; it must never be better than the shipped path. + assert fast_residual <= ref_residual + 1e-9, ( + f"fast sweep left {fast_residual:.6f} deg where the full grid found " + f"{ref_residual:.6f} deg (fast {fast_yaw:.3f}/{fast_pitch:.3f}, " + f"reference {ref_yaw:.3f}/{ref_pitch:.3f})" + ) + # And the polished answer should be far better than the grid's own step. + assert fast_residual <= ref_residual + 0.05 + + +def test_the_closed_form_pitch_beats_a_direct_scan(): + """Guard the sign convention the closed form depends on. + + There are two cancellation-free coefficients and picking the wrong one returns + the antipode: a pitch 180 degrees away that still looks like a number. This is + the check that caught exactly that, so it is stated directly against a numeric + scan of the pitch rather than against the rest of the solver. + """ + stack = PlatformStack() + for unit in _random_units(6, seed=99): + yaws = np.array([-80.0, -35.0, -10.0, 0.0, 15.0, 47.0, 90.0]) + closed_form = stack._best_pitch_per_yaw(unit, yaws) # noqa: SLF001 + for yaw, pitch in zip(yaws, closed_form): + scan = np.arange(-360.0, 360.0, 0.1) + yawed = stack._turn(stack.yaw_axis, np.array([float(yaw)]), unit)[0] # noqa: SLF001 + # The residual is monotone in the dot product, so comparing the dot is + # enough and avoids a million arccos calls. + tool = stack.tool_direction + cosine, sine = np.cos(np.radians(scan)), np.sin(np.radians(scan)) + stationary, first, second, sign_first, sign_second = ( + stack._rotation_coefficients(stack.pitch_axis) # noqa: SLF001 + ) + dot = ( + yawed[stationary] * tool[stationary] + + (yawed[first] * cosine + sign_first * yawed[second] * sine) * tool[first] + + (sign_second * yawed[first] * sine + yawed[second] * cosine) * tool[second] + ) + achieved = stack._cell(unit, float(yaw), float(pitch)) # noqa: SLF001 + achieved_dot = float(np.dot(achieved, tool)) + assert achieved_dot >= float(dot.max()) - 1e-9, ( + f"closed-form pitch {pitch:.3f} deg at yaw {yaw:.1f} reaches dot " + f"{achieved_dot:.6f}, but a direct scan reaches {dot.max():.6f}" + ) + + +def _independent_residual( + stack: PlatformStack, normal_sensor: np.ndarray, yaw_deg: float, pitch_deg: float +) -> float: + """The residual worked out from raw matrices built here, not by the solver. + + ``residual_for`` shares ``rotation_about`` with the solver, so it cannot catch + an error in that shared helper. This builds the two 3x3 matrices from the + rotation formulas directly, so the two paths agree only if both are right. + """ + normal = stack.normal_to_platform(np.asarray(normal_sensor, dtype=np.float64)) + unit = normal / np.linalg.norm(normal) + + yaw, pitch = np.radians(yaw_deg), np.radians(pitch_deg) + about_yaw = np.array( + [ + [np.cos(yaw), -np.sin(yaw), 0.0], + [np.sin(yaw), np.cos(yaw), 0.0], + [0.0, 0.0, 1.0], + ] + ) + about_pitch = np.array( + [ + [np.cos(pitch), 0.0, np.sin(pitch)], + [0.0, 1.0, 0.0], + [-np.sin(pitch), 0.0, np.cos(pitch)], + ] + ) + turned = about_pitch @ about_yaw @ unit + return float( + np.degrees(np.arccos(np.clip(float(turned @ stack.tool_direction), -1.0, 1.0))) + ) + + +@pytest.mark.parametrize("normal", NORMALS) +def test_a_returned_pose_actually_achieves_its_residual(normal): + """Independent of how it was found: apply the angles and measure.""" + stack = PlatformStack() + pose = stack.solve(np.array([0.0, 5.0, 0.0]), normal) + measured = residual_for(stack, normal, pose.yaw_deg, pose.pitch_deg) + assert measured == pytest.approx(pose.residual_deg, abs=1e-6) + + +@pytest.mark.parametrize("normal", NORMALS) +def test_the_residual_survives_an_independently_built_rotation(normal): + """The shared rotation helper cannot be the only thing agreeing with itself.""" + stack = PlatformStack() + pose = stack.solve(np.array([0.0, 5.0, 0.0]), normal) + measured = _independent_residual(stack, normal, pose.yaw_deg, pose.pitch_deg) + assert measured == pytest.approx(pose.residual_deg, abs=1e-6) + + +def test_every_sampled_patch_normal_is_reachable_within_the_yaw_limit(): + """The whole patch aligns, and the yaw limit is not what stops it. + + An earlier revision of this solver claimed a quarter of the patch was + unreachable at 65 deg. That was the solver's bug. The real answer is that the + 360 deg pitch stage does the work and the +-90 deg yaw limit costs at most a + quarter of a degree anywhere on the patch, so no re-mount is needed. + """ + stack = PlatformStack() + worst = 0.0 + for normal in NORMALS: + pose = stack.solve(np.array([0.0, 5.0, 0.0]), normal) + assert pose.ok + worst = max(worst, pose.residual_deg) + assert worst < 0.5, f"worst residual across the sampled patch is {worst:.3f} deg" + + +def test_a_generic_normal_aligns_almost_exactly(): + """Away from the yaw clamp the solver should reach the polish floor.""" + stack = PlatformStack() + for normal in NORMALS[:6]: + pose = stack.solve(np.array([0.0, 5.0, 0.0]), normal) + assert pose.residual_deg < 0.01, ( + f"normal {normal} left {pose.residual_deg:.4f} deg behind" + ) + + +def test_the_nominal_normal_needs_no_rotation(): + """Sensor +Y is the platform +Z, so the patch's centre is already aligned.""" + stack = PlatformStack() + pose = stack.solve(np.array([0.0, 5.0, 0.0]), np.array([0.0, 1.0, 0.0])) + assert pose.ok + assert pose.residual_deg == pytest.approx(0.0, abs=1e-6) + + +def test_a_normal_straight_into_the_tool_is_flagged_indeterminate(): + stack = PlatformStack() + pose = stack.solve(np.array([0.0, 5.0, 0.0]), np.array([0.0, -1.0, 0.0])) + assert pose.ok + assert not pose.yaw_determined + assert "indeterminate" in pose.reason + + +def test_a_degenerate_normal_is_refused(): + stack = PlatformStack() + pose = stack.solve(np.array([0.0, 5.0, 0.0]), np.zeros(3)) + assert not pose.ok + assert "degenerate" in pose.reason + + +def test_the_yaw_limit_is_respected(): + stack = PlatformStack(yaw_limit_deg=90.0) + for normal in NORMALS: + pose = stack.solve(np.array([0.0, 5.0, 0.0]), normal) + assert abs(pose.yaw_deg) <= 90.0 + 1e-6 + + +def test_the_pitch_limit_is_respected(): + stack = PlatformStack(pitch_limit_deg=45.0) + for normal in NORMALS: + pose = stack.solve(np.array([0.0, 5.0, 0.0]), normal) + assert abs(pose.pitch_deg) <= 45.0 + 1e-6 + + +def test_tightening_the_envelope_can_only_leave_more_residual(): + """A narrower stage cannot align better than a wider one.""" + normal = np.array([-0.6, 0.2, 0.77]) + point = np.array([0.0, 5.0, 0.0]) + wide = PlatformStack(yaw_limit_deg=180.0).solve(point, normal).residual_deg + narrow = PlatformStack(yaw_limit_deg=30.0).solve(point, normal).residual_deg + assert narrow >= wide - 1e-6 + + +def test_an_unbounded_yaw_can_align_arbitrary_normals(): + """The envelope, not the solver, is what leaves residual.""" + stack = PlatformStack(yaw_limit_deg=180.0, pitch_limit_deg=180.0) + worst = 0.0 + for index in range(40): + normal = np.random.default_rng(index).normal(size=3) + normal = normal / np.linalg.norm(normal) + worst = max(worst, stack.solve(np.array([0.0, 5.0, 0.0]), normal).residual_deg) + assert worst < 0.5, f"worst residual {worst:.3f} deg with a free yaw" + + +def test_the_reported_xy_is_the_contact_point_not_the_origin(): + stack = PlatformStack() + point = np.array([2.0, 5.0, 3.0]) + pose = stack.solve(point, np.array([0.0, 1.0, 0.0])) + platform = stack.point_to_platform(point) + assert pose.target_xy_mm[0] == pytest.approx(platform[0]) + assert pose.target_xy_mm[1] == pytest.approx(platform[1]) + + +def test_the_plate_lifts_the_point_along_the_tool_axis(): + stack = PlatformStack() + point = np.array([1.0, 5.0, 2.0]) + plain = stack.point_to_platform(point) + lifted = stack.point_to_platform(point, plate_thickness_mm=2.5) + assert np.allclose(lifted - plain, [0.0, 0.0, 2.5]) + + +def test_the_tool_offset_shifts_the_reported_point(): + stack = PlatformStack(tool_offset_mm=(1.0, 2.0, 3.0)) + point = np.array([0.0, 5.0, 0.0]) + pose = stack.solve(point, np.array([0.0, 1.0, 0.0])) + plain = PlatformStack().solve(point, np.array([0.0, 1.0, 0.0])) + assert pose.target_xy_mm[0] == pytest.approx(plain.target_xy_mm[0] + 1.0) + assert pose.target_xy_mm[1] == pytest.approx(plain.target_xy_mm[1] + 2.0) + + +def test_the_approach_sign_is_validated(): + with pytest.raises(ValueError): + PlatformStack(approach_sign=0) + + +def test_swapping_the_approach_sign_changes_the_target(): + """The sign is the difference between pressing in and pulling away.""" + normal = np.array([0.0, 1.0, 0.0]) + point = np.array([0.0, 5.0, 0.0]) + inward = PlatformStack(approach_sign=-1).solve(point, normal) + outward = PlatformStack(approach_sign=1).solve(point, normal) + assert inward.residual_deg == pytest.approx(0.0, abs=1e-6) + assert outward.pitch_deg != pytest.approx(inward.pitch_deg) + + +def test_a_pose_describes_itself(): + pose = Pose(ok=True, target_xy_mm=(1.0, 2.0), yaw_deg=3.0, pitch_deg=4.0, residual_deg=0.01) + text = pose.describe() + assert "yaw" in text and "pitch" in text and "residual" in text + assert Pose(ok=False, reason="nope").describe() == "no pose: nope" + + +def test_an_aligned_pose_reports_that_it_reached_tolerance(): + aligned = Pose(ok=True, residual_deg=0.0) + assert aligned.reached_tolerance and aligned.reachable + # ``reachable`` is the coarser statement: within half a degree counts as + # "the machine can put the tool on this normal". + loose = Pose(ok=True, residual_deg=ALIGNED_TOL_DEG * 10) + assert not loose.reached_tolerance + assert loose.reachable + far = Pose(ok=True, residual_deg=30.0) + assert not far.reached_tolerance and not far.reachable diff --git a/apps/autocal/tests/test_sensor_bundle.py b/apps/autocal/tests/test_sensor_bundle.py new file mode 100644 index 0000000..5660fd6 --- /dev/null +++ b/apps/autocal/tests/test_sensor_bundle.py @@ -0,0 +1,226 @@ +"""The sensor export bundle: unit derivation, validation, and the flat view. + +The unit factor is the assertion that matters most. `export.py` writes vertices +in the sensor frame *without* converting the design unit, and the conversion +lives in `world_to_cam`'s linear block — so a consumer that assumes millimetres +is wrong by ~54x, and everything downstream (platform coordinates, press depths) +is wrong with it. These tests pin that down. + +The bundle used here is the bench-confirmed one. When it is not present the +tests skip rather than fail: this suite has to run on machines that do not have +the sensor-design checkout. +""" + +from __future__ import annotations + +import numpy as np +import pytest + +from orisys_autocal.sensor.export_bundle import ( + BundleError, + load_bundle, + mm_per_unit_from_pose, +) +from orisys_autocal.sensor.uv_flat import rasterise + +#: The bench-confirmed export, and the figures measured from it. +BUNDLE = r"D:\orisys_ws\orisys-sensor-design\build\export\finger\r01\compute" +EXPECTED_VERTICES = 1032 +EXPECTED_TRIANGLES = 1974 +EXPECTED_MM_PER_UNIT = 0.018388452316735537 +EXPECTED_EXTENT_MM = (15.0, 7.5, 15.81) + + +def _bundle(): + if not __import__("pathlib").Path(BUNDLE).is_dir(): + pytest.skip("the sensor-design export is not on this machine") + return load_bundle( + BUNDLE, + expected_vertices=EXPECTED_VERTICES, + expected_triangles=EXPECTED_TRIANGLES, + ) + + +@pytest.fixture(scope="module") +def bundle(): + return _bundle() + + +# ------------------------------------------------------------------- the scale + + +def test_the_unit_factor_is_derived_from_the_pose(bundle): + assert bundle.mm_per_unit == pytest.approx(EXPECTED_MM_PER_UNIT, rel=1e-9) + + +def test_the_physical_size_is_millimetres_not_mesh_units(bundle): + """The failure this whole module exists to catch: a 54x unit error.""" + extent = bundle.extent_mm + assert extent[0] == pytest.approx(EXPECTED_EXTENT_MM[0], abs=0.02) + assert extent[1] == pytest.approx(EXPECTED_EXTENT_MM[1], abs=0.02) + assert extent[2] == pytest.approx(EXPECTED_EXTENT_MM[2], abs=0.02) + + +def test_a_rigid_pose_means_one_millimetre_per_unit(): + assert mm_per_unit_from_pose(np.eye(4)) == pytest.approx(1.0) + + +def test_a_uniformly_scaled_pose_reports_that_scale(): + pose = np.eye(4) + pose[:3, :3] *= 0.5 + assert mm_per_unit_from_pose(pose) == pytest.approx(0.5) + + +def test_a_non_uniform_pose_is_refused(): + """A per-axis scale cannot be undone by one factor; guessing is worse.""" + pose = np.eye(4) + pose[0, 0] = 0.01 + pose[1, 1] = 0.02 + pose[2, 2] = 0.03 + with pytest.raises(BundleError) as excinfo: + mm_per_unit_from_pose(pose) + assert "uniform" in str(excinfo.value) + + +def test_a_mirrored_pose_is_refused(): + """A reflection would put every point on the wrong side of the sensor.""" + pose = np.eye(4) + pose[0, 0] = -1.0 + with pytest.raises(BundleError) as excinfo: + mm_per_unit_from_pose(pose) + assert "mirror" in str(excinfo.value).lower() + + +def test_a_degenerate_pose_is_refused(): + with pytest.raises(BundleError): + mm_per_unit_from_pose(np.zeros((4, 4))) + + +# ------------------------------------------------------------------- the mesh + + +def test_the_mesh_matches_the_design_profile(bundle): + assert bundle.vertices_mm.shape == (EXPECTED_VERTICES, 3) + assert bundle.faces.shape == (EXPECTED_TRIANGLES, 3) + assert bundle.uv_data.shape[0] == EXPECTED_TRIANGLES * 3 + assert bundle.loop_to_vertex.shape[0] == EXPECTED_TRIANGLES * 3 + + +def test_every_face_index_is_in_range(bundle): + assert int(bundle.faces.max()) < bundle.vertices_mm.shape[0] + assert int(bundle.faces.min()) >= 0 + assert int(bundle.loop_to_vertex.max()) < bundle.vertices_mm.shape[0] + + +def test_the_surface_area_is_physically_plausible(bundle): + """~356 mm^2 for a 15 x 7.5 x 15.8 mm patch: a unit error would show here.""" + assert bundle.surface_area_mm2 == pytest.approx(355.77, abs=1.0) + + +def test_the_stored_normals_are_coherent(bundle): + """Mixed winding would make 'the normal' meaningless.""" + normals = bundle.face_normals + centroid = bundle.vertices_mm.mean(axis=0) + centres = bundle.vertices_mm[bundle.faces].mean(axis=1) + outward = np.einsum("ij,ij->i", normals, centres - centroid) + assert float((outward > 0).mean()) > 0.95 + + +def test_a_missing_file_is_named(tmp_dir): + """A path that is not a directory at all says so, and names the path.""" + absent = tmp_dir / "no-such-revision" + with pytest.raises(BundleError) as excinfo: + load_bundle(absent) + assert "not a directory" in str(excinfo.value) + assert absent.name in str(excinfo.value) + + +def test_an_incomplete_bundle_names_what_is_missing(tmp_dir): + (tmp_dir / "mesh.npz").write_bytes(b"") + with pytest.raises(BundleError) as excinfo: + load_bundle(tmp_dir) + message = str(excinfo.value) + assert "housing.npz" in message and "world_to_cam.npy" in message + + +def test_a_count_mismatch_is_refused(bundle): + with pytest.raises(BundleError) as excinfo: + load_bundle(BUNDLE, expected_vertices=999) + assert "999" in str(excinfo.value) + assert str(EXPECTED_VERTICES) in str(excinfo.value) + + +# --------------------------------------------------------------- the flat view + + +@pytest.fixture(scope="module") +def flat(bundle): + return rasterise(bundle, (256, 256)) + + +def test_the_flat_image_is_not_blank(flat): + assert flat.image_rgb.shape == (256, 256, 3) + assert flat.image_rgb.dtype == np.uint8 + assert not bool((flat.image_rgb == 0).all()) + + +def test_a_reasonable_share_of_the_image_carries_surface(flat): + assert 0.5 < flat.valid_fraction < 0.9 + + +def test_a_corner_pixel_is_an_island_gap(flat): + """The atlas is a single island, so a corner is outside it.""" + assert flat.sample(0, 0) is None + assert flat.sample(flat.width - 1, flat.height - 1) is None + + +def test_an_out_of_range_pixel_is_reported_not_crashed(flat): + assert flat.sample(-1, 5) is None + assert flat.sample(5, flat.height + 10) is None + + +def test_every_covered_pixel_lies_on_its_recorded_face(bundle, flat): + """The lookup table must be the mesh, not an approximation of it.""" + covered = np.argwhere(flat.face_index >= 0) + step = max(1, len(covered) // 300) + worst = 0.0 + for row, column in covered[::step]: + face = int(flat.face_index[row, column]) + triangle = bundle.vertices_mm[bundle.faces[face]] + normal = np.cross(triangle[1] - triangle[0], triangle[2] - triangle[0]) + normal = normal / np.linalg.norm(normal) + distance = abs(float((flat.positions_mm[row, column] - triangle[0]) @ normal)) + worst = max(worst, distance) + assert worst < 1e-9 + + +def test_every_covered_pixel_reports_its_face_normal(bundle, flat): + covered = np.argwhere(flat.face_index >= 0) + step = max(1, len(covered) // 300) + for row, column in covered[::step]: + face = int(flat.face_index[row, column]) + assert float(flat.normals[row, column] @ bundle.face_normals[face]) == pytest.approx(1.0) + + +def test_a_uv_corner_lands_on_a_pixel_near_its_vertex(bundle, flat): + """The round trip: vertex -> its UV pixel -> looked-up 3D is within a pixel. + + Most corners land exactly on their vertex; a few land one pixel off a seam, + which is what the tolerance is for. + """ + uv = bundle.uv_data.reshape(-1, 3, 2) + worst = 0.0 + misses = 0 + for index in range(0, len(uv), 97): + face, corner = divmod(index, 3) + x, y = flat.pixel_for_uv(float(uv[face, corner, 0]), float(uv[face, corner, 1])) + sample = flat.sample(x, y) + if sample is None: + # A corner exactly on an island boundary can miss; count it and keep + # the check meaningful by requiring that it stays rare. + misses += 1 + continue + vertex = bundle.vertices_mm[bundle.loop_to_vertex[index]] + worst = max(worst, float(np.linalg.norm(sample[0] - vertex))) + assert worst < 0.1, "a looked-up point is more than a pixel from its vertex" + assert misses < len(range(0, len(uv), 97)) * 0.05 diff --git a/apps/autocal/tests/test_uv_texture.py b/apps/autocal/tests/test_uv_texture.py new file mode 100644 index 0000000..1071e1e --- /dev/null +++ b/apps/autocal/tests/test_uv_texture.py @@ -0,0 +1,126 @@ +"""The UV-space texture: loading it, resampling it, and falling back without it. + +The texture is authored in UV coordinates, black outside the island, so it +resamples into exactly the space the picker already uses. These tests pin the two +things that would silently ruin that: the orientation of the resample, and the +claim that adding a texture cannot change where a click lands. +""" + +from __future__ import annotations + +from pathlib import Path + +import numpy as np +import pytest + +from orisys_autocal.sensor import ( + DEFAULT_FLAT_SIZE, + load_bundle, + load_uv_texture, + rasterise, + texture_path_for, +) + +BUNDLE = Path(r"D:\orisys_ws\orisys-sensor-design\build\export\finger\r01\compute") + +pytestmark = pytest.mark.skipif( + not BUNDLE.is_dir(), reason=f"no sensor export at {BUNDLE}" +) + + +@pytest.fixture(scope="module") +def bundle(): + return load_bundle(BUNDLE) + + +@pytest.fixture(scope="module") +def texture(bundle): + path = texture_path_for(bundle) + if path is None: + pytest.skip("this export has no texture beside it") + loaded = load_uv_texture(path) + if loaded is None: + pytest.skip(f"could not decode {path}") + return loaded + + +def test_the_texture_is_found_from_the_bundle(bundle): + """Derived, not configured: a mismatched pair resamples the wrong artwork.""" + path = texture_path_for(bundle) + if path is None: + pytest.skip("this export has no texture beside it") + assert path.is_file() + assert path.name == "patch_texture_compute.png" + assert path.parent.parent.name == "finger" + + +def test_loading_a_texture_returns_an_rgb_ready_array(texture): + assert texture.ndim == 3 and texture.shape[2] == 4 + assert texture.dtype == np.uint8 + assert texture.shape[0] > 1 and texture.shape[1] > 1 + + +def test_a_missing_texture_is_none_not_an_exception(): + """The picker has to keep working without artwork.""" + assert load_uv_texture(r"D:\definitely\not\here.png") is None + + +def test_the_texture_is_black_outside_the_island_and_not_inside(texture): + """The island's v extent is where the artwork lives; above and below is black.""" + height = texture.shape[0] + top = texture[: int(height * 0.05), :, :3] + bottom = texture[int(height * 0.95) :, :, :3] + middle = texture[int(height * 0.45) : int(height * 0.55), :, :3] + assert int(top.max()) == 0, "the top band should be unused black" + assert int(bottom.max()) == 0, "the bottom band should be unused black" + assert int(middle.max()) > 0, "the island band should carry content" + + +def test_a_texture_does_not_change_where_a_click_lands(bundle, texture): + """The lookup tables are the measurement; the image is only what you aim at.""" + size = (256, 256) + plain = rasterise(bundle, size) + textured = rasterise(bundle, size, texture=texture) + + assert plain.textured is False + assert textured.textured is True + assert np.array_equal(plain.face_index, textured.face_index) + assert np.allclose(plain.positions_mm, textured.positions_mm) + assert np.allclose(plain.normals, textured.normals) + assert plain.uv_min == textured.uv_min and plain.uv_max == textured.uv_max + + +def test_the_textured_image_samples_the_texture_in_the_right_place(bundle, texture): + """Resample at the pixel's own UV, including the flip, or the map is mirrored.""" + textured = rasterise(bundle, (256, 256), texture=texture) + covered = np.nonzero(textured.face_index >= 0) + rows, columns = covered[0][::97], covered[1][::97] + assert rows.size > 20 + + height, width = textured.face_index.shape + u0, v0 = textured.uv_min + u1, v1 = textured.uv_max + texture_height, texture_width = texture.shape[0], texture.shape[1] + + for row, column in zip(rows, columns): + u = u0 + column / (width - 1) * (u1 - u0) + v = v1 - row / (height - 1) * (v1 - v0) + tex_x = int(round(u * (texture_width - 1))) + tex_y = int(round((1.0 - v) * (texture_height - 1))) + expected = texture[tex_y, tex_x, :3] + assert np.array_equal(textured.image_rgb[row, column], expected), ( + f"pixel ({column}, {row}) does not carry the texture sample at its own UV" + ) + + +def test_gap_pixels_stay_black_when_textured(bundle, texture): + """A fabricated colour in the gap would look like part of the patch.""" + textured = rasterise(bundle, (256, 256), texture=texture) + gaps = textured.face_index < 0 + if not gaps.any(): + pytest.skip("this raster has no gap pixels") + assert int(textured.image_rgb[gaps].max()) == 0 + + +def test_the_default_flat_size_is_still_the_design_size(): + assert DEFAULT_FLAT_SIZE == (1024, 1024) diff --git a/apps/autocal/tools/layout_mockup.py b/apps/autocal/tools/layout_mockup.py index 16bb739..2e1d06c 100644 --- a/apps/autocal/tools/layout_mockup.py +++ b/apps/autocal/tools/layout_mockup.py @@ -101,7 +101,7 @@ STRINGS = { "nodes": "节点数量", "axes_count": "轴数量", "busdata_group": "总线数据 (SDO / RTEX) ▶", - "tabs": ["轴调试", "测试序列", "传感器", "表面分布", "标定点"], + "tabs": ["轴调试", "测试序列", "手指调试", "传感器", "表面分布", "标定点"], "readout_title": "轴状态", "enabled": "已使能", "disabled": "未使能", @@ -151,6 +151,38 @@ STRINGS = { ["3", "4.000", "0.000", "12.400", "待测", "—", "—"], ], "points_hint": "坐标由网格生成并逐点校验，不接受手工输入：手工输入的坐标没有任何检查，一个错数字会毁掉整条序列。", + "finger_bundle_label": "导出包体", + "finger_bundle_value": "D:\\orisys_ws\\orisys-sensor-design\\build\\export\\finger\\r01", + "finger_load": "载入包体…", + "finger_initial_view": "初始视角", + "finger_reset": "复位视角", + "finger_textured_note": "UV 贴图：patch_texture_compute.png", + "finger_axes_note": "黄色为工具轴 · X/Y 为工作台，Z 为工具行程 · 绿=对齐，红=不可对齐", + "finger_bundle_summary": "1032 顶点 · 1974 三角面 · 15.00 × 7.50 × 15.81 mm · 0.018388 mm/单位", + "finger_flat_title": "展平图像（点击取点）", + "finger_pick_hint": "点击传感面上的像素即可取点；落在 UV 缝隙上的像素会被拒绝。", + "finger_readout_title": "读数", + "finger_readout_rows": [ + ["传感器 (mm)", "2.841, 3.512, 0.940", None], + ["法向", "-0.18, -0.88, 0.43", None], + ["平台 (mm)", "2.841, 0.940, -3.512", None], + ["X / Y (mm)", "2.841, 0.940", None], + ["yaw / pitch", "-78.42° / 23.10°", None], + ["对轴残差", "0.03° 可对轴", "#12b76a"], + ["坐标系映射", "x→x, y→z, z→-y", None], + ], + "finger_plate_label": "安装板厚 (mm)", + "finger_plate_value": "0.000", + "finger_preview_title": "三维预览", + "finger_preview_legend": "曲面颜色 = 对轴残差（绿为对正，红为对不上） · 黄色为 Z 轴", + "finger_point_cols": ["#", "像素 u, v", "sensor x", "sensor y", "sensor z", "nx", "ny", "nz", "X / Y", "Z", "yaw", "pitch"], + "finger_point_rows": [ + ["1", "0.35, 0.40", "2.841", "3.512", "0.940", "-0.18", "-0.88", "0.43", "2.841 / 0.940", "7.958", "-78.42", "23.10"], + ["2", "0.62, 0.55", "5.113", "2.204", "-1.870", "0.21", "0.55", "-0.81", "5.113 / -1.870", "7.958", "12.05", "8.44"], + ["3", "0.50, 0.72", "3.902", "6.118", "2.405", "0.55", "0.19", "0.81", "3.902 / 2.405", "7.958", "-90.00", "51.66"], + ], + "finger_bad_word": "撞 yaw 行程限位", + "finger_point_count": "3 个取点 · 展平 1024 × 1024 · 有效像素 72.9%", "sensor_note": "设备与信号源、实时图像、力曲线", "sensor_curve": "力曲线 (FN / FX / FY)", "surface_note": "多点标定灵敏度 → 三维表面色图", @@ -196,7 +228,7 @@ STRINGS = { "nodes": "Nodes", "axes_count": "Axes", "busdata_group": "Bus data (SDO / RTEX) ▶", - "tabs": ["Axis debug", "Test sequence", "Sensor", "Surface map", "Calibration points"], + "tabs": ["Axis debug", "Test sequence", "Finger debug", "Sensor", "Surface map", "Calibration points"], "readout_title": "Axis state", "enabled": "Enabled", "disabled": "Disabled", @@ -246,6 +278,38 @@ STRINGS = { ["3", "4.000", "0.000", "12.400", "Pending", "—", "—"], ], "points_hint": "Coordinates are generated and checked per point; they are not typed. A typed coordinate is one nothing has checked.", + "finger_bundle_label": "Export bundle", + "finger_bundle_value": "D:\\orisys_ws\\orisys-sensor-design\\build\\export\\finger\\r01", + "finger_load": "Load bundle…", + "finger_initial_view": "Initial view", + "finger_reset": "Reset view", + "finger_textured_note": "UV texture: patch_texture_compute.png", + "finger_axes_note": "Yellow is the tool axis · X/Y are the table, Z the tool travel · green aligned, red not", + "finger_bundle_summary": "1032 vertices · 1974 triangles · 15.00 × 7.50 × 15.81 mm · 0.018388 mm per unit", + "finger_flat_title": "Flat image (click to pick)", + "finger_pick_hint": "Click a pixel on the sensing surface to pick a point; pixels in a UV gap are refused.", + "finger_readout_title": "Readouts", + "finger_readout_rows": [ + ["Sensor (mm)", "2.841, 3.512, 0.940", None], + ["Normal", "-0.18, -0.88, 0.43", None], + ["Platform (mm)", "2.841, 0.940, -3.512", None], + ["X / Y (mm)", "2.841, 0.940", None], + ["yaw / pitch", "-78.42° / 23.10°", None], + ["Residual", "0.03° can align", "#12b76a"], + ["Frame map", "x→x, y→z, z→-y", None], + ], + "finger_plate_label": "Mounting plate (mm)", + "finger_plate_value": "0.000", + "finger_preview_title": "3D preview", + "finger_preview_legend": "Surface colour = alignment residual (green aligned, red not) · yellow is the Z axis", + "finger_point_cols": ["#", "pixel u, v", "sensor x", "sensor y", "sensor z", "nx", "ny", "nz", "X / Y", "Z", "yaw", "pitch"], + "finger_point_rows": [ + ["1", "0.35, 0.40", "2.841", "3.512", "0.940", "-0.18", "-0.88", "0.43", "2.841 / 0.940", "7.958", "-78.42", "23.10"], + ["2", "0.62, 0.55", "5.113", "2.204", "-1.870", "0.21", "0.55", "-0.81", "5.113 / -1.870", "7.958", "12.05", "8.44"], + ["3", "0.50, 0.72", "3.902", "6.118", "2.405", "0.55", "0.19", "0.81", "3.902 / 2.405", "7.958", "-90.00", "51.66"], + ], + "finger_bad_word": "hits yaw travel limit", + "finger_point_count": "3 picked points · flat 1024 × 1024 · 72.9% valid pixels", "sensor_note": "Device and source, live image, force curves", "sensor_curve": "Force curves (FN / FX / FY)", "surface_note": "Multi-point calibration sensitivity → 3D surface colour map", @@ -875,16 +939,151 @@ def draw_points_tab(painter: QPainter, palette: Palette, s: dict, rect: QRect) - _button(painter, QRect(inner.x() + 250, y, 100, 28), "删除" if s["axis_label"] == "轴号" else "Remove", palette) +def draw_finger_tab(painter: QPainter, palette: Palette, s: dict, rect: QRect) -> None: + """The finger debugger: pick on the flat image, read the pose, check the 3D preview.""" + inner = rect.adjusted(10, 8, -10, -8) + zh = s["axis_label"] == "轴号" + y = inner.y() + + _text(painter, QRect(inner.x(), y, 90, 24), s["finger_bundle_label"], palette, size=10, muted=True) + _field(painter, QRect(inner.x() + 92, y, 300, 24), s["finger_bundle_value"], palette) + _button(painter, QRect(inner.x() + 400, y, 100, 24), s["finger_load"], palette) + _button(painter, QRect(inner.x() + 508, y, 100, 24), s["finger_initial_view"], palette) + _button(painter, QRect(inner.x() + 616, y, 100, 24), s["finger_reset"], palette) + y += 34 + _text(painter, QRect(inner.x(), y, inner.width(), 18), s["finger_bundle_summary"], palette, size=9, muted=True) + y += 24 + + body_h = int((inner.height() - (y - inner.y())) * 0.62) + table_h = inner.bottom() - (y + body_h) - 8 + gap = 10 + usable = inner.width() - 2 * gap + picker_w = int(usable * 28 / 100) + readout_w = int(usable * 32 / 100) + preview_w = usable - picker_w - readout_w + + picker = QRect(inner.x(), y, picker_w, body_h) + readouts = QRect(picker.right() + gap, y, readout_w, body_h) + preview = QRect(readouts.right() + gap, y, preview_w, body_h) + + # Flat unrolled image. + inner_picker = _panel(painter, picker, palette) + _text(painter, QRect(inner_picker.x(), inner_picker.y(), inner_picker.width(), 18), s["finger_flat_title"], palette, size=10, bold=True) + flat = QRect(inner_picker.x(), inner_picker.y() + 22, inner_picker.width(), inner_picker.height() - 22 - 40) + # The textured UV space: black outside the island, artwork inside it. + painter.fillRect(flat, QColor("#000000")) + island = flat.adjusted(int(flat.width() * 0.06), int(flat.height() * 0.16), -int(flat.width() * 0.06), -int(flat.height() * 0.16)) + painter.setPen(QPen(QColor("#000000"), 0)) + columns, rows = 22, 14 + for row in range(rows): + for column in range(columns): + cell = QRect( + island.x() + column * island.width() // columns, + island.y() + row * island.height() // rows, + island.width() // columns + 1, + island.height() // rows + 1, + ) + shade = ((row * 7 + column * 13) % 5) / 4.0 + painter.fillRect( + cell, + QColor(int(60 + 150 * shade), int(40 + 120 * ((row + column) % 3) / 2), int(90 + 120 * (1 - shade))), + ) + for index, (fx, fy) in enumerate(((0.35, 0.4), (0.62, 0.55), (0.5, 0.72))): + mark = QPoint(int(flat.x() + fx * flat.width()), int(flat.y() + fy * flat.height())) + painter.setPen(QPen(QColor("#ffffff"), 2)) + painter.setBrush(QColor("#d92d20") if index == 2 else QColor("#12b76a")) + painter.drawEllipse(mark, 4, 4) + # Leave no brush behind: a later panel fills its background, and a leaked + # brush turns that fill into the marker colour. + painter.setBrush(Qt.BrushStyle.NoBrush) + _text(painter, QRect(inner_picker.x(), flat.bottom() + 2, inner_picker.width(), 18), s["finger_textured_note"], palette, size=9, muted=True) + _text(painter, QRect(inner_picker.x(), flat.bottom() + 20, inner_picker.width(), 18), s["finger_pick_hint"], palette, size=9, muted=True) + + # Readouts. + inner_read = _panel(painter, readouts, palette) + _text(painter, QRect(inner_read.x(), inner_read.y(), inner_read.width(), 18), s["finger_readout_title"], palette, size=10, bold=True) + row_y = inner_read.y() + 24 + label_w = int(inner_read.width() * 0.42) + for label, value, colour in s["finger_readout_rows"]: + _text(painter, QRect(inner_read.x(), row_y, label_w, 20), label, palette, size=9, muted=True) + _text(painter, QRect(inner_read.x() + label_w, row_y, inner_read.width() - label_w, 20), value, palette, size=9, colour=QColor(colour) if colour else None) + row_y += 20 + _text(painter, QRect(inner_read.x(), inner_read.bottom() - 36, inner_read.width(), 18), s["finger_plate_label"], palette, size=9, muted=True) + _field(painter, QRect(inner_read.x(), inner_read.bottom() - 20, inner_read.width(), 20), s["finger_plate_value"], palette) + + # 3D preview: the tool axis and a normal with its residual. + inner_preview = _panel(painter, preview, palette) + _text(painter, QRect(inner_preview.x(), inner_preview.y(), inner_preview.width(), 18), s["finger_preview_title"], palette, size=10, bold=True) + view = QRect(inner_preview.x(), inner_preview.y() + 22, inner_preview.width(), inner_preview.height() - 22 - 34) + painter.fillRect(view, QColor("#12161c")) + painter.setPen(QPen(QColor("#2a313c"), 1)) + for i in range(1, 4): + painter.drawLine(view.x() + i * view.width() // 4, view.y(), view.x() + i * view.width() // 4, view.bottom()) + cx, cy = view.center().x(), view.center().y() + 40 + axis = QPoint(cx, view.y() + 24) + painter.setPen(QPen(QColor("#f2c94c"), 3)) + painter.drawLine(axis, QPoint(cx, cy)) + painter.setBrush(QColor("#f2c94c")) + painter.drawEllipse(QPoint(cx, cy), 3, 3) + # The motion axes: the platform frame, drawn in the sensor frame. + for colour, end in ( + ("#e65454", QPoint(cx - 96, cy + 18)), + ("#61cc61", QPoint(cx + 30, cy + 92)), + ("#5c8cf2", QPoint(cx + 88, cy - 26)), + ): + painter.setPen(QPen(QColor(colour), 2)) + painter.drawLine(QPoint(cx, cy), end) + painter.setPen(QPen(QColor("#12b76a"), 3)) + painter.drawLine(QPoint(cx, cy), QPoint(cx + 26, cy - 96)) + painter.setPen(QPen(QColor("#d92d20"), 3)) + painter.drawLine(QPoint(cx, cy), QPoint(cx - 74, cy - 78)) + painter.setBrush(Qt.BrushStyle.NoBrush) + painter.setPen(QPen(QColor("#4a5568"), 1)) + painter.drawLine(QPoint(cx - 90, cy), QPoint(cx + 90, cy)) + painter.setPen(QPen(QColor("#e65454"), 1)) + painter.drawText(QPoint(cx - 108, cy + 24), "X") + painter.setPen(QPen(QColor("#61cc61"), 1)) + painter.drawText(QPoint(cx + 34, cy + 100), "Y") + painter.setPen(QPen(QColor("#5c8cf2"), 1)) + painter.drawText(QPoint(cx + 92, cy - 28), "Z") + _text(painter, QRect(inner_preview.x(), view.bottom() + 2, inner_preview.width(), 30), s["finger_axes_note"], palette, size=8, muted=True) + + # Picked points. + table = QRect(inner.x(), y + body_h + 8, inner.width(), max(60, table_h)) + painter.fillRect(table, palette.window) + painter.setPen(QPen(palette.border, 1)) + painter.drawRect(table.adjusted(0, 0, -1, -1)) + header_h = 24 + painter.fillRect(QRect(table.x(), table.y(), table.width(), header_h), palette.panel) + columns = s["finger_point_cols"] + widths = [int(table.width() * share) for share in (0.06, 0.09, 0.1, 0.09, 0.09, 0.07, 0.07, 0.07, 0.09, 0.09, 0.09, 0.09)] + x = table.x() + for name, width in zip(columns, widths): + _text(painter, QRect(x, table.y(), width, header_h), name, palette, size=9, muted=True, align=Qt.AlignmentFlag.AlignCenter) + x += width + for index, row in enumerate(s["finger_point_rows"]): + row_y = table.y() + header_h + index * 26 + if index % 2 == 0: + painter.fillRect(QRect(table.x() + 1, row_y, table.width() - 2, 25), palette.panel) + x = table.x() + for value, width in zip(row, widths): + colour = palette.danger if value == s["finger_bad_word"] else None + _text(painter, QRect(x, row_y, width, 26), value, palette, size=10, align=Qt.AlignmentFlag.AlignCenter, colour=colour) + x += width + _text(painter, QRect(table.x() + 8, table.bottom() + 2, table.width(), 18), s["finger_point_count"], palette, size=9, muted=True) + + TAB_PAINTERS = { "axes": draw_axis_tab, "sequence": draw_sequence_tab, + "finger": draw_finger_tab, "sensor": draw_sensor_tab, "surface": draw_surface_tab, "points": draw_points_tab, } #: Tab index -> painter key, for the tab strip labels. -TAB_KEYS = ("axes", "sequence", "sensor", "surface", "points") +TAB_KEYS = ("axes", "sequence", "finger", "sensor", "surface", "points") def draw_page( @@ -983,8 +1182,10 @@ def main() -> int: parser.add_argument("--height", type=int, default=PAGE_H) parser.add_argument( "--page", - default="axes", - choices=("axes", "sequence", "sensor", "surface", "points", "all"), + default=TAB_KEYS[0], + # Derived, not listed: a literal here silently rejects any tab added to + # TAB_KEYS, which is how the finger page first went missing. + choices=(*TAB_KEYS, "all"), help="which page to draw; 'all' writes one full-size image per page", ) args = parser.parse_args() diff --git a/apps/autocal/tools/panel_check.py b/apps/autocal/tools/panel_check.py index 9909cc4..6662580 100644 --- a/apps/autocal/tools/panel_check.py +++ b/apps/autocal/tools/panel_check.py @@ -145,7 +145,8 @@ def main() -> int: # ---- the permanent shell ---------------------------------------- checker.that( - window.workspace.page_keys() == ["axes", "sequence", "sensor", "surface", "points"], + window.workspace.page_keys() + == ["axes", "sequence", "finger", "sensor", "surface", "points"], f"unexpected tab set: {window.workspace.page_keys()}", ) checker.that(