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