orisys-tech/orisys-app
3
0
Fork 0
Code Issues 2 Pull Requests Actions Packages Projects Releases Wiki Activity

added motion trajectory simulation

Browse Source
This commit is contained in:
ykcheungab
2026-09-23 09:58:00 +08:00
parent f1bec62ca4
commit 3347d17a20
25 changed files with 5198 additions and 1762 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/autocal/src/orisys_autocal/main.py
+12
View File
@@ -51,6 +51,17 @@ def _parse_args(argv: list[str]) -> argparse.Namespace:
return parser.parse_args(argv)
def _request_compatible_gl() -> bool:
"""Ask for a GL profile the 3D preview can draw with; see the preview module.
Thin wrapper so the entry point does not have to know the details — the
reasoning and the version choice live next to the drawing code that needs them.
"""
from .ui.pose_preview import configure_compatible_gl
return configure_compatible_gl()
def main(argv: list[str] | None = None) -> int:
args = _parse_args(list(argv if argv is not None else sys.argv[1:]))
_force_utf8_streams()
@@ -80,6 +91,7 @@ def main(argv: list[str] | None = None) -> int:
app = QApplication(sys.argv[:1])
app.setApplicationName("Orisys AutoCal")
app.setApplicationVersion(app_version)
_request_compatible_gl()
app.setStyleSheet(build_qss(theme))
transport = None
apps/autocal/src/orisys_autocal/motion/kinematics.py
+432 -66
View File
@@ -94,17 +94,47 @@ def rotation_about(axis: str, degrees: float) -> np.ndarray:
raise ValueError(f"unknown axis {axis!r}")
@dataclass(frozen=True)
class Branch:
"""One exact closed-form solution of the alignment, before it is chosen.
Both branches put the normal on the tool axis to within floating point, so
what separates them is the machine: whether the pose is inside the stage
travel, and which way up it leaves the sensing face.
"""
yaw_deg: float
pitch_deg: float
residual_deg: float
upright: bool
in_range: bool
def describe(self) -> str:
return (
f"yaw {self.yaw_deg:+.3f} pitch {self.pitch_deg:+.3f} "
f"residual {self.residual_deg:.6f} "
f"{'upright' if self.upright else 'inverted'}"
f"{'' if self.in_range else ' out-of-range'}"
)
@dataclass(frozen=True)
class Pose:
"""One solved platform pose, or the reason there is not one."""
ok: bool
#: Platform-frame coordinates of the contact point, mm.
#: Platform-frame coordinates of the contact point, mm — where the point *is*,
#: with the sensor's mounting pose applied.
target_xy_mm: tuple[float, float] = (0.0, 0.0)
#: Platform-frame Z of the contact point, mm. Carries the mounting plate and
#: any Z tool offset, which ``target_xy_mm`` cannot: without it those inputs
#: are computed and then discarded, and nothing downstream can tell.
contact_z_mm: float = 0.0
#: The move the XY table has to make, mm: the tool's position minus where the
#: contact ends up after the stages turn. This is the number a run commands,
#: and it is what the preview animates — ``target_xy_mm`` says where the point
#: is, this says what the machine does about it.
table_xy_mm: tuple[float, float] = (0.0, 0.0)
yaw_deg: float = 0.0
pitch_deg: float = 0.0
#: The separate Z axis. Reported, never solved for here.
@@ -113,6 +143,13 @@ class Pose:
residual_deg: float = 0.0
#: Whether the yaw could be determined at all (indeterminate at the pole).
yaw_determined: bool = True
#: Whether the pose leaves the sensing face pointing along the tool axis. A
#: pose can align the normal perfectly and still be turned over; the residual
#: alone cannot say which, so it is reported rather than inferred.
upright: bool = True
#: False when the chosen pose is outside the stage travel — reported, never
#: silently clamped.
in_range: bool = True
reason: str = ""
@property
@@ -134,7 +171,8 @@ class Pose:
yaw = f"{self.yaw_deg:+.3f}" if self.yaw_determined else "indeterminate"
return (
f"X {self.target_xy_mm[0]:+.3f} Y {self.target_xy_mm[1]:+.3f} "
f"Z {self.contact_z_mm:+.3f} yaw {yaw} pitch {self.pitch_deg:+.3f} "
f"Z {self.contact_z_mm:+.3f} move {self.table_xy_mm[0]:+.3f} "
f"{self.table_xy_mm[1]:+.3f} yaw {yaw} pitch {self.pitch_deg:+.3f} "
f"residual {self.residual_deg:.4f} deg"
)
@@ -147,23 +185,51 @@ class PlatformStack:
yaw_axis: str = YAW_AXIS
pitch_axis: str = PITCH_AXIS
tool_axis: str = TOOL_AXIS
#: Which way the tool pushes along ``tool_axis`` relative to the patch
#: normal the mesh stores. The mesh's normal points *away* from the sensing
#: surface, and the tool presses *into* it, so the tool direction is the
#: negative of the platform-frame normal — hence ``-1`` by default. Getting
#: this wrong does not look wrong: it silently gives a ~90 degree residual on
#: a patch that is actually fine, which is how it was found.
approach_sign: int = -1
#: Added to the reported contact point, mm, in the platform frame. The tool
#: tip offset and any fixture height the caller wants folded in.
tool_offset_mm: tuple[float, float, float] = (0.0, 0.0, 0.0)
#: Which way along ``tool_axis`` the sensing face is aimed, relative to the
#: normal the mesh stores. It is ``+1``: the sensor sits at the bottom of its
#: shell with the face up, so the face — and the normal — point along
#: ``+tool_axis``, and the tool feeds down onto it.
#:
#: This was ``-1``, which asked the solver to bring every normal onto **-Z**.
#: The residual could not see the difference (``arccos`` is sign-blind, so a
#: face turned fully over still scored 0.0000 deg), so the mistake showed up
#: only as the model flipping upside down: **981 of 1032 vertices** ended with
#: the face pointing down. With ``+1`` that is **51**, and those are edge-on
#: rather than inverted (worst cosine -0.013).
approach_sign: int = 1
#: Where the tool is, in the table frame, mm. Default ``(0, 0)`` — straight
#: above the table origin, which is where a dialled-in machine has it. Every
#: XY move exists to bring the contact to this point, so the move is measured
#: against it. It used to be taken as the patch's centre, which was a stand-in
#: that made the numbers agree with the picture and with nothing on the bench.
#:
#: A tool-tip offset is *this number*, not a second one. It was a separate
#: vector, added to the reported coordinates while the drawing left the geometry
#: alone: a machine with a 10 mm tip offset reported one landing point and drew
#: another, 10 mm away — which is exactly what "x and y are not aligning" looked
#: like on the bench. Two spellings of "where the tool is" is one too many.
tool_xy_mm: tuple[float, float] = (0.0, 0.0)
#: How the sensor actually sits on the stack when the machine is at zero:
#: a translation along the table's x/y/z, mm, and a yaw about the table's z.
#: Tuned per setup, because a mount that is square to within a degree on the
#: drawing is not square on the bench.
#:
#: **z carries the mounting plate.** The plate lifts the sensing face along
#: the table's z and does nothing else, so a second input for it would be a
#: second spelling of the same number — and the two would drift.
sensor_offset_mm: tuple[float, float, float] = (0.0, 0.0, 0.0)
#: The sensor's mounting yaw about the table's z, degrees. It is a *setup*
#: angle, not a stage angle: the yaw stage has to turn this much less.
sensor_yaw_deg: float = 0.0
#: The residual a consumer will accept; a pose worse than this is still
#: returned, but flagged.
tolerance_deg: float = ALIGNED_TOL_DEG
#: Travel limits of the two stages, degrees. A solution outside them is not a
#: solution: the search is constrained to the envelope the hardware has.
#: The bench's figures: pitch turns continuously (360°), yaw is ±90°.
yaw_limit_deg: float = 90.0
#: The bench's figures, confirmed against the trunnion drawing: **yaw about the
#: vertical turns ±180** (a full half-turn either way) and pitch about the
#: horizontal turns a full 360.
yaw_limit_deg: float = 180.0
pitch_limit_deg: float = 360.0
def __post_init__(self) -> None:
@@ -181,27 +247,95 @@ class PlatformStack:
@property
def tool_direction(self) -> np.ndarray:
"""The unit vector the tool pushes along, in the platform frame."""
"""The unit vector the sensing face is aimed along, in the platform frame.
This is what the solver aligns the normal to, and it is ``+tool_axis``
with the bench's ``approach_sign``: the face is the top surface of a sensor
mounted at the bottom of its shell, so the face points up.
"""
direction = np.zeros(3, dtype=np.float64)
direction[("x", "y", "z").index(self.tool_axis)] = float(self.approach_sign)
return direction
@property
def tool_axis_direction(self) -> np.ndarray:
"""``+tool_axis``: the same direction, named for the drawing.
Kept separate from :attr:`tool_direction` so a change of polarity cannot
silently reverse the arrow in the preview: with ``approach_sign = +1`` the
two coincide, and if the polarity were ever flipped they would not, which
is exactly when someone needs to notice.
"""
direction = np.zeros(3, dtype=np.float64)
direction[("x", "y", "z").index(self.tool_axis)] = 1.0
return direction
def normal_to_platform(self, normal_sensor: np.ndarray) -> np.ndarray:
return to_platform(np.asarray(normal_sensor, dtype=np.float64), self.axis_map)
def point_to_platform(
self, point_sensor_mm: np.ndarray, *, plate_thickness_mm: float = 0.0
) -> np.ndarray:
"""A sensor-frame point in the platform frame.
"""A sensor-frame point in the platform frame, **ignoring the setup**.
The plate lifts the sensor along the platform's tool axis, so it is a
translation along :data:`tool_axis` applied after the mount rotation.
translation along :data:`tool_axis` applied after the mount rotation. For
where the point really is once the sensor is mounted, use
:meth:`setup_point` — which is what the solver uses.
"""
platform = to_platform(np.asarray(point_sensor_mm, dtype=np.float64), self.axis_map)
lift = np.zeros(3, dtype=np.float64)
lift[("x", "y", "z").index(self.tool_axis)] = float(plate_thickness_mm)
return platform + lift
def setup_rotation(self) -> np.ndarray:
"""``R_z(sensor_yaw)``: the sensor's mounting yaw, in the table frame.
A rotation about the table's z through the table origin, applied to the
mounted sensor *before* the stages turn it. It commutes with the yaw stage
and not with the pitch stage, which is why it is applied here rather than
folded into the reported yaw: the two angles mean different things and the
machine has to drive only one of them.
"""
if not np.isfinite(self.sensor_yaw_deg):
raise ValueError(f"unknown sensor_yaw_deg {self.sensor_yaw_deg!r}")
return rotation_about("z", float(self.sensor_yaw_deg))
def setup_point(
self, point_sensor_mm: np.ndarray, *, plate_thickness_mm: float = 0.0
) -> np.ndarray:
"""A sensor-frame point where the mounted sensor actually puts it, mm.
``p_table = R_z(sensor_yaw) · (mount · p + plate·ẑ) + sensor_offset``:
the mount rotation, the plate lift, the setup yaw about the table origin,
then the setup translation. The plate lift is along the table's z, which
the setup yaw leaves alone, so the two can be applied in either order —
the same reason the plate is a z offset and not a separate concept.
"""
platform = self.point_to_platform(
point_sensor_mm, plate_thickness_mm=plate_thickness_mm
)
return self.setup_rotation() @ platform + np.asarray(
self.sensor_offset_mm, dtype=np.float64
)
def setup_normal(self, normal_sensor: np.ndarray) -> np.ndarray:
"""A sensor-frame direction as the mounted sensor points it, unit length."""
normal = self.setup_rotation() @ self.normal_to_platform(normal_sensor)
length = float(np.linalg.norm(normal))
return normal if length < 1e-12 else normal / length
def platform_rotation(self, yaw_deg: float, pitch_deg: float) -> np.ndarray:
"""``R_pitch · R_yaw``: the stage rotation written in the table frame.
:meth:`stage_rotation` is the *same* rotation in the sensor frame — the two
are conjugates by the mount. Anything that reasons about where a point of
the table ends up (the contact, the table's own frame) needs this one.
"""
return rotation_about(self.pitch_axis, pitch_deg) @ rotation_about(
self.yaw_axis, yaw_deg
)
# -------------------------------------------------------------------- solve
def solve(
@@ -211,74 +345,306 @@ class PlatformStack:
*,
plate_thickness_mm: float = 0.0,
tool_z_mm: float = 0.0,
method: str = "analytical",
) -> Pose:
"""The yaw/pitch that put the tool axis on ``normal_sensor``, and the XY.
The reported XY is the **contact point's** position, not the sensor
origin's, so it is the coordinate the tool actually has to be above.
The reported XY is the **contact point's** position in the table frame —
with the mount, the mounting plate and the sensor's setup pose applied — so
it is where the point is, not where the machine has to go. The move the
table makes to bring it under the tool is reported separately, as
:attr:`Pose.table_xy_mm`.
``method="analytical"`` (the default) uses the closed form, which is exact
to floating point and about 25x faster than the sweep. ``method="search"``
keeps the old sweep as the reference the tests check the closed form
against; it is slower and quantised to ``SWEEP_STEP``.
"""
normal = self.normal_to_platform(normal_sensor)
normal = self.setup_normal(normal_sensor)
length = float(np.linalg.norm(normal))
if not np.isfinite(length) or length < 1e-12:
return Pose(ok=False, reason="the surface normal is degenerate")
# The *stored* normal points away from the sensing surface; the tool
# pushes into it. ``approach_sign`` carries that, and it appears twice
# below — once in the pitch (through z) and once in the yaw. Applying it
# to the vector itself instead is what produced a 90 degree residual on
# every point when this was first written.
unit = normal / length
sign = float(self.approach_sign)
tool = self.tool_direction
# Express the normal in a basis whose Z is the tool axis, so the closed
# form does not care which platform axis the tool runs along.
basis = _tool_basis(self.tool_axis)
local = basis @ unit
point = self.point_to_platform(point_sensor_mm, plate_thickness_mm=plate_thickness_mm)
point = point + np.asarray(self.tool_offset_mm, dtype=np.float64)
# Solve numerically, and deliberately so.
#
# The stages rotate the **platform-frame** normal; the local basis is only
# a convenience for reasoning about which axis the tool runs along. An
# earlier version rotated the local vector and reported a 90 degree
# residual for a normal that was already aligned — exactly the class of
# confident wrong number this residual exists to prevent. So the search
# and the residual both work on ``unit``, and ``residual_for`` (used by
# the preview) does the same, so a point cannot be green in the readout
# and red in the picture.
#
# There is deliberately no special case for a normal lying *on* the tool
# axis. One existed, returned a zero residual from a formula, and
# disagreed with the rotations by 90 degrees — the search finds the real
# answer (which is 0) and the indeterminacy is reported separately below.
yaw_deg, pitch_deg, residual = self._search_alignment(unit, tool)
point = self.setup_point(point_sensor_mm, plate_thickness_mm=plate_thickness_mm)
# At the pole the azimuth is undefined, so every yaw gives the same
# alignment. Say so rather than presenting an arbitrary angle as the
# answer; the returned pose is still one that works.
# alignment. Say so rather than presenting an arbitrary angle as the answer.
on_axis = float(np.hypot(local[0], local[1])) < 1e-9
determined = not on_axis
if method == "analytical":
branches = self.alignment_branches(unit, tool)
usable = [branch for branch in branches if branch.in_range]
if usable:
chosen = self.select_branch(usable)
yaw_deg = chosen.yaw_deg
pitch_deg = chosen.pitch_deg
residual = chosen.residual_deg
upright = chosen.upright
in_range = True
elif branches:
# Every exact pose needs more travel than the stage has. Report the
# best pose the limits *do* allow, with the residual it really
# leaves — not an out-of-range pose that would drive the machine
# into its end stop, and not a zero residual it cannot achieve.
yaw_deg, pitch_deg, residual = self._best_within_limits(unit, tool)
upright = self.uprightness(yaw_deg, pitch_deg) >= 0.0
in_range = False
else:
yaw_deg, pitch_deg, residual = self._search_alignment(unit, tool)
upright = self.uprightness(yaw_deg, pitch_deg) >= 0.0
in_range = False
elif method == "search":
yaw_deg, pitch_deg, residual = self._search_alignment(unit, tool)
upright = self.uprightness(yaw_deg, pitch_deg) >= 0.0
in_range = True
else: # pragma: no cover - guarded by the signature
raise ValueError(f"unknown method {method!r}")
reason = ""
if on_axis:
reason = "the normal lies on the tool axis; yaw is indeterminate"
elif not in_range:
reason = "the pose is outside the stage travel"
elif residual > self.tolerance_deg:
reason = "no pose meets the tolerance"
# The stages turn the contact about the table origin; the table then slides
# it under the tool. Reported rather than animated here, but computed from
# the same angles the pose reports, so a caller cannot disagree with it.
landed = self.platform_rotation(yaw_deg, pitch_deg) @ point
tool_x, tool_y = (float(value) for value in self.tool_xy_mm)
return Pose(
ok=True,
target_xy_mm=(self._axis(point, "x"), self._axis(point, "y")),
contact_z_mm=self._axis(point, "z"),
table_xy_mm=(tool_x - self._axis(landed, "x"), tool_y - self._axis(landed, "y")),
yaw_deg=yaw_deg,
pitch_deg=pitch_deg,
tool_z_mm=float(tool_z_mm),
residual_deg=residual,
yaw_determined=determined,
yaw_determined=not on_axis,
upright=upright,
in_range=in_range,
reason=reason,
)
# ------------------------------------------------------- closed-form solve
def alignment_branches(
self, unit: np.ndarray, tool: np.ndarray
) -> list["Branch"]:
"""Every exact pose that puts ``unit`` on ``tool``, in closed form.
Two ``atan2`` calls, no search. The chain is yaw-then-pitch, so:
* the **pitch** cannot change the normal's component along the pitch axis,
which means the yaw must first bring that component onto the tool's. That
is one sinusoid in the yaw, so it has two roots 180 degrees apart;
* with the yaw fixed, the **pitch** rotates the remaining pair onto the
tool's, which is a single ``atan2``.
Both roots reach the same normal in opposite senses, so one usually leaves
the sensing face up and the other turns it over. Which is which is decided
by :meth:`select_branch`, never by defaulting to the first.
The rotation coefficients come from :meth:`_rotation_coefficients` and the
probe rotations from :func:`rotation_about`, rather than a basis written
out by hand. Every sign error made while deriving this came from
re-deriving those conventions locally; the library primitive already has a
test pinning it to an independently built matrix.
"""
unit = np.asarray(unit, dtype=np.float64)
tool = np.asarray(tool, dtype=np.float64)
pitch_stationary = self._rotation_coefficients(self.pitch_axis)[0]
yaw_stationary = self._yaw_stationary
if pitch_stationary == yaw_stationary: # pragma: no cover - axes perpendicular
return []
# Solve for the yaw numerically-but-exactly, then the pitch in closed form.
#
# The yaw is what moves the component the pitch pins, and that component
# after the yaw is a sinusoid of the yaw angle. Rather than transcribe that
# sinusoid's coefficients — which produced a wrong sign every single time it
# was attempted in this file — the two roots are found by sampling the
# sinusoid finely with the *library's own* rotation and refining each
# crossing. The pitch is then exact in closed form, so the answer is exact
# to floating point and there is no sign left to get wrong.
roots = self._yaw_roots(unit, float(tool[pitch_stationary]), pitch_stationary)
if not roots:
return []
branches: list[Branch] = []
seen: set[float] = set()
for yaw_deg in roots:
if any(abs(yaw_deg - prior) < 1e-6 for prior in seen):
continue
seen.add(yaw_deg)
yawed = rotation_about(self.yaw_axis, yaw_deg) @ unit
pitch_deg = self._pitch_for(yawed, tool)
branches.append(
Branch(
yaw_deg=yaw_deg,
pitch_deg=pitch_deg,
residual_deg=self._residual(unit, yaw_deg, pitch_deg),
upright=self.uprightness(yaw_deg, pitch_deg) >= 0.0,
in_range=(
abs(yaw_deg) <= self._yaw_limit_effective() + 1e-9
and abs(pitch_deg) <= self._pitch_limit_effective() + 1e-9
),
)
)
return branches
def _yaw_roots(
self, unit: np.ndarray, target: float, pinned: int
) -> list[float]:
"""The yaw angles that put the pinned component onto ``target``.
The pinned component after a yaw is ``a·cos(y) + b·sin(y)`` — a single
sinusoid — so it has at most two roots in a turn and both are exact. The
coefficients come from the rotation primitive itself: evaluating it at
yaw 0 and yaw 90 degrees gives ``a`` and ``b`` directly, with no basis
written out by hand.
That last point is the whole reason this shape was chosen. Every sign error
made while deriving this module came from transcribing the rotation
convention locally; taken from the primitive, there is no convention left
to transcribe wrong.
"""
at_zero = np.asarray(unit, dtype=np.float64)
at_ninety = rotation_about(self.yaw_axis, 90.0) @ at_zero
a = float(at_zero[pinned])
b = float(at_ninety[pinned])
amplitude = math.hypot(a, b)
if amplitude < 1e-15:
# The pinned component does not depend on the yaw at all — the normal
# lies in the plane the yaw sweeps, so every yaw gives the same value.
# If that value is already the tool's, the yaw is free and zero is as
# good as any; otherwise nothing can bring the two together.
return [0.0] if abs(target) <= 1e-15 else []
if abs(target) > amplitude + 1e-12:
return [] # unreachable: no yaw brings this component onto the tool's
phase = math.atan2(b, a)
offset = math.acos(max(-1.0, min(1.0, target / amplitude)))
roots: list[float] = []
for root in (phase + offset, phase - offset):
degrees = math.degrees(root)
degrees = (degrees + 180.0) % 360.0 - 180.0
if not any(abs(degrees - prior) < 1e-9 for prior in roots):
roots.append(degrees)
return roots
def _best_within_limits(
self, unit: np.ndarray, tool: np.ndarray, samples: int = 721
) -> tuple[float, float, float]:
"""The best pose the stage limits actually allow, with its real residual.
Used when no exact pose fits. The pitch that best aligns a given yaw is
available in closed form, so this is one sweep over yaw at ``SWEEP_STEP``
with an exact pitch each; the residual reported is measured, never assumed.
"""
pitch_limit = self._pitch_limit_effective()
best: tuple[float, float, float] | None = None
step = max(SWEEP_STEP, (2.0 * self._yaw_limit_effective()) / max(samples - 1, 1))
for yaw_deg in np.arange(-self._yaw_limit_effective(), self._yaw_limit_effective() + step, step):
yawed = rotation_about(self.yaw_axis, float(yaw_deg)) @ unit
pitch_deg = self._pitch_for(yawed, tool)
pitch_deg = max(-pitch_limit, min(pitch_limit, pitch_deg))
residual = self._residual(unit, float(yaw_deg), pitch_deg)
if best is None or residual < best[2]:
best = (float(yaw_deg), pitch_deg, residual)
assert best is not None
return best
def _pitch_for(self, yawed: np.ndarray, tool: np.ndarray) -> float:
"""The pitch that puts an already-yawed normal onto ``tool``.
Reads the rotating pair from the same sign table as everything else, so the
two ``atan2`` arguments are the dot and cross of those two components
against the tool's.
"""
_, first, second, sign_first, sign_second = self._rotation_coefficients(
self.pitch_axis
)
u1, u2 = float(yawed[first]), float(yawed[second])
w1, w2 = float(tool[first]), float(tool[second])
# turned[first] = u1·c + sign_first ·u2·s
# turned[second] = sign_second·u1·s + u2·c
cosine_term = u1 * w1 + sign_second * u2 * w2
sine_term = sign_first * u2 * w1 - u1 * w2
return math.degrees(math.atan2(sine_term, cosine_term))
@staticmethod
def select_branch(branches: list["Branch"]) -> "Branch":
"""Pick from :meth:`alignment_branches`: reachable, then upright, then best.
Every branch aligns to within floating point, so alignment itself rarely
separates them. What does is the machine: a pose inside the stage travel
beats one outside it, and — among those — a pose that leaves the sensing
face up beats one that turns the sensor over.
Alignment is still the third key rather than ignored: if one branch really
is materially better than another, that wins, because the point of the
stages is to align. The tie only decides between poses that are the same
to any tolerance the machine can hold.
"""
def rank(branch: "Branch") -> tuple:
# Alignment first: if one branch genuinely aligns better, that is the
# answer, because aligning is what the stages are for. Reachability and
# orientation decide between branches that align the same, which is the
# usual case — the closed form puts both within floating point of exact.
return (
branch.residual_deg,
not branch.in_range,
not branch.upright,
abs(branch.yaw_deg) + abs(branch.pitch_deg),
)
return min(branches, key=rank)
def reachable(self, unit: np.ndarray, tool: np.ndarray) -> tuple[bool, str]:
"""Whether any pose can align this normal, and why not if none can.
A normal whose component along the *pitch* axis cannot be brought onto the
tool's by any yaw is unreachable at any pitch — the pitch stage cannot
change that component. Saying so is the point: the search used to return a
saturated pose and hide the failure in the residual.
"""
try:
branches = self.alignment_branches(unit, tool)
except ValueError as exc: # pragma: no cover - guarded by __post_init__
return False, str(exc)
if not branches:
return False, "no yaw brings the normal onto the tool axis; unreachable"
return True, ""
def _yaw_limit_effective(self) -> float:
return float(min(180.0, max(0.0, self.yaw_limit_deg)))
def _pitch_limit_effective(self) -> float:
return float(min(360.0, max(0.0, self.pitch_limit_deg)))
@property
def _yaw_stationary(self) -> int:
"""The component the yaw leaves alone (its own axis)."""
return self._rotation_coefficients(self.yaw_axis)[0]
@property
def _yaw_sign_first(self) -> float:
return self._rotation_coefficients(self.yaw_axis)[3]
@property
def _yaw_sign_second(self) -> float:
return self._rotation_coefficients(self.yaw_axis)[4]
def _rotation_coefficients(
self, axis: str
) -> tuple[int, int, int, float, float]:
@@ -604,24 +970,24 @@ class PlatformStack:
)
def uprightness(self, yaw_deg: float, pitch_deg: float) -> float:
"""How much the sensing face still points along the tool axis, in -1..1.
"""How much the sensing face still points up, in -1..1.
Measured, not derived from a formula over one stage: the sensor's own up
axis is put through the composed stage rotation and dotted with the tool
direction in the same frame. Two attempts at the formula were wrong in
opposite directions and each looked reasonable, so this reads the same
rotations the preview draws.
Measured, not derived: the sensor's own up axis (mesh ``+Y``, which
``AXIS_MAP`` sends to platform ``+Z``) is put through the composed stage
rotation and compared with that same axis *unrotated*. ``+1`` is the
nominal pose — no rotation, face straight up — ``-1`` is fully turned over.
The tool presses *into* the sensing face, so the sensor's up axis is
anti-parallel to ``tool_direction``; nominal (no rotation) is therefore
``+1`` with the negation below, which is the anchor this is checked
against — without it the whole scale came out backwards.
Both vectors are expressed in the same frame, so this is the angle between
the face and where it started, and it does not depend on which way the tool
points. An earlier version dotted the turned axis against the tool
direction, which quietly moved with ``approach_sign`` and so reported the
polarity as an orientation.
"""
turned = self.stage_rotation(yaw_deg, pitch_deg) @ np.array([0.0, 1.0, 0.0])
reference = to_sensor(np.eye(3)[1], self.axis_map)
reference = reference / max(float(np.linalg.norm(reference)), 1e-12)
turned = self.stage_rotation(yaw_deg, pitch_deg) @ reference
turned = turned / max(float(np.linalg.norm(turned)), 1e-12)
tool_local = to_sensor(np.asarray(self.tool_direction, dtype=np.float64), self.axis_map)
tool_local = tool_local / max(float(np.linalg.norm(tool_local)), 1e-12)
return float(turned @ tool_local)
return float(turned @ reference)
def ends_upright(self, yaw_deg: float, pitch_deg: float) -> bool:
"""``uprightness`` as a decision, with a dead band for numerical ties."""
apps/autocal/src/orisys_autocal/preferences.py
+55 -9
View File
@@ -179,15 +179,24 @@ def default_preferences() -> dict[str, Any]:
#: The UV-space texture for the flat picker. Preferred over the derived
#: path when it exists; empty falls back to the layout map.
"finger_texture_path": DEFAULT_FINGER_TEXTURE,
#: The mounting plate lifts the sensor along the platform's tool axis, so
#: a fixture thickness is one number rather than a transform.
"finger_plate_thickness_mm": 0.0,
#: Tool tip offset from the platform reference, mm.
"finger_tool_offset_mm": [0.0, 0.0, 0.0],
#: Stage travel, degrees. The bench's figures: pitch turns continuously,
#: yaw is limited — and that limit is what decides how much of the patch
#: can be aligned at all.
"finger_yaw_limit_deg": 90.0,
#: How the sensor actually sits on the stack when the machine is at zero:
#: x/y/z translation along the table's axes and a yaw about its z. **The z
#: is the mounting plate**, so the plate is not a setting of its own — it
#: lifts the sensing face and does nothing else, and two fields for one
#: number drift apart.
"finger_sensor_offset_mm": [0.0, 0.0, 0.0],
"finger_sensor_yaw_deg": 0.0,
#: Where the tool is, in the table frame, mm. Straight above the table
#: origin unless the real machine says otherwise; every XY move is measured
#: against it, so it is what makes the reported move match the bench. A tool
#: tip offset belongs here — it used to be a separate field, and having two
#: of them is what let the picture and the numbers disagree.
"finger_tool_xy_mm": [0.0, 0.0],
#: Stage travel, degrees. Confirmed at the bench: the vertical stage turns
#: a full half-turn either way (±180), and the horizontal stage turns
#: continuously. An earlier value of 90 was wrong and made half the patch
#: look unreachable when it is not.
"finger_yaw_limit_deg": 180.0,
"finger_pitch_limit_deg": 360.0,
}
@@ -199,6 +208,43 @@ def load_preferences() -> dict[str, Any]:
axis_params = prefs.setdefault("axis_params", {})
for axis, values in _default_axis_params().items():
axis_params.setdefault(axis, values)
# The mounting plate used to be a setting of its own. It is now the z of the
# sensor's mounting pose, so a stored value is carried over rather than quietly
# dropped — a machine that had it tuned would otherwise come back 4 mm off.
plate = prefs.pop("finger_plate_thickness_mm", None)
try:
plate = float(plate) if plate is not None else 0.0
except (TypeError, ValueError):
plate = 0.0
offset = prefs.get("finger_sensor_offset_mm")
offset = list(offset) if isinstance(offset, list) else [0.0, 0.0, 0.0]
while len(offset) < 3:
offset.append(0.0)
if plate and not offset[2]:
offset[2] = plate
prefs["finger_sensor_offset_mm"] = offset
# The tool tip offset used to be a vector added to the reported coordinates
# while the drawing ignored it, so the picture and the numbers disagreed by
# exactly that much. It is the tool's position now: fold a stored x/y into it,
# and take whatever is left out of the document so nothing reads it again. Its
# z had no meaning the model kept — the tool's height is the Z axis's job — so
# it is dropped, deliberately and visibly rather than silently applied.
legacy = prefs.pop("finger_tool_offset_mm", None)
tool = prefs.get("finger_tool_xy_mm")
tool = list(tool) if isinstance(tool, list) else [0.0, 0.0]
while len(tool) < 2:
tool.append(0.0)
if isinstance(legacy, list) and len(legacy) >= 2:
for index in range(2):
try:
value = float(legacy[index])
except (TypeError, ValueError):
continue
if value and not tool[index]:
tool[index] = value
prefs["finger_tool_xy_mm"] = tool
return prefs
apps/autocal/src/orisys_autocal/ui/pose_preview.py
+1122 -182
View File
File diff suppressed because it is too large Load Diff
apps/autocal/src/orisys_autocal/ui/sensor_panel.py
+629 -109
View File
@@ -18,11 +18,12 @@ not.
from __future__ import annotations
import math
from pathlib import Path
import numpy as np
from PySide6.QtCore import QPoint, Qt, Signal
from PySide6.QtGui import QImage, QPainter, QPen, QPixmap
from PySide6.QtGui import QColor, QImage, QPainter, QPen, QPixmap
from PySide6.QtWidgets import (
QCheckBox,
QDoubleSpinBox,
@@ -51,6 +52,11 @@ from ..sensor.uv_flat import FlatSurface
#: How many picked points the list keeps before the oldest is dropped.
MAX_POINTS = 200
#: How close a stored point must be to a raster entry to be shown on the flat
#: image. One pixel of the 1024-wide raster is about 0.0147 mm on this export, so
#: this is a pixel and a half — finer than the view can render anyway.
_POINT_MATCH_MM = 0.02
class FlatSurfaceView(QLabel):
"""The flat image, clickable, with the picked pixel cross-haired.
@@ -75,6 +81,7 @@ class FlatSurfaceView(QLabel):
self._flat: FlatSurface | None = None
self._pixmap: QPixmap | None = None
self._marker: tuple[int, int] | None = None
self._selected: tuple[int, int] | None = None
self.setCursor(Qt.CursorShape.CrossCursor)
@property
@@ -84,6 +91,7 @@ class FlatSurfaceView(QLabel):
def set_flat(self, flat: FlatSurface | None) -> None:
self._flat = flat
self._marker = None
self._selected = None
if flat is None:
self._pixmap = None
self.clear()
@@ -101,21 +109,63 @@ class FlatSurfaceView(QLabel):
self._marker = pixel
self._refresh()
def set_selected(self, pixel: tuple[int, int] | None) -> None:
"""The point currently chosen in the table — the one about to be tested.
Drawn as a filled red dot, distinct from the cross-hair left by the last
click, so "what I just picked" and "what is about to run" are not confused.
"""
self._selected = pixel
self._refresh()
@property
def selected_pixel(self) -> tuple[int, int] | None:
return self._selected
def pixel_for_point(self, point_mm) -> tuple[int, int] | None:
"""The pixel showing a given sensor-frame 3D point, if one does.
Inverse of :meth:`FlatSurface.sample`: search the table for the entry whose
recorded position matches. Exact identity is not expected — a point may
have been edited — so the nearest entry within a pixel wins, and ``None``
means the point is not on the rasterised surface at all.
"""
flat = self._flat
if flat is None:
return None
target = np.asarray(point_mm, dtype=np.float64)
covered = flat.face_index >= 0
if not covered.any():
return None
rows, columns = np.nonzero(covered)
positions = flat.positions_mm[rows, columns]
distances = np.linalg.norm(positions - target, axis=1)
best = int(np.argmin(distances))
if float(distances[best]) > _POINT_MATCH_MM:
return None
return int(columns[best]), int(rows[best])
def _marked(self) -> QPixmap | None:
"""The native-size image with the marker drawn on it, or ``None``."""
"""The native-size image with both markers drawn on it, or ``None``."""
if self._pixmap is None:
return None
if self._marker is None:
if self._marker is None and self._selected is None:
return self._pixmap
canvas = self._pixmap.copy()
painter = QPainter(canvas)
try:
x, y = self._marker
painter.setPen(QPen(Qt.GlobalColor.white, 1, Qt.PenStyle.SolidLine))
painter.drawLine(x - 8, y, x + 8, y)
painter.drawLine(x, y - 8, x, y + 8)
painter.setPen(QPen(Qt.GlobalColor.black, 1, Qt.PenStyle.SolidLine))
painter.drawEllipse(QPoint(x, y), 5, 5)
if self._marker is not None:
x, y = self._marker
painter.setPen(QPen(Qt.GlobalColor.white, 1, Qt.PenStyle.SolidLine))
painter.drawLine(x - 8, y, x + 8, y)
painter.drawLine(x, y - 8, x, y + 8)
painter.setPen(QPen(Qt.GlobalColor.black, 1, Qt.PenStyle.SolidLine))
painter.drawEllipse(QPoint(x, y), 5, 5)
if self._selected is not None:
x, y = self._selected
painter.setPen(QPen(QColor("#ffffff"), 2))
painter.setBrush(QColor("#d92d20"))
painter.drawEllipse(QPoint(x, y), 6, 6)
finally:
painter.end()
return canvas
@@ -177,12 +227,27 @@ class FingerPanel(QGroupBox):
pickRequested = Signal(int, int)
stackChanged = Signal()
#: Columns of the point table, minus the leading select checkbox. The first six
#: are the ones an operator may edit — the sensor-frame point and its normal —
#: and everything after them is *solved*, so it is redrawn from the edit rather
#: than typed into.
COLUMNS: tuple[str, ...] = (
"x", "y", "z", "nx", "ny", "nz",
"X", "Y", "Z", "yaw", "pitch", "residual",
)
#: How many of ``COLUMNS`` are inputs rather than solved outputs.
INPUT_COLUMNS = 6
#: Offset of the first data column; column 0 holds the select checkbox.
SELECT_COLUMN = 0
def __init__(
self,
stack: PlatformStack | None = None,
*,
bundle_path: str = "",
plate_thickness_mm: float = 0.0,
sensor_offset_mm: tuple[float, float, float] = (0.0, 0.0, 0.0),
sensor_yaw_deg: float = 0.0,
tool_xy_mm: tuple[float, float] = (0.0, 0.0),
parent: QWidget | None = None,
) -> None:
super().__init__(parent)
@@ -191,6 +256,11 @@ class FingerPanel(QGroupBox):
self._bundle: SensorBundle | None = None
#: Full-precision picked rows, index-aligned with the table.
self._picked: list[dict[str, float]] = []
#: True while this panel is writing the table itself, so the ``itemChanged``
#: handler can tell its own redraw from an operator's edit. Without it every
#: solved-column update would be re-read as an edit and re-solve.
self._writing = False
self._headers = self.COLUMNS
outer = QVBoxLayout(self)
outer.setContentsMargins(10, 8, 10, 10)
@@ -212,11 +282,21 @@ class FingerPanel(QGroupBox):
body.addWidget(picker_box, 2)
body.addWidget(readout_box, 3)
body.addWidget(preview_box, 5)
# The body outranks the table, because the 3D view is the pane an operator
# actually reads the pose from, and at an even split it came out 240 px tall
# — a box too short to judge a normal in, under three lines of legend.
outer.addLayout(body, 1)
outer.addWidget(self._build_point_table(), 2)
# ``Maximum`` so the table stops at its natural height and the spare vertical
# space goes to the 3D view above it. Left expandable it ate the window: the
# preview stayed at its 240 px floor while the table had hundreds of empty
# pixels under three rows.
table_box = self._build_point_table()
table_box.setSizePolicy(QSizePolicy.Policy.Preferred, QSizePolicy.Policy.Maximum)
outer.addWidget(table_box)
self.set_plate_thickness(plate_thickness_mm)
self.set_sensor_setup(sensor_offset_mm, sensor_yaw_deg)
self.set_tool_xy(*tool_xy_mm)
self.retranslate()
# ------------------------------------------------------------------ widgets
@@ -286,9 +366,9 @@ class FingerPanel(QGroupBox):
"sensor_normal",
"platform_point",
"pose",
"table_move",
"residual",
"frames",
"bundle",
)
for row, key in enumerate(rows):
label = QLabel(box)
@@ -307,33 +387,101 @@ class FingerPanel(QGroupBox):
grid.addWidget(value, row, 1)
self._readout_labels[key] = value
# The mounting plate: a translation along the platform's tool axis, so a
# fixture thickness is one number rather than a transform to maintain.
self.plate_label = QLabel(box)
self.plate_label.setObjectName("hintLabel")
self.plate_spin = QDoubleSpinBox(box)
self.plate_spin.setRange(-100.0, 100.0)
self.plate_spin.setDecimals(3)
self.plate_spin.setSingleStep(0.5)
self.plate_spin.setSuffix(" mm")
self.plate_spin.setFixedHeight(28)
self.plate_spin.valueChanged.connect(self._on_plate_changed)
grid.addWidget(self.plate_label, len(rows), 0)
grid.addWidget(self.plate_spin, len(rows), 1)
# The mounting pose and the tool: the four numbers and the two that describe
# where the parts actually are. Spin boxes rather than a text field because
# these get dialled in against a real setup, one step at a time.
self.tool_label = QLabel(box)
self.tool_label.setObjectName("hintLabel")
tool_row = QWidget(box)
tool_row_layout = QHBoxLayout(tool_row)
tool_row_layout.setContentsMargins(0, 0, 0, 0)
tool_row_layout.setSpacing(6)
self.tool_x_spin = self._mm_spin(box, -500.0, 500.0)
self.tool_y_spin = self._mm_spin(box, -500.0, 500.0)
tool_row_layout.addWidget(self.tool_x_spin)
tool_row_layout.addWidget(self.tool_y_spin)
grid.addWidget(self.tool_label, len(rows), 0)
grid.addWidget(tool_row, len(rows), 1)
self.offset_label = QLabel(box)
self.offset_label.setObjectName("hintLabel")
self.offset_edit = QLineEdit(box)
self.offset_edit.setFixedHeight(28)
self.offset_edit.setToolTip("sensor origin offset, x,y,z mm")
self.offset_edit.editingFinished.connect(self._on_offset_changed)
grid.addWidget(self.offset_label, len(rows) + 1, 0)
grid.addWidget(self.offset_edit, len(rows) + 1, 1)
self.setup_xy_label = QLabel(box)
self.setup_xy_label.setObjectName("hintLabel")
setup_xy_row = QWidget(box)
setup_xy_layout = QHBoxLayout(setup_xy_row)
setup_xy_layout.setContentsMargins(0, 0, 0, 0)
setup_xy_layout.setSpacing(6)
self.sensor_x_spin = self._mm_spin(box, -500.0, 500.0)
self.sensor_y_spin = self._mm_spin(box, -500.0, 500.0)
setup_xy_layout.addWidget(self.sensor_x_spin)
setup_xy_layout.addWidget(self.sensor_y_spin)
grid.addWidget(self.setup_xy_label, len(rows) + 1, 0)
grid.addWidget(setup_xy_row, len(rows) + 1, 1)
self.setup_zy_label = QLabel(box)
self.setup_zy_label.setObjectName("hintLabel")
setup_zy_row = QWidget(box)
setup_zy_layout = QHBoxLayout(setup_zy_row)
setup_zy_layout.setContentsMargins(0, 0, 0, 0)
setup_zy_layout.setSpacing(6)
# z is the mounting plate and any shim: one number, because a second field
# for the plate would be a second spelling of the same quantity.
self.sensor_z_spin = self._mm_spin(box, -100.0, 200.0)
self.sensor_yaw_spin = QDoubleSpinBox(box)
self.sensor_yaw_spin.setObjectName("compactField")
self.sensor_yaw_spin.setRange(-180.0, 180.0)
self.sensor_yaw_spin.setDecimals(3)
self.sensor_yaw_spin.setSingleStep(0.1)
self.sensor_yaw_spin.setSuffix(" °")
self.sensor_yaw_spin.setFixedHeight(28)
self.sensor_yaw_spin.setToolTip(t("finger.setup_hint"))
setup_zy_layout.addWidget(self.sensor_z_spin)
setup_zy_layout.addWidget(self.sensor_yaw_spin)
grid.addWidget(self.setup_zy_label, len(rows) + 2, 0)
grid.addWidget(setup_zy_row, len(rows) + 2, 1)
for spin in (
self.tool_x_spin,
self.tool_y_spin,
self.sensor_x_spin,
self.sensor_y_spin,
self.sensor_z_spin,
self.sensor_yaw_spin,
):
spin.valueChanged.connect(self._on_frame_inputs_changed)
# The two summary lines live here rather than under the 3D view, where they
# were competing with it for height: both are numbers about the pick, so they
# belong beside the numbers. Three lines of legend under the picture left the
# picture 240 px tall on a 1080 screen — the one pane that has to be read.
self.reach_label = QLabel(box)
self.reach_label.setObjectName("hintLabel")
self.reach_label.setWordWrap(True)
grid.addWidget(self.reach_label, len(rows) + 3, 0, 1, 2)
self.path_label = QLabel(box)
self.path_label.setObjectName("hintLabel")
self.path_label.setWordWrap(True)
grid.addWidget(self.path_label, len(rows) + 4, 0, 1, 2)
grid.setRowStretch(len(rows) + 5, 1)
grid.setColumnStretch(1, 1)
self._readout_box = box
return box
def _mm_spin(self, box: QWidget, low: float, high: float) -> QDoubleSpinBox:
"""A millimetre field with the shared range, step and suffix.
``compactField`` because these sit two to a row: the theme's normal input
padding makes a field 40 px tall, which is right in a form and overflows a
cell in a paired column.
"""
spin = QDoubleSpinBox(box)
spin.setObjectName("compactField")
spin.setRange(low, high)
spin.setDecimals(3)
spin.setSingleStep(0.1)
spin.setSuffix(" mm")
spin.setFixedHeight(28)
return spin
def _build_preview(self) -> QWidget:
from .pose_preview import PosePreview
@@ -343,11 +491,32 @@ class FingerPanel(QGroupBox):
column.setContentsMargins(8, 6, 8, 8)
column.setSpacing(6)
self.preview = PosePreview(box)
# The full colour key, on hover. The one-line label below carries the short
# version: a three-sentence legend under the picture took the height the
# picture needed, and a key nobody can see is no better than no key.
self.preview.setToolTip(t("finger.preview_legend"))
column.addWidget(self.preview, 1)
checks = QHBoxLayout()
checks.setContentsMargins(0, 0, 0, 0)
checks.setSpacing(10)
self.axes_check = QCheckBox(box)
self.axes_check.setChecked(True)
self.axes_check.toggled.connect(self.preview.set_show_axes)
column.addWidget(self.axes_check)
checks.addWidget(self.axes_check)
# The table's own frame: origin plane, the two rotation axes through it, the
# table marker, and the sensor base lifted by its z setup offset. Kept
# separate from the sensor axes above, because this is the machine's frame.
self.table_check = QCheckBox(box)
self.table_check.setChecked(True)
self.table_check.toggled.connect(self.preview.set_show_table)
checks.addWidget(self.table_check)
# The planned path: the arc the stages sweep, then the straight table move.
self.trajectory_check = QCheckBox(box)
self.trajectory_check.setChecked(True)
self.trajectory_check.toggled.connect(self.preview.set_show_trajectory)
checks.addWidget(self.trajectory_check)
checks.addStretch(1)
column.addLayout(checks)
controls = QHBoxLayout()
controls.setContentsMargins(0, 0, 0, 0)
controls.setSpacing(6)
@@ -358,6 +527,10 @@ class FingerPanel(QGroupBox):
self.sim_button.setEnabled(False)
self.sim_button.clicked.connect(self.preview.start_simulation)
self.preview.simulationChanged.connect(self._on_simulation_changed)
# A failed paint has to reach a label. Left to the widget alone it showed as
# a scene that was merely missing some of its parts, which is how a broken
# colour call hid the base plane and the whole trajectory without a word.
self.preview.problemChanged.connect(lambda _message: self._refresh_reach())
controls.addWidget(self.sim_button)
controls.addStretch(1)
column.addLayout(controls)
@@ -368,10 +541,6 @@ class FingerPanel(QGroupBox):
self.legend_label.setObjectName("hintLabel")
self.legend_label.setWordWrap(True)
column.addWidget(self.legend_label)
self.reach_label = QLabel(box)
self.reach_label.setObjectName("hintLabel")
self.reach_label.setWordWrap(True)
column.addWidget(self.reach_label)
self._preview_box = box
return box
@@ -382,35 +551,49 @@ class FingerPanel(QGroupBox):
column.setContentsMargins(8, 6, 8, 8)
column.setSpacing(6)
headers = (
"x", "y", "z", "nx", "ny", "nz",
"X", "Y", "Z", "yaw", "pitch", "residual",
)
self.table = QTableWidget(0, len(headers), box)
self.table = QTableWidget(0, len(self._headers) + 1, box)
self.table.verticalHeader().setVisible(False)
self.table.setSelectionMode(QTableWidget.SelectionMode.NoSelection)
self.table.setEditTriggers(QTableWidget.EditTrigger.NoEditTriggers)
self.table.setSelectionBehavior(QTableWidget.SelectionBehavior.SelectRows)
self.table.setSelectionMode(QTableWidget.SelectionMode.ExtendedSelection)
# Editable, but only on a deliberate double-click: a single click selects
# the row (and drives the markers), which is what an operator does most.
self.table.setEditTriggers(
QTableWidget.EditTrigger.DoubleClicked
| QTableWidget.EditTrigger.EditKeyPressed
)
self.table.setAlternatingRowColors(True)
for index in range(len(headers)):
self.table.setColumnWidth(0, 34)
for index in range(1, len(self._headers) + 1):
self.table.setColumnWidth(index, 82)
self.table.horizontalHeader().setSectionResizeMode(
len(headers) - 1, QHeaderView.ResizeMode.Stretch
len(self._headers), QHeaderView.ResizeMode.Stretch
)
self.table.setMinimumHeight(90)
self.table.itemChanged.connect(self._on_item_changed)
self.table.itemSelectionChanged.connect(self._on_selection_changed)
column.addWidget(self.table, 1)
row = QHBoxLayout()
self.clear_button = QPushButton(box)
self.clear_button.clicked.connect(self.clear_points)
self.remove_button = QPushButton(box)
self.remove_button.clicked.connect(self.remove_selected)
self.up_button = QPushButton(box)
self.up_button.clicked.connect(lambda: self.move_selected(-1))
self.down_button = QPushButton(box)
self.down_button.clicked.connect(lambda: self.move_selected(1))
for button in (self.clear_button, self.remove_button, self.up_button, self.down_button):
row.addWidget(button)
row.addStretch(1)
self._selected_label = QLabel(box)
self._selected_label.setObjectName("hintLabel")
row.addWidget(self._selected_label)
self.count_label = QLabel(box)
self.count_label.setObjectName("hintLabel")
row.addWidget(self.clear_button)
row.addStretch(1)
row.addWidget(self.count_label)
column.addLayout(row)
self._table_box = box
self._headers = headers
return box
# -------------------------------------------------------------------- state
@@ -424,13 +607,68 @@ class FingerPanel(QGroupBox):
return self._stack
@property
def plate_thickness_mm(self) -> float:
return float(self.plate_spin.value())
def sensor_offset_mm(self) -> tuple[float, float, float]:
"""How the sensor is mounted, in the table frame: x, y and z (the plate)."""
return (
float(self.sensor_x_spin.value()),
float(self.sensor_y_spin.value()),
float(self.sensor_z_spin.value()),
)
def set_plate_thickness(self, value: float) -> None:
self.plate_spin.blockSignals(True)
self.plate_spin.setValue(float(value))
self.plate_spin.blockSignals(False)
@property
def sensor_yaw_deg(self) -> float:
return float(self.sensor_yaw_spin.value())
@property
def tool_xy_mm(self) -> tuple[float, float]:
return (float(self.tool_x_spin.value()), float(self.tool_y_spin.value()))
def set_tool_xy(self, x_mm: float, y_mm: float) -> None:
for spin, value in ((self.tool_x_spin, x_mm), (self.tool_y_spin, y_mm)):
spin.blockSignals(True)
spin.setValue(float(value))
spin.blockSignals(False)
self._apply_frame_inputs()
def set_sensor_setup(self, offset_mm, yaw_deg: float) -> None:
"""Put the mounting pose into the fields, then push it to the stack."""
values = tuple(float(value) for value in offset_mm) + (0.0, 0.0, 0.0)
for spin, value in zip(
(self.sensor_x_spin, self.sensor_y_spin, self.sensor_z_spin), values
):
spin.blockSignals(True)
spin.setValue(value)
spin.blockSignals(False)
self.sensor_yaw_spin.blockSignals(True)
self.sensor_yaw_spin.setValue(float(yaw_deg))
self.sensor_yaw_spin.blockSignals(False)
self._apply_frame_inputs()
def _on_frame_inputs_changed(self) -> None:
"""The setup or the tool moved: the geometry has changed, so re-solve."""
self._apply_frame_inputs()
self._resolve_everything()
def _apply_frame_inputs(self) -> None:
self._stack.sensor_offset_mm = self.sensor_offset_mm
self._stack.sensor_yaw_deg = self.sensor_yaw_deg
self._stack.tool_xy_mm = self.tool_xy_mm
self.preview.set_setup(self.sensor_offset_mm, self.sensor_yaw_deg)
self.preview.set_tool_xy(*self.tool_xy_mm)
def _resolve_everything(self) -> None:
"""Re-solve every stored row, then redraw what the current one shows.
Every row, not only the current one: the setup and the tool move the whole
machine, so a table still holding the old X/Y/Z would be a table of numbers
from a machine that no longer exists.
"""
for row in range(len(self._picked)):
self.resolve_row(row)
self._after_points_changed()
self._refresh_path()
self._refresh_reach()
self.stackChanged.emit()
def set_bundle_path(self, path: str) -> None:
self.path_edit.setText(path)
@@ -441,6 +679,7 @@ class FingerPanel(QGroupBox):
self.preview.set_bundle(bundle)
self.refresh_readouts()
self._refresh_reach()
self.path_label.setText("")
def _on_simulation_changed(self, running: bool) -> None:
"""Keep the button honest about what it will do next."""
@@ -470,12 +709,19 @@ class FingerPanel(QGroupBox):
def clear_points(self) -> None:
self._picked.clear()
self._writing = True
try:
self.table.setRowCount(0)
finally:
self._writing = False
self.preview.set_pose(None)
self.preview.set_point(None, None)
self._sync_sim_button()
self.table.setRowCount(0)
self.preview.set_highlight(None)
self.view.set_marker(None)
self.count_label.setText(t("finger.point_count", count=0))
self.view.set_selected(None)
self._sync_sim_button()
self._refresh_count()
self.path_label.setText("")
def point_count(self) -> int:
return self.table.rowCount()
@@ -501,9 +747,9 @@ class FingerPanel(QGroupBox):
return None
point, normal, face = sample
pose = self._stack.solve(
point, normal, plate_thickness_mm=self.plate_thickness_mm
)
pose = self._stack.solve(point, normal)
# The click leaves a cross-hair where it landed *and* promotes the new row
# to the selection, so the red dot marks the same point in both views.
self.view.set_marker((x, y))
self.preview.set_stack(self._stack)
self.preview.set_point(point, normal)
@@ -522,8 +768,16 @@ class FingerPanel(QGroupBox):
# Hand the solved pose to the preview so the play button demonstrates
# exactly the numbers on screen, rather than a second solve of its own.
self.preview.set_pose(pose)
self.preview.set_highlight(point)
self._sync_sim_button()
self.refresh_readouts(point, normal, pose, face)
self._refresh_path()
# Select the row just added, so the table agrees with the dot.
self._writing = True
try:
self.table.setCurrentCell(self.table.rowCount() - 1, 1)
finally:
self._writing = False
return row
def _append_row(self, row: dict[str, float]) -> None:
@@ -532,17 +786,249 @@ class FingerPanel(QGroupBox):
# the displayed text would quietly hand a later export coordinates the
# solver never produced.
self._picked.append(dict(row))
index = self.table.rowCount()
self.table.insertRow(index)
for column, key in enumerate(self._headers):
item = QTableWidgetItem(f"{row[key]:.3f}")
item.setTextAlignment(Qt.AlignmentFlag.AlignRight | Qt.AlignmentFlag.AlignVCenter)
self.table.setItem(index, column, item)
self.table.insertRow(self.table.rowCount())
self._write_row(self.table.rowCount() - 1)
while self.table.rowCount() > MAX_POINTS:
self.table.removeRow(0)
while len(self._picked) > MAX_POINTS:
self._picked.pop(0)
self.count_label.setText(t("finger.point_count", count=self.table.rowCount()))
self._refresh_count()
# ------------------------------------------------------- table row plumbing
def _write_row(self, index: int) -> None:
"""Render one ``_picked`` row into the table, without re-triggering edits."""
if not (0 <= index < len(self._picked)):
return
row = self._picked[index]
self._writing = True
try:
for offset, key in enumerate(self._headers):
column = offset + 1
item = self.table.item(index, column)
if item is None:
item = QTableWidgetItem()
item.setTextAlignment(
Qt.AlignmentFlag.AlignRight | Qt.AlignmentFlag.AlignVCenter
)
if offset >= self.INPUT_COLUMNS:
# Solved values are shown, not typed: editing the normal and
# reading the pose is the workflow, and the other way round
# would need an inverse the table cannot express.
item.setFlags(item.flags() & ~Qt.ItemFlag.ItemIsEditable)
item.setForeground(QColor("#5c6678"))
self.table.setItem(index, column, item)
item.setText(f"{row[key]:.3f}")
self._ensure_selector(index)
finally:
self._writing = False
def _ensure_selector(self, index: int, checked: bool = False) -> None:
"""Give a row its select checkbox, created once."""
if self.table.cellWidget(index, self.SELECT_COLUMN) is not None:
return
holder = QWidget(self.table)
layout = QHBoxLayout(holder)
layout.setContentsMargins(0, 0, 0, 0)
layout.setAlignment(Qt.AlignmentFlag.AlignCenter)
check = QCheckBox(holder)
check.setChecked(checked)
check.toggled.connect(self._refresh_count)
layout.addWidget(check)
self.table.setCellWidget(index, self.SELECT_COLUMN, holder)
def _selector(self, index: int) -> QCheckBox | None:
holder = self.table.cellWidget(index, self.SELECT_COLUMN)
if holder is None:
return None
return holder.findChild(QCheckBox)
def selected_rows(self) -> list[int]:
"""Rows whose checkbox is ticked, ascending."""
return [
index
for index in range(self.table.rowCount())
if (check := self._selector(index)) is not None and check.isChecked()
]
def remove_selected(self) -> int:
"""Drop every ticked row, and the last-picked point if none is ticked."""
rows = self.selected_rows()
if not rows:
row = self.table.currentRow()
rows = [row] if row >= 0 else []
for index in sorted(rows, reverse=True):
self.table.removeRow(index)
if 0 <= index < len(self._picked):
self._picked.pop(index)
self._refresh_count()
self._after_points_changed()
return len(rows)
def move_selected(self, offset: int) -> bool:
"""Shift the ticked rows (or the current one) up or down one place.
Moves the topmost row first when going down and the bottom-most first when
going up, so a contiguous block keeps its internal order and no row is
swapped twice. The current row follows the move, so repeating the action
keeps pushing the same point rather than stranding the selection.
"""
rows = self.selected_rows() or (
[self.table.currentRow()] if self.table.currentRow() >= 0 else []
)
if not rows:
return False
ordered = sorted(rows, reverse=offset > 0)
moved = False
landed = self.table.currentRow()
for index in ordered:
target = index + offset
if not (0 <= target < self.table.rowCount()):
continue
if target in rows: # a selected neighbour will move too; do not swap twice
continue
self._swap_rows(index, target)
if index == self.table.currentRow():
landed = target
moved = True
if moved:
self._writing = True
try:
self.table.setCurrentCell(landed, max(1, self.table.currentColumn()))
finally:
self._writing = False
self._refresh_count()
self._after_points_changed()
return moved
def _swap_rows(self, first: int, second: int) -> None:
"""Exchange two table rows and their stored points, selection included."""
self._picked[first], self._picked[second] = self._picked[second], self._picked[first]
checked = [
bool((check := self._selector(row)) is not None and check.isChecked())
for row in (first, second)
]
self._writing = True
try:
# Rebuild both rows from the swapped data: simpler than moving items, and
# it cannot leave a stale value behind. ``takeCellWidget`` does not exist,
# so the old checkboxes are detached by replacing them with None.
for row in (first, second):
for column in range(1, len(self._headers) + 1):
self.table.takeItem(row, column)
self.table.removeCellWidget(row, self.SELECT_COLUMN)
self._write_row(first)
self._write_row(second)
finally:
self._writing = False
for row, was_checked in zip((first, second), checked):
check = self._selector(row)
if check is not None:
check.setChecked(was_checked)
def _refresh_count(self) -> None:
total = self.table.rowCount()
chosen = len(self.selected_rows())
self.count_label.setText(t("finger.point_count", count=total))
self.remove_button.setEnabled(total > 0)
self.up_button.setEnabled(total > 0)
self.down_button.setEnabled(total > 0)
self._selected_label.setText(
t("finger.selected_count", count=chosen) if chosen else ""
)
# ----------------------------------------------------------------- editing
def _on_item_changed(self, item: QTableWidgetItem) -> None:
"""An operator edited a cell: re-solve the row and refresh everything.
Only the six input columns are editable, so an edit always means "this
sensor-frame point or normal changed". The pose columns are redrawn from the
new solve rather than trusted, which is what keeps the table and the readouts
from disagreeing.
"""
if self._writing:
return
column = item.column() - 1
row = item.row()
if column < 0 or column >= self.INPUT_COLUMNS:
return
if not (0 <= row < len(self._picked)):
return
try:
value = float(item.text())
except ValueError:
# Put the old number back rather than storing a non-number.
self._write_row(row)
self.status_label.setText(t("finger.edit_not_a_number"))
return
if not math.isfinite(value):
self._write_row(row)
self.status_label.setText(t("finger.edit_not_a_number"))
return
record = self._picked[row]
record[self._headers[column]] = value
self.resolve_row(row)
self._after_points_changed()
def resolve_row(self, row: int) -> None:
"""Re-solve a stored row from its sensor-frame point and normal."""
if not (0 <= row < len(self._picked)):
return
record = self._picked[row]
point = np.array([record["x"], record["y"], record["z"]], dtype=np.float64)
normal = np.array([record["nx"], record["ny"], record["nz"]], dtype=np.float64)
length = float(np.linalg.norm(normal))
if length < 1e-12:
return
normal = normal / length
pose = self._stack.solve(point, normal)
record.update(
{
"nx": float(normal[0]), "ny": float(normal[1]), "nz": float(normal[2]),
"X": float(pose.target_xy_mm[0]), "Y": float(pose.target_xy_mm[1]),
"Z": float(pose.contact_z_mm),
"yaw": float(pose.yaw_deg), "pitch": float(pose.pitch_deg),
"residual": float(pose.residual_deg),
}
)
self._write_row(row)
def _on_selection_changed(self) -> None:
"""Follow the current row in both pictures, so the marker means something."""
row = self.table.currentRow()
if not (0 <= row < len(self._picked)):
return
self.show_point(row)
def show_point(self, row: int) -> None:
"""Point the 2D and 3D markers at one row: this is the point to be tested."""
if not (0 <= row < len(self._picked)):
return
record = self._picked[row]
point = np.array([record["x"], record["y"], record["z"]], dtype=np.float64)
normal = np.array([record["nx"], record["ny"], record["nz"]], dtype=np.float64)
self.preview.set_point(point, normal)
self.preview.set_highlight(point)
if self.view.flat is not None:
pixel = self.view.pixel_for_point(point)
self.view.set_selected(pixel)
pose = self._stack.solve(point, normal)
self._last_pose = pose
self.preview.set_pose(pose)
self._sync_sim_button()
self.refresh_readouts(point, normal, pose)
self._refresh_path()
def _after_points_changed(self) -> None:
"""Keep the picture honest after a delete, reorder or edit."""
row = self.table.currentRow()
if 0 <= row < len(self._picked):
self.show_point(row)
else:
self.preview.set_highlight(None)
self.view.set_selected(None)
# ----------------------------------------------------------------- readouts
@@ -550,29 +1036,25 @@ class FingerPanel(QGroupBox):
_last_face = -1
def refresh_readouts(self, point=None, normal=None, pose=None, face: int = -1) -> None:
bundle = self._bundle
self._readout_labels["frames"].setText(describe_frames(self._stack.axis_map))
if bundle is None:
self._readout_labels["bundle"].setText("—")
for key in ("sensor_point", "sensor_normal", "platform_point", "pose", "residual"):
if self._bundle is None:
for key in (
"sensor_point",
"sensor_normal",
"platform_point",
"pose",
"table_move",
"residual",
):
self._readout_labels[key].setText("—")
return
info = bundle.describe()
self._readout_labels["bundle"].setText(
t(
"finger.bundle_summary",
vertices=info["vertices"],
triangles=info["triangles"],
extent=" × ".join(f"{value:.2f}" for value in info["extent_mm"]),
scale=f"{info['mm_per_unit']:.6f}",
)
)
if point is None:
self._readout_labels["table_move"].setText("—")
return
platform = self._stack.point_to_platform(
point, plate_thickness_mm=self.plate_thickness_mm
)
# The contact where the mounted sensor actually puts it, so the platform
# coordinates on screen are the machine's, not the drawing's.
platform = self._stack.setup_point(point)
self._readout_labels["sensor_point"].setText(
" ".join(f"{value:+.3f}" for value in point) + " mm"
)
@@ -582,6 +1064,11 @@ class FingerPanel(QGroupBox):
self._readout_labels["platform_point"].setText(
" ".join(f"{value:+.3f}" for value in platform) + " mm"
)
self._readout_labels["table_move"].setText(
"—"
if pose is None
else f"{pose.table_xy_mm[0]:+.3f} {pose.table_xy_mm[1]:+.3f} mm"
)
if pose is not None:
self._readout_labels["pose"].setText(pose.describe())
self._readout_labels["residual"].setText(
@@ -597,6 +1084,39 @@ class FingerPanel(QGroupBox):
)
del face
def _refresh_path(self) -> None:
"""Report what the planned motion does, and where it actually arrives.
The last line is the x-y check the bench asks about. It is printed as two
pairs — where the drawing lands the contact, and where the tool is — because
they are computed two different ways (the drawing's own transform, and the
tool's setup number) and must agree. When they do not, the line says so
instead of leaving it to be judged off a picture.
"""
summary = self.preview.trajectory_summary()
if not summary:
self.path_label.setText("")
return
text = t(
"finger.path_summary",
reach=f"{summary['reach_mm']:.1f}",
radius=f"{summary['arc_radius_mm']:.1f}",
)
landing = self.preview.landing_xy_mm()
if landing is not None:
tool = self._stack.tool_xy_mm
gap = float(np.hypot(landing[0] - tool[0], landing[1] - tool[1]))
verdict = (
t("finger.landing_on_tool")
if gap <= 1e-6
else t("finger.landing_off_tool", gap=f"{gap:.3f}")
)
text = (
f"{text} {t('finger.landing', x=f'{landing[0]:+.3f}', y=f'{landing[1]:+.3f}')}"
f" / {t('finger.tool_at', x=f'{tool[0]:+.3f}', y=f'{tool[1]:+.3f}')} {verdict}"
)
self.path_label.setText(text)
def _refresh_reach(self) -> None:
"""The reach summary, or why there is no picture instead of silently nothing.
@@ -620,22 +1140,6 @@ class FingerPanel(QGroupBox):
# ------------------------------------------------------------------ handlers
def _on_plate_changed(self, _value: float) -> None:
self.stackChanged.emit()
def _on_offset_changed(self) -> None:
parts = self.offset_edit.text().replace(",", " ").split()
try:
triple = tuple(float(part) for part in parts[:3])
except ValueError:
return
if len(triple) == 3:
self._stack.tool_offset_mm = triple # type: ignore[assignment]
self.stackChanged.emit()
def set_offset(self, triple) -> None:
self.offset_edit.setText(" ".join(f"{value:g}" for value in triple))
# ------------------------------------------------------------- translations
def retranslate(self) -> None:
@@ -653,22 +1157,38 @@ class FingerPanel(QGroupBox):
"sensor_normal": "finger.row_sensor_normal",
"platform_point": "finger.row_platform_point",
"pose": "finger.row_pose",
"table_move": "finger.row_table_move",
"residual": "finger.row_residual",
"frames": "finger.row_frames",
"bundle": "finger.row_bundle",
}
grid = self._readout_box.layout()
for row, key in enumerate(labels):
widget = grid.itemAtPosition(row, 0)
if widget is not None and widget.widget() is not None:
widget.widget().setText(t(labels[key]))
self.plate_label.setText(t("finger.plate_thickness"))
self.offset_label.setText(t("finger.tool_offset"))
self.legend_label.setText(t("finger.preview_legend"))
self.tool_label.setText(t("finger.tool_xy"))
self.setup_xy_label.setText(t("finger.sensor_xy"))
self.setup_zy_label.setText(t("finger.sensor_zy"))
for spin in (self.tool_x_spin, self.tool_y_spin):
spin.setToolTip(t("finger.tool_hint"))
for spin in (self.sensor_x_spin, self.sensor_y_spin, self.sensor_z_spin):
spin.setToolTip(t("finger.setup_hint"))
self.sensor_yaw_spin.setToolTip(t("finger.setup_hint"))
self.legend_label.setText(t("finger.preview_legend_short"))
self.preview.setToolTip(t("finger.preview_legend"))
self.axes_check.setText(t("finger.show_axes"))
self.table_check.setText(t("finger.show_table"))
self.trajectory_check.setText(t("finger.show_trajectory"))
self.sim_button.setToolTip(t("finger.sim_hint"))
self._sync_sim_button()
self.table.setHorizontalHeaderLabels(
[""] + [t(f"finger.col_{key}") for key in self._headers]
)
self.clear_button.setText(t("finger.clear_points"))
self.count_label.setText(t("finger.point_count", count=self.table.rowCount()))
self.table.setHorizontalHeaderLabels([t(f"finger.col_{key}") for key in self._headers])
self.remove_button.setText(t("finger.remove_selected"))
self.up_button.setText(t("finger.move_up"))
self.down_button.setText(t("finger.move_down"))
self.table.setToolTip(t("finger.table_hint"))
self._refresh_count()
self.status_label.setText(t("finger.select_hint"))
apps/autocal/src/orisys_autocal/ui/window.py
+41 -12
View File
@@ -74,6 +74,25 @@ logger = logging.getLogger(__name__)
#: window width.
MAIN_SPLITTER_SIZES = (330, 1560)
def _triple(value: object, length: int) -> tuple[float, ...]:
"""A numeric preference as a fixed-length tuple, tolerant of a hand-edited file.
``None``, a wrong length or a string in the JSON must not stop the window from
opening: the finger screen's setup numbers are all zero by default, and a
malformed one is better replaced than raised on.
"""
values = value if isinstance(value, (list, tuple)) else ()
numbers = []
for item in list(values)[:length]:
try:
numbers.append(float(item))
except (TypeError, ValueError):
numbers.append(0.0)
while len(numbers) < length:
numbers.append(0.0)
return tuple(numbers)
#: Width below which the rail would clip its own controls.
MAIN_MINIMUM_WIDTH = 1440
@@ -230,17 +249,14 @@ class AutoCalMainWindow(QMainWindow):
yaw_limit_deg=float(self.prefs.get("finger_yaw_limit_deg") or 90.0),
pitch_limit_deg=float(self.prefs.get("finger_pitch_limit_deg") or 360.0),
)
offset = self.prefs.get("finger_tool_offset_mm") or [0.0, 0.0, 0.0]
if isinstance(offset, list) and len(offset) == 3:
self.finger_stack.tool_offset_mm = tuple(float(v) for v in offset)
self.finger_panel = FingerPanel(
self.finger_stack,
bundle_path=str(self.prefs.get("finger_bundle_path") or ""),
plate_thickness_mm=float(self.prefs.get("finger_plate_thickness_mm") or 0.0),
sensor_offset_mm=_triple(self.prefs.get("finger_sensor_offset_mm"), 3),
sensor_yaw_deg=float(self.prefs.get("finger_sensor_yaw_deg") or 0.0),
tool_xy_mm=_triple(self.prefs.get("finger_tool_xy_mm"), 2),
parent=body,
)
self.finger_panel.set_offset(self.finger_stack.tool_offset_mm)
self.workspace = WorkspaceTabs(
self.axis_panel, self.sequence_panel, self.finger_panel, body
)
@@ -286,6 +302,12 @@ class AutoCalMainWindow(QMainWindow):
self.bus_data_panel.rtexWriteRequested.connect(self._on_rtex_write)
self.status_strip.stopAllRequested.connect(self._on_stop_all)
self.status_strip.pauseRequested.connect(self._on_pause_polling)
# The readout rail's own batch buttons, connected once here. They were
# connected from inside the finger screen's stack-changed handler, which
# meant every edit on that screen added another copy.
self.axis_readout.stopAllRequested.connect(self._on_stop_all)
self.axis_readout.clearAllRequested.connect(self._on_clear_all_alarms)
self.axis_readout.enableAllRequested.connect(self._on_enable_all)
self.run_bar.runRequested.connect(self._on_run_requested)
self.run_bar.pauseRequested.connect(self._on_run_pause)
self.run_bar.abortRequested.connect(self._on_run_abort)
@@ -321,7 +343,9 @@ class AutoCalMainWindow(QMainWindow):
t(
"finger.loaded",
vertices=info["vertices"],
triangles=info["triangles"],
extent=" × ".join(f"{value:.2f}" for value in info["extent_mm"]),
scale=f"{info['mm_per_unit']:.6f}",
),
ok=True,
)
@@ -396,13 +420,18 @@ class AutoCalMainWindow(QMainWindow):
)
def _on_finger_stack_changed(self) -> None:
"""The mounting pose, the tool position or the limits moved.
Only the state that depends on them is refreshed here. This handler used to
connect three axis-readout signals as well, on every call — harmless while
the stack changed once at startup, and a multiplying pile of duplicate
connections the moment a spin box started changing it.
"""
self.finger_panel.preview.set_stack(self.finger_panel.stack)
self.finger_panel._refresh_reach()
self.prefs["finger_plate_thickness_mm"] = self.finger_panel.plate_thickness_mm
self.prefs["finger_tool_offset_mm"] = list(self.finger_panel.stack.tool_offset_mm)
self.axis_readout.stopAllRequested.connect(self._on_stop_all)
self.axis_readout.clearAllRequested.connect(self._on_clear_all_alarms)
self.axis_readout.enableAllRequested.connect(self._on_enable_all)
self.finger_panel._refresh_reach() # noqa: SLF001 - the panel owns the label
self.prefs["finger_sensor_offset_mm"] = list(self.finger_panel.sensor_offset_mm)
self.prefs["finger_sensor_yaw_deg"] = self.finger_panel.sensor_yaw_deg
self.prefs["finger_tool_xy_mm"] = list(self.finger_panel.tool_xy_mm)
# ---------------------------------------------------------------- run bar