added motion simulator feature
This commit is contained in:
ykcheungab
@@ -181,6 +181,7 @@
|
||||
"workspace": {
|
||||
"tab_axes": "Axis debug",
|
||||
"tab_sequence": "Test sequence",
|
||||
"tab_finger": "Finger debug",
|
||||
"tab_sensor": "Sensor",
|
||||
"tab_surface": "Surface map",
|
||||
"tab_points": "Calibration points",
|
||||
@@ -189,6 +190,54 @@
|
||||
"reserved_surface": "Multi-point calibration sensitivity will be drawn as a colour map on the sensor's 3D surface here: each point's sensitivity interpolated and coloured, with a legend and range.",
|
||||
"reserved_points": "The calibration point table and capture controls will appear here: platform coordinates, reference load, sampling statistics and the resulting sensitivity."
|
||||
},
|
||||
"finger": {
|
||||
"title": "Finger calibration-point debug",
|
||||
"bundle_path": "Export bundle",
|
||||
"load": "Load bundle…",
|
||||
"reset_view": "Reset view",
|
||||
"initial_view": "Initial view",
|
||||
"textured": "UV texture: {name}",
|
||||
"untextured": "No UV texture: showing the shaded layout map.",
|
||||
"texture_missing": "No texture at {path}: showing the shaded layout map.",
|
||||
"flat_title": "Flat image (click to pick)",
|
||||
"readout_title": "Readouts",
|
||||
"preview_title": "3D preview",
|
||||
"points_title": "Picked points",
|
||||
"select_hint": "Click a point on the flat image to pick a calibration point.",
|
||||
"no_bundle": "No export bundle loaded.",
|
||||
"no_surface": "Pixel ({x}, {y}) is not on the sensing surface (UV gap).",
|
||||
"row_sensor_point": "Sensor point (mm)",
|
||||
"row_sensor_normal": "Surface normal",
|
||||
"row_platform_point": "Platform point (mm)",
|
||||
"row_pose": "Pose X / Y / yaw / pitch",
|
||||
"row_residual": "Alignment residual",
|
||||
"row_frames": "Frame mapping",
|
||||
"row_bundle": "Bundle",
|
||||
"bundle_summary": "{vertices} vertices · {triangles} triangles · {extent} mm · {scale} mm per unit",
|
||||
"plate_thickness": "Mounting plate",
|
||||
"tool_offset": "Tool offset x,y,z",
|
||||
"clear_points": "Clear points",
|
||||
"point_count": "{count} points",
|
||||
"residual_value": "{residual}° — {verdict}",
|
||||
"reachable": "this normal can be aligned with Z",
|
||||
"unreachable": "this normal cannot be aligned with Z (yaw travel limit)",
|
||||
"reach_summary": "Alignable surface vertices {aligned}/{vertices} ({share}%), worst {worst}°",
|
||||
"preview_legend": "Surface colour = alignment residual (green aligned, red not) · yellow is the tool axis · X/Y are the table, Z the tool travel",
|
||||
"show_axes": "Show motion axes",
|
||||
"sim_play": "Play motion",
|
||||
"sim_stop": "Stop",
|
||||
"sim_hint": "Play the solved motion: the stages rotate the normal onto the tool axis, then the XY table moves. Pick a point first.",
|
||||
"preview_no_gl": "No OpenGL context: {error}",
|
||||
"preview_draw_failed": "Drawing failed: {error}",
|
||||
"preview_solve_failed": "Solve failed: {error}",
|
||||
"col_x": "x", "col_y": "y", "col_z": "z",
|
||||
"col_nx": "nx", "col_ny": "ny", "col_nz": "nz",
|
||||
"col_X": "X", "col_Y": "Y", "col_Z": "Z",
|
||||
"col_yaw": "yaw", "col_pitch": "pitch", "col_residual": "residual(°)",
|
||||
"loaded": "Bundle loaded: {vertices} vertices · {extent} mm",
|
||||
"load_failed": "Load failed: {error}",
|
||||
"picked": "Picked: residual {residual}° · yaw {yaw}° · pitch {pitch}°"
|
||||
},
|
||||
"sequence": {
|
||||
"title": "Test sequence design",
|
||||
"profile_title": "Point generation",
|
||||
|
||||
@@ -181,6 +181,7 @@
|
||||
"workspace": {
|
||||
"tab_axes": "轴调试",
|
||||
"tab_sequence": "测试序列",
|
||||
"tab_finger": "手指调试",
|
||||
"tab_sensor": "传感器",
|
||||
"tab_surface": "表面分布",
|
||||
"tab_points": "标定点",
|
||||
@@ -189,6 +190,54 @@
|
||||
"reserved_surface": "多点标定的灵敏度将在此以色图叠加在传感器三维表面上：每个标定点的灵敏度经插值着色，并给出色标与范围。",
|
||||
"reserved_points": "标定点列表与采集控制将在此显示：平台坐标、参考载荷、采样统计与灵敏度结果。"
|
||||
},
|
||||
"finger": {
|
||||
"title": "手指标定点调试",
|
||||
"bundle_path": "导出包",
|
||||
"load": "载入导出包…",
|
||||
"reset_view": "复位视角",
|
||||
"initial_view": "初始视角",
|
||||
"textured": "UV 贴图：{name}",
|
||||
"untextured": "无 UV 贴图：当前显示的是着色布局图。",
|
||||
"texture_missing": "未找到贴图 {path}：当前显示的是着色布局图。",
|
||||
"flat_title": "展开平面图（点击选点）",
|
||||
"readout_title": "读数",
|
||||
"preview_title": "三维预览",
|
||||
"points_title": "已选标定点",
|
||||
"select_hint": "在展开图上点击一点以选取标定点。",
|
||||
"no_bundle": "尚未载入导出包。",
|
||||
"no_surface": "像素 ({x}, {y}) 不在传感面上（UV 空隙）。",
|
||||
"row_sensor_point": "传感器坐标 (mm)",
|
||||
"row_sensor_normal": "曲面法向",
|
||||
"row_platform_point": "平台坐标 (mm)",
|
||||
"row_pose": "位姿 X / Y / yaw / pitch",
|
||||
"row_residual": "对齐残差",
|
||||
"row_frames": "坐标映射",
|
||||
"row_bundle": "导出包",
|
||||
"bundle_summary": "{vertices} 顶点 · {triangles} 三角面 · {extent} mm · 每单位 {scale} mm",
|
||||
"plate_thickness": "安装板厚度",
|
||||
"tool_offset": "工具偏移 x,y,z",
|
||||
"clear_points": "清空标定点",
|
||||
"point_count": "共 {count} 点",
|
||||
"residual_value": "{residual}° — {verdict}",
|
||||
"reachable": "该点法向可与 Z 轴对齐",
|
||||
"unreachable": "该点法向无法与 Z 轴对齐（受 yaw 行程限制）",
|
||||
"reach_summary": "曲面可对齐顶点 {aligned}/{vertices}（{share}%），最差 {worst}°",
|
||||
"preview_legend": "表面颜色 = 对齐残差（绿=对齐，红=不可对齐）· 黄色为工具轴 · X/Y 为工作台，Z 为工具行程",
|
||||
"show_axes": "显示运动轴",
|
||||
"sim_play": "播放动作",
|
||||
"sim_stop": "停止",
|
||||
"sim_hint": "播放该点的求解动作：先由转台把法向转到工具轴上，再由 XY 工作台移动。请先取点。",
|
||||
"preview_no_gl": "无 OpenGL 上下文：{error}",
|
||||
"preview_draw_failed": "绘制失败：{error}",
|
||||
"preview_solve_failed": "求解失败：{error}",
|
||||
"col_x": "x", "col_y": "y", "col_z": "z",
|
||||
"col_nx": "nx", "col_ny": "ny", "col_nz": "nz",
|
||||
"col_X": "X", "col_Y": "Y", "col_Z": "Z",
|
||||
"col_yaw": "yaw", "col_pitch": "pitch", "col_residual": "残差(°)",
|
||||
"loaded": "已载入导出包：{vertices} 顶点 · {extent} mm",
|
||||
"load_failed": "载入失败: {error}",
|
||||
"picked": "已选点：残差 {residual}° · yaw {yaw}° · pitch {pitch}°"
|
||||
},
|
||||
"sequence": {
|
||||
"title": "测试序列设计",
|
||||
"profile_title": "标定点生成",
|
||||
|
||||
@@ -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.
|
||||
DEFAULT_THEME = "light"
|
||||
|
||||
#: The sensor-design export the finger debug screen loads by default. The bench
|
||||
#: confirmed this revision, and it carries the mesh, housing, intrinsics and the
|
||||
#: pose the unit scale is derived from.
|
||||
DEFAULT_FINGER_BUNDLE = (
|
||||
r"D:\orisys_ws\orisys-sensor-design\build\export\finger\r01\compute"
|
||||
)
|
||||
|
||||
#: The UV-space artwork for that revision. Resolved from the bundle when it can
|
||||
#: be, so the two cannot be pointed at different revisions; this preference is
|
||||
#: the override for a checkout that keeps its textures elsewhere.
|
||||
DEFAULT_FINGER_TEXTURE = (
|
||||
r"D:\orisys_ws\orisys-sensor-design\build\textures\finger\r01"
|
||||
r"\patch_texture_compute.png"
|
||||
)
|
||||
|
||||
|
||||
def _default_sequence_profile() -> dict[str, Any]:
|
||||
"""The point-generation and run parameters of workflow P5.
|
||||
@@ -157,6 +172,23 @@ def default_preferences() -> dict[str, Any]:
|
||||
"sensor_setup": _default_sensor_setup(),
|
||||
#: P5: grid generation and run parameters.
|
||||
"sequence_profile": _default_sequence_profile(),
|
||||
# --- finger debug screen ---------------------------------------------
|
||||
#: The export bundle the debug screen loads. Empty means "ask", and the
|
||||
#: default below points at the revision the bench confirmed.
|
||||
"finger_bundle_path": DEFAULT_FINGER_BUNDLE,
|
||||
#: The UV-space texture for the flat picker. Preferred over the derived
|
||||
#: path when it exists; empty falls back to the layout map.
|
||||
"finger_texture_path": DEFAULT_FINGER_TEXTURE,
|
||||
#: The mounting plate lifts the sensor along the platform's tool axis, so
|
||||
#: a fixture thickness is one number rather than a transform.
|
||||
"finger_plate_thickness_mm": 0.0,
|
||||
#: Tool tip offset from the platform reference, mm.
|
||||
"finger_tool_offset_mm": [0.0, 0.0, 0.0],
|
||||
#: Stage travel, degrees. The bench's figures: pitch turns continuously,
|
||||
#: yaw is limited — and that limit is what decides how much of the patch
|
||||
#: can be aligned at all.
|
||||
"finger_yaw_limit_deg": 90.0,
|
||||
"finger_pitch_limit_deg": 360.0,
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -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"))
|
||||
@@ -14,6 +14,8 @@ import subprocess
|
||||
import sys
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
|
||||
from orisys_app_shared import API_VERSION, __version__ as shared_version
|
||||
from orisys_app_shared.i18n import set_locale, t
|
||||
from orisys_app_shared.paths import outputs_dir
|
||||
@@ -37,7 +39,10 @@ from PySide6.QtWidgets import (
|
||||
|
||||
from .. import __version__ as app_version
|
||||
from ..motion import AxisParams, ConnectionSpec, MotionPoller, MotionService, Snapshot
|
||||
from ..motion.kinematics import PlatformStack
|
||||
from ..preferences import load_preferences, save_preferences
|
||||
from ..sensor import BundleError, load_bundle, load_uv_texture, rasterise, texture_path_for
|
||||
from ..sensor.uv_flat import DEFAULT_FLAT_SIZE
|
||||
from ..sequence_io import (
|
||||
FILE_SUFFIX,
|
||||
SequenceDocument,
|
||||
@@ -52,6 +57,7 @@ from .brand_bar import BrandBar
|
||||
from .bus_data_panel import BusDataPanel
|
||||
from .bus_panel import BusPanel, default_bas_dir
|
||||
from .connection_panel import ConnectionPanel
|
||||
from .sensor_panel import FingerPanel
|
||||
from .sequence_panel import SequencePanel
|
||||
from .status_strip import RunBar, StatusStrip
|
||||
from .workspace import TABS, WorkspaceTabs
|
||||
@@ -89,7 +95,7 @@ _COMMAND_NAMES = {
|
||||
class AutoCalMainWindow(QMainWindow):
|
||||
"""Platform control, five axes, one window."""
|
||||
|
||||
def __init__(self, prefs: dict | None = None) -> None:
|
||||
def __init__(self, prefs: dict | None = None, *, auto_load_bundle: bool = True) -> None:
|
||||
super().__init__()
|
||||
self.prefs = dict(prefs or load_preferences())
|
||||
self._theme = str(self.prefs.get("theme") or "dark")
|
||||
@@ -98,6 +104,12 @@ class AutoCalMainWindow(QMainWindow):
|
||||
self._pending: dict[int, str] = {}
|
||||
self._last_error = ""
|
||||
|
||||
# Apply the remembered language before anything is built. The translator
|
||||
# is process-global, and only the menu ever set it, so a saved language
|
||||
# was ignored on every launch: the file said en_US and the window came up
|
||||
# in whatever locale the process happened to start in.
|
||||
set_locale(self._language)
|
||||
|
||||
self.setWindowTitle(t("app.title"))
|
||||
self.setMinimumSize(*MINIMUM_WINDOW_SIZE)
|
||||
|
||||
@@ -107,6 +119,12 @@ class AutoCalMainWindow(QMainWindow):
|
||||
|
||||
self._restore_geometry()
|
||||
self.retranslate()
|
||||
|
||||
# Load the remembered bundle, so the finger debugger comes up working.
|
||||
# Without this every session began with a filled-in path over two empty
|
||||
# views, which reads as a broken screen rather than as an unloaded one.
|
||||
if auto_load_bundle:
|
||||
self._load_remembered_bundle()
|
||||
self._apply_theme()
|
||||
|
||||
# --------------------------------------------------------------- build UI
|
||||
@@ -204,7 +222,28 @@ class AutoCalMainWindow(QMainWindow):
|
||||
parent=body,
|
||||
)
|
||||
self.sequence_panel = SequencePanel(self.prefs.get("sequence_profile") or {}, body)
|
||||
self.workspace = WorkspaceTabs(self.axis_panel, self.sequence_panel, body)
|
||||
|
||||
# The finger debug screen: UV picking and a simulated pose, with no
|
||||
# platform connected. Its stack carries the bench's stage travel limits,
|
||||
# because a pose outside them is not a pose.
|
||||
self.finger_stack = PlatformStack(
|
||||
yaw_limit_deg=float(self.prefs.get("finger_yaw_limit_deg") or 90.0),
|
||||
pitch_limit_deg=float(self.prefs.get("finger_pitch_limit_deg") or 360.0),
|
||||
)
|
||||
offset = self.prefs.get("finger_tool_offset_mm") or [0.0, 0.0, 0.0]
|
||||
if isinstance(offset, list) and len(offset) == 3:
|
||||
self.finger_stack.tool_offset_mm = tuple(float(v) for v in offset)
|
||||
self.finger_panel = FingerPanel(
|
||||
self.finger_stack,
|
||||
bundle_path=str(self.prefs.get("finger_bundle_path") or ""),
|
||||
plate_thickness_mm=float(self.prefs.get("finger_plate_thickness_mm") or 0.0),
|
||||
parent=body,
|
||||
)
|
||||
self.finger_panel.set_offset(self.finger_stack.tool_offset_mm)
|
||||
|
||||
self.workspace = WorkspaceTabs(
|
||||
self.axis_panel, self.sequence_panel, self.finger_panel, body
|
||||
)
|
||||
|
||||
self.main_splitter = QSplitter(Qt.Orientation.Horizontal, body)
|
||||
self.main_splitter.addWidget(self.left_rail)
|
||||
@@ -252,6 +291,115 @@ class AutoCalMainWindow(QMainWindow):
|
||||
self.run_bar.abortRequested.connect(self._on_run_abort)
|
||||
self.sequence_panel.exportRequested.connect(self._on_export_sequence)
|
||||
self.sequence_panel.importRequested.connect(self._on_import_sequence)
|
||||
self.finger_panel.loadRequested.connect(self._on_load_bundle)
|
||||
self.finger_panel.pickRequested.connect(self._on_pick_point)
|
||||
self.finger_panel.stackChanged.connect(self._on_finger_stack_changed)
|
||||
|
||||
# ---------------------------------------------------------- finger debug
|
||||
|
||||
def load_bundle(self, path: str, *, flat_size: tuple[int, int] = DEFAULT_FLAT_SIZE):
|
||||
"""Load an export bundle and build its flat view.
|
||||
|
||||
Bundle validation and rasterising are both allowed to fail: a wrong
|
||||
export, a missing file or an unreadable mesh must leave the rest of the
|
||||
app alone and report the reason.
|
||||
"""
|
||||
bundle = load_bundle(
|
||||
path,
|
||||
# The design profile declares these for this revision; a mismatch
|
||||
# means a stale or wrong export.
|
||||
expected_vertices=1032,
|
||||
expected_triangles=1974,
|
||||
)
|
||||
texture_path, texture = self._resolve_texture(bundle)
|
||||
flat = rasterise(bundle, flat_size, texture=texture)
|
||||
self.finger_panel.set_bundle(bundle, flat)
|
||||
self.finger_panel.set_texture_state(textured=flat.textured, path=str(texture_path or ""))
|
||||
self.prefs["finger_bundle_path"] = str(path)
|
||||
info = bundle.describe()
|
||||
self._report_sequence(
|
||||
t(
|
||||
"finger.loaded",
|
||||
vertices=info["vertices"],
|
||||
extent=" × ".join(f"{value:.2f}" for value in info["extent_mm"]),
|
||||
),
|
||||
ok=True,
|
||||
)
|
||||
return bundle
|
||||
|
||||
def _resolve_texture(self, bundle) -> tuple[Path | None, np.ndarray | None]:
|
||||
"""The UV texture for this bundle, or nothing.
|
||||
|
||||
Tried in order: the path recorded in preferences, then the one derived
|
||||
from the bundle's own location. The derived path wins the tie in practice
|
||||
because a texture from another revision would be resampled onto the wrong
|
||||
mesh, and the two are usually the same file anyway.
|
||||
|
||||
Neither being found is not an error — the picker falls back to the layout
|
||||
map and says so.
|
||||
"""
|
||||
candidates = [texture_path_for(bundle)]
|
||||
configured = str(self.prefs.get("finger_texture_path") or "").strip()
|
||||
if configured:
|
||||
candidates.append(Path(configured))
|
||||
for candidate in candidates:
|
||||
if candidate is None or not candidate.is_file():
|
||||
continue
|
||||
texture = load_uv_texture(candidate)
|
||||
if texture is not None:
|
||||
return candidate, texture
|
||||
return (candidates[0] if candidates else None), None
|
||||
|
||||
def _load_remembered_bundle(self) -> None:
|
||||
"""Load whatever bundle the preferences point at, tolerating absence.
|
||||
|
||||
A missing directory is the normal case on a machine that has never
|
||||
exported one, so it is reported quietly in the panel's own status line
|
||||
rather than raised into the startup path.
|
||||
"""
|
||||
path = str(self.prefs.get("finger_bundle_path") or "").strip()
|
||||
if not path or not Path(path).is_dir():
|
||||
return
|
||||
try:
|
||||
self.load_bundle(path)
|
||||
except (BundleError, OSError, ValueError) as exc:
|
||||
self.finger_panel.status_label.setText(
|
||||
t("finger.load_failed", error=str(exc))
|
||||
)
|
||||
|
||||
def _on_load_bundle(self) -> None:
|
||||
start = str(self.prefs.get("finger_bundle_path") or "")
|
||||
path = QFileDialog.getExistingDirectory(
|
||||
self, t("finger.load"), start, QFileDialog.Option.ShowDirsOnly
|
||||
)
|
||||
if not path:
|
||||
return
|
||||
try:
|
||||
self.load_bundle(path)
|
||||
except (BundleError, OSError, ValueError) as exc:
|
||||
self._report_sequence(t("finger.load_failed", error=str(exc)), ok=False)
|
||||
|
||||
def _on_pick_point(self, x: int, y: int) -> None:
|
||||
row = self.finger_panel.handle_pick(x, y)
|
||||
if row is None:
|
||||
# A UV gap or no bundle: the panel says which; do not imply success.
|
||||
self.statusBar().showMessage(self.finger_panel.status_label.text())
|
||||
return
|
||||
self._report_sequence(
|
||||
t(
|
||||
"finger.picked",
|
||||
residual=f"{row['residual']:.3f}",
|
||||
yaw=f"{row['yaw']:.2f}",
|
||||
pitch=f"{row['pitch']:.2f}",
|
||||
),
|
||||
ok=row["residual"] <= 0.5,
|
||||
)
|
||||
|
||||
def _on_finger_stack_changed(self) -> None:
|
||||
self.finger_panel.preview.set_stack(self.finger_panel.stack)
|
||||
self.finger_panel._refresh_reach()
|
||||
self.prefs["finger_plate_thickness_mm"] = self.finger_panel.plate_thickness_mm
|
||||
self.prefs["finger_tool_offset_mm"] = list(self.finger_panel.stack.tool_offset_mm)
|
||||
self.axis_readout.stopAllRequested.connect(self._on_stop_all)
|
||||
self.axis_readout.clearAllRequested.connect(self._on_clear_all_alarms)
|
||||
self.axis_readout.enableAllRequested.connect(self._on_enable_all)
|
||||
@@ -855,6 +1003,7 @@ class AutoCalMainWindow(QMainWindow):
|
||||
self.axis_panel.retranslate()
|
||||
self.axis_readout.retranslate()
|
||||
self.sequence_panel.retranslate()
|
||||
self.finger_panel.retranslate()
|
||||
self.workspace.retranslate()
|
||||
|
||||
self.menu_file.setTitle(t("menu.file"))
|
||||
|
||||
@@ -31,6 +31,7 @@ from .sequence_panel import SequencePanel
|
||||
TABS: tuple[tuple[str, str], ...] = (
|
||||
("axes", "workspace.tab_axes"),
|
||||
("sequence", "workspace.tab_sequence"),
|
||||
("finger", "workspace.tab_finger"),
|
||||
("sensor", "workspace.tab_sensor"),
|
||||
("surface", "workspace.tab_surface"),
|
||||
("points", "workspace.tab_points"),
|
||||
@@ -78,6 +79,7 @@ class WorkspaceTabs(QTabWidget):
|
||||
self,
|
||||
axis_panel: AxisPanel,
|
||||
sequence_panel: SequencePanel,
|
||||
finger_panel: QWidget | None = None,
|
||||
parent: QWidget | None = None,
|
||||
) -> None:
|
||||
super().__init__(parent)
|
||||
@@ -87,13 +89,16 @@ class WorkspaceTabs(QTabWidget):
|
||||
|
||||
self._pages["axes"] = axis_panel
|
||||
self._pages["sequence"] = sequence_panel
|
||||
if finger_panel is not None:
|
||||
self._pages["finger"] = finger_panel
|
||||
for key, locale_key in _PLACEHOLDERS.items():
|
||||
page = PlaceholderPage(locale_key, self)
|
||||
self._pages[key] = page
|
||||
self._placeholder_keys[key] = locale_key
|
||||
|
||||
for key, _locale_key in TABS:
|
||||
self.addTab(self._pages[key], "")
|
||||
if key in self._pages:
|
||||
self.addTab(self._pages[key], "")
|
||||
|
||||
self.currentChanged.connect(self._on_current_changed)
|
||||
self.retranslate()
|
||||
@@ -101,7 +106,8 @@ class WorkspaceTabs(QTabWidget):
|
||||
# -------------------------------------------------------------------- state
|
||||
|
||||
def page_keys(self) -> list[str]:
|
||||
return [key for key, _locale_key in TABS]
|
||||
"""The tabs actually present, in display order."""
|
||||
return [key for key, _locale_key in TABS if key in self._pages]
|
||||
|
||||
def page(self, key: str) -> QWidget:
|
||||
return self._pages[key]
|
||||
@@ -118,7 +124,8 @@ class WorkspaceTabs(QTabWidget):
|
||||
|
||||
def retranslate(self) -> None:
|
||||
for index, (key, locale_key) in enumerate(TABS):
|
||||
self.setTabText(index, t(locale_key))
|
||||
if key in self._pages:
|
||||
self.setTabText(index, t(locale_key))
|
||||
for key, locale_key in self._placeholder_keys.items():
|
||||
page = self._pages[key]
|
||||
if isinstance(page, PlaceholderPage):
|
||||
|
||||
@@ -116,7 +116,10 @@ def make_window(qt_app, monkeypatch):
|
||||
from orisys_app_shared.i18n import set_locale
|
||||
|
||||
set_locale(str(prefs.get("language") or "zh_CN"))
|
||||
window = AutoCalMainWindow(prefs=prefs)
|
||||
# The bundle is not auto-loaded here: rasterising 1024x1024 costs about a
|
||||
# second, and every window in this suite would pay it. The one test that
|
||||
# covers auto-loading turns it back on for itself.
|
||||
window = AutoCalMainWindow(prefs=prefs, auto_load_bundle=False)
|
||||
# Swap in the simulated platform: no test in this suite touches a DLL.
|
||||
window.poller.service._transport = DemoTransport() # noqa: SLF001
|
||||
created.append(window)
|
||||
|
||||
@@ -58,6 +58,9 @@ _SCOPED_ENUMS = {
|
||||
"PenStyle",
|
||||
"GlobalColor",
|
||||
"ItemFlag",
|
||||
"TextElideMode",
|
||||
"TextFlag",
|
||||
"AspectRatioMode",
|
||||
),
|
||||
"QTableWidget": ("SelectionMode", "EditTrigger"),
|
||||
"QSizePolicy": ("Policy",),
|
||||
|
||||
@@ -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()
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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)
|
||||
@@ -101,7 +101,7 @@ STRINGS = {
|
||||
"nodes": "节点数量",
|
||||
"axes_count": "轴数量",
|
||||
"busdata_group": "总线数据 (SDO / RTEX) ▶",
|
||||
"tabs": ["轴调试", "测试序列", "传感器", "表面分布", "标定点"],
|
||||
"tabs": ["轴调试", "测试序列", "手指调试", "传感器", "表面分布", "标定点"],
|
||||
"readout_title": "轴状态",
|
||||
"enabled": "已使能",
|
||||
"disabled": "未使能",
|
||||
@@ -151,6 +151,38 @@ STRINGS = {
|
||||
["3", "4.000", "0.000", "12.400", "待测", "—", "—"],
|
||||
],
|
||||
"points_hint": "坐标由网格生成并逐点校验，不接受手工输入：手工输入的坐标没有任何检查，一个错数字会毁掉整条序列。",
|
||||
"finger_bundle_label": "导出包体",
|
||||
"finger_bundle_value": "D:\\orisys_ws\\orisys-sensor-design\\build\\export\\finger\\r01",
|
||||
"finger_load": "载入包体…",
|
||||
"finger_initial_view": "初始视角",
|
||||
"finger_reset": "复位视角",
|
||||
"finger_textured_note": "UV 贴图：patch_texture_compute.png",
|
||||
"finger_axes_note": "黄色为工具轴 · X/Y 为工作台，Z 为工具行程 · 绿=对齐，红=不可对齐",
|
||||
"finger_bundle_summary": "1032 顶点 · 1974 三角面 · 15.00 × 7.50 × 15.81 mm · 0.018388 mm/单位",
|
||||
"finger_flat_title": "展平图像（点击取点）",
|
||||
"finger_pick_hint": "点击传感面上的像素即可取点；落在 UV 缝隙上的像素会被拒绝。",
|
||||
"finger_readout_title": "读数",
|
||||
"finger_readout_rows": [
|
||||
["传感器 (mm)", "2.841, 3.512, 0.940", None],
|
||||
["法向", "-0.18, -0.88, 0.43", None],
|
||||
["平台 (mm)", "2.841, 0.940, -3.512", None],
|
||||
["X / Y (mm)", "2.841, 0.940", None],
|
||||
["yaw / pitch", "-78.42° / 23.10°", None],
|
||||
["对轴残差", "0.03° 可对轴", "#12b76a"],
|
||||
["坐标系映射", "x→x, y→z, z→-y", None],
|
||||
],
|
||||
"finger_plate_label": "安装板厚 (mm)",
|
||||
"finger_plate_value": "0.000",
|
||||
"finger_preview_title": "三维预览",
|
||||
"finger_preview_legend": "曲面颜色 = 对轴残差（绿为对正，红为对不上） · 黄色为 Z 轴",
|
||||
"finger_point_cols": ["#", "像素 u, v", "sensor x", "sensor y", "sensor z", "nx", "ny", "nz", "X / Y", "Z", "yaw", "pitch"],
|
||||
"finger_point_rows": [
|
||||
["1", "0.35, 0.40", "2.841", "3.512", "0.940", "-0.18", "-0.88", "0.43", "2.841 / 0.940", "7.958", "-78.42", "23.10"],
|
||||
["2", "0.62, 0.55", "5.113", "2.204", "-1.870", "0.21", "0.55", "-0.81", "5.113 / -1.870", "7.958", "12.05", "8.44"],
|
||||
["3", "0.50, 0.72", "3.902", "6.118", "2.405", "0.55", "0.19", "0.81", "3.902 / 2.405", "7.958", "-90.00", "51.66"],
|
||||
],
|
||||
"finger_bad_word": "撞 yaw 行程限位",
|
||||
"finger_point_count": "3 个取点 · 展平 1024 × 1024 · 有效像素 72.9%",
|
||||
"sensor_note": "设备与信号源、实时图像、力曲线",
|
||||
"sensor_curve": "力曲线 (FN / FX / FY)",
|
||||
"surface_note": "多点标定灵敏度 → 三维表面色图",
|
||||
@@ -196,7 +228,7 @@ STRINGS = {
|
||||
"nodes": "Nodes",
|
||||
"axes_count": "Axes",
|
||||
"busdata_group": "Bus data (SDO / RTEX) ▶",
|
||||
"tabs": ["Axis debug", "Test sequence", "Sensor", "Surface map", "Calibration points"],
|
||||
"tabs": ["Axis debug", "Test sequence", "Finger debug", "Sensor", "Surface map", "Calibration points"],
|
||||
"readout_title": "Axis state",
|
||||
"enabled": "Enabled",
|
||||
"disabled": "Disabled",
|
||||
@@ -246,6 +278,38 @@ STRINGS = {
|
||||
["3", "4.000", "0.000", "12.400", "Pending", "—", "—"],
|
||||
],
|
||||
"points_hint": "Coordinates are generated and checked per point; they are not typed. A typed coordinate is one nothing has checked.",
|
||||
"finger_bundle_label": "Export bundle",
|
||||
"finger_bundle_value": "D:\\orisys_ws\\orisys-sensor-design\\build\\export\\finger\\r01",
|
||||
"finger_load": "Load bundle…",
|
||||
"finger_initial_view": "Initial view",
|
||||
"finger_reset": "Reset view",
|
||||
"finger_textured_note": "UV texture: patch_texture_compute.png",
|
||||
"finger_axes_note": "Yellow is the tool axis · X/Y are the table, Z the tool travel · green aligned, red not",
|
||||
"finger_bundle_summary": "1032 vertices · 1974 triangles · 15.00 × 7.50 × 15.81 mm · 0.018388 mm per unit",
|
||||
"finger_flat_title": "Flat image (click to pick)",
|
||||
"finger_pick_hint": "Click a pixel on the sensing surface to pick a point; pixels in a UV gap are refused.",
|
||||
"finger_readout_title": "Readouts",
|
||||
"finger_readout_rows": [
|
||||
["Sensor (mm)", "2.841, 3.512, 0.940", None],
|
||||
["Normal", "-0.18, -0.88, 0.43", None],
|
||||
["Platform (mm)", "2.841, 0.940, -3.512", None],
|
||||
["X / Y (mm)", "2.841, 0.940", None],
|
||||
["yaw / pitch", "-78.42° / 23.10°", None],
|
||||
["Residual", "0.03° can align", "#12b76a"],
|
||||
["Frame map", "x→x, y→z, z→-y", None],
|
||||
],
|
||||
"finger_plate_label": "Mounting plate (mm)",
|
||||
"finger_plate_value": "0.000",
|
||||
"finger_preview_title": "3D preview",
|
||||
"finger_preview_legend": "Surface colour = alignment residual (green aligned, red not) · yellow is the Z axis",
|
||||
"finger_point_cols": ["#", "pixel u, v", "sensor x", "sensor y", "sensor z", "nx", "ny", "nz", "X / Y", "Z", "yaw", "pitch"],
|
||||
"finger_point_rows": [
|
||||
["1", "0.35, 0.40", "2.841", "3.512", "0.940", "-0.18", "-0.88", "0.43", "2.841 / 0.940", "7.958", "-78.42", "23.10"],
|
||||
["2", "0.62, 0.55", "5.113", "2.204", "-1.870", "0.21", "0.55", "-0.81", "5.113 / -1.870", "7.958", "12.05", "8.44"],
|
||||
["3", "0.50, 0.72", "3.902", "6.118", "2.405", "0.55", "0.19", "0.81", "3.902 / 2.405", "7.958", "-90.00", "51.66"],
|
||||
],
|
||||
"finger_bad_word": "hits yaw travel limit",
|
||||
"finger_point_count": "3 picked points · flat 1024 × 1024 · 72.9% valid pixels",
|
||||
"sensor_note": "Device and source, live image, force curves",
|
||||
"sensor_curve": "Force curves (FN / FX / FY)",
|
||||
"surface_note": "Multi-point calibration sensitivity → 3D surface colour map",
|
||||
@@ -875,16 +939,151 @@ def draw_points_tab(painter: QPainter, palette: Palette, s: dict, rect: QRect) -
|
||||
_button(painter, QRect(inner.x() + 250, y, 100, 28), "删除" if s["axis_label"] == "轴号" else "Remove", palette)
|
||||
|
||||
|
||||
def draw_finger_tab(painter: QPainter, palette: Palette, s: dict, rect: QRect) -> None:
|
||||
"""The finger debugger: pick on the flat image, read the pose, check the 3D preview."""
|
||||
inner = rect.adjusted(10, 8, -10, -8)
|
||||
zh = s["axis_label"] == "轴号"
|
||||
y = inner.y()
|
||||
|
||||
_text(painter, QRect(inner.x(), y, 90, 24), s["finger_bundle_label"], palette, size=10, muted=True)
|
||||
_field(painter, QRect(inner.x() + 92, y, 300, 24), s["finger_bundle_value"], palette)
|
||||
_button(painter, QRect(inner.x() + 400, y, 100, 24), s["finger_load"], palette)
|
||||
_button(painter, QRect(inner.x() + 508, y, 100, 24), s["finger_initial_view"], palette)
|
||||
_button(painter, QRect(inner.x() + 616, y, 100, 24), s["finger_reset"], palette)
|
||||
y += 34
|
||||
_text(painter, QRect(inner.x(), y, inner.width(), 18), s["finger_bundle_summary"], palette, size=9, muted=True)
|
||||
y += 24
|
||||
|
||||
body_h = int((inner.height() - (y - inner.y())) * 0.62)
|
||||
table_h = inner.bottom() - (y + body_h) - 8
|
||||
gap = 10
|
||||
usable = inner.width() - 2 * gap
|
||||
picker_w = int(usable * 28 / 100)
|
||||
readout_w = int(usable * 32 / 100)
|
||||
preview_w = usable - picker_w - readout_w
|
||||
|
||||
picker = QRect(inner.x(), y, picker_w, body_h)
|
||||
readouts = QRect(picker.right() + gap, y, readout_w, body_h)
|
||||
preview = QRect(readouts.right() + gap, y, preview_w, body_h)
|
||||
|
||||
# Flat unrolled image.
|
||||
inner_picker = _panel(painter, picker, palette)
|
||||
_text(painter, QRect(inner_picker.x(), inner_picker.y(), inner_picker.width(), 18), s["finger_flat_title"], palette, size=10, bold=True)
|
||||
flat = QRect(inner_picker.x(), inner_picker.y() + 22, inner_picker.width(), inner_picker.height() - 22 - 40)
|
||||
# The textured UV space: black outside the island, artwork inside it.
|
||||
painter.fillRect(flat, QColor("#000000"))
|
||||
island = flat.adjusted(int(flat.width() * 0.06), int(flat.height() * 0.16), -int(flat.width() * 0.06), -int(flat.height() * 0.16))
|
||||
painter.setPen(QPen(QColor("#000000"), 0))
|
||||
columns, rows = 22, 14
|
||||
for row in range(rows):
|
||||
for column in range(columns):
|
||||
cell = QRect(
|
||||
island.x() + column * island.width() // columns,
|
||||
island.y() + row * island.height() // rows,
|
||||
island.width() // columns + 1,
|
||||
island.height() // rows + 1,
|
||||
)
|
||||
shade = ((row * 7 + column * 13) % 5) / 4.0
|
||||
painter.fillRect(
|
||||
cell,
|
||||
QColor(int(60 + 150 * shade), int(40 + 120 * ((row + column) % 3) / 2), int(90 + 120 * (1 - shade))),
|
||||
)
|
||||
for index, (fx, fy) in enumerate(((0.35, 0.4), (0.62, 0.55), (0.5, 0.72))):
|
||||
mark = QPoint(int(flat.x() + fx * flat.width()), int(flat.y() + fy * flat.height()))
|
||||
painter.setPen(QPen(QColor("#ffffff"), 2))
|
||||
painter.setBrush(QColor("#d92d20") if index == 2 else QColor("#12b76a"))
|
||||
painter.drawEllipse(mark, 4, 4)
|
||||
# Leave no brush behind: a later panel fills its background, and a leaked
|
||||
# brush turns that fill into the marker colour.
|
||||
painter.setBrush(Qt.BrushStyle.NoBrush)
|
||||
_text(painter, QRect(inner_picker.x(), flat.bottom() + 2, inner_picker.width(), 18), s["finger_textured_note"], palette, size=9, muted=True)
|
||||
_text(painter, QRect(inner_picker.x(), flat.bottom() + 20, inner_picker.width(), 18), s["finger_pick_hint"], palette, size=9, muted=True)
|
||||
|
||||
# Readouts.
|
||||
inner_read = _panel(painter, readouts, palette)
|
||||
_text(painter, QRect(inner_read.x(), inner_read.y(), inner_read.width(), 18), s["finger_readout_title"], palette, size=10, bold=True)
|
||||
row_y = inner_read.y() + 24
|
||||
label_w = int(inner_read.width() * 0.42)
|
||||
for label, value, colour in s["finger_readout_rows"]:
|
||||
_text(painter, QRect(inner_read.x(), row_y, label_w, 20), label, palette, size=9, muted=True)
|
||||
_text(painter, QRect(inner_read.x() + label_w, row_y, inner_read.width() - label_w, 20), value, palette, size=9, colour=QColor(colour) if colour else None)
|
||||
row_y += 20
|
||||
_text(painter, QRect(inner_read.x(), inner_read.bottom() - 36, inner_read.width(), 18), s["finger_plate_label"], palette, size=9, muted=True)
|
||||
_field(painter, QRect(inner_read.x(), inner_read.bottom() - 20, inner_read.width(), 20), s["finger_plate_value"], palette)
|
||||
|
||||
# 3D preview: the tool axis and a normal with its residual.
|
||||
inner_preview = _panel(painter, preview, palette)
|
||||
_text(painter, QRect(inner_preview.x(), inner_preview.y(), inner_preview.width(), 18), s["finger_preview_title"], palette, size=10, bold=True)
|
||||
view = QRect(inner_preview.x(), inner_preview.y() + 22, inner_preview.width(), inner_preview.height() - 22 - 34)
|
||||
painter.fillRect(view, QColor("#12161c"))
|
||||
painter.setPen(QPen(QColor("#2a313c"), 1))
|
||||
for i in range(1, 4):
|
||||
painter.drawLine(view.x() + i * view.width() // 4, view.y(), view.x() + i * view.width() // 4, view.bottom())
|
||||
cx, cy = view.center().x(), view.center().y() + 40
|
||||
axis = QPoint(cx, view.y() + 24)
|
||||
painter.setPen(QPen(QColor("#f2c94c"), 3))
|
||||
painter.drawLine(axis, QPoint(cx, cy))
|
||||
painter.setBrush(QColor("#f2c94c"))
|
||||
painter.drawEllipse(QPoint(cx, cy), 3, 3)
|
||||
# The motion axes: the platform frame, drawn in the sensor frame.
|
||||
for colour, end in (
|
||||
("#e65454", QPoint(cx - 96, cy + 18)),
|
||||
("#61cc61", QPoint(cx + 30, cy + 92)),
|
||||
("#5c8cf2", QPoint(cx + 88, cy - 26)),
|
||||
):
|
||||
painter.setPen(QPen(QColor(colour), 2))
|
||||
painter.drawLine(QPoint(cx, cy), end)
|
||||
painter.setPen(QPen(QColor("#12b76a"), 3))
|
||||
painter.drawLine(QPoint(cx, cy), QPoint(cx + 26, cy - 96))
|
||||
painter.setPen(QPen(QColor("#d92d20"), 3))
|
||||
painter.drawLine(QPoint(cx, cy), QPoint(cx - 74, cy - 78))
|
||||
painter.setBrush(Qt.BrushStyle.NoBrush)
|
||||
painter.setPen(QPen(QColor("#4a5568"), 1))
|
||||
painter.drawLine(QPoint(cx - 90, cy), QPoint(cx + 90, cy))
|
||||
painter.setPen(QPen(QColor("#e65454"), 1))
|
||||
painter.drawText(QPoint(cx - 108, cy + 24), "X")
|
||||
painter.setPen(QPen(QColor("#61cc61"), 1))
|
||||
painter.drawText(QPoint(cx + 34, cy + 100), "Y")
|
||||
painter.setPen(QPen(QColor("#5c8cf2"), 1))
|
||||
painter.drawText(QPoint(cx + 92, cy - 28), "Z")
|
||||
_text(painter, QRect(inner_preview.x(), view.bottom() + 2, inner_preview.width(), 30), s["finger_axes_note"], palette, size=8, muted=True)
|
||||
|
||||
# Picked points.
|
||||
table = QRect(inner.x(), y + body_h + 8, inner.width(), max(60, table_h))
|
||||
painter.fillRect(table, palette.window)
|
||||
painter.setPen(QPen(palette.border, 1))
|
||||
painter.drawRect(table.adjusted(0, 0, -1, -1))
|
||||
header_h = 24
|
||||
painter.fillRect(QRect(table.x(), table.y(), table.width(), header_h), palette.panel)
|
||||
columns = s["finger_point_cols"]
|
||||
widths = [int(table.width() * share) for share in (0.06, 0.09, 0.1, 0.09, 0.09, 0.07, 0.07, 0.07, 0.09, 0.09, 0.09, 0.09)]
|
||||
x = table.x()
|
||||
for name, width in zip(columns, widths):
|
||||
_text(painter, QRect(x, table.y(), width, header_h), name, palette, size=9, muted=True, align=Qt.AlignmentFlag.AlignCenter)
|
||||
x += width
|
||||
for index, row in enumerate(s["finger_point_rows"]):
|
||||
row_y = table.y() + header_h + index * 26
|
||||
if index % 2 == 0:
|
||||
painter.fillRect(QRect(table.x() + 1, row_y, table.width() - 2, 25), palette.panel)
|
||||
x = table.x()
|
||||
for value, width in zip(row, widths):
|
||||
colour = palette.danger if value == s["finger_bad_word"] else None
|
||||
_text(painter, QRect(x, row_y, width, 26), value, palette, size=10, align=Qt.AlignmentFlag.AlignCenter, colour=colour)
|
||||
x += width
|
||||
_text(painter, QRect(table.x() + 8, table.bottom() + 2, table.width(), 18), s["finger_point_count"], palette, size=9, muted=True)
|
||||
|
||||
|
||||
TAB_PAINTERS = {
|
||||
"axes": draw_axis_tab,
|
||||
"sequence": draw_sequence_tab,
|
||||
"finger": draw_finger_tab,
|
||||
"sensor": draw_sensor_tab,
|
||||
"surface": draw_surface_tab,
|
||||
"points": draw_points_tab,
|
||||
}
|
||||
|
||||
#: Tab index -> painter key, for the tab strip labels.
|
||||
TAB_KEYS = ("axes", "sequence", "sensor", "surface", "points")
|
||||
TAB_KEYS = ("axes", "sequence", "finger", "sensor", "surface", "points")
|
||||
|
||||
|
||||
def draw_page(
|
||||
@@ -983,8 +1182,10 @@ def main() -> int:
|
||||
parser.add_argument("--height", type=int, default=PAGE_H)
|
||||
parser.add_argument(
|
||||
"--page",
|
||||
default="axes",
|
||||
choices=("axes", "sequence", "sensor", "surface", "points", "all"),
|
||||
default=TAB_KEYS[0],
|
||||
# Derived, not listed: a literal here silently rejects any tab added to
|
||||
# TAB_KEYS, which is how the finger page first went missing.
|
||||
choices=(*TAB_KEYS, "all"),
|
||||
help="which page to draw; 'all' writes one full-size image per page",
|
||||
)
|
||||
args = parser.parse_args()
|
||||
|
||||
@@ -145,7 +145,8 @@ def main() -> int:
|
||||
|
||||
# ---- the permanent shell ----------------------------------------
|
||||
checker.that(
|
||||
window.workspace.page_keys() == ["axes", "sequence", "sensor", "surface", "points"],
|
||||
window.workspace.page_keys()
|
||||
== ["axes", "sequence", "finger", "sensor", "surface", "points"],
|
||||
f"unexpected tab set: {window.workspace.page_keys()}",
|
||||
)
|
||||
checker.that(
|
||||
|
||||
Reference in New Issue
Block a user