autocal layout first design
This commit is contained in:
ykcheungab
@@ -0,0 +1,42 @@
|
||||
# Vendored ZMotion runtime (internal use)
|
||||
|
||||
These files are copied verbatim from the ZMotion SDK that ships with the
|
||||
controller's `ZDevelop` install, as unpacked from the vendor example bundle:
|
||||
|
||||
D:\orisys_ws\zmotion\Python64\dll库文件\
|
||||
|
||||
| File | Bytes | SHA-256 (first 16) |
|
||||
|---|---|---|
|
||||
| `zauxdll.dll` | 4410880 | `282369258733A0D6` |
|
||||
| `zmotion.dll` | 3596288 | `30DC519539783EE9` |
|
||||
| `zauxdll2.h` | 179106 | `BC8788DE71A7900B` |
|
||||
|
||||
`zauxdll2.h` is kept only as the parameter contract of record for the wrappers
|
||||
in `orisys_app_shared/zmotion/client.py` (register values, argument order,
|
||||
return codes). It is not loaded at run time.
|
||||
|
||||
## Why they are vendored
|
||||
|
||||
`zmcdll/zauxdllPython.py` in the vendor example resolves its DLLs relative to
|
||||
its own module location, plus a current-working-directory fallback. That makes
|
||||
`import` depend on where the process happens to be started from. Copying the
|
||||
two runtime DLLs next to the app and resolving them explicitly (see
|
||||
`orisys_app_shared/zmotion/loader.py`) removes that, at the cost of ~8 MB in
|
||||
the repo.
|
||||
|
||||
## Overriding
|
||||
|
||||
Set `ORISYS_ZMOTION_DLL_DIR` to point at a different directory — for example a
|
||||
newer controller-support CD — and it wins over this folder:
|
||||
|
||||
```powershell
|
||||
$env:ORISYS_ZMOTION_DLL_DIR = "D:\Zmotion\bin"
|
||||
```
|
||||
|
||||
## Redistribution
|
||||
|
||||
Internal use only. The ZMotion SDK is vendor-supplied hardware support
|
||||
software with its own terms; these DLLs must **not** be redistributed inside
|
||||
the customer-facing Orisys Studio installer. AutoCal is a separate, internal
|
||||
application and is deliberately not packaged by `build/studio` (see
|
||||
`build/README.md`).
|
||||
Binary file not shown.
File diff suppressed because it is too large
Load Diff
Binary file not shown.
@@ -125,6 +125,72 @@ def _ensure_bgr(img: np.ndarray) -> np.ndarray:
|
||||
return img
|
||||
|
||||
|
||||
def _policy_from_style(
|
||||
draw_style: str,
|
||||
*,
|
||||
flow_on_px: float | None = None,
|
||||
flow_off_px: float | None = None,
|
||||
flow_grid_spacing: int | None = None,
|
||||
max_arrows: int | None = None,
|
||||
):
|
||||
from orisys_app_shared.scene3d.visual_policy import VisualPolicy
|
||||
|
||||
finger = str(draw_style).lower().startswith("f")
|
||||
defaults = _FINGER_ARROW_DRAW_KW if finger else _ARROW_DRAW_KW
|
||||
return VisualPolicy(
|
||||
flow_on_px=float(defaults["threshold"] if flow_on_px is None else flow_on_px),
|
||||
flow_off_px=float(
|
||||
(0.3 if finger else max(0.0, float(defaults["threshold"]) * 0.75))
|
||||
if flow_off_px is None
|
||||
else flow_off_px
|
||||
),
|
||||
flow_grid_spacing=int(
|
||||
defaults["grid_spacing"] if flow_grid_spacing is None else flow_grid_spacing
|
||||
),
|
||||
flow_arrow_scale=float(defaults["arrow_scale"]),
|
||||
flow_edge_margin=8 if finger else 15,
|
||||
max_arrows=400 if max_arrows is None else int(max_arrows),
|
||||
).normalised()
|
||||
|
||||
|
||||
def _paint_shared_flow_arrows(
|
||||
canvas: np.ndarray,
|
||||
flow: np.ndarray,
|
||||
*,
|
||||
valid_mask: Optional[np.ndarray],
|
||||
policy,
|
||||
colormap: Optional[np.ndarray],
|
||||
below_threshold_color: tuple[int, int, int],
|
||||
) -> np.ndarray:
|
||||
"""Draw arrows from :func:`select_flow_samples` so 2D matches 3D sample points."""
|
||||
from orisys_app_shared.scene3d.visual_policy import select_flow_samples
|
||||
|
||||
image = canvas.copy()
|
||||
samples = select_flow_samples(flow, valid_mask, policy)
|
||||
if samples.rows.size == 0:
|
||||
return image
|
||||
mag = samples.magnitude
|
||||
mag_range = float(mag.max() - mag.min()) if mag.size else 0.0
|
||||
if mag_range > 0:
|
||||
norm = ((mag - mag.min()) / mag_range * 255.0).astype(np.uint8)
|
||||
else:
|
||||
norm = np.zeros(mag.shape, dtype=np.uint8)
|
||||
cmap = cv2.COLORMAP_JET if colormap is None else colormap
|
||||
colors = cv2.applyColorMap(norm.reshape(-1, 1), cmap)
|
||||
for i in range(samples.rows.size):
|
||||
pt1 = (int(samples.cols[i]), int(samples.rows[i]))
|
||||
pt2 = (
|
||||
int(round(samples.cols[i] + samples.dx[i])),
|
||||
int(round(samples.rows[i] + samples.dy[i])),
|
||||
)
|
||||
color = tuple(int(c) for c in colors[i, 0])
|
||||
if mag[i] < policy.flow_on_px:
|
||||
cv2.circle(image, pt1, radius=1, color=below_threshold_color, thickness=-1)
|
||||
else:
|
||||
cv2.arrowedLine(image, pt1, pt2, color, thickness=1, tipLength=0.2)
|
||||
return image
|
||||
|
||||
|
||||
def draw_arrow_display(
|
||||
processed_frame: np.ndarray,
|
||||
flow: Optional[np.ndarray],
|
||||
@@ -134,25 +200,81 @@ def draw_arrow_display(
|
||||
draw_style: str = "box",
|
||||
valid_mask: Optional[np.ndarray] = None,
|
||||
colormap: Optional[np.ndarray] = None,
|
||||
exterior_bgr: Sequence[int] | None = None,
|
||||
island_bgr: Sequence[int] | None = None,
|
||||
flow_on_px: float | None = None,
|
||||
flow_off_px: float | None = None,
|
||||
flow_grid_spacing: int | None = None,
|
||||
max_arrows: int | None = None,
|
||||
) -> np.ndarray:
|
||||
"""Draw flow arrows on a solid canvas or over the processed frame."""
|
||||
"""Draw flow arrows on a solid canvas or over the processed frame.
|
||||
|
||||
For ``pure_color``, the UV ``valid_mask`` paints the finger island in
|
||||
``island_bgr`` and the exterior in ``exterior_bgr`` (defaults: theme canvas
|
||||
inside, slightly darker outside). Without a mask the whole rectangle uses
|
||||
``canvas_bgr`` / ``island_bgr`` for backward compatibility.
|
||||
|
||||
Finger style uses the shared :mod:`visual_policy` sampler so 2D and 3D arrows
|
||||
share the same UV sample points and thresholds. Box style keeps the SDK path.
|
||||
"""
|
||||
base = _ensure_bgr(processed_frame)
|
||||
if flow is None:
|
||||
return base
|
||||
draw_kw = dict(_FINGER_ARROW_DRAW_KW if str(draw_style).lower().startswith("f") else _ARROW_DRAW_KW)
|
||||
finger = str(draw_style).lower().startswith("f")
|
||||
draw_kw = dict(_FINGER_ARROW_DRAW_KW if finger else _ARROW_DRAW_KW)
|
||||
if colormap is not None:
|
||||
draw_kw["colormap"] = colormap
|
||||
try:
|
||||
if background == "pure_color":
|
||||
color_bgr = ARROW_BG_BGR if canvas_bgr is None else tuple(int(v) for v in canvas_bgr)
|
||||
inside = color_bgr if island_bgr is None else tuple(int(v) for v in island_bgr)
|
||||
outside = (
|
||||
tuple(max(0, int(v) - 12) for v in inside)
|
||||
if exterior_bgr is None
|
||||
else tuple(int(v) for v in exterior_bgr)
|
||||
)
|
||||
if valid_mask is not None:
|
||||
from orisys_app_shared.scene3d.visual_policy import (
|
||||
build_domain_mask,
|
||||
paint_domain_background,
|
||||
)
|
||||
|
||||
domain, _ = build_domain_mask(
|
||||
valid_mask, shape_hw=base.shape[:2]
|
||||
)
|
||||
canvas = paint_domain_background(
|
||||
base.shape[:2],
|
||||
domain,
|
||||
island_bgr=inside, # type: ignore[arg-type]
|
||||
exterior_bgr=outside, # type: ignore[arg-type]
|
||||
)
|
||||
valid_mask = domain
|
||||
else:
|
||||
canvas = np.empty_like(base)
|
||||
canvas[:, :] = inside
|
||||
else:
|
||||
canvas = base
|
||||
|
||||
if finger:
|
||||
policy = _policy_from_style(
|
||||
draw_style,
|
||||
flow_on_px=flow_on_px,
|
||||
flow_off_px=flow_off_px,
|
||||
flow_grid_spacing=flow_grid_spacing,
|
||||
max_arrows=max_arrows,
|
||||
)
|
||||
return _paint_shared_flow_arrows(
|
||||
canvas,
|
||||
np.asarray(flow),
|
||||
valid_mask=valid_mask,
|
||||
policy=policy,
|
||||
colormap=colormap,
|
||||
below_threshold_color=tuple(draw_kw["below_threshold_color"]),
|
||||
)
|
||||
|
||||
import orisys
|
||||
|
||||
if background == "pure_color":
|
||||
canvas = np.empty_like(base)
|
||||
color_bgr = ARROW_BG_BGR if canvas_bgr is None else tuple(int(v) for v in canvas_bgr)
|
||||
canvas[:, :] = color_bgr
|
||||
return orisys.util.draw_arrows(
|
||||
canvas, flow, valid_mask=valid_mask, **draw_kw
|
||||
)
|
||||
return orisys.util.draw_arrows(base, flow, valid_mask=valid_mask, **draw_kw)
|
||||
return orisys.util.draw_arrows(canvas, flow, valid_mask=valid_mask, **draw_kw)
|
||||
except Exception:
|
||||
return base
|
||||
|
||||
|
||||
@@ -14,6 +14,7 @@ from orisys_app_shared.acquisition.sinks import RecordSinkMixin
|
||||
# The packet and the geometry come from ``models``, so the finger worker does not
|
||||
# depend on the box worker; only genuinely shared helpers are imported from it.
|
||||
from orisys_app_shared.acquisition.models import (
|
||||
ContactObservation,
|
||||
FixedCameraMode,
|
||||
SdkPacket,
|
||||
ensure_bgr as _ensure_bgr,
|
||||
@@ -539,6 +540,11 @@ class FingerAcquisitionWorker(RecordSinkMixin):
|
||||
depth_valid = (depth is not None) and not compute_error
|
||||
if depth_valid:
|
||||
last_depth_frame_id = frame_id
|
||||
contact = _contact_from_sensor(
|
||||
tactile,
|
||||
frame_id=frame_id,
|
||||
compute_error=compute_error,
|
||||
)
|
||||
packet = SdkPacket(
|
||||
raw_frame=raw_frame,
|
||||
processed_frame=_ensure_bgr(unrolled),
|
||||
@@ -573,6 +579,7 @@ class FingerAcquisitionWorker(RecordSinkMixin):
|
||||
depth_valid=depth_valid,
|
||||
depth_frame_id=frame_id if depth_valid else last_depth_frame_id,
|
||||
forces_valid=bool(forces.get("valid", True)) and not compute_error,
|
||||
contact=contact,
|
||||
)
|
||||
with self._lock:
|
||||
self._latest = packet
|
||||
@@ -593,3 +600,30 @@ class FingerAcquisitionWorker(RecordSinkMixin):
|
||||
if self._stop.is_set():
|
||||
self._set_end_reason("stopped")
|
||||
self._status("Finger acquisition stopped")
|
||||
|
||||
|
||||
def _contact_from_sensor(sensor, *, frame_id: int, compute_error: str) -> ContactObservation:
|
||||
"""Copy the SDK contact centre after ``compute_contact``; never invent one."""
|
||||
if compute_error:
|
||||
return ContactObservation(found=False, valid=False, frame_id=int(frame_id))
|
||||
found = bool(getattr(sensor, "centroid_found", False))
|
||||
contact_ok = bool(getattr(sensor, "compute_contact_valid", True))
|
||||
centroid = getattr(sensor, "centroid", None)
|
||||
xy: tuple[float, float] | None = None
|
||||
if found and centroid is not None:
|
||||
try:
|
||||
arr = np.asarray(centroid, dtype=np.float64).reshape(-1)
|
||||
if arr.size >= 2 and np.all(np.isfinite(arr[:2])):
|
||||
xy = (float(arr[0]), float(arr[1]))
|
||||
else:
|
||||
found = False
|
||||
except Exception: # noqa: BLE001
|
||||
found = False
|
||||
xy = None
|
||||
return ContactObservation(
|
||||
centroid_uv_xy=xy,
|
||||
found=bool(found and xy is not None),
|
||||
valid=bool(contact_ok and found and xy is not None),
|
||||
frame_id=int(frame_id),
|
||||
domain_id="",
|
||||
)
|
||||
|
||||
@@ -83,12 +83,25 @@ class ImageFrame:
|
||||
#: True when the producer knows this frame's measurement is not current (the
|
||||
#: coordinator's "stale" state); the panel can say so under the picture.
|
||||
stale: bool = False
|
||||
#: Per-field provenance mirroring ``SdkPacket``; ``None`` means unstated.
|
||||
flow_valid: Optional[bool] = None
|
||||
flow_frame_id: Optional[int] = None
|
||||
depth_valid: Optional[bool] = None
|
||||
depth_frame_id: Optional[int] = None
|
||||
|
||||
def has_flow(self) -> bool:
|
||||
return self.flow is not None
|
||||
if self.flow is None:
|
||||
return False
|
||||
if self.flow_valid is False:
|
||||
return False
|
||||
return True
|
||||
|
||||
def has_depth(self) -> bool:
|
||||
return self.depth is not None
|
||||
if self.depth is None:
|
||||
return False
|
||||
if self.depth_valid is False:
|
||||
return False
|
||||
return True
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
@@ -195,6 +208,8 @@ def render_image(
|
||||
if wanted == DEPTH:
|
||||
# Never substitute another picture for a missing depth map: that made "no
|
||||
# depth" look like a depth reading. The panel says so instead.
|
||||
if frame.depth_valid is False:
|
||||
return None
|
||||
return depth_map_to_bgr(
|
||||
frame.depth,
|
||||
flow_colormap(theme),
|
||||
@@ -204,7 +219,7 @@ def render_image(
|
||||
if frame.image_bgr is None:
|
||||
return None
|
||||
if wanted in (ARROW_PURE, ARROW_STITCHED):
|
||||
if frame.flow is None:
|
||||
if frame.flow is None or frame.flow_valid is False:
|
||||
# The base picture with no arrows, and the plan's ``missing`` says why.
|
||||
return ensure_bgr(np.asarray(frame.image_bgr))
|
||||
return draw_arrow_display(
|
||||
|
||||
@@ -32,6 +32,22 @@ class FixedCameraMode:
|
||||
return f"{self.width}x{self.height} @{self.fps:g}"
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class ContactObservation:
|
||||
"""Contact centre in the UV domain, copied from the SDK after ``compute_contact``.
|
||||
|
||||
Units are UV pixels with ``(x, y)`` order. Missing or failed contact is
|
||||
``found=False`` / ``valid=False`` — never a fabricated centre. Old recordings
|
||||
without this field do not invent a yellow contact arrow.
|
||||
"""
|
||||
|
||||
centroid_uv_xy: Optional[tuple[float, float]] = None
|
||||
found: bool = False
|
||||
valid: bool = False
|
||||
frame_id: int = 0
|
||||
domain_id: str = ""
|
||||
|
||||
|
||||
@dataclass
|
||||
class SdkPacket:
|
||||
"""One acquired frame: the images, the force readout and its provenance."""
|
||||
@@ -103,6 +119,8 @@ class SdkPacket:
|
||||
#: Whether the force triplet belongs to this frame (False when the chain or the
|
||||
#: readout failed, whatever ``valid`` says about the record).
|
||||
forces_valid: Optional[bool] = None
|
||||
#: Contact centre from ``compute_contact``; optional so old call sites stay valid.
|
||||
contact: Optional[ContactObservation] = None
|
||||
|
||||
|
||||
def load_fixed_mode(config_ref: str | Path) -> FixedCameraMode:
|
||||
|
||||
@@ -30,6 +30,11 @@ from orisys_app_shared.force.session import (
|
||||
summarise,
|
||||
)
|
||||
from orisys_app_shared.force.units import DisplayForces, select_display_forces
|
||||
from orisys_app_shared.force.presentation import (
|
||||
DisplayForce,
|
||||
ForcePresentation,
|
||||
ZeroEstimate,
|
||||
)
|
||||
|
||||
__all__ = [
|
||||
"CHANNEL_KEYS",
|
||||
@@ -39,15 +44,18 @@ __all__ = [
|
||||
"CalSessionStore",
|
||||
"CalibrationPoint",
|
||||
"CalibrationRecorder",
|
||||
"DisplayForce",
|
||||
"DisplayForces",
|
||||
"FilterSettings",
|
||||
"ForceFilterChain",
|
||||
"ForcePresentation",
|
||||
"MetricHistory",
|
||||
"PlotState",
|
||||
"SelectionStats",
|
||||
"SessionSample",
|
||||
"SessionSlice",
|
||||
"SessionStore",
|
||||
"ZeroEstimate",
|
||||
"build_force_calibration_v2",
|
||||
"describe_state",
|
||||
"effective_filters",
|
||||
|
||||
@@ -0,0 +1,288 @@
|
||||
"""Display-only force presentation: baseline, deadband, and formatting.
|
||||
|
||||
Never mutates recorded samples. Curves, statistics and CSV keep the measurement
|
||||
values; value cards (and optional display copies) go through this layer.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass, field
|
||||
from typing import Optional, Sequence
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class ZeroEstimate:
|
||||
"""Outcome of an idle-window bias / noise estimate."""
|
||||
|
||||
offsets: tuple[float, float, float]
|
||||
deadbands: tuple[float, float, float]
|
||||
sample_count: int
|
||||
quality: str # "ok" | "insufficient" | "unstable" | "contaminated"
|
||||
message: str = ""
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class DisplayForce:
|
||||
"""One displayed triplet after offset + deadband (measurement unchanged)."""
|
||||
|
||||
values: tuple[float, float, float]
|
||||
suppressed: tuple[bool, bool, bool]
|
||||
unit: str
|
||||
zero_zone: bool
|
||||
|
||||
|
||||
@dataclass
|
||||
class ForcePresentation:
|
||||
"""Stateful display policy for force value cards.
|
||||
|
||||
Startup auto-baseline collects quiet samples, freezes offsets and deadbands,
|
||||
then applies a symmetric hysteretic deadband only to the display copy.
|
||||
"""
|
||||
|
||||
enabled: bool = True
|
||||
unit: str = "raw"
|
||||
baseline_seconds: float = 3.0
|
||||
min_samples: int = 30
|
||||
max_flow_p95: float = 0.4
|
||||
require_no_contact: bool = True
|
||||
#: Residual percentile used as the per-axis deadband after offset removal.
|
||||
deadband_percentile: float = 99.0
|
||||
#: Extra margin on top of the percentile residual.
|
||||
deadband_margin: float = 1.05
|
||||
#: Floor so tiny MAD windows do not produce a zero deadband.
|
||||
min_deadband: float = 50.0
|
||||
hysteresis_ratio: float = 0.8
|
||||
raw_significant_digits: int = 4
|
||||
newton_decimals: int = 3
|
||||
|
||||
_collecting: bool = field(default=True, init=False, repr=False)
|
||||
_samples: list[tuple[float, float, float]] = field(
|
||||
default_factory=list, init=False, repr=False
|
||||
)
|
||||
_sample_times: list[float] = field(default_factory=list, init=False, repr=False)
|
||||
_offsets: Optional[tuple[float, float, float]] = field(
|
||||
default=None, init=False, repr=False
|
||||
)
|
||||
_deadbands: Optional[tuple[float, float, float]] = field(
|
||||
default=None, init=False, repr=False
|
||||
)
|
||||
_suppressed: tuple[bool, bool, bool] = field(
|
||||
default=(False, False, False), init=False, repr=False
|
||||
)
|
||||
_last_estimate: Optional[ZeroEstimate] = field(
|
||||
default=None, init=False, repr=False
|
||||
)
|
||||
_t0: Optional[float] = field(default=None, init=False, repr=False)
|
||||
|
||||
def reset_session(self, *, unit: str = "raw") -> None:
|
||||
"""Start a new idle estimate for a session."""
|
||||
self.unit = "N" if str(unit).upper().startswith("N") else "raw"
|
||||
self._collecting = self.enabled and self.unit == "raw"
|
||||
self._samples = []
|
||||
self._sample_times = []
|
||||
self._offsets = None
|
||||
self._deadbands = None
|
||||
self._suppressed = (False, False, False)
|
||||
self._last_estimate = None
|
||||
self._t0 = None
|
||||
|
||||
def request_reestimate(self) -> None:
|
||||
"""Operator-triggered restart of the idle window."""
|
||||
self.reset_session(unit=self.unit)
|
||||
|
||||
@property
|
||||
def ready(self) -> bool:
|
||||
return self._offsets is not None and self._deadbands is not None
|
||||
|
||||
@property
|
||||
def last_estimate(self) -> Optional[ZeroEstimate]:
|
||||
return self._last_estimate
|
||||
|
||||
def observe(
|
||||
self,
|
||||
values: Sequence[float],
|
||||
*,
|
||||
t_rel_s: float,
|
||||
valid: bool = True,
|
||||
flow_p95: Optional[float] = None,
|
||||
contact_found: bool = False,
|
||||
) -> Optional[ZeroEstimate]:
|
||||
"""Feed one live sample into the startup estimator.
|
||||
|
||||
Returns a :class:`ZeroEstimate` when the window closes (ok or failed).
|
||||
"""
|
||||
if not self._collecting or self.unit != "raw":
|
||||
return None
|
||||
if self._t0 is None:
|
||||
self._t0 = float(t_rel_s)
|
||||
if not valid:
|
||||
return None
|
||||
if self.require_no_contact and contact_found:
|
||||
# Contaminated window — keep collecting but mark later if needed.
|
||||
return None
|
||||
if flow_p95 is not None and float(flow_p95) > self.max_flow_p95:
|
||||
return None
|
||||
sample = tuple(float(v) for v in values[:3])
|
||||
if len(sample) < 3 or not all(np.isfinite(sample)):
|
||||
return None
|
||||
self._samples.append(sample) # type: ignore[arg-type]
|
||||
self._sample_times.append(float(t_rel_s))
|
||||
elapsed = float(t_rel_s) - float(self._t0)
|
||||
if elapsed < self.baseline_seconds and len(self._samples) < self.min_samples * 2:
|
||||
return None
|
||||
if len(self._samples) < self.min_samples:
|
||||
estimate = ZeroEstimate(
|
||||
offsets=(0.0, 0.0, 0.0),
|
||||
deadbands=(0.0, 0.0, 0.0),
|
||||
sample_count=len(self._samples),
|
||||
quality="insufficient",
|
||||
message=f"only {len(self._samples)} quiet samples in {elapsed:.2f}s",
|
||||
)
|
||||
self._last_estimate = estimate
|
||||
self._collecting = False
|
||||
return estimate
|
||||
estimate = self.fit_zero(self._samples)
|
||||
self._last_estimate = estimate
|
||||
self._collecting = False
|
||||
if estimate.quality == "ok":
|
||||
self._offsets = estimate.offsets
|
||||
self._deadbands = estimate.deadbands
|
||||
return estimate
|
||||
|
||||
def fit_zero(self, samples: Sequence[Sequence[float]]) -> ZeroEstimate:
|
||||
"""Estimate offsets and deadbands from quiet samples (does not apply them)."""
|
||||
block = np.asarray(samples, dtype=np.float64)
|
||||
if block.ndim != 2 or block.shape[1] < 3 or block.shape[0] < 3:
|
||||
return ZeroEstimate(
|
||||
offsets=(0.0, 0.0, 0.0),
|
||||
deadbands=(0.0, 0.0, 0.0),
|
||||
sample_count=int(block.shape[0]) if block.ndim == 2 else 0,
|
||||
quality="insufficient",
|
||||
message="not enough samples to estimate a baseline",
|
||||
)
|
||||
offsets: list[float] = []
|
||||
deadbands: list[float] = []
|
||||
for axis in range(3):
|
||||
col = block[:, axis]
|
||||
finite = col[np.isfinite(col)]
|
||||
if finite.size < 3:
|
||||
return ZeroEstimate(
|
||||
offsets=(0.0, 0.0, 0.0),
|
||||
deadbands=(0.0, 0.0, 0.0),
|
||||
sample_count=int(block.shape[0]),
|
||||
quality="insufficient",
|
||||
message=f"axis {axis} lacks finite samples",
|
||||
)
|
||||
med = float(np.median(finite))
|
||||
residual = np.abs(finite - med)
|
||||
p = float(np.percentile(residual, self.deadband_percentile))
|
||||
db = max(self.min_deadband, p * self.deadband_margin)
|
||||
# Reject wildly unstable windows (p99 >> median magnitude scale).
|
||||
mad = float(np.median(residual))
|
||||
if mad > 0 and p > 20.0 * mad and p > 2000.0:
|
||||
return ZeroEstimate(
|
||||
offsets=(0.0, 0.0, 0.0),
|
||||
deadbands=(0.0, 0.0, 0.0),
|
||||
sample_count=int(block.shape[0]),
|
||||
quality="unstable",
|
||||
message=f"axis {axis} residual p{self.deadband_percentile:g}={p:.1f}",
|
||||
)
|
||||
offsets.append(med)
|
||||
deadbands.append(db)
|
||||
return ZeroEstimate(
|
||||
offsets=(offsets[0], offsets[1], offsets[2]),
|
||||
deadbands=(deadbands[0], deadbands[1], deadbands[2]),
|
||||
sample_count=int(block.shape[0]),
|
||||
quality="ok",
|
||||
)
|
||||
|
||||
def apply_estimate(self, estimate: ZeroEstimate) -> None:
|
||||
"""Explicitly freeze an estimate (e.g. after user confirmation)."""
|
||||
if estimate.quality != "ok":
|
||||
return
|
||||
self._offsets = estimate.offsets
|
||||
self._deadbands = estimate.deadbands
|
||||
self._collecting = False
|
||||
self._last_estimate = estimate
|
||||
|
||||
def accept(
|
||||
self,
|
||||
values: Sequence[float],
|
||||
*,
|
||||
unit: Optional[str] = None,
|
||||
available: tuple[bool, bool, bool] = (True, True, True),
|
||||
) -> DisplayForce:
|
||||
"""Return a display copy; never mutates ``values``."""
|
||||
unit = self.unit if unit is None else (
|
||||
"N" if str(unit).upper().startswith("N") else "raw"
|
||||
)
|
||||
raw = [float(v) for v in values[:3]]
|
||||
while len(raw) < 3:
|
||||
raw.append(float("nan"))
|
||||
if (
|
||||
not self.enabled
|
||||
or unit != "raw"
|
||||
or self._offsets is None
|
||||
or self._deadbands is None
|
||||
):
|
||||
return DisplayForce(
|
||||
values=(raw[0], raw[1], raw[2]),
|
||||
suppressed=(False, False, False),
|
||||
unit=unit,
|
||||
zero_zone=False,
|
||||
)
|
||||
|
||||
out: list[float] = []
|
||||
suppressed: list[bool] = []
|
||||
prev = self._suppressed
|
||||
for i in range(3):
|
||||
if not available[i] or not np.isfinite(raw[i]):
|
||||
out.append(raw[i])
|
||||
suppressed.append(False)
|
||||
continue
|
||||
centered = raw[i] - self._offsets[i]
|
||||
on = self._deadbands[i]
|
||||
off = on * self.hysteresis_ratio
|
||||
was = prev[i]
|
||||
if was:
|
||||
quiet = abs(centered) <= on
|
||||
else:
|
||||
quiet = abs(centered) <= off
|
||||
if quiet:
|
||||
out.append(0.0)
|
||||
suppressed.append(True)
|
||||
else:
|
||||
out.append(centered)
|
||||
suppressed.append(False)
|
||||
self._suppressed = (suppressed[0], suppressed[1], suppressed[2])
|
||||
return DisplayForce(
|
||||
values=(out[0], out[1], out[2]),
|
||||
suppressed=self._suppressed,
|
||||
unit=unit,
|
||||
zero_zone=any(self._suppressed),
|
||||
)
|
||||
|
||||
def format_force(self, value: float, unit: Optional[str] = None) -> str:
|
||||
"""Stable digit formatting for value cards."""
|
||||
unit = self.unit if unit is None else (
|
||||
"N" if str(unit).upper().startswith("N") else "raw"
|
||||
)
|
||||
if not np.isfinite(value):
|
||||
return "—"
|
||||
# Avoid signed zero after deadband.
|
||||
if value == 0.0 or value == -0.0:
|
||||
value = 0.0
|
||||
if unit == "N":
|
||||
return f"{value:.{self.newton_decimals}f}"
|
||||
digits = max(2, int(self.raw_significant_digits))
|
||||
abs_v = abs(value)
|
||||
if abs_v >= 1000:
|
||||
return f"{value:.0f}"
|
||||
if abs_v >= 100:
|
||||
return f"{value:.1f}"
|
||||
if abs_v >= 10:
|
||||
return f"{value:.2f}"
|
||||
# Significant figures for small residual readings.
|
||||
return f"{value:.{digits}g}"
|
||||
@@ -1,34 +1,80 @@
|
||||
"""Resolve app, config, and output paths for editable and frozen runs."""
|
||||
"""Resolve app, config, and output paths for editable and frozen runs.
|
||||
|
||||
Several desktop products share this package. Which one a process belongs to is
|
||||
decided once, at start-up:
|
||||
|
||||
* ``ORISYS_APP_ID`` — the directory name under ``apps/`` (e.g. ``studio``,
|
||||
``autocal``). Set it **before** importing anything from this package; an app
|
||||
that changes it later gets whichever root was resolved first, because these
|
||||
helpers are called during import of logging/prefs.
|
||||
* Otherwise the historic behaviour applies, which resolves ``apps/studio``.
|
||||
That keeps existing callers working without them passing anything.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import os
|
||||
import sys
|
||||
from pathlib import Path
|
||||
|
||||
#: Environment variable naming the app directory under ``apps/``.
|
||||
APP_ID_ENV = "ORISYS_APP_ID"
|
||||
|
||||
#: Fallback app when ``ORISYS_APP_ID`` is unset — the first product built here.
|
||||
DEFAULT_APP_ID = "studio"
|
||||
|
||||
|
||||
def is_frozen() -> bool:
|
||||
return bool(getattr(sys, "frozen", False))
|
||||
|
||||
|
||||
def _studio_root_from_shared() -> Path:
|
||||
"""Repo layout: shared/src/orisys_app_shared -> apps/studio."""
|
||||
def app_id() -> str:
|
||||
"""The app directory name for this process."""
|
||||
return os.environ.get(APP_ID_ENV, "").strip() or DEFAULT_APP_ID
|
||||
|
||||
|
||||
def apps_dir() -> Path:
|
||||
"""The ``apps/`` directory of the source tree."""
|
||||
here = Path(__file__).resolve()
|
||||
# .../orisys-app/shared/src/orisys_app_shared/paths.py
|
||||
# .../orisys-app/shared/src/orisys_app_shared/paths.py -> .../orisys-app
|
||||
repo_root = here.parents[3]
|
||||
return repo_root / "apps" / "studio"
|
||||
return repo_root / "apps"
|
||||
|
||||
|
||||
def _default_app_root_from_shared(app_id_value: str) -> Path:
|
||||
"""Repo layout: shared/src/orisys_app_shared -> apps/<app>."""
|
||||
return apps_dir() / app_id_value
|
||||
|
||||
|
||||
def app_root() -> Path:
|
||||
"""Return the studio app root (editable) or EXE directory (frozen)."""
|
||||
"""Return this app's root (editable) or the EXE directory (frozen)."""
|
||||
if is_frozen():
|
||||
return Path(sys.executable).resolve().parent
|
||||
# Prefer cwd override when launched from apps/studio
|
||||
|
||||
wanted = app_id()
|
||||
|
||||
# Launched from inside an app directory (``python apps/<app>/run.py``).
|
||||
cwd = Path.cwd().resolve()
|
||||
if (cwd / "config").is_dir() and (cwd / "run.py").is_file():
|
||||
return cwd
|
||||
if (cwd / "apps" / "studio" / "config").is_dir():
|
||||
return cwd / "apps" / "studio"
|
||||
return _studio_root_from_shared()
|
||||
|
||||
# Launched from the repo root.
|
||||
candidate = cwd / "apps" / wanted
|
||||
if (candidate / "config").is_dir():
|
||||
return candidate
|
||||
|
||||
# A named app that is not where we expected must not silently fall back to
|
||||
# another product's config: that would write one app's preferences into
|
||||
# another's tree.
|
||||
if wanted == DEFAULT_APP_ID:
|
||||
return _default_app_root_from_shared(wanted)
|
||||
unresolved = _default_app_root_from_shared(wanted)
|
||||
if (unresolved / "config").is_dir():
|
||||
return unresolved
|
||||
raise FileNotFoundError(
|
||||
f"{APP_ID_ENV}={wanted!r} does not name an app with a config directory "
|
||||
f"(looked in {cwd / 'apps' / wanted} and {unresolved})"
|
||||
)
|
||||
|
||||
|
||||
def config_dir() -> Path:
|
||||
|
||||
@@ -1,4 +1,10 @@
|
||||
"""Load/save preference.json with defaults merge."""
|
||||
"""Load/save preference.json with defaults merge.
|
||||
|
||||
The default schema here is the **studio's**. A second product passes its own
|
||||
schema to :func:`load_preferences` / :func:`save_preferences` instead of
|
||||
inheriting camera and 3D-pane keys it has no use for; omitting the argument
|
||||
keeps the historic behaviour, so existing callers are unaffected.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
@@ -44,6 +50,7 @@ _DEFAULTS: dict[str, Any] = {
|
||||
"image_view_mode": "arrows_pure",
|
||||
"arrow_rotation_deg": 90,
|
||||
"show_fps": False,
|
||||
"raw_auto_zero": True,
|
||||
#: The GL profile the 3D view is started with. One per process: the surface
|
||||
#: format is chosen before ``QApplication`` exists and cannot be switched later.
|
||||
"gl_profile": "desktop33",
|
||||
@@ -74,22 +81,31 @@ def _load_json(path: Path) -> dict[str, Any]:
|
||||
return data if isinstance(data, dict) else {}
|
||||
|
||||
|
||||
def default_preferences() -> dict[str, Any]:
|
||||
merged = deepcopy(_DEFAULTS)
|
||||
def default_preferences(schema: dict[str, Any] | None = None) -> dict[str, Any]:
|
||||
"""The schema's defaults, overlaid with any shipped ``preferences.default.json``.
|
||||
|
||||
Args:
|
||||
schema: the app's own defaults; ``None`` means this package's (studio's).
|
||||
"""
|
||||
merged = deepcopy(_DEFAULTS if schema is None else schema)
|
||||
merged.update(_load_json(preferences_default_path()))
|
||||
return merged
|
||||
|
||||
|
||||
def load_preferences() -> dict[str, Any]:
|
||||
prefs = default_preferences()
|
||||
def load_preferences(schema: dict[str, Any] | None = None) -> dict[str, Any]:
|
||||
"""Stored preferences, with schema defaults filling anything absent."""
|
||||
prefs = default_preferences(schema)
|
||||
prefs.update(_load_json(preference_path()))
|
||||
return prefs
|
||||
|
||||
|
||||
def save_preferences(prefs: dict[str, Any]) -> Path:
|
||||
def save_preferences(
|
||||
prefs: dict[str, Any], schema: dict[str, Any] | None = None
|
||||
) -> Path:
|
||||
"""Persist preferences, writing the schema's defaults for absent keys."""
|
||||
path = preference_path()
|
||||
path.parent.mkdir(parents=True, exist_ok=True)
|
||||
payload = default_preferences()
|
||||
payload = default_preferences(schema)
|
||||
payload.update(prefs)
|
||||
path.write_text(json.dumps(payload, indent=2, ensure_ascii=False) + "\n", encoding="utf-8")
|
||||
return path
|
||||
|
||||
@@ -15,6 +15,7 @@ from orisys_app_shared.scene3d.contracts import (
|
||||
LENGTH_UNIT_M,
|
||||
SENSOR_FRAME,
|
||||
UNAVAILABLE_DESCRIPTOR,
|
||||
ArrowInstances,
|
||||
CameraMatrices,
|
||||
CameraPreset,
|
||||
ColorScale,
|
||||
@@ -41,6 +42,7 @@ from orisys_app_shared.scene3d.contracts import (
|
||||
SceneState,
|
||||
SurfaceBinding,
|
||||
SurfaceFieldFrame,
|
||||
SurfaceMapping,
|
||||
ThreeDError,
|
||||
U32,
|
||||
VertexFields,
|
||||
@@ -49,6 +51,7 @@ from orisys_app_shared.scene3d.contracts import (
|
||||
freeze,
|
||||
rest_descriptor,
|
||||
summarize_parts,
|
||||
validate_arrow_instances,
|
||||
validate_assets,
|
||||
validate_binding,
|
||||
validate_display_settings,
|
||||
@@ -62,6 +65,7 @@ __all__ = [
|
||||
"LENGTH_UNIT_M",
|
||||
"SENSOR_FRAME",
|
||||
"UNAVAILABLE_DESCRIPTOR",
|
||||
"ArrowInstances",
|
||||
"CameraMatrices",
|
||||
"CameraPreset",
|
||||
"ColorScale",
|
||||
@@ -88,6 +92,7 @@ __all__ = [
|
||||
"SceneState",
|
||||
"SurfaceBinding",
|
||||
"SurfaceFieldFrame",
|
||||
"SurfaceMapping",
|
||||
"ThreeDError",
|
||||
"U32",
|
||||
"VertexFields",
|
||||
@@ -96,6 +101,7 @@ __all__ = [
|
||||
"freeze",
|
||||
"rest_descriptor",
|
||||
"summarize_parts",
|
||||
"validate_arrow_instances",
|
||||
"validate_assets",
|
||||
"validate_binding",
|
||||
"validate_display_settings",
|
||||
|
||||
@@ -42,7 +42,13 @@ from orisys_app_shared.scene3d.contracts import (
|
||||
validate_surface_frame,
|
||||
)
|
||||
from orisys_app_shared.scene3d.legacy import vertex_normals
|
||||
from orisys_app_shared.scene3d.overlays import (
|
||||
build_flow_overlay,
|
||||
build_normal_overlay,
|
||||
mesh_span_from_mapping,
|
||||
)
|
||||
from orisys_app_shared.scene3d.sampling import SurfaceSampler
|
||||
from orisys_app_shared.scene3d.visual_policy import VisualPolicy, select_flow_samples
|
||||
|
||||
#: Reported when a display gain is applied to a displacement that is only
|
||||
#: approximate. Informational: the picture is legitimate as an illustration, and
|
||||
@@ -75,6 +81,8 @@ class SceneComposer:
|
||||
#: draw, the picture is the asset's own geometry, and recomputing its normals
|
||||
#: every frame was pure cost (measured as the bulk of a 20k-vertex compose).
|
||||
self._rest_geometry: dict[str, tuple[np.ndarray, np.ndarray]] = {}
|
||||
#: Last UV grid of "arrow was on" samples for flow hysteresis.
|
||||
self._flow_previously_on: Optional[np.ndarray] = None
|
||||
|
||||
def compose(
|
||||
self,
|
||||
@@ -134,6 +142,9 @@ class SceneComposer:
|
||||
|
||||
# Measured before the frame is built, so the frame carries *its own* cost: the
|
||||
# shared ``last_stats`` object is rewritten by the next task.
|
||||
flow_arrows, contact_arrow = self._build_overlays(
|
||||
assets, frame, settings, diagnostics
|
||||
)
|
||||
total_ms = (time.perf_counter() - started) * 1000.0
|
||||
self.last_stats.total_ms = total_ms
|
||||
|
||||
@@ -162,10 +173,89 @@ class SceneComposer:
|
||||
# ``last_stats`` object is rewritten by the next task, so a reader on the
|
||||
# GUI thread would report another frame's cost.
|
||||
compose_ms=total_ms,
|
||||
flow_arrows=flow_arrows,
|
||||
contact_arrow=contact_arrow,
|
||||
)
|
||||
validate_render_frame(result, vertices=int(rest_positions.shape[0]))
|
||||
return result
|
||||
|
||||
def _build_overlays(
|
||||
self,
|
||||
assets: SceneAssets,
|
||||
frame: SurfaceFieldFrame,
|
||||
settings: DisplaySettings,
|
||||
diagnostics: list[Diagnostic],
|
||||
):
|
||||
flow_arrows = None
|
||||
contact_arrow = None
|
||||
mapping = assets.surface_mapping
|
||||
if mapping is None:
|
||||
if settings.flow_arrows_enabled or settings.contact_arrow_enabled:
|
||||
diagnostics.append(
|
||||
diagnostic(
|
||||
"field_unavailable",
|
||||
"this scene package has no UV surface mapping; overlays are hidden",
|
||||
)
|
||||
)
|
||||
return flow_arrows, contact_arrow
|
||||
|
||||
policy = VisualPolicy(
|
||||
flow_on_px=float(settings.flow_on_px),
|
||||
flow_off_px=float(settings.flow_off_px),
|
||||
flow_grid_spacing=int(settings.flow_grid_spacing),
|
||||
max_arrows=int(settings.max_arrows),
|
||||
).normalised()
|
||||
if (
|
||||
settings.flow_arrows_enabled
|
||||
and frame.flow_uv is not None
|
||||
and frame.flow_valid_mask is not None
|
||||
):
|
||||
try:
|
||||
flow = np.asarray(frame.flow_uv)
|
||||
h, w = int(flow.shape[0]), int(flow.shape[1])
|
||||
previously_on = self._flow_previously_on
|
||||
if previously_on is not None and previously_on.shape != (h, w):
|
||||
previously_on = None
|
||||
samples = select_flow_samples(
|
||||
flow,
|
||||
frame.flow_valid_mask,
|
||||
policy,
|
||||
previously_on=previously_on,
|
||||
)
|
||||
on_mask = np.zeros((h, w), dtype=bool)
|
||||
if samples.rows.size:
|
||||
on_mask[samples.rows, samples.cols] = True
|
||||
self._flow_previously_on = on_mask
|
||||
flow_arrows, extra = build_flow_overlay(
|
||||
samples,
|
||||
mapping,
|
||||
style=policy,
|
||||
normal_offset_m=mesh_span_from_mapping(mapping) * 0.002,
|
||||
)
|
||||
diagnostics.extend(extra)
|
||||
except Exception as exc: # noqa: BLE001
|
||||
diagnostics.append(
|
||||
diagnostic(
|
||||
"field_unavailable",
|
||||
f"flow overlay failed: {type(exc).__name__}: {exc}",
|
||||
)
|
||||
)
|
||||
flow_arrows = None
|
||||
self._flow_previously_on = None
|
||||
else:
|
||||
self._flow_previously_on = None
|
||||
|
||||
if settings.contact_arrow_enabled and frame.contact_valid:
|
||||
contact_arrow, extra = build_normal_overlay(
|
||||
contact_uv_xy=frame.contact_uv_xy,
|
||||
contact_found=frame.contact_found,
|
||||
contact_valid=frame.contact_valid,
|
||||
fn=float(frame.contact_fn),
|
||||
mapping=mapping,
|
||||
)
|
||||
diagnostics.extend(extra)
|
||||
return flow_arrows, contact_arrow
|
||||
|
||||
def _rest(self, assets: SceneAssets) -> tuple[np.ndarray, np.ndarray]:
|
||||
"""The asset's own positions and normals, computed once per asset.
|
||||
|
||||
|
||||
@@ -188,6 +188,51 @@ class SurfaceFieldFrame:
|
||||
#: only claim). When it is older than ``identity.frame_id`` the array is a stale
|
||||
#: measurement: a consumer must not colour it as the current instant.
|
||||
computed_frame_id: Optional[int] = None
|
||||
#: Optional flow field kept alongside a depth scalar so overlays can draw arrows
|
||||
#: without forcing the colour channel to switch away from depth.
|
||||
flow_uv: Optional[F32] = None # [H,W,2]
|
||||
flow_valid_mask: Optional[Mask] = None # [H,W]
|
||||
#: Contact centre in the same UV domain as ``scalar``; absent on older packets.
|
||||
contact_uv_xy: Optional[tuple[float, float]] = None
|
||||
contact_found: bool = False
|
||||
contact_valid: bool = False
|
||||
#: Normal-force reading used only for the contact-arrow length mapping.
|
||||
contact_fn: float = 0.0
|
||||
|
||||
|
||||
ArrowKind = Literal["flow", "contact_normal"]
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class ArrowInstances:
|
||||
"""Per-frame arrow overlay geometry in the sensor frame (metres).
|
||||
|
||||
Directions are unit vectors; lengths are in the same units as origins.
|
||||
``M`` is bounded by the visual policy so the GL layer never uploads unbounded
|
||||
Python mesh objects.
|
||||
"""
|
||||
|
||||
origins: F32 # [M,3]
|
||||
directions: F32 # [M,3], unit
|
||||
lengths: F32 # [M]
|
||||
colors: F32 # [M,4], RGBA 0–1
|
||||
kind: ArrowKind
|
||||
#: Optional per-arrow thickness scale relative to a template (contact arrows).
|
||||
thickness_scales: Optional[F32] = None # [M]
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class SurfaceMapping:
|
||||
"""UV-domain positions and normals in the sensor frame, for overlay geometry.
|
||||
|
||||
Optional on older packages: without it the composer draws the scalar surface
|
||||
only and reports a soft overlay degradation.
|
||||
"""
|
||||
|
||||
shape_hw: tuple[int, int]
|
||||
positions_sensor_m: F32 # [H,W,3]
|
||||
normals_sensor: F32 # [H,W,3], unit where valid
|
||||
valid: Mask # [H,W]
|
||||
|
||||
|
||||
# -------------------------------------------------------------------- assets
|
||||
@@ -489,6 +534,9 @@ class SceneAssets:
|
||||
#: Where this package may be displayed (stage 5). ``None`` means "not locked",
|
||||
#: which is what every package written before the lock existed carries.
|
||||
platform_lock: Optional[PlatformLock] = None
|
||||
#: UV→scene positions/normals for overlays. Optional so older packages degrade
|
||||
#: to scalar-only rendering without failing to load.
|
||||
surface_mapping: Optional[SurfaceMapping] = None
|
||||
|
||||
def part(self, part_id: str) -> Optional[MeshPart]:
|
||||
for item in self.parts:
|
||||
@@ -530,6 +578,9 @@ class SceneCapabilities:
|
||||
displacement_available: bool = False
|
||||
displacement_quality: Optional[Quality] = None
|
||||
source_kind: str = "live"
|
||||
#: Whether flow / contact overlays can be built for this source.
|
||||
flow_arrows_available: bool = False
|
||||
contact_arrow_available: bool = False
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
@@ -548,6 +599,17 @@ class DisplaySettings:
|
||||
#: producer did not claim for this frame is refused before either is coloured (see
|
||||
#: ``orisys_studio.scene3d.adapters``).
|
||||
field: str = "depth"
|
||||
#: Draw optical-flow arrows on the sensing surface (independent of ``field``).
|
||||
flow_arrows_enabled: bool = True
|
||||
#: Draw the yellow contact-normal arrow at the contact centre.
|
||||
contact_arrow_enabled: bool = True
|
||||
#: Flow magnitude (px) at which arrows appear / disappear (hysteresis: on > off).
|
||||
flow_on_px: float = 0.4
|
||||
flow_off_px: float = 0.3
|
||||
#: UV grid spacing shared by 2D and 3D flow arrows.
|
||||
flow_grid_spacing: int = 10
|
||||
#: Cap on arrows uploaded per overlay kind.
|
||||
max_arrows: int = 400
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
@@ -579,6 +641,9 @@ class RenderFrame:
|
||||
#: What composing this frame cost, measured where it was composed. Carried per frame
|
||||
#: rather than read from the composer's shared counter, which the next task rewrites.
|
||||
compose_ms: float = 0.0
|
||||
#: Optional overlays; absent on older packages / frames without mapping.
|
||||
flow_arrows: Optional[ArrowInstances] = None
|
||||
contact_arrow: Optional[ArrowInstances] = None
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
@@ -1031,6 +1096,37 @@ def validate_render_frame(frame: RenderFrame, *, vertices: int) -> None:
|
||||
raise error("field_domain_mismatch", "render scalar is float32 and valid is bool")
|
||||
if not frame.asset_hash.strip() or not frame.geometry_id.strip():
|
||||
raise error("geometry_mismatch", "a render frame must name its asset and geometry")
|
||||
for label, arrows in (
|
||||
("flow_arrows", frame.flow_arrows),
|
||||
("contact_arrow", frame.contact_arrow),
|
||||
):
|
||||
if arrows is None:
|
||||
continue
|
||||
validate_arrow_instances(arrows, name=label)
|
||||
|
||||
|
||||
def validate_arrow_instances(arrows: ArrowInstances, *, name: str = "arrows") -> None:
|
||||
"""Shapes and finiteness for one overlay batch."""
|
||||
for field_name, array, width in (
|
||||
("origins", arrows.origins, 3),
|
||||
("directions", arrows.directions, 3),
|
||||
("lengths", arrows.lengths, None),
|
||||
("colors", arrows.colors, 4),
|
||||
):
|
||||
_readonly_c_contiguous(f"{name}.{field_name}", array)
|
||||
count = int(arrows.origins.shape[0])
|
||||
_shape(f"{name}.origins", arrows.origins, (count, 3))
|
||||
_shape(f"{name}.directions", arrows.directions, (count, 3))
|
||||
_shape(f"{name}.lengths", arrows.lengths, (count,))
|
||||
_shape(f"{name}.colors", arrows.colors, (count, 4))
|
||||
_finite(f"{name}.origins", arrows.origins)
|
||||
_finite(f"{name}.directions", arrows.directions)
|
||||
_finite(f"{name}.lengths", arrows.lengths)
|
||||
if arrows.thickness_scales is not None:
|
||||
_readonly_c_contiguous(f"{name}.thickness_scales", arrows.thickness_scales)
|
||||
_shape(f"{name}.thickness_scales", arrows.thickness_scales, (count,))
|
||||
if arrows.kind not in ("flow", "contact_normal"):
|
||||
raise error("field_domain_mismatch", f"{name}: unknown arrow kind {arrows.kind!r}")
|
||||
|
||||
|
||||
#: The descriptor a reference frame carries: nothing is measured, and the UI has
|
||||
|
||||
@@ -53,6 +53,7 @@ from orisys_app_shared.scene3d.digest import (
|
||||
geometry_id,
|
||||
source_geometry_digest,
|
||||
)
|
||||
from orisys_app_shared.scene3d.overlays import build_surface_mapping
|
||||
|
||||
#: Keys the importer needs, and what it does when they are missing.
|
||||
MESH_KEYS = ("vertices", "faces", "uv_data", "loop_to_vertex")
|
||||
@@ -573,6 +574,16 @@ class LegacyFingerImporter:
|
||||
# source vertex, and an operator asking "which triangle is this?"
|
||||
# deserves an answer rather than a guess.
|
||||
source_vertex_of_render=freeze(source_of_render, dtype=np.int32),
|
||||
surface_mapping=(
|
||||
build_surface_mapping(
|
||||
domain.world_xyz,
|
||||
domain.valid_mask,
|
||||
meters_per_source_unit=units,
|
||||
transform_sensor_from_source=transform,
|
||||
)
|
||||
if domain.world_xyz is not None
|
||||
else None
|
||||
),
|
||||
)
|
||||
return assets
|
||||
|
||||
|
||||
@@ -0,0 +1,501 @@
|
||||
"""NumPy overlay geometry for 3D flow and contact-normal arrows.
|
||||
|
||||
Ported from the old Open3D example semantics without importing Open3D or the
|
||||
examples tree. Outputs :class:`ArrowInstances` for the GL backend.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
from typing import Literal, Optional
|
||||
|
||||
import numpy as np
|
||||
|
||||
from orisys_app_shared.scene3d.contracts import (
|
||||
ArrowInstances,
|
||||
Diagnostic,
|
||||
SurfaceMapping,
|
||||
diagnostic,
|
||||
freeze,
|
||||
)
|
||||
from orisys_app_shared.scene3d.visual_policy import FlowSamples, VisualPolicy
|
||||
|
||||
ForceStrengthMode = Literal["log", "power", "linear"]
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class ContactArrowStyle:
|
||||
"""Visual length/thickness mapping for the yellow contact-normal arrow."""
|
||||
|
||||
normal_force_scale: float = 1.0
|
||||
min_length_fraction: float = 0.02
|
||||
max_length_fraction: float = 0.30
|
||||
min_thickness_scale: float = 0.20
|
||||
max_thickness_scale: float = 1.0
|
||||
reference_normal_force: float = 10000.0
|
||||
force_strength_mode: ForceStrengthMode = "log"
|
||||
force_strength_gamma: float = 0.5
|
||||
color_rgba: tuple[float, float, float, float] = (1.0, 0.92, 0.10, 1.0)
|
||||
#: Lift the origin slightly along the outward normal to avoid z-fighting.
|
||||
normal_offset_fraction: float = 0.005
|
||||
|
||||
|
||||
DEFAULT_CONTACT_ARROW_STYLE = ContactArrowStyle()
|
||||
|
||||
FLOW_TAIL_RGBA = (38 / 255.0, 235 / 255.0, 89 / 255.0, 0.85)
|
||||
FLOW_HEAD_RGBA = (13 / 255.0, 115 / 255.0, 31 / 255.0, 0.95)
|
||||
|
||||
|
||||
def _empty_arrows(kind: str) -> ArrowInstances:
|
||||
z3 = freeze(np.zeros((0, 3), dtype=np.float32), dtype=np.float32)
|
||||
z1 = freeze(np.zeros((0,), dtype=np.float32), dtype=np.float32)
|
||||
z4 = freeze(np.zeros((0, 4), dtype=np.float32), dtype=np.float32)
|
||||
return ArrowInstances(
|
||||
origins=z3,
|
||||
directions=z3,
|
||||
lengths=z1,
|
||||
colors=z4,
|
||||
kind=kind, # type: ignore[arg-type]
|
||||
thickness_scales=z1,
|
||||
)
|
||||
|
||||
|
||||
def force_visual_strength(
|
||||
force: float,
|
||||
*,
|
||||
style: ContactArrowStyle = DEFAULT_CONTACT_ARROW_STYLE,
|
||||
) -> float:
|
||||
"""Map normal force Fn to a unitless strength in [0, 1] for length/thickness."""
|
||||
reference = style.reference_normal_force
|
||||
if force <= 0.0 or reference <= 0.0:
|
||||
return 0.0
|
||||
ratio = force / reference
|
||||
if style.force_strength_mode == "linear":
|
||||
strength = ratio
|
||||
elif style.force_strength_mode == "power":
|
||||
strength = ratio ** style.force_strength_gamma
|
||||
else:
|
||||
strength = np.log1p(ratio) / np.log1p(1.0)
|
||||
return float(np.clip(strength, 0.0, 1.0))
|
||||
|
||||
|
||||
def length_from_force(
|
||||
force: float,
|
||||
mesh_span: float,
|
||||
*,
|
||||
style: ContactArrowStyle = DEFAULT_CONTACT_ARROW_STYLE,
|
||||
) -> float:
|
||||
strength = force_visual_strength(force, style=style)
|
||||
frac = style.min_length_fraction + (
|
||||
style.max_length_fraction - style.min_length_fraction
|
||||
) * strength
|
||||
return float(mesh_span * frac * style.normal_force_scale)
|
||||
|
||||
|
||||
def thickness_scale_from_force(
|
||||
force: float,
|
||||
*,
|
||||
style: ContactArrowStyle = DEFAULT_CONTACT_ARROW_STYLE,
|
||||
) -> float:
|
||||
strength = force_visual_strength(force, style=style)
|
||||
return float(
|
||||
style.min_thickness_scale
|
||||
+ (style.max_thickness_scale - style.min_thickness_scale) * strength
|
||||
)
|
||||
|
||||
|
||||
def mesh_span_from_mapping(mapping: SurfaceMapping) -> float:
|
||||
positions = np.asarray(mapping.positions_sensor_m, dtype=np.float64)
|
||||
valid = np.asarray(mapping.valid, dtype=bool)
|
||||
if not np.any(valid):
|
||||
return 1e-3
|
||||
pts = positions[valid]
|
||||
extent = np.ptp(pts, axis=0)
|
||||
return float(max(np.max(extent), 1e-6))
|
||||
|
||||
|
||||
def build_flow_overlay(
|
||||
samples: FlowSamples,
|
||||
mapping: SurfaceMapping,
|
||||
*,
|
||||
style: Optional[VisualPolicy] = None,
|
||||
normal_offset_m: float = 0.0,
|
||||
) -> tuple[ArrowInstances, tuple[Diagnostic, ...]]:
|
||||
"""Map shared UV flow samples onto the sensing surface."""
|
||||
del style # lengths come from UV tip deltas projected into scene metres
|
||||
if samples.rows.size == 0:
|
||||
return _empty_arrows("flow"), ()
|
||||
|
||||
h, w = mapping.shape_hw
|
||||
positions = np.asarray(mapping.positions_sensor_m, dtype=np.float64)
|
||||
normals = np.asarray(mapping.normals_sensor, dtype=np.float64)
|
||||
valid = np.asarray(mapping.valid, dtype=bool)
|
||||
if positions.shape[:2] != (h, w) or normals.shape[:2] != (h, w):
|
||||
return _empty_arrows("flow"), (
|
||||
diagnostic(
|
||||
"geometry_mismatch",
|
||||
"surface mapping shape disagrees with its declared domain",
|
||||
),
|
||||
)
|
||||
|
||||
rows = np.asarray(samples.rows, dtype=np.int32)
|
||||
cols = np.asarray(samples.cols, dtype=np.int32)
|
||||
end_x = np.clip(np.round(cols + samples.dx).astype(np.int32), 0, w - 1)
|
||||
end_y = np.clip(np.round(rows + samples.dy).astype(np.int32), 0, h - 1)
|
||||
|
||||
inside = (
|
||||
(rows >= 0)
|
||||
& (rows < h)
|
||||
& (cols >= 0)
|
||||
& (cols < w)
|
||||
& valid[rows, cols]
|
||||
& valid[end_y, end_x]
|
||||
)
|
||||
if not np.any(inside):
|
||||
return _empty_arrows("flow"), ()
|
||||
|
||||
rows = rows[inside]
|
||||
cols = cols[inside]
|
||||
end_x = end_x[inside]
|
||||
end_y = end_y[inside]
|
||||
origins = positions[rows, cols].copy()
|
||||
ends = positions[end_y, end_x]
|
||||
normals_o = normals[rows, cols]
|
||||
if normal_offset_m:
|
||||
origins = origins + normal_offset_m * normals_o
|
||||
|
||||
vectors = ends - origins
|
||||
lengths = np.linalg.norm(vectors, axis=1)
|
||||
keep = lengths > 1e-9
|
||||
if not np.any(keep):
|
||||
return _empty_arrows("flow"), ()
|
||||
origins = origins[keep]
|
||||
vectors = vectors[keep]
|
||||
lengths = lengths[keep]
|
||||
directions = vectors / lengths[:, None]
|
||||
|
||||
colors = np.tile(np.asarray(FLOW_TAIL_RGBA, dtype=np.float32), (len(origins), 1))
|
||||
# Stronger arrows tip toward the head colour.
|
||||
t = np.clip(lengths / max(float(np.percentile(lengths, 95)), 1e-9), 0.0, 1.0)
|
||||
head = np.asarray(FLOW_HEAD_RGBA, dtype=np.float32)
|
||||
colors = colors * (1.0 - t)[:, None] + head * t[:, None]
|
||||
|
||||
return (
|
||||
ArrowInstances(
|
||||
origins=freeze(origins, dtype=np.float32),
|
||||
directions=freeze(directions, dtype=np.float32),
|
||||
lengths=freeze(lengths, dtype=np.float32),
|
||||
colors=freeze(colors, dtype=np.float32),
|
||||
kind="flow",
|
||||
),
|
||||
(),
|
||||
)
|
||||
|
||||
|
||||
def build_normal_overlay(
|
||||
*,
|
||||
contact_uv_xy: Optional[tuple[float, float]],
|
||||
contact_found: bool,
|
||||
contact_valid: bool,
|
||||
fn: float,
|
||||
mapping: SurfaceMapping,
|
||||
style: ContactArrowStyle = DEFAULT_CONTACT_ARROW_STYLE,
|
||||
) -> tuple[ArrowInstances, tuple[Diagnostic, ...]]:
|
||||
"""Yellow arrow at the contact centre along the local surface normal.
|
||||
|
||||
Length is a *display* mapping of FN (log/power), not a calibrated newton
|
||||
vector. The arrow points opposite the outward normal (into the contact).
|
||||
"""
|
||||
if not contact_found or not contact_valid or contact_uv_xy is None:
|
||||
return _empty_arrows("contact_normal"), ()
|
||||
force = max(float(fn), 0.0)
|
||||
if force <= 0.0 or not np.isfinite(force):
|
||||
return _empty_arrows("contact_normal"), ()
|
||||
|
||||
h, w = mapping.shape_hw
|
||||
cx = int(round(float(contact_uv_xy[0])))
|
||||
cy = int(round(float(contact_uv_xy[1])))
|
||||
if not (0 <= cy < h and 0 <= cx < w):
|
||||
return _empty_arrows("contact_normal"), (
|
||||
diagnostic(
|
||||
"field_unavailable",
|
||||
"contact centre lies outside the UV domain",
|
||||
x=str(cx),
|
||||
y=str(cy),
|
||||
),
|
||||
)
|
||||
valid = np.asarray(mapping.valid, dtype=bool)
|
||||
if not bool(valid[cy, cx]):
|
||||
return _empty_arrows("contact_normal"), (
|
||||
diagnostic(
|
||||
"field_unavailable",
|
||||
"contact centre is outside the valid sensing mask",
|
||||
x=str(cx),
|
||||
y=str(cy),
|
||||
),
|
||||
)
|
||||
|
||||
origin = np.asarray(mapping.positions_sensor_m[cy, cx], dtype=np.float64)
|
||||
normal = np.asarray(mapping.normals_sensor[cy, cx], dtype=np.float64)
|
||||
if not (np.all(np.isfinite(origin)) and np.all(np.isfinite(normal))):
|
||||
return _empty_arrows("contact_normal"), ()
|
||||
n_norm = float(np.linalg.norm(normal))
|
||||
if n_norm < 1e-8:
|
||||
return _empty_arrows("contact_normal"), ()
|
||||
direction = -normal / n_norm
|
||||
span = mesh_span_from_mapping(mapping)
|
||||
length = length_from_force(force, span, style=style)
|
||||
length = float(
|
||||
np.clip(
|
||||
length,
|
||||
span * style.min_length_fraction,
|
||||
span * style.max_length_fraction,
|
||||
)
|
||||
)
|
||||
if length <= 0.0:
|
||||
return _empty_arrows("contact_normal"), ()
|
||||
offset = span * style.normal_offset_fraction
|
||||
origin = origin + offset * (normal / n_norm)
|
||||
thickness = thickness_scale_from_force(force, style=style)
|
||||
color = np.asarray(style.color_rgba, dtype=np.float32).reshape(1, 4)
|
||||
|
||||
return (
|
||||
ArrowInstances(
|
||||
origins=freeze(origin.reshape(1, 3), dtype=np.float32),
|
||||
directions=freeze(direction.reshape(1, 3), dtype=np.float32),
|
||||
lengths=freeze(np.asarray([length], dtype=np.float32), dtype=np.float32),
|
||||
colors=freeze(color, dtype=np.float32),
|
||||
kind="contact_normal",
|
||||
thickness_scales=freeze(
|
||||
np.asarray([thickness], dtype=np.float32), dtype=np.float32
|
||||
),
|
||||
),
|
||||
(),
|
||||
)
|
||||
|
||||
|
||||
def compute_surface_frames(
|
||||
world_xyz: np.ndarray,
|
||||
valid_mask: np.ndarray,
|
||||
) -> tuple[np.ndarray, np.ndarray, np.ndarray]:
|
||||
"""Estimate tangent_x, tangent_y and unit normals on a UV grid.
|
||||
|
||||
Mirrors the SDK ``compute_uv_surface_frames`` contract without importing it.
|
||||
"""
|
||||
xyz = np.asarray(world_xyz, dtype=np.float64)
|
||||
mask = np.asarray(valid_mask, dtype=bool)
|
||||
h, w = mask.shape
|
||||
tangent_x = np.zeros((h, w, 3), dtype=np.float64)
|
||||
tangent_y = np.zeros((h, w, 3), dtype=np.float64)
|
||||
normals = np.zeros((h, w, 3), dtype=np.float64)
|
||||
|
||||
# Forward differences where possible, else backward.
|
||||
dx = np.zeros_like(xyz)
|
||||
dy = np.zeros_like(xyz)
|
||||
dx[:, :-1] = xyz[:, 1:] - xyz[:, :-1]
|
||||
dx[:, -1] = xyz[:, -1] - xyz[:, -2] if w > 1 else 0.0
|
||||
dy[:-1, :] = xyz[1:, :] - xyz[:-1, :]
|
||||
dy[-1, :] = xyz[-1, :] - xyz[-2, :] if h > 1 else 0.0
|
||||
|
||||
# Invalidate differences that leave the mask.
|
||||
right_ok = np.zeros((h, w), dtype=bool)
|
||||
left_ok = np.zeros((h, w), dtype=bool)
|
||||
down_ok = np.zeros((h, w), dtype=bool)
|
||||
up_ok = np.zeros((h, w), dtype=bool)
|
||||
right_ok[:, :-1] = mask[:, :-1] & mask[:, 1:]
|
||||
left_ok[:, 1:] = mask[:, 1:] & mask[:, :-1]
|
||||
down_ok[:-1, :] = mask[:-1, :] & mask[1:, :]
|
||||
up_ok[1:, :] = mask[1:, :] & mask[:-1, :]
|
||||
|
||||
use_fwd_x = right_ok
|
||||
use_back_x = (~right_ok) & left_ok
|
||||
dx_back = np.zeros_like(xyz)
|
||||
dx_back[:, 1:] = xyz[:, 1:] - xyz[:, :-1]
|
||||
tx = np.where(use_fwd_x[..., None], dx, 0.0)
|
||||
tx = np.where(use_back_x[..., None], dx_back, tx)
|
||||
# Fallback axis for isolated pixels.
|
||||
isolated_x = mask & ~(use_fwd_x | use_back_x)
|
||||
tx[isolated_x] = np.array([1.0, 0.0, 0.0])
|
||||
|
||||
use_fwd_y = down_ok
|
||||
use_back_y = (~down_ok) & up_ok
|
||||
dy_back = np.zeros_like(xyz)
|
||||
dy_back[1:, :] = xyz[1:, :] - xyz[:-1, :]
|
||||
ty = np.where(use_fwd_y[..., None], dy, 0.0)
|
||||
ty = np.where(use_back_y[..., None], dy_back, ty)
|
||||
isolated_y = mask & ~(use_fwd_y | use_back_y)
|
||||
ty[isolated_y] = np.array([0.0, 1.0, 0.0])
|
||||
|
||||
n = np.cross(tx, ty)
|
||||
n_norm = np.linalg.norm(n, axis=-1, keepdims=True)
|
||||
bad = n_norm[..., 0] < 1e-12
|
||||
n = np.divide(n, np.maximum(n_norm, 1e-12), out=np.zeros_like(n))
|
||||
n[bad] = np.array([0.0, 0.0, 1.0])
|
||||
|
||||
tx_norm = np.linalg.norm(tx, axis=-1, keepdims=True)
|
||||
ty_norm = np.linalg.norm(ty, axis=-1, keepdims=True)
|
||||
tangent_x = np.divide(tx, np.maximum(tx_norm, 1e-12), out=np.zeros_like(tx))
|
||||
tangent_y = np.divide(ty, np.maximum(ty_norm, 1e-12), out=np.zeros_like(ty))
|
||||
normals = n
|
||||
tangent_x[~mask] = 0.0
|
||||
tangent_y[~mask] = 0.0
|
||||
normals[~mask] = 0.0
|
||||
return tangent_x, tangent_y, normals
|
||||
|
||||
|
||||
def build_surface_mapping(
|
||||
world_xyz: np.ndarray,
|
||||
valid_mask: np.ndarray,
|
||||
*,
|
||||
meters_per_source_unit: float,
|
||||
transform_sensor_from_source: np.ndarray,
|
||||
) -> SurfaceMapping:
|
||||
"""Convert a legacy UV ``world_xyz`` grid into sensor-metre ``SurfaceMapping``."""
|
||||
units = float(meters_per_source_unit)
|
||||
if not np.isfinite(units) or units <= 0.0:
|
||||
raise ValueError("meters_per_source_unit must be finite and positive")
|
||||
transform = np.asarray(transform_sensor_from_source, dtype=np.float64).reshape(4, 4)
|
||||
xyz = np.asarray(world_xyz, dtype=np.float64) * units
|
||||
mask = np.asarray(valid_mask, dtype=bool)
|
||||
h, w = mask.shape
|
||||
flat = xyz.reshape(-1, 3)
|
||||
ones = np.ones((flat.shape[0], 1), dtype=np.float64)
|
||||
homo = np.concatenate([flat, ones], axis=1)
|
||||
sensor = (homo @ transform.T)[:, :3].reshape(h, w, 3)
|
||||
_, _, normals = compute_surface_frames(sensor, mask)
|
||||
# Rotate is already baked into sensor positions; recompute normals in sensor frame.
|
||||
return SurfaceMapping(
|
||||
shape_hw=(h, w),
|
||||
positions_sensor_m=freeze(sensor, dtype=np.float32),
|
||||
normals_sensor=freeze(normals, dtype=np.float32),
|
||||
valid=freeze(mask, dtype=np.bool_),
|
||||
)
|
||||
|
||||
|
||||
def _rotation_matrix_z_to(direction: np.ndarray) -> np.ndarray:
|
||||
direction = np.asarray(direction, dtype=np.float64).reshape(3)
|
||||
norm = np.linalg.norm(direction)
|
||||
if norm < 1e-8:
|
||||
return np.eye(3, dtype=np.float64)
|
||||
direction = direction / norm
|
||||
z_axis = np.array([0.0, 0.0, 1.0], dtype=np.float64)
|
||||
cross = np.cross(z_axis, direction)
|
||||
dot = float(np.dot(z_axis, direction))
|
||||
if dot > 0.9999:
|
||||
return np.eye(3, dtype=np.float64)
|
||||
if dot < -0.9999:
|
||||
return np.diag([1.0, -1.0, -1.0]).astype(np.float64)
|
||||
skew = np.array(
|
||||
[
|
||||
[0.0, -cross[2], cross[1]],
|
||||
[cross[2], 0.0, -cross[0]],
|
||||
[-cross[1], cross[0], 0.0],
|
||||
],
|
||||
dtype=np.float64,
|
||||
)
|
||||
return np.eye(3, dtype=np.float64) + skew + skew @ skew * (1.0 / (1.0 + dot))
|
||||
|
||||
|
||||
def _cylinder_template(resolution: int = 8) -> tuple[np.ndarray, np.ndarray]:
|
||||
"""Unit-radius, unit-height cylinder along +Z, centred on the origin."""
|
||||
angles = np.linspace(0.0, 2.0 * np.pi, resolution, endpoint=False)
|
||||
ring = np.column_stack([np.cos(angles), np.sin(angles), np.zeros(resolution)])
|
||||
bottom = ring.copy()
|
||||
bottom[:, 2] = -0.5
|
||||
top = ring.copy()
|
||||
top[:, 2] = 0.5
|
||||
vertices = np.vstack([bottom, top])
|
||||
triangles = []
|
||||
for i in range(resolution):
|
||||
j = (i + 1) % resolution
|
||||
triangles.append([i, j, resolution + i])
|
||||
triangles.append([j, resolution + j, resolution + i])
|
||||
return (
|
||||
np.asarray(vertices, dtype=np.float64),
|
||||
np.asarray(triangles, dtype=np.int32),
|
||||
)
|
||||
|
||||
|
||||
def _cone_template(resolution: int = 8) -> tuple[np.ndarray, np.ndarray]:
|
||||
"""Unit-radius cone with apex at z=1 and base at z=0."""
|
||||
angles = np.linspace(0.0, 2.0 * np.pi, resolution, endpoint=False)
|
||||
base = np.column_stack([np.cos(angles), np.sin(angles), np.zeros(resolution)])
|
||||
apex = np.array([[0.0, 0.0, 1.0]], dtype=np.float64)
|
||||
vertices = np.vstack([base, apex])
|
||||
apex_i = resolution
|
||||
triangles = [[i, (i + 1) % resolution, apex_i] for i in range(resolution)]
|
||||
return (
|
||||
np.asarray(vertices, dtype=np.float64),
|
||||
np.asarray(triangles, dtype=np.int32),
|
||||
)
|
||||
|
||||
|
||||
def arrow_instances_to_mesh(
|
||||
arrows: ArrowInstances,
|
||||
*,
|
||||
default_radius_fraction: float = 0.02,
|
||||
) -> tuple[np.ndarray, np.ndarray, np.ndarray]:
|
||||
"""Merge arrow instances into one triangle mesh (positions, triangles, colors)."""
|
||||
empty_v = np.zeros((0, 3), dtype=np.float32)
|
||||
empty_t = np.zeros((0, 3), dtype=np.uint32)
|
||||
empty_c = np.zeros((0, 4), dtype=np.float32)
|
||||
count = int(arrows.origins.shape[0])
|
||||
if count == 0:
|
||||
return empty_v, empty_t, empty_c
|
||||
|
||||
cyl_v, cyl_t = _cylinder_template()
|
||||
cone_v, cone_t = _cone_template()
|
||||
origins = np.asarray(arrows.origins, dtype=np.float64)
|
||||
directions = np.asarray(arrows.directions, dtype=np.float64)
|
||||
lengths = np.asarray(arrows.lengths, dtype=np.float64)
|
||||
colors = np.asarray(arrows.colors, dtype=np.float64)
|
||||
thickness = (
|
||||
np.asarray(arrows.thickness_scales, dtype=np.float64)
|
||||
if arrows.thickness_scales is not None
|
||||
else np.ones(count, dtype=np.float64)
|
||||
)
|
||||
|
||||
mean_length = float(np.mean(lengths)) if count else 1.0
|
||||
base_radius = max(mean_length * default_radius_fraction, 1e-5)
|
||||
|
||||
parts_v: list[np.ndarray] = []
|
||||
parts_t: list[np.ndarray] = []
|
||||
parts_c: list[np.ndarray] = []
|
||||
offset = 0
|
||||
for i in range(count):
|
||||
length = float(lengths[i])
|
||||
if length <= 1e-9:
|
||||
continue
|
||||
direction = directions[i]
|
||||
origin = origins[i]
|
||||
radius = base_radius * float(max(thickness[i], 0.05))
|
||||
rotation = _rotation_matrix_z_to(direction)
|
||||
# Tail: 75% cylinder, head: 25% cone.
|
||||
tail_len = length * 0.75
|
||||
head_len = length * 0.25
|
||||
# Cylinder from 0..tail_len along +Z then rotate.
|
||||
scaled = cyl_v.copy()
|
||||
scaled[:, 0] *= radius
|
||||
scaled[:, 1] *= radius
|
||||
scaled[:, 2] = (scaled[:, 2] + 0.5) * tail_len
|
||||
tail = scaled @ rotation.T + origin
|
||||
# Cone from tail_len..length.
|
||||
scaled_c = cone_v.copy()
|
||||
scaled_c[:, 0] *= radius * 1.8
|
||||
scaled_c[:, 1] *= radius * 1.8
|
||||
scaled_c[:, 2] = scaled_c[:, 2] * head_len + tail_len
|
||||
head = scaled_c @ rotation.T + origin
|
||||
|
||||
for verts, tris in ((tail, cyl_t), (head, cone_t)):
|
||||
parts_v.append(verts)
|
||||
parts_t.append(tris + offset)
|
||||
parts_c.append(np.tile(colors[i], (len(verts), 1)))
|
||||
offset += len(verts)
|
||||
|
||||
if not parts_v:
|
||||
return empty_v, empty_t, empty_c
|
||||
return (
|
||||
np.vstack(parts_v).astype(np.float32),
|
||||
np.vstack(parts_t).astype(np.uint32),
|
||||
np.vstack(parts_c).astype(np.float32),
|
||||
)
|
||||
@@ -100,7 +100,12 @@ SENSING_ARRAYS = {
|
||||
"sensing/sample_indices.npy": ("i4", 2),
|
||||
"sensing/sample_weights.npy": ("f4", 2),
|
||||
}
|
||||
OPTIONAL_ARRAYS = {"sensing/source_vertex.npy": ("i4", 1)}
|
||||
OPTIONAL_ARRAYS = {
|
||||
"sensing/source_vertex.npy": ("i4", 1),
|
||||
"sensing/surface_positions.npy": ("f4", 3),
|
||||
"sensing/surface_normals.npy": ("f4", 3),
|
||||
"sensing/surface_valid.npy": ("i4", 2),
|
||||
}
|
||||
HOUSING_ARRAYS = {
|
||||
"housing/positions.npy": ("f4", 2),
|
||||
"housing/normals.npy": ("f4", 2),
|
||||
@@ -228,6 +233,17 @@ def _arrays_of(assets: SceneAssets) -> dict[str, np.ndarray]:
|
||||
arrays["sensing/source_vertex.npy"] = np.ascontiguousarray(
|
||||
assets.source_vertex_of_render
|
||||
)
|
||||
if assets.surface_mapping is not None:
|
||||
mapping = assets.surface_mapping
|
||||
arrays["sensing/surface_positions.npy"] = np.ascontiguousarray(
|
||||
mapping.positions_sensor_m
|
||||
)
|
||||
arrays["sensing/surface_normals.npy"] = np.ascontiguousarray(
|
||||
mapping.normals_sensor
|
||||
)
|
||||
arrays["sensing/surface_valid.npy"] = np.ascontiguousarray(
|
||||
np.asarray(mapping.valid, dtype=np.int32)
|
||||
)
|
||||
for part in assets.parts:
|
||||
if part.role == "housing":
|
||||
arrays["housing/positions.npy"] = np.ascontiguousarray(part.positions_sensor_m)
|
||||
@@ -684,6 +700,19 @@ def scene_from_contents(
|
||||
recomputed=recomputed_binding[:16],
|
||||
)
|
||||
source = arrays.get("sensing/source_vertex.npy")
|
||||
surface_pos = arrays.get("sensing/surface_positions.npy")
|
||||
surface_nrm = arrays.get("sensing/surface_normals.npy")
|
||||
surface_valid = arrays.get("sensing/surface_valid.npy")
|
||||
surface_mapping = None
|
||||
if surface_pos is not None and surface_nrm is not None and surface_valid is not None:
|
||||
from orisys_app_shared.scene3d.contracts import SurfaceMapping
|
||||
|
||||
surface_mapping = SurfaceMapping(
|
||||
shape_hw=(shape[0], shape[1]),
|
||||
positions_sensor_m=freeze(surface_pos, dtype=np.float32),
|
||||
normals_sensor=freeze(surface_nrm, dtype=np.float32),
|
||||
valid=freeze(np.asarray(surface_valid, dtype=bool), dtype=np.bool_),
|
||||
)
|
||||
compatible = dict(manifest.get("compatible") or {})
|
||||
source_geometry = dict(manifest.get("source_geometry") or {})
|
||||
assets = SceneAssets(
|
||||
@@ -719,6 +748,7 @@ def scene_from_contents(
|
||||
compatible_hardware_revisions=tuple(
|
||||
str(item) for item in (compatible.get("hardware_revisions") or ())
|
||||
),
|
||||
surface_mapping=surface_mapping,
|
||||
)
|
||||
validate_assets(assets, expected)
|
||||
return assets
|
||||
|
||||
@@ -22,6 +22,7 @@ from orisys_app_shared.scene3d.contracts import (
|
||||
MeshPart,
|
||||
SceneAssets,
|
||||
SurfaceBinding,
|
||||
SurfaceMapping,
|
||||
freeze,
|
||||
)
|
||||
from orisys_app_shared.scene3d.digest import field_domain_id, geometry_id
|
||||
@@ -130,6 +131,19 @@ def _binding() -> SurfaceBinding:
|
||||
)
|
||||
|
||||
|
||||
def _surface_mapping(plate: MeshPart) -> SurfaceMapping:
|
||||
"""Flat UV→scene mapping aligned with the synthetic plate grid."""
|
||||
positions = np.asarray(plate.positions_sensor_m, dtype=np.float32).reshape(GRID, GRID, 3)
|
||||
normals = np.asarray(plate.normals_sensor, dtype=np.float32).reshape(GRID, GRID, 3)
|
||||
valid = np.ones((GRID, GRID), dtype=bool)
|
||||
return SurfaceMapping(
|
||||
shape_hw=(GRID, GRID),
|
||||
positions_sensor_m=freeze(positions, dtype=np.float32),
|
||||
normals_sensor=freeze(normals, dtype=np.float32),
|
||||
valid=freeze(valid, dtype=np.bool_),
|
||||
)
|
||||
|
||||
|
||||
def poc_scene() -> SceneAssets:
|
||||
"""The synthetic scene: one sensing plate, one housing box, one preset."""
|
||||
plate = _plate()
|
||||
@@ -166,6 +180,7 @@ def poc_scene() -> SceneAssets:
|
||||
),
|
||||
field_domain_id=POC_DOMAIN_ID,
|
||||
shape_hw=(GRID, GRID),
|
||||
surface_mapping=_surface_mapping(plate),
|
||||
)
|
||||
|
||||
|
||||
|
||||
@@ -0,0 +1,231 @@
|
||||
"""Shared visual policy for 2D/3D flow sampling and domain masks.
|
||||
|
||||
No Qt, no GL, no SDK: both the arrow canvas and the 3D overlays consume the same
|
||||
sample points and thresholds so "arrows respond / depth does not" stays a
|
||||
measurement difference rather than a display bug.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
from typing import Optional
|
||||
|
||||
import numpy as np
|
||||
|
||||
from orisys_app_shared.scene3d.contracts import Diagnostic, Mask, diagnostic, error, freeze
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class VisualPolicy:
|
||||
"""Units are explicit: flow thresholds are pixels; depth thresholds are relative."""
|
||||
|
||||
flow_on_px: float = 0.4
|
||||
flow_off_px: float = 0.3
|
||||
flow_grid_spacing: int = 10
|
||||
flow_arrow_scale: float = 1.0
|
||||
flow_edge_margin: int = 8
|
||||
flow_length_margin: float = 0.8
|
||||
max_arrows: int = 400
|
||||
depth_on: float = 0.0
|
||||
depth_off: float = 0.0
|
||||
#: Inside / outside colours for the pure-color 2D canvas (BGR).
|
||||
island_bgr: tuple[int, int, int] = (0x16, 0x13, 0x12)
|
||||
exterior_bgr: tuple[int, int, int] = (0x0A, 0x09, 0x08)
|
||||
|
||||
def normalised(self) -> "VisualPolicy":
|
||||
on = max(0.0, float(self.flow_on_px))
|
||||
off = max(0.0, float(self.flow_off_px))
|
||||
if on < off:
|
||||
on, off = off, on
|
||||
spacing = max(1, int(self.flow_grid_spacing))
|
||||
return VisualPolicy(
|
||||
flow_on_px=on,
|
||||
flow_off_px=off,
|
||||
flow_grid_spacing=spacing,
|
||||
flow_arrow_scale=max(1e-9, float(self.flow_arrow_scale)),
|
||||
flow_edge_margin=max(0, int(self.flow_edge_margin)),
|
||||
flow_length_margin=max(0.05, float(self.flow_length_margin)),
|
||||
max_arrows=max(1, int(self.max_arrows)),
|
||||
depth_on=float(self.depth_on),
|
||||
depth_off=float(self.depth_off),
|
||||
island_bgr=tuple(int(v) for v in self.island_bgr),
|
||||
exterior_bgr=tuple(int(v) for v in self.exterior_bgr),
|
||||
)
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class FlowSamples:
|
||||
"""Shared UV sample points for 2D and 3D flow arrows."""
|
||||
|
||||
rows: np.ndarray # [K] int
|
||||
cols: np.ndarray # [K] int
|
||||
dx: np.ndarray # [K] float — tip offset in UV pixels
|
||||
dy: np.ndarray # [K] float
|
||||
magnitude: np.ndarray # [K] float — |2*flow|
|
||||
|
||||
|
||||
def build_domain_mask(
|
||||
geometry_mask: Optional[np.ndarray],
|
||||
texture_roi: Optional[np.ndarray] = None,
|
||||
*,
|
||||
shape_hw: Optional[tuple[int, int]] = None,
|
||||
) -> tuple[Optional[Mask], tuple[Diagnostic, ...]]:
|
||||
"""Strict intersection of geometry and optional texture ROI.
|
||||
|
||||
Shape mismatches raise rather than silently resize: a wrong mask invents
|
||||
valid samples or erases the finger outline.
|
||||
"""
|
||||
if geometry_mask is None and texture_roi is None:
|
||||
return None, ()
|
||||
diagnostics: list[Diagnostic] = []
|
||||
geo = None if geometry_mask is None else np.asarray(geometry_mask, dtype=bool)
|
||||
roi = None if texture_roi is None else np.asarray(texture_roi, dtype=bool)
|
||||
if shape_hw is not None:
|
||||
expected = (int(shape_hw[0]), int(shape_hw[1]))
|
||||
for name, array in (("geometry_mask", geo), ("texture_roi", roi)):
|
||||
if array is None:
|
||||
continue
|
||||
if tuple(array.shape[:2]) != expected:
|
||||
raise error(
|
||||
"field_domain_mismatch",
|
||||
f"{name} shape {tuple(array.shape[:2])} does not match "
|
||||
f"domain {expected[0]}x{expected[1]}",
|
||||
)
|
||||
if geo is not None and roi is not None:
|
||||
if tuple(geo.shape[:2]) != tuple(roi.shape[:2]):
|
||||
raise error(
|
||||
"field_domain_mismatch",
|
||||
"geometry_mask and texture_roi describe different grids",
|
||||
geometry=f"{geo.shape[0]}x{geo.shape[1]}",
|
||||
texture=f"{roi.shape[0]}x{roi.shape[1]}",
|
||||
)
|
||||
mask = geo & roi
|
||||
elif geo is not None:
|
||||
mask = geo
|
||||
else:
|
||||
mask = roi
|
||||
diagnostics.append(
|
||||
diagnostic(
|
||||
"geometry_unverified",
|
||||
"display_mask uses texture_roi alone; no geometry_mask was given",
|
||||
)
|
||||
)
|
||||
return freeze(mask, dtype=np.bool_), tuple(diagnostics)
|
||||
|
||||
|
||||
def select_flow_samples(
|
||||
flow: np.ndarray,
|
||||
mask: Optional[np.ndarray],
|
||||
policy: VisualPolicy,
|
||||
*,
|
||||
previously_on: Optional[np.ndarray] = None,
|
||||
) -> FlowSamples:
|
||||
"""Sample optical flow on a UV grid with hysteresis and mask clipping.
|
||||
|
||||
``previously_on`` is a boolean grid of the same shape as ``flow`` (or flat
|
||||
over the candidate points) used for on/off hysteresis; pass ``None`` to use
|
||||
``flow_on_px`` only (stateless).
|
||||
"""
|
||||
policy = policy.normalised()
|
||||
flow = np.asarray(flow, dtype=np.float64)
|
||||
if flow.ndim != 3 or flow.shape[-1] != 2:
|
||||
raise error("field_domain_mismatch", "flow must be [H,W,2]")
|
||||
h, w = int(flow.shape[0]), int(flow.shape[1])
|
||||
spacing = policy.flow_grid_spacing
|
||||
# Never erase a small domain: an edge larger than half the short side leaves
|
||||
# no legal sample points (the synthetic 3×3 PoC is the extreme case).
|
||||
edge = min(int(policy.flow_edge_margin), max(0, (min(h, w) - 1) // 2))
|
||||
rr = np.arange(spacing // 2, h, spacing, dtype=np.int32)
|
||||
cc = np.arange(spacing // 2, w, spacing, dtype=np.int32)
|
||||
rr, cc = np.meshgrid(rr, cc, indexing="ij")
|
||||
rr = rr.reshape(-1)
|
||||
cc = cc.reshape(-1)
|
||||
|
||||
disp = 2.0 * flow[rr, cc, :]
|
||||
mag = np.linalg.norm(disp, axis=1)
|
||||
arrow_scale = policy.flow_arrow_scale
|
||||
dx = disp[:, 0] / arrow_scale
|
||||
dy = disp[:, 1] / arrow_scale
|
||||
length = np.sqrt(dx * dx + dy * dy)
|
||||
margin = policy.flow_length_margin
|
||||
scale = np.where(
|
||||
length > spacing * margin,
|
||||
spacing / np.maximum(length, 1e-9),
|
||||
1.0,
|
||||
) * margin
|
||||
dx = dx * scale
|
||||
dy = dy * scale
|
||||
end_x = np.clip(np.round(cc + dx).astype(np.int32), 0, w - 1)
|
||||
end_y = np.clip(np.round(rr + dy).astype(np.int32), 0, h - 1)
|
||||
|
||||
threshold = policy.flow_on_px
|
||||
if previously_on is not None:
|
||||
prev = np.asarray(previously_on, dtype=bool)
|
||||
if prev.shape == (h, w):
|
||||
was_on = prev[rr, cc]
|
||||
threshold = np.where(was_on, policy.flow_off_px, policy.flow_on_px)
|
||||
|
||||
valid = (
|
||||
(cc >= edge)
|
||||
& (cc < w - edge)
|
||||
& (rr >= edge)
|
||||
& (rr < h - edge)
|
||||
& (mag >= threshold)
|
||||
& np.isfinite(mag)
|
||||
)
|
||||
if mask is not None:
|
||||
m = np.asarray(mask, dtype=bool)
|
||||
if tuple(m.shape[:2]) != (h, w):
|
||||
raise error(
|
||||
"field_domain_mismatch",
|
||||
"flow mask shape does not match the flow field",
|
||||
mask=f"{m.shape[0]}x{m.shape[1]}",
|
||||
flow=f"{h}x{w}",
|
||||
)
|
||||
valid &= m[rr, cc]
|
||||
valid &= m[end_y, end_x]
|
||||
|
||||
if not np.any(valid):
|
||||
empty = np.empty(0, dtype=np.int32)
|
||||
empty_f = np.empty(0, dtype=np.float64)
|
||||
return FlowSamples(rows=empty, cols=empty, dx=empty_f, dy=empty_f, magnitude=empty_f)
|
||||
|
||||
idx = np.flatnonzero(valid)
|
||||
if idx.size > policy.max_arrows:
|
||||
# Keep the strongest magnitudes when the grid is dense.
|
||||
order = np.argsort(mag[idx])[::-1][: policy.max_arrows]
|
||||
idx = idx[order]
|
||||
|
||||
return FlowSamples(
|
||||
rows=rr[idx].astype(np.int32, copy=False),
|
||||
cols=cc[idx].astype(np.int32, copy=False),
|
||||
dx=dx[idx].astype(np.float64, copy=False),
|
||||
dy=dy[idx].astype(np.float64, copy=False),
|
||||
magnitude=mag[idx].astype(np.float64, copy=False),
|
||||
)
|
||||
|
||||
|
||||
def paint_domain_background(
|
||||
shape_hw: tuple[int, int],
|
||||
mask: Optional[np.ndarray],
|
||||
*,
|
||||
island_bgr: tuple[int, int, int],
|
||||
exterior_bgr: tuple[int, int, int],
|
||||
) -> np.ndarray:
|
||||
"""BGR canvas with distinct colours inside / outside the UV island."""
|
||||
h, w = int(shape_hw[0]), int(shape_hw[1])
|
||||
canvas = np.empty((h, w, 3), dtype=np.uint8)
|
||||
canvas[:, :] = tuple(int(v) for v in exterior_bgr)
|
||||
if mask is None:
|
||||
canvas[:, :] = tuple(int(v) for v in island_bgr)
|
||||
return canvas
|
||||
m = np.asarray(mask, dtype=bool)
|
||||
if tuple(m.shape[:2]) != (h, w):
|
||||
raise error(
|
||||
"field_domain_mismatch",
|
||||
"background mask shape does not match the canvas",
|
||||
mask=f"{m.shape[0]}x{m.shape[1]}",
|
||||
canvas=f"{h}x{w}",
|
||||
)
|
||||
canvas[m] = tuple(int(v) for v in island_bgr)
|
||||
return canvas
|
||||
@@ -46,6 +46,8 @@
|
||||
"brand_bar_bg": "#12315D",
|
||||
"brand_bar_fg": "#FFFFFF",
|
||||
"brand_bar_fg_muted": "#B9C8E4",
|
||||
"status_bar_bg": "#0B1017",
|
||||
"status_bar_text": "#B9C2CF",
|
||||
"arrow_canvas": "#0B1830",
|
||||
"flow_ramp": "#0B1830,#6E4AD0,#FA02F3,#FF8A6B,#D8FF00"
|
||||
}
|
||||
|
||||
@@ -46,6 +46,8 @@
|
||||
"brand_bar_bg": "#12315D",
|
||||
"brand_bar_fg": "#FFFFFF",
|
||||
"brand_bar_fg_muted": "#B9C8E4",
|
||||
"status_bar_bg": "#EDF1F8",
|
||||
"status_bar_text": "#141A24",
|
||||
"arrow_canvas": "#12315D",
|
||||
"flow_ramp": "#12315D,#6E4AD0,#FA02F3,#FF8A6B,#D8FF00"
|
||||
}
|
||||
|
||||
@@ -0,0 +1,45 @@
|
||||
"""Typed, side-effect-free client for the ZMotion ZMC controller DLLs.
|
||||
|
||||
Only the calls the AutoCal platform control needs are wrapped here, rather than
|
||||
copying the vendor's 5381-line ``zmcdll/zauxdllPython.py``. The vendor wrapper
|
||||
is transcribed method by method -- its Chinese docstrings are kept because they
|
||||
document the parameter contract (the English reference for the same calls would
|
||||
be the ``ZAUXDLL``/``ZAux_Direct_*`` API documentation).
|
||||
|
||||
Differences from the vendor file that matter:
|
||||
|
||||
* **No import side effects.** Importing this package does not probe, load, or
|
||||
``print()`` anything about the native library. The DLL is resolved and loaded
|
||||
on first :func:`load_dll`, so a machine with no controller still imports the
|
||||
whole app.
|
||||
* **No path guessing at import time.** See :mod:`.loader`.
|
||||
* **Errors are values, not tuples.** Every wrapper returns a
|
||||
:class:`ZmotionResult` (or :class:`ZmotionValue` for reads), so a caller can
|
||||
never silently treat a failed read's zero as data.
|
||||
|
||||
No Qt binding is imported here, deliberately: this module is usable from a
|
||||
headless test process and from either binding.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from .client import (
|
||||
ZAuxError,
|
||||
ZmotionClient,
|
||||
ZmotionDllNotFound,
|
||||
ZmotionResult,
|
||||
ZmotionValue,
|
||||
)
|
||||
from .loader import ZAUXDLL_DLL, ZMOTION_DLL, load_dll, resolve_dll_dir
|
||||
|
||||
__all__ = [
|
||||
"ZAUXDLL_DLL",
|
||||
"ZMOTION_DLL",
|
||||
"ZAuxError",
|
||||
"ZmotionClient",
|
||||
"ZmotionDllNotFound",
|
||||
"ZmotionResult",
|
||||
"ZmotionValue",
|
||||
"load_dll",
|
||||
"resolve_dll_dir",
|
||||
]
|
||||
@@ -0,0 +1,468 @@
|
||||
"""Typed wrappers around the ZMotion ``ZAux_*`` controller API.
|
||||
|
||||
Every wrapper returns :class:`ZmotionResult` (commands) or :class:`ZmotionValue`
|
||||
(reads) instead of the vendor's ``(code, ctypes_value)`` pairs, so a failed read
|
||||
can never be mistaken for data. The ~25 calls here are exactly the ones the
|
||||
AutoCal platform control uses; see ``ZAUXDLL2.h``
|
||||
(``shared/resources/zmotion/x64/``) for the underlying contract.
|
||||
|
||||
Loading is lazy: constructing :class:`ZmotionClient` does not touch the native
|
||||
library, so importing the app on a machine with no controller (or no DLL at all)
|
||||
is always safe. Call :meth:`ZmotionClient.load` explicitly, or let
|
||||
:meth:`ZmotionClient.connect` do it.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import ctypes
|
||||
from dataclasses import dataclass
|
||||
from typing import Any # noqa: F401 (kept for ZmotionValue.value's annotation)
|
||||
|
||||
from .loader import ZmotionDllNotFound, load_dll, resolve_dll_dir
|
||||
|
||||
__all__ = [
|
||||
"ZAuxError",
|
||||
"ZmotionClient",
|
||||
"ZmotionDllNotFound",
|
||||
"ZmotionResult",
|
||||
"ZmotionValue",
|
||||
]
|
||||
|
||||
|
||||
class ZAuxError(RuntimeError):
|
||||
"""A ``ZAux_*`` call returned a non-zero code.
|
||||
|
||||
Carries the numeric code so callers can map it to a localised message
|
||||
without re-parsing a string.
|
||||
"""
|
||||
|
||||
def __init__(self, code: int, detail: str = "") -> None:
|
||||
self.code = int(code)
|
||||
self.detail = detail
|
||||
message = f"ZMotion error {self.code}"
|
||||
if detail:
|
||||
message = f"{message}: {detail}"
|
||||
super().__init__(message)
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class ZmotionResult:
|
||||
"""Outcome of a command that returns only a status code."""
|
||||
|
||||
code: int
|
||||
|
||||
@property
|
||||
def ok(self) -> bool:
|
||||
return self.code == 0
|
||||
|
||||
def raise_for_status(self, detail: str = "") -> "ZmotionResult":
|
||||
if not self.ok:
|
||||
raise ZAuxError(self.code, detail)
|
||||
return self
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class ZmotionValue:
|
||||
"""Outcome of a read: a status code plus the value it produced."""
|
||||
|
||||
code: int
|
||||
value: Any
|
||||
|
||||
@property
|
||||
def ok(self) -> bool:
|
||||
return self.code == 0
|
||||
|
||||
@property
|
||||
def value_or_none(self) -> Any | None:
|
||||
"""The value when the read succeeded, otherwise ``None``.
|
||||
|
||||
Callers that want a default must say so themselves; silently using a
|
||||
failed read's zero is what makes a wrong number look like a
|
||||
measurement.
|
||||
"""
|
||||
return self.value if self.ok else None
|
||||
|
||||
def raise_for_status(self, detail: str = "") -> "ZmotionValue":
|
||||
if not self.ok:
|
||||
raise ZAuxError(self.code, detail)
|
||||
return self
|
||||
|
||||
|
||||
class ZmotionClient:
|
||||
"""The controller connection, one instance per process.
|
||||
|
||||
Not thread-safe: the underlying DLL handle is shared state. AutoCal owns one
|
||||
client inside a single poller thread (see
|
||||
``orisys_autocal.motion.poller``) and never calls it from two threads.
|
||||
"""
|
||||
|
||||
def __init__(self) -> None:
|
||||
self._dll: ctypes.CDLL | None = None
|
||||
self._handle = ctypes.c_void_p()
|
||||
self._dll_dir: str | None = None
|
||||
|
||||
# ------------------------------------------------------------------ state
|
||||
|
||||
@property
|
||||
def loaded(self) -> bool:
|
||||
return self._dll is not None
|
||||
|
||||
@property
|
||||
def connected(self) -> bool:
|
||||
return self._handle.value is not None
|
||||
|
||||
@property
|
||||
def dll_dir(self) -> str | None:
|
||||
"""Directory the runtime was loaded from, for logs and About boxes."""
|
||||
return self._dll_dir
|
||||
|
||||
def load(self) -> "ZmotionClient":
|
||||
"""Resolve and load the native runtime. Idempotent."""
|
||||
if self._dll is None:
|
||||
resolved = resolve_dll_dir()
|
||||
self._dll = load_dll(resolved)
|
||||
self._dll_dir = str(resolved)
|
||||
return self
|
||||
|
||||
def _require_handle(self) -> ctypes.c_void_p:
|
||||
if self._handle.value is None:
|
||||
raise ZAuxError(-2, "no controller connection is open")
|
||||
return self._handle
|
||||
|
||||
# ------------------------------------------------------------ connection
|
||||
|
||||
def open_eth(self, ip: str) -> ZmotionResult:
|
||||
"""Connect to a controller by IPv4 address (string form)."""
|
||||
self.load()
|
||||
self._handle = ctypes.c_void_p()
|
||||
ret = self._dll.ZAux_OpenEth( # type: ignore[union-attr]
|
||||
ip.encode("utf-8"), ctypes.pointer(self._handle)
|
||||
)
|
||||
if ret != 0:
|
||||
self._handle = ctypes.c_void_p()
|
||||
return ZmotionResult(int(ret))
|
||||
|
||||
def open_com(self, com_id: int) -> ZmotionResult:
|
||||
"""Connect over a serial port by index."""
|
||||
self.load()
|
||||
self._handle = ctypes.c_void_p()
|
||||
ret = self._dll.ZAux_OpenCom( # type: ignore[union-attr]
|
||||
ctypes.c_int(com_id), ctypes.pointer(self._handle)
|
||||
)
|
||||
if ret != 0:
|
||||
self._handle = ctypes.c_void_p()
|
||||
return ZmotionResult(int(ret))
|
||||
|
||||
def open_pci(self, card_num: int) -> ZmotionResult:
|
||||
"""Connect to a PCI/PCIe card by card index."""
|
||||
self.load()
|
||||
self._handle = ctypes.c_void_p()
|
||||
ret = self._dll.ZAux_OpenPci( # type: ignore[union-attr]
|
||||
ctypes.c_int(card_num), ctypes.pointer(self._handle)
|
||||
)
|
||||
if ret != 0:
|
||||
self._handle = ctypes.c_void_p()
|
||||
return ZmotionResult(int(ret))
|
||||
|
||||
def fast_open(self, conn_type: int, connect_string: str, timeout_ms: int) -> ZmotionResult:
|
||||
"""Open a local/simulated controller (``ZAux_FastOpen``)."""
|
||||
self.load()
|
||||
self._handle = ctypes.c_void_p()
|
||||
ret = self._dll.ZAux_FastOpen( # type: ignore[union-attr]
|
||||
ctypes.c_int(conn_type),
|
||||
connect_string.encode("utf-8"),
|
||||
ctypes.c_uint32(timeout_ms),
|
||||
ctypes.pointer(self._handle),
|
||||
)
|
||||
if ret != 0:
|
||||
self._handle = ctypes.c_void_p()
|
||||
return ZmotionResult(int(ret))
|
||||
|
||||
def close(self) -> ZmotionResult:
|
||||
"""Close the connection. Safe to call when nothing is open."""
|
||||
if self._dll is None or self._handle.value is None:
|
||||
self._handle = ctypes.c_void_p()
|
||||
return ZmotionResult(0)
|
||||
ret = self._dll.ZAux_Close(self._handle) # type: ignore[union-attr]
|
||||
self._handle = ctypes.c_void_p()
|
||||
return ZmotionResult(int(ret))
|
||||
|
||||
def search_eth(self, buffer_length: int = 10240, timeout_ms: int = 200) -> ZmotionValue:
|
||||
"""Scan the local network segment for controllers.
|
||||
|
||||
Returns the whitespace-separated address list as text. Does not need an
|
||||
open connection.
|
||||
"""
|
||||
self.load()
|
||||
buffer = (ctypes.c_char * int(buffer_length))()
|
||||
ret = self._dll.ZAux_SearchEthlist( # type: ignore[union-attr]
|
||||
buffer, ctypes.c_uint32(buffer_length), ctypes.c_uint32(timeout_ms)
|
||||
)
|
||||
text = buffer.value.decode("utf-8", errors="ignore") if buffer.value else ""
|
||||
return ZmotionValue(int(ret), text.strip())
|
||||
|
||||
# ----------------------------------------------------------- BAS / command
|
||||
|
||||
def bas_download(self, filename: str, run_mode: int = 1) -> ZmotionResult:
|
||||
"""Download a ``.bas`` program to the controller ROM.
|
||||
|
||||
``run_mode=1`` downloads to ROM, which makes the controller run the
|
||||
program's task after the configured delay (see ``ECAT.bas``).
|
||||
"""
|
||||
ret = self._dll.ZAux_BasDown( # type: ignore[union-attr]
|
||||
self._require_handle(), filename.encode("utf-8"), ctypes.c_uint32(run_mode)
|
||||
)
|
||||
return ZmotionResult(int(ret))
|
||||
|
||||
def execute(self, command: str) -> ZmotionValue:
|
||||
"""Run an online command string (e.g. ``RUN "ECAT.bas",4``)."""
|
||||
ret = self._dll.ZAux_Execute( # type: ignore[union-attr]
|
||||
self._require_handle(),
|
||||
command.encode("utf-8"),
|
||||
None,
|
||||
ctypes.c_uint32(0),
|
||||
)
|
||||
return ZmotionValue(int(ret), command)
|
||||
|
||||
def get_user_var(self, name: str) -> ZmotionValue:
|
||||
"""Read a global variable from the running ``.bas`` program.
|
||||
|
||||
The vendor wrapper returns a ``c_float`` here while other reads return a
|
||||
plain float; both are normalised to a Python float.
|
||||
"""
|
||||
value = ctypes.c_float()
|
||||
ret = self._dll.ZAux_Direct_GetUserVar( # type: ignore[union-attr]
|
||||
self._require_handle(), name.encode("utf-8"), ctypes.byref(value)
|
||||
)
|
||||
return ZmotionValue(int(ret), float(value.value))
|
||||
|
||||
# --------------------------------------------------------------- bus state
|
||||
|
||||
def bus_node_count(self, slot: int = 0) -> ZmotionValue:
|
||||
"""Number of slave nodes the controller found on the bus."""
|
||||
value = ctypes.c_uint32()
|
||||
ret = self._dll.ZAux_BusCmd_GetNodeNum( # type: ignore[union-attr]
|
||||
self._require_handle(), ctypes.c_int(slot), ctypes.byref(value)
|
||||
)
|
||||
return ZmotionValue(int(ret), int(value.value))
|
||||
|
||||
def bus_home_status(self, axis: int) -> ZmotionValue:
|
||||
"""Driver homing status: 1 once homing has completed."""
|
||||
value = ctypes.c_int()
|
||||
ret = self._dll.ZAux_BusCmd_GetHomeStatus( # type: ignore[union-attr]
|
||||
self._require_handle(), ctypes.c_int(axis), ctypes.byref(value)
|
||||
)
|
||||
return ZmotionValue(int(ret), int(value.value))
|
||||
|
||||
def bus_datum(self, axis: int, home_mode: int) -> ZmotionResult:
|
||||
"""Start driver homing with a CiA-402 homing method."""
|
||||
ret = self._dll.ZAux_BusCmd_Datum( # type: ignore[union-attr]
|
||||
self._require_handle(), ctypes.c_int(axis), ctypes.c_int(home_mode)
|
||||
)
|
||||
return ZmotionResult(int(ret))
|
||||
|
||||
def bus_set_datum_offset(self, axis: int, value: float) -> ZmotionResult:
|
||||
"""Set the homing offset (machine units)."""
|
||||
ret = self._dll.ZAux_BusCmd_SetDatumOffpos( # type: ignore[union-attr]
|
||||
self._require_handle(), ctypes.c_int(axis), ctypes.c_float(value)
|
||||
)
|
||||
return ZmotionResult(int(ret))
|
||||
|
||||
def bus_drive_clear(self, axis: int, mode: int = 0) -> ZmotionResult:
|
||||
"""Clear a drive fault/alert."""
|
||||
ret = self._dll.ZAux_BusCmd_DriveClear( # type: ignore[union-attr]
|
||||
self._require_handle(), ctypes.c_int(axis), ctypes.c_int(mode)
|
||||
)
|
||||
return ZmotionResult(int(ret))
|
||||
|
||||
def sdo_read(
|
||||
self, slot: int, node: int, index: int, subindex: int, data_type: int
|
||||
) -> ZmotionValue:
|
||||
"""Read an EtherCAT object-dictionary entry.
|
||||
|
||||
``data_type``: 1=int8, 2=int16, 3=int32, 4=uint8, 5=uint16, 6=uint32.
|
||||
"""
|
||||
value = ctypes.c_int()
|
||||
ret = self._dll.ZAux_BusCmd_SDORead( # type: ignore[union-attr]
|
||||
self._require_handle(),
|
||||
ctypes.c_int(slot),
|
||||
ctypes.c_int(node),
|
||||
ctypes.c_int(index),
|
||||
ctypes.c_int(subindex),
|
||||
ctypes.c_int(data_type),
|
||||
ctypes.byref(value),
|
||||
)
|
||||
return ZmotionValue(int(ret), int(value.value))
|
||||
|
||||
def sdo_write(
|
||||
self,
|
||||
slot: int,
|
||||
node: int,
|
||||
index: int,
|
||||
subindex: int,
|
||||
data_type: int,
|
||||
value: int,
|
||||
) -> ZmotionResult:
|
||||
"""Write an EtherCAT object-dictionary entry. See :meth:`sdo_read`."""
|
||||
ret = self._dll.ZAux_BusCmd_SDOWrite( # type: ignore[union-attr]
|
||||
self._require_handle(),
|
||||
ctypes.c_int(slot),
|
||||
ctypes.c_int(node),
|
||||
ctypes.c_int(index),
|
||||
ctypes.c_int(subindex),
|
||||
ctypes.c_int(data_type),
|
||||
ctypes.c_int(value),
|
||||
)
|
||||
return ZmotionResult(int(ret))
|
||||
|
||||
def rtex_read(self, axis: int, parameter: int) -> ZmotionValue:
|
||||
"""Read an RTEX drive parameter."""
|
||||
value = ctypes.c_float()
|
||||
ret = self._dll.ZAux_BusCmd_RtexRead( # type: ignore[union-attr]
|
||||
self._require_handle(),
|
||||
ctypes.c_int(axis),
|
||||
ctypes.c_int(parameter),
|
||||
ctypes.byref(value),
|
||||
)
|
||||
return ZmotionValue(int(ret), float(value.value))
|
||||
|
||||
def rtex_write(self, axis: int, parameter: int, value: float) -> ZmotionResult:
|
||||
"""Write an RTEX drive parameter."""
|
||||
ret = self._dll.ZAux_BusCmd_RtexWrite( # type: ignore[union-attr]
|
||||
self._require_handle(),
|
||||
ctypes.c_int(axis),
|
||||
ctypes.c_int(parameter),
|
||||
ctypes.c_float(value),
|
||||
)
|
||||
return ZmotionResult(int(ret))
|
||||
|
||||
# ------------------------------------------------------------------- axis
|
||||
|
||||
def set_axis_enable(self, axis: int, value: int) -> ZmotionResult:
|
||||
"""Servo enable (1) / disable (0) for one axis."""
|
||||
ret = self._dll.ZAux_Direct_SetAxisEnable( # type: ignore[union-attr]
|
||||
self._require_handle(), ctypes.c_int(axis), ctypes.c_int(value)
|
||||
)
|
||||
return ZmotionResult(int(ret))
|
||||
|
||||
def get_axis_enable(self, axis: int) -> ZmotionValue:
|
||||
value = ctypes.c_int()
|
||||
ret = self._dll.ZAux_Direct_GetAxisEnable( # type: ignore[union-attr]
|
||||
self._require_handle(), ctypes.c_int(axis), ctypes.byref(value)
|
||||
)
|
||||
return ZmotionValue(int(ret), int(value.value))
|
||||
|
||||
def set_units(self, axis: int, value: float) -> ZmotionResult:
|
||||
"""Pulse equivalent (user units per pulse)."""
|
||||
ret = self._dll.ZAux_Direct_SetUnits( # type: ignore[union-attr]
|
||||
self._require_handle(), ctypes.c_int(axis), ctypes.c_float(value)
|
||||
)
|
||||
return ZmotionResult(int(ret))
|
||||
|
||||
def get_units(self, axis: int) -> ZmotionValue:
|
||||
value = ctypes.c_float()
|
||||
ret = self._dll.ZAux_Direct_GetUnits( # type: ignore[union-attr]
|
||||
self._require_handle(), ctypes.c_int(axis), ctypes.byref(value)
|
||||
)
|
||||
return ZmotionValue(int(ret), float(value.value))
|
||||
|
||||
def set_speed(self, axis: int, value: float) -> ZmotionResult:
|
||||
ret = self._dll.ZAux_Direct_SetSpeed( # type: ignore[union-attr]
|
||||
self._require_handle(), ctypes.c_int(axis), ctypes.c_float(value)
|
||||
)
|
||||
return ZmotionResult(int(ret))
|
||||
|
||||
def get_speed(self, axis: int) -> ZmotionValue:
|
||||
value = ctypes.c_float()
|
||||
ret = self._dll.ZAux_Direct_GetSpeed( # type: ignore[union-attr]
|
||||
self._require_handle(), ctypes.c_int(axis), ctypes.byref(value)
|
||||
)
|
||||
return ZmotionValue(int(ret), float(value.value))
|
||||
|
||||
def set_accel(self, axis: int, value: float) -> ZmotionResult:
|
||||
ret = self._dll.ZAux_Direct_SetAccel( # type: ignore[union-attr]
|
||||
self._require_handle(), ctypes.c_int(axis), ctypes.c_float(value)
|
||||
)
|
||||
return ZmotionResult(int(ret))
|
||||
|
||||
def get_accel(self, axis: int) -> ZmotionValue:
|
||||
value = ctypes.c_float()
|
||||
ret = self._dll.ZAux_Direct_GetAccel( # type: ignore[union-attr]
|
||||
self._require_handle(), ctypes.c_int(axis), ctypes.byref(value)
|
||||
)
|
||||
return ZmotionValue(int(ret), float(value.value))
|
||||
|
||||
def set_creep(self, axis: int, value: float) -> ZmotionResult:
|
||||
"""Creep (slow homing) speed."""
|
||||
ret = self._dll.ZAux_Direct_SetCreep( # type: ignore[union-attr]
|
||||
self._require_handle(), ctypes.c_int(axis), ctypes.c_float(value)
|
||||
)
|
||||
return ZmotionResult(int(ret))
|
||||
|
||||
def get_atype(self, axis: int) -> ZmotionValue:
|
||||
"""Axis type (65=position, 66=speed, 67=torque)."""
|
||||
value = ctypes.c_int()
|
||||
ret = self._dll.ZAux_Direct_GetAtype( # type: ignore[union-attr]
|
||||
self._require_handle(), ctypes.c_int(axis), ctypes.byref(value)
|
||||
)
|
||||
return ZmotionValue(int(ret), int(value.value))
|
||||
|
||||
def get_dpos(self, axis: int) -> ZmotionValue:
|
||||
"""Commanded position, in user units."""
|
||||
value = ctypes.c_float()
|
||||
ret = self._dll.ZAux_Direct_GetDpos( # type: ignore[union-attr]
|
||||
self._require_handle(), ctypes.c_int(axis), ctypes.byref(value)
|
||||
)
|
||||
return ZmotionValue(int(ret), float(value.value))
|
||||
|
||||
def get_mpos(self, axis: int) -> ZmotionValue:
|
||||
"""Measured (feedback) position, in user units."""
|
||||
value = ctypes.c_float()
|
||||
ret = self._dll.ZAux_Direct_GetMpos( # type: ignore[union-attr]
|
||||
self._require_handle(), ctypes.c_int(axis), ctypes.byref(value)
|
||||
)
|
||||
return ZmotionValue(int(ret), float(value.value))
|
||||
|
||||
def get_axis_status(self, axis: int) -> ZmotionValue:
|
||||
"""Axis status word. Bit 6 is the homing-in-progress bit."""
|
||||
value = ctypes.c_int()
|
||||
ret = self._dll.ZAux_Direct_GetAxisStatus( # type: ignore[union-attr]
|
||||
self._require_handle(), ctypes.c_int(axis), ctypes.byref(value)
|
||||
)
|
||||
return ZmotionValue(int(ret), int(value.value))
|
||||
|
||||
def get_if_idle(self, axis: int) -> ZmotionValue:
|
||||
"""0 while the axis is moving, -1 when it has stopped."""
|
||||
value = ctypes.c_int()
|
||||
ret = self._dll.ZAux_Direct_GetIfIdle( # type: ignore[union-attr]
|
||||
self._require_handle(), ctypes.c_int(axis), ctypes.byref(value)
|
||||
)
|
||||
return ZmotionValue(int(ret), int(value.value))
|
||||
|
||||
def single_vmove(self, axis: int, direction: int) -> ZmotionResult:
|
||||
"""Continuous jog: ``direction`` 1 for forward, -1 for reverse."""
|
||||
ret = self._dll.ZAux_Direct_Single_Vmove( # type: ignore[union-attr]
|
||||
self._require_handle(), ctypes.c_int(axis), ctypes.c_int(direction)
|
||||
)
|
||||
return ZmotionResult(int(ret))
|
||||
|
||||
def single_move(self, axis: int, distance: float) -> ZmotionResult:
|
||||
"""Move by a distance relative to the current position."""
|
||||
ret = self._dll.ZAux_Direct_Single_Move( # type: ignore[union-attr]
|
||||
self._require_handle(), ctypes.c_int(axis), ctypes.c_float(distance)
|
||||
)
|
||||
return ZmotionResult(int(ret))
|
||||
|
||||
def single_move_abs(self, axis: int, position: float) -> ZmotionResult:
|
||||
"""Move to an absolute position."""
|
||||
ret = self._dll.ZAux_Direct_Single_MoveAbs( # type: ignore[union-attr]
|
||||
self._require_handle(), ctypes.c_int(axis), ctypes.c_float(position)
|
||||
)
|
||||
return ZmotionResult(int(ret))
|
||||
|
||||
def single_cancel(self, axis: int, mode: int = 2) -> ZmotionResult:
|
||||
"""Stop the axis. ``mode=2`` is a decelerating stop."""
|
||||
ret = self._dll.ZAux_Direct_Single_Cancel( # type: ignore[union-attr]
|
||||
self._require_handle(), ctypes.c_int(axis), ctypes.c_int(mode)
|
||||
)
|
||||
return ZmotionResult(int(ret))
|
||||
@@ -0,0 +1,143 @@
|
||||
"""Resolve and load the vendored ZMotion runtime DLLs.
|
||||
|
||||
Resolution order:
|
||||
|
||||
1. ``ORISYS_ZMOTION_DLL_DIR`` — an explicit override, e.g. a newer
|
||||
controller-support CD.
|
||||
2. ``<exe dir>/_internal/zmotion/x64`` and ``<exe dir>/zmotion/x64`` — a frozen
|
||||
(PyInstaller) bundle, matching how ``build/studio`` places branding.
|
||||
3. ``shared/resources/zmotion/x64`` in the source tree — the vendored copy
|
||||
(see ``shared/resources/zmotion/x64/README.md`` for provenance).
|
||||
|
||||
Nothing here runs at import time; :func:`load_dll` is called by the app once a
|
||||
connection is actually requested.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import ctypes
|
||||
import os
|
||||
import sys
|
||||
from pathlib import Path
|
||||
|
||||
ZAUXDLL_DLL = "zauxdll.dll"
|
||||
ZMOTION_DLL = "zmotion.dll"
|
||||
|
||||
#: Environment variable that overrides the vendored DLL directory.
|
||||
DLL_DIR_ENV = "ORISYS_ZMOTION_DLL_DIR"
|
||||
|
||||
|
||||
class ZmotionDllNotFound(OSError):
|
||||
"""The ZMotion runtime could not be located.
|
||||
|
||||
Carries every path that was searched, so the message can tell an operator
|
||||
what to install or which environment variable to set instead of just
|
||||
naming the missing file.
|
||||
"""
|
||||
|
||||
def __init__(self, name: str, searched: list[Path]) -> None:
|
||||
self.name = name
|
||||
self.searched = searched
|
||||
listing = "\n".join(f" - {path}" for path in searched)
|
||||
super().__init__(
|
||||
f"{name} not found. Searched:\n{listing}\n"
|
||||
f"Install the ZMotion runtime, or set {DLL_DIR_ENV} to the folder "
|
||||
f"that contains it."
|
||||
)
|
||||
|
||||
|
||||
def _source_tree_dir() -> Path:
|
||||
# .../orisys-app/shared/src/orisys_app_shared/zmotion/loader.py -> shared/
|
||||
return Path(__file__).resolve().parents[3] / "resources" / "zmotion" / "x64"
|
||||
|
||||
|
||||
def _frozen_dirs() -> list[Path]:
|
||||
if not getattr(sys, "frozen", False):
|
||||
return []
|
||||
exe_dir = Path(sys.executable).resolve().parent
|
||||
return [exe_dir / "_internal" / "zmotion" / "x64", exe_dir / "zmotion" / "x64"]
|
||||
|
||||
|
||||
def candidate_dll_dirs() -> list[Path]:
|
||||
"""Every directory that will be searched for the runtime DLLs, in order.
|
||||
|
||||
An explicit ``ORISYS_ZMOTION_DLL_DIR`` is authoritative: it is the only
|
||||
candidate, so a typo fails loudly instead of quietly loading a different
|
||||
build of the runtime than the operator asked for. On a motion controller,
|
||||
silently using the wrong DLL version is exactly the kind of failure that
|
||||
looks like a working machine until it moves.
|
||||
"""
|
||||
override = os.environ.get(DLL_DIR_ENV, "").strip()
|
||||
if override:
|
||||
return [Path(override).expanduser()]
|
||||
|
||||
candidates: list[Path] = []
|
||||
candidates.extend(_frozen_dirs())
|
||||
candidates.append(_source_tree_dir())
|
||||
|
||||
seen: set[Path] = set()
|
||||
unique: list[Path] = []
|
||||
for path in candidates:
|
||||
resolved = path.resolve() if path.exists() else path
|
||||
if resolved not in seen:
|
||||
seen.add(resolved)
|
||||
unique.append(resolved)
|
||||
return unique
|
||||
|
||||
|
||||
def resolve_dll_dir() -> Path:
|
||||
"""Return the first candidate directory that holds ``zauxdll.dll``.
|
||||
|
||||
Raises:
|
||||
ZmotionDllNotFound: no candidate directory contains the runtime.
|
||||
"""
|
||||
searched = candidate_dll_dirs()
|
||||
for path in searched:
|
||||
if (path / ZAUXDLL_DLL).is_file():
|
||||
return path
|
||||
raise ZmotionDllNotFound(ZAUXDLL_DLL, searched)
|
||||
|
||||
|
||||
def _prepare_search_path(dll_dir: Path) -> None:
|
||||
"""Make ``dll_dir`` available to the loader for dependent DLLs.
|
||||
|
||||
``zauxdll.dll`` links against ``zmotion.dll`` beside it, so the directory
|
||||
must be on the search path *before* ``zauxdll.dll`` is loaded. Both the
|
||||
process ``PATH`` and the loader's own DLL directory are set: the former is
|
||||
what the vendor wrapper relied on, the latter is the documented Win32
|
||||
mechanism.
|
||||
"""
|
||||
dll_dir_str = str(dll_dir)
|
||||
path_entries = os.environ.get("PATH", "").split(os.pathsep)
|
||||
if dll_dir_str not in path_entries:
|
||||
os.environ["PATH"] = dll_dir_str + os.pathsep + os.environ.get("PATH", "")
|
||||
|
||||
if sys.platform == "win32":
|
||||
try:
|
||||
kernel32 = ctypes.WinDLL("kernel32", use_last_error=True)
|
||||
kernel32.SetDllDirectoryW(ctypes.c_wchar_p(dll_dir_str))
|
||||
except (OSError, AttributeError): # pragma: no cover - defensive
|
||||
# Not fatal: the PATH entry above is usually enough, and the real
|
||||
# failure (if any) surfaces as a clearer error from load_dll.
|
||||
pass
|
||||
|
||||
|
||||
def load_dll(dll_dir: Path | None = None) -> ctypes.CDLL:
|
||||
"""Load ``zauxdll.dll`` with its directory on the search path.
|
||||
|
||||
Args:
|
||||
dll_dir: directory to load from; resolved automatically when omitted.
|
||||
|
||||
Returns:
|
||||
The loaded CDLL.
|
||||
|
||||
Raises:
|
||||
ZmotionDllNotFound: the runtime is not present.
|
||||
OSError: the DLL is present but the platform loader rejected it.
|
||||
"""
|
||||
resolved = dll_dir or resolve_dll_dir()
|
||||
dll_path = resolved / ZAUXDLL_DLL
|
||||
if not dll_path.is_file():
|
||||
raise ZmotionDllNotFound(ZAUXDLL_DLL, [resolved])
|
||||
_prepare_search_path(resolved)
|
||||
return ctypes.CDLL(str(dll_path))
|
||||
@@ -0,0 +1,87 @@
|
||||
"""Display-only force baseline / deadband (value cards)."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import numpy as np
|
||||
|
||||
from orisys_app_shared.force.presentation import ForcePresentation, ZeroEstimate
|
||||
|
||||
|
||||
def test_fit_zero_rejects_insufficient_samples() -> None:
|
||||
presentation = ForcePresentation(enabled=True, min_samples=30)
|
||||
estimate = presentation.fit_zero([(1.0, 2.0, 3.0), (1.1, 2.1, 3.1)])
|
||||
assert estimate.quality == "insufficient"
|
||||
|
||||
|
||||
def test_fit_zero_and_accept_deadband() -> None:
|
||||
rng = np.random.default_rng(0)
|
||||
bias = np.array([1262.0, -11.0, -353.0])
|
||||
noise = rng.normal(0.0, 20.0, size=(80, 3))
|
||||
samples = (bias + noise).tolist()
|
||||
presentation = ForcePresentation(
|
||||
enabled=True,
|
||||
min_samples=30,
|
||||
baseline_seconds=0.0,
|
||||
min_deadband=50.0,
|
||||
deadband_percentile=99.0,
|
||||
)
|
||||
estimate = presentation.fit_zero(samples)
|
||||
assert estimate.quality == "ok"
|
||||
presentation.apply_estimate(estimate)
|
||||
idle = presentation.accept(tuple(bias), unit="raw")
|
||||
assert idle.zero_zone
|
||||
assert idle.values == (0.0, 0.0, 0.0)
|
||||
# Curves / CSV keep the measurement: accept never mutates the input tuple.
|
||||
measured = (1262.0, -11.0, -353.0)
|
||||
presentation.accept(measured, unit="raw")
|
||||
assert measured == (1262.0, -11.0, -353.0)
|
||||
|
||||
|
||||
def test_observe_skips_contact_contamination() -> None:
|
||||
presentation = ForcePresentation(
|
||||
enabled=True,
|
||||
min_samples=5,
|
||||
baseline_seconds=0.1,
|
||||
require_no_contact=True,
|
||||
)
|
||||
for i in range(10):
|
||||
out = presentation.observe(
|
||||
(1000.0, 0.0, 0.0),
|
||||
t_rel_s=i * 0.05,
|
||||
contact_found=True,
|
||||
flow_p95=0.1,
|
||||
)
|
||||
assert out is None
|
||||
assert not presentation.ready
|
||||
|
||||
|
||||
def test_observe_freezes_after_quiet_window() -> None:
|
||||
presentation = ForcePresentation(
|
||||
enabled=True,
|
||||
min_samples=10,
|
||||
baseline_seconds=0.5,
|
||||
min_deadband=10.0,
|
||||
)
|
||||
estimate: ZeroEstimate | None = None
|
||||
for i in range(40):
|
||||
estimate = presentation.observe(
|
||||
(100.0 + (i % 3), -5.0, 2.0),
|
||||
t_rel_s=i * 0.02,
|
||||
flow_p95=0.1,
|
||||
contact_found=False,
|
||||
)
|
||||
if estimate is not None:
|
||||
break
|
||||
assert estimate is not None
|
||||
assert estimate.quality == "ok"
|
||||
assert presentation.ready
|
||||
# Further observes do not chase a new zero once frozen.
|
||||
later = presentation.observe((5000.0, 0.0, 0.0), t_rel_s=10.0, flow_p95=0.0)
|
||||
assert later is None
|
||||
|
||||
|
||||
def test_format_force_stable_digits() -> None:
|
||||
presentation = ForcePresentation(enabled=True)
|
||||
assert presentation.format_force(0.0, unit="raw") == "0"
|
||||
assert presentation.format_force(1262.4, unit="raw") == "1262"
|
||||
assert presentation.format_force(1.2345, unit="N") == "1.234"
|
||||
@@ -127,6 +127,7 @@ def test_the_backend_renders_what_the_primitives_rendered_before():
|
||||
background="captured" if mode == "arrows_stitched" else "pure_color",
|
||||
draw_style="finger",
|
||||
valid_mask=mask,
|
||||
colormap=flow_colormap("dark"),
|
||||
)
|
||||
got = backend.render_image(frame, plan, mode=mode, draw_style="finger")
|
||||
assert np.array_equal(got, expected), mode
|
||||
|
||||
@@ -0,0 +1,73 @@
|
||||
"""Pure-NumPy overlay geometry for Finger 3D arrows."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import numpy as np
|
||||
|
||||
from orisys_app_shared.scene3d.overlays import (
|
||||
arrow_instances_to_mesh,
|
||||
build_flow_overlay,
|
||||
build_normal_overlay,
|
||||
force_visual_strength,
|
||||
)
|
||||
from orisys_app_shared.scene3d.synthetic import poc_scene
|
||||
from orisys_app_shared.scene3d.visual_policy import VisualPolicy, select_flow_samples
|
||||
|
||||
|
||||
def test_force_visual_strength_is_capped() -> None:
|
||||
assert force_visual_strength(0.0) == 0.0
|
||||
strong = force_visual_strength(1e9)
|
||||
mild = force_visual_strength(100.0)
|
||||
assert strong >= mild > 0.0
|
||||
assert strong <= 1.0 + 1e-9
|
||||
|
||||
|
||||
def test_flow_and_normal_overlays_on_poc_mapping() -> None:
|
||||
assets = poc_scene()
|
||||
mapping = assets.surface_mapping
|
||||
assert mapping is not None
|
||||
h, w = mapping.shape_hw
|
||||
flow = np.zeros((h, w, 2), dtype=np.float64)
|
||||
flow[1, 1] = (0.8, 0.0)
|
||||
mask = np.asarray(mapping.valid, dtype=bool)
|
||||
policy = VisualPolicy(
|
||||
flow_on_px=0.4,
|
||||
flow_grid_spacing=1,
|
||||
flow_edge_margin=0,
|
||||
max_arrows=50,
|
||||
)
|
||||
samples = select_flow_samples(flow, mask, policy)
|
||||
arrows, diags = build_flow_overlay(samples, mapping, style=policy)
|
||||
assert not diags
|
||||
assert arrows is not None and arrows.origins.shape[0] >= 1
|
||||
contact, cdiags = build_normal_overlay(
|
||||
contact_uv_xy=(1.0, 1.0),
|
||||
contact_found=True,
|
||||
contact_valid=True,
|
||||
fn=500.0,
|
||||
mapping=mapping,
|
||||
)
|
||||
assert not cdiags
|
||||
assert contact is not None and contact.origins.shape[0] == 1
|
||||
# Yellow-ish contact colour.
|
||||
assert float(contact.colors[0, 0]) > 0.7
|
||||
assert float(contact.colors[0, 1]) > 0.7
|
||||
|
||||
positions, triangles, colors = arrow_instances_to_mesh(arrows)
|
||||
assert positions.ndim == 2 and positions.shape[1] == 3
|
||||
assert triangles.ndim == 2 and triangles.shape[1] == 3
|
||||
assert colors.shape[0] == positions.shape[0]
|
||||
|
||||
|
||||
def test_missing_contact_yields_no_arrow() -> None:
|
||||
assets = poc_scene()
|
||||
mapping = assets.surface_mapping
|
||||
assert mapping is not None
|
||||
arrows, _ = build_normal_overlay(
|
||||
contact_uv_xy=(1.0, 1.0),
|
||||
contact_found=False,
|
||||
contact_valid=True,
|
||||
fn=100.0,
|
||||
mapping=mapping,
|
||||
)
|
||||
assert arrows.origins.shape[0] == 0
|
||||
@@ -0,0 +1,73 @@
|
||||
"""Shared 2D/3D flow sampling and domain-mask rules."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from orisys_app_shared.scene3d.contracts import ThreeDError
|
||||
from orisys_app_shared.scene3d.visual_policy import (
|
||||
VisualPolicy,
|
||||
build_domain_mask,
|
||||
paint_domain_background,
|
||||
select_flow_samples,
|
||||
)
|
||||
|
||||
|
||||
def test_build_domain_mask_rejects_shape_mismatch() -> None:
|
||||
geo = np.ones((4, 5), dtype=bool)
|
||||
with pytest.raises(ThreeDError):
|
||||
build_domain_mask(geo, shape_hw=(3, 5))
|
||||
|
||||
|
||||
def test_build_domain_mask_intersects_roi() -> None:
|
||||
geo = np.ones((3, 3), dtype=bool)
|
||||
roi = np.zeros((3, 3), dtype=bool)
|
||||
roi[1, 1] = True
|
||||
mask, diags = build_domain_mask(geo, roi)
|
||||
assert mask is not None
|
||||
assert bool(mask[1, 1]) and not bool(mask[0, 0])
|
||||
assert diags == ()
|
||||
|
||||
|
||||
def test_select_flow_samples_uses_shared_grid_and_mask() -> None:
|
||||
h, w = 40, 40
|
||||
flow = np.zeros((h, w, 2), dtype=np.float64)
|
||||
flow[15, 15] = (0.5, 0.0) # |2*flow| = 1.0
|
||||
mask = np.ones((h, w), dtype=bool)
|
||||
mask[:, :5] = False
|
||||
policy = VisualPolicy(flow_on_px=0.4, flow_grid_spacing=10, flow_edge_margin=0)
|
||||
samples = select_flow_samples(flow, mask, policy)
|
||||
assert samples.rows.size >= 1
|
||||
assert all(c >= 5 for c in samples.cols.tolist())
|
||||
pairs = set(zip(samples.rows.tolist(), samples.cols.tolist()))
|
||||
assert (15, 15) in pairs
|
||||
|
||||
|
||||
def test_select_flow_samples_hysteresis_keeps_weaker_motion() -> None:
|
||||
h, w = 30, 30
|
||||
flow = np.zeros((h, w, 2), dtype=np.float64)
|
||||
# |2*flow| = 0.7: below on (0.8) but above off (0.5)
|
||||
flow[15, 15] = (0.35, 0.0)
|
||||
policy = VisualPolicy(
|
||||
flow_on_px=0.8,
|
||||
flow_off_px=0.5,
|
||||
flow_grid_spacing=10,
|
||||
flow_edge_margin=0,
|
||||
)
|
||||
previously = np.zeros((h, w), dtype=bool)
|
||||
previously[15, 15] = True
|
||||
off = select_flow_samples(flow, None, policy, previously_on=None)
|
||||
on = select_flow_samples(flow, None, policy, previously_on=previously)
|
||||
assert off.rows.size == 0
|
||||
assert on.rows.size == 1
|
||||
|
||||
|
||||
def test_paint_domain_background_colours() -> None:
|
||||
mask = np.zeros((4, 4), dtype=bool)
|
||||
mask[1:3, 1:3] = True
|
||||
canvas = paint_domain_background(
|
||||
(4, 4), mask, island_bgr=(10, 20, 30), exterior_bgr=(1, 2, 3)
|
||||
)
|
||||
assert tuple(canvas[0, 0]) == (1, 2, 3)
|
||||
assert tuple(canvas[1, 1]) == (10, 20, 30)
|
||||
Reference in New Issue
Block a user