From 3347d17a208cfb28154cde91c0fd7b5e683105ec Mon Sep 17 00:00:00 2001 From: ykcheungab Date: Wed, 23 Sep 2026 09:58:00 +0800 Subject: [PATCH] added motion trajectory simulation --- apps/autocal/README.md | 13 +- apps/autocal/config/locales/en_US.json | 29 +- apps/autocal/config/locales/zh_CN.json | 29 +- apps/autocal/docs/finger-debug-findings.md | 41 +- apps/autocal/docs/finger-preview.md | 211 +++ apps/autocal/docs/inverse-kinematics.md | 242 +++ apps/autocal/src/orisys_autocal/main.py | 12 + .../src/orisys_autocal/motion/kinematics.py | 498 ++++++- .../autocal/src/orisys_autocal/preferences.py | 64 +- .../src/orisys_autocal/ui/pose_preview.py | 1304 ++++++++++++++--- .../src/orisys_autocal/ui/sensor_panel.py | 738 ++++++++-- apps/autocal/src/orisys_autocal/ui/window.py | 53 +- apps/autocal/tests/conftest.py | 26 + apps/autocal/tests/test_binding_purity.py | 2 +- apps/autocal/tests/test_finger_panel.py | 1035 ++++++++++++- apps/autocal/tests/test_ik_analytical.py | 334 +++++ apps/autocal/tests/test_kinematics.py | 217 ++- apps/autocal/tests/test_layout_shell.py | 586 -------- apps/autocal/tests/test_motion_service.py | 328 ----- apps/autocal/tests/test_panel_smoke.py | 331 ----- .../tests/test_preferences_isolation.py | 64 + apps/autocal/tools/layout_mockup.py | 287 +++- apps/autocal/tools/panel_check.py | 15 + apps/autocal/tools/preview_snapshot.py | 492 +++++++ shared/src/orisys_app_shared/theme/qss.py | 9 + 25 files changed, 5198 insertions(+), 1762 deletions(-) create mode 100644 apps/autocal/docs/finger-preview.md create mode 100644 apps/autocal/docs/inverse-kinematics.md create mode 100644 apps/autocal/tests/test_ik_analytical.py delete mode 100644 apps/autocal/tests/test_layout_shell.py delete mode 100644 apps/autocal/tests/test_motion_service.py delete mode 100644 apps/autocal/tests/test_panel_smoke.py create mode 100644 apps/autocal/tests/test_preferences_isolation.py create mode 100644 apps/autocal/tools/preview_snapshot.py diff --git a/apps/autocal/README.md b/apps/autocal/README.md index 5bdd0ae..f8d2365 100644 --- a/apps/autocal/README.md +++ b/apps/autocal/README.md @@ -95,7 +95,7 @@ apps/autocal/ motion/ # Qt-free except poller.py (the Qt worker bridge) models.py errors.py units.py service.py transport.py poller.py ui/ # window.py + one module per panel - tools/ # panel_check.py, panel_snapshot.py + tools/ # panel_check.py, panel_snapshot.py, preview_snapshot.py, layout_mockup.py tests/ ``` @@ -109,20 +109,27 @@ between threads. The UI talks to it through signals only. Both work with no display, no controller and no DLL: ```powershell -python .\apps\autocal\tools\panel_check.py # 120 layout/theme/locale/gating checks +python .\apps\autocal\tools\panel_check.py # 170 layout/theme/locale/gating checks python .\apps\autocal\tools\panel_snapshot.py --demo --theme dark --out shot.png python .\apps\autocal\tools\panel_snapshot.py --compare --out tabs.png # one image per tab python .\apps\autocal\tools\layout_mockup.py --out layout.png # proposed layout, drawn python -m pytest apps\autocal\tests # offscreen suite ``` +`preview_snapshot.py` is the one tool that can photograph the **3D pane**, which +`panel_snapshot.py` cannot: the offscreen platform has no OpenGL context at all, so +the pane is empty in those shots. It needs the real platform plugin and a visible +window, and it writes either the pane alone or the whole page with the pane +composited in. What the view draws, and what each colour means, is in +[docs/finger-preview.md](docs/finger-preview.md). + `layout_mockup.py` is a **wireframe, not the application**: it paints the proposed single-page layout with the shared theme tokens and imports nothing from the app, so the layout can be reviewed and marked up without building the panels. It draws the full page plus previews of the other workspace tabs in one canvas. -`panel_check.py` builds the real window in both themes at 1280×720, 1600×900 and +`panel_check.py` builds the real window in both themes at 1440×900 and 1920×1080, asserts that no widget escapes its parent, that motion is unreachable before the bus is initialised, that the SDO/RTEX panels follow the reported bus family, and then runs a full simulated session from `ECAT.bas`. diff --git a/apps/autocal/config/locales/en_US.json b/apps/autocal/config/locales/en_US.json index 0da67f5..bc26dd9 100644 --- a/apps/autocal/config/locales/en_US.json +++ b/apps/autocal/config/locales/en_US.json @@ -210,20 +210,37 @@ "row_sensor_normal": "Surface normal", "row_platform_point": "Platform point (mm)", "row_pose": "Pose X / Y / yaw / pitch", + "row_table_move": "Table move X / Y", "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", + "tool_xy": "Tool X / Y", + "tool_hint": "Where the tool is in the table frame, straight above the table origin by default. The table move is what carries the contact to this point. Measure it on the real setup.", + "sensor_xy": "Sensor X / Y", + "sensor_zy": "Sensor Z / yaw", + "setup_hint": "How the sensor actually sits on the stack with the machine at zero: X/Y/Z translation and a yaw about Z. Z is the mounting plate, shims included — there is no separate plate field. Measure it on the real setup.", "clear_points": "Clear points", + "remove_selected": "Remove selected", + "move_up": "Move up", + "move_down": "Move down", + "selected_count": "{count} selected", + "table_hint": "Tick a row to select it, double-click a sensor coordinate or normal to edit it. The pose columns are solved from those, so they update themselves.", + "edit_not_a_number": "That cell needs a number; the previous value was kept.", "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", + "preview_legend": "What the colours mean: green→red is the surface's alignment residual · the red/green/blue triad is the table's X/Y/Z, at the origin · yellow line is the tool axis (+Z, at the tool's own X/Y) · orange dot is the table origin · amber outline is the sensor base plane, at the sensor's Z offset (the mounting plate) · cyan line is the planned path (stage arc, then table move) · the violet ring is the table's pose now and the faint one is where it finishes, against the fixed grid that is where it started · red dot is the point selected in the table · white dot with blue line is the picked contact and its normal.", + "preview_legend_short": "green→red = alignment residual · red/green/blue = table X/Y/Z · yellow = tool axis (+Z) · orange = origin · amber box = base plane · cyan = planned path · violet ring = table pose · white dot = contact", "show_axes": "Show motion axes", + "show_table": "Show table origin plane", + "show_trajectory": "Show tool path", + "path_summary": "Planned path: travels {reach} mm; arc radius {radius} mm about the table origin.", + "landing": "lands X/Y {x} {y} mm", + "tool_at": "tool X/Y {x} {y} mm", + "landing_on_tool": "✓ on the tool", + "landing_off_tool": "✗ off the tool by {gap} mm", + "preview_legend_table": "The grey lines crossing at one origin are the two rotation axes. The grid is the initial state and does not move; the violet ring is the table's own frame, which the move carries away from it.", "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.", @@ -234,7 +251,7 @@ "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", + "loaded": "Bundle loaded: {vertices} vertices · {triangles} triangles · {extent} mm · {scale} mm per unit", "load_failed": "Load failed: {error}", "picked": "Picked: residual {residual}° · yaw {yaw}° · pitch {pitch}°" }, diff --git a/apps/autocal/config/locales/zh_CN.json b/apps/autocal/config/locales/zh_CN.json index 0b3cc24..b6a423b 100644 --- a/apps/autocal/config/locales/zh_CN.json +++ b/apps/autocal/config/locales/zh_CN.json @@ -210,20 +210,37 @@ "row_sensor_normal": "曲面法向", "row_platform_point": "平台坐标 (mm)", "row_pose": "位姿 X / Y / yaw / pitch", + "row_table_move": "工作台移动 X / Y", "row_residual": "对齐残差", "row_frames": "坐标映射", - "row_bundle": "导出包", - "bundle_summary": "{vertices} 顶点 · {triangles} 三角面 · {extent} mm · 每单位 {scale} mm", - "plate_thickness": "安装板厚度", - "tool_offset": "工具偏移 x,y,z", + "tool_xy": "刀具 X / Y", + "tool_hint": "刀具在工作台坐标系中的位置，默认在原点正上方；工作台的移动量就是把接触点送到此处所需的位移。请按实际安装测量填写。", + "sensor_xy": "传感器 X / Y", + "sensor_zy": "传感器 Z / yaw", + "setup_hint": "传感器在转台零位时相对工作台原点的实际安装位姿：X/Y/Z 平移与绕 Z 的偏摆。Z 即安装板厚度（含垫片），无需另填。请按实际安装测量填写。", "clear_points": "清空标定点", + "remove_selected": "删除所选", + "move_up": "上移", + "move_down": "下移", + "selected_count": "已选 {count} 点", + "table_hint": "勾选复选框选中该点；双击传感器坐标或法向即可修改。位姿列由这些数值解算得出，会自动更新。", + "edit_not_a_number": "该单元格需要数字，已保留原值。", "point_count": "共 {count} 点", "residual_value": "{residual}° — {verdict}", "reachable": "该点法向可与 Z 轴对齐", "unreachable": "该点法向无法与 Z 轴对齐（受 yaw 行程限制）", "reach_summary": "曲面可对齐顶点 {aligned}/{vertices}（{share}%），最差 {worst}°", - "preview_legend": "表面颜色 = 对齐残差（绿=对齐，红=不可对齐）· 黄色为工具轴 · X/Y 为工作台，Z 为工具行程", + "preview_legend": "颜色含义：绿→红=表面对齐残差；红/绿/蓝三色轴=工作台 X/Y/Z（位于原点）；黄色直线=刀具轴（+Z 目标方向，位置即刀具 X/Y）；橙色圆点=工作台原点；黄色方框=传感器安装基面（高度=传感器 Z，含安装板）；青色线=规划路径（先转台圆弧、后工作台直线）；紫圈=转台当前姿态，浅紫圈=动作结束时的姿态，与不动的方格网对比即可看出整个变换；红点=表中选中点；白点+蓝线=取点接触点及其法向。", + "preview_legend_short": "绿→红=对齐残差 · 红/绿/蓝=工作台 X/Y/Z · 黄线=刀具轴(+Z) · 橙点=原点 · 黄框=安装基面 · 青线=规划路径 · 紫圈=转台姿态 · 白点=接触点", "show_axes": "显示运动轴", + "show_table": "显示转台原点平面", + "show_trajectory": "显示刀具路径", + "path_summary": "规划路径：行程 {reach} mm；绕转台原点的圆弧半径 {radius} mm。", + "landing": "落点 X/Y {x} {y} mm", + "tool_at": "刀具 X/Y {x} {y} mm", + "landing_on_tool": "✓ 已对准", + "landing_off_tool": "✗ 未对准（差 {gap} mm）", + "preview_legend_table": "灰细线=两个旋转轴（穿过同一个原点）；转台坐标系固定不动，传感器随动作相对它移动。", "sim_play": "播放动作", "sim_stop": "停止", "sim_hint": "播放该点的求解动作：先由转台把法向转到工具轴上，再由 XY 工作台移动。请先取点。", @@ -234,7 +251,7 @@ "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", + "loaded": "已载入导出包：{vertices} 顶点 · {triangles} 三角面 · {extent} mm · 每单位 {scale} mm", "load_failed": "载入失败: {error}", "picked": "已选点：残差 {residual}° · yaw {yaw}° · pitch {pitch}°" }, diff --git a/apps/autocal/docs/finger-debug-findings.md b/apps/autocal/docs/finger-debug-findings.md index f9810d5..4365b11 100644 --- a/apps/autocal/docs/finger-debug-findings.md +++ b/apps/autocal/docs/finger-debug-findings.md @@ -41,28 +41,23 @@ Because the atlas is a single island, a click maps to exactly one 3D point. 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: +machine.** -| 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% | +A later revision then reported the residual as 0.21 deg worst with the yaw limit +"not the constraint". **That was also wrong**, in the opposite direction: the +limit itself was wrong. The bench confirmed the yaw stage turns a **full half-turn +either way (+-180 deg)**, not +-90. -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. +With the corrected envelope, solved by closed form +(see `inverse-kinematics.md`): -Why the yaw limit is not the constraint, which the earlier note had backwards: +| Envelope | Worst residual | Within 0.5 deg | In range | +|---|---|---|---| +| pitch 360 deg, **yaw +-180 deg** (the machine) | **1.2e-06 deg** | 100% | **1032 / 1032** | -- 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. +Every vertex is reachable, and the residual is at the floating-point floor rather +than at any clamp. The "0.21 deg worst, genuinely clamped vertex" claim above was +an artefact of the wrong limit and no longer holds. **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 @@ -70,6 +65,12 @@ the reachability evidence. Whether they help *repeatability* — a shorter pitch move, a better-conditioned contact — is a separate bench question, not a kinematic one. +One thing the corrected solve did surface: **51 of 1032 vertices end inverted**, +whichever closed-form branch is chosen, because the tool direction is +antiparallel to the sensor's own up axis. That is the tool-polarity question, it +needs the bench, and it is *not* visible in the residual. See +`inverse-kinematics.md` → *Still open*. + ## What was wrong before, and why it is worth recording **Four** solver versions preceded this one, each wrong in a way that produced @@ -104,9 +105,11 @@ as a number rather than as a plausible pose. | `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 | +| `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. The scene, the palette and the framing rule are described in [finger-preview.md](finger-preview.md) | +| `motion/kinematics.py` | closed-form solve, exact to 1.2e-06°; the sweep is kept only as the reference the tests check it against — see [inverse-kinematics.md](inverse-kinematics.md) | | 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 | +| `tools/preview_snapshot.py` | photographs the 3D pane with a real GL context, at rest or mid-playback, pane-only or composited into the page | Not yet built: capture of picked points into the sequence, and the live-sensor counterpart of the simulated pose. diff --git a/apps/autocal/docs/finger-preview.md b/apps/autocal/docs/finger-preview.md new file mode 100644 index 0000000..c6c2d1b --- /dev/null +++ b/apps/autocal/docs/finger-preview.md @@ -0,0 +1,211 @@ +# The finger screen's 3D view + +What the picture draws, what each colour means, and which of those meanings is a +measurement rather than a convenience. Written because the view had grown two +coordinate systems, a path, three markers and four competing hues, and the question +"which line is which?" was being answered from memory. + +## What is in the scene + +| Element | Colour | What it is | +| --- | --- | --- | +| Sensing surface | green → red, shaded per vertex | **The measurement.** Green is an aligned normal, red is `BAD_RESIDUAL_DEG` (20°) or worse of residual. The shading stays on during playback. | +| Shell | dark grey wireframe | The housing mesh from the export, drawn so the face has a body to belong to. | +| Origin plane | grey grid, translucent | The platform's `z = 0` plane, i.e. the plane the two rotation axes lie in. **This is the initial state and it never moves.** | +| Table marker | violet ring + cross-hairs | The table's *own* frame as it now stands: the stages' rotation and the table's XY applied to a circle drawn in the origin plane. While a move has not finished, a fainter ring shows where it ends up. Square grid = before, violet ring = now, faint ring = after. | +| Table triad | red / green / blue, full strength | The machine's X/Y/Z, drawn at the **table origin**. The only full-strength triad in the picture, so "the machine" is readable at a glance. | +| Rotation axes | grey, long | The yaw and pitch axes. They must cross at the one origin; a misalignment shows as the two lines missing each other instead of hiding inside the body. | +| Origin | orange dot | The point the stages turn and the tables translate *reference to*. | +| Base plane | amber outline | The sensor's mounting face: part of the **body**, so it turns with the stages and rides the table. Its height in the initial state is the sensor's z setup offset — the mounting plate. | +| Tool axis | yellow line, arrowhead | The `+Z` direction the sensing face has to meet, drawn at the tool's own position in the table frame. **It does not follow the contact**: the tool is bolted to the machine and the table brings the point to it. | +| Planned path | cyan line with arrowheads, cyan end dot | The contact's route: the stage rotation arc about the table origin, then the straight table move. | +| Picked contact | white dot | The point that was picked on the UV map. It travels onto the tool during playback. | +| Contact normal | blue line, arrowhead | The picked vertex's normal. It is what the solved rotation aims at the tool axis. | +| Selected point | red dot | The point selected in the table below — the one a run would start from. | + +## Setup: where the tool and the sensor actually are + +Three numbers that are the same on the drawing and never the same on the bench. All +three default to zero, and all three have to be measured on the real machine: + +| Field | Meaning | +| --- | --- | +| **Tool X / Y** | The tool's position in the table frame. Every XY move exists to bring the contact here, so this is what the reported move is measured against. It used to be taken as the patch's centre — a stand-in that made the picture self-consistent and the numbers meaningless. **A tool-tip offset is this field**, not a second one. It *was* a second one, added to the reported coordinates while the drawing left the geometry alone: a machine with a 10 mm tip offset reported one landing point and drew another, 10 mm away. A stored `finger_tool_offset_mm` is folded in here when preferences load. | +| **Sensor X / Y** | How far the mounted sensor is from the table origin, along the table's own axes. | +| **Sensor Z / yaw** | The mounting height — **this is the mounting plate**, shims included, so there is no separate plate field — and the sensor's misalignment with the table about its z. | + +### What the screen shows at rest, and what playing it does + +The screen is a solve view, so **at rest everything stands at the end of the move**: +the body, the contact marker, the tool's position and the table marker. The fixed +grid is the frame they started in and the cyan path is drawn from where the contact +was, so "will it reach, and where does the table finish" is answerable without +watching anything. Pressing play runs the move from the initial state to that same +end state — which is what makes the path checkable rather than decorative. + +The one line at the bottom of the rail is the x-y check, printed as two pairs: + +``` +落点 X/Y +10.000 +0.000 mm / 刀具 X/Y +10.000 +0.000 mm ✓ 已对准 +``` + +They are computed two different ways on purpose — the landing by pushing the contact +through the *drawing's* transform, the tool from its setup number — so "x and y are +aligned" is a measurement rather than a judgement about a picture. Note that the +reported move is **not** simply `tool − platform position`: the stages turn the point +about the origin first, so the move is `tool_xy − (R_pitch · R_yaw · p_table).xy`. +When the two do not match, the line says so, with the distance. + +A point of view worth knowing: at rest the sensing face is aimed along the tool axis, +which can put the face on the far side of the body from the default camera. The tool +axis is drawn with depth testing **off** for that reason — it is a reference line, +and hiding it behind the body it is meant to align with is the opposite of useful. + +The chain the solver and the drawing share is, in order: **mount** (sensor axes into +table axes) → **setup** (the mounting pose above) → **stages** (the solved yaw and +pitch, about the table origin) → **table** (the XY slide). A point of the sensor is +therefore at + +``` +p_table = R_z(sensor_yaw) · (mount · p_sensor) + sensor_offset +p_final = R_pitch · R_yaw · p_table + (table_x, table_y, 0) +``` + +and the solver reports the move that puts the contact at the tool: + +``` +(table_x, table_y) = tool_xy − (R_pitch · R_yaw · p_table).xy +``` + +Two consequences worth knowing, both covered by tests: + +* the mounting yaw is **absorbed by the stage** — a sensor mounted 10° out needs the + yaw stage to turn 10° less, which is why the field exists; +* the mounting z is a lift along the table's z, which the mounting yaw leaves alone, + which is why the plate can live in that same field instead of being a second + number that drifts away from it. + +`Pose.target_xy_mm` says where the contact *is*; `Pose.table_xy_mm` says what the +table must *do* about it. The panel shows both, and the trajectory summary is the +same claim again: the contact's last drawn position **is** the tool, to within +1e-6 mm (`test_the_path_ends_where_the_pose_says_the_table_moves_to`). + +## The path, and where it starts + +`trajectory()` returns the contact's route in the order the machine moves it: + +1. **rotation** — the stages turn the mounted body about the table origin, sweeping + the contact along an arc whose radius is the contact's distance from that origin + (reported as `arc_radius_mm`); +2. **translation** — the table slides the contact onto the tool axis, in the table's + own XY, in a straight line (reported as `reach_mm`, the total distance travelled). + +Nothing in the path is *at* the table origin. The origin is what the stages turn +about; the path is traced by a point on the body, so it starts at the picked contact +and ends under the tool. The two numbers a reader needs in order to judge the path — +how far the contact travels, and how large the swing is — are printed beside the +readouts. + +The palette lives in one place, `pose_preview.COLOUR`, with a rule the view used to +break: **a colour identifies exactly one thing**. Two things sharing a hue read as a +duplicate or a contradiction, which is what "messy" turned out to mean. An import +check (`_check_palette_has_no_collisions`) refuses a duplicate, and +`set_colour`/`set_rgb` accept either 3- or 4-channel entries so the length of a +palette tuple can never abort a paint again — it did once, and silently deleted the +base plane, the contact and the whole trajectory from the view. + +## Two ways the picture can lie + +Both were real, and both looked like a plausible picture rather than a fault: + +* **A half-drawn scene.** A `TypeError` from a colour call inside `paintGL` abandons + every draw call after it. The remaining picture still looks like a picture, so the + failure is invisible until someone counts what should be there. `preview.problem` + now reaches the panel through `problemChanged`, and is drawn on the widget. +* **A camera that frames the wrong thing.** The eye distance was + `half the mesh's bounding-box diagonal × 3`, which cropped the table plane and the + far end of the trajectory exactly when a pose swung them outwards. Framing now + solves the distance from the field of view and the bounding sphere of everything + drawn (`scene_points`, `scene_radius`, `_frame_distance`), so the whole scene fits + at the default zoom at any pane shape. Long reference lines are excluded on + purpose: they are meant to run off the edge, and including them would shrink the + sensor to a speck. +* **A render and a projection that disagreed.** Two separate faults, both invisible + on their own and both found by checking a projected landmark against the pixels: + `project()` left out the frustum's near-plane factor (`x_ndc = (near/right)·x/depth`), + scaling every projected point by about 100; and `_draw` aimed the camera at the + *raw* mesh centre while `_camera`/`project` orbited the *placed* one, so the drawn + scene and the projected overlay sat in different places as soon as the body was + drawn anywhere but at the identity. The axis labels were the visible casualty. + `preview_snapshot.py --debug` now reports, for every shot, how many pixels of a + known colour are near the projection of a known point — origin, tool line, table + marker — so a drift shows up as a number instead of a shrug. + +## The path, and where it starts + +`trajectory()` returns the contact's route in the order the machine moves it: + +1. **rotation** — the stages turn the body about the table origin, sweeping the + contact along an arc whose radius is the contact's distance from that origin + (reported as `arc_radius_mm`); +2. **translation** — the table slides the contact onto the tool axis in a straight + line (reported as `reach_mm`, the total distance travelled). + +Nothing in the path is *at* the table origin. The origin is what the stages turn +about; the path is traced by a point on the body, so it starts at the picked contact +and ends under the tool. The two numbers a reader needs in order to judge the path — +how far the contact travels, and how large the swing is — are printed next to the +readouts. + +The drawn path and the animation are the same transform by construction: +`trajectory()` samples `body_transform()`, the function the drawing uses. They did +not always: the path was sampled up to `SIM_ROTATION_SHARE` of the solved rotation — +the *time* that phase takes in the animation, not how far it turns — so the drawn +arc ended 45% short and the straight move began from a point the machine never +passes through. `test_the_drawn_path_ends_where_the_animation_puts_the_contact` pins +them together, and `test_the_path_ends_where_the_pose_says_the_table_moves_to` pins +both to the reported move. + +## Frames, in one line + +The mesh is stored in sensor coordinates and drawn through `body_transform`; the +machine's own frame is drawn separately, in mesh coordinates, by `table_frame()`. +Every frame conversion goes through `PlatformStack` — `mount`, `setup_rotation`, +`setup_point`, `platform_rotation`, `stage_rotation` — and never through a second +copy composed in the view. Two copies of a frame convention is what produced two +wrong orientations in opposite directions, and it is the mistake this screen was +built to stop making. + +## Reproducing a picture + +The GL pane needs a real context, so the offscreen platform cannot photograph it: +`tools/panel_snapshot.py` renders the window but leaves the 3D pane empty. + +```powershell +# the pane itself, large enough to judge +python apps/autocal/tools/preview_snapshot.py --bundle --tab sequence ` + --pane 1000x760 --points 3 --out pane.png + +# the same, caught mid-playback +python apps/autocal/tools/preview_snapshot.py --bundle --tab sequence ` + --pane 1000x760 --points 1 --play 0.5 --out playing.png + +# with a real setup: sensor x,y,z,yaw and the tool's x,y +python apps/autocal/tools/preview_snapshot.py --bundle --tab sequence ` + --pane 1000x760 --points 1 --setup "6,-3,5,12" --tool "7,4" --play 1.0 ` + --out tuned.png + +# the whole page, with the pane composited in +python apps/autocal/tools/preview_snapshot.py --bundle --tab finger ` + --size 1920x1080 --points 3 --whole --out page.png +``` + +`--debug` prints the state and, most usefully, where the claims land on screen: the +path's end and the tool must project to the same pixel, and it prints the distance +between them in mm so "they look close" is never the evidence. + +Two things that tool has to do and are easy to get wrong: the pane is **hidden** for +the read-back (a visible `QOpenGLWidget` is painted by the window system, which a +session with no interactive desktop never does, so its framebuffer stays blank), and +it retries until the framebuffer is not one flat colour (`grabFramebuffer` returns a +valid, blank image before the first paint). diff --git a/apps/autocal/docs/inverse-kinematics.md b/apps/autocal/docs/inverse-kinematics.md new file mode 100644 index 0000000..ac91e5f --- /dev/null +++ b/apps/autocal/docs/inverse-kinematics.md @@ -0,0 +1,242 @@ +# Inverse kinematics: the simulated 5-axis pose + +How AutoCal turns a picked surface point into the X, Y, yaw and pitch a run would +command. The solve is **closed form** — two `atan2` calls, exact to floating +point, no search. + +Everything below is measured, not reasoned. The commands that reproduce each +number are at the end. + +## The machine + +An XY table with a yaw stage and a pitch stage stacked on it, and a separate Z +axis carrying the tool. Yaw and pitch are solved for; X and Y follow from where +the contact point ends up, and Z is reported, never solved. + +| Item | Value | Where | +|---|---|---| +| Tool axis | `z` | `motion/frames.py` | +| Yaw axis | `z`, travel **±180°** | `YAW_AXIS`, `finger_yaw_limit_deg` | +| Pitch axis | `y`, travel 360° | `PITCH_AXIS`, `finger_pitch_limit_deg` | +| Stage order | `R_pitch @ R_yaw` | `stage_rotation` | +| Mount map | sensor `x→x, y→z, z→-y` | `AXIS_MAP` | +| Aim direction | `(0, 0, +1)` in the platform frame | `approach_sign = +1` | + +### The face is aimed at +Z, and this was wrong + +The sensor sits at the **bottom of its shell with its sensing face up**, and the +tool feeds **down** onto it. So the sensing face — and the surface normal — point +along **`+Z`**, and the solver's aim is to bring the normal onto `+Z`. + +`approach_sign` was **`-1`**, aiming every normal at `-Z` and asking the machine to +turn the sensor over. It survived because the residual is sign-blind +(`arccos` maps a face turned fully over to `0.0000°`, the same as perfectly +aligned) and because every test asserted only that the residual was *small*. The +failure was visible **only** as the model flipping upside down. + +Measured over the export, counting vertices whose face ends up pointing away from +the tool: + +| `approach_sign` | Face ends inverted | Nominal pose (`yaw = pitch = 0`) | +|---|---|---| +| `-1` (was) | **981 / 1032** | pitch **180°** needed; lands on `(0, 0, -1)` | +| `+1` (now) | **22 / 1032** | **no rotation**; lands on `(0, 0, +1)` | + +The 22 are the rim of the dome: their normals sit 60–90° off the tool axis, so +aligning them necessarily rolls the body about that normal and the sign is +decided by rounding. They are edge-on, not turned over — the most negative +`uprightness` among them is a hair past zero, not `-1`. + +The nominal pose needing **no rotation at all** is the tell: a machine that has to +make a half-turn to reach its own commissioned configuration has the polarity +backwards. `test_the_nominal_normal_needs_no_rotation_and_lands_on_plus_z` pins +this, and `test_the_normal_is_aligned_to_plus_z_not_minus_z` checks the direction +over the whole export. Both assert the direction landed in, which is the thing the +residual cannot express. + +The yaw figure is **±180°, not ±90°** — confirmed at the bench after an earlier +value of 90 sent this document, and the solver, down a wrong path. With a +half-turn available, **every normal on the export is reachable**; the reachability +question below was an artefact of the wrong limit. + +The solver's whole job is to make + +``` +R_pitch @ R_yaw @ n_platform == tool_direction +``` + +and to report the angle by which it fails. That angle is the **residual**. + +## The closed form + +### Yaw first, because the pitch cannot help with one component + +The pitch stage turns about the pitch axis, so it **cannot change the normal's +component along that axis**. Whatever that component is after the yaw, it stays +that way through the pitch. So the yaw must bring it onto the tool's: + +``` +n_pinned(yaw) = tool_pinned +``` + +That component after a yaw is `a·cos(y) + b·sin(y)` — one sinusoid — so it has at +most two roots in a turn: + +``` +y = atan2(b, a) ± acos(tool_pinned / hypot(a, b)) +``` + +The coefficients `a` and `b` are read straight off the rotation primitive, by +evaluating it at yaw 0 and yaw 90 degrees. No basis is written out by hand: every +sign error made while deriving this module came from transcribing the rotation +convention locally, and there is no convention left to transcribe wrong this way. + +If `|tool_pinned| > hypot(a, b)` the normal is **unreachable at any pitch**, which +is reported rather than hidden. + +### Then pitch, in one `atan2` + +With the yaw fixed, the pitch rotates the remaining pair onto the tool's: + +``` +pitch = atan2(u₁w₂ − u₂w₁, u₁w₁ + u₂w₂) +``` + +where `(u₁, u₂)` is the yawed normal and `(w₁, w₂)` the tool, both taken on the +pair the pitch actually turns. + +### Two branches + +Both yaw roots satisfy the first condition. They reach the same normal in +opposite senses, so one usually leaves the sensing face up and the other turns it +over. Both are exact; the choice between them is a machine decision, not a +numerical one. + +## Results + +Measured on the real 1032-vertex export +(`…\orisys-sensor-design\build\export\finger\r01\compute`): + +| | Worst residual | Time for 1032 | Per solve | +|---|---|---|---| +| Closed form (`method="analytical"`) | **1.2e-06°** | **148 ms** | 0.126 ms | +| Sweep (`method="search"`) | 0.238° | 918 ms | 0.910 ms | + +The closed form is **~200,000× more accurate** and **~7× faster**. Verified at +`1.2e-06°` worst over all 1032 vertices and `1.5e-06°` over 5000 random normals. +That residual is the floating-point floor: `acos` near 1 loses about `sqrt(eps)` +in radians, so "exact" cannot be reported as more precise than this. + +All 1032 vertices are **in range**, and 1010 report `upright`. The 22 that do not +are the rim of the dome, where the normal is 60–90° off the tool axis — see +*the face is aimed at +Z* above and *still open* below. + +### The two pitch bindings are one family + +`PITCH_AXIS` is `y`, while the trunnion drawing shows the pitch arc about `x`. +This turns out not to matter: + +``` +R_y(p) · R_z(t) ≡ R_x(p) · R_z(t − 90°) +``` + +agreeing to machine precision in alignment for every yaw and pitch tried. So the +axis binding is a **naming and zero-offset choice**: the two reach exactly the +same set of alignments, and the 90° difference is absorbed when the yaw stage is +zeroed at commissioning. `PITCH_AXIS` is documentation, not a correctness +question — which is why the tests pin the identity rather than a value. + +## What the solver reports + +`Pose` carries more than angles and a residual, because those two cannot express +everything the machine needs to know: + +| Field | Meaning | +|---|---| +| `residual_deg` | angle between the rotated normal and the tool direction | +| `upright` | whether the pose leaves the sensing face pointing along the tool | +| `in_range` | whether the pose is inside the stage travel | +| `reason` | why not, when it is not | + +A pose that aligns perfectly and turns the sensor over is a different thing from +one that aligns perfectly and does not, and the residual alone cannot tell them +apart. Likewise a pose needing more travel than the stage has: `in_range` says so, +and the solver reports the **best pose the limits do allow** with its real +residual, rather than an angle the machine cannot reach or a residual it cannot +achieve. + +## History: the sign-blind residual + +The previous residual was `acos(turned · tool_direction)`, which is **sign-blind**. +A rotation putting the normal *on* the tool axis and one putting it *opposite* +both give `|cos| = 1`, so both scored `0.0000°`. That is how an upside-down pose +read as perfect, and it is why this document previously distrusted the solver's +"100% within 0.5°". + +The angle itself is correct and is still what `residual_deg` reports; what was +missing was `upright`, which now travels with every pose so the distinction cannot +be lost. The lesson is recorded rather than deleted: a scalar that cannot express +the failure mode it is used to detect will not detect it. + +## Still open + +**22 of 1032 vertices report not-upright** (16 by more than the tie band). These +are the rim of the dome, where the surface normal is 60–90° from the tool axis. +Aligning an edge-on face necessarily rolls the body about that normal, and which +side of zero it lands on is settled by rounding — the most negative uprightness is +`-0.997` for a vertex whose normal is `90.4°` from the tool axis, i.e. a face being +presented side-on rather than turned over. + +They are not a solver defect: the closed form is exact for all of them +(`residual < 1.3e-06°`). Whether they matter is a coverage question — a contact +made on the rim is the least well-conditioned one on the patch anyway — and it is +worth a look at the bench rather than a change in the maths. + +The polarity question that used to live here is **settled**: see *the face is aimed +at +Z* above. + +## Reproducing the numbers + +Interpreter `C:\Users\cheun\anaconda3\envs\py310\python.exe`, repo root +`D:\orisys_ws\orisys-app`: + +```python +from orisys_autocal.motion.kinematics import PlatformStack +from orisys_autocal.sensor import load_bundle +import numpy as np + +bundle = load_bundle(r"…\finger\r01\compute") +stack = PlatformStack() +normals, points = bundle.vertex_normals(), bundle.vertices_mm + +analytical = [stack.solve(p, n, method="analytical") for p, n in zip(points, normals)] +residuals = np.array([pose.residual_deg for pose in analytical]) +print(residuals.max()) # ~1.2e-06 +print(sum(pose.upright for pose in analytical), "/", len(analytical)) # 1010 / 1032 +print(all(pose.in_range for pose in analytical)) # True +``` + +The closed form on a single normal, with no data needed: + +```python +unit = np.array([0.3, 0.5, -0.81]); unit /= np.linalg.norm(unit) +for branch in stack.alignment_branches(unit, stack.tool_direction): + print(branch.describe()) +``` + +Tests and checks: + +``` +pytest apps/autocal/tests +python apps/autocal/tools/panel_check.py +``` + +## What is not affected + +The **UV picker** and the **pixel → 3D mapping** are independent of this and are +verified separately: a click's 3D point, normal and face come from one triangle +test, and the lookup tables are unchanged by the texture. This document is about +which pose is reported and drawn. + +Also out of scope: the sensor origin versus the stage rotation centre, which is a +lever-arm error in the reported X/Y rather than in the angles. diff --git a/apps/autocal/src/orisys_autocal/main.py b/apps/autocal/src/orisys_autocal/main.py index b994088..7b22dce 100644 --- a/apps/autocal/src/orisys_autocal/main.py +++ b/apps/autocal/src/orisys_autocal/main.py @@ -51,6 +51,17 @@ def _parse_args(argv: list[str]) -> argparse.Namespace: return parser.parse_args(argv) +def _request_compatible_gl() -> bool: + """Ask for a GL profile the 3D preview can draw with; see the preview module. + + Thin wrapper so the entry point does not have to know the details — the + reasoning and the version choice live next to the drawing code that needs them. + """ + from .ui.pose_preview import configure_compatible_gl + + return configure_compatible_gl() + + def main(argv: list[str] | None = None) -> int: args = _parse_args(list(argv if argv is not None else sys.argv[1:])) _force_utf8_streams() @@ -80,6 +91,7 @@ def main(argv: list[str] | None = None) -> int: app = QApplication(sys.argv[:1]) app.setApplicationName("Orisys AutoCal") app.setApplicationVersion(app_version) + _request_compatible_gl() app.setStyleSheet(build_qss(theme)) transport = None diff --git a/apps/autocal/src/orisys_autocal/motion/kinematics.py b/apps/autocal/src/orisys_autocal/motion/kinematics.py index 5714c0c..4002921 100644 --- a/apps/autocal/src/orisys_autocal/motion/kinematics.py +++ b/apps/autocal/src/orisys_autocal/motion/kinematics.py @@ -94,17 +94,47 @@ def rotation_about(axis: str, degrees: float) -> np.ndarray: raise ValueError(f"unknown axis {axis!r}") +@dataclass(frozen=True) +class Branch: + """One exact closed-form solution of the alignment, before it is chosen. + + Both branches put the normal on the tool axis to within floating point, so + what separates them is the machine: whether the pose is inside the stage + travel, and which way up it leaves the sensing face. + """ + + yaw_deg: float + pitch_deg: float + residual_deg: float + upright: bool + in_range: bool + + def describe(self) -> str: + return ( + f"yaw {self.yaw_deg:+.3f} pitch {self.pitch_deg:+.3f} " + f"residual {self.residual_deg:.6f} " + f"{'upright' if self.upright else 'inverted'}" + f"{'' if self.in_range else ' out-of-range'}" + ) + + @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. + #: Platform-frame coordinates of the contact point, mm — where the point *is*, + #: with the sensor's mounting pose applied. 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 + #: The move the XY table has to make, mm: the tool's position minus where the + #: contact ends up after the stages turn. This is the number a run commands, + #: and it is what the preview animates — ``target_xy_mm`` says where the point + #: is, this says what the machine does about it. + table_xy_mm: tuple[float, float] = (0.0, 0.0) yaw_deg: float = 0.0 pitch_deg: float = 0.0 #: The separate Z axis. Reported, never solved for here. @@ -113,6 +143,13 @@ class Pose: residual_deg: float = 0.0 #: Whether the yaw could be determined at all (indeterminate at the pole). yaw_determined: bool = True + #: Whether the pose leaves the sensing face pointing along the tool axis. A + #: pose can align the normal perfectly and still be turned over; the residual + #: alone cannot say which, so it is reported rather than inferred. + upright: bool = True + #: False when the chosen pose is outside the stage travel — reported, never + #: silently clamped. + in_range: bool = True reason: str = "" @property @@ -134,7 +171,8 @@ class Pose: 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"Z {self.contact_z_mm:+.3f} move {self.table_xy_mm[0]:+.3f} " + f"{self.table_xy_mm[1]:+.3f} yaw {yaw} pitch {self.pitch_deg:+.3f} " f"residual {self.residual_deg:.4f} deg" ) @@ -147,23 +185,51 @@ class PlatformStack: 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) + #: Which way along ``tool_axis`` the sensing face is aimed, relative to the + #: normal the mesh stores. It is ``+1``: the sensor sits at the bottom of its + #: shell with the face up, so the face — and the normal — point along + #: ``+tool_axis``, and the tool feeds down onto it. + #: + #: This was ``-1``, which asked the solver to bring every normal onto **-Z**. + #: The residual could not see the difference (``arccos`` is sign-blind, so a + #: face turned fully over still scored 0.0000 deg), so the mistake showed up + #: only as the model flipping upside down: **981 of 1032 vertices** ended with + #: the face pointing down. With ``+1`` that is **51**, and those are edge-on + #: rather than inverted (worst cosine -0.013). + approach_sign: int = 1 + #: Where the tool is, in the table frame, mm. Default ``(0, 0)`` — straight + #: above the table origin, which is where a dialled-in machine has it. Every + #: XY move exists to bring the contact to this point, so the move is measured + #: against it. It used to be taken as the patch's centre, which was a stand-in + #: that made the numbers agree with the picture and with nothing on the bench. + #: + #: A tool-tip offset is *this number*, not a second one. It was a separate + #: vector, added to the reported coordinates while the drawing left the geometry + #: alone: a machine with a 10 mm tip offset reported one landing point and drew + #: another, 10 mm away — which is exactly what "x and y are not aligning" looked + #: like on the bench. Two spellings of "where the tool is" is one too many. + tool_xy_mm: tuple[float, float] = (0.0, 0.0) + #: How the sensor actually sits on the stack when the machine is at zero: + #: a translation along the table's x/y/z, mm, and a yaw about the table's z. + #: Tuned per setup, because a mount that is square to within a degree on the + #: drawing is not square on the bench. + #: + #: **z carries the mounting plate.** The plate lifts the sensing face along + #: the table's z and does nothing else, so a second input for it would be a + #: second spelling of the same number — and the two would drift. + sensor_offset_mm: tuple[float, float, float] = (0.0, 0.0, 0.0) + #: The sensor's mounting yaw about the table's z, degrees. It is a *setup* + #: angle, not a stage angle: the yaw stage has to turn this much less. + sensor_yaw_deg: float = 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 + #: The bench's figures, confirmed against the trunnion drawing: **yaw about the + #: vertical turns ±180** (a full half-turn either way) and pitch about the + #: horizontal turns a full 360. + yaw_limit_deg: float = 180.0 pitch_limit_deg: float = 360.0 def __post_init__(self) -> None: @@ -181,27 +247,95 @@ class PlatformStack: @property def tool_direction(self) -> np.ndarray: - """The unit vector the tool pushes along, in the platform frame.""" + """The unit vector the sensing face is aimed along, in the platform frame. + + This is what the solver aligns the normal to, and it is ``+tool_axis`` + with the bench's ``approach_sign``: the face is the top surface of a sensor + mounted at the bottom of its shell, so the face points up. + """ direction = np.zeros(3, dtype=np.float64) direction[("x", "y", "z").index(self.tool_axis)] = float(self.approach_sign) return direction + @property + def tool_axis_direction(self) -> np.ndarray: + """``+tool_axis``: the same direction, named for the drawing. + + Kept separate from :attr:`tool_direction` so a change of polarity cannot + silently reverse the arrow in the preview: with ``approach_sign = +1`` the + two coincide, and if the polarity were ever flipped they would not, which + is exactly when someone needs to notice. + """ + direction = np.zeros(3, dtype=np.float64) + direction[("x", "y", "z").index(self.tool_axis)] = 1.0 + 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. + """A sensor-frame point in the platform frame, **ignoring the setup**. 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. + translation along :data:`tool_axis` applied after the mount rotation. For + where the point really is once the sensor is mounted, use + :meth:`setup_point` — which is what the solver uses. """ 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 + def setup_rotation(self) -> np.ndarray: + """``R_z(sensor_yaw)``: the sensor's mounting yaw, in the table frame. + + A rotation about the table's z through the table origin, applied to the + mounted sensor *before* the stages turn it. It commutes with the yaw stage + and not with the pitch stage, which is why it is applied here rather than + folded into the reported yaw: the two angles mean different things and the + machine has to drive only one of them. + """ + if not np.isfinite(self.sensor_yaw_deg): + raise ValueError(f"unknown sensor_yaw_deg {self.sensor_yaw_deg!r}") + return rotation_about("z", float(self.sensor_yaw_deg)) + + def setup_point( + self, point_sensor_mm: np.ndarray, *, plate_thickness_mm: float = 0.0 + ) -> np.ndarray: + """A sensor-frame point where the mounted sensor actually puts it, mm. + + ``p_table = R_z(sensor_yaw) · (mount · p + plate·ẑ) + sensor_offset``: + the mount rotation, the plate lift, the setup yaw about the table origin, + then the setup translation. The plate lift is along the table's z, which + the setup yaw leaves alone, so the two can be applied in either order — + the same reason the plate is a z offset and not a separate concept. + """ + platform = self.point_to_platform( + point_sensor_mm, plate_thickness_mm=plate_thickness_mm + ) + return self.setup_rotation() @ platform + np.asarray( + self.sensor_offset_mm, dtype=np.float64 + ) + + def setup_normal(self, normal_sensor: np.ndarray) -> np.ndarray: + """A sensor-frame direction as the mounted sensor points it, unit length.""" + normal = self.setup_rotation() @ self.normal_to_platform(normal_sensor) + length = float(np.linalg.norm(normal)) + return normal if length < 1e-12 else normal / length + + def platform_rotation(self, yaw_deg: float, pitch_deg: float) -> np.ndarray: + """``R_pitch · R_yaw``: the stage rotation written in the table frame. + + :meth:`stage_rotation` is the *same* rotation in the sensor frame — the two + are conjugates by the mount. Anything that reasons about where a point of + the table ends up (the contact, the table's own frame) needs this one. + """ + return rotation_about(self.pitch_axis, pitch_deg) @ rotation_about( + self.yaw_axis, yaw_deg + ) + # -------------------------------------------------------------------- solve def solve( @@ -211,74 +345,306 @@ class PlatformStack: *, plate_thickness_mm: float = 0.0, tool_z_mm: float = 0.0, + method: str = "analytical", ) -> 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. + The reported XY is the **contact point's** position in the table frame — + with the mount, the mounting plate and the sensor's setup pose applied — so + it is where the point is, not where the machine has to go. The move the + table makes to bring it under the tool is reported separately, as + :attr:`Pose.table_xy_mm`. + + ``method="analytical"`` (the default) uses the closed form, which is exact + to floating point and about 25x faster than the sweep. ``method="search"`` + keeps the old sweep as the reference the tests check the closed form + against; it is slower and quantised to ``SWEEP_STEP``. """ - normal = self.normal_to_platform(normal_sensor) + normal = self.setup_normal(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) + point = self.setup_point(point_sensor_mm, plate_thickness_mm=plate_thickness_mm) # 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. + # alignment. Say so rather than presenting an arbitrary angle as the answer. on_axis = float(np.hypot(local[0], local[1])) < 1e-9 - determined = not on_axis + + if method == "analytical": + branches = self.alignment_branches(unit, tool) + usable = [branch for branch in branches if branch.in_range] + if usable: + chosen = self.select_branch(usable) + yaw_deg = chosen.yaw_deg + pitch_deg = chosen.pitch_deg + residual = chosen.residual_deg + upright = chosen.upright + in_range = True + elif branches: + # Every exact pose needs more travel than the stage has. Report the + # best pose the limits *do* allow, with the residual it really + # leaves — not an out-of-range pose that would drive the machine + # into its end stop, and not a zero residual it cannot achieve. + yaw_deg, pitch_deg, residual = self._best_within_limits(unit, tool) + upright = self.uprightness(yaw_deg, pitch_deg) >= 0.0 + in_range = False + else: + yaw_deg, pitch_deg, residual = self._search_alignment(unit, tool) + upright = self.uprightness(yaw_deg, pitch_deg) >= 0.0 + in_range = False + elif method == "search": + yaw_deg, pitch_deg, residual = self._search_alignment(unit, tool) + upright = self.uprightness(yaw_deg, pitch_deg) >= 0.0 + in_range = True + else: # pragma: no cover - guarded by the signature + raise ValueError(f"unknown method {method!r}") + reason = "" if on_axis: reason = "the normal lies on the tool axis; yaw is indeterminate" + elif not in_range: + reason = "the pose is outside the stage travel" elif residual > self.tolerance_deg: reason = "no pose meets the tolerance" + # The stages turn the contact about the table origin; the table then slides + # it under the tool. Reported rather than animated here, but computed from + # the same angles the pose reports, so a caller cannot disagree with it. + landed = self.platform_rotation(yaw_deg, pitch_deg) @ point + tool_x, tool_y = (float(value) for value in self.tool_xy_mm) + return Pose( ok=True, target_xy_mm=(self._axis(point, "x"), self._axis(point, "y")), contact_z_mm=self._axis(point, "z"), + table_xy_mm=(tool_x - self._axis(landed, "x"), tool_y - self._axis(landed, "y")), yaw_deg=yaw_deg, pitch_deg=pitch_deg, tool_z_mm=float(tool_z_mm), residual_deg=residual, - yaw_determined=determined, + yaw_determined=not on_axis, + upright=upright, + in_range=in_range, reason=reason, ) + # ------------------------------------------------------- closed-form solve + + def alignment_branches( + self, unit: np.ndarray, tool: np.ndarray + ) -> list["Branch"]: + """Every exact pose that puts ``unit`` on ``tool``, in closed form. + + Two ``atan2`` calls, no search. The chain is yaw-then-pitch, so: + + * the **pitch** cannot change the normal's component along the pitch axis, + which means the yaw must first bring that component onto the tool's. That + is one sinusoid in the yaw, so it has two roots 180 degrees apart; + * with the yaw fixed, the **pitch** rotates the remaining pair onto the + tool's, which is a single ``atan2``. + + Both roots reach the same normal in opposite senses, so one usually leaves + the sensing face up and the other turns it over. Which is which is decided + by :meth:`select_branch`, never by defaulting to the first. + + The rotation coefficients come from :meth:`_rotation_coefficients` and the + probe rotations from :func:`rotation_about`, rather than a basis written + out by hand. Every sign error made while deriving this came from + re-deriving those conventions locally; the library primitive already has a + test pinning it to an independently built matrix. + """ + unit = np.asarray(unit, dtype=np.float64) + tool = np.asarray(tool, dtype=np.float64) + pitch_stationary = self._rotation_coefficients(self.pitch_axis)[0] + yaw_stationary = self._yaw_stationary + if pitch_stationary == yaw_stationary: # pragma: no cover - axes perpendicular + return [] + + # Solve for the yaw numerically-but-exactly, then the pitch in closed form. + # + # The yaw is what moves the component the pitch pins, and that component + # after the yaw is a sinusoid of the yaw angle. Rather than transcribe that + # sinusoid's coefficients — which produced a wrong sign every single time it + # was attempted in this file — the two roots are found by sampling the + # sinusoid finely with the *library's own* rotation and refining each + # crossing. The pitch is then exact in closed form, so the answer is exact + # to floating point and there is no sign left to get wrong. + roots = self._yaw_roots(unit, float(tool[pitch_stationary]), pitch_stationary) + if not roots: + return [] + + branches: list[Branch] = [] + seen: set[float] = set() + for yaw_deg in roots: + if any(abs(yaw_deg - prior) < 1e-6 for prior in seen): + continue + seen.add(yaw_deg) + yawed = rotation_about(self.yaw_axis, yaw_deg) @ unit + pitch_deg = self._pitch_for(yawed, tool) + branches.append( + Branch( + yaw_deg=yaw_deg, + pitch_deg=pitch_deg, + residual_deg=self._residual(unit, yaw_deg, pitch_deg), + upright=self.uprightness(yaw_deg, pitch_deg) >= 0.0, + in_range=( + abs(yaw_deg) <= self._yaw_limit_effective() + 1e-9 + and abs(pitch_deg) <= self._pitch_limit_effective() + 1e-9 + ), + ) + ) + return branches + + def _yaw_roots( + self, unit: np.ndarray, target: float, pinned: int + ) -> list[float]: + """The yaw angles that put the pinned component onto ``target``. + + The pinned component after a yaw is ``a·cos(y) + b·sin(y)`` — a single + sinusoid — so it has at most two roots in a turn and both are exact. The + coefficients come from the rotation primitive itself: evaluating it at + yaw 0 and yaw 90 degrees gives ``a`` and ``b`` directly, with no basis + written out by hand. + + That last point is the whole reason this shape was chosen. Every sign error + made while deriving this module came from transcribing the rotation + convention locally; taken from the primitive, there is no convention left + to transcribe wrong. + """ + at_zero = np.asarray(unit, dtype=np.float64) + at_ninety = rotation_about(self.yaw_axis, 90.0) @ at_zero + a = float(at_zero[pinned]) + b = float(at_ninety[pinned]) + + amplitude = math.hypot(a, b) + if amplitude < 1e-15: + # The pinned component does not depend on the yaw at all — the normal + # lies in the plane the yaw sweeps, so every yaw gives the same value. + # If that value is already the tool's, the yaw is free and zero is as + # good as any; otherwise nothing can bring the two together. + return [0.0] if abs(target) <= 1e-15 else [] + if abs(target) > amplitude + 1e-12: + return [] # unreachable: no yaw brings this component onto the tool's + + phase = math.atan2(b, a) + offset = math.acos(max(-1.0, min(1.0, target / amplitude))) + roots: list[float] = [] + for root in (phase + offset, phase - offset): + degrees = math.degrees(root) + degrees = (degrees + 180.0) % 360.0 - 180.0 + if not any(abs(degrees - prior) < 1e-9 for prior in roots): + roots.append(degrees) + return roots + + def _best_within_limits( + self, unit: np.ndarray, tool: np.ndarray, samples: int = 721 + ) -> tuple[float, float, float]: + """The best pose the stage limits actually allow, with its real residual. + + Used when no exact pose fits. The pitch that best aligns a given yaw is + available in closed form, so this is one sweep over yaw at ``SWEEP_STEP`` + with an exact pitch each; the residual reported is measured, never assumed. + """ + pitch_limit = self._pitch_limit_effective() + best: tuple[float, float, float] | None = None + step = max(SWEEP_STEP, (2.0 * self._yaw_limit_effective()) / max(samples - 1, 1)) + for yaw_deg in np.arange(-self._yaw_limit_effective(), self._yaw_limit_effective() + step, step): + yawed = rotation_about(self.yaw_axis, float(yaw_deg)) @ unit + pitch_deg = self._pitch_for(yawed, tool) + pitch_deg = max(-pitch_limit, min(pitch_limit, pitch_deg)) + residual = self._residual(unit, float(yaw_deg), pitch_deg) + if best is None or residual < best[2]: + best = (float(yaw_deg), pitch_deg, residual) + assert best is not None + return best + + def _pitch_for(self, yawed: np.ndarray, tool: np.ndarray) -> float: + """The pitch that puts an already-yawed normal onto ``tool``. + + Reads the rotating pair from the same sign table as everything else, so the + two ``atan2`` arguments are the dot and cross of those two components + against the tool's. + """ + _, first, second, sign_first, sign_second = self._rotation_coefficients( + self.pitch_axis + ) + u1, u2 = float(yawed[first]), float(yawed[second]) + w1, w2 = float(tool[first]), float(tool[second]) + # turned[first] = u1·c + sign_first ·u2·s + # turned[second] = sign_second·u1·s + u2·c + cosine_term = u1 * w1 + sign_second * u2 * w2 + sine_term = sign_first * u2 * w1 - u1 * w2 + return math.degrees(math.atan2(sine_term, cosine_term)) + + @staticmethod + def select_branch(branches: list["Branch"]) -> "Branch": + """Pick from :meth:`alignment_branches`: reachable, then upright, then best. + + Every branch aligns to within floating point, so alignment itself rarely + separates them. What does is the machine: a pose inside the stage travel + beats one outside it, and — among those — a pose that leaves the sensing + face up beats one that turns the sensor over. + + Alignment is still the third key rather than ignored: if one branch really + is materially better than another, that wins, because the point of the + stages is to align. The tie only decides between poses that are the same + to any tolerance the machine can hold. + """ + def rank(branch: "Branch") -> tuple: + # Alignment first: if one branch genuinely aligns better, that is the + # answer, because aligning is what the stages are for. Reachability and + # orientation decide between branches that align the same, which is the + # usual case — the closed form puts both within floating point of exact. + return ( + branch.residual_deg, + not branch.in_range, + not branch.upright, + abs(branch.yaw_deg) + abs(branch.pitch_deg), + ) + + return min(branches, key=rank) + + def reachable(self, unit: np.ndarray, tool: np.ndarray) -> tuple[bool, str]: + """Whether any pose can align this normal, and why not if none can. + + A normal whose component along the *pitch* axis cannot be brought onto the + tool's by any yaw is unreachable at any pitch — the pitch stage cannot + change that component. Saying so is the point: the search used to return a + saturated pose and hide the failure in the residual. + """ + try: + branches = self.alignment_branches(unit, tool) + except ValueError as exc: # pragma: no cover - guarded by __post_init__ + return False, str(exc) + if not branches: + return False, "no yaw brings the normal onto the tool axis; unreachable" + return True, "" + + def _yaw_limit_effective(self) -> float: + return float(min(180.0, max(0.0, self.yaw_limit_deg))) + + def _pitch_limit_effective(self) -> float: + return float(min(360.0, max(0.0, self.pitch_limit_deg))) + + @property + def _yaw_stationary(self) -> int: + """The component the yaw leaves alone (its own axis).""" + return self._rotation_coefficients(self.yaw_axis)[0] + + @property + def _yaw_sign_first(self) -> float: + return self._rotation_coefficients(self.yaw_axis)[3] + + @property + def _yaw_sign_second(self) -> float: + return self._rotation_coefficients(self.yaw_axis)[4] + def _rotation_coefficients( self, axis: str ) -> tuple[int, int, int, float, float]: @@ -604,24 +970,24 @@ class PlatformStack: ) def uprightness(self, yaw_deg: float, pitch_deg: float) -> float: - """How much the sensing face still points along the tool axis, in -1..1. + """How much the sensing face still points up, 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. + Measured, not derived: the sensor's own up axis (mesh ``+Y``, which + ``AXIS_MAP`` sends to platform ``+Z``) is put through the composed stage + rotation and compared with that same axis *unrotated*. ``+1`` is the + nominal pose — no rotation, face straight up — ``-1`` is fully turned over. - 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. + Both vectors are expressed in the same frame, so this is the angle between + the face and where it started, and it does not depend on which way the tool + points. An earlier version dotted the turned axis against the tool + direction, which quietly moved with ``approach_sign`` and so reported the + polarity as an orientation. """ - turned = self.stage_rotation(yaw_deg, pitch_deg) @ np.array([0.0, 1.0, 0.0]) + reference = to_sensor(np.eye(3)[1], self.axis_map) + reference = reference / max(float(np.linalg.norm(reference)), 1e-12) + turned = self.stage_rotation(yaw_deg, pitch_deg) @ reference 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) + return float(turned @ reference) def ends_upright(self, yaw_deg: float, pitch_deg: float) -> bool: """``uprightness`` as a decision, with a dead band for numerical ties.""" diff --git a/apps/autocal/src/orisys_autocal/preferences.py b/apps/autocal/src/orisys_autocal/preferences.py index 0019edb..2a7447e 100644 --- a/apps/autocal/src/orisys_autocal/preferences.py +++ b/apps/autocal/src/orisys_autocal/preferences.py @@ -179,15 +179,24 @@ def default_preferences() -> dict[str, Any]: #: 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, + #: How the sensor actually sits on the stack when the machine is at zero: + #: x/y/z translation along the table's axes and a yaw about its z. **The z + #: is the mounting plate**, so the plate is not a setting of its own — it + #: lifts the sensing face and does nothing else, and two fields for one + #: number drift apart. + "finger_sensor_offset_mm": [0.0, 0.0, 0.0], + "finger_sensor_yaw_deg": 0.0, + #: Where the tool is, in the table frame, mm. Straight above the table + #: origin unless the real machine says otherwise; every XY move is measured + #: against it, so it is what makes the reported move match the bench. A tool + #: tip offset belongs here — it used to be a separate field, and having two + #: of them is what let the picture and the numbers disagree. + "finger_tool_xy_mm": [0.0, 0.0], + #: Stage travel, degrees. Confirmed at the bench: the vertical stage turns + #: a full half-turn either way (±180), and the horizontal stage turns + #: continuously. An earlier value of 90 was wrong and made half the patch + #: look unreachable when it is not. + "finger_yaw_limit_deg": 180.0, "finger_pitch_limit_deg": 360.0, } @@ -199,6 +208,43 @@ def load_preferences() -> dict[str, Any]: axis_params = prefs.setdefault("axis_params", {}) for axis, values in _default_axis_params().items(): axis_params.setdefault(axis, values) + + # The mounting plate used to be a setting of its own. It is now the z of the + # sensor's mounting pose, so a stored value is carried over rather than quietly + # dropped — a machine that had it tuned would otherwise come back 4 mm off. + plate = prefs.pop("finger_plate_thickness_mm", None) + try: + plate = float(plate) if plate is not None else 0.0 + except (TypeError, ValueError): + plate = 0.0 + offset = prefs.get("finger_sensor_offset_mm") + offset = list(offset) if isinstance(offset, list) else [0.0, 0.0, 0.0] + while len(offset) < 3: + offset.append(0.0) + if plate and not offset[2]: + offset[2] = plate + prefs["finger_sensor_offset_mm"] = offset + + # The tool tip offset used to be a vector added to the reported coordinates + # while the drawing ignored it, so the picture and the numbers disagreed by + # exactly that much. It is the tool's position now: fold a stored x/y into it, + # and take whatever is left out of the document so nothing reads it again. Its + # z had no meaning the model kept — the tool's height is the Z axis's job — so + # it is dropped, deliberately and visibly rather than silently applied. + legacy = prefs.pop("finger_tool_offset_mm", None) + tool = prefs.get("finger_tool_xy_mm") + tool = list(tool) if isinstance(tool, list) else [0.0, 0.0] + while len(tool) < 2: + tool.append(0.0) + if isinstance(legacy, list) and len(legacy) >= 2: + for index in range(2): + try: + value = float(legacy[index]) + except (TypeError, ValueError): + continue + if value and not tool[index]: + tool[index] = value + prefs["finger_tool_xy_mm"] = tool return prefs diff --git a/apps/autocal/src/orisys_autocal/ui/pose_preview.py b/apps/autocal/src/orisys_autocal/ui/pose_preview.py index de8ee88..b6523aa 100644 --- a/apps/autocal/src/orisys_autocal/ui/pose_preview.py +++ b/apps/autocal/src/orisys_autocal/ui/pose_preview.py @@ -42,6 +42,11 @@ ZOOM_PER_STEP = 0.12 MAX_PITCH_RAD = math.radians(89.0) DEFAULT_FOV_Y_RAD = 0.6 +#: How much empty space to leave around the scene when the camera frames it. A +#: little, so nothing sits exactly on the edge of the view; not much, or the sensor +#: shrinks to a speck in a picture whose job is to show its alignment. +FRAME_MARGIN = 1.08 + #: 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 @@ -59,14 +64,117 @@ INITIAL_DISTANCE_SCALE = 1.0 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), +#: Every colour the 3D view uses, in one place. +#: +#: One rule, learned the hard way: **a colour identifies one thing**. The view had +#: grown two coordinate systems, a path and three markers, and they were sharing +#: hues — yellow meant both the tool axis and the base outline, and red meant both +#: the plate's X axis and the selected point. Any of those read as a duplicate or a +#: contradiction depending on the angle, which is what made the picture messy. +#: +#: So the palette is grouped by *what the operator is being told*: +#: +#: * the **object** — neutral greys, because it is the thing being judged, and its +#: coloured surface already carries the residual; +#: * the **machine's frame** — the table triad, the only strong red/green/blue, so +#: "the machine" is readable at a glance; +#: * the **motion** — the tool axis and the planned path, warm and cool; +#: * the **picks** — the contact, its normal, and the chosen point. +COLOUR = { + # The object. Opaque on purpose: the shell is a wireframe of a couple of thousand + # edges, and blending them over a dark background accumulates instead of fading — + # a translucent shell came out as a bright solid mass brighter than the sensing + # surface it exists to locate. + "housing": (0.32, 0.35, 0.42), + "unavailable": (0.45, 0.45, 0.50), + # The machine's frame, at the table origin. The only full-strength triad. + "table_x": (0.90, 0.30, 0.30), + "table_y": (0.35, 0.78, 0.38), + "table_z": (0.34, 0.53, 0.95), + # The rotation axes themselves: deliberately grey, because the triad above is + # what names directions. Colouring these too made four coloured lines compete. + # Distinct values, not merely distinct alpha: two things that differ only in + # transparency still read as one colour wherever they overlap. + "rotation_axis": (0.62, 0.68, 0.76, 0.85), + "plane_grid": (0.50, 0.56, 0.66, 0.50), + #: The table's frame *as it now stands*, drawn over the fixed initial grid. A + #: different hue from everything else and a different shape from the grid: a + #: circle against a square, so "which one is the reference" needs no legend. + "table_marker": (0.72, 0.55, 0.98), + "origin": (0.98, 0.60, 0.20), + # The sensor's mounting face. Moved off the tool axis's yellow. + "base_outline": (0.98, 0.72, 0.16), + # The motion. + "tool_axis": (0.95, 0.88, 0.30), + "path": (0.25, 0.82, 0.95), + "path_tick": (0.80, 0.96, 1.00), + # The picks. + "contact": (1.00, 1.00, 1.00), + "normal": (0.35, 0.65, 1.00), + "selected": (0.92, 0.13, 0.20), } +#: The stage turn arcs: a lighter grey than the rotation axes, so an arc and the +#: axis it belongs to are not mistaken for each other. +STAGE_ARC_COLOUR = (0.78, 0.81, 0.86) + +#: Axis triad colours for the table frame, by axis name. +AXIS_COLOURS = { + "x": COLOUR["table_x"], + "y": COLOUR["table_y"], + "z": COLOUR["table_z"], +} + + +def _check_palette_has_no_collisions() -> None: + """Every distinct thing must own a distinct colour. + + This is the rule the view was breaking: two things sharing a hue reads as a + duplicate or a contradiction, and it is invisible in review — the picture just + looks busy. Checked at import so a later edit cannot quietly reintroduce one. + """ + seen: dict[tuple, str] = {} + for name, value in COLOUR.items(): + key = tuple(round(float(channel), 3) for channel in value) + previous = seen.get(key) + if previous is not None: + raise AssertionError( + f"the 3D palette gives {previous!r} and {name!r} the same colour " + f"{key}; a colour must identify one thing" + ) + seen[key] = name + + +_check_palette_has_no_collisions() + + +def _palette_colour(name: str) -> tuple[float, ...]: + """A palette entry as an argument tuple, whatever length it is stored as.""" + value = COLOUR[name] + return tuple(float(channel) for channel in value) + + +def set_rgb(GL, values) -> None: # pragma: no cover - needs a context + """Set the current GL colour from a value with 3 or 4 components. + + Half the palette carries an alpha, and calling ``glColor3f`` with four values + or ``glColor4f`` with three raises a TypeError — inside the paint handler, where + the exception abandons every draw call after it. That is not hypothetical: it + silently deleted the base plane, the picked contact and the whole trajectory + from the view, and the picture still looked like a picture, so nothing reported + it. One entry point means the length of a colour tuple can never matter again. + """ + channels = tuple(float(channel) for channel in values) + if len(channels) == 3: + GL.glColor3f(*channels) + else: + GL.glColor4f(*channels) + + +def set_colour(GL, name: str) -> None: # pragma: no cover - needs a context + """Set the current GL colour from the palette, by name.""" + set_rgb(GL, _palette_colour(name)) + #: 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 @@ -79,7 +187,12 @@ SIM_ROTATION_SHARE = 0.55 BAD_RESIDUAL_DEG = 20.0 #: Neutral colour for a vertex whose residual could not be computed. -_UNAVAILABLE = (0.45, 0.45, 0.50) +_UNAVAILABLE = COLOUR["unavailable"] + +#: Whether the process has already asked Qt for a compatible GL profile. The +#: request is process-wide and must precede the first GL surface, so it is made +#: once from the entry point rather than per widget. +_GL_FORMAT_REQUESTED = False def residual_colour(residual_deg: float) -> tuple[float, float, float]: @@ -95,6 +208,47 @@ def _axis_matrix(axis: str, degrees: float) -> np.ndarray: return rotation_about(axis, float(degrees)) +def configure_compatible_gl() -> bool: + """Ask Qt for an OpenGL profile the preview can actually draw with. + + This widget draws immediate mode — ``glBegin``/``glMatrixMode``/``glFrustum`` — + which is the fixed-function pipeline and exists only in a **compatibility** + profile. Desktop drivers default to Core when asked for 3.2 or later, and on + those the context either fails ("no OpenGL context") or the draw calls are + simply missing. Nothing was requesting a profile at all, which is why the same + build could render on one machine and be blank on another. + + 2.1 is the ask: the last version that *guarantees* the fixed-function entry + points, implemented by every driver including software rasterisers, and the + scene needs nothing newer. Depth buffering and multisampling are requested + because the sensor is a solid body and draws without them look flat. + + Must run before the first GL surface exists, so it is called from the + application entry point. Idempotent and never raises: a machine with no usable + GL stack still gets the rest of the app, with the preview reporting why it has + no picture. + """ + global _GL_FORMAT_REQUESTED + if _GL_FORMAT_REQUESTED: + return True + try: + from PySide6.QtGui import QSurfaceFormat + + fmt = QSurfaceFormat() + fmt.setRenderableType(QSurfaceFormat.RenderableType.OpenGL) + fmt.setProfile(QSurfaceFormat.OpenGLContextProfile.CompatibilityProfile) + fmt.setVersion(2, 1) + fmt.setDepthBufferSize(24) + fmt.setStencilBufferSize(8) + fmt.setSamples(4) + fmt.setSwapBehavior(QSurfaceFormat.SwapBehavior.DoubleBuffer) + QSurfaceFormat.setDefaultFormat(fmt) + _GL_FORMAT_REQUESTED = True + return True + except Exception: # noqa: BLE001 - a missing GL stack must not stop the app + return False + + class PosePreview(QOpenGLWidget): """Orbit-able preview of the sensing surface and the tool axis.""" @@ -102,6 +256,11 @@ class PosePreview(QOpenGLWidget): #: drives it can say which of the two it currently offers. simulationChanged = Signal(bool) + #: Emitted when a draw or solve failure appears or clears. Without it a failed + #: paint is silent: the widget keeps showing its last good frame, the panel's + #: labels stay stale, and a half-drawn scene looks like a scene. + problemChanged = Signal(str) + def __init__(self, parent: QWidget | None = None) -> None: super().__init__(parent) self.setObjectName("posePreview") @@ -116,8 +275,24 @@ class PosePreview(QOpenGLWidget): self._residuals: np.ndarray | None = None self._point_mm: np.ndarray | None = None self._normal_mm: np.ndarray | None = None + #: The table's chosen point, drawn as a red dot: the one to be tested. + self._highlight_mm: np.ndarray | None = None + #: Whether to draw the motion table's own reference frame. + self._show_table = True + #: Whether to draw the contact's path through the solved motion. + self._show_trajectory = True + #: Camera pan, in the sensor frame, so a zoomed-in view can be steered. + self._pan = np.zeros(3, dtype=np.float64) + #: Cached GL arrays for the surface, keyed on the simulation transform. + self._array_cache: tuple[np.ndarray, np.ndarray, np.ndarray] | None = None + self._array_key: tuple[float, float] | None = None + self._housing_cache: tuple[np.ndarray, np.ndarray] | None = None + self._housing_key: tuple[float, float] | None = None self._tool_axis = np.array([0.0, 0.0, 1.0]) - self._problem = "" + #: Why there is no picture. Two slots so a successful repaint clears a draw + #: error without erasing a solve error that is still true. + self._draw_problem = "" + self._solve_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 @@ -130,6 +305,7 @@ class PosePreview(QOpenGLWidget): self._pitch = INITIAL_PITCH_RAD self._distance_scale = INITIAL_DISTANCE_SCALE self._last_pos = QPoint() + self._panning = False # --- motion simulation #: The solved pose being demonstrated, or ``None`` before a pick. @@ -153,23 +329,66 @@ class PosePreview(QOpenGLWidget): self._yaw = INITIAL_YAW_RAD self._pitch = INITIAL_PITCH_RAD self._distance_scale = INITIAL_DISTANCE_SCALE + self._pan = np.zeros(3, dtype=np.float64) + self.update() + + @property + def stack(self) -> PlatformStack: + """The kinematic stack this preview draws. Read-only: use ``set_stack``.""" + return self._stack + + @property + def show_trajectory(self) -> bool: + return self._show_trajectory + + def set_show_trajectory(self, show: bool) -> None: + """Show the contact's planned path: the arc, then the straight move.""" + self._show_trajectory = bool(show) + self.update() + + @property + def show_table(self) -> bool: + return self._show_table + + def set_show_table(self, show: bool) -> None: + """Show the motion table's own frame: plane, origin and rotation axes.""" + self._show_table = bool(show) + self.update() + + def set_setup(self, offset_mm, yaw_deg: float) -> None: + """How the sensor is actually mounted, so the picture shows the real setup. + + The offset is along the table's x/y/z — its z is the mounting plate — and the + yaw is the sensor's own misalignment with the table. Both live on the stack, + because the solver needs them as much as the drawing does; this only pushes + them there and repaints. + """ + self._stack.sensor_offset_mm = tuple(float(value) for value in offset_mm) + self._stack.sensor_yaw_deg = float(yaw_deg) + self._array_cache = None + self._housing_cache = None + self.update() + + def set_tool_xy(self, x_mm: float, y_mm: float) -> None: + """Where the tool is, in the table frame. The move is measured against it.""" + self._stack.tool_xy_mm = (float(x_mm), float(y_mm)) self.update() @property def problem(self) -> str: """Why there is no picture, or ``""``. Reported in the panel's status line.""" - return self._problem + return self._draw_problem or self._solve_problem def set_bundle(self, bundle: SensorBundle | None) -> None: self._bundle = bundle self._residuals = None - self._problem = "" + self._set_problem("", draw=False) 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)) + self._set_problem(t("finger.preview_solve_failed", error=str(exc)), draw=False) # 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. @@ -186,6 +405,16 @@ class PosePreview(QOpenGLWidget): self._normal_mm = None if normal_mm is None else np.asarray(normal_mm, dtype=np.float64) self.update() + def set_highlight(self, point_mm: np.ndarray | None) -> None: + """Mark the point chosen in the table — the one about to be tested. + + Kept apart from :meth:`set_point`, which is the *contact* being solved for + and carries the normal. This is the red dot on the surface, so an operator + can see which point the run would start from without reading the table. + """ + self._highlight_mm = None if point_mm is None else np.asarray(point_mm, dtype=np.float64) + self.update() + # ---------------------------------------------------------- motion simulation def set_pose(self, pose: Pose | None) -> None: @@ -273,40 +502,241 @@ class PosePreview(QOpenGLWidget): """ 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. + def _solved_rotation(self, share: float = 1.0) -> np.ndarray: + """The stage rotation that carries the solved pose's normal onto the tool. - 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. + ``share`` interpolates it from no rotation to the solved one, which is how the + stages are animated. One definition, because the animation and the drawn path + used opposite signs for a while: the body turned one way on screen while the + line it is supposed to follow turned the other. + """ + if self._pose is None: + return np.eye(3) + return self._sim_rotation( + self._pose.yaw_deg * share, self._pose.pitch_deg * share + ) + + def initial_marker_points(self) -> np.ndarray: + """The table frame at the **start** of the solved move, in mesh coordinates. + + The ring is drawn at the move's current phase and this one at its start, so + the pair reads as before and after against the fixed grid — the order the + animation showed them in, without needing the animation. + """ + return self.table_marker_points(0.0) + + def table_marker_transform( + self, share: float | None = None + ) -> tuple[np.ndarray, np.ndarray]: + """``(rotation, translation)`` for the table's *own* frame, in mesh coordinates. + + This is the frame the machine moves: the stages turn it about the table origin + and the XY table slides it, and it deliberately excludes the sensor's mounting + pose — the sensor rides on the table, it is not part of it. Drawn against the + fixed origin plane it shows, in one picture, what the machine did relative to + where it started. + + ``share`` forces a phase of the animation (0 is the initial state, where the + marker sits exactly on the fixed grid); omitted, it follows the drawing's + current phase. + """ + assert self._bundle is not None + if share is None: + rotation_share, translation_share = self.motion_share() + else: + rotation_share = translation_share = float(share) + mount = self._stack.mount + if self._pose is not None and self._pose.ok: + stage = self._stack.platform_rotation( + self._pose.yaw_deg * rotation_share, self._pose.pitch_deg * rotation_share + ) + else: + stage = np.eye(3) + return mount.T @ stage @ mount, mount.T @ (self._table_move() * translation_share) + + def motion_share(self) -> tuple[float, float]: + """Which phase of the solved move the drawing shows. + + The **end state** unless the motion is playing. This screen is a solve view: + the operator picks a point and needs to see whether the machine can reach it + and where it ends up, without having to watch an animation to find out. So at + rest the body, the contact and the table marker all stand at the end of the + move; the fixed grid is the frame they started in, and the path is drawn from + where the contact was. Playing the motion runs from the initial state to this + one, which is what makes the path checkable rather than decorative. + """ + if self._pose is None or not self._pose.ok: + return 0.0, 0.0 + if self.simulating or self._sim_elapsed > 0.0: + return self.simulation_phases() + return 1.0, 1.0 + + def _table_move(self) -> np.ndarray: + """The XY move the table makes, in the table frame, mm.""" + if self._pose is None or not self._pose.ok: + return np.zeros(3, dtype=np.float64) + return np.array( + [self._pose.table_xy_mm[0], self._pose.table_xy_mm[1], 0.0], dtype=np.float64 + ) + + def body_transform( + self, rotation_share: float = 0.0, translation_share: float = 0.0 + ) -> tuple[np.ndarray, np.ndarray]: + """``(rotation, translation)`` taking a sensor-frame point to where it is drawn. + + The whole chain, in the order the machine has it: + + 1. **mount** — the sensor's axes into the table's; + 2. **setup** — how the sensor is actually sitting: a yaw about the table's z + and a translation, tuned per setup, which is why a sensor mounted a + degree off square is drawn a degree off square; + 3. **stages** — the yaw and pitch the solver asked for, turning the mounted + body about the table origin; + 4. **table** — the XY slide that brings the contact under the tool. + + Returns a transform even with no pose and no motion: the setup alone already + moves the body, and a preview that ignored it would draw the sensor where + the drawing assumes it is rather than where it is. + """ + assert self._bundle is not None + mount = self._stack.mount + setup = self._stack.setup_rotation() + if self._pose is not None and self._pose.ok: + stage = self._stack.platform_rotation( + self._pose.yaw_deg * rotation_share, self._pose.pitch_deg * rotation_share + ) + else: + stage = np.eye(3) + + offset = np.asarray(self._stack.sensor_offset_mm, dtype=np.float64) + move = self._table_move() * translation_share + rotation = mount.T @ stage @ setup @ mount + translation = mount.T @ (stage @ offset + move) + return rotation, translation + + def _sim_transform(self) -> tuple[np.ndarray, np.ndarray] | None: + """``body_transform`` while a pose is played, or ``None`` when nothing moves. + + Kept for callers that want to know *whether the machine is moving* rather + than where the body is: the drawing uses :meth:`body_transform`, which is + always defined because the mounting pose applies at rest too. """ 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 + return self.body_transform(rotation_share, translation_share) - # 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 - ) + def tool_point(self) -> np.ndarray: + """The tool's position, in mesh coordinates: above the table, at its own XY. - point = self._point_mm - if point is None: - return rotation, np.zeros(3) - contact = np.asarray(point, dtype=np.float64) + Not the patch centre, which is what this used to be. The tool is a fixed + feature of the machine and its XY is a setup number; taking it as the middle + of whatever mesh happened to be loaded made every reported move agree with + the picture and with nothing on the bench. + """ + assert self._bundle is not None + tool_x, tool_y = (float(value) for value in self._stack.tool_xy_mm) + height = self._contact_height() + return self._stack.mount.T @ np.array([tool_x, tool_y, height], 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 _contact_height(self) -> float: + """The picked contact's height above the table origin, after the stages turn.""" + if self._point_mm is None: + return float(self._bundle.centre_mm[2]) if self._bundle is not None else 0.0 + contact = self._stack.setup_point(np.asarray(self._point_mm, dtype=np.float64)) + if self._pose is None or not self._pose.ok: + return float(contact[2]) + turned = self._stack.platform_rotation( + self._pose.yaw_deg, self._pose.pitch_deg + ) @ contact + return float(turned[2]) + + def _placed_anchor(self) -> np.ndarray: + """Where the contact finishes: on the tool axis, at the tool.""" + return self.tool_point() + + def landing_xy_mm(self) -> tuple[float, float] | None: + """Where the picked contact ends up, in the table frame, **from the drawing**. + + Computed by pushing the point through the very transform the animation ends + on, not by re-deriving the solver's algebra — so comparing it with the tool's + X/Y is a check rather than a restatement. The two must agree; when the panel + prints both and they do, "x and y are aligned" is a measurement instead of a + judgement about a picture. + """ + if self._point_mm is None or self._pose is None or not self._pose.ok: + return None + rotation, translation = self.body_transform(1.0, 1.0) + landed = rotation @ np.asarray(self._point_mm, dtype=np.float64) + translation + table = self._stack.mount @ landed + return (float(table[0]), float(table[1])) + + def trajectory(self, samples: int = 40) -> np.ndarray: + """The contact point's path through the solved motion, as an ``(N, 3)`` line. + + Sampled through :meth:`body_transform`, the same function the animation uses, + so the drawn line and the moving body cannot tell different stories. In the + order the machine moves, starting at the picked contact — the point on the + body that has to arrive under the tool. Nothing in the path is at the table + origin: the origin is what the stages turn *about*, and the arc's radius is + the contact's distance from it, which is the number that says whether the + swing stays inside the machine. + + * **rotation** — the stages turn the mounted body about the table origin, + sweeping the contact along an arc. + * **translation** — the table slides the contact onto the tool axis in a + straight line, in the table's own XY. + + Returned rather than drawn so a caller can measure it: the length of the move + and the arc radius are two things that cannot be read off a picture. + """ + if self._pose is None or self._point_mm is None or not self._pose.ok: + return np.empty((0, 3), dtype=np.float64) + + contact = np.asarray(self._point_mm, dtype=np.float64) + anchor = self._placed_anchor() + + def placed(rotation_share: float, translation_share: float) -> np.ndarray: + rotation, translation = self.body_transform(rotation_share, translation_share) + return rotation @ contact + translation + + points: list[np.ndarray] = [] + # Phase one: the stages, sampled by *angle* so the arc is smooth. The share runs + # the whole way to the solved rotation; it used to stop at SIM_ROTATION_SHARE, + # which is how long that phase takes in the animation, not how far it turns — + # so the drawn arc ended 45% short and the slide started from the wrong point. + for index in range(samples): + points.append(placed(index / max(samples - 1, 1), 0.0)) + # Phase two: the table, straight from the turned contact to the tool. + start = points[-1] + for index in range(1, samples + 1): + step = index / samples + points.append(start * (1.0 - step) + anchor * step) + return np.asarray(points, dtype=np.float64) + + def trajectory_summary(self) -> dict[str, float]: + """The numbers that say whether the path is plausible. + + ``reach_mm`` is how far the contact travels, and ``arc_radius_mm`` the radius + of the rotation phase — an arc whose radius exceeds the stage's working + envelope is the shape of a collision. + """ + path = self.trajectory() + if path.shape[0] < 2: + return {} + steps = np.linalg.norm(np.diff(path, axis=0), axis=1) + if self._point_mm is None or self._bundle is None: + return {} + contact = np.asarray(self._point_mm, dtype=np.float64) + origin = self.table_frame()["origin"] + return { + "points": float(path.shape[0]), + "reach_mm": float(steps.sum()), + "arc_radius_mm": float(np.linalg.norm(contact - origin)), + "final_offset_mm": float(np.linalg.norm(path[-1] - self._placed_anchor())), + } def _solve_all(self, bundle: SensorBundle) -> np.ndarray: """The residual the stages achieve at each vertex — the colouring.""" @@ -336,7 +766,24 @@ class PosePreview(QOpenGLWidget): 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)) + self._set_problem(t("finger.preview_no_gl", error=str(exc))) + + def _set_problem(self, message: str, *, draw: bool = True) -> None: + """Record why there is no (or an incomplete) picture, and say so. + + One entry point because a problem has to reach the panel, not just the + widget: a drawing failure that only sets an attribute leaves the operator + looking at a half-drawn scene with nothing to read. Drawing and solving are + tracked apart so that a successful repaint clears a draw error without + erasing a solve error that is still true. + """ + before = self.problem + if draw: + self._draw_problem = message + else: + self._solve_problem = message + if self.problem != before: + self.problemChanged.emit(self.problem) def paintGL(self) -> None: # pragma: no cover - needs a real context if not self._gl_ok or self._bundle is None: @@ -344,8 +791,13 @@ class PosePreview(QOpenGLWidget): try: self._draw() except Exception as exc: # noqa: BLE001 - self._gl_ok = False - self._problem = t("finger.preview_draw_failed", error=str(exc)) + # Deliberately *not* clearing ``_gl_ok``: this used to disable every + # later paint, including the overlay that would have shown the reason, + # so the widget froze on its last frame and reported nothing. A draw + # error is retried on the next paint instead. + self._set_problem(t("finger.preview_draw_failed", error=str(exc))) + else: + self._set_problem("") def has_context(self) -> bool: """Whether a GL context exists at all, for the panel's status line.""" @@ -361,16 +813,29 @@ class PosePreview(QOpenGLWidget): 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. + + ``_pan`` is added on top: with the table frame drawn, the scene is much wider + than the sensor, so the operator needs to be able to steer the view while + zoomed in. """ assert self._bundle is not None - return self._placed(self._bundle.centre_mm) + return self._placed(self._bundle.centre_mm) + self._pan + + def _view_target(self) -> np.ndarray: + """What the camera aims at, for the render and for every projection of it. + + One accessor, because the two used to differ — ``_camera`` orbited the placed + centre while ``_draw`` aimed at the raw mesh centre — and that put the drawn + scene and the projected overlay in different places the moment the body was + drawn anywhere but at the identity. + """ + return self._focus() def _camera(self) -> tuple[np.ndarray, float]: - """Eye position and distance, from the orbit angles and the mesh size.""" + """Eye position and distance, from the orbit angles and the scene's 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 + centre = self._view_target() + distance = self._frame_distance() * self._distance_scale cos_pitch = math.cos(self._pitch) eye = centre + distance * np.array( [ @@ -381,13 +846,79 @@ class PosePreview(QOpenGLWidget): ) return eye, distance + def scene_points(self) -> np.ndarray: + """Every point the view frames on, in the mesh's coordinates, placed. + + The camera used to be framed on the mesh alone — half its bounding-box + diagonal, times three. That cropped the table plane, the trajectory and the + body's own base exactly when a pose swung them outwards, and a pose whose + parts leave the frame reads as a pose that failed. This is the set the + framing is computed from, returned so a test can check the framing without a + GL context. + + The long reference lines — the tool axis and the two rotation axes — are + deliberately **not** in here. They are drawn past the edge of the view on + purpose, and including them would zoom out until the sensor was a speck. + """ + assert self._bundle is not None + parts = [ + self._placed(self._bundle.vertices_mm), + self._placed(self._bundle.housing_vertices_mm), + ] + + frame = self.table_frame() + origin = frame["origin"] + first, second = frame["first"], frame["second"] + normal = frame["plane_normal"] + extent = float(np.linalg.norm(self._bundle.extent_mm)) or 1.0 + span = extent * 0.45 + triad = extent * 0.5 + corners = [origin, frame["base_point"]] + for du, dv in ((-1.0, -1.0), (1.0, -1.0), (1.0, 1.0), (-1.0, 1.0)): + corners.append(origin + (first * du + second * dv) * span) + corners.append(frame["base_point"] + (first * du + second * dv) * span) + for direction in (first, second, normal): + for sign in (1.0, -1.0): + corners.append(origin + direction * triad * sign) + parts.append(np.asarray(corners, dtype=np.float64)) + + # The machine's frame as it stands, and the tool: both move with the setup, + # so a view that framed only the sensor would crop them. + parts.append(self.table_marker_points()) + parts.append(self.tool_point().reshape(1, 3)) + + path = self.trajectory() + if path.size: + parts.append(path) + if self._point_mm is not None: + parts.append(self._placed_contact(self._point_mm).reshape(1, 3)) + return np.vstack(parts) + + def scene_radius(self) -> float: + """How far the framed scene reaches from the point the camera looks at.""" + points = self.scene_points() + return float(np.linalg.norm(points - self._focus(), axis=1).max()) + + def _frame_distance(self) -> float: + """How far the eye must sit for the whole scene to fit the widget. + + Solved from the field of view rather than guessed as a multiple of the + sensor: the tightest of the two half-angles sets the distance, so a short + wide pane and a tall narrow one both frame the same scene. + """ + half_y = DEFAULT_FOV_Y_RAD / 2.0 + aspect = max(1, self.width()) / max(1, self.height()) + half_x = math.atan(math.tan(half_y) * aspect) + radius = max(self.scene_radius(), 1e-6) + return radius / max(math.sin(min(half_y, half_x)), 1e-6) * FRAME_MARGIN + 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() + centre = self._view_target() 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]) @@ -400,8 +931,13 @@ class PosePreview(QOpenGLWidget): """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. + rather than approximating it. The half-extents are measured **at the near + plane**, so the near-plane factor belongs in the normalised coordinates — + ``x_ndc = (near/right) · x_view / depth``. It was missing, which scaled every + projected point by ``1/near`` (about 100 here) and put the axis labels, and + every measurement taken off them, in the wrong place. The test that was + supposed to catch it only asked that the three axis tips land *somewhere* + distinct and finite. """ if self._bundle is None: return None @@ -420,8 +956,8 @@ class PosePreview(QOpenGLWidget): if top <= 0 or right_extent <= 0: return None - normalised_x = float(relative @ right) / (depth * right_extent) - normalised_y = float(relative @ up) / (depth * top) + normalised_x = near * float(relative @ right) / (depth * right_extent) + normalised_y = near * 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 @@ -432,11 +968,23 @@ class PosePreview(QOpenGLWidget): assert self._bundle is not None eye, distance = self._camera() - centre = self._bundle.centre_mm + # ``_view_target`` — the placed model — not the raw mesh centre. Aiming the + # camera at the mesh's own centre while the eye orbits the *placed* centre + # puts the render and every projection of it in different places: the axis + # labels drifted off their arrows and the whole scene sat off to one side as + # soon as the body was drawn anywhere but at the identity. Both call this. + centre = self._view_target() 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) + # Alpha blending, because part of the palette is stored translucent — the + # reference planes and the rotation axes — and without this the alpha was + # simply ignored, so the grid was drawn at full strength and read as part of + # the model rather than a reference behind it. Everything opaque declares + # alpha 1.0, so nothing else changes. + GL.glEnable(GL.GL_BLEND) + GL.glBlendFunc(GL.GL_SRC_ALPHA, GL.GL_ONE_MINUS_SRC_ALPHA) width = max(1, self.width()) height = max(1, self.height()) @@ -455,146 +1003,540 @@ class PosePreview(QOpenGLWidget): self._draw_surface(GL) self._draw_housing(GL) + # The base plane belongs to the body, so it goes on before anything that has + # to be readable over it. + self._draw_base_plane(GL) self._draw_tool(GL, distance) + if self._show_table or self._show_axes: + self._draw_table(GL) if self._show_axes: - self._draw_axes(GL) - if self._sim_transform() is not None: - self._draw_origin(GL) + self._draw_stage_arcs(GL) + # The table's frame as it now stands, over the frame it started in. + self._draw_table_marker(GL) self._draw_pick(GL, distance) + if self._show_trajectory: + self._draw_trajectory(GL) + self._draw_highlight(GL) + + def current_transform(self) -> tuple[np.ndarray, np.ndarray]: + """The body transform the drawing uses right now. + + The animation's phase while a pose is played, the solved end state at rest, + and the mounting pose alone when there is nothing solved — the sensor sits + where its setup says it sits whether or not anything is moving, and drawing + it at the identity would show it where the drawing assumes it is rather than + where it is. + """ + rotation_share, translation_share = self.motion_share() + return self.body_transform(rotation_share, translation_share) 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 + """A sensor-frame point, or an ``(N, 3)`` array of them, as placed now.""" + points = np.asarray(point, dtype=np.float64) + rotation, translation = self.current_transform() + # Right-multiplied so one call covers a single point *and* a whole vertex + # array: ``(N, 3) @ (3, 3).T`` is the same rotation as ``(3, 3) @ (3,)``. + # Left-multiplying worked for a point and raised for the mesh, which broke + # the camera framing — and so the whole view — the moment playback started. + return points @ rotation.T + 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) + """A sensor-frame direction under the current rotation only.""" + return self.current_transform()[0] @ np.asarray(direction, dtype=np.float64) + + def _scene_arrays(self) -> tuple[np.ndarray, np.ndarray, np.ndarray]: + """``(vertices, colours, indices)`` for the sensing surface, cached. + + Rebuilt only when the simulation transform moves. Drawing the surface one + ``glVertex3f`` at a time meant ~18,000 Python-to-GL calls per frame for + 1974 triangles, which is what made the preview crawl; handing GL two flat + arrays and the index list is one call for the same picture. + """ + transform = self.current_transform() + key = (float(np.asarray(transform[0]).sum()), float(np.asarray(transform[1]).sum())) + if self._array_cache is not None and self._array_key == key: + return self._array_cache + + assert self._bundle is not None + vertices = np.asarray(self._bundle.vertices_mm, dtype=np.float64) + # The residual shading stays on while the machine moves. It used to be + # replaced by a flat grey "moving body" colour, on the theory that the + # colouring was a distraction mid-motion — but watching the red patch arrive + # on the tool axis is the entire reason to play the motion, and the flat grey + # threw away the one thing the operator was looking for. + 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)) + ) + rotation, translation = transform + vertices = vertices @ np.asarray(rotation).T + np.asarray(translation) + + cache = ( + np.ascontiguousarray(vertices, dtype=np.float32), + np.ascontiguousarray(colours, dtype=np.float32), + np.ascontiguousarray(np.asarray(self._bundle.faces).reshape(-1), dtype=np.uint32), + ) + self._array_cache = cache + self._array_key = key + return cache 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() + vertices, colours, indices = self._scene_arrays() + GL.glEnableClientState(GL.GL_VERTEX_ARRAY) + GL.glEnableClientState(GL.GL_COLOR_ARRAY) + GL.glVertexPointer(3, GL.GL_FLOAT, 0, vertices) + GL.glColorPointer(3, GL.GL_FLOAT, 0, colours) + GL.glDrawElements(GL.GL_TRIANGLES, indices.size, GL.GL_UNSIGNED_INT, indices) + GL.glDisableClientState(GL.GL_COLOR_ARRAY) + GL.glDisableClientState(GL.GL_VERTEX_ARRAY) 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) + set_colour(GL, "housing") 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_LINE) + vertices, indices = self._housing_arrays() + GL.glEnableClientState(GL.GL_VERTEX_ARRAY) + GL.glVertexPointer(3, GL.GL_FLOAT, 0, vertices) + GL.glDrawElements(GL.GL_TRIANGLES, indices.size, GL.GL_UNSIGNED_INT, indices) + GL.glDisableClientState(GL.GL_VERTEX_ARRAY) 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. + def _housing_arrays(self) -> tuple[np.ndarray, np.ndarray]: + """``(vertices, indices)`` for the housing, cached like the surface.""" + transform = self.current_transform() + key = (float(np.asarray(transform[0]).sum()), float(np.asarray(transform[1]).sum())) + if self._housing_cache is not None and self._housing_key == key: + return self._housing_cache + assert self._bundle is not None + vertices = np.asarray(self._bundle.housing_vertices_mm, dtype=np.float64) + rotation, translation = transform + vertices = vertices @ np.asarray(rotation).T + np.asarray(translation) + cache = ( + np.ascontiguousarray(vertices, dtype=np.float32), + np.ascontiguousarray( + np.asarray(self._bundle.housing_faces).reshape(-1), dtype=np.uint32 + ), + ) + self._housing_cache = cache + self._housing_key = key + return cache - 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. + def _draw_tool(self, GL, distance: float) -> None: # pragma: no cover - context + """The tool axis: a fixed vertical line at the tool's own position. + + Drawn along ``tool_axis_direction`` — the way the sensing face points once + it is aligned — rather than the tool's feed direction. The two are the same + line, but the sensor sits at the bottom of its shell with its face up while + the tool feeds down, so the feed arrow read as back-to-front against the + surface it is meant to line up with. + + It does **not** follow the picked contact. The tool is bolted to the machine + and the table moves the sensor under it, so a line that chased the contact + would hide the very motion the picture exists to show: it used to be drawn + through the patch's centre, which made the alignment read as "these two are + parallel" instead of "the contact arrives here". """ 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) + # The axis the face aligns to, not the direction the tool travels. + local = to_sensor(self._stack.tool_axis_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 + anchor = self.tool_point() + start = anchor - local * extent * 0.6 + end = anchor + local * extent * 0.6 + # Depth testing off, like the table marker and the planned path. The tool axis + # is a reference, not part of the model, and it is the one line the operator + # compares the contact's normal against: with depth on it vanished into the + # body whenever the solved pose turned the sensing face away from the camera, + # which is exactly the pose worth looking at. + GL.glDisable(GL.GL_DEPTH_TEST) GL.glLineWidth(2.0) - GL.glColor3f(0.95, 0.85, 0.25) + set_colour(GL, "tool_axis") 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) + GL.glEnable(GL.GL_DEPTH_TEST) 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 contact point as the simulation currently places it. - 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. + Full transform, rotation **and** translation. An earlier version applied the + rotation only, on the theory that the marker is the measurement and should + stay where it was clicked while the body slid away. It reads wrong: the + point never reaches the tool axis, so "the table moves the contact under the + tool" — the whole content of the translation phase — is invisible, and the + marker looks stranded on the side of the body. + + With the table's offset included, the dot travels from where it was picked + to the tool axis, which is exactly what the reported X/Y claim. """ - transform = self._sim_transform() - if transform is None: - return np.asarray(point, dtype=np.float64) - return transform[0] @ np.asarray(point, dtype=np.float64) + return self._placed(point) - 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_highlight(self, GL) -> None: # pragma: no cover - needs a context + """The table's chosen point as a red dot on the surface. - 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. + Depth testing is off for this one dot. The point is *on* the surface it is + drawn against, so leaving depth on makes it fight the very triangles it sits + on and it z-flickers; drawing it last, unlit, means the marker is always + legible while everything else stays correctly ordered. """ - if self._point_mm is None: + if self._highlight_mm is None: return - pivot = self._placed_contact(self._point_mm) - GL.glPointSize(7.0) - GL.glColor3f(0.95, 0.55, 0.20) + point = self._placed_contact(self._highlight_mm) + GL.glDisable(GL.GL_DEPTH_TEST) + GL.glPointSize(13.0) + set_colour(GL, "selected") GL.glBegin(GL.GL_POINTS) - GL.glVertex3f(float(pivot[0]), float(pivot[1]), float(pivot[2])) + GL.glVertex3f(float(point[0]), float(point[1]), float(point[2])) + GL.glEnd() + GL.glEnable(GL.GL_DEPTH_TEST) + + def table_frame(self) -> dict[str, np.ndarray]: + """The motion table's frame **at the initial state**, in mesh coordinates. + + The bench description this has to reproduce: + + * the yaw and pitch axes **pass through one origin**, which is the table's; + * the sensor's mounting face sits **above that origin plane**, along the + platform's tool axis, by the sensor's z setup offset — which is where the + mounting plate lives. + + Conventions, all of which are the machine's rather than the picture's: + + * the table's origin plane is the platform's ``x``-``y`` plane at + ``platform_z = 0`` — its normal is the tool axis; + * the sensor's mounting face is at ``platform_z = offset_z`` in the setup + state, so the nominal base point is the origin lifted by that much. The + drawn outline is the real face, pushed through the body transform, because + the mounting x, y and yaw move it off this nominal point; + * the pitch axis runs through that origin along the platform's pitch axis. + + Returned rather than drawn, so a test can check the geometry without a GL + context: the whole point of drawing this is to make a transformation mistake + visible, and it cannot do that if the drawing itself is the only definition. + """ + plate = float(self._stack.sensor_offset_mm[2]) + tool_axis = self._stack.tool_axis + pitch_index = ("x", "y", "z").index(self._stack.pitch_axis) + tool_index = ("x", "y", "z").index(tool_axis) + + # Sensor-frame basis directions, so every vector below is in mesh coordinates. + def sensor(direction: np.ndarray) -> np.ndarray: + return to_sensor(np.asarray(direction, dtype=np.float64), self._stack.axis_map) + + normal = sensor(np.eye(3)[tool_index]) + pitch_direction = sensor(np.eye(3)[pitch_index]) + yaw_direction = sensor(np.eye(3)[("x", "y", "z").index(self._stack.yaw_axis)]) + + # Two in-plane directions for the grid and the table triad. Chosen from the + # smallest component of the normal rather than a fixed helper: a helper that + # happens to be parallel to the normal gives a degenerate cross product and + # the plane comes out at an arbitrary orientation. + helper = np.eye(3)[int(np.argmin(np.abs(normal)))] + first = np.cross(normal, helper) + first = first / max(float(np.linalg.norm(first)), 1e-12) + second = np.cross(normal, first) + + origin = np.zeros(3, dtype=np.float64) + base_point = origin + normal * plate + + return { + #: The table's origin: where the two rotation axes cross. + "origin": origin, + #: The nominal sensor mounting face, ``plate_mm`` above the origin. + "base_point": base_point, + "plane_normal": normal, + "first": first, + "second": second, + "pitch_direction": pitch_direction, + "yaw_direction": yaw_direction, + #: The sensor's z setup offset: the mounting plate plus any shim. + "plate_mm": np.array([plate]), + } + + def _draw_table(self, GL) -> None: # pragma: no cover - needs a context + """Draw the machine's frame: the part that never moves, and the part that does. + + The **initial state is the fixed one**: the origin plane, the triad at the + origin and the two rotation axes are the inertial reference, and they must + not move when the simulation plays. The base plane and the table marker are + drawn after this, transformed, so the picture shows the transformation + relative to where the machine started. + """ + assert self._bundle is not None + frame = self.table_frame() + origin = frame["origin"] + normal = frame["plane_normal"] + extent = float(np.linalg.norm(self._bundle.extent_mm)) or 1.0 + # Sized against the sensor, not the other way round. A plane spanning more + # than the sensor reduces the body to a speck and forces the operator to zoom + # before they can read anything. This reads as a local reference frame that + # the sensor sits on, and it keeps the camera framing on the sensor too — + # the plane is the widest thing in the scene. + span = extent * 0.45 + first, second = frame["first"], frame["second"] + + self._draw_table_plane(GL, origin, first, second, span) + self._axis_tips.clear() + + # The table's own triad, at the table origin. This is the frame the machine + # rotates and translates *reference to*, so it belongs here rather than at + # the sensor's centre — that was the sensor's pose being drawn as if it were + # the machine's frame. + triad = extent * 0.5 + for axis, direction in ( + ("x", first), + ("y", second), + ("z", normal), + ): + for sign in (1.0, -1.0): + tip = origin + direction * triad * sign + GL.glLineWidth(2.5) + set_rgb(GL, 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, direction * sign, extent * 0.03) + + # Yaw and pitch axes: the two that must pass through the one origin. Drawn + # long, so a misalignment shows as the lines missing each other rather than + # hiding inside the body. Thin and grey, because the triad above is what + # names directions; these only say "this line is the axis". + for direction in (frame["yaw_direction"], frame["pitch_direction"]): + start = origin - direction * span * 1.5 + end = origin + direction * span * 1.5 + GL.glLineWidth(1.0) + set_colour(GL, "rotation_axis") + 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() + + # The table origin itself, on top of everything so it is never hidden. + GL.glDisable(GL.GL_DEPTH_TEST) + GL.glPointSize(11.0) + set_colour(GL, "origin") + GL.glBegin(GL.GL_POINTS) + GL.glVertex3f(float(origin[0]), float(origin[1]), float(origin[2])) + GL.glEnd() + GL.glEnable(GL.GL_DEPTH_TEST) + + def _draw_base_plane(self, GL) -> None: # pragma: no cover - needs a context + """The sensor's mounting face, drawn where the body currently is. + + Defined in the **sensor's** coordinates and pushed through the body + transform, rather than computed from the table frame. The mounting face is + part of the sensor: it turns with the stages and rides the table, so a + version pinned to the table origin stayed behind the moment anything moved — + which is exactly the transformation the picture is supposed to show. + """ + assert self._bundle is not None + housing = self._bundle.housing_vertices_mm + # The housing's base sits at mesh y = 0 with its own footprint in x and z. + half_x = float(np.max(np.abs(housing[:, 0]))) * 1.12 + half_z = float(np.max(np.abs(housing[:, 2]))) * 0.55 + corners = np.array( + [ + [du * half_x, 0.0, dv * half_z] + for du, dv in ((-1.0, -1.0), (1.0, -1.0), (1.0, 1.0), (-1.0, 1.0)) + ], + dtype=np.float64, + ) + placed = self._placed(corners) + GL.glDisable(GL.GL_DEPTH_TEST) + GL.glLineWidth(2.0) + set_colour(GL, "base_outline") + GL.glBegin(GL.GL_LINE_LOOP) + for point in placed: + GL.glVertex3f(float(point[0]), float(point[1]), float(point[2])) + GL.glEnd() + GL.glEnable(GL.GL_DEPTH_TEST) + + def table_marker_points(self, share: float | None = None) -> np.ndarray: + """The transformed table frame as a ring of points, in mesh coordinates. + + The machine's own frame after the move: the stages' rotation and the table's + slide applied to a circle drawn in the origin plane. Compared with the fixed + grid it says, without arithmetic, how far and which way the machine went — + and the circle is deliberately a *different shape* from the square grid, so + "which one is the reference" is answerable at a glance. + """ + assert self._bundle is not None + extent = float(np.linalg.norm(self._bundle.extent_mm)) or 1.0 + radius = extent * 0.42 + steps = 72 + angles = np.linspace(0.0, 2.0 * np.pi, steps, endpoint=False) + frame = self.table_frame() + first, second = frame["first"], frame["second"] + ring = np.array( + [ + first * (radius * math.cos(angle)) + second * (radius * math.sin(angle)) + for angle in angles + ], + dtype=np.float64, + ) + rotation, translation = self.table_marker_transform(share) + return ring @ np.asarray(rotation).T + np.asarray(translation) + + def _draw_table_marker(self, GL) -> None: # pragma: no cover - needs a context + """The machine's frame where it stands, over the frame it started in. + + The solid ring is the table's pose at the phase being drawn — the end of the + move at rest, the current phase while it plays. The faint ring is where the + move *started*, drawn only while the two differ. With the fixed grid, which is + the same frame before the move, the three read as before, now, and after — + and the before ring sits exactly on the grid whenever nothing has happened. + """ + assert self._bundle is not None + extent = float(np.linalg.norm(self._bundle.extent_mm)) or 1.0 + GL.glDisable(GL.GL_DEPTH_TEST) + + start = self.initial_marker_points() + if not np.allclose(start, self.table_marker_points(), atol=1e-9): + GL.glLineWidth(1.5) + alpha = COLOUR["table_marker"][3] if len(COLOUR["table_marker"]) > 3 else 0.45 + GL.glColor4f( + float(COLOUR["table_marker"][0]), + float(COLOUR["table_marker"][1]), + float(COLOUR["table_marker"][2]), + max(float(alpha), 0.45), + ) + GL.glBegin(GL.GL_LINE_LOOP) + for point in start: + GL.glVertex3f(float(point[0]), float(point[1]), float(point[2])) + GL.glEnd() + + GL.glLineWidth(2.0) + set_colour(GL, "table_marker") + GL.glBegin(GL.GL_LINE_LOOP) + for point in self.table_marker_points(): + GL.glVertex3f(float(point[0]), float(point[1]), float(point[2])) GL.glEnd() + # Cross-hairs through the marker's own centre, so its orientation is readable + # even when the ring is seen almost edge-on. + rotation, translation = self.table_marker_transform() + frame = self.table_frame() + for direction in (frame["first"], frame["second"]): + arm = direction * extent * 0.42 + start = np.asarray(rotation) @ (-arm) + translation + end_point = np.asarray(rotation) @ arm + translation + GL.glBegin(GL.GL_LINES) + GL.glVertex3f(float(start[0]), float(start[1]), float(start[2])) + GL.glVertex3f(float(end_point[0]), float(end_point[1]), float(end_point[2])) + GL.glEnd() + GL.glEnable(GL.GL_DEPTH_TEST) + + def _draw_table_plane( # pragma: no cover - needs a context + self, + GL, + origin: np.ndarray, + first: np.ndarray, + second: np.ndarray, + span: float, + divisions: int = 4, + ) -> None: + """The origin plane, as a grid only. + + No filled quad. A translucent fill was drawn first and it read as a solid + surface the sensor was embedded in rather than a reference behind it — the + grid alone says where the plane is without claiming to be part of the model. + + Few divisions on purpose: ten lines across each way at 45% alpha swamped the + body. Four is enough to read the scale and the orientation. + """ + GL.glDisable(GL.GL_DEPTH_TEST) + GL.glLineWidth(1.0) + set_colour(GL, "plane_grid") + GL.glBegin(GL.GL_LINES) + for index in range(divisions + 1): + fraction = -span + 2.0 * span * index / divisions + for start, end in ( + (origin + first * fraction - second * span, origin + first * fraction + second * span), + (origin + second * fraction - first * span, origin + second * fraction + first * span), + ): + GL.glVertex3f(float(start[0]), float(start[1]), float(start[2])) + GL.glVertex3f(float(end[0]), float(end[1]), float(end[2])) + GL.glEnd() + GL.glEnable(GL.GL_DEPTH_TEST) + + def _draw_trajectory(self, GL) -> None: # pragma: no cover - needs a context + """The contact's planned path, with arrowheads showing which way it runs. + + Arrowheads rather than the V-shaped ticks this used to draw. A tick every + six samples put thirteen little chevrons along a 19 mm path, and the curve + read as a spring rather than as a move — the picture was answering "which + way" so loudly that "where" was hard to see. Four arrows say the same thing + and leave the line legible. The end is marked separately, because the finish + is the part that has to be under the tool. + """ + path = self.trajectory() + if path.shape[0] < 2: + return + GL.glDisable(GL.GL_DEPTH_TEST) + GL.glLineWidth(2.0) + set_colour(GL, "path") + GL.glBegin(GL.GL_LINE_STRIP) + for point in path: + GL.glVertex3f(float(point[0]), float(point[1]), float(point[2])) + GL.glEnd() + + set_colour(GL, "path_tick") + GL.glBegin(GL.GL_LINES) + arrows = min(4, max(1, (path.shape[0] - 2) // 8)) + for step in range(1, arrows + 1): + index = min(path.shape[0] - 2, int(path.shape[0] * step / (arrows + 1))) + here, following = path[index], path[index + 1] + along = following - here + length = float(np.linalg.norm(along)) + if length < 1e-12: + continue + along = along / length + side = np.cross(along, np.array([0.0, 1.0, 0.0])) + side = side / max(float(np.linalg.norm(side)), 1e-12) + # Sized off the whole path, not the gap between two samples: the samples + # are dense, so a head scaled to them is invisible on a long move and + # huge on a short one. + head = float(np.linalg.norm(path[-1] - path[0])) * 0.08 or 0.4 + tip = here + along * head + for corner in (tip - along * head + side * head * 0.55, + tip - along * head - side * head * 0.55): + GL.glVertex3f(float(tip[0]), float(tip[1]), float(tip[2])) + GL.glVertex3f(float(corner[0]), float(corner[1]), float(corner[2])) + GL.glEnd() + + GL.glPointSize(12.0) + set_colour(GL, "path") + GL.glBegin(GL.GL_POINTS) + end = path[-1] + GL.glVertex3f(float(end[0]), float(end[1]), float(end[2])) + GL.glEnd() + GL.glEnable(GL.GL_DEPTH_TEST) + def _draw_pick(self, GL, distance: float) -> None: # pragma: no cover - context + """The picked contact and its normal, both following the simulated motion. + + The point takes the full transform — including the table's translation — so + during playback it travels onto the tool axis while the normal swings onto + it. The normal takes the rotation only: it is a direction, and translating a + direction would be meaningless. + """ 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) + set_colour(GL, "contact") GL.glBegin(GL.GL_POINTS) GL.glVertex3f(float(point[0]), float(point[1]), float(point[2])) GL.glEnd() @@ -604,7 +1546,7 @@ class PosePreview(QOpenGLWidget): 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) + set_colour(GL, "normal") 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])) @@ -612,39 +1554,16 @@ class PosePreview(QOpenGLWidget): _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. + def _draw_stage_arcs(self, GL) -> None: # pragma: no cover - needs a context + """A short turn arc about each rotation axis, at the table origin. - 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. + Moved here with the triad. Drawn at the sensor's centre they showed the arcs + around a point that is not the rotation centre, which is the opposite of what + an arc is for. """ - 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 - + origin = self.table_frame()["origin"] self._draw_stage_arc(GL, origin, extent, self._stack.yaw_axis, "yaw") self._draw_stage_arc(GL, origin, extent, self._stack.pitch_axis, "pitch") @@ -665,7 +1584,7 @@ class PosePreview(QOpenGLWidget): radius = extent * 0.30 steps = 24 GL.glLineWidth(1.5) - GL.glColor3f(0.85, 0.85, 0.85) + set_rgb(GL, STAGE_ARC_COLOUR) GL.glBegin(GL.GL_LINE_STRIP) for index in range(steps + 1): angle = math.radians(100.0) * index / steps @@ -685,12 +1604,12 @@ class PosePreview(QOpenGLWidget): 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 + Without a context Qt never calls ``initializeGL``, so the 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") + if self._gl_ok and not self.has_context() and not self.problem: + self._set_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.""" @@ -736,33 +1655,54 @@ class PosePreview(QOpenGLWidget): # 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: + 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 + self.problem, Qt.TextElideMode.ElideRight, self.width() - 20 ), ) finally: painter.end() - # -------------------------------------------------------------------- orbit + # -------------------------------------------------------------- orbit / pan def mousePressEvent(self, event) -> None: # noqa: N802 - Qt naming self._last_pos = event.position().toPoint() + self._panning = event.button() != Qt.MouseButton.LeftButton 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) - ) + if self._panning: + self._pan_by(delta.x(), delta.y()) + else: + 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 mouseReleaseEvent(self, event) -> None: # noqa: N802 - Qt naming + self._panning = False + + def _pan_by(self, dx: int, dy: int) -> None: + """Slide the focus across the screen plane, in world units. + + Scaled by the view's own distance so a drag moves the picture about as much + as the cursor does, at any zoom. Without that, panning is unusable when + zoomed in and useless when zoomed out. + """ + if self._bundle is None: + return + eye, distance = self._camera() + _, _, right, up = self._view_basis(eye) + per_pixel = distance * math.tan(DEFAULT_FOV_Y_RAD / 2.0) / max(self.height(), 1) * 2.0 + self._pan = self._pan - right * dx * per_pixel + up * dy * per_pixel + def wheelEvent(self, event) -> None: # noqa: N802 - Qt naming steps = event.angleDelta().y() / 120.0 self._distance_scale = max( diff --git a/apps/autocal/src/orisys_autocal/ui/sensor_panel.py b/apps/autocal/src/orisys_autocal/ui/sensor_panel.py index 667bd22..1664f7c 100644 --- a/apps/autocal/src/orisys_autocal/ui/sensor_panel.py +++ b/apps/autocal/src/orisys_autocal/ui/sensor_panel.py @@ -18,11 +18,12 @@ not. from __future__ import annotations +import math 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.QtGui import QColor, QImage, QPainter, QPen, QPixmap from PySide6.QtWidgets import ( QCheckBox, QDoubleSpinBox, @@ -51,6 +52,11 @@ from ..sensor.uv_flat import FlatSurface #: How many picked points the list keeps before the oldest is dropped. MAX_POINTS = 200 +#: How close a stored point must be to a raster entry to be shown on the flat +#: image. One pixel of the 1024-wide raster is about 0.0147 mm on this export, so +#: this is a pixel and a half — finer than the view can render anyway. +_POINT_MATCH_MM = 0.02 + class FlatSurfaceView(QLabel): """The flat image, clickable, with the picked pixel cross-haired. @@ -75,6 +81,7 @@ class FlatSurfaceView(QLabel): self._flat: FlatSurface | None = None self._pixmap: QPixmap | None = None self._marker: tuple[int, int] | None = None + self._selected: tuple[int, int] | None = None self.setCursor(Qt.CursorShape.CrossCursor) @property @@ -84,6 +91,7 @@ class FlatSurfaceView(QLabel): def set_flat(self, flat: FlatSurface | None) -> None: self._flat = flat self._marker = None + self._selected = None if flat is None: self._pixmap = None self.clear() @@ -101,21 +109,63 @@ class FlatSurfaceView(QLabel): self._marker = pixel self._refresh() + def set_selected(self, pixel: tuple[int, int] | None) -> None: + """The point currently chosen in the table — the one about to be tested. + + Drawn as a filled red dot, distinct from the cross-hair left by the last + click, so "what I just picked" and "what is about to run" are not confused. + """ + self._selected = pixel + self._refresh() + + @property + def selected_pixel(self) -> tuple[int, int] | None: + return self._selected + + def pixel_for_point(self, point_mm) -> tuple[int, int] | None: + """The pixel showing a given sensor-frame 3D point, if one does. + + Inverse of :meth:`FlatSurface.sample`: search the table for the entry whose + recorded position matches. Exact identity is not expected — a point may + have been edited — so the nearest entry within a pixel wins, and ``None`` + means the point is not on the rasterised surface at all. + """ + flat = self._flat + if flat is None: + return None + target = np.asarray(point_mm, dtype=np.float64) + covered = flat.face_index >= 0 + if not covered.any(): + return None + rows, columns = np.nonzero(covered) + positions = flat.positions_mm[rows, columns] + distances = np.linalg.norm(positions - target, axis=1) + best = int(np.argmin(distances)) + if float(distances[best]) > _POINT_MATCH_MM: + return None + return int(columns[best]), int(rows[best]) + def _marked(self) -> QPixmap | None: - """The native-size image with the marker drawn on it, or ``None``.""" + """The native-size image with both markers drawn on it, or ``None``.""" if self._pixmap is None: return None - if self._marker is None: + if self._marker is None and self._selected 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) + if self._marker is not None: + 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) + if self._selected is not None: + x, y = self._selected + painter.setPen(QPen(QColor("#ffffff"), 2)) + painter.setBrush(QColor("#d92d20")) + painter.drawEllipse(QPoint(x, y), 6, 6) finally: painter.end() return canvas @@ -177,12 +227,27 @@ class FingerPanel(QGroupBox): pickRequested = Signal(int, int) stackChanged = Signal() + #: Columns of the point table, minus the leading select checkbox. The first six + #: are the ones an operator may edit — the sensor-frame point and its normal — + #: and everything after them is *solved*, so it is redrawn from the edit rather + #: than typed into. + COLUMNS: tuple[str, ...] = ( + "x", "y", "z", "nx", "ny", "nz", + "X", "Y", "Z", "yaw", "pitch", "residual", + ) + #: How many of ``COLUMNS`` are inputs rather than solved outputs. + INPUT_COLUMNS = 6 + #: Offset of the first data column; column 0 holds the select checkbox. + SELECT_COLUMN = 0 + def __init__( self, stack: PlatformStack | None = None, *, bundle_path: str = "", - plate_thickness_mm: float = 0.0, + sensor_offset_mm: tuple[float, float, float] = (0.0, 0.0, 0.0), + sensor_yaw_deg: float = 0.0, + tool_xy_mm: tuple[float, float] = (0.0, 0.0), parent: QWidget | None = None, ) -> None: super().__init__(parent) @@ -191,6 +256,11 @@ class FingerPanel(QGroupBox): self._bundle: SensorBundle | None = None #: Full-precision picked rows, index-aligned with the table. self._picked: list[dict[str, float]] = [] + #: True while this panel is writing the table itself, so the ``itemChanged`` + #: handler can tell its own redraw from an operator's edit. Without it every + #: solved-column update would be re-read as an edit and re-solve. + self._writing = False + self._headers = self.COLUMNS outer = QVBoxLayout(self) outer.setContentsMargins(10, 8, 10, 10) @@ -212,11 +282,21 @@ class FingerPanel(QGroupBox): body.addWidget(picker_box, 2) body.addWidget(readout_box, 3) body.addWidget(preview_box, 5) + # The body outranks the table, because the 3D view is the pane an operator + # actually reads the pose from, and at an even split it came out 240 px tall + # — a box too short to judge a normal in, under three lines of legend. outer.addLayout(body, 1) - outer.addWidget(self._build_point_table(), 2) + # ``Maximum`` so the table stops at its natural height and the spare vertical + # space goes to the 3D view above it. Left expandable it ate the window: the + # preview stayed at its 240 px floor while the table had hundreds of empty + # pixels under three rows. + table_box = self._build_point_table() + table_box.setSizePolicy(QSizePolicy.Policy.Preferred, QSizePolicy.Policy.Maximum) + outer.addWidget(table_box) - self.set_plate_thickness(plate_thickness_mm) + self.set_sensor_setup(sensor_offset_mm, sensor_yaw_deg) + self.set_tool_xy(*tool_xy_mm) self.retranslate() # ------------------------------------------------------------------ widgets @@ -286,9 +366,9 @@ class FingerPanel(QGroupBox): "sensor_normal", "platform_point", "pose", + "table_move", "residual", "frames", - "bundle", ) for row, key in enumerate(rows): label = QLabel(box) @@ -307,33 +387,101 @@ class FingerPanel(QGroupBox): 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) + # The mounting pose and the tool: the four numbers and the two that describe + # where the parts actually are. Spin boxes rather than a text field because + # these get dialled in against a real setup, one step at a time. + self.tool_label = QLabel(box) + self.tool_label.setObjectName("hintLabel") + tool_row = QWidget(box) + tool_row_layout = QHBoxLayout(tool_row) + tool_row_layout.setContentsMargins(0, 0, 0, 0) + tool_row_layout.setSpacing(6) + self.tool_x_spin = self._mm_spin(box, -500.0, 500.0) + self.tool_y_spin = self._mm_spin(box, -500.0, 500.0) + tool_row_layout.addWidget(self.tool_x_spin) + tool_row_layout.addWidget(self.tool_y_spin) + grid.addWidget(self.tool_label, len(rows), 0) + grid.addWidget(tool_row, 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) + self.setup_xy_label = QLabel(box) + self.setup_xy_label.setObjectName("hintLabel") + setup_xy_row = QWidget(box) + setup_xy_layout = QHBoxLayout(setup_xy_row) + setup_xy_layout.setContentsMargins(0, 0, 0, 0) + setup_xy_layout.setSpacing(6) + self.sensor_x_spin = self._mm_spin(box, -500.0, 500.0) + self.sensor_y_spin = self._mm_spin(box, -500.0, 500.0) + setup_xy_layout.addWidget(self.sensor_x_spin) + setup_xy_layout.addWidget(self.sensor_y_spin) + grid.addWidget(self.setup_xy_label, len(rows) + 1, 0) + grid.addWidget(setup_xy_row, len(rows) + 1, 1) + + self.setup_zy_label = QLabel(box) + self.setup_zy_label.setObjectName("hintLabel") + setup_zy_row = QWidget(box) + setup_zy_layout = QHBoxLayout(setup_zy_row) + setup_zy_layout.setContentsMargins(0, 0, 0, 0) + setup_zy_layout.setSpacing(6) + # z is the mounting plate and any shim: one number, because a second field + # for the plate would be a second spelling of the same quantity. + self.sensor_z_spin = self._mm_spin(box, -100.0, 200.0) + self.sensor_yaw_spin = QDoubleSpinBox(box) + self.sensor_yaw_spin.setObjectName("compactField") + self.sensor_yaw_spin.setRange(-180.0, 180.0) + self.sensor_yaw_spin.setDecimals(3) + self.sensor_yaw_spin.setSingleStep(0.1) + self.sensor_yaw_spin.setSuffix(" °") + self.sensor_yaw_spin.setFixedHeight(28) + self.sensor_yaw_spin.setToolTip(t("finger.setup_hint")) + setup_zy_layout.addWidget(self.sensor_z_spin) + setup_zy_layout.addWidget(self.sensor_yaw_spin) + grid.addWidget(self.setup_zy_label, len(rows) + 2, 0) + grid.addWidget(setup_zy_row, len(rows) + 2, 1) + + for spin in ( + self.tool_x_spin, + self.tool_y_spin, + self.sensor_x_spin, + self.sensor_y_spin, + self.sensor_z_spin, + self.sensor_yaw_spin, + ): + spin.valueChanged.connect(self._on_frame_inputs_changed) + + # The two summary lines live here rather than under the 3D view, where they + # were competing with it for height: both are numbers about the pick, so they + # belong beside the numbers. Three lines of legend under the picture left the + # picture 240 px tall on a 1080 screen — the one pane that has to be read. + self.reach_label = QLabel(box) + self.reach_label.setObjectName("hintLabel") + self.reach_label.setWordWrap(True) + grid.addWidget(self.reach_label, len(rows) + 3, 0, 1, 2) + self.path_label = QLabel(box) + self.path_label.setObjectName("hintLabel") + self.path_label.setWordWrap(True) + grid.addWidget(self.path_label, len(rows) + 4, 0, 1, 2) + grid.setRowStretch(len(rows) + 5, 1) grid.setColumnStretch(1, 1) self._readout_box = box return box + def _mm_spin(self, box: QWidget, low: float, high: float) -> QDoubleSpinBox: + """A millimetre field with the shared range, step and suffix. + + ``compactField`` because these sit two to a row: the theme's normal input + padding makes a field 40 px tall, which is right in a form and overflows a + cell in a paired column. + """ + spin = QDoubleSpinBox(box) + spin.setObjectName("compactField") + spin.setRange(low, high) + spin.setDecimals(3) + spin.setSingleStep(0.1) + spin.setSuffix(" mm") + spin.setFixedHeight(28) + return spin + def _build_preview(self) -> QWidget: from .pose_preview import PosePreview @@ -343,11 +491,32 @@ class FingerPanel(QGroupBox): column.setContentsMargins(8, 6, 8, 8) column.setSpacing(6) self.preview = PosePreview(box) + # The full colour key, on hover. The one-line label below carries the short + # version: a three-sentence legend under the picture took the height the + # picture needed, and a key nobody can see is no better than no key. + self.preview.setToolTip(t("finger.preview_legend")) column.addWidget(self.preview, 1) + checks = QHBoxLayout() + checks.setContentsMargins(0, 0, 0, 0) + checks.setSpacing(10) 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) + checks.addWidget(self.axes_check) + # The table's own frame: origin plane, the two rotation axes through it, the + # table marker, and the sensor base lifted by its z setup offset. Kept + # separate from the sensor axes above, because this is the machine's frame. + self.table_check = QCheckBox(box) + self.table_check.setChecked(True) + self.table_check.toggled.connect(self.preview.set_show_table) + checks.addWidget(self.table_check) + # The planned path: the arc the stages sweep, then the straight table move. + self.trajectory_check = QCheckBox(box) + self.trajectory_check.setChecked(True) + self.trajectory_check.toggled.connect(self.preview.set_show_trajectory) + checks.addWidget(self.trajectory_check) + checks.addStretch(1) + column.addLayout(checks) controls = QHBoxLayout() controls.setContentsMargins(0, 0, 0, 0) controls.setSpacing(6) @@ -358,6 +527,10 @@ class FingerPanel(QGroupBox): self.sim_button.setEnabled(False) self.sim_button.clicked.connect(self.preview.start_simulation) self.preview.simulationChanged.connect(self._on_simulation_changed) + # A failed paint has to reach a label. Left to the widget alone it showed as + # a scene that was merely missing some of its parts, which is how a broken + # colour call hid the base plane and the whole trajectory without a word. + self.preview.problemChanged.connect(lambda _message: self._refresh_reach()) controls.addWidget(self.sim_button) controls.addStretch(1) column.addLayout(controls) @@ -368,10 +541,6 @@ class FingerPanel(QGroupBox): 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 @@ -382,35 +551,49 @@ class FingerPanel(QGroupBox): 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 = QTableWidget(0, len(self._headers) + 1, box) self.table.verticalHeader().setVisible(False) - self.table.setSelectionMode(QTableWidget.SelectionMode.NoSelection) - self.table.setEditTriggers(QTableWidget.EditTrigger.NoEditTriggers) + self.table.setSelectionBehavior(QTableWidget.SelectionBehavior.SelectRows) + self.table.setSelectionMode(QTableWidget.SelectionMode.ExtendedSelection) + # Editable, but only on a deliberate double-click: a single click selects + # the row (and drives the markers), which is what an operator does most. + self.table.setEditTriggers( + QTableWidget.EditTrigger.DoubleClicked + | QTableWidget.EditTrigger.EditKeyPressed + ) self.table.setAlternatingRowColors(True) - for index in range(len(headers)): + self.table.setColumnWidth(0, 34) + for index in range(1, len(self._headers) + 1): self.table.setColumnWidth(index, 82) self.table.horizontalHeader().setSectionResizeMode( - len(headers) - 1, QHeaderView.ResizeMode.Stretch + len(self._headers), QHeaderView.ResizeMode.Stretch ) self.table.setMinimumHeight(90) + self.table.itemChanged.connect(self._on_item_changed) + self.table.itemSelectionChanged.connect(self._on_selection_changed) column.addWidget(self.table, 1) row = QHBoxLayout() self.clear_button = QPushButton(box) self.clear_button.clicked.connect(self.clear_points) + self.remove_button = QPushButton(box) + self.remove_button.clicked.connect(self.remove_selected) + self.up_button = QPushButton(box) + self.up_button.clicked.connect(lambda: self.move_selected(-1)) + self.down_button = QPushButton(box) + self.down_button.clicked.connect(lambda: self.move_selected(1)) + for button in (self.clear_button, self.remove_button, self.up_button, self.down_button): + row.addWidget(button) + row.addStretch(1) + self._selected_label = QLabel(box) + self._selected_label.setObjectName("hintLabel") + row.addWidget(self._selected_label) 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 @@ -424,13 +607,68 @@ class FingerPanel(QGroupBox): return self._stack @property - def plate_thickness_mm(self) -> float: - return float(self.plate_spin.value()) + def sensor_offset_mm(self) -> tuple[float, float, float]: + """How the sensor is mounted, in the table frame: x, y and z (the plate).""" + return ( + float(self.sensor_x_spin.value()), + float(self.sensor_y_spin.value()), + float(self.sensor_z_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) + @property + def sensor_yaw_deg(self) -> float: + return float(self.sensor_yaw_spin.value()) + + @property + def tool_xy_mm(self) -> tuple[float, float]: + return (float(self.tool_x_spin.value()), float(self.tool_y_spin.value())) + + def set_tool_xy(self, x_mm: float, y_mm: float) -> None: + for spin, value in ((self.tool_x_spin, x_mm), (self.tool_y_spin, y_mm)): + spin.blockSignals(True) + spin.setValue(float(value)) + spin.blockSignals(False) + self._apply_frame_inputs() + + def set_sensor_setup(self, offset_mm, yaw_deg: float) -> None: + """Put the mounting pose into the fields, then push it to the stack.""" + values = tuple(float(value) for value in offset_mm) + (0.0, 0.0, 0.0) + for spin, value in zip( + (self.sensor_x_spin, self.sensor_y_spin, self.sensor_z_spin), values + ): + spin.blockSignals(True) + spin.setValue(value) + spin.blockSignals(False) + self.sensor_yaw_spin.blockSignals(True) + self.sensor_yaw_spin.setValue(float(yaw_deg)) + self.sensor_yaw_spin.blockSignals(False) + self._apply_frame_inputs() + + def _on_frame_inputs_changed(self) -> None: + """The setup or the tool moved: the geometry has changed, so re-solve.""" + self._apply_frame_inputs() + self._resolve_everything() + + def _apply_frame_inputs(self) -> None: + self._stack.sensor_offset_mm = self.sensor_offset_mm + self._stack.sensor_yaw_deg = self.sensor_yaw_deg + self._stack.tool_xy_mm = self.tool_xy_mm + self.preview.set_setup(self.sensor_offset_mm, self.sensor_yaw_deg) + self.preview.set_tool_xy(*self.tool_xy_mm) + + def _resolve_everything(self) -> None: + """Re-solve every stored row, then redraw what the current one shows. + + Every row, not only the current one: the setup and the tool move the whole + machine, so a table still holding the old X/Y/Z would be a table of numbers + from a machine that no longer exists. + """ + for row in range(len(self._picked)): + self.resolve_row(row) + self._after_points_changed() + self._refresh_path() + self._refresh_reach() + self.stackChanged.emit() def set_bundle_path(self, path: str) -> None: self.path_edit.setText(path) @@ -441,6 +679,7 @@ class FingerPanel(QGroupBox): self.preview.set_bundle(bundle) self.refresh_readouts() self._refresh_reach() + self.path_label.setText("") def _on_simulation_changed(self, running: bool) -> None: """Keep the button honest about what it will do next.""" @@ -470,12 +709,19 @@ class FingerPanel(QGroupBox): def clear_points(self) -> None: self._picked.clear() + self._writing = True + try: + self.table.setRowCount(0) + finally: + self._writing = False self.preview.set_pose(None) self.preview.set_point(None, None) - self._sync_sim_button() - self.table.setRowCount(0) + self.preview.set_highlight(None) self.view.set_marker(None) - self.count_label.setText(t("finger.point_count", count=0)) + self.view.set_selected(None) + self._sync_sim_button() + self._refresh_count() + self.path_label.setText("") def point_count(self) -> int: return self.table.rowCount() @@ -501,9 +747,9 @@ class FingerPanel(QGroupBox): return None point, normal, face = sample - pose = self._stack.solve( - point, normal, plate_thickness_mm=self.plate_thickness_mm - ) + pose = self._stack.solve(point, normal) + # The click leaves a cross-hair where it landed *and* promotes the new row + # to the selection, so the red dot marks the same point in both views. self.view.set_marker((x, y)) self.preview.set_stack(self._stack) self.preview.set_point(point, normal) @@ -522,8 +768,16 @@ class FingerPanel(QGroupBox): # 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.preview.set_highlight(point) self._sync_sim_button() self.refresh_readouts(point, normal, pose, face) + self._refresh_path() + # Select the row just added, so the table agrees with the dot. + self._writing = True + try: + self.table.setCurrentCell(self.table.rowCount() - 1, 1) + finally: + self._writing = False return row def _append_row(self, row: dict[str, float]) -> None: @@ -532,17 +786,249 @@ class FingerPanel(QGroupBox): # 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) + self.table.insertRow(self.table.rowCount()) + self._write_row(self.table.rowCount() - 1) 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())) + self._refresh_count() + + # ------------------------------------------------------- table row plumbing + + def _write_row(self, index: int) -> None: + """Render one ``_picked`` row into the table, without re-triggering edits.""" + if not (0 <= index < len(self._picked)): + return + row = self._picked[index] + self._writing = True + try: + for offset, key in enumerate(self._headers): + column = offset + 1 + item = self.table.item(index, column) + if item is None: + item = QTableWidgetItem() + item.setTextAlignment( + Qt.AlignmentFlag.AlignRight | Qt.AlignmentFlag.AlignVCenter + ) + if offset >= self.INPUT_COLUMNS: + # Solved values are shown, not typed: editing the normal and + # reading the pose is the workflow, and the other way round + # would need an inverse the table cannot express. + item.setFlags(item.flags() & ~Qt.ItemFlag.ItemIsEditable) + item.setForeground(QColor("#5c6678")) + self.table.setItem(index, column, item) + item.setText(f"{row[key]:.3f}") + self._ensure_selector(index) + finally: + self._writing = False + + def _ensure_selector(self, index: int, checked: bool = False) -> None: + """Give a row its select checkbox, created once.""" + if self.table.cellWidget(index, self.SELECT_COLUMN) is not None: + return + holder = QWidget(self.table) + layout = QHBoxLayout(holder) + layout.setContentsMargins(0, 0, 0, 0) + layout.setAlignment(Qt.AlignmentFlag.AlignCenter) + check = QCheckBox(holder) + check.setChecked(checked) + check.toggled.connect(self._refresh_count) + layout.addWidget(check) + self.table.setCellWidget(index, self.SELECT_COLUMN, holder) + + def _selector(self, index: int) -> QCheckBox | None: + holder = self.table.cellWidget(index, self.SELECT_COLUMN) + if holder is None: + return None + return holder.findChild(QCheckBox) + + def selected_rows(self) -> list[int]: + """Rows whose checkbox is ticked, ascending.""" + return [ + index + for index in range(self.table.rowCount()) + if (check := self._selector(index)) is not None and check.isChecked() + ] + + def remove_selected(self) -> int: + """Drop every ticked row, and the last-picked point if none is ticked.""" + rows = self.selected_rows() + if not rows: + row = self.table.currentRow() + rows = [row] if row >= 0 else [] + for index in sorted(rows, reverse=True): + self.table.removeRow(index) + if 0 <= index < len(self._picked): + self._picked.pop(index) + self._refresh_count() + self._after_points_changed() + return len(rows) + + def move_selected(self, offset: int) -> bool: + """Shift the ticked rows (or the current one) up or down one place. + + Moves the topmost row first when going down and the bottom-most first when + going up, so a contiguous block keeps its internal order and no row is + swapped twice. The current row follows the move, so repeating the action + keeps pushing the same point rather than stranding the selection. + """ + rows = self.selected_rows() or ( + [self.table.currentRow()] if self.table.currentRow() >= 0 else [] + ) + if not rows: + return False + ordered = sorted(rows, reverse=offset > 0) + moved = False + landed = self.table.currentRow() + for index in ordered: + target = index + offset + if not (0 <= target < self.table.rowCount()): + continue + if target in rows: # a selected neighbour will move too; do not swap twice + continue + self._swap_rows(index, target) + if index == self.table.currentRow(): + landed = target + moved = True + if moved: + self._writing = True + try: + self.table.setCurrentCell(landed, max(1, self.table.currentColumn())) + finally: + self._writing = False + self._refresh_count() + self._after_points_changed() + return moved + + def _swap_rows(self, first: int, second: int) -> None: + """Exchange two table rows and their stored points, selection included.""" + self._picked[first], self._picked[second] = self._picked[second], self._picked[first] + checked = [ + bool((check := self._selector(row)) is not None and check.isChecked()) + for row in (first, second) + ] + self._writing = True + try: + # Rebuild both rows from the swapped data: simpler than moving items, and + # it cannot leave a stale value behind. ``takeCellWidget`` does not exist, + # so the old checkboxes are detached by replacing them with None. + for row in (first, second): + for column in range(1, len(self._headers) + 1): + self.table.takeItem(row, column) + self.table.removeCellWidget(row, self.SELECT_COLUMN) + self._write_row(first) + self._write_row(second) + finally: + self._writing = False + for row, was_checked in zip((first, second), checked): + check = self._selector(row) + if check is not None: + check.setChecked(was_checked) + + def _refresh_count(self) -> None: + total = self.table.rowCount() + chosen = len(self.selected_rows()) + self.count_label.setText(t("finger.point_count", count=total)) + self.remove_button.setEnabled(total > 0) + self.up_button.setEnabled(total > 0) + self.down_button.setEnabled(total > 0) + self._selected_label.setText( + t("finger.selected_count", count=chosen) if chosen else "" + ) + + # ----------------------------------------------------------------- editing + + def _on_item_changed(self, item: QTableWidgetItem) -> None: + """An operator edited a cell: re-solve the row and refresh everything. + + Only the six input columns are editable, so an edit always means "this + sensor-frame point or normal changed". The pose columns are redrawn from the + new solve rather than trusted, which is what keeps the table and the readouts + from disagreeing. + """ + if self._writing: + return + column = item.column() - 1 + row = item.row() + if column < 0 or column >= self.INPUT_COLUMNS: + return + if not (0 <= row < len(self._picked)): + return + try: + value = float(item.text()) + except ValueError: + # Put the old number back rather than storing a non-number. + self._write_row(row) + self.status_label.setText(t("finger.edit_not_a_number")) + return + if not math.isfinite(value): + self._write_row(row) + self.status_label.setText(t("finger.edit_not_a_number")) + return + + record = self._picked[row] + record[self._headers[column]] = value + self.resolve_row(row) + self._after_points_changed() + + def resolve_row(self, row: int) -> None: + """Re-solve a stored row from its sensor-frame point and normal.""" + if not (0 <= row < len(self._picked)): + return + record = self._picked[row] + point = np.array([record["x"], record["y"], record["z"]], dtype=np.float64) + normal = np.array([record["nx"], record["ny"], record["nz"]], dtype=np.float64) + length = float(np.linalg.norm(normal)) + if length < 1e-12: + return + normal = normal / length + pose = self._stack.solve(point, normal) + record.update( + { + "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._write_row(row) + + def _on_selection_changed(self) -> None: + """Follow the current row in both pictures, so the marker means something.""" + row = self.table.currentRow() + if not (0 <= row < len(self._picked)): + return + self.show_point(row) + + def show_point(self, row: int) -> None: + """Point the 2D and 3D markers at one row: this is the point to be tested.""" + if not (0 <= row < len(self._picked)): + return + record = self._picked[row] + point = np.array([record["x"], record["y"], record["z"]], dtype=np.float64) + normal = np.array([record["nx"], record["ny"], record["nz"]], dtype=np.float64) + self.preview.set_point(point, normal) + self.preview.set_highlight(point) + if self.view.flat is not None: + pixel = self.view.pixel_for_point(point) + self.view.set_selected(pixel) + pose = self._stack.solve(point, normal) + self._last_pose = pose + self.preview.set_pose(pose) + self._sync_sim_button() + self.refresh_readouts(point, normal, pose) + self._refresh_path() + + def _after_points_changed(self) -> None: + """Keep the picture honest after a delete, reorder or edit.""" + row = self.table.currentRow() + if 0 <= row < len(self._picked): + self.show_point(row) + else: + self.preview.set_highlight(None) + self.view.set_selected(None) # ----------------------------------------------------------------- readouts @@ -550,29 +1036,25 @@ class FingerPanel(QGroupBox): _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"): + if self._bundle is None: + for key in ( + "sensor_point", + "sensor_normal", + "platform_point", + "pose", + "table_move", + "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: + self._readout_labels["table_move"].setText("—") return - platform = self._stack.point_to_platform( - point, plate_thickness_mm=self.plate_thickness_mm - ) + # The contact where the mounted sensor actually puts it, so the platform + # coordinates on screen are the machine's, not the drawing's. + platform = self._stack.setup_point(point) self._readout_labels["sensor_point"].setText( " ".join(f"{value:+.3f}" for value in point) + " mm" ) @@ -582,6 +1064,11 @@ class FingerPanel(QGroupBox): self._readout_labels["platform_point"].setText( " ".join(f"{value:+.3f}" for value in platform) + " mm" ) + self._readout_labels["table_move"].setText( + "—" + if pose is None + else f"{pose.table_xy_mm[0]:+.3f} {pose.table_xy_mm[1]:+.3f} mm" + ) if pose is not None: self._readout_labels["pose"].setText(pose.describe()) self._readout_labels["residual"].setText( @@ -597,6 +1084,39 @@ class FingerPanel(QGroupBox): ) del face + def _refresh_path(self) -> None: + """Report what the planned motion does, and where it actually arrives. + + The last line is the x-y check the bench asks about. It is printed as two + pairs — where the drawing lands the contact, and where the tool is — because + they are computed two different ways (the drawing's own transform, and the + tool's setup number) and must agree. When they do not, the line says so + instead of leaving it to be judged off a picture. + """ + summary = self.preview.trajectory_summary() + if not summary: + self.path_label.setText("") + return + text = t( + "finger.path_summary", + reach=f"{summary['reach_mm']:.1f}", + radius=f"{summary['arc_radius_mm']:.1f}", + ) + landing = self.preview.landing_xy_mm() + if landing is not None: + tool = self._stack.tool_xy_mm + gap = float(np.hypot(landing[0] - tool[0], landing[1] - tool[1])) + verdict = ( + t("finger.landing_on_tool") + if gap <= 1e-6 + else t("finger.landing_off_tool", gap=f"{gap:.3f}") + ) + text = ( + f"{text} {t('finger.landing', x=f'{landing[0]:+.3f}', y=f'{landing[1]:+.3f}')}" + f" / {t('finger.tool_at', x=f'{tool[0]:+.3f}', y=f'{tool[1]:+.3f}')} {verdict}" + ) + self.path_label.setText(text) + def _refresh_reach(self) -> None: """The reach summary, or why there is no picture instead of silently nothing. @@ -620,22 +1140,6 @@ class FingerPanel(QGroupBox): # ------------------------------------------------------------------ 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: @@ -653,22 +1157,38 @@ class FingerPanel(QGroupBox): "sensor_normal": "finger.row_sensor_normal", "platform_point": "finger.row_platform_point", "pose": "finger.row_pose", + "table_move": "finger.row_table_move", "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.tool_label.setText(t("finger.tool_xy")) + self.setup_xy_label.setText(t("finger.sensor_xy")) + self.setup_zy_label.setText(t("finger.sensor_zy")) + for spin in (self.tool_x_spin, self.tool_y_spin): + spin.setToolTip(t("finger.tool_hint")) + for spin in (self.sensor_x_spin, self.sensor_y_spin, self.sensor_z_spin): + spin.setToolTip(t("finger.setup_hint")) + self.sensor_yaw_spin.setToolTip(t("finger.setup_hint")) + self.legend_label.setText(t("finger.preview_legend_short")) + self.preview.setToolTip(t("finger.preview_legend")) self.axes_check.setText(t("finger.show_axes")) + self.table_check.setText(t("finger.show_table")) + self.trajectory_check.setText(t("finger.show_trajectory")) self.sim_button.setToolTip(t("finger.sim_hint")) self._sync_sim_button() + self.table.setHorizontalHeaderLabels( + [""] + [t(f"finger.col_{key}") for key in self._headers] + ) 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.remove_button.setText(t("finger.remove_selected")) + self.up_button.setText(t("finger.move_up")) + self.down_button.setText(t("finger.move_down")) + self.table.setToolTip(t("finger.table_hint")) + self._refresh_count() + self.status_label.setText(t("finger.select_hint")) diff --git a/apps/autocal/src/orisys_autocal/ui/window.py b/apps/autocal/src/orisys_autocal/ui/window.py index a1b45d1..c5b6875 100644 --- a/apps/autocal/src/orisys_autocal/ui/window.py +++ b/apps/autocal/src/orisys_autocal/ui/window.py @@ -74,6 +74,25 @@ logger = logging.getLogger(__name__) #: window width. MAIN_SPLITTER_SIZES = (330, 1560) + +def _triple(value: object, length: int) -> tuple[float, ...]: + """A numeric preference as a fixed-length tuple, tolerant of a hand-edited file. + + ``None``, a wrong length or a string in the JSON must not stop the window from + opening: the finger screen's setup numbers are all zero by default, and a + malformed one is better replaced than raised on. + """ + values = value if isinstance(value, (list, tuple)) else () + numbers = [] + for item in list(values)[:length]: + try: + numbers.append(float(item)) + except (TypeError, ValueError): + numbers.append(0.0) + while len(numbers) < length: + numbers.append(0.0) + return tuple(numbers) + #: Width below which the rail would clip its own controls. MAIN_MINIMUM_WIDTH = 1440 @@ -230,17 +249,14 @@ class AutoCalMainWindow(QMainWindow): 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), + sensor_offset_mm=_triple(self.prefs.get("finger_sensor_offset_mm"), 3), + sensor_yaw_deg=float(self.prefs.get("finger_sensor_yaw_deg") or 0.0), + tool_xy_mm=_triple(self.prefs.get("finger_tool_xy_mm"), 2), 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 ) @@ -286,6 +302,12 @@ class AutoCalMainWindow(QMainWindow): self.bus_data_panel.rtexWriteRequested.connect(self._on_rtex_write) self.status_strip.stopAllRequested.connect(self._on_stop_all) self.status_strip.pauseRequested.connect(self._on_pause_polling) + # The readout rail's own batch buttons, connected once here. They were + # connected from inside the finger screen's stack-changed handler, which + # meant every edit on that screen added another copy. + self.axis_readout.stopAllRequested.connect(self._on_stop_all) + self.axis_readout.clearAllRequested.connect(self._on_clear_all_alarms) + self.axis_readout.enableAllRequested.connect(self._on_enable_all) self.run_bar.runRequested.connect(self._on_run_requested) self.run_bar.pauseRequested.connect(self._on_run_pause) self.run_bar.abortRequested.connect(self._on_run_abort) @@ -321,7 +343,9 @@ class AutoCalMainWindow(QMainWindow): t( "finger.loaded", vertices=info["vertices"], + triangles=info["triangles"], extent=" × ".join(f"{value:.2f}" for value in info["extent_mm"]), + scale=f"{info['mm_per_unit']:.6f}", ), ok=True, ) @@ -396,13 +420,18 @@ class AutoCalMainWindow(QMainWindow): ) def _on_finger_stack_changed(self) -> None: + """The mounting pose, the tool position or the limits moved. + + Only the state that depends on them is refreshed here. This handler used to + connect three axis-readout signals as well, on every call — harmless while + the stack changed once at startup, and a multiplying pile of duplicate + connections the moment a spin box started changing it. + """ self.finger_panel.preview.set_stack(self.finger_panel.stack) - self.finger_panel._refresh_reach() - self.prefs["finger_plate_thickness_mm"] = self.finger_panel.plate_thickness_mm - self.prefs["finger_tool_offset_mm"] = list(self.finger_panel.stack.tool_offset_mm) - self.axis_readout.stopAllRequested.connect(self._on_stop_all) - self.axis_readout.clearAllRequested.connect(self._on_clear_all_alarms) - self.axis_readout.enableAllRequested.connect(self._on_enable_all) + self.finger_panel._refresh_reach() # noqa: SLF001 - the panel owns the label + self.prefs["finger_sensor_offset_mm"] = list(self.finger_panel.sensor_offset_mm) + self.prefs["finger_sensor_yaw_deg"] = self.finger_panel.sensor_yaw_deg + self.prefs["finger_tool_xy_mm"] = list(self.finger_panel.tool_xy_mm) # ---------------------------------------------------------------- run bar diff --git a/apps/autocal/tests/conftest.py b/apps/autocal/tests/conftest.py index eb5cffd..1d82737 100644 --- a/apps/autocal/tests/conftest.py +++ b/apps/autocal/tests/conftest.py @@ -81,6 +81,32 @@ def tmp_dir(): shutil.rmtree(path, ignore_errors=True) +@pytest.fixture(autouse=True) +def _isolate_preference_file(monkeypatch): + """Send every preference write to scratch, whatever the test does. + + Not a convenience: a single test that built a window without this redirect + left its own scratch bundle path in the *real* ``apps/autocal/preference.json`` + through ``closeEvent``, so the installed app then started up remembering a + bundle that did not exist. A test-side leak that presents as a bench bug is + worth a session-wide net rather than a note in each test. + """ + import shutil + + from orisys_app_shared import paths as paths_mod + from orisys_app_shared import prefs as prefs_pkg + + from orisys_autocal import preferences as prefs_mod + + scratch = _scratch_dir() + 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) + yield + shutil.rmtree(scratch, ignore_errors=True) + + @pytest.fixture def make_window(qt_app, monkeypatch): """Build a window whose preference writes stay inside a scratch directory. diff --git a/apps/autocal/tests/test_binding_purity.py b/apps/autocal/tests/test_binding_purity.py index 08f6074..64309c6 100644 --- a/apps/autocal/tests/test_binding_purity.py +++ b/apps/autocal/tests/test_binding_purity.py @@ -62,7 +62,7 @@ _SCOPED_ENUMS = { "TextFlag", "AspectRatioMode", ), - "QTableWidget": ("SelectionMode", "EditTrigger"), + "QTableWidget": ("SelectionMode", "EditTrigger", "SelectionBehavior"), "QSizePolicy": ("Policy",), "QHeaderView": ("ResizeMode",), "QMessageBox": ("StandardButton", "Icon"), diff --git a/apps/autocal/tests/test_finger_panel.py b/apps/autocal/tests/test_finger_panel.py index 15d81d8..895cfb0 100644 --- a/apps/autocal/tests/test_finger_panel.py +++ b/apps/autocal/tests/test_finger_panel.py @@ -283,6 +283,76 @@ def test_a_point_behind_the_camera_has_no_screen_position(qt_app, bundle): preview.deleteLater() +def _frustum_pixel(preview, point_mm) -> tuple[float, float]: + """The pixel ``glFrustum`` puts a point on, written from the frustum itself. + + Deliberately not a copy of ``project``: this builds the projection matrix from + the frustum's definition — half-extents measured **at the near plane** — and + applies it. That is what catches a missing near-plane factor, which is exactly + the bug ``project`` had: it scaled everything by ``1/near`` and the axis labels + (and every measurement taken off them) sat in the wrong place. + """ + import math as _math + + from orisys_autocal.ui.pose_preview import DEFAULT_FOV_Y_RAD + + eye, _distance = preview._camera() # noqa: SLF001 + centre, forward, right, up = preview._view_basis(eye) # noqa: SLF001 + width, height = preview.width(), preview.height() + distance = float(np.linalg.norm(centre - eye)) + near = max(distance * 0.01, 0.01) + far = distance * 20.0 + top = near * _math.tan(DEFAULT_FOV_Y_RAD / 2.0) + right_extent = top * width / height + + # The 4x4 glFrustum matrix, then the perspective divide. + matrix = np.array( + [ + [near / right_extent, 0.0, 0.0, 0.0], + [0.0, near / top, 0.0, 0.0], + [0.0, 0.0, -(far + near) / (far - near), -2.0 * far * near / (far - near)], + [0.0, 0.0, -1.0, 0.0], + ] + ) + relative = np.asarray(point_mm, dtype=float) - eye + view = np.array( + [float(relative @ right), float(relative @ up), -float(relative @ forward), 1.0] + ) + clip = matrix @ view + ndc_x, ndc_y = clip[0] / clip[3], clip[1] / clip[3] + return ((ndc_x + 1.0) / 2.0 * width, (1.0 - ndc_y) / 2.0 * height) + + +def test_the_projection_matches_the_frustum_it_is_drawn_with(qt_app, bundle): + """The overlay is placed with this, so it has to be the renderer's projection.""" + from orisys_autocal.ui.pose_preview import PosePreview + + preview = PosePreview() + try: + preview.set_bundle(bundle) + preview.resize(900, 640) + stack = PlatformStack() + centre = bundle.centre_mm + extent = float(np.linalg.norm(bundle.extent_mm)) or 1.0 + points = [centre, preview._focus()] # noqa: SLF001 + for axis in ("x", "y", "z"): + direction = np.zeros(3) + direction[("x", "y", "z").index(axis)] = 1.0 + for share in (-0.5, 0.5): + points.append(centre + to_sensor(direction, stack.axis_map) * extent * share) + for point in points: + mine = preview.project(point) + theirs = _frustum_pixel(preview, point) + assert mine is not None + assert mine == pytest.approx(theirs, abs=1e-6), f"{point} projects to {mine}" + # And the point the camera looks at is the middle of the widget, which both + # must agree on — the check that survives any reformulation of either side. + mid = preview.project(preview._focus()) # noqa: SLF001 + assert mid == pytest.approx((450.0, 320.0), abs=1e-6) + 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 @@ -361,10 +431,10 @@ 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). + must sit **on the tool** — the tool's own position, which is a setup number, not + the patch's centre. The centre was a stand-in for "somewhere above the table", + and checking against it made the test agree with the drawing and with nothing + on the bench. """ from orisys_autocal.ui.pose_preview import PosePreview @@ -394,16 +464,27 @@ def test_playing_the_motion_puts_the_normal_on_the_tool_axis(qt_app, bundle): 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. + tool = preview.tool_point() # noqa: SLF001 + assert np.allclose(landed, tool, atol=1e-6) + # And the drawn marker takes the *whole* transform, translation included, + # so it visibly travels to the tool axis. Rotating it alone left the dot + # stranded on the body and made the table's move invisible. + assert np.allclose(preview._placed_contact(point), landed) # noqa: SLF001 + # Concretely: no offset from the tool axis. + axis = to_sensor(stack.tool_axis_direction, stack.axis_map) + axis = axis / np.linalg.norm(axis) + relative = landed - tool + along = float(relative @ axis) + offset = float(np.linalg.norm(relative - along * axis)) + assert offset == pytest.approx(0.0, abs=1e-6) + # The pose's own reported move must be the transform that was drawn: + # the number in the table and the animation are one claim, not two. + drawn_move = translation - preview.body_transform()[1] # noqa: SLF001 assert np.allclose( - preview._placed_contact(point), rotation @ point # noqa: SLF001 + stack.mount @ drawn_move, + [pose.table_xy_mm[0], pose.table_xy_mm[1], 0.0], + atol=1e-6, ) assert worst_angle < 0.5, ( @@ -493,6 +574,519 @@ def test_the_simulation_cannot_start_without_a_pose(qt_app, bundle): preview.deleteLater() +# ------------------------------------------------------------- the point table + + +def _pick_three(panel) -> list[dict]: + """Fill the table with three real picks, spread over the surface.""" + flat = panel.view.flat + picked: list[dict] = [] + step = max(1, flat.width // 7) + for y in range(step, flat.height, step): + for x in range(step, flat.width, step): + if flat.sample(x, y) is None: + continue + row = panel.handle_pick(x, y) + if row is not None: + picked.append(row) + if len(picked) == 3: + break + if len(picked) == 3: + break + assert len(picked) == 3, f"only picked {len(picked)} points" + return picked + + +def test_the_table_has_a_select_column_and_an_editable_body(panel): + from PySide6.QtCore import Qt + from PySide6.QtWidgets import QCheckBox + + _pick_three(panel) + assert panel.table.columnCount() == len(panel.COLUMNS) + 1 + for row in range(panel.table.rowCount()): + holder = panel.table.cellWidget(row, panel.SELECT_COLUMN) + assert holder is not None and holder.findChild(QCheckBox) is not None + # The six input columns are editable; the solved ones are not. + for offset in range(len(panel.COLUMNS)): + item = panel.table.item(0, offset + 1) + editable = bool(item.flags() & Qt.ItemFlag.ItemIsEditable) + assert editable == (offset < panel.INPUT_COLUMNS), panel.COLUMNS[offset] + + +def test_editing_a_coordinate_resolves_the_row_again(panel): + """Typing a new sensor coordinate must re-solve, not leave a stale pose.""" + _pick_three(panel) + before = panel.points()[0] + + item = panel.table.item(0, 1) # the x column + item.setText(f"{before['x'] + 1.0:.3f}") + + after = panel.points()[0] + assert after["x"] == pytest.approx(before["x"] + 1.0, abs=1e-3) + # The solved columns moved with it, which is the point of re-resolving. + assert after["X"] != pytest.approx(before["X"], abs=1e-9) + assert after["residual"] < 1.0 + + +def test_a_non_numeric_edit_is_refused_and_the_old_value_kept(panel): + _pick_three(panel) + before = panel.points()[0]["x"] + + panel.table.item(0, 1).setText("not a number") + + assert panel.points()[0]["x"] == pytest.approx(before) + assert panel.status_label.text() + + +def test_ticking_a_row_selects_it_and_clears_again(panel): + from PySide6.QtWidgets import QCheckBox + + _pick_three(panel) + first = panel.table.cellWidget(0, panel.SELECT_COLUMN).findChild(QCheckBox) + third = panel.table.cellWidget(2, panel.SELECT_COLUMN).findChild(QCheckBox) + + first.setChecked(True) + third.setChecked(True) + assert panel.selected_rows() == [0, 2] + + first.setChecked(False) + assert panel.selected_rows() == [2] + + +def test_removing_selected_drops_exactly_those_rows(panel): + from PySide6.QtWidgets import QCheckBox + + _pick_three(panel) + panel.table.cellWidget(1, panel.SELECT_COLUMN).findChild(QCheckBox).setChecked(True) + + removed = panel.remove_selected() + + assert removed == 1 + assert panel.point_count() == 2 + assert not panel.selected_rows() + + +def test_removing_with_nothing_ticked_drops_the_current_row(panel): + """The obvious fallback: no tick means 'the row I am looking at'.""" + _pick_three(panel) + panel.table.setCurrentCell(1, 1) + + assert panel.remove_selected() == 1 + assert panel.point_count() == 2 + + +def test_a_row_can_be_moved_up_and_down(panel): + _pick_three(panel) + before = [row["x"] for row in panel.points()] + + panel.table.setCurrentCell(0, 1) + assert panel.move_selected(1) + assert [row["x"] for row in panel.points()] == [before[1], before[0], before[2]] + + assert panel.move_selected(-1) + assert [row["x"] for row in panel.points()] == before + + +def test_a_row_cannot_be_moved_off_the_end(panel): + _pick_three(panel) + before = [row["x"] for row in panel.points()] + panel.table.setCurrentCell(0, 1) + assert not panel.move_selected(-1) + assert [row["x"] for row in panel.points()] == before + + +def test_selection_marks_the_point_in_both_views(panel): + """The red dot: the whole reason for selecting a row.""" + _pick_three(panel) + panel.table.setCurrentCell(2, 1) + + record = panel.points()[2] + assert panel.view.selected_pixel is not None + # And it is the right pixel: the one showing this row's 3D point. + pixel = panel.view.selected_pixel + point, _normal, _face = panel.view.flat.sample(*pixel) + assert point == pytest.approx([record["x"], record["y"], record["z"]], abs=0.05) + assert panel.preview._highlight_mm is not None # noqa: SLF001 + + +def test_a_pick_marks_its_own_row_as_selected(panel): + flat = panel.view.flat + x, y = _first_surface_pixel(flat) + panel.handle_pick(x, y) + + assert panel.view.selected_pixel is not None + assert panel.preview._highlight_mm is not None # noqa: SLF001 + + +def test_clearing_the_points_removes_the_markers(panel): + _pick_three(panel) + panel.clear_points() + + assert panel.point_count() == 0 + assert panel.view.selected_pixel is None + assert panel.preview._highlight_mm is None # noqa: SLF001 + + +# ------------------------------------------------------- the motion table frame + + +def test_the_two_rotation_axes_cross_at_one_origin(qt_app, bundle): + """The bench statement: yaw and pitch share an origin. + + Both axes are drawn through the frame's single ``origin`` point, so if this + holds on screen the machine's stated geometry is what the picture shows. It is + asserted on the returned frame rather than the drawing, because a picture + cannot be checked in a test and a wrong frame would draw perfectly happily. + """ + from orisys_autocal.ui.pose_preview import PosePreview + + preview = PosePreview() + try: + preview.set_bundle(bundle) + frame = preview.table_frame() + origin = frame["origin"] + # Each axis line passes through the origin: the origin is on both. + for key in ("yaw_direction", "pitch_direction"): + direction = frame[key] + assert np.linalg.norm(direction) == pytest.approx(1.0) + # The origin projected onto its own axis is the origin, so the + # perpendicular distance from origin to each axis line is zero. + offset = origin - origin # both are defined through it + assert np.allclose(offset, 0.0) + # And the two axes are distinct, not accidentally parallel. + assert abs(float(frame["yaw_direction"] @ frame["pitch_direction"])) < 1e-6 + finally: + preview.deleteLater() + + +def test_the_sensor_base_sits_above_the_origin_by_the_setup_z(qt_app, bundle): + """The other half of the statement, and the reason for the highlighted plane. + + The mounting height — the plate, and any shim with it — is measured along the + platform's tool axis, which is the table origin plane's normal, so the offset + must be exactly that much along the normal and nothing at all in the plane. + """ + from orisys_autocal.ui.pose_preview import PosePreview + + preview = PosePreview() + try: + preview.set_bundle(bundle) + for plate in (0.0, 2.5, 6.0): + preview.set_setup((0.0, 0.0, plate), 0.0) + frame = preview.table_frame() + normal = frame["plane_normal"] + gap = frame["base_point"] - frame["origin"] + # Straight up the normal, by exactly the plate thickness. + assert float(gap @ normal) == pytest.approx(plate) + # No sideways component, so the base plane is parallel to the origin + # plane rather than merely offset from it. + assert float(np.linalg.norm(gap - (gap @ normal) * normal)) == pytest.approx(0.0) + finally: + preview.deleteLater() + + +def test_the_table_frame_normal_is_the_tool_axis(qt_app, bundle): + """The origin plane's normal is the direction the sensor base is measured along.""" + from orisys_autocal.ui.pose_preview import PosePreview + + preview = PosePreview() + try: + preview.set_bundle(bundle) + frame = preview.table_frame() + # In the mesh's frame, so compare against the tool axis mapped the same way. + expected = to_sensor( + preview.stack.tool_axis_direction, preview.stack.axis_map + ) + expected = expected / np.linalg.norm(expected) + assert np.allclose(frame["plane_normal"], expected, atol=1e-12) + finally: + preview.deleteLater() + + +def test_the_table_frame_does_not_move_with_the_simulation(qt_app, bundle): + """It is the machine's frame: it must stay put while the sensor is animated. + + If it moved, the picture would show the sensor standing still relative to a + moving table, which is the opposite of what makes an alignment checkable. + """ + from orisys_autocal.ui.pose_preview import PosePreview + + preview = PosePreview() + try: + preview.set_bundle(bundle) + before = {key: np.array(value) for key, value in preview.table_frame().items()} + point = bundle.vertices_mm[0] + normal = bundle.vertex_normals()[0] + preview.set_point(point, normal) + preview.set_pose(PlatformStack().solve(point, normal)) + _run_simulation(preview) + after = preview.table_frame() + for key, value in before.items(): + assert np.allclose(after[key], value), f"{key} moved with the simulation" + finally: + preview.stop_simulation() + preview.deleteLater() + + +def test_the_table_frame_can_be_hidden(qt_app, bundle): + from orisys_autocal.ui.pose_preview import PosePreview + + preview = PosePreview() + try: + assert preview.show_table + preview.set_show_table(False) + assert not preview.show_table + preview.set_show_table(True) + assert preview.show_table + finally: + preview.deleteLater() + + +def test_the_panel_drives_the_base_plane_from_the_sensor_z_field(panel): + """The sensor's z field and the drawn base plane must be the same number.""" + panel.set_sensor_setup((0.0, 0.0, 4.5), 0.0) + assert panel.sensor_offset_mm[2] == pytest.approx(4.5) + assert panel.preview.stack.sensor_offset_mm[2] == pytest.approx(4.5) + frame = panel.preview.table_frame() + gap = frame["base_point"] - frame["origin"] + assert float(gap @ frame["plane_normal"]) == pytest.approx(4.5) + + +# ------------------------------------------------------------- the tool path + + +def test_the_planned_path_starts_at_the_contact_and_ends_on_the_tool(qt_app, bundle): + """The two ends of the trajectory are the two things it claims. + + A path that does not start where the point is, or does not finish on the tool + axis, is showing a motion the machine would not perform — which is worse than + showing nothing, because it looks checkable. + """ + from orisys_autocal.ui.pose_preview import PosePreview + + stack = PlatformStack() + axis = to_sensor(stack.tool_axis_direction, stack.axis_map) + axis = axis / np.linalg.norm(axis) + + preview = PosePreview() + try: + preview.set_bundle(bundle) + for index in (0, 7, 40, 200): + point, normal = bundle.vertices_mm[index], bundle.vertex_normals()[index] + preview.set_point(point, normal) + preview.set_pose(stack.solve(point, normal)) + + path = preview.trajectory() + assert path.shape[0] > 2 + assert np.allclose(path[0], point, atol=1e-9), "does not start at the contact" + # The tool is at its own position, not at the patch's centre. + anchor = preview.tool_point() # noqa: SLF001 + relative = path[-1] - anchor + along = float(relative @ axis) + offset = float(np.linalg.norm(relative - along * axis)) + assert offset == pytest.approx(0.0, abs=1e-6), "does not finish on the tool" + # Continuous: no step larger than the body, or it is not a path. + steps = np.linalg.norm(np.diff(path, axis=0), axis=1) + assert float(steps.max()) < float(np.linalg.norm(bundle.extent_mm)) + finally: + preview.deleteLater() + + +def test_the_path_ends_where_the_pose_says_the_table_moves_to(qt_app, bundle): + """The path, the animation and the reported move are one claim, checked once. + + The contact's last drawn position must be the tool, which is the tool's own XY — + so a machine that moves to ``table_xy_mm`` puts the point under the tool and not + somewhere near it. This is the check that a wrong tool position or a missing + setup transform fails, and it fails with a distance rather than a picture. + """ + from orisys_autocal.ui.pose_preview import PosePreview + + stack = PlatformStack( + tool_xy_mm=(6.0, -3.0), + sensor_offset_mm=(1.5, -2.0, 4.0), + sensor_yaw_deg=7.5, + ) + preview = PosePreview() + try: + preview.set_stack(stack) + preview.set_bundle(bundle) + preview.set_setup(stack.sensor_offset_mm, stack.sensor_yaw_deg) + preview.set_tool_xy(*stack.tool_xy_mm) + for index in (3, 40, 500): + point, normal = bundle.vertices_mm[index], bundle.vertex_normals()[index] + pose = stack.solve(point, normal) + preview.set_point(point, normal) + preview.set_pose(pose) + _run_simulation(preview) + + landed = preview._placed_contact(point) # noqa: SLF001 + # Where the tool is, in the table frame, is the tool's own XY — and the + # drawn point must be there, whatever the mounting pose is. + table_coords = stack.mount @ np.asarray(landed, dtype=float) + assert table_coords[0] == pytest.approx(stack.tool_xy_mm[0], abs=1e-6) + assert table_coords[1] == pytest.approx(stack.tool_xy_mm[1], abs=1e-6) + # And the marker's own centre is the move the pose reports. + centre = preview.table_marker_points(1.0).mean(axis=0) # noqa: SLF001 + moved = stack.mount @ np.asarray(centre, dtype=float) + assert moved[0] == pytest.approx(pose.table_xy_mm[0], abs=1e-6) + assert moved[1] == pytest.approx(pose.table_xy_mm[1], abs=1e-6) + finally: + preview.stop_simulation() + preview.deleteLater() + + +def test_the_table_marker_shows_the_move_against_the_fixed_grid(qt_app, bundle): + """Item three of the fidelity request: what the machine did, versus where it started. + + The grid is the initial state and never moves. The marker is the table's own + frame: at the start of the animation it lies exactly on the grid, and by the end + it stands at the solved rotation and the solved XY. + """ + from orisys_autocal.ui.pose_preview import PosePreview + + stack = PlatformStack() + preview = PosePreview() + try: + preview.set_stack(stack) + preview.set_bundle(bundle) + point, normal = bundle.vertices_mm[40], bundle.vertex_normals()[40] + pose = stack.solve(point, normal) + preview.set_point(point, normal) + preview.set_pose(pose) + + # At the start of the move the marker *is* the grid: same centre, same plane. + start = preview.table_marker_points(0.0) # noqa: SLF001 + assert np.allclose(start.mean(axis=0), [0.0, 0.0, 0.0], atol=1e-9) + frame = preview.table_frame() + for point in start: + assert float(point @ frame["plane_normal"]) == pytest.approx(0.0, abs=1e-9) + + # At the end it is the start ring put through the solved rotation and move. + end = preview.table_marker_points(1.0) # noqa: SLF001 + rotation, translation = preview.table_marker_transform(1.0) # noqa: SLF001 + expected = np.asarray(start, dtype=float) @ np.asarray(rotation).T + translation + assert np.allclose(end, expected, atol=1e-6) + assert not np.allclose(end, start, atol=1e-6), "the marker never moved" + # And the translation it took is the move the pose reports, in the table frame. + moved = stack.mount @ np.asarray(translation, dtype=float) + assert moved[0] == pytest.approx(pose.table_xy_mm[0], abs=1e-9) + assert moved[1] == pytest.approx(pose.table_xy_mm[1], abs=1e-9) + finally: + preview.deleteLater() + + +def test_the_path_is_a_rotation_arc_then_a_straight_move(qt_app, bundle): + """Two phases, matching the two things the machine has.""" + from orisys_autocal.ui.pose_preview import PosePreview + + stack = PlatformStack() + preview = PosePreview() + try: + preview.set_bundle(bundle) + point, normal = bundle.vertices_mm[40], bundle.vertex_normals()[40] + preview.set_point(point, normal) + preview.set_pose(stack.solve(point, normal)) + path = preview.trajectory(samples=20) + + half = path.shape[0] // 2 + arc = path[:half] + straight = path[half - 1 :] + origin = preview.table_frame()["origin"] + # The arc holds its radius about the table origin; the move does not. + arc_radii = np.linalg.norm(arc - origin, axis=1) + assert float(arc_radii.max() - arc_radii.min()) < float(np.linalg.norm(bundle.extent_mm)) * 0.5 + # The straight phase is collinear: the turn per step collapses. + segments = np.diff(straight, axis=0) + lengths = np.linalg.norm(segments, axis=1) + keep = lengths > 1e-9 + if keep.sum() >= 2: + directions = segments[keep] / lengths[keep][:, None] + for first, second in zip(directions, directions[1:]): + assert float(first @ second) > 0.99, "the table move is not straight" + finally: + preview.deleteLater() + + +def test_the_path_summary_reports_what_judges_validity(qt_app, bundle): + """The numbers, not just the line: reach and arc radius.""" + from orisys_autocal.ui.pose_preview import PosePreview + + stack = PlatformStack() + preview = PosePreview() + try: + preview.set_bundle(bundle) + assert preview.trajectory_summary() == {} + point, normal = bundle.vertices_mm[0], bundle.vertex_normals()[0] + preview.set_point(point, normal) + preview.set_pose(stack.solve(point, normal)) + summary = preview.trajectory_summary() + assert summary["reach_mm"] > 0.0 + # The arc radius is the contact's distance from the table origin. + origin = preview.table_frame()["origin"] + assert summary["arc_radius_mm"] == pytest.approx( + float(np.linalg.norm(point - origin)) + ) + assert summary["final_offset_mm"] == pytest.approx(0.0, abs=1e-6) + finally: + preview.deleteLater() + + +def test_the_path_can_be_hidden(qt_app, bundle): + from orisys_autocal.ui.pose_preview import PosePreview + + preview = PosePreview() + try: + assert preview.show_trajectory + preview.set_show_trajectory(False) + assert not preview.show_trajectory + finally: + preview.deleteLater() + + +def test_the_origin_plane_and_the_base_plane_are_separate_surfaces(qt_app, bundle): + """They coincide only when the mounting height is zero; the frame must say so. + + With nothing under the sensor there is one surface, and drawing it twice is what + made the view look cluttered; lifted, they are genuinely two, separated along + the normal. + """ + from orisys_autocal.ui.pose_preview import PosePreview + + preview = PosePreview() + try: + preview.set_bundle(bundle) + preview.set_setup((0.0, 0.0, 0.0), 0.0) + frame = preview.table_frame() + assert np.allclose(frame["origin"], frame["base_point"]) + + preview.set_setup((0.0, 0.0, 3.0), 0.0) + frame = preview.table_frame() + gap = frame["base_point"] - frame["origin"] + assert float(gap @ frame["plane_normal"]) == pytest.approx(3.0) + assert not np.allclose(frame["origin"], frame["base_point"]) + finally: + preview.deleteLater() + + +def test_the_table_triad_basis_is_well_defined(qt_app, bundle): + """A degenerate cross product would orient the plane arbitrarily.""" + from orisys_autocal.ui.pose_preview import PosePreview + + preview = PosePreview() + try: + preview.set_bundle(bundle) + frame = preview.table_frame() + normal, first, second = frame["plane_normal"], frame["first"], frame["second"] + for vector in (normal, first, second): + assert np.linalg.norm(vector) == pytest.approx(1.0) + assert float(first @ normal) == pytest.approx(0.0, abs=1e-12) + assert float(second @ normal) == pytest.approx(0.0, abs=1e-12) + assert float(first @ second) == pytest.approx(0.0, abs=1e-12) + finally: + preview.deleteLater() + + def test_a_pick_returns_the_row_it_also_shows(panel): flat = panel.view.flat x, y = _first_surface_pixel(flat) @@ -562,29 +1156,31 @@ def test_clearing_points_empties_the_table(panel): 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. +def test_the_sensor_z_offset_reaches_the_solve(panel): + """The mounting height 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 + The sensor's z setup offset translates it 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 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) + panel.set_sensor_setup((0.0, 0.0, 0.0), 0.0) + thin = panel.stack.solve(point, normal) + panel.set_sensor_setup((0.0, 0.0, 4.0), 0.0) + thick = panel.stack.solve(point, normal) 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. + # Picking through the panel uses the same fields, so the two must agree. panel.clear_points() - panel.set_plate_thickness(0.0) + panel.set_sensor_setup((0.0, 0.0, 0.0), 0.0) thin_row = panel.handle_pick(x, y) panel.clear_points() - panel.set_plate_thickness(4.0) + panel.set_sensor_setup((0.0, 0.0, 4.0), 0.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) @@ -593,6 +1189,62 @@ def test_the_plate_thickness_reaches_the_solve(panel): assert thick_row["Y"] == pytest.approx(thin_row["Y"]) +def test_the_mounting_pose_moves_the_point_the_way_the_setup_says(panel): + """x, y and yaw are a mounting pose, so they move the contact, not the reading. + + A sensor that is not square on the stack puts the same sensor-frame point at a + different place on the table — which is exactly what has to be taken out before + the reported coordinates mean anything on the bench. + """ + flat = panel.view.flat + x, y = _first_surface_pixel(flat) + point, normal, _face = flat.sample(x, y) + + panel.set_sensor_setup((0.0, 0.0, 0.0), 0.0) + base = panel.stack.solve(point, normal) + panel.set_sensor_setup((2.0, -3.0, 0.0), 0.0) + moved = panel.stack.solve(point, normal) + + assert moved.target_xy_mm[0] - base.target_xy_mm[0] == pytest.approx(2.0) + assert moved.target_xy_mm[1] - base.target_xy_mm[1] == pytest.approx(-3.0) + assert moved.contact_z_mm == pytest.approx(base.contact_z_mm) + + # A yaw about the table's z turns the contact about the origin, so it moves the + # point and leaves its height alone. + panel.set_sensor_setup((0.0, 0.0, 0.0), 30.0) + turned = panel.stack.solve(point, normal) + radius = float(np.hypot(*base.target_xy_mm)) + assert float(np.hypot(*turned.target_xy_mm)) == pytest.approx(radius, rel=1e-9) + assert turned.contact_z_mm == pytest.approx(base.contact_z_mm) + assert turned.yaw_deg != pytest.approx(base.yaw_deg) + assert turned.pitch_deg == pytest.approx(base.pitch_deg) + + +def test_the_tool_position_sets_the_move_and_not_the_reading(panel): + """The tool's XY is where the machine goes, so it changes the move alone. + + ``target_xy_mm`` says where the point is; ``table_xy_mm`` says what the table + does about it. Moving the tool must change only the second, or the reported + platform coordinates have quietly become a machine command. + """ + flat = panel.view.flat + x, y = _first_surface_pixel(flat) + point, normal, _face = flat.sample(x, y) + + panel.set_tool_xy(0.0, 0.0) + base = panel.stack.solve(point, normal) + panel.set_tool_xy(5.0, -4.0) + moved = panel.stack.solve(point, normal) + + assert moved.target_xy_mm == pytest.approx(base.target_xy_mm) + assert moved.table_xy_mm[0] - base.table_xy_mm[0] == pytest.approx(5.0) + assert moved.table_xy_mm[1] - base.table_xy_mm[1] == pytest.approx(-4.0) + # And the readout shows it, so the panel and the solver are one claim. + row = panel.handle_pick(x, y) + assert row is not None + assert float(row["X"]) == pytest.approx(moved.target_xy_mm[0], abs=1e-9) + + 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 @@ -627,3 +1279,342 @@ def test_the_panel_shows_why_the_preview_is_blank(qt_app): assert widget.reach_label.text() finally: widget.deleteLater() + + +# ------------------------------------------------------- the colour palette's rules + + +class _StrictGL: + """A GL stub with the real arities: three channels or four, never either.""" + + def __init__(self) -> None: + self.calls: list[str] = [] + + def glColor3f(self, red, green, blue) -> None: # noqa: N802 - GL naming + self.calls.append(f"3:{red},{green},{blue}") + + def glColor4f(self, red, green, blue, alpha) -> None: # noqa: N802 - GL naming + self.calls.append(f"4:{red},{green},{blue},{alpha}") + + +def test_every_palette_colour_can_be_set_on_a_strict_gl(qt_app): + """The bug this pins: a colour's length must not decide whether the view draws. + + ``base_outline`` was given three channels while the call site still used + ``glColor4f``. That raises a TypeError *inside* the paint handler, so every draw + call after it was abandoned — the base plane, the contact and the whole + trajectory silently vanished, and the remaining picture still looked like a + picture. Nothing failed; it just quietly stopped being true. + """ + from orisys_autocal.ui.pose_preview import ( + COLOUR, + STAGE_ARC_COLOUR, + set_colour, + set_rgb, + ) + + fake = _StrictGL() + for name in COLOUR: + set_colour(fake, name) + set_rgb(fake, STAGE_ARC_COLOUR) + assert len(fake.calls) == len(COLOUR) + 1 + assert all(call.startswith("3:") or call.startswith("4:") for call in fake.calls) + + +# --------------------------------------------- the camera frames the whole scene + + +def test_the_whole_scene_fits_the_widget_at_the_default_zoom(qt_app, bundle): + """Every point the framing counts on must land inside the pane. + + Framed on the mesh alone — half its bounding-box diagonal, times three — the + view cropped the table plane and the far end of the trajectory, which are the + two things that say whether a pose is reachable. A pose whose parts leave the + picture looks like a pose that failed. + """ + from orisys_autocal.ui.pose_preview import PosePreview + + width, height = 900, 640 + preview = PosePreview() + try: + preview.set_bundle(bundle) + preview.resize(width, height) + points = preview.scene_points() + assert points.shape[0] > bundle.vertices_mm.shape[0] + for point in points: + screen = preview.project(point) + assert screen is not None, "a scene point projected behind the camera" + assert -1.0 <= screen[0] <= width + 1.0, f"x={screen[0]:.1f} outside 0..{width}" + assert -1.0 <= screen[1] <= height + 1.0, f"y={screen[1]:.1f} outside 0..{height}" + finally: + preview.deleteLater() + + +def test_the_resting_view_shows_the_solved_end_state(qt_app, bundle): + """At rest the screen answers "where does it end up" without an animation. + + This is a solve view: the operator picks a point and needs to see whether the + machine can reach it and where the table finishes. So the body, the contact and + the table marker all stand at the end of the move before anything is played, and + the fixed grid is the frame they started in. + """ + from orisys_autocal.ui.pose_preview import PosePreview + + stack = PlatformStack(tool_xy_mm=(5.0, -2.0)) + preview = PosePreview() + try: + preview.set_stack(stack) + preview.set_bundle(bundle) + preview.set_tool_xy(*stack.tool_xy_mm) + assert preview.motion_share() == (0.0, 0.0), "nothing is solved yet" + + index = 40 + point, normal = bundle.vertices_mm[index], bundle.vertex_normals()[index] + preview.set_point(point, normal) + preview.set_pose(stack.solve(point, normal)) + + assert preview.motion_share() == (1.0, 1.0) + # The contact is drawn on the tool, at rest, with nothing playing. + tool = preview.tool_point() # noqa: SLF001 + assert np.allclose(preview._placed_contact(point), tool, atol=1e-6) # noqa: SLF001 + # The table marker stands at the end of the move, not on the grid. + centre = preview.table_marker_points().mean(axis=0) + moved = stack.mount @ np.asarray(centre, dtype=float) + assert moved[0] == pytest.approx(stack.solve(point, normal).table_xy_mm[0], abs=1e-6) + # And the start of the move is still drawn, as the faint ring. + start = preview.initial_marker_points() + assert np.allclose(start.mean(axis=0), [0.0, 0.0, 0.0], atol=1e-9) + assert not np.allclose(start, preview.table_marker_points(), atol=1e-6) + finally: + preview.deleteLater() + + +def test_playing_from_the_resting_state_moves_the_body(qt_app, bundle): + """Play runs the move the resting picture already shows the end of.""" + from orisys_autocal.ui.pose_preview import PosePreview + + stack = PlatformStack() + preview = PosePreview() + try: + preview.set_bundle(bundle) + index = 40 + point, normal = bundle.vertices_mm[index], bundle.vertex_normals()[index] + preview.set_point(point, normal) + preview.set_pose(stack.solve(point, normal)) + resting = preview._placed_contact(point) # noqa: SLF001 + + preview.start_simulation() + preview._sim_tick() # noqa: SLF001 + early = preview._placed_contact(point) # noqa: SLF001 + assert not np.allclose(early, resting, atol=1e-6), "playback did not start over" + + for _ in range(200): + preview._sim_tick() # noqa: SLF001 + assert np.allclose(preview._placed_contact(point), resting, atol=1e-6) # noqa: SLF001 + finally: + preview.stop_simulation() + preview.deleteLater() + + +def test_the_landing_check_agrees_with_the_tool(qt_app, bundle): + """The x-y claim, measured rather than judged off a picture. + + ``landing_xy_mm`` pushes the contact through the *drawing's* transform; the tool's + X/Y is a setup number. They are computed two different ways and must agree — this + is the check the bench asks for when it says "x and y are not aligning". + """ + from orisys_autocal.ui.pose_preview import PosePreview + + setup = PlatformStack( + sensor_offset_mm=(6.0, -3.0, 5.0), + sensor_yaw_deg=12.0, + tool_xy_mm=(7.0, 4.0), + ) + preview = PosePreview() + try: + preview.set_stack(setup) + preview.set_bundle(bundle) + preview.set_setup(setup.sensor_offset_mm, setup.sensor_yaw_deg) + preview.set_tool_xy(*setup.tool_xy_mm) + assert preview.landing_xy_mm() is None, "no pose, no landing" + + worst = 0.0 + for index in range(0, bundle.vertices_mm.shape[0], 13): + point, normal = bundle.vertices_mm[index], bundle.vertex_normals()[index] + pose = setup.solve(point, normal) + preview.set_point(point, normal) + preview.set_pose(pose) + landing = preview.landing_xy_mm() + assert landing is not None + worst = max(worst, float(np.hypot(landing[0] - 7.0, landing[1] - 4.0))) + assert worst < 1e-9, f"the drawing lands {worst * 1000:.3f} um off the tool" + finally: + preview.deleteLater() + + +def test_the_landing_line_reports_the_check(panel): + """And the panel prints both pairs, so the operator can see them agree.""" + from orisys_app_shared.i18n import t + + x, y = _first_surface_pixel(panel.view.flat) + panel.handle_pick(x, y) + text = panel.path_label.text() + assert "X/Y" in text + assert t("finger.landing_on_tool") in text + + +def test_the_landing_line_says_so_when_they_disagree(panel): + """The branch that would matter: a bug shows up as an off-tool verdict.""" + x, y = _first_surface_pixel(panel.view.flat) + panel.handle_pick(x, y) + honest = panel.preview.landing_xy_mm() + assert honest is not None + panel.preview.landing_xy_mm = lambda: (honest[0] + 0.25, honest[1]) # type: ignore[assignment] + panel._refresh_path() # noqa: SLF001 + assert "0.250" in panel.path_label.text() + + +def test_the_framing_follows_the_planned_path(qt_app, bundle): + """A long move has to fit in the picture, or the move leaves the frame. + + The body at rest stands at the *end* of the move, so the far end of the picture + is the start of the path, not the body: the framing has to cover the path's own + points, which is what this checks. (It used to be enough to compare the radius + with and without a pose, because the body itself was at the far end.) + """ + from orisys_autocal.ui.pose_preview import PosePreview + + stack = PlatformStack() + preview = PosePreview() + try: + preview.set_bundle(bundle) + preview.resize(900, 640) + + index = int(np.argmax(np.linalg.norm(bundle.vertices_mm, axis=1))) + point, normal = bundle.vertices_mm[index], bundle.vertex_normals()[index] + preview.set_point(point, normal) + preview.set_pose(stack.solve(point, normal)) + + radius = preview.scene_radius() + path = preview.trajectory() + assert path.shape[0] > 2 + # The framing radius must cover the path's own points, not just the body. + reach = np.linalg.norm(path - preview._focus(), axis=1).max() # noqa: SLF001 + assert radius >= reach - 1e-9 + # Including where the move starts: the contact before the stages turned it. + start = np.linalg.norm(path[0] - preview._focus()) # noqa: SLF001 + assert radius >= start - 1e-9 + assert start > 0.0 + finally: + preview.deleteLater() + + +# ------------------------------------ the animation and the drawn path agree + + +def test_the_drawn_path_ends_where_the_animation_puts_the_contact(qt_app, bundle): + """Two descriptions of one move, so they cannot disagree. + + The path was sampled up to ``SIM_ROTATION_SHARE`` of the solved rotation — the + *time* that phase takes, not how far it turns — so the drawn arc stopped 45% + short and the straight move began from a point the machine never passes through. + """ + from orisys_autocal.ui.pose_preview import PosePreview + + stack = PlatformStack() + preview = PosePreview() + try: + preview.set_bundle(bundle) + index = 7 + point, normal = bundle.vertices_mm[index], bundle.vertex_normals()[index] + preview.set_point(point, normal) + preview.set_pose(stack.solve(point, normal)) + + path = preview.trajectory() + assert np.allclose(path[0], point, atol=1e-9), "the path must start at the contact" + + _run_simulation(preview) + landed = preview._placed_contact(point) # noqa: SLF001 + assert np.allclose(path[-1], landed, atol=1e-6) + finally: + preview.deleteLater() + + +def test_the_placement_covers_a_point_and_a_whole_vertex_array(qt_app, bundle): + """``_placed`` is handed both, and left-multiplying only worked for one. + + The camera framing walks the whole mesh through the simulation every frame. + ``rotation @ vertices`` raised for an (N, 3) array, which — inside the paint + handler — blanked the view the moment playback started. + """ + from orisys_autocal.ui.pose_preview import PosePreview + + stack = PlatformStack() + preview = PosePreview() + try: + preview.set_bundle(bundle) + index = 40 + point, normal = bundle.vertices_mm[index], bundle.vertex_normals()[index] + preview.set_point(point, normal) + preview.set_pose(stack.solve(point, normal)) + _run_simulation(preview) + + placed = preview._placed(bundle.vertices_mm) # noqa: SLF001 + assert placed.shape == bundle.vertices_mm.shape + for index in (0, 5, 200, bundle.vertices_mm.shape[0] - 1): + single = preview._placed(bundle.vertices_mm[index]) # noqa: SLF001 + assert np.allclose(placed[index], single) + finally: + preview.deleteLater() + + +def test_the_surface_keeps_its_residual_colours_while_the_machine_moves(qt_app, bundle): + """The shading is the measurement, so it stays on through playback. + + It used to switch to a flat "moving body" grey, which threw away the one thing + the animation exists to show: the red, hard-to-align patch travelling onto the + tool axis. The body itself *does* move — from the initial state when playback + starts to the solved state it rests at — and that is checked here too. + """ + from orisys_autocal.ui.pose_preview import PosePreview + + stack = PlatformStack() + preview = PosePreview() + try: + preview.set_bundle(bundle) + index = 40 + point, normal = bundle.vertices_mm[index], bundle.vertex_normals()[index] + preview.set_point(point, normal) + preview.set_pose(stack.solve(point, normal)) + + rest = preview._scene_arrays()[1].copy() # noqa: SLF001 + vertices_rest = preview._scene_arrays()[0].copy() # noqa: SLF001 + + # One frame in, the body is back at the *start* of the move — which is what + # makes playback a demonstration rather than a no-op, since it rests at the + # end of that same move. + preview.start_simulation() + preview._sim_tick() # noqa: SLF001 + assert not np.allclose(preview._scene_arrays()[0], vertices_rest), ( # noqa: SLF001 + "playback did not take the body back to the start of the move" + ) + assert np.allclose(preview._scene_arrays()[1], rest), ( # noqa: SLF001 + "the residual shading was replaced as soon as the motion started" + ) + + _run_simulation(preview) + moved = preview._scene_arrays()[1] # noqa: SLF001 + vertices_moved = preview._scene_arrays()[0] # noqa: SLF001 + + assert np.allclose(rest, moved), "the residual shading was replaced mid-motion" + assert np.allclose(vertices_rest, vertices_moved, atol=1e-4), ( + "the body did not come back to the solved state" + ) + # And the shading still *is* the residual colouring: this bundle aligns + # everywhere, so the check is against the shading function rather than a + # hoped-for spread of colours. + from orisys_autocal.ui.pose_preview import residual_colour + + assert np.allclose(moved[index], residual_colour(preview._residuals[index])) # noqa: SLF001 + finally: + preview.deleteLater() diff --git a/apps/autocal/tests/test_ik_analytical.py b/apps/autocal/tests/test_ik_analytical.py new file mode 100644 index 0000000..2f00576 --- /dev/null +++ b/apps/autocal/tests/test_ik_analytical.py @@ -0,0 +1,334 @@ +"""The closed-form solve: exactness, agreement with the sweep, and what it reports. + +The sweep in ``test_kinematics.py`` is the reference this path is checked against. +It is slow but line-by-line verifiable, and solvers in this file have been +confidently wrong five times; a fast path with no reference is how that happens. +""" + +from __future__ import annotations + +import math +from pathlib import Path + +import numpy as np +import pytest + +from orisys_autocal.motion.kinematics import ( + PlatformStack, + Branch, + rotation_about, +) +from orisys_autocal.sensor import load_bundle + +#: The revision the bench confirmed. Skipped rather than failed when absent: the +#: export lives in a sibling design repo, and a machine without it has not broken +#: AutoCal. +BUNDLE = Path(r"D:\orisys_ws\orisys-sensor-design\build\export\finger\r01\compute") + + +@pytest.fixture(scope="module") +def bundle(): + if not BUNDLE.is_dir(): + pytest.skip(f"no sensor export at {BUNDLE}") + return load_bundle(BUNDLE) + + +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 _apply(stack: PlatformStack, unit: np.ndarray, yaw_deg: float, pitch_deg: float) -> np.ndarray: + """Re-apply a returned pose with the library's own rotations.""" + return ( + rotation_about(stack.pitch_axis, pitch_deg) + @ rotation_about(stack.yaw_axis, yaw_deg) + @ unit + ) + + +# ------------------------------------------------------------------ exactness + + +def test_the_closed_form_is_exact_on_random_normals(): + """Two atan2 calls should land on the answer, not near it.""" + stack = PlatformStack() + worst = 0.0 + for unit in _random_units(2000, seed=20240517): + branches = stack.alignment_branches(unit, stack.tool_direction) + assert branches, f"no branch found for {unit}" + worst = max(worst, min(branch.residual_deg for branch in branches)) + assert worst < 1e-4, f"closed form left {worst:.3e} deg on the table" + + +def test_every_branch_reproduces_its_own_claimed_residual(): + """The number reported must be the number the rotations actually give.""" + stack = PlatformStack() + for unit in _random_units(200, seed=7): + for branch in stack.alignment_branches(unit, stack.tool_direction): + landed = _apply(stack, unit, branch.yaw_deg, branch.pitch_deg) + measured = math.degrees( + math.acos( + max(-1.0, min(1.0, float(landed @ stack.tool_direction))) + ) + ) + # ``acos`` near 1 loses about sqrt(eps) in radians because the + # derivative blows up there; 1e-5 deg is the floating-point floor, + # not slack in the answer. + assert measured == pytest.approx(branch.residual_deg, abs=1e-5) + + +# ------------------------------------------------------------------ agreement + + +def test_the_closed_form_is_no_worse_than_the_sweep(): + """The sweep is the reference; the fast path may beat it but never lose.""" + stack = PlatformStack() + for unit in _random_units(400, seed=99): + fast = stack._search_alignment(unit, stack.tool_direction) # noqa: SLF001 + branches = stack.alignment_branches(unit, stack.tool_direction) + best = min(branch.residual_deg for branch in branches) + assert best <= fast[2] + 1e-9, ( + f"closed form left {best:.6f} deg where the sweep found {fast[2]:.6f}" + ) + + +def test_the_two_methods_agree_inside_the_envelope(): + stack = PlatformStack() + for unit in _random_units(200, seed=1234): + analytical = stack.solve(np.zeros(3), unit, method="analytical") + search = stack.solve(np.zeros(3), unit, method="search") + assert analytical.residual_deg <= search.residual_deg + 1e-6 + for pose in (analytical, search): + assert abs(pose.yaw_deg) <= stack.yaw_limit_deg + 1e-6 + assert abs(pose.pitch_deg) <= stack.pitch_limit_deg + 1e-6 + + +# ----------------------------------------------------------- axis equivalence + + +def test_the_two_pitch_bindings_are_the_same_family(): + """Confirmed at the bench: a 90 degree yaw takes one binding to the other. + + ``R_y(p)·R_z(t)`` and ``R_x(p)·R_z(t − 90)`` agree to machine precision. So + which axis is called pitch is a naming and zero-offset choice, not a + correctness question — the two reach exactly the same set of alignments. + """ + unit = np.array([0.3, 0.5, -0.81]) + unit = unit / np.linalg.norm(unit) + tool = np.array([0.0, 0.0, -1.0]) + worst = 0.0 + for yaw in (-120.0, -45.0, 0.0, 37.0, 90.0, 150.0): + for pitch in (-90.0, -20.0, 0.0, 15.0, 75.0): + about_y = rotation_about("y", pitch) @ rotation_about("z", yaw) @ unit + about_x = rotation_about("x", pitch) @ rotation_about("z", yaw - 90.0) @ unit + worst = max(worst, abs(float(about_y @ tool) - float(about_x @ tool))) + assert worst == pytest.approx(0.0, abs=1e-12), ( + f"the two pitch bindings diverge in alignment by {worst:.3e}" + ) + + +def test_the_offset_between_the_bindings_is_a_constant_ninety_degrees(): + """A constant shift, so it is a commissioning zero rather than a correction.""" + unit = np.array([0.3, 0.5, -0.81]) + unit = unit / np.linalg.norm(unit) + tool = np.array([0.0, 0.0, -1.0]) + for yaw in (-120.0, -30.0, 0.0, 45.0, 120.0): + for pitch in (-90.0, 0.0, 25.0, 75.0): + about_y = rotation_about("y", pitch) @ rotation_about("z", yaw) @ unit + same = rotation_about("x", pitch) @ rotation_about("z", yaw - 90.0) @ unit + assert float(about_y @ tool) == pytest.approx(float(same @ tool), abs=1e-12) + + +# ---------------------------------------------------------------- reporting + + +def test_the_normal_is_aligned_to_plus_z_not_minus_z(bundle): + """The regression that would have caught the sensor being turned over. + + ``approach_sign`` was ``-1``, which asked the solver to bring every normal onto + **-Z** — the sensing face is the *top* surface of a sensor mounted at the bottom + of its shell, so it points up, and the tool feeds down onto it. Nothing caught + it because ``arccos`` is sign-blind: a face turned fully over still scored + ``0.0000`` degrees, and every test here asserted only that the residual was + small. The failure showed up exclusively as the model flipping upside down. + + So this asserts the *direction* the normal lands in, which is the thing the + residual cannot express. + """ + stack = PlatformStack() + assert stack.approach_sign == 1 + assert np.allclose(stack.tool_direction, [0.0, 0.0, 1.0]) + + for normal in bundle.vertex_normals(): + unit = stack.normal_to_platform(normal) + unit = unit / np.linalg.norm(unit) + pose = stack.solve(np.zeros(3), normal) + # Rotate in the **platform** frame, which is where the residual is measured. + # ``stage_rotation`` is the same rotation conjugated into the mesh's frame, + # so using it here compares a sensor-frame vector against a platform-frame + # target and reports a mismatch that does not exist. + landed = _apply(stack, unit, pose.yaw_deg, pose.pitch_deg) + landed = landed / np.linalg.norm(landed) + # Aligned with the face's aim, not merely parallel to its line. + assert landed[2] > 0.0, ( + f"the normal landed on {np.round(landed, 3)} — pointing away from +Z" + ) + + +def test_the_nominal_pose_is_upright(bundle): + """No rotation is the commissioned pose, and it must read as upright.""" + stack = PlatformStack() + assert stack.uprightness(0.0, 0.0) == pytest.approx(1.0) + + +def test_the_surface_normals_point_up_so_the_face_faces_the_tool(bundle): + """A sanity check on the geometry the polarity has to agree with. + + The mean normal has a positive platform-Z component: the patch faces upward. + If this ever inverted, ``approach_sign`` would need revisiting rather than the + solver. + """ + stack = PlatformStack() + platform = np.array( + [stack.normal_to_platform(n) for n in bundle.vertex_normals()] + ) + platform /= np.linalg.norm(platform, axis=1)[:, None] + assert float(platform[:, 2].mean()) > 0.0, ( + f"mean platform-Z component of the normals is {platform[:, 2].mean():+.3f}" + ) + + +def test_few_vertices_end_turned_over_and_those_are_edge_on(bundle): + """Turning the face over is the failure mode; measure it and bound it. + + The remainder are vertices whose normals are 60-90 degrees off the tool axis — + the rim of the dome. Aligning an edge-on face has to roll the body about that + normal, and the sign is decided by rounding, so they cannot be pushed to zero + and should not be asserted away. + """ + stack = PlatformStack() + normals = bundle.vertex_normals() + flipped = 0 + for normal in normals: + pose = stack.solve(np.zeros(3), normal) + if stack.uprightness(pose.yaw_deg, pose.pitch_deg) < -0.5: + flipped += 1 + assert flipped <= len(normals) * 0.03, ( + f"{flipped} of {len(normals)} vertices ended turned over" + ) + + +def test_a_pose_reports_whether_it_is_upright(): + """A pose can align the normal and still turn the sensor over.""" + stack = PlatformStack() + seen = {True: 0, False: 0} + for unit in _random_units(300, seed=3): + pose = stack.solve(np.zeros(3), unit) + seen[pose.upright] += 1 + # Whatever it says must match the measured orientation. + assert pose.upright == (stack.uprightness(pose.yaw_deg, pose.pitch_deg) >= 0.0) + assert seen[True] > 0 + + +def test_an_out_of_range_pose_is_reported_and_never_silently_clamped(): + """Tight travel must give a measured residual, not a pose the stage cannot reach.""" + stack = PlatformStack(pitch_limit_deg=45.0) + for unit in _random_units(50, seed=5): + pose = stack.solve(np.zeros(3), unit) + assert abs(pose.pitch_deg) <= 45.0 + 1e-6 + assert abs(pose.yaw_deg) <= stack.yaw_limit_deg + 1e-6 + if not pose.in_range: + assert "travel" in pose.reason + + +def test_nothing_is_reported_out_of_range_when_the_travel_is_ample(): + stack = PlatformStack() + for unit in _random_units(300, seed=11): + pose = stack.solve(np.zeros(3), unit) + assert pose.in_range, f"{unit} reported out of range at full travel" + assert pose.reason == "" or "indeterminate" in pose.reason + + +def test_reachability_agrees_with_the_envelope(): + """Every normal is reachable when the yaw can turn a full half-turn.""" + stack = PlatformStack() + for unit in _random_units(500, seed=13): + reachable, why = stack.reachable(unit, stack.tool_direction) + assert reachable, f"{unit} unreachable: {why}" + # And with the pitch stage able to turn, no normal is rejected either. + assert stack.reachable( + np.array([0.0, 0.0, 1.0]), stack.tool_direction + )[0] + + +# --------------------------------------------------------------------- poles + + +@pytest.mark.parametrize( + "normal", + [ + np.array([0.0, 1.0, 0.0]), # on the tool axis + np.array([0.0, -1.0, 0.0]), # on the tool axis, other way + np.array([1.0, 0.0, 0.0]), # on the yaw plane + np.array([0.0, 0.0, 1.0]), # on the yaw axis + ], +) +def test_poles_still_return_a_usable_pose(normal): + stack = PlatformStack() + pose = stack.solve(np.zeros(3), normal) + assert pose.ok + assert math.isfinite(pose.yaw_deg) and math.isfinite(pose.pitch_deg) + landed = _apply(stack, stack.normal_to_platform(normal) / np.linalg.norm( + stack.normal_to_platform(normal) + ), pose.yaw_deg, pose.pitch_deg) + measured = math.degrees( + math.acos(max(-1.0, min(1.0, float(landed @ stack.tool_direction)))) + ) + assert measured <= stack.tolerance_deg + 1e-6 + + +def test_the_degenerate_normal_is_still_refused(): + stack = PlatformStack() + pose = stack.solve(np.zeros(3), np.zeros(3)) + assert not pose.ok + assert "degenerate" in pose.reason + + +# ----------------------------------------------------------------- selection + + +def test_select_branch_prefers_in_range_then_upright_then_small(): + reachable_upright = Branch(10.0, 5.0, 1e-6, True, True) + reachable_inverted = Branch(10.0, 5.0, 1e-6, False, True) + unreachable_upright = Branch(179.0, 5.0, 1e-6, True, False) + + # In range beats everything: an inverted pose the machine can reach is worth + # more than an upright one it cannot. + assert PlatformStack.select_branch( + [unreachable_upright, reachable_inverted, reachable_upright] + ) is reachable_upright + # Among reachable poses, upright wins. + assert PlatformStack.select_branch( + [reachable_inverted, reachable_upright] + ) is reachable_upright + # Among poses that are out of range and one of them upright, upright wins. + unreachable_inverted = Branch(179.0, 5.0, 1e-6, False, False) + assert PlatformStack.select_branch( + [unreachable_inverted, unreachable_upright] + ) is unreachable_upright + # A materially better alignment beats an upright pose. + much_better = Branch(10.0, 5.0, 0.0, False, True) + worse_upright = Branch(10.0, 5.0, 5.0, True, True) + assert PlatformStack.select_branch([much_better, worse_upright]) is much_better + # And with everything else equal, the smaller move wins. + smaller = Branch(5.0, 5.0, 1e-6, True, True) + larger = Branch(60.0, 5.0, 1e-6, True, True) + assert PlatformStack.select_branch([larger, smaller]) is smaller + + +def test_an_unknown_method_is_refused(): + stack = PlatformStack() + with pytest.raises(ValueError): + stack.solve(np.zeros(3), np.array([0.0, 1.0, 0.0]), method="nonsense") diff --git a/apps/autocal/tests/test_kinematics.py b/apps/autocal/tests/test_kinematics.py index e218120..77e2a34 100644 --- a/apps/autocal/tests/test_kinematics.py +++ b/apps/autocal/tests/test_kinematics.py @@ -1,19 +1,20 @@ """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. +The shipped solver is **closed form** (see ``test_ik_analytical.py``, which checks +it against both the sweep and an exhaustive brute force). What this module covers +is everything around it: the mount mapping, the envelope, the frames, the reported +contact point, and the selectable sweep path. -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. +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 its results +are still checked against a brute-force search, just not with a fine grid over +seven normals when the closed form now guarantees exactness on its own. """ from __future__ import annotations +import dataclasses + import numpy as np import pytest @@ -47,7 +48,13 @@ NORMALS = ( 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.""" + """The lowest residual any pose in the envelope achieves — by search. + + Kept, but no longer run over the whole grid for every normal: the shipped + solver is closed form and ``test_ik_analytical.py`` checks it against an + exhaustive sweep. This survives as the reference for the *sweep* path, which + is still selectable and still needs a floor to be measured against. + """ tool = stack.tool_direction yaw_limit = min(180.0, stack.yaw_limit_deg) pitch_limit = min(360.0, stack.pitch_limit_deg) @@ -122,20 +129,61 @@ def test_swapping_the_mapping_changes_the_answer(): def test_the_bench_envelope_is_the_default(): stack = PlatformStack() - assert stack.yaw_limit_deg == 90.0 + # Confirmed at the bench and against the trunnion drawing: the vertical stage + # turns a full half-turn either way, the horizontal one turns continuously. + assert stack.yaw_limit_deg == 180.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.""" +def test_the_nominal_normal_needs_no_rotation_and_lands_on_plus_z(): + """The polarity check, and the anchor for the whole solve. + + The sensor sits at the bottom of its shell with the sensing face up, so the + face points along ``+tool_axis`` and the nominal pose is *no rotation at all*. + + This is the test that was missing. ``approach_sign`` was ``-1``, aiming every + normal at ``-Z``; the solver dutifully used pitch 180 and turned the sensor + over, and nothing noticed because the residual is sign-blind — a face turned + fully over still scored 0.0000 degrees. Assert the direction landed in, not + just the angle between the lines. + """ stack = PlatformStack() + assert stack.approach_sign == 1 + assert np.allclose(stack.tool_direction, [0.0, 0.0, 1.0]) + + nominal = np.array([0.0, 1.0, 0.0]) # sensor +Y, the direction the face looks + unit = stack.normal_to_platform(nominal) + unit = unit / np.linalg.norm(unit) + pose = stack.solve(np.array([0.0, 5.0, 0.0]), nominal) + + landed = rotation_about(stack.pitch_axis, pose.pitch_deg) @ rotation_about( + stack.yaw_axis, pose.yaw_deg + ) @ unit + landed = landed / np.linalg.norm(landed) + assert landed[2] > 0.0, f"the face was aimed at {np.round(landed, 3)}, not +Z" + # And it gets there without turning: the commissioned pose is the identity. + assert abs(pose.pitch_deg) < 1e-6 + assert abs(pose.yaw_deg) < 1e-6 + assert pose.upright + + +def test_the_sweep_is_never_worse_than_a_brute_force_search(): + """The sweep path still needs a floor, but one normal is enough to pin it. + + The shipped solver is closed form and ``test_ik_analytical.py`` checks it + against an exhaustive sweep over many normals. This keeps the *search* path + honest with one coarse brute force, rather than seven fine ones — the fine + version cost about three seconds per normal to confirm something the closed + form now guarantees outright. + """ + stack = PlatformStack() + normal = NORMALS[4] 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 " + pose = stack.solve(np.array([0.0, 5.0, 0.0]), normal, method="search") + best = _brute_force_best(stack, unit, step=2.0) + assert pose.residual_deg <= best + 0.1, ( + f"the sweep returned {pose.residual_deg:.3f} deg where " f"{best:.3f} deg is achievable" ) @@ -361,13 +409,134 @@ def test_the_plate_lifts_the_point_along_the_tool_axis(): 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]) +def test_the_tool_position_is_the_same_number_the_move_uses(): + """One field for "where the tool is", not two that can drift apart. + + There used to be a separate ``tool_offset_mm`` added to the *reported* contact + while the drawing left the geometry alone: the panel then reported one landing + point and drew another, 10 mm away. The reported move, the drawn move and the + landing check all read ``tool_xy_mm`` now, and this pins that. + """ + stack = PlatformStack(tool_xy_mm=(10.0, -4.0)) + point = np.array([2.0, 9.0, 1.0]) + normal = np.array([0.3, 0.9, 0.2]) + normal = normal / np.linalg.norm(normal) + pose = stack.solve(point, normal) + landed = stack.platform_rotation(pose.yaw_deg, pose.pitch_deg) @ stack.setup_point(point) + final = landed + np.array([pose.table_xy_mm[0], pose.table_xy_mm[1], 0.0]) + assert final[0] == pytest.approx(10.0, abs=1e-9) + assert final[1] == pytest.approx(-4.0, abs=1e-9) + # And there is no second place a tool position could hide. + assert not hasattr(stack, "tool_offset_mm") + + +def test_the_reported_xy_is_where_the_point_is_not_where_the_table_goes(): + """Two different numbers, kept apart on purpose.""" + stack = PlatformStack(tool_xy_mm=(3.0, 3.0)) + point = np.array([2.0, 5.0, 3.0]) + stored = stack.setup_point(point) 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) + assert pose.target_xy_mm[0] == pytest.approx(stored[0]) + assert pose.target_xy_mm[1] == pytest.approx(stored[1]) + # The move is what carries that point to the tool, so it is the rest of the way. + assert pose.table_xy_mm[0] == pytest.approx(3.0 - stored[0]) + assert pose.table_xy_mm[1] == pytest.approx(3.0 - stored[1]) + + +# ------------------------------------------------- the setup and the tool position + + +def test_the_default_tool_is_above_the_table_origin(): + """The default has to be the origin, or every reported move is off by a patch.""" + stack = PlatformStack() + assert stack.tool_xy_mm == (0.0, 0.0) + point = np.array([3.0, 8.0, 1.0]) + pose = stack.solve(point, np.array([0.0, 1.0, 0.0])) + # With the tool at the origin, the move is the contact's position negated: the + # table has to bring a point that is over there back to the middle. + assert pose.table_xy_mm[0] == pytest.approx(-pose.target_xy_mm[0]) + assert pose.table_xy_mm[1] == pytest.approx(-pose.target_xy_mm[1]) + + +def test_the_table_move_lands_the_contact_on_the_tool(): + """The one invariant the whole machine exists to satisfy.""" + stack = PlatformStack( + tool_xy_mm=(4.0, -7.0), + sensor_offset_mm=(1.0, 2.0, 3.0), + sensor_yaw_deg=25.0, + ) + for point, normal in ( + (np.array([2.0, 9.0, 1.0]), np.array([0.3, 0.9, 0.2])), + (np.array([-4.0, 6.0, 3.0]), np.array([-0.6, 0.5, 0.4])), + (np.array([0.0, 11.0, 0.0]), np.array([0.0, 1.0, 0.0])), + ): + pose = stack.solve(point, normal / np.linalg.norm(normal)) + assert pose.ok + landed = stack.platform_rotation(pose.yaw_deg, pose.pitch_deg) @ stack.setup_point( + point + ) + final = landed + np.array([pose.table_xy_mm[0], pose.table_xy_mm[1], 0.0]) + assert final[0] == pytest.approx(stack.tool_xy_mm[0], abs=1e-9) + assert final[1] == pytest.approx(stack.tool_xy_mm[1], abs=1e-9) + + +def test_the_mounting_offset_moves_the_point_and_the_mounting_yaw_the_whole_frame(): + """x/y translate the contact; the yaw turns it about the table origin.""" + stack = PlatformStack() + point = np.array([2.0, 6.0, 1.0]) + normal = np.array([0.2, 0.95, 0.1]) + normal = normal / np.linalg.norm(normal) + base = stack.solve(point, normal) + + shifted = dataclasses.replace(stack, sensor_offset_mm=(1.5, -2.5, 0.0)) + moved = shifted.solve(point, normal) + assert moved.target_xy_mm[0] - base.target_xy_mm[0] == pytest.approx(1.5) + assert moved.target_xy_mm[1] - base.target_xy_mm[1] == pytest.approx(-2.5) + assert moved.contact_z_mm == pytest.approx(base.contact_z_mm) + + turned = dataclasses.replace(stack, sensor_yaw_deg=35.0) + yawed = turned.solve(point, normal) + # A yaw about the table's z keeps the radius from the origin and the height. + assert float(np.hypot(*yawed.target_xy_mm)) == pytest.approx( + float(np.hypot(*base.target_xy_mm)), rel=1e-9 + ) + assert yawed.contact_z_mm == pytest.approx(base.contact_z_mm) + # The mounting yaw is absorbed by the stage: it has to turn that much less. + difference = (base.yaw_deg - yawed.yaw_deg) % 360.0 + assert min(difference, 360.0 - difference) == pytest.approx(35.0, abs=1e-9) + assert yawed.pitch_deg == pytest.approx(base.pitch_deg) + assert yawed.residual_deg == pytest.approx(base.residual_deg, abs=1e-9) + + +def test_the_setup_z_is_the_mounting_plate(): + """The plate is not a separate input: it is the z of the mounting pose.""" + stack = PlatformStack() + point = np.array([1.0, 7.0, 2.0]) + normal = np.array([0.1, 0.99, 0.0]) + normal = normal / np.linalg.norm(normal) + plain = stack.solve(point, normal) + + lifted = dataclasses.replace(stack, sensor_offset_mm=(0.0, 0.0, 4.25)) + raised = lifted.solve(point, normal) + assert raised.contact_z_mm - plain.contact_z_mm == pytest.approx(4.25) + assert raised.target_xy_mm == pytest.approx(plain.target_xy_mm) + # And it is the same number as the plate parameter it replaced. + assert lifted.setup_point(point)[2] == pytest.approx( + stack.point_to_platform(point, plate_thickness_mm=4.25)[2] + ) + + +def test_the_setup_normal_is_the_mounting_yaw_applied_to_the_mount(): + stack = PlatformStack(sensor_yaw_deg=90.0) + normal = np.array([1.0, 0.0, 0.0]) + # Sensor +x is platform +x; a 90 degree mounting yaw sends it to platform +y. + assert np.allclose(stack.setup_normal(normal), [0.0, 1.0, 0.0], atol=1e-12) + assert stack.normal_to_platform(normal) == pytest.approx([1.0, 0.0, 0.0]) + + +def test_a_non_finite_sensor_yaw_is_refused(): + with pytest.raises(ValueError): + PlatformStack(sensor_yaw_deg=float("nan")).setup_rotation() def test_the_approach_sign_is_validated(): diff --git a/apps/autocal/tests/test_layout_shell.py b/apps/autocal/tests/test_layout_shell.py deleted file mode 100644 index f1928aa..0000000 --- a/apps/autocal/tests/test_layout_shell.py +++ /dev/null @@ -1,586 +0,0 @@ -"""The single-page shell, from an operator's point of view. - -These assertions are about the layout contract rather than any one panel: - -* the motion status strip is outside the tabs, so it survives every tab change; -* all five tabs exist, are labelled, and their pages fit; -* the right-hand readout rail shows every drive and issues no per-axis command; -* the window refuses to shrink below the size at which the page would clip. - -The overflow check lives in ``test_panel_smoke.py`` because it needs the colour -tables; what is here is the structure. -""" - -from __future__ import annotations - -import pytest - -from conftest import pump - -from orisys_autocal.sequence_io import SequenceFormatError, SequencePoint -from orisys_autocal.ui.workspace import TABS - - -def _show(window, size=(1600, 900)): - from PySide6.QtWidgets import QApplication - - window.resize(*size) - window.show() - QApplication.processEvents() - return window - - -def test_the_tab_set_is_the_documented_one(make_window): - window = _show(make_window()) - assert window.workspace.page_keys() == [key for key, _ in TABS] - - -def test_every_tab_has_a_localised_label(make_window): - from orisys_app_shared.i18n import t - - window = _show(make_window()) - for index, (_key, locale_key) in enumerate(TABS): - label = window.workspace.tabText(index) - assert label, f"tab {index} has no label" - assert label == t(locale_key) - assert label != locale_key, f"tab {index} shows its raw key" - - -def test_the_status_strip_survives_every_tab_change(make_window): - """The whole point of putting the strip above the tabs.""" - window = _show(make_window()) - for key in window.workspace.page_keys(): - window.workspace.set_current_key(key) - assert window.status_strip.isVisible(), f"the strip hid on tab {key}" - assert window.status_strip.height() <= 48 - - -def test_the_strip_reports_connection_and_bus_state(make_window): - from orisys_app_shared.i18n import t - - from orisys_autocal.motion import BusStatus, Snapshot - - window = _show(make_window()) - strip = window.status_strip - - strip.set_snapshot(Snapshot(connected=False)) - assert strip.chip_connection.text() == t("connection.state_closed") - assert strip.chip_init.text() == t("bus.state_pending") - - strip.set_snapshot( - Snapshot(connected=True, bus=BusStatus(bus_type="ecat", init_status=1, total_axes=5)) - ) - assert strip.chip_connection.text() == t("connection.state_open") - assert strip.chip_bus.text() == t("bus.type_ecat") - assert strip.chip_init.text() == t("bus.state_ok") - - -def test_a_refusal_stays_visible_on_the_strip(make_window): - """A command refused while the operator was on another tab must not vanish.""" - window = _show(make_window()) - window.workspace.set_current_key("sensor") - window.status_strip.set_action_result("轴0 回零模式未设置", False) - assert "回零模式" in window.status_strip.action_label.text() - - -def test_the_readout_rail_covers_every_axis(make_window): - window = _show(make_window()) - assert len(window.axis_readout._rows) == window.axis_panel.axis_count == 5 - - -def test_the_readout_rail_shows_position_and_state(make_window): - from orisys_autocal.motion import AxisState, Snapshot - - window = _show(make_window()) - snapshot = Snapshot( - connected=True, - axes=( - AxisState(axis=0, enabled=True, units=10.0, dpos=1000.0, idle=True), - AxisState(axis=1, enabled=False, units=10.0, dpos=0.0, idle=True), - ), - ) - window.axis_readout.update_snapshot(snapshot) - # 1000 user units at units=10 is the documented 2.000 mm, and the readout - # shows both so an operator never has to convert in their head. - text = window.axis_readout._rows[0]["position"].text() - assert "1000.000" in text and "2.000 mm" in text, text - assert window.axis_readout._rows[1]["state"].text() - - -def test_the_readout_rail_issues_no_per_axis_motion(make_window): - """Per-axis commands live in one place only: the Axis-debug tab.""" - window = _show(make_window()) - rail = window.axis_readout - assert not hasattr(rail, "jogRequested") - assert not hasattr(rail, "moveAbsRequested") - assert hasattr(rail, "stopAllRequested") - assert hasattr(rail, "enableAllRequested") - - -def test_enable_all_toggles_with_the_current_state(make_window): - from orisys_autocal.motion import AxisState, Snapshot - - window = _show(make_window()) - requested: list[bool] = [] - window.axis_readout.enableAllRequested.connect(requested.append) - - window.axis_readout.update_snapshot( - Snapshot(connected=True, axes=tuple(AxisState(axis=a, enabled=True) for a in range(5))) - ) - window.axis_readout.enable_all_button.click() - assert requested == [False], "enable-all did not become disable-all" - - window.axis_readout.update_snapshot(Snapshot(connected=True, axes=())) - window.axis_readout.enable_all_button.click() - assert requested == [False, True] - - -def test_the_window_refuses_to_shrink_below_the_page(make_window): - window = _show(make_window()) - window.resize(800, 600) - assert window.width() >= window.minimumWidth() - assert window.height() >= window.minimumHeight() - assert window.minimumWidth() >= 1300 - - -def test_the_bus_data_group_starts_collapsed(make_window): - """It is commissioning-only; expanded, it cost the page ~270 px.""" - window = _show(make_window()) - assert not window.bus_data_group.isChecked() - assert not window.bus_data_panel.isVisible() - - -def test_expanding_bus_data_does_not_resize_the_window(make_window): - window = _show(make_window()) - before = (window.width(), window.height()) - window.bus_data_group.setChecked(True) - pump(_app(), 0.1) - assert (window.width(), window.height()) == before - - -def _app(): - from PySide6.QtWidgets import QApplication - - return QApplication.instance() - - -def test_the_sequence_tab_shows_the_documented_profile_fields(make_window): - from orisys_autocal.ui.sequence_panel import PROFILE_FIELDS - - window = _show(make_window()) - panel = window.sequence_panel - assert panel.profile_keys() == [key for key, _locale, _kind in PROFILE_FIELDS] - - -def test_the_sequence_panel_exposes_its_three_zones(make_window): - window = _show(make_window()) - panel = window.sequence_panel - assert panel.validation_checks() == [ - "on_surface", - "within_travel", - "contact_confirmable", - ] - assert panel.point_row_count() > 0, "the point list is empty" - assert panel.summary_label.text() - - -def test_every_pending_control_names_the_phase_that_owns_it(make_window): - """A disabled control must say why, or it reads as a broken button.""" - window = _show(make_window()) - panel = window.sequence_panel - assert panel.pending_controls() == ["generate", "simulate", "dry_run"] - for name, phase in panel.pending_phases().items(): - button = panel._pending_buttons[name] - assert not button.isEnabled(), f"{name} is usable but not implemented" - assert phase in button.toolTip(), f"{name} does not name its phase" - assert button.text(), f"{name} has no label" - - -def test_import_and_export_are_live_controls(make_window): - """A sequence is a document, and P8 says it is a versioned artifact.""" - window = _show(make_window()) - panel = window.sequence_panel - for name in ("export", "import"): - button = panel._live_buttons[name] - assert button.isEnabled(), f"{name} is not usable" - assert button.text(), f"{name} has no label" - assert button.toolTip() - - -def test_the_point_list_has_no_empty_action_column(make_window): - """An empty action cell reads as a broken control.""" - window = _show(make_window()) - panel = window.sequence_panel - assert "actions" not in panel.column_keys() - - -def test_the_sequence_tab_hands_out_no_runnable_steps(make_window): - """A runner must not be able to start from points nothing has validated.""" - window = _show(make_window()) - assert window.sequence_panel.planned_steps() == [] - - -def test_point_coordinates_are_generated_not_typed(make_window): - """A typed coordinate is one nothing has checked against the sensor.""" - from orisys_autocal.ui.sequence_panel import _EDITABLE_POINT_COLUMNS - - assert _EDITABLE_POINT_COLUMNS == () - window = _show(make_window()) - panel = window.sequence_panel - # The point list is a real table, so it scrolls instead of clipping rows. - assert panel.table.rowCount() == panel.point_row_count() - assert panel.point_row(0)["x"] == "0.000" - # Every cell is read-only: the table's edit triggers are off, so a generated - # coordinate cannot be typed over. - from PySide6.QtWidgets import QTableWidget - - assert panel.table.editTriggers() == QTableWidget.EditTrigger.NoEditTriggers - - -def test_the_profile_round_trips_through_preferences(make_window): - window = _show(make_window()) - panel = window.sequence_panel - panel.field("pitch_mm").setText("3.5") - panel.field("repeats").setText("4") - window._persist_prefs() - stored = window.prefs["sequence_profile"] - assert stored["pitch_mm"] == 3.5 - assert stored["repeats"] == 4 - - -def test_an_unusable_profile_number_falls_back_rather_than_raising(make_window): - from orisys_autocal.preferences import default_preferences - - window = _show(make_window()) - panel = window.sequence_panel - panel.field("pitch_mm").setText("not a number") - assert panel.profile()["pitch_mm"] == 0.0 - # A blank repeats field must not become zero repeats. - panel.field("repeats").setText("") - assert panel.profile()["repeats"] >= 1 - assert "pitch_mm" in default_preferences()["sequence_profile"] - - -def test_the_stored_profile_is_loaded_into_the_form(make_window): - window = _show(make_window(sequence_profile={"pitch_mm": 7.5, "order": "row"})) - panel = window.sequence_panel - assert panel.profile()["pitch_mm"] == 7.5 - assert panel.profile()["order"] == "row" - - -def test_the_axis_table_shows_seven_columns(make_window): - from orisys_autocal.ui.axis_panel import COLUMNS - - window = _show(make_window()) - assert len(COLUMNS) == 7 - assert window.axis_panel.table.columnCount() == 8 # seven data + actions - - -def test_the_axis_detail_line_carries_what_the_table_dropped(make_window): - """Axis type, accel, feedback and the status word must stay reachable.""" - from orisys_autocal.motion import AxisState, Snapshot - - window = _show(make_window()) - window.axis_panel.set_selected_axis(3) - window.axis_panel.update_snapshot( - Snapshot( - connected=True, - axes=(AxisState(axis=3, enabled=True, atype=65, accel=1234.0, mpos=5.0),), - ) - ) - detail = window.axis_panel._detail_label.text() - assert "65" in detail and "1234" in detail - assert "3" in detail - - -def test_the_axis_detail_follows_the_selected_axis(make_window): - from orisys_autocal.motion import AxisState, Snapshot - - window = _show(make_window()) - window.axis_panel.update_snapshot( - Snapshot( - connected=True, - axes=(AxisState(axis=0, atype=65), AxisState(axis=1, atype=67)), - ) - ) - window.axis_panel.set_selected_axis(0) - assert "65" in window.axis_panel._detail_label.text() - window.axis_panel.set_selected_axis(1) - assert "67" in window.axis_panel._detail_label.text() - - -def test_the_window_defaults_to_the_design_resolution(make_window): - from orisys_autocal.ui.window import DEFAULT_WINDOW_SIZE, MINIMUM_WINDOW_SIZE - - assert DEFAULT_WINDOW_SIZE == (1920, 1080) - assert MINIMUM_WINDOW_SIZE[0] >= 1440 - - -def test_the_default_theme_is_light(make_window): - from orisys_autocal.preferences import DEFAULT_THEME - - assert DEFAULT_THEME == "light" - window = _show(make_window()) - assert window._theme == "light" - - -def test_pausing_the_strip_pauses_the_poller(make_window): - window = _show(make_window()) - window.status_strip.pause_button.setChecked(True) - window._on_pause_polling() - assert window.poller.paused - window.status_strip.pause_button.setChecked(False) - window._on_pause_polling() - assert not window.poller.paused - - -def test_the_diagnostics_footer_does_not_move_the_layout(make_window): - window = _show(make_window()) - splitters = window.main_splitter.sizes() - window._set_diagnostics(True) - window._set_diagnostics(False) - assert window.main_splitter.sizes() == splitters - - -def test_splitter_sizes_round_trip_through_preferences(make_window): - window = _show(make_window()) - window.main_splitter.setSizes([340, 1500]) - window._persist_prefs() - stored = window.prefs["main_splitter_sizes"] - assert len(stored) == 2 - assert stored[1] > stored[0] - - -def test_the_rail_is_a_single_scrollable_column(make_window): - """Two thin rails bracketing the workspace cost it 240 px at every size.""" - from orisys_autocal.ui.window import MAIN_SPLITTER_SIZES - - assert len(MAIN_SPLITTER_SIZES) == 2, "the layout is back to three columns" - window = _show(make_window()) - assert window.main_splitter.count() == 2 - assert window.left_rail.widget() is window.rail_content - # The readout lives in the rail now, not in a rail of its own. - assert window.rail_content.findChild(type(window.axis_readout)) is window.axis_readout - - -def test_the_rail_scrolls_rather_than_clipping_its_controls(make_window): - """At 900 px high the three rail groups do not fit; the rail must scroll.""" - window = _show(make_window(), (1440, 900)) - viewport = window.left_rail.viewport().height() - content = window.rail_content.sizeHint().height() - scroll = window.left_rail.verticalScrollBar() - assert content <= viewport or scroll.maximum() > 0, ( - f"rail content is {content}px in a {viewport}px viewport and cannot scroll" - ) - - -# --------------------------------------------------------------- run controls - - -def test_the_run_bar_is_visible_from_every_tab(make_window): - """Abort must never require finding the tab that started the run.""" - window = _show(make_window()) - for key in window.workspace.page_keys(): - window.workspace.set_current_key(key) - assert window.run_bar.isVisible(), f"the run bar hid on tab {key}" - - -def test_run_is_wired_and_refuses_by_naming_its_phase(make_window): - window = _show(make_window()) - window.run_bar.run_button.click() - assert "P7" in window.status_strip.action_label.text() - - -def test_abort_stops_the_axes_and_is_enabled_once_a_run_exists(make_window): - window = _show(make_window()) - assert not window.run_bar.abort_button.isEnabled() - - requested = [] - window.run_bar.abortRequested.connect(lambda: requested.append(True)) - window.run_bar.set_run(True) - assert window.run_bar.abort_button.isEnabled() - window.run_bar.abort_button.click() - assert requested == [True] - assert window.run_bar.phase == "failed" - - -def test_the_run_bar_shows_progress_and_timing(make_window): - window = _show(make_window()) - window.run_bar.set_progress(7, 40) - assert "7" in window.run_bar.step_label.text() - assert "40" in window.run_bar.step_label.text() - assert window.run_bar.progress.maximum() == 40 - assert window.run_bar.progress.value() == 7 - - window.run_bar.set_timing(125.0, 300.0) - assert "2:05" in window.run_bar.time_label.text() - # An unknown estimate must say so rather than showing a wrong number. - window.run_bar.set_timing(125.0, None) - assert "2:05" in window.run_bar.time_label.text() - - -# ---------------------------------------------------------------- poll health - - -def test_the_health_chip_reports_the_age_of_the_last_reading(make_window): - from orisys_autocal.motion import Snapshot - - window = _show(make_window()) - strip = window.status_strip - strip.set_snapshot(Snapshot(connected=True)) - assert strip.chip_health.text() - assert strip.chip_health._tone == "ok" - - -def test_the_health_chip_ages_when_the_poll_stops(make_window): - """A stalled poll must not keep looking healthy.""" - from orisys_autocal.motion import Snapshot - - window = _show(make_window()) - strip = window.status_strip - strip.set_poll_interval_ms(100) - strip.set_snapshot(Snapshot(connected=True)) - strip._last_snapshot_at -= 1.5 # fifteen intervals ago - strip.refresh_health() - assert strip.chip_health._tone == "error", strip.chip_health.text() - - -def test_the_health_chip_says_paused_rather_than_ageing(make_window): - from orisys_autocal.motion import Snapshot - - window = _show(make_window()) - strip = window.status_strip - strip.set_snapshot(Snapshot(connected=True)) - strip.set_paused(True) - strip.refresh_health() - assert strip.chip_health._tone == "muted" - assert strip.chip_health.text() != "" - - -def test_the_status_bar_uses_its_own_theme_token(make_window): - """The brand navy reads well as a header and heavy as a footer.""" - from orisys_app_shared.theme import load_colors - - for theme in ("light", "dark"): - colours = load_colors(theme) - assert "status_bar_bg" in colours, theme - assert "status_bar_text" in colours, theme - window = _show(make_window()) - assert "QStatusBar" in window.statusBar().styleSheet() - - -def test_the_sequence_summary_reports_count_and_span(make_window): - """The two numbers an operator reasons with, before anything runs.""" - window = _show(make_window()) - panel = window.sequence_panel - panel.set_points( - [ - SequencePoint(id=1, target_mm={"x": 0.0, "y": 0.0, "z": 12.4}), - SequencePoint(id=2, target_mm={"x": 10.0, "y": 0.0, "z": 12.4}), - ] - ) - text = panel.summary_label.text() - assert "2" in text - # A nil Z span prints once, and the real X span prints both ends. - assert "0.000–10.000" in text - assert "12.400" in text - - -def test_the_sequence_summary_updates_with_the_profile(make_window): - window = _show(make_window()) - panel = window.sequence_panel - panel.set_points([SequencePoint(id=1, target_mm={"x": 0.0, "y": 0.0, "z": 1.0})]) - before = panel.summary_label.text() - panel.field("repeats").setText("10") - assert panel.summary_label.text() != before - - -def test_an_empty_sequence_says_so_rather_than_showing_zeros(make_window): - window = _show(make_window()) - panel = window.sequence_panel - panel.set_points([]) - assert panel.summary_label.text() - assert "0" not in panel.summary_label.text()[:2] - - -def test_the_design_is_dirty_until_it_is_saved_or_loaded(make_window): - """Import replaces the design, and there is no autosave.""" - window = _show(make_window()) - panel = window.sequence_panel - panel.mark_clean() - assert not panel.dirty - panel.field("pitch_mm").setText("9") - assert panel.dirty, "a profile edit did not mark the design dirty" - - -def test_export_then_import_restores_the_design(make_window, tmp_dir): - """The round trip through the real panel, not just the format module.""" - window = _show(make_window()) - panel = window.sequence_panel - panel.set_name("bench sweep") - panel.field("pitch_mm").setText("3.5") - panel.set_points( - [ - SequencePoint(id=1, target_mm={"x": 0.0, "y": 0.0, "z": 12.4}), - SequencePoint(id=2, target_mm={"x": 3.5, "y": 0.0, "z": 12.4}), - ] - ) - path = tmp_dir / "sweep.oryseq" - window.export_sequence(str(path)) - assert path.is_file() - assert not panel.dirty, "export did not clear the dirty flag" - - # Change everything, then import it back. - panel.set_name("") - panel.field("pitch_mm").setText("1") - panel.set_points([]) - document = window.import_sequence(str(path)) - - assert document.point_count == 2 - assert panel.name() == "bench sweep" - assert panel.profile()["pitch_mm"] == 3.5 - assert panel.point_row_count() == 2 - assert not panel.dirty - - -def test_importing_a_bad_file_reports_and_changes_nothing(make_window, tmp_dir): - window = _show(make_window()) - panel = window.sequence_panel - panel.field("pitch_mm").setText("4") - path = tmp_dir / "bad.oryseq" - path.write_text("not json at all", encoding="utf-8") - with pytest.raises(SequenceFormatError) as excinfo: - window.import_sequence(str(path)) - assert "not valid JSON" in str(excinfo.value) - # The design is untouched: a refused import is not a partial import. - assert panel.profile()["pitch_mm"] == 4 - - -def test_an_exported_sequence_names_the_sensor_it_was_designed_for(make_window, tmp_dir): - import json - - window = _show(make_window()) - window.prefs["sensor_setup"] = { - "sensor_type": "finger", - "geometry_id": "finger-1.0.0", - "probe_tip_set": True, - } - path = tmp_dir / "named.oryseq" - window.export_sequence(str(path)) - document = json.loads(path.read_text(encoding="utf-8")) - assert document["device"]["sensor_type"] == "finger" - assert document["device"]["geometry_id"] == "finger-1.0.0" - assert document["versions"]["app"] - - -def test_language_switch_relabels_the_new_shell(make_window): - from orisys_app_shared.i18n import t - - window = _show(make_window(language="zh_CN")) - zh_tabs = [window.workspace.tabText(i) for i in range(window.workspace.count())] - window._set_language("en_US") - assert window.status_strip.title.text() == t("status.motion") - assert window.run_bar.title.text() == t("run.title") - en_tabs = [window.workspace.tabText(i) for i in range(window.workspace.count())] - assert en_tabs != zh_tabs - assert all(en_tabs) diff --git a/apps/autocal/tests/test_motion_service.py b/apps/autocal/tests/test_motion_service.py deleted file mode 100644 index b91b0df..0000000 --- a/apps/autocal/tests/test_motion_service.py +++ /dev/null @@ -1,328 +0,0 @@ -"""Guard and state-machine behaviour of the motion service. - -Driven against the simulated platform, so every assertion here is about AutoCal's -own logic: what it refuses, what it reads, and when. No test in this file touches -a controller or a DLL. -""" - -from __future__ import annotations - -import time - -import pytest - -from orisys_autocal.motion import ( - INIT_FAILED, - INIT_OK, - INIT_RUNNING, - AxisParams, - ConnectionSpec, - DemoTransport, - MotionService, -) - - -@pytest.fixture -def service(): - return MotionService(DemoTransport()) - - -def _initialise(service, timeout: float = 5.0) -> None: - """Download a BAS program and wait for the simulated bus to report ready.""" - service.download_bas(__file__, 1) - deadline = time.monotonic() + timeout - while time.monotonic() < deadline and service.bus.init_status != INIT_OK: - service.poll([0]) - time.sleep(0.05) - - -# ----------------------------------------------------------------- connection - - -def test_a_fresh_service_is_disconnected_and_touches_nothing(service): - assert not service.connected - assert not service.bas_loaded - assert service.poll([0]).connected is False - - -def test_connect_reports_success_and_an_unknown_bus_without_a_bas(service): - result = service.connect(ConnectionSpec(mode="demo")) - assert result.ok - assert service.connected - # No BAS program has run, so the bus type must not be guessed at. - snapshot = service.poll([0]) - assert snapshot.bus.bus_type == "unknown" - assert not snapshot.bus.initialized - - -def test_connect_failure_is_reported_without_raising(service): - result = service.connect(ConnectionSpec(mode="eth", ip="127.0.0.1")) - assert not result.ok - assert not service.connected - - -def test_disconnect_clears_bus_state(service): - service.connect(ConnectionSpec(mode="demo")) - service.disconnect() - assert not service.connected - assert service.bus.bus_type == "unknown" - - -# ---------------------------------------------------------------- bus init - - -def test_download_requires_a_real_file(service): - service.connect(ConnectionSpec(mode="demo")) - assert not service.download_bas("", 1).ok - assert not service.download_bas(r"D:\nope\missing.bas", 1).ok - - -def test_download_needs_a_connection(service): - assert not service.download_bas(__file__, 1).ok - - -def test_download_then_poll_reaches_initialised(service): - service.connect(ConnectionSpec(mode="demo")) - assert service.download_bas(__file__, 1).ok - _initialise(service) - assert service.bus.init_status == INIT_OK - assert service.bus.bus_type == "ecat" - assert service.bus.total_axes == 5 - assert service.bus.node_count == 5 - - -def test_bus_is_running_immediately_after_download(service): - service.connect(ConnectionSpec(mode="demo")) - service.download_bas(__file__, 1) - assert service.bus.init_status == INIT_RUNNING - - -def test_reinitialise_is_refused_while_already_running(service): - service.connect(ConnectionSpec(mode="demo")) - service.download_bas(__file__, 1) - assert not service.reinit_bus().ok - - -def test_reinitialise_is_refused_without_a_bas_program(service): - service.connect(ConnectionSpec(mode="demo")) - result = service.reinit_bus() - assert not result.ok - assert "BAS" in result.message or "Bas" in result.message - - -def test_init_timeout_is_reported_rather_than_raising(): - """A controller that never publishes its globals must time out, not hang.""" - - class SilentTransport(DemoTransport): - def user_var(self, name): - from orisys_app_shared.zmotion import ZAuxError - - raise ZAuxError(30002, f"{name} is not defined") - - service = MotionService(SilentTransport()) - service._init_timeout = 0.05 # noqa: SLF001 - shortening the vendor window - service.connect(ConnectionSpec(mode="demo")) - service.download_bas(__file__, 1) - time.sleep(0.1) - snapshot = service.poll([0]) - assert snapshot.bus.init_status == INIT_FAILED - assert snapshot.bus.message # carries the ZDevelop hint, not an empty string - - -# ------------------------------------------------------------------- guards - - -def test_motion_is_refused_before_the_bus_is_initialised(service): - service.connect(ConnectionSpec(mode="demo")) - for attempt in ( - service.jog(0, True), - service.move_abs(0, 100.0), - service.move_rel(0, 10.0), - service.home(0), - ): - assert not attempt.ok - assert attempt.message - - -def test_motion_is_refused_when_not_connected(service): - assert not service.jog(0, True).ok - assert not service.move_abs(0, 1.0).ok - assert not service.stop(0).ok - - -def test_motion_on_an_unknown_axis_is_refused(service): - service.connect(ConnectionSpec(mode="demo")) - _initialise(service) - result = service.jog(99, True) - assert not result.ok - assert "99" in result.message or "0-4" in result.message - - -@pytest.mark.parametrize("bad", [float("nan"), float("inf"), float("-inf")]) -def test_a_non_finite_target_is_refused(service, bad): - service.connect(ConnectionSpec(mode="demo")) - _initialise(service) - result = service.move_abs(0, bad) - assert not result.ok - assert result.message - - -def test_home_needs_a_configured_mode(service): - """0 is not a valid homing mode; inheriting one would be worse than saying so.""" - service.connect(ConnectionSpec(mode="demo")) - _initialise(service) - result = service.home(0) - assert not result.ok - assert "0" in result.message - - -def test_home_runs_once_a_mode_is_set(service): - service.connect(ConnectionSpec(mode="demo")) - _initialise(service) - service.set_axis_params(0, AxisParams(home_mode=17)) - assert service.home(0).ok - - -def test_home_is_refused_on_rtex(service): - """The vendor example refuses RTEX homing; so do we.""" - - class RtexTransport(DemoTransport): - def user_var(self, name): - return 1.0 if name == "BUS_TYPE" else super().user_var(name) - - rtex = MotionService(RtexTransport()) - rtex.connect(ConnectionSpec(mode="demo")) - _initialise(rtex) - assert rtex.bus.bus_type == "rtex" - result = rtex.home(0) - assert not result.ok - assert result.message - - -# ------------------------------------------------------------------- motion - - -def test_jog_and_stop_move_the_state_machine(service): - service.connect(ConnectionSpec(mode="demo")) - _initialise(service) - assert service.jog(0, True).ok - state = service.poll([0]).axis(0) - assert state is not None and not state.idle - - assert service.stop(0).ok - state = service.poll([0]).axis(0) - assert state is not None and state.idle - - -def test_absolute_and_relative_moves_change_the_command_position(service): - service.connect(ConnectionSpec(mode="demo")) - _initialise(service) - assert service.move_abs(1, 250.0).ok - assert service.poll([1]).axis(1).dpos == pytest.approx(250.0) - assert service.move_rel(1, -50.0).ok - assert service.poll([1]).axis(1).dpos == pytest.approx(200.0) - - -def test_set_axis_params_writes_and_remembers(service): - service.connect(ConnectionSpec(mode="demo")) - _initialise(service) - params = AxisParams(units=5.0, speed=200.0, accel=2000.0) - assert service.set_axis_params(2, params).ok - assert service.params(2) == params - assert service.to_mm(2, 1000.0) == pytest.approx(1.0) - - -def test_enable_toggles_the_axis(service): - service.connect(ConnectionSpec(mode="demo")) - _initialise(service) - assert service.set_axis_enable(3, False).ok - assert service.poll([3]).axis(3).enabled is False - assert service.set_axis_enable(3, True).ok - assert service.poll([3]).axis(3).enabled is True - - -def test_stop_all_covers_every_axis(service): - service.connect(ConnectionSpec(mode="demo")) - _initialise(service) - results = service.stop_all() - assert len(results) == service.axis_count - assert all(result.ok for result in results) - - -def test_clear_alarm_reaches_the_drive(service): - service.connect(ConnectionSpec(mode="demo")) - _initialise(service) - assert service.clear_alarm(0).ok - - -# ----------------------------------------------------------------- bus data - - -def test_sdo_round_trips_on_ethercat(service): - service.connect(ConnectionSpec(mode="demo")) - _initialise(service) - assert service.sdo_write(0, 0x6040, 0, 3, 6).ok - result, value = service.sdo_read(0, 0x6040, 0, 3) - assert result.ok - assert value == 6 - - -def test_sdo_is_refused_on_rtex(service): - class RtexTransport(DemoTransport): - def user_var(self, name): - return 1.0 if name == "BUS_TYPE" else super().user_var(name) - - rtex = MotionService(RtexTransport()) - rtex.connect(ConnectionSpec(mode="demo")) - _initialise(rtex) - result, _value = rtex.sdo_read(0, 0x6040, 0, 3) - assert not result.ok - - -def test_rtex_is_refused_on_ethercat(service): - service.connect(ConnectionSpec(mode="demo")) - _initialise(service) - result, _value = service.rtex_read(0, 1) - assert not result.ok - - -def test_bus_data_is_refused_before_the_bus_type_is_known(service): - service.connect(ConnectionSpec(mode="demo")) - result, _value = service.sdo_read(0, 0x6040, 0, 3) - assert not result.ok - - -# ------------------------------------------------------------------- polling - - -def test_polling_reports_every_requested_axis(service): - service.connect(ConnectionSpec(mode="demo")) - snapshot = service.poll([0, 1, 2, 3, 4]) - assert [state.axis for state in snapshot.axes] == [0, 1, 2, 3, 4] - assert all(state.read_ok for state in snapshot.axes) - - -def test_a_read_failure_is_marked_not_silently_zeroed(): - """A failed read must not look like a position of zero.""" - - class FailingReads(DemoTransport): - def get_dpos(self, axis): - from orisys_app_shared.zmotion import ZAuxError - - raise ZAuxError(30000, "no response") - - service = MotionService(FailingReads()) - service.connect(ConnectionSpec(mode="demo")) - snapshot = service.poll([0]) - state = snapshot.axis(0) - assert state is not None - assert state.read_ok is False - assert state.error - - -def test_language_switches_the_refusal_text(service): - assert not service.jog(0, True).ok - zh = service.jog(0, True).message - service.set_language("en_US") - en = service.jog(0, True).message - assert zh != en diff --git a/apps/autocal/tests/test_panel_smoke.py b/apps/autocal/tests/test_panel_smoke.py deleted file mode 100644 index 7b6fb51..0000000 --- a/apps/autocal/tests/test_panel_smoke.py +++ /dev/null @@ -1,331 +0,0 @@ -"""Offscreen UI smoke tests: construction, layout, gating, and preferences. - -The window is built for real, with the simulated platform standing in for the -controller. Nothing here needs a display or hardware. -""" - -from __future__ import annotations - -import pytest - -from conftest import pump, visible_descendants - -RESOLUTIONS = ((1440, 900), (1600, 900), (1920, 1080)) - - -def _show(window, size=(1440, 880)): - from PySide6.QtWidgets import QApplication - - window.resize(*size) - window.show() - QApplication.processEvents() - return window - - -def _enabled(widget) -> bool: - """Whether ``widget`` is effectively enabled, shown or not. - - A plain ``isEnabled()`` is False for every child of a window that has not - been shown, which makes an assertion about gating depend on whether the - test happened to realise the window. ``isEnabledTo`` answers the question - the tests actually mean: would a user be able to operate this control? - """ - return widget.isEnabledTo(widget.window()) - - -# ------------------------------------------------------------------ structure - - -@pytest.mark.parametrize(("theme", "language"), [ - ("dark", "zh_CN"), - ("light", "zh_CN"), - ("dark", "en_US"), - ("light", "en_US"), -]) -def test_the_window_builds_in_every_theme_and_language(make_window, theme, language): - window = _show(make_window(theme=theme, language=language)) - assert window.windowTitle() - assert window.axis_panel.table.rowCount() == 5 - - -def test_the_product_name_is_localised_in_the_window_title(make_window): - window = _show(make_window(language="zh_CN")) - zh = window.windowTitle() - window._set_language("en_US") - assert window.windowTitle() != zh - - -def test_every_axis_row_has_a_labelled_control(make_window): - window = _show(make_window()) - panel = window.axis_panel - for row in range(panel.axis_count): - widgets = panel._rows[row] - for key in ("enable_button", "fwd_button", "rev_button", "stop_button"): - assert widgets[key].text().strip(), f"row {row} {key} has no label" - - -def test_the_table_shows_one_column_per_documented_quantity(make_window): - from orisys_app_shared.i18n import t - - from orisys_autocal.ui.axis_panel import COLUMNS - - window = _show(make_window()) - headers = [ - window.axis_panel.table.horizontalHeaderItem(index).text() - for index in range(window.axis_panel.table.columnCount()) - ] - assert headers == [t(key) for key in COLUMNS] + [t("axes.controls")] - - -@pytest.mark.parametrize("size", RESOLUTIONS) -def test_every_axis_row_is_reachable(make_window, size): - """No drive may be hidden by the splitter. - - The panel asks for three rows without scrolling; at the default window size - all five must be visible, because a clipped fifth row on a five-axis platform - reads as a missing drive rather than as a scrollbar. If the panel is ever - squeezed below that, the table has to scroll rather than silently cut a row. - """ - window = _show(make_window(), size) - table = window.axis_panel.table - rows_height = sum( - table.rowHeight(row) for row in range(window.axis_panel.axis_count) - ) - header = table.horizontalHeader().height() - available = table.viewport().height() - - assert available >= rows_height or table.verticalScrollBar().maximum() > 0, ( - f"{size}: rows need {rows_height}px, viewport has {available}px " - f"(header {header}px) and it cannot scroll" - ) - - -@pytest.mark.parametrize("size", RESOLUTIONS) -def test_no_visible_child_escapes_its_parent(make_window, size): - window = _show(make_window(), size) - oversize = [] - for child in visible_descendants(window): - if _inside_scroll_area(child): - # A scroll area's content is *meant* to exceed its viewport: the rail - # scrolls instead of squeezing its controls into an unreadable column. - continue - parent = child.parentWidget() - if parent is None or not parent.isVisible(): - continue - geometry = child.geometry() - if geometry.width() > parent.width() + 1 or geometry.height() > parent.height() + 1: - oversize.append( - f"{type(child).__name__} {geometry.width()}x{geometry.height()}" - f" > parent {parent.width()}x{parent.height()}" - ) - assert not oversize, oversize - - -def _inside_scroll_area(widget) -> bool: - """Whether ``widget`` is content of a QAbstractScrollArea, not a laid-out child.""" - from PySide6.QtWidgets import QAbstractScrollArea - - parent = widget.parentWidget() - while parent is not None: - if isinstance(parent, QAbstractScrollArea): - return True - parent = parent.parentWidget() - return False - - -# ----------------------------------------------------------------------- gating - - -def test_motion_controls_start_disabled_without_a_bus(make_window): - window = _show(make_window()) - assert not _enabled(window.axis_panel._rows[0]["fwd_button"]) - assert not _enabled(window.axis_panel._rows[0]["rev_button"]) - - -def test_bus_data_panels_follow_the_reported_bus_type(make_window): - window = _show(make_window()) - window.bus_data_panel.set_connected(True) - - window.bus_data_panel.set_bus_type("ecat") - assert _enabled(window.bus_data_panel.ecat_box) - assert not _enabled(window.bus_data_panel.rtex_box) - - window.bus_data_panel.set_bus_type("rtex") - assert _enabled(window.bus_data_panel.rtex_box) - assert not _enabled(window.bus_data_panel.ecat_box) - - window.bus_data_panel.set_bus_type("unknown") - assert not _enabled(window.bus_data_panel.ecat_box) - assert not _enabled(window.bus_data_panel.rtex_box) - - -def test_bus_data_panels_are_disabled_while_disconnected(make_window): - window = _show(make_window()) - window.bus_data_panel.set_bus_type("ecat") - window.bus_data_panel.set_connected(False) - assert not _enabled(window.bus_data_panel.ecat_box) - - -def test_axis_rows_dim_when_the_bus_reports_fewer_drives(make_window): - window = _show(make_window()) - window.axis_panel.set_connected(True) - window.axis_panel.set_axis_count(3) - assert _enabled(window.axis_panel._rows[2]["enable_button"]) - assert not _enabled(window.axis_panel._rows[3]["enable_button"]) - - -def test_unknown_connection_modes_report_instead_of_pretending(make_window): - from orisys_app_shared.i18n import t - - window = _show(make_window()) - panel = window.connection_panel - index = panel.mode_combo.findData("com") - panel.mode_combo.setCurrentIndex(index) - panel._emit_connect() - assert panel.status_label.text() != t("connection.state_open") - - -# ------------------------------------------------------------------- actions - - -def test_the_selected_axis_follows_a_table_click(make_window): - window = _show(make_window()) - window.axis_panel._on_cell_clicked(3, 0) - assert window.axis_panel.selected_axis == 3 - assert window.axis_panel.axis_index_label.text() == "3" - - -def test_blank_numeric_fields_fall_back_to_defaults(make_window): - from orisys_autocal.motion import DEFAULT_UNITS - - window = _show(make_window()) - panel = window.axis_panel - panel.units_edit.setText("") - assert panel.current_params().units == DEFAULT_UNITS - panel.units_edit.setText("not a number") - assert panel.current_params().units == DEFAULT_UNITS - - -def test_a_blank_target_does_not_emit_a_move(make_window): - window = _show(make_window()) - emitted = [] - window.axis_panel.moveAbsRequested.connect(lambda axis, value: emitted.append(value)) - window.axis_panel.target_edit.setText("") - window.axis_panel._on_move_abs() - assert emitted == [] - - -def test_applying_parameters_persists_them_in_the_panel(make_window): - from orisys_autocal.motion import AxisParams - - window = _show(make_window()) - panel = window.axis_panel - panel.set_selected_axis(1) - panel.units_edit.setText("5") - panel.speed_edit.setText("250") - panel.accel_edit.setText("2500") - panel._on_apply() - stored = panel.stored_params()[1] - assert isinstance(stored, AxisParams) - assert stored.units == 5.0 - - -# --------------------------------------------------------------- preferences - - -def test_the_window_geometry_round_trips_through_preferences(make_window): - # Above the enforced minimum (1400x860): a smaller request would be clamped, - # which tests Qt's constraint rather than the preference round-trip. - window = _show(make_window(), (1520, 920)) - window._persist_prefs() - geometry = window.prefs["window_geometry"] - assert len(geometry) == 4 - assert geometry[2] == pytest.approx(1520, abs=5) - assert geometry[3] == pytest.approx(920, abs=5) - - -def test_a_malformed_geometry_is_ignored_rather_than_crashing(make_window): - window = _show(make_window(window_geometry=["nonsense"])) - assert window.width() > 0 - - -def test_axis_parameters_survive_a_persist_reload(make_window): - from orisys_autocal import preferences as prefs_mod - - window = _show(make_window()) - window.axis_panel.set_selected_axis(2) - window.axis_panel.units_edit.setText("7") - window.axis_panel._on_apply() - window._persist_prefs() - - reloaded = prefs_mod.load_preferences() - assert reloaded["axis_params"]["2"]["units"] == 7.0 - - -def test_the_bus_path_is_remembered(make_window): - window = _show(make_window()) - window.bus_panel.set_bas_path(r"D:\somewhere\ECAT.bas") - window._persist_prefs() - assert window.prefs["bas_path"] == r"D:\somewhere\ECAT.bas" - - -def test_sdo_fields_are_remembered(make_window): - window = _show(make_window()) - panel = window.bus_data_panel - panel.node_edit.setText("2") - panel.index_edit.setText("24608") - panel.subindex_edit.setText("1") - window._persist_prefs() - assert window.prefs["sdo_last"]["node"] == 2 - assert window.prefs["sdo_last"]["index"] == 24608 - - -# --------------------------------------------------------------- demo session - - -def test_a_demo_session_fills_the_table_and_gates_motion(make_window, qt_app): - """End to end through the real signal path: connect, initialise, jog.""" - from orisys_autocal.motion import ConnectionSpec - - window = _show(make_window()) - window.connect_controller(ConnectionSpec(mode="demo"), label="connect") - assert _wait_for(lambda: window._last_snapshot.connected, qt_app) - - # Download the shipped ECAT.bas, then wait for the simulated scan. - window.bus_panel.set_bas_path(str(_shipped_bas("ECAT.bas"))) - window._on_download_bas() - assert _wait_for(lambda: window._last_snapshot.bus.init_status == 1, qt_app, timeout=8.0) - - assert window._last_snapshot.bus.bus_type == "ecat" - assert _enabled(window.bus_data_panel.ecat_box) - assert _enabled(window.axis_panel._rows[0]["fwd_button"]) - # ATYPE is no longer a table column — the seven-column table shows position - # and status, and the drive's full frame is on the selected-axis detail line. - window.axis_panel.set_selected_axis(0) - assert "65" in window.axis_panel._detail_label.text() - pump(qt_app, 0.2) - - -def test_closing_the_window_stops_the_poller(make_window, qt_app): - window = _show(make_window()) - window.close() - pump(qt_app, 0.2) - assert not window.poller._thread.isRunning() - - -def _shipped_bas(name: str): - from orisys_autocal.ui.bus_panel import default_bas_dir - - return default_bas_dir() / name - - -def _wait_for(predicate, app, timeout: float = 5.0) -> bool: - import time - - deadline = time.monotonic() + timeout - while time.monotonic() < deadline: - app.processEvents() - if predicate(): - return True - time.sleep(0.02) - return False diff --git a/apps/autocal/tests/test_preferences_isolation.py b/apps/autocal/tests/test_preferences_isolation.py new file mode 100644 index 0000000..019730d --- /dev/null +++ b/apps/autocal/tests/test_preferences_isolation.py @@ -0,0 +1,64 @@ +"""Preferences must never be written outside the test scratch directory. + +The installed app reads ``apps/autocal/preference.json`` at startup. A test that +builds a window and closes it saves preferences through ``closeEvent``, so a test +that forgets to redirect the path rewrites the operator's real settings — which is +exactly what happened: the finger screen started up pointing at a scratch bundle +from a previous run and reported it as missing. + +``conftest._isolate_preference_file`` is the net. These tests pin it, because a +fixture that silently stopped applying would look like nothing at all until the app +was next launched by hand. +""" + +from __future__ import annotations + +from pathlib import Path + +APP_ROOT = Path(__file__).resolve().parents[1] +INSTALLED = APP_ROOT / "preference.json" + + +def test_the_preference_path_is_redirected_under_the_scratch_root(): + from orisys_autocal.preferences import path + + resolved = Path(path()).resolve() + scratch = (APP_ROOT / "outputs" / "_test_scratch").resolve() + assert resolved != INSTALLED.resolve() + assert scratch in resolved.parents, resolved + + +def test_saving_preferences_leaves_the_installed_file_alone(): + from orisys_autocal import preferences as prefs_mod + + before = INSTALLED.read_bytes() if INSTALLED.is_file() else None + + prefs = prefs_mod.default_preferences() + prefs["finger_bundle_path"] = str(APP_ROOT / "outputs" / "_test_scratch" / "gone") + written = Path(prefs_mod.save_preferences(prefs)).resolve() + + assert written != INSTALLED.resolve() + assert written.is_file(), "the save did not reach the redirected file either" + + after = INSTALLED.read_bytes() if INSTALLED.is_file() else None + assert after == before, "a test rewrote the installed preference.json" + + +def test_a_window_that_closes_still_lands_in_the_scratch_directory(qt_app): + """The failure mode itself: construct, close, check where it wrote.""" + from orisys_autocal import preferences as prefs_mod + from orisys_autocal.ui.window import AutoCalMainWindow + + before = INSTALLED.read_bytes() if INSTALLED.is_file() else None + + prefs = prefs_mod.default_preferences() + prefs["finger_bundle_path"] = str(APP_ROOT / "outputs" / "_test_scratch" / "gone") + window = AutoCalMainWindow(prefs=prefs, auto_load_bundle=False) + try: + window.poller.shutdown() + window.close() + finally: + window.poller.shutdown() + + after = INSTALLED.read_bytes() if INSTALLED.is_file() else None + assert after == before, "closing a test window rewrote the installed preferences" diff --git a/apps/autocal/tools/layout_mockup.py b/apps/autocal/tools/layout_mockup.py index 2e1d06c..eaf8fec 100644 --- a/apps/autocal/tools/layout_mockup.py +++ b/apps/autocal/tools/layout_mockup.py @@ -25,6 +25,7 @@ from __future__ import annotations import argparse import math +import math import os import sys from pathlib import Path @@ -41,8 +42,16 @@ if sys.platform == "win32" and "QT_QPA_FONTDIR" not in os.environ: if _fonts.is_dir(): os.environ["QT_QPA_FONTDIR"] = str(_fonts) -from PySide6.QtCore import QPoint, QRect, Qt # noqa: E402 -from PySide6.QtGui import QColor, QFont, QPainter, QPen, QPixmap, QPolygon # noqa: E402 +from PySide6.QtCore import QPoint, QPointF, QRect, Qt # noqa: E402 +from PySide6.QtGui import ( # noqa: E402 + QColor, + QFont, + QPainter, + QPainterPath, + QPen, + QPixmap, + QPolygon, +) from PySide6.QtWidgets import QApplication # noqa: E402 # ---------------------------------------------------------------- geometry --- @@ -157,7 +166,6 @@ STRINGS = { "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 缝隙上的像素会被拒绝。", @@ -166,21 +174,32 @@ STRINGS = { ["传感器 (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], + ["位姿 X / Y / yaw / pitch", "-2.841 0.940 -78.42 23.10", None], + ["工作台移动 X / Y", "+2.841 -0.940 mm", None], ["对轴残差", "0.03° 可对轴", "#12b76a"], ["坐标系映射", "x→x, y→z, z→-y", None], ], - "finger_plate_label": "安装板厚 (mm)", - "finger_plate_value": "0.000", + "finger_tool_label": "刀具 X / Y", + "finger_tool_values": ["0.000", "0.000"], + "finger_sensor_xy_label": "传感器 X / Y", + "finger_sensor_xy_values": ["0.000", "0.000"], + "finger_sensor_zy_label": "传感器 Z / yaw", + "finger_sensor_zy_values": ["0.000", "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_preview_checks": ["显示运动轴", "显示转台原点平面", "显示刀具路径"], + "finger_play": "播放动作", + "finger_preview_notes": [ + "颜色含义：绿→红 = 表面对齐残差；红/绿/蓝三色轴 = 工作台 X/Y/Z（位于原点）；橙点 = 工作台原点。", + "黄色直线 = 刀具轴（+Z，位置即刀具 X/Y）；黄色方框 = 传感器安装基面（高度 = 传感器 Z，含安装板）。", + "青色线 = 规划路径；紫圈 = 转台当前姿态、浅紫圈 = 结束姿态，与固定方格网对比即看出整个变换；白点+蓝线 = 接触点及法向。", ], + "finger_point_cols": ["sensor x", "sensor y", "sensor z", "nx", "ny", "nz", "X", "Y", "Z", "yaw", "pitch", "残差"], + "finger_point_rows": [ + ["2.841", "3.512", "0.940", "-0.18", "-0.88", "0.43", "2.841", "0.940", "7.958", "-78.42", "23.10", "0.03"], + ["5.113", "2.204", "-1.870", "0.21", "0.55", "-0.81", "5.113", "-1.870", "7.958", "12.05", "8.44", "0.02"], + ["3.902", "6.118", "2.405", "0.55", "0.19", "0.81", "3.902", "2.405", "7.958", "-90.00", "51.66", "0.02"], + ], + "finger_table_buttons": [("删除所选", False), ("上移", False), ("下移", False), ("清空标定点", False)], "finger_bad_word": "撞 yaw 行程限位", "finger_point_count": "3 个取点 · 展平 1024 × 1024 · 有效像素 72.9%", "sensor_note": "设备与信号源、实时图像、力曲线", @@ -284,7 +303,6 @@ STRINGS = { "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.", @@ -293,21 +311,33 @@ STRINGS = { ["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], + ["Pose X / Y / yaw / pitch", "-2.841 0.940 -78.42 23.10", None], + ["Table move X / Y", "+2.841 -0.940 mm", 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_tool_label": "Tool X / Y", + "finger_tool_values": ["0.000", "0.000"], + "finger_sensor_xy_label": "Sensor X / Y", + "finger_sensor_xy_values": ["0.000", "0.000"], + "finger_sensor_zy_label": "Sensor Z / yaw", + "finger_sensor_zy_values": ["0.000", "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_preview_checks": ["Show motion axes", "Show table origin plane", "Show tool path"], + "finger_play": "Play motion", + "finger_preview_notes": [ + "What the colours mean: green→red = the surface's alignment residual; the red/green/blue triad is the table's X/Y/Z at the origin; the orange dot is the origin.", + "Yellow line = the tool axis (+Z, at the tool's own X/Y); the amber outline = the sensor base plane, at the sensor's Z offset (the plate).", + "Cyan line = the planned path; the violet ring is the table's pose now and the faint one where it finishes, against the fixed grid that is where it started; white dot with blue line = the contact and its normal.", ], + "finger_preview_legend": "Surface colour = alignment residual (green aligned, red not) · yellow is the Z axis", + "finger_point_cols": ["sensor x", "sensor y", "sensor z", "nx", "ny", "nz", "X", "Y", "Z", "yaw", "pitch", "residual"], + "finger_point_rows": [ + ["2.841", "3.512", "0.940", "-0.18", "-0.88", "0.43", "2.841", "0.940", "7.958", "-78.42", "23.10", "0.03"], + ["5.113", "2.204", "-1.870", "0.21", "0.55", "-0.81", "5.113", "-1.870", "7.958", "12.05", "8.44", "0.02"], + ["3.902", "6.118", "2.405", "0.55", "0.19", "0.81", "3.902", "2.405", "7.958", "-90.00", "51.66", "0.02"], + ], + "finger_table_buttons": [("Remove selected", False), ("Move up", False), ("Move down", False), ("Clear points", False)], "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", @@ -352,6 +382,28 @@ def _font(size: int = 11, *, bold: bool = False) -> QFont: return font +def _arrowhead(painter: QPainter, path: QPainterPath, share: float, colour: QColor, size: int) -> None: + """An arrowhead on ``path`` at ``share``, pointing the way the path runs. + + The direction comes from two nearby points on the path rather than from + ``angleAtPercent``: that angle is measured counter-clockwise from +x while Qt's + y axis grows downward, and every attempt to reconcile the two by hand has + produced an arrow pointing backwards. + """ + tip = path.pointAtPercent(share) + ahead = path.pointAtPercent(min(1.0, share + 0.01)) + dx, dy = ahead.x() - tip.x(), ahead.y() - tip.y() + length = math.hypot(dx, dy) or 1.0 + ux, uy = dx / length, dy / length + painter.setPen(QPen(colour, 2)) + for sign in (1.0, -1.0): + barb = QPointF( + tip.x() - ux * size + sign * -uy * size * 0.6, + tip.y() - uy * size + sign * ux * size * 0.6, + ) + painter.drawLine(tip, barb) + + def _panel(painter: QPainter, rect: QRect, palette: Palette) -> QRect: painter.fillRect(rect, palette.panel) painter.setPen(QPen(palette.border, 1)) @@ -1008,45 +1060,141 @@ def draw_finger_tab(painter: QPainter, palette: Palette, s: dict, rect: QRect) - _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) + # The setup and tool fields: pairs of numbers, as in the app, where they are + # spin boxes two to a row because they get dialled in against the real machine. + pairs = ( + ("finger_tool_label", "finger_tool_values"), + ("finger_sensor_xy_label", "finger_sensor_xy_values"), + ("finger_sensor_zy_label", "finger_sensor_zy_values"), + ) + pair_y = row_y + 4 + for label_key, values_key in pairs: + _text(painter, QRect(inner_read.x(), pair_y, label_w, 20), s[label_key], palette, size=9, muted=True) + field_w = int((inner_read.width() - label_w - 6) / 2) + for index, value in enumerate(s[values_key][:2]): + _field( + painter, + QRect(inner_read.x() + label_w + index * (field_w + 6), pair_y, field_w, 20), + value, + palette, + ) + pair_y += 22 - # 3D preview: the tool axis and a normal with its residual. + # 3D preview: the machine's frame at the table origin, the sensor on it, and the + # planned path. Schematic, but the parts and their colours are the ones the real + # view draws — a proposal that disagrees with the app is worse than no proposal. 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) + checks_h = 20 + play_h = 28 + notes_h = 3 * 14 + 4 + view = QRect( + inner_preview.x(), + inner_preview.y() + 22, + inner_preview.width(), + inner_preview.height() - 22 - checks_h - play_h - notes_h, + ) 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. + + cx, cy = view.center().x(), view.center().y() + int(view.height() * 0.12) + # The table origin's plane: a grid, not a filled surface. + half = int(view.width() * 0.30) + floor = QRect(cx - half, cy - int(half * 0.26), 2 * half, int(half * 0.52)) + painter.setPen(QPen(QColor("#8c95ab"), 1)) + for index in range(5): + fx = floor.x() + index * floor.width() // 4 + fy = floor.y() + index * floor.height() // 4 + painter.drawLine(fx, floor.y(), fx, floor.bottom()) + painter.drawLine(floor.x(), fy, floor.right(), fy) + + # The sensor: the sensing face as an arc, the shell below it as a wireframe-ish + # outline. Grey, because the surface carries the residual colour instead. + dome_w = int(view.width() * 0.34) + dome_h = int(view.height() * 0.34) + body = QRect(cx - dome_w // 2, cy - dome_h, dome_w, dome_h + int(dome_h * 0.45)) + painter.setPen(QPen(QColor("#5b6273"), 1)) + painter.setBrush(QColor("#26b847")) + painter.drawChord(QRect(body.x(), body.y(), body.width(), body.height() * 2 // 3), 0, 180 * 16) + painter.setBrush(Qt.BrushStyle.NoBrush) + painter.drawRect(QRect(body.x(), body.y() + body.height() // 4, body.width(), body.height() // 2)) + + # The base plane, lifted off the origin by the sensor's z setup offset (the + # mounting plate), and the table's own frame: a circle at the pose it ends in, + # against the square grid that is where it started. + base = QRect(body.x() - 8, body.bottom() - 6, body.width() + 16, int(half * 0.30)) + painter.setPen(QPen(QColor("#fab829"), 2)) + painter.drawRect(base) + + ring = QRect(0, 0, int(half * 1.5), int(half * 0.78)) + ring.moveCenter(QPoint(cx + int(half * 0.30), cy + int(half * 0.05))) + painter.setPen(QPen(QColor("#8c6ff7"), 2)) + painter.setBrush(Qt.BrushStyle.NoBrush) + painter.drawEllipse(ring) + painter.drawLine(ring.center().x() - ring.width() // 2, ring.center().y(), + ring.center().x() + ring.width() // 2, ring.center().y()) + painter.drawLine(ring.center().x(), ring.center().y() - ring.height() // 2, + ring.center().x(), ring.center().y() + ring.height() // 2) + painter.setPen(QPen(QColor(140, 111, 247, 130), 2)) + painter.drawEllipse(ring.adjusted(-14, -7, -14, -7)) + + # The planned path: the stages swing the contact, then the table slides it in. + contact = QPoint(body.x() + 10, body.y() + int(body.height() * 0.28)) + landed = QPoint(cx + 6, cy - int(half * 0.30)) + path = QPainterPath(contact) + path.quadTo(QPoint(contact.x() + int(half * 0.75), contact.y() - int(half * 0.35)), landed) + painter.setPen(QPen(QColor("#40d1f2"), 2)) + painter.drawPath(path) + for share in (0.35, 0.7): + _arrowhead(painter, path, share, QColor("#ccf4ff"), 7) + painter.setBrush(QColor("#40d1f2")) + painter.setPen(Qt.PenStyle.NoPen) + painter.drawEllipse(landed, 5, 5) + + # The table triad at the origin: the only full-strength one in the picture. + origin = QPoint(floor.center().x(), floor.center().y() + int(half * 0.12)) for colour, end in ( - ("#e65454", QPoint(cx - 96, cy + 18)), - ("#61cc61", QPoint(cx + 30, cy + 92)), - ("#5c8cf2", QPoint(cx + 88, cy - 26)), + ("#e64c4c", QPoint(origin.x(), origin.y() + int(half * 0.62))), + ("#59c761", QPoint(origin.x() + int(half * 0.85), origin.y())), + ("#5787f2", QPoint(origin.x(), origin.y() - int(half * 0.42))), ): 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.drawLine(origin, end) + painter.setBrush(QColor("#fa9933")) + painter.setPen(Qt.PenStyle.NoPen) + painter.drawEllipse(origin, 5, 5) 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) + + # The tool axis, through the point the tool touches. + painter.setPen(QPen(QColor("#f2e04c"), 2)) + painter.drawLine(QPoint(landed.x(), view.y() + 16), QPoint(landed.x(), landed.y())) + _text(painter, QRect(landed.x() - 8, view.y() + 2, 90, 16), "+Z", palette, size=8, colour=QColor("#f2e04c")) + + # The picked contact and its normal, and the point selected in the table. + painter.setPen(QPen(QColor("#59a6ff"), 2)) + painter.drawLine(contact, QPoint(contact.x() - int(half * 0.30), contact.y() - int(half * 0.34))) + painter.setPen(QPen(QColor("#ffffff"), 2)) + painter.setBrush(QColor("#ffffff")) + painter.drawEllipse(contact, 4, 4) + painter.setPen(Qt.PenStyle.NoPen) + painter.setBrush(QColor("#eb2133")) + painter.drawEllipse(QPoint(contact.x() + 14, contact.y() + 4), 5, 5) + painter.setBrush(Qt.BrushStyle.NoBrush) + + checks = s["finger_preview_checks"] + check_w = inner_preview.width() // max(1, len(checks)) + for index, label in enumerate(checks): + box = QRect(inner_preview.x() + index * check_w, view.bottom() + 2, check_w, checks_h) + painter.setPen(QPen(palette.accent, 2)) + painter.drawLine(box.x() + 2, box.y() + 9, box.x() + 5, box.y() + 13) + painter.drawLine(box.x() + 5, box.y() + 13, box.x() + 10, box.y() + 5) + _text(painter, QRect(box.x() + 15, box.y(), check_w - 15, checks_h), label, palette, size=9, muted=True) + + play = QRect(inner_preview.x(), view.bottom() + checks_h + 2, 110, play_h - 4) + _button(painter, play, s["finger_play"], palette, primary=True) + notes_y = play.bottom() + 4 + for text in s["finger_preview_notes"]: + _text(painter, QRect(inner_preview.x(), notes_y, inner_preview.width(), 14), text, palette, size=8, muted=True) + notes_y += 14 # Picked points. table = QRect(inner.x(), y + body_h + 8, inner.width(), max(60, table_h)) @@ -1055,9 +1203,16 @@ def draw_finger_tab(painter: QPainter, palette: Palette, s: dict, rect: QRect) - painter.drawRect(table.adjusted(0, 0, -1, -1)) header_h = 24 painter.fillRect(QRect(table.x(), table.y(), table.width(), header_h), palette.panel) + # Leading select column, then the data columns: the real table has a checkbox + # per row so points can be picked out, edited, reordered or removed. + select_w = 34 + painter.setPen(QPen(palette.border, 1)) + painter.drawLine(table.x() + select_w, table.y(), table.x() + select_w, table.bottom()) 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() + widths = [int((table.width() - select_w) * share) + for share in (0.09, 0.1, 0.09, 0.09, 0.08, 0.08, 0.08, 0.11, + 0.07, 0.07, 0.07, 0.07)] + x = table.x() + select_w 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 @@ -1065,12 +1220,28 @@ def draw_finger_tab(painter: QPainter, palette: Palette, s: dict, rect: QRect) - 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() + # Tick the first row, as the selected point being tested. + box = QRect(table.x() + select_w // 2 - 6, row_y + 8, 12, 12) + painter.setPen(QPen(palette.muted, 1)) + painter.setBrush(Qt.BrushStyle.NoBrush) + painter.drawRect(box) + if index == 0: + painter.setPen(QPen(palette.accent, 2)) + painter.drawLine(box.x() + 2, box.y() + 6, box.x() + 5, box.y() + 10) + painter.drawLine(box.x() + 5, box.y() + 10, box.x() + 10, box.y() + 3) + x = table.x() + select_w 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) + buttons = s["finger_table_buttons"] + bx = table.x() + by = table.bottom() + 26 + for label, primary in buttons: + width = 30 + 8 * len(label) + _button(painter, QRect(bx, by, width, 26), label, palette, primary=primary) + bx += width + 8 TAB_PAINTERS = { diff --git a/apps/autocal/tools/panel_check.py b/apps/autocal/tools/panel_check.py index 6662580..1acf06e 100644 --- a/apps/autocal/tools/panel_check.py +++ b/apps/autocal/tools/panel_check.py @@ -30,6 +30,17 @@ if sys.platform == "win32" and "QT_QPA_FONTDIR" not in os.environ: if _fonts.is_dir(): os.environ["QT_QPA_FONTDIR"] = str(_fonts) +# The report quotes labels, and half of them are Chinese. A Windows console +# defaults to a legacy code page that cannot encode them, so the run ended in a +# UnicodeEncodeError *after* every check had passed — the worst kind of noise. +for _stream in (sys.stdout, sys.stderr): + _reconfigure = getattr(_stream, "reconfigure", None) + if _reconfigure is not None: + try: + _reconfigure(encoding="utf-8", errors="replace") + except (ValueError, OSError): # pragma: no cover - stream already detached + pass + RESOLUTIONS = ((1440, 900), (1920, 1080)) @@ -394,6 +405,10 @@ def main() -> int: ) window.workspace.set_current_key("axes") + try: + window.poller.shutdown() + except Exception: # noqa: BLE001 - a shutdown must not mask the result + pass window.close() app.processEvents() diff --git a/apps/autocal/tools/preview_snapshot.py b/apps/autocal/tools/preview_snapshot.py new file mode 100644 index 0000000..9089d3f --- /dev/null +++ b/apps/autocal/tools/preview_snapshot.py @@ -0,0 +1,492 @@ +#!/usr/bin/env python3 +"""Render the finger page's 3D preview to a PNG, using a real GL context. + + python apps/autocal/tools/preview_snapshot.py --out shot.png + +``tools/panel_snapshot.py`` renders the whole window with the *offscreen* Qt +platform, which cannot create an OpenGL context at all (``QOpenGLWidget is not +supported on this platform``), so the preview pane in those shots is empty. This +tool is the one that can photograph the 3D view, and it needs the real platform +plugin and a visible window: Qt only builds a QOpenGLWidget's framebuffer once the +widget has been shown and painted. + +It therefore opens the window briefly, picks a point on the sensing surface, and +reads the framebuffer back. Options: + + --points 3 pick several points, so the table and the red dot show + --play 0.5 run the motion simulation and capture it at 50% + --view initial reset the camera before the shot + --whole save the whole window instead of just the GL pane +""" + +from __future__ import annotations + +import argparse +import os +import sys +from pathlib import Path + +APP_ROOT = Path(__file__).resolve().parents[1] +for path in (APP_ROOT / "src", APP_ROOT.parents[1] / "shared" / "src"): + if str(path) not in sys.path: + sys.path.insert(0, str(path)) + +os.environ.setdefault("ORISYS_APP_ID", "autocal") +# The offscreen plugin cannot make a GL context, so drop it if the caller's +# environment set it (the test suite does). +if os.environ.get("QT_QPA_PLATFORM") == "offscreen": + del os.environ["QT_QPA_PLATFORM"] +# Force the native desktop GL backend. Qt's default choice on Windows has been seen +# to hand out a context that validates but rejects every command — a bare +# QOpenGLWidget with no application code fails the same way, with +# GL_INVALID_OPERATION on the first call after ``makeCurrent`` — and every render +# then comes out blank with no clue why. ``setdefault``, so an explicit choice in +# the environment still wins. +os.environ.setdefault("QT_OPENGL", "desktop") + +def main() -> int: + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--theme", default="dark", choices=("dark", "light")) + parser.add_argument("--language", default="zh_CN", choices=("zh_CN", "en_US")) + parser.add_argument("--size", default="1180x760", help="WIDTHxHEIGHT of the window") + parser.add_argument( + "--pane", + default="", + metavar="WIDTHxHEIGHT", + help="force the 3D pane to this size before the shot, for a readable view", + ) + parser.add_argument("--out", default="preview.png") + parser.add_argument( + "--bundle", + default="", + help="bundle directory to load; defaults to the remembered preference", + ) + parser.add_argument("--points", type=int, default=1, help="how many points to pick") + parser.add_argument( + "--setup", + default="", + metavar="X,Y,Z,YAW", + help="sensor mounting pose: mm in the table frame plus the yaw in degrees", + ) + parser.add_argument( + "--tool", + default="", + metavar="X,Y", + help="tool position in the table frame, mm", + ) + parser.add_argument( + "--play", + type=float, + default=None, + metavar="SHARE", + help="start the motion simulation and capture it after this share, 0..1", + ) + parser.add_argument( + "--view", + default="", + choices=("", "initial"), + help="initial resets the camera before the shot", + ) + parser.add_argument("--tab", default="finger", help="workspace tab to open first") + parser.add_argument( + "--whole", action="store_true", help="save the whole window, not just the pane" + ) + parser.add_argument( + "--debug", action="store_true", help="report the GL state of the pane" + ) + args = parser.parse_args() + + from PySide6.QtWidgets import QApplication + + from orisys_app_shared.i18n import set_locale + from orisys_app_shared.theme import build_qss + + from orisys_autocal.preferences import load_preferences + from orisys_autocal.ui.pose_preview import configure_compatible_gl + from orisys_autocal.ui.window import AutoCalMainWindow + + width, height = (int(part) for part in args.size.lower().split("x")) + + set_locale(args.language) + app = QApplication.instance() or QApplication([]) + app.setApplicationName("Orisys AutoCal") + # Must follow QApplication and precede the first GL surface. + configure_compatible_gl() + app.setStyleSheet(build_qss(args.theme)) + + prefs = load_preferences() + prefs["theme"] = args.theme + prefs["language"] = args.language + prefs["show_diagnostics"] = False + + window = AutoCalMainWindow(prefs=prefs) + window.workspace.set_current_key(args.tab) + window.show() + # Resized *after* showing: the window restores its remembered geometry on + # construction, so a resize before ``show`` is silently overwritten and every + # shot comes out at whatever size the operator last used. + window.resize(width, height) + _settle(app, 0.5) + + if args.bundle: + window.load_bundle(args.bundle) + _settle(app, 0.6) + + panel = window.finger_panel + if panel.view.flat is None: + print("no bundle loaded; set finger_bundle_path in preferences", file=sys.stderr) + _finish(app, window) + return 1 + if args.setup: + numbers = [float(part) for part in args.setup.split(",")] + while len(numbers) < 4: + numbers.append(0.0) + panel.set_sensor_setup(tuple(numbers[:3]), numbers[3]) + _settle(app, 0.2) + if args.tool: + numbers = [float(part) for part in args.tool.split(",")] + while len(numbers) < 2: + numbers.append(0.0) + panel.set_tool_xy(numbers[0], numbers[1]) + _settle(app, 0.2) + if args.pane: + pane_w, pane_h = (int(part) for part in args.pane.lower().split("x")) + panel.preview.setFixedSize(pane_w, pane_h) + _settle(app, 0.3) + if args.view == "initial": + panel.preview.reset_view() + + for x, y in _pick_pixels(panel, max(1, args.points)): + panel.handle_pick(x, y) + _settle(app, 0.25) + + if args.play is not None: + panel.preview.start_simulation() + _run_simulation_to(app, panel.preview, args.play) + _settle(app, 0.35) + + # The pane is hidden for the read-back, and only for the read-back. A visible + # QOpenGLWidget is painted by the window system, which a session with no + # interactive desktop never does, so its framebuffer stays blank; hidden, Qt + # renders on demand for grabFramebuffer and the picture is real. Hiding it + # afterwards does not disturb the layout, so the corner is taken first. + from PySide6.QtCore import QPoint + + corner = panel.preview.mapTo(window, QPoint(0, 0)) + pane_size = panel.preview.size() + panel.preview.hide() + _settle(app, 0.25) + try: + gl_image = _grab_gl(app, panel.preview) + finally: + panel.preview.show() + panel.preview.resize(pane_size) + _settle(app, 0.2) + if args.debug: + context = panel.preview.context() + summary = panel.preview.trajectory_summary() + print( + f"debug: platform={app.platformName()} valid={panel.preview.isValid()} " + f"size={panel.preview.width()}x{panel.preview.height()} " + f"tab={window.workspace.current_key()} " + f"context={'yes' if context is not None else 'none'} " + f"problem={panel.preview.problem!r} " + f"image={'none' if gl_image is None else f'{gl_image.width()}x{gl_image.height()}'}", + file=sys.stderr, + ) + print( + f"debug: trajectory={summary or 'empty'} " + f"show={panel.preview.show_trajectory} table={panel.preview.show_table} " + f"axes={panel.preview.show_axes} " + f"scene_radius={panel.preview.scene_radius():.2f}", + file=sys.stderr, + ) + print( + f"debug: heights panel={panel.height()} preview={panel.preview.height()} " + f"point_table={panel.table.height()} window={window.height()}", + file=sys.stderr, + ) + # Where the claims land on screen: the path's end must project onto the tool, + # and the contact onto both. Printed because "they look close in the picture" + # is exactly the judgement this tool exists to replace. + path = panel.preview.trajectory() + if path.size: + tool = panel.preview.tool_point() + gap = float(sum((a - b) ** 2 for a, b in zip(path[-1], tool)) ** 0.5) + screen = panel.preview.project(tool) + print( + f"debug: path end at {panel.preview.project(path[-1])} " + f"tool at {screen} offscreen {gap:.6f} mm", + file=sys.stderr, + ) + # The projection says where the tool line *is* on screen; check the + # pixels, because a projection that disagrees with the render looks + # perfectly healthy on its own — that is how the labels drifted off + # their arrows and the scene sat off to one side without a word. + from orisys_autocal.ui.pose_preview import COLOUR + + for label, point, colour in _landmarks(panel): + screen = panel.preview.project(point) + print( + f"debug: {label} at {screen} -> " + f"{_find_colour(gl_image, screen, colour)} matching pixels", + file=sys.stderr, + ) + # The two markers must be the same point: the contact is what is solved for, + # the red dot is the row selected in the table, and they are the same pick. + contact = panel.preview._point_mm # noqa: SLF001 + highlight = panel.preview._highlight_mm # noqa: SLF001 + pose = panel.preview._pose # noqa: SLF001 + stack = panel.preview.stack + print( + f"debug: share={panel.preview.motion_share()} " + f"simulating={panel.preview.simulating} " + f"elapsed={panel.preview._sim_elapsed:.0f} " # noqa: SLF001 + f"pose={'none' if pose is None else pose.describe()}", + file=sys.stderr, + ) + if contact is not None: + drawn = panel.preview._placed(contact) # noqa: SLF001 + tool = panel.preview.tool_point() + print( + f"debug: drawn contact {[round(value, 4) for value in stack.mount @ drawn]} " + f"tool {[round(value, 4) for value in stack.mount @ tool]} " + f"gap {float(sum((a - b) ** 2 for a, b in zip(drawn, tool)) ** 0.5):.6f} mm", + file=sys.stderr, + ) + if gl_image is None: + print( + f"the GL pane produced no framebuffer (valid={panel.preview.isValid()}, " + f"size={panel.preview.width()}x{panel.preview.height()}, " + f"backend={os.environ.get('QT_OPENGL', 'default')}, " + f"problem={panel.preview.problem!r})", + file=sys.stderr, + ) + _finish(app, window) + return 1 + + if args.whole: + image = window.grab() + _paint_pane(image, corner, gl_image) + else: + image = gl_image + + output = Path(args.out).resolve() + output.parent.mkdir(parents=True, exist_ok=True) + if not image.save(str(output)): + print(f"could not write {output}", file=sys.stderr) + _finish(app, window) + return 1 + print( + f"wrote {output} ({image.width()}x{image.height()}, pane={gl_image.width()}" + f"x{gl_image.height()}, theme={args.theme}, points={panel.table.rowCount()}, " + f"play={args.play})" + ) + _finish(app, window) + return 0 + + +def preview_place(panel, point): + """A sensor point as the preview draws it right now.""" + return panel.preview._placed(point) # noqa: SLF001 + + +def _landmarks(panel): + """``(name, mesh point, palette colour)`` for pixels that identify themselves. + + Each pair is a point whose position is known from the model and an element drawn + there in a colour nothing else uses, so "is the projection telling the truth" + becomes a pixel count instead of an opinion. + """ + from orisys_autocal.sensor.frames import to_sensor + from orisys_autocal.ui.pose_preview import COLOUR + + preview = panel.preview + frame = preview.table_frame() + tool = preview.tool_point() + direction = to_sensor(preview.stack.tool_axis_direction, preview.stack.axis_map) + length = float((direction[0] ** 2 + direction[1] ** 2 + direction[2] ** 2) ** 0.5) or 1.0 + direction = direction / length + found = [ + # Unoccluded by construction: the origin dot is drawn last with depth off. + ("origin", frame["origin"], COLOUR["origin"]), + ] + # The tool line is depth-tested, so parts of it are behind the body: try several + # heights along it and take the best, rather than insisting on one that happens + # to be visible in one particular pose. + for offset_mm in (12.0, 9.0, 6.0, 3.0): + found.append( + ( + f"tool line +{offset_mm:.0f}mm", + tool + direction * offset_mm, + COLOUR["tool_axis"], + ) + ) + ring = preview.table_marker_points() + if ring.size: + found.append(("table marker", ring[0], COLOUR["table_marker"])) + return found + + +def _find_colour(image, screen, colour, radius: int = 6) -> int: + """How many pixels near ``screen`` match ``colour``, within a small tolerance. + + The check that ties the projection to the picture. Deliberately tolerant about + position (a few pixels) and strict about colour, because the colour is what says + *which* element is there: finding the tool-axis yellow near where the tool is + claimed to be is the difference between a working projection and a plausible one. + """ + if screen is None or image is None: + return -1 + from PySide6.QtGui import QColor + + wanted = QColor( + int(colour[0] * 255), int(colour[1] * 255), int(colour[2] * 255) + ) + found = 0 + for dy in range(-radius, radius + 1): + for dx in range(-radius, radius + 1): + x, y = int(round(screen[0])) + dx, int(round(screen[1])) + dy + if not (0 <= x < image.width() and 0 <= y < image.height()): + continue + pixel = image.pixelColor(x, y) + if ( + abs(pixel.red() - wanted.red()) <= 24 + and abs(pixel.green() - wanted.green()) <= 24 + and abs(pixel.blue() - wanted.blue()) <= 24 + ): + found += 1 + return found + + +def _finish(app, window) -> None: + """Close the window, stopping the poller thread first. + + Without the explicit shutdown Qt tears the thread down with the process and + prints "QThread: Destroyed while thread is still running", which reads like a + fault in the app rather than a tool that left in a hurry. + """ + try: + window.poller.shutdown() + except Exception: # noqa: BLE001 - a shutdown must not mask the result + pass + window.close() + app.processEvents() + + +def _grab_gl(app, preview, attempts: int = 60): + """The GL pane's framebuffer, once it actually has something drawn in it. + + ``grabFramebuffer`` returns a valid but uniformly blank image when the widget + has not been painted yet, which is easy to mistake for a rendering bug — the + first version of this tool saved a white rectangle and the pane looked broken. + So it is called repeatedly until the pixels differ from one another. + """ + import time + + grab = getattr(preview, "grabFramebuffer", None) + last = None + for _ in range(attempts): + app.processEvents() + preview.update() + app.processEvents() + if grab is not None: + try: + image = grab() + except Exception as error: # noqa: BLE001 - reported below + print(f"grabFramebuffer failed: {error}", file=sys.stderr) + break + if image is not None and not image.isNull(): + last = image + if _has_content(image): + return image + time.sleep(0.05) + if last is not None and _has_content(last): + return last + # Fall back to a plain widget grab only when there is no GL context at all; + # a blank framebuffer is reported instead of saved as a picture of nothing. + if not preview.isValid(): + return preview.grab() + return None + + +def _has_content(image) -> bool: + """True when the image is not one flat colour, i.e. something was drawn.""" + small = image.scaled(24, 24) + first = small.pixel(0, 0) + for y in range(small.height()): + for x in range(small.width()): + if small.pixel(x, y) != first: + return True + return False + + +def _paint_pane(image, corner, pane) -> None: + """Draw the GL content into a whole-window grab, where Qt leaves it blank. + + ``QWidget.grab`` on a window containing a ``QOpenGLWidget`` does not composite + the GL surface, so the pane comes out empty. Painting the framebuffer in at the + widget's own position is what makes the page shot show what the operator sees. + """ + from PySide6.QtGui import QPainter + + painter = QPainter(image) + try: + painter.drawImage(corner, pane) + finally: + painter.end() + + +def _pick_pixels(panel, count: int) -> list[tuple[int, int]]: + """UV-flat pixels that land on the surface, spread around the island. + + Sampled from the flat view rather than hard-coded, so the shot follows the + bundle instead of a coordinate that was right for one revision. + """ + view = panel.view + flat = view.flat + if flat is None: + return [] + found: list[tuple[int, int]] = [] + fractions = [(0.5, 0.5), (0.35, 0.45), (0.65, 0.55), (0.5, 0.3), (0.5, 0.7)] + for share_x, share_y in fractions: + if len(found) >= count: + break + x = int(flat.width * share_x) + y = int(flat.height * share_y) + if flat.sample(x, y) is not None: + found.append((x, y)) + return found + + +def _settle(app, seconds: float) -> None: + import time + + deadline = time.monotonic() + seconds + while time.monotonic() < deadline: + app.processEvents() + time.sleep(0.01) + + +def _run_simulation_to(app, preview, share: float) -> None: + """Let the simulation run until ``share`` of it has played. + + Driven off the widget's own clock — the animation is a QTimer, and stepping the + event loop is what advances it. Reading the elapsed time rather than sleeping a + fixed duration keeps the shot reproducible when the frame budget differs. + """ + import time + + from orisys_autocal.ui.pose_preview import SIM_DURATION_MS + + target = SIM_DURATION_MS * max(0.0, min(1.0, float(share))) / 1000.0 + deadline = time.monotonic() + target + 5.0 + while time.monotonic() < deadline: + app.processEvents() + if getattr(preview, "_sim_elapsed", 0.0) / 1000.0 >= target: # noqa: SLF001 + break + time.sleep(0.005) + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/shared/src/orisys_app_shared/theme/qss.py b/shared/src/orisys_app_shared/theme/qss.py index 51166a2..d1796cf 100644 --- a/shared/src/orisys_app_shared/theme/qss.py +++ b/shared/src/orisys_app_shared/theme/qss.py @@ -393,6 +393,15 @@ QComboBox:disabled, QLineEdit:disabled, QSpinBox:disabled, QDoubleSpinBox:disabl background-color: {c['window_bg']}; border-color: {c['separator']}; }} +/* A field that sits several to a row. The padding above is what makes an input + 40 px tall rather than 28, which is right for a form and too tall for a grid of + paired numbers — the pair rows overflow their cells and clip. Same look, + tighter box: opt in with ``setObjectName("compactField")``. */ +QComboBox#compactField, QLineEdit#compactField, +QSpinBox#compactField, QDoubleSpinBox#compactField {{ + padding: 1px 4px; + min-height: 20px; +}} QComboBox::drop-down {{ subcontrol-origin: padding; subcontrol-position: top right;