added motion simulator feature

This commit is contained in:
ykcheungab
2026-09-21 16:11:45 +08:00
parent ccedb9ddd9
commit f1bec62ca4
21 changed files with 5025 additions and 12 deletions
+49
View File
@@ -181,6 +181,7 @@
"workspace": { "workspace": {
"tab_axes": "Axis debug", "tab_axes": "Axis debug",
"tab_sequence": "Test sequence", "tab_sequence": "Test sequence",
"tab_finger": "Finger debug",
"tab_sensor": "Sensor", "tab_sensor": "Sensor",
"tab_surface": "Surface map", "tab_surface": "Surface map",
"tab_points": "Calibration points", "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_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." "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": { "sequence": {
"title": "Test sequence design", "title": "Test sequence design",
"profile_title": "Point generation", "profile_title": "Point generation",
+49
View File
@@ -181,6 +181,7 @@
"workspace": { "workspace": {
"tab_axes": "轴调试", "tab_axes": "轴调试",
"tab_sequence": "测试序列", "tab_sequence": "测试序列",
"tab_finger": "手指调试",
"tab_sensor": "传感器", "tab_sensor": "传感器",
"tab_surface": "表面分布", "tab_surface": "表面分布",
"tab_points": "标定点", "tab_points": "标定点",
@@ -189,6 +190,54 @@
"reserved_surface": "多点标定的灵敏度将在此以色图叠加在传感器三维表面上:每个标定点的灵敏度经插值着色,并给出色标与范围。", "reserved_surface": "多点标定的灵敏度将在此以色图叠加在传感器三维表面上:每个标定点的灵敏度经插值着色,并给出色标与范围。",
"reserved_points": "标定点列表与采集控制将在此显示:平台坐标、参考载荷、采样统计与灵敏度结果。" "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": { "sequence": {
"title": "测试序列设计", "title": "测试序列设计",
"profile_title": "标定点生成", "profile_title": "标定点生成",
+112
View File
@@ -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.
@@ -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)))
)
@@ -32,6 +32,21 @@ POINT_ORDERS = ("row", "serpentine", "distance")
#: selectable for a dim room. #: selectable for a dim room.
DEFAULT_THEME = "light" 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]: def _default_sequence_profile() -> dict[str, Any]:
"""The point-generation and run parameters of workflow P5. """The point-generation and run parameters of workflow P5.
@@ -157,6 +172,23 @@ def default_preferences() -> dict[str, Any]:
"sensor_setup": _default_sensor_setup(), "sensor_setup": _default_sensor_setup(),
#: P5: grid generation and run parameters. #: P5: grid generation and run parameters.
"sequence_profile": _default_sequence_profile(), "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,
} }
@@ -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",
]
@@ -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)
@@ -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")
)
@@ -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/<sensor>/<revision>/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 ``<build>/export/<sensor>/<revision>/<profile>`` and the
texture at ``<build>/textures/<sensor>/<revision>/patch_texture_<profile>.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
@@ -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()
@@ -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"))
+151 -2
View File
@@ -14,6 +14,8 @@ import subprocess
import sys import sys
from pathlib import Path from pathlib import Path
import numpy as np
from orisys_app_shared import API_VERSION, __version__ as shared_version from orisys_app_shared import API_VERSION, __version__ as shared_version
from orisys_app_shared.i18n import set_locale, t from orisys_app_shared.i18n import set_locale, t
from orisys_app_shared.paths import outputs_dir from orisys_app_shared.paths import outputs_dir
@@ -37,7 +39,10 @@ from PySide6.QtWidgets import (
from .. import __version__ as app_version from .. import __version__ as app_version
from ..motion import AxisParams, ConnectionSpec, MotionPoller, MotionService, Snapshot from ..motion import AxisParams, ConnectionSpec, MotionPoller, MotionService, Snapshot
from ..motion.kinematics import PlatformStack
from ..preferences import load_preferences, save_preferences 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 ( from ..sequence_io import (
FILE_SUFFIX, FILE_SUFFIX,
SequenceDocument, SequenceDocument,
@@ -52,6 +57,7 @@ from .brand_bar import BrandBar
from .bus_data_panel import BusDataPanel from .bus_data_panel import BusDataPanel
from .bus_panel import BusPanel, default_bas_dir from .bus_panel import BusPanel, default_bas_dir
from .connection_panel import ConnectionPanel from .connection_panel import ConnectionPanel
from .sensor_panel import FingerPanel
from .sequence_panel import SequencePanel from .sequence_panel import SequencePanel
from .status_strip import RunBar, StatusStrip from .status_strip import RunBar, StatusStrip
from .workspace import TABS, WorkspaceTabs from .workspace import TABS, WorkspaceTabs
@@ -89,7 +95,7 @@ _COMMAND_NAMES = {
class AutoCalMainWindow(QMainWindow): class AutoCalMainWindow(QMainWindow):
"""Platform control, five axes, one window.""" """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__() super().__init__()
self.prefs = dict(prefs or load_preferences()) self.prefs = dict(prefs or load_preferences())
self._theme = str(self.prefs.get("theme") or "dark") self._theme = str(self.prefs.get("theme") or "dark")
@@ -98,6 +104,12 @@ class AutoCalMainWindow(QMainWindow):
self._pending: dict[int, str] = {} self._pending: dict[int, str] = {}
self._last_error = "" 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.setWindowTitle(t("app.title"))
self.setMinimumSize(*MINIMUM_WINDOW_SIZE) self.setMinimumSize(*MINIMUM_WINDOW_SIZE)
@@ -107,6 +119,12 @@ class AutoCalMainWindow(QMainWindow):
self._restore_geometry() self._restore_geometry()
self.retranslate() 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() self._apply_theme()
# --------------------------------------------------------------- build UI # --------------------------------------------------------------- build UI
@@ -204,7 +222,28 @@ class AutoCalMainWindow(QMainWindow):
parent=body, parent=body,
) )
self.sequence_panel = SequencePanel(self.prefs.get("sequence_profile") or {}, 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 = QSplitter(Qt.Orientation.Horizontal, body)
self.main_splitter.addWidget(self.left_rail) self.main_splitter.addWidget(self.left_rail)
@@ -252,6 +291,115 @@ class AutoCalMainWindow(QMainWindow):
self.run_bar.abortRequested.connect(self._on_run_abort) self.run_bar.abortRequested.connect(self._on_run_abort)
self.sequence_panel.exportRequested.connect(self._on_export_sequence) self.sequence_panel.exportRequested.connect(self._on_export_sequence)
self.sequence_panel.importRequested.connect(self._on_import_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.stopAllRequested.connect(self._on_stop_all)
self.axis_readout.clearAllRequested.connect(self._on_clear_all_alarms) self.axis_readout.clearAllRequested.connect(self._on_clear_all_alarms)
self.axis_readout.enableAllRequested.connect(self._on_enable_all) self.axis_readout.enableAllRequested.connect(self._on_enable_all)
@@ -855,6 +1003,7 @@ class AutoCalMainWindow(QMainWindow):
self.axis_panel.retranslate() self.axis_panel.retranslate()
self.axis_readout.retranslate() self.axis_readout.retranslate()
self.sequence_panel.retranslate() self.sequence_panel.retranslate()
self.finger_panel.retranslate()
self.workspace.retranslate() self.workspace.retranslate()
self.menu_file.setTitle(t("menu.file")) self.menu_file.setTitle(t("menu.file"))
@@ -31,6 +31,7 @@ from .sequence_panel import SequencePanel
TABS: tuple[tuple[str, str], ...] = ( TABS: tuple[tuple[str, str], ...] = (
("axes", "workspace.tab_axes"), ("axes", "workspace.tab_axes"),
("sequence", "workspace.tab_sequence"), ("sequence", "workspace.tab_sequence"),
("finger", "workspace.tab_finger"),
("sensor", "workspace.tab_sensor"), ("sensor", "workspace.tab_sensor"),
("surface", "workspace.tab_surface"), ("surface", "workspace.tab_surface"),
("points", "workspace.tab_points"), ("points", "workspace.tab_points"),
@@ -78,6 +79,7 @@ class WorkspaceTabs(QTabWidget):
self, self,
axis_panel: AxisPanel, axis_panel: AxisPanel,
sequence_panel: SequencePanel, sequence_panel: SequencePanel,
finger_panel: QWidget | None = None,
parent: QWidget | None = None, parent: QWidget | None = None,
) -> None: ) -> None:
super().__init__(parent) super().__init__(parent)
@@ -87,13 +89,16 @@ class WorkspaceTabs(QTabWidget):
self._pages["axes"] = axis_panel self._pages["axes"] = axis_panel
self._pages["sequence"] = sequence_panel self._pages["sequence"] = sequence_panel
if finger_panel is not None:
self._pages["finger"] = finger_panel
for key, locale_key in _PLACEHOLDERS.items(): for key, locale_key in _PLACEHOLDERS.items():
page = PlaceholderPage(locale_key, self) page = PlaceholderPage(locale_key, self)
self._pages[key] = page self._pages[key] = page
self._placeholder_keys[key] = locale_key self._placeholder_keys[key] = locale_key
for key, _locale_key in TABS: 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.currentChanged.connect(self._on_current_changed)
self.retranslate() self.retranslate()
@@ -101,7 +106,8 @@ class WorkspaceTabs(QTabWidget):
# -------------------------------------------------------------------- state # -------------------------------------------------------------------- state
def page_keys(self) -> list[str]: 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: def page(self, key: str) -> QWidget:
return self._pages[key] return self._pages[key]
@@ -118,7 +124,8 @@ class WorkspaceTabs(QTabWidget):
def retranslate(self) -> None: def retranslate(self) -> None:
for index, (key, locale_key) in enumerate(TABS): 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(): for key, locale_key in self._placeholder_keys.items():
page = self._pages[key] page = self._pages[key]
if isinstance(page, PlaceholderPage): if isinstance(page, PlaceholderPage):
+4 -1
View File
@@ -116,7 +116,10 @@ def make_window(qt_app, monkeypatch):
from orisys_app_shared.i18n import set_locale from orisys_app_shared.i18n import set_locale
set_locale(str(prefs.get("language") or "zh_CN")) 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. # Swap in the simulated platform: no test in this suite touches a DLL.
window.poller.service._transport = DemoTransport() # noqa: SLF001 window.poller.service._transport = DemoTransport() # noqa: SLF001
created.append(window) created.append(window)
@@ -58,6 +58,9 @@ _SCOPED_ENUMS = {
"PenStyle", "PenStyle",
"GlobalColor", "GlobalColor",
"ItemFlag", "ItemFlag",
"TextElideMode",
"TextFlag",
"AspectRatioMode",
), ),
"QTableWidget": ("SelectionMode", "EditTrigger"), "QTableWidget": ("SelectionMode", "EditTrigger"),
"QSizePolicy": ("Policy",), "QSizePolicy": ("Policy",),
+629
View File
@@ -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()
+404
View File
@@ -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
+226
View File
@@ -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
+126
View File
@@ -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)
+206 -5
View File
@@ -101,7 +101,7 @@ STRINGS = {
"nodes": "节点数量", "nodes": "节点数量",
"axes_count": "轴数量", "axes_count": "轴数量",
"busdata_group": "总线数据 (SDO / RTEX) ▶", "busdata_group": "总线数据 (SDO / RTEX) ▶",
"tabs": ["轴调试", "测试序列", "传感器", "表面分布", "标定点"], "tabs": ["轴调试", "测试序列", "手指调试", "传感器", "表面分布", "标定点"],
"readout_title": "轴状态", "readout_title": "轴状态",
"enabled": "已使能", "enabled": "已使能",
"disabled": "未使能", "disabled": "未使能",
@@ -151,6 +151,38 @@ STRINGS = {
["3", "4.000", "0.000", "12.400", "待测", "—", "—"], ["3", "4.000", "0.000", "12.400", "待测", "—", "—"],
], ],
"points_hint": "坐标由网格生成并逐点校验,不接受手工输入:手工输入的坐标没有任何检查,一个错数字会毁掉整条序列。", "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_note": "设备与信号源、实时图像、力曲线",
"sensor_curve": "力曲线 (FN / FX / FY)", "sensor_curve": "力曲线 (FN / FX / FY)",
"surface_note": "多点标定灵敏度 → 三维表面色图", "surface_note": "多点标定灵敏度 → 三维表面色图",
@@ -196,7 +228,7 @@ STRINGS = {
"nodes": "Nodes", "nodes": "Nodes",
"axes_count": "Axes", "axes_count": "Axes",
"busdata_group": "Bus data (SDO / RTEX) ▶", "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", "readout_title": "Axis state",
"enabled": "Enabled", "enabled": "Enabled",
"disabled": "Disabled", "disabled": "Disabled",
@@ -246,6 +278,38 @@ STRINGS = {
["3", "4.000", "0.000", "12.400", "Pending", "—", "—"], ["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.", "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_note": "Device and source, live image, force curves",
"sensor_curve": "Force curves (FN / FX / FY)", "sensor_curve": "Force curves (FN / FX / FY)",
"surface_note": "Multi-point calibration sensitivity → 3D surface colour map", "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) _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 = { TAB_PAINTERS = {
"axes": draw_axis_tab, "axes": draw_axis_tab,
"sequence": draw_sequence_tab, "sequence": draw_sequence_tab,
"finger": draw_finger_tab,
"sensor": draw_sensor_tab, "sensor": draw_sensor_tab,
"surface": draw_surface_tab, "surface": draw_surface_tab,
"points": draw_points_tab, "points": draw_points_tab,
} }
#: Tab index -> painter key, for the tab strip labels. #: 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( def draw_page(
@@ -983,8 +1182,10 @@ def main() -> int:
parser.add_argument("--height", type=int, default=PAGE_H) parser.add_argument("--height", type=int, default=PAGE_H)
parser.add_argument( parser.add_argument(
"--page", "--page",
default="axes", default=TAB_KEYS[0],
choices=("axes", "sequence", "sensor", "surface", "points", "all"), # 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", help="which page to draw; 'all' writes one full-size image per page",
) )
args = parser.parse_args() args = parser.parse_args()
+2 -1
View File
@@ -145,7 +145,8 @@ def main() -> int:
# ---- the permanent shell ---------------------------------------- # ---- the permanent shell ----------------------------------------
checker.that( 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()}", f"unexpected tab set: {window.workspace.page_keys()}",
) )
checker.that( checker.that(