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