更新多点标定功能
This commit is contained in:
@@ -0,0 +1,901 @@
|
||||
"""Ordered multi-point normal-force acquisition for the finger sensor.
|
||||
|
||||
Points picked on the finger page already carry a surface normal and a solved
|
||||
platform pose. This campaign stores that pose and can hand the same five-axis
|
||||
targets to the existing move planner. It does not command the platform itself,
|
||||
and it does not invent a press depth or a force loop.
|
||||
|
||||
A weight reference is the number the operator types. A six-axis reference is the
|
||||
raw Fx, Fy, Fz transformed by the rotation the operator entered, then projected
|
||||
onto the tool axis the pose solver already aligns the surface normal to. No raw
|
||||
axis is treated as the normal on its own.
|
||||
|
||||
Images, when a camera is already running, are written by the shared recorder.
|
||||
Frames and reference windows meet by host monotonic time, not by row number.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import csv
|
||||
import json
|
||||
import math
|
||||
import re
|
||||
import time
|
||||
from dataclasses import dataclass
|
||||
from datetime import date, datetime, timezone
|
||||
from pathlib import Path
|
||||
from uuid import uuid4
|
||||
|
||||
from .calibration_session import write_json
|
||||
from .sensor.frames import PLATFORM_AXES, TOOL_AXIS
|
||||
|
||||
SENSOR_MODEL = "finger"
|
||||
|
||||
|
||||
def next_point_id(existing: list[str], today: date | None = None) -> str:
|
||||
"""Next unused id of the form YYYYMMDDpoint0, YYYYMMDDpoint1, and so on."""
|
||||
stamp = (today or date.today()).strftime("%Y%m%d")
|
||||
pattern = re.compile(rf"^{stamp}point(\d+)$")
|
||||
numbers = [
|
||||
int(match.group(1))
|
||||
for ident in existing
|
||||
if (match := pattern.fullmatch(ident.strip()))
|
||||
]
|
||||
number = 0 if not numbers else max(numbers) + 1
|
||||
return f"{stamp}point{number}"
|
||||
|
||||
SYNC = {
|
||||
"method": "host_monotonic_time_overlap",
|
||||
"images": (
|
||||
"Each saved frame stores t_host_mono_ns at the moment the computer "
|
||||
"received it. The video file's playback clock is not the sync clock."
|
||||
),
|
||||
"reference_force": (
|
||||
"Each hold stores t_start_host_mono_ns and t_end_host_mono_ns. "
|
||||
"Weight mode stores the normal force the operator typed. "
|
||||
"Six-axis mode stores raw Fx, Fy, Fz and F_ref_normal_N = (R @ F) · tool_axis, "
|
||||
"where R is the force-sensor-to-tool rotation entered for this session "
|
||||
"and tool_axis is the axis the pose solver aligns the surface normal to. "
|
||||
"A raw axis is not used as the normal."
|
||||
),
|
||||
"association": (
|
||||
"A frame belongs to a hold when its t_host_mono_ns is in "
|
||||
"[t_start_host_mono_ns, t_end_host_mono_ns). Rows are not paired."
|
||||
),
|
||||
"automatic_force_control": False,
|
||||
"platform_motion": False,
|
||||
"reason": (
|
||||
"No reference-force sensor or force-loop interface is connected. "
|
||||
"A fixed press depth is not used in its place."
|
||||
),
|
||||
}
|
||||
|
||||
REFERENCE_HEADER = (
|
||||
"step_index",
|
||||
"branch",
|
||||
"target_load_n",
|
||||
"t_start_host_mono_ns",
|
||||
"t_end_host_mono_ns",
|
||||
"F_ref_normal_N",
|
||||
"site_id",
|
||||
"x",
|
||||
"y",
|
||||
"z",
|
||||
"coordinate_frame",
|
||||
"repeat_index",
|
||||
"repeat_count",
|
||||
"hold_complete",
|
||||
"reference_source",
|
||||
"fx_N",
|
||||
"fy_N",
|
||||
"fz_N",
|
||||
"force_to_tool_rotation",
|
||||
"nx",
|
||||
"ny",
|
||||
"nz",
|
||||
"pose_x_mm",
|
||||
"pose_y_mm",
|
||||
"pose_yaw_deg",
|
||||
"pose_pitch_deg",
|
||||
"pose_z_mm",
|
||||
"normal_aligned_to",
|
||||
)
|
||||
|
||||
ACTIVE = frozenset({"load", "hold", "liftoff"})
|
||||
|
||||
|
||||
@dataclass
|
||||
class SiteSpec:
|
||||
"""One point. A solved point also carries the finger page's normal and pose."""
|
||||
|
||||
site_id: str
|
||||
x: float
|
||||
y: float
|
||||
z: float
|
||||
levels_n: tuple[float, ...]
|
||||
hold_s: float
|
||||
repeats: int
|
||||
normal: tuple[float, float, float] | None = None
|
||||
pose: dict | None = None
|
||||
uv_px: tuple[float, float] | None = None
|
||||
position_frame: str = ""
|
||||
|
||||
def position(self) -> dict[str, float]:
|
||||
return {"x": self.x, "y": self.y, "z": self.z}
|
||||
|
||||
|
||||
def load_steps(
|
||||
levels: list[float],
|
||||
max_force_n: float,
|
||||
*,
|
||||
min_force_n: float = 0.0,
|
||||
) -> list[tuple[str, float]]:
|
||||
"""Baseline, then the entered levels, then unload back to zero.
|
||||
|
||||
The unloaded baseline is always recorded. It is not one of the loaded levels.
|
||||
"""
|
||||
if not math.isfinite(max_force_n) or max_force_n <= 0:
|
||||
raise ValueError("maximum force must be a positive number you enter")
|
||||
lower = float(min_force_n)
|
||||
if not math.isfinite(lower) or lower < 0 or lower > max_force_n:
|
||||
raise ValueError("minimum force must be between 0 and the maximum force")
|
||||
values = [float(value) for value in levels]
|
||||
if not values or any(
|
||||
not math.isfinite(value) or value <= 0 or value < lower or value > max_force_n
|
||||
for value in values
|
||||
):
|
||||
raise ValueError("each load level must be inside the force range you entered")
|
||||
if values != sorted(set(values)):
|
||||
raise ValueError("load levels must be strictly increasing")
|
||||
return [("baseline", 0.0), *[("load", value) for value in values], *[
|
||||
("unload", value) for value in [*reversed(values[:-1]), 0.0]
|
||||
]]
|
||||
|
||||
|
||||
def parse_rotation(values: list[float]) -> tuple[tuple[float, float, float], ...]:
|
||||
"""Force-sensor frame to tool frame, row major. Nothing is filled in for you."""
|
||||
if len(values) != 9:
|
||||
raise ValueError("the force-sensor to tool rotation needs 9 numbers, row major")
|
||||
numbers = tuple(_finite(value, "rotation") for value in values)
|
||||
rows = (numbers[0:3], numbers[3:6], numbers[6:9])
|
||||
columns = tuple(tuple(row[index] for row in rows) for index in range(3))
|
||||
|
||||
def dot(left: tuple[float, ...], right: tuple[float, ...]) -> float:
|
||||
return sum(a * b for a, b in zip(left, right))
|
||||
|
||||
for index, column in enumerate(columns):
|
||||
if abs(dot(column, column) - 1.0) > 1e-6:
|
||||
raise ValueError("each column of the rotation must have length 1")
|
||||
for earlier in columns[:index]:
|
||||
if abs(dot(column, earlier)) > 1e-6:
|
||||
raise ValueError("the columns of the rotation must be perpendicular")
|
||||
determinant = (
|
||||
columns[0][0] * (columns[1][1] * columns[2][2] - columns[1][2] * columns[2][1])
|
||||
- columns[0][1] * (columns[1][0] * columns[2][2] - columns[1][2] * columns[2][0])
|
||||
+ columns[0][2] * (columns[1][0] * columns[2][1] - columns[1][1] * columns[2][0])
|
||||
)
|
||||
if abs(determinant - 1.0) > 1e-6:
|
||||
raise ValueError("the transform must be a rotation with determinant +1")
|
||||
return rows
|
||||
|
||||
|
||||
def normal_force_n(
|
||||
fx: float,
|
||||
fy: float,
|
||||
fz: float,
|
||||
rotation: tuple[tuple[float, float, float], ...],
|
||||
) -> float:
|
||||
"""Project a raw force onto the tool axis after the entered rotation.
|
||||
|
||||
``F_tool = R @ F_sensor`` and ``F_normal = F_tool · tool_axis``. ``tool_axis``
|
||||
is the platform axis the pose solver aligns the surface normal to. The raw
|
||||
components are not readable as the normal without that product.
|
||||
"""
|
||||
raw = (_finite(fx, "fx"), _finite(fy, "fy"), _finite(fz, "fz"))
|
||||
transformed = tuple(sum(a * b for a, b in zip(row, raw)) for row in rotation)
|
||||
return transformed[PLATFORM_AXES.index(TOOL_AXIS)]
|
||||
|
||||
|
||||
def _finite(value: float, label: str) -> float:
|
||||
number = float(value)
|
||||
if not math.isfinite(number):
|
||||
raise ValueError(f"{label} must be a finite number")
|
||||
return number
|
||||
|
||||
|
||||
class Campaign:
|
||||
"""Walk sites in list order. One repeat is one load, hold, record and unload."""
|
||||
|
||||
def __init__(self) -> None:
|
||||
self.sites: list[SiteSpec] = []
|
||||
self.phase = "idle"
|
||||
self.directory: Path | None = None
|
||||
self.coordinate_frame = ""
|
||||
self.max_force_n: float | None = None
|
||||
self.min_force_n = 0.0
|
||||
self.sensor_model = ""
|
||||
self.sensor_sn = ""
|
||||
self.simulate = False
|
||||
self.reason = ""
|
||||
self._document: dict | None = None
|
||||
self._site_index = 0
|
||||
self._repeat_index = 0
|
||||
self._step_index = 0
|
||||
self._steps: list[tuple[str, float]] = []
|
||||
self._hold_s = 0.0
|
||||
self._hold_started_ns: int | None = None
|
||||
self._active: tuple | None = None
|
||||
self._trial_dir: Path | None = None
|
||||
self._reference = None
|
||||
self._events = None
|
||||
self._completed_holds = 0
|
||||
self._trial_serial = 0
|
||||
self._reference_file = None
|
||||
self._events_file = None
|
||||
self._session_events_file = None
|
||||
self._session_events = None
|
||||
self._recorder = None
|
||||
self._image_worker = None
|
||||
self._pending_worker = None
|
||||
self._fps = 0.0
|
||||
self._rotation: tuple[tuple[float, float, float], ...] | None = None
|
||||
self._pending: dict | None = None
|
||||
self._single_site = False
|
||||
|
||||
@property
|
||||
def is_active(self) -> bool:
|
||||
return self.phase in ACTIVE
|
||||
|
||||
def add_site(
|
||||
self,
|
||||
site_id: str,
|
||||
x: float,
|
||||
y: float,
|
||||
z: float,
|
||||
levels: list[float],
|
||||
hold_s: float,
|
||||
repeats: int,
|
||||
*,
|
||||
max_force_n: float,
|
||||
) -> SiteSpec:
|
||||
self._require_idle()
|
||||
ident = site_id.strip()
|
||||
if not ident or any(site.site_id == ident for site in self.sites):
|
||||
raise ValueError("each point needs a unique id")
|
||||
spec = self._spec(ident, x, y, z, levels, hold_s, repeats, max_force_n)
|
||||
self.sites.append(spec)
|
||||
return spec
|
||||
|
||||
def add_solved_site(
|
||||
self,
|
||||
site_id: str,
|
||||
x: float,
|
||||
y: float,
|
||||
z: float,
|
||||
normal: tuple[float, float, float],
|
||||
pose: dict,
|
||||
levels: list[float],
|
||||
hold_s: float,
|
||||
repeats: int,
|
||||
*,
|
||||
max_force_n: float,
|
||||
uv_px: tuple[float, float] | None = None,
|
||||
min_force_n: float = 0.0,
|
||||
) -> SiteSpec:
|
||||
"""A point whose normal and pose already came from the finger solver."""
|
||||
self._require_idle()
|
||||
ident = site_id.strip()
|
||||
if not ident or any(site.site_id == ident for site in self.sites):
|
||||
raise ValueError("each point needs a unique id")
|
||||
nx, ny, nz = (_finite(value, "normal") for value in normal)
|
||||
length = math.sqrt(nx * nx + ny * ny + nz * nz)
|
||||
if length < 1e-9:
|
||||
raise ValueError("the surface normal is missing")
|
||||
unit = (nx / length, ny / length, nz / length)
|
||||
required = ("x", "y", "yaw", "pitch", "z")
|
||||
if any(key not in pose for key in required):
|
||||
raise ValueError("the solved pose is missing a platform target")
|
||||
stored = {
|
||||
"x": _finite(pose["x"], "pose x"),
|
||||
"y": _finite(pose["y"], "pose y"),
|
||||
"yaw": _finite(pose["yaw"], "pose yaw"),
|
||||
"pitch": _finite(pose["pitch"], "pose pitch"),
|
||||
"z": _finite(pose["z"], "pose z"),
|
||||
"residual_deg": float(pose.get("residual_deg", 0.0)),
|
||||
"upright": bool(pose.get("upright", True)),
|
||||
"in_range": bool(pose.get("in_range", False)),
|
||||
"ok": bool(pose.get("ok", False)),
|
||||
"normal_aligned_to": str(pose.get("normal_aligned_to") or f"tool_axis:{TOOL_AXIS}"),
|
||||
"reason": str(pose.get("reason") or ""),
|
||||
}
|
||||
spec = self._spec(ident, x, y, z, levels, hold_s, repeats, max_force_n, min_force_n=min_force_n)
|
||||
spec.normal = unit
|
||||
spec.pose = stored
|
||||
spec.uv_px = None if uv_px is None else (float(uv_px[0]), float(uv_px[1]))
|
||||
spec.position_frame = "sensor"
|
||||
self.sites.append(spec)
|
||||
return spec
|
||||
|
||||
def command_targets(self, site_id: str) -> dict[str, float]:
|
||||
"""Five-axis targets for the existing pose move. This does not move anything."""
|
||||
site = self.sites[self._index(site_id)]
|
||||
pose = site.pose
|
||||
if pose is None:
|
||||
raise ValueError("this point has no solved pose; import it from the finger page")
|
||||
if not pose.get("ok") or not pose.get("in_range"):
|
||||
raise ValueError("the solved pose is outside the stage travel; it is stored but not commanded")
|
||||
return {key: float(pose[key]) for key in ("x", "y", "yaw", "pitch", "z")}
|
||||
|
||||
def set_force_rotation(self, values: list[float]) -> None:
|
||||
"""Store the force-sensor-to-tool rotation. There is no default matrix."""
|
||||
self._rotation = parse_rotation(list(values))
|
||||
if self._document is not None:
|
||||
self._document["force_to_tool_rotation"] = [value for row in self._rotation for value in row]
|
||||
self._document["normal_projection"] = (
|
||||
"F_tool = R @ F_sensor; F_normal = F_tool · tool_axis. "
|
||||
"R is the entered rotation. No raw axis is the normal."
|
||||
)
|
||||
self._save()
|
||||
|
||||
def update_site(
|
||||
self,
|
||||
site_id: str,
|
||||
x: float,
|
||||
y: float,
|
||||
z: float,
|
||||
levels: list[float],
|
||||
hold_s: float,
|
||||
repeats: int,
|
||||
*,
|
||||
max_force_n: float,
|
||||
new_id: str | None = None,
|
||||
) -> None:
|
||||
self._require_idle()
|
||||
index = self._index(site_id)
|
||||
ident = (new_id if new_id is not None else site_id).strip()
|
||||
if any(site.site_id == ident and i != index for i, site in enumerate(self.sites)):
|
||||
raise ValueError("each point needs a unique id")
|
||||
previous = self.sites[index]
|
||||
spec = self._spec(ident, x, y, z, levels, hold_s, repeats, max_force_n)
|
||||
spec.normal = previous.normal
|
||||
spec.pose = previous.pose
|
||||
spec.uv_px = previous.uv_px
|
||||
spec.position_frame = previous.position_frame
|
||||
self.sites[index] = spec
|
||||
|
||||
def remove_site(self, site_id: str) -> None:
|
||||
self._require_idle()
|
||||
del self.sites[self._index(site_id)]
|
||||
|
||||
def move_site(self, site_id: str, delta: int) -> None:
|
||||
self._require_idle()
|
||||
index = self._index(site_id)
|
||||
target = index + delta
|
||||
if not 0 <= target < len(self.sites):
|
||||
return
|
||||
self.sites[index], self.sites[target] = self.sites[target], self.sites[index]
|
||||
|
||||
def start(
|
||||
self,
|
||||
root: Path,
|
||||
*,
|
||||
sensor_model: str,
|
||||
sensor_sn: str,
|
||||
coordinate_frame: str,
|
||||
max_force_n: float,
|
||||
simulate: bool,
|
||||
min_force_n: float = 0.0,
|
||||
) -> Path:
|
||||
if self.is_active:
|
||||
raise RuntimeError("a multi-point acquisition is already running")
|
||||
if sensor_model.strip() != SENSOR_MODEL:
|
||||
raise ValueError("multi-point acquisition uses the finger product model")
|
||||
if not sensor_sn.strip():
|
||||
raise ValueError("sensor SN is required")
|
||||
if not str(root).strip():
|
||||
raise ValueError("output directory is required")
|
||||
if not coordinate_frame.strip():
|
||||
raise ValueError("enter the name of the coordinate frame; it is not assumed")
|
||||
if not self.sites:
|
||||
raise ValueError("add at least one point")
|
||||
force = _finite(max_force_n, "maximum force")
|
||||
lower = _finite(min_force_n, "minimum force")
|
||||
if force <= 0:
|
||||
raise ValueError("maximum force must be a positive number you enter")
|
||||
for site in self.sites:
|
||||
load_steps(list(site.levels_n), force, min_force_n=lower)
|
||||
self.coordinate_frame = coordinate_frame.strip()
|
||||
self.max_force_n = force
|
||||
self.min_force_n = lower
|
||||
self.sensor_model = sensor_model.strip()
|
||||
self.sensor_sn = sensor_sn.strip()
|
||||
self.simulate = bool(simulate)
|
||||
self.reason = ""
|
||||
self._single_site = False
|
||||
self._completed_holds = 0
|
||||
self._trial_serial = 0
|
||||
self.directory = _session_directory(Path(root), self.sensor_model, self.sensor_sn)
|
||||
self._document = {
|
||||
"format_version": "autocal-multipoint-1.0",
|
||||
"session_id": self.directory.name,
|
||||
"sensor_model": self.sensor_model,
|
||||
"sensor_sn": self.sensor_sn,
|
||||
"coordinate_frame": self.coordinate_frame,
|
||||
"max_force_n": force,
|
||||
"min_force_n": lower,
|
||||
"coordinate_mapping": "operator_entered",
|
||||
"contact_zero": "not_commanded",
|
||||
"max_descent": "not_set",
|
||||
"motion_limits": "not_applied",
|
||||
"acquisition_commands_motion": False,
|
||||
"automatic_force_control": False,
|
||||
"platform_motion": False,
|
||||
"simulate": self.simulate,
|
||||
"sync": SYNC,
|
||||
"sites": [self._site_record(site) for site in self.sites],
|
||||
"trials": [],
|
||||
"started_at": datetime.now(timezone.utc).isoformat(),
|
||||
"status": "in_progress",
|
||||
}
|
||||
self._save()
|
||||
events = (self.directory / "events.csv").open("w", newline="", encoding="utf-8")
|
||||
writer = csv.writer(events)
|
||||
writer.writerow(("t_host_mono_ns", "t_host_utc_ns", "kind", "detail"))
|
||||
events.flush()
|
||||
self._session_events_file = events
|
||||
self._session_events = writer
|
||||
self._site_index = 0
|
||||
self._repeat_index = 0
|
||||
self._open_repeat()
|
||||
self.phase = "load"
|
||||
return self.directory
|
||||
|
||||
def confirm_reference(self, force_n: float) -> None:
|
||||
if self.phase == "liftoff":
|
||||
raise RuntimeError("confirm the indenter has left the sensor before the next point")
|
||||
if self.phase != "load":
|
||||
raise RuntimeError("no load step is waiting for confirmation")
|
||||
branch, target = self._steps[self._step_index]
|
||||
force = _finite(force_n, "reference force")
|
||||
if force < 0 or force > float(self.max_force_n):
|
||||
raise ValueError("reference force must be between 0 and the maximum force you entered")
|
||||
if branch == "baseline" and force != 0:
|
||||
raise ValueError("the unloaded baseline needs a reference of 0 when you are using weights")
|
||||
now = time.perf_counter_ns()
|
||||
self._pending = {"source": "weight", "force": force, "fx": "", "fy": "", "fz": ""}
|
||||
self._active = (self._step_index, branch, target, force, now)
|
||||
self._hold_started_ns = now
|
||||
self._event("hold_started", {
|
||||
"step_index": self._step_index, "branch": branch, "target_load_n": target, "source": "weight",
|
||||
})
|
||||
self.phase = "hold"
|
||||
|
||||
def confirm_wrench(self, fx: float, fy: float, fz: float) -> None:
|
||||
"""Record a six-axis sample. The normal is the projection, not one raw axis."""
|
||||
if self.phase == "liftoff":
|
||||
raise RuntimeError("confirm the indenter has left the sensor before the next point")
|
||||
if self.phase != "load":
|
||||
raise RuntimeError("no load step is waiting for confirmation")
|
||||
if self._rotation is None:
|
||||
raise ValueError("enter the 9-number force-sensor to tool rotation before a six-axis reading")
|
||||
force = normal_force_n(fx, fy, fz, self._rotation)
|
||||
if force < -1e-9:
|
||||
raise ValueError("the normal component is negative; check the rotation instead of using one axis")
|
||||
if force > float(self.max_force_n) + 1e-9:
|
||||
raise ValueError("the normal component is above the maximum force you entered")
|
||||
_branch, _target = self._steps[self._step_index]
|
||||
now = time.perf_counter_ns()
|
||||
self._pending = {
|
||||
"source": "wrench",
|
||||
"force": force,
|
||||
"fx": _finite(fx, "fx"),
|
||||
"fy": _finite(fy, "fy"),
|
||||
"fz": _finite(fz, "fz"),
|
||||
}
|
||||
self._active = (self._step_index, _branch, _target, force, now)
|
||||
self._hold_started_ns = now
|
||||
self._event("hold_started", {
|
||||
"step_index": self._step_index,
|
||||
"branch": _branch,
|
||||
"target_load_n": _target,
|
||||
"source": "wrench",
|
||||
"fx_N": self._pending["fx"],
|
||||
"fy_N": self._pending["fy"],
|
||||
"fz_N": self._pending["fz"],
|
||||
"F_ref_normal_N": force,
|
||||
})
|
||||
self.phase = "hold"
|
||||
|
||||
def complete_hold(self, *, now_ns: int | None = None) -> None:
|
||||
if self.phase != "hold" or self._active is None or self._hold_started_ns is None:
|
||||
raise RuntimeError("no hold is in progress")
|
||||
now = time.perf_counter_ns() if now_ns is None else int(now_ns)
|
||||
if now + 1_000_000 < self._hold_started_ns + int(self._hold_s * 1e9):
|
||||
raise RuntimeError("the hold time has not elapsed")
|
||||
step_index, branch, target, force, start = self._active
|
||||
self._reference.writerow(self._reference_values(
|
||||
step_index, branch, target, start, now, force, 1,
|
||||
))
|
||||
self._reference_file.flush()
|
||||
self._event("hold_ended", {"step_index": step_index, "hold_complete": 1})
|
||||
self._active = None
|
||||
self._pending = None
|
||||
self._hold_started_ns = None
|
||||
self._completed_holds += 1
|
||||
self._step_index += 1
|
||||
if self._step_index < len(self._steps):
|
||||
self.phase = "load"
|
||||
return
|
||||
self._close_repeat(complete=True)
|
||||
self._repeat_index += 1
|
||||
if self._single_site:
|
||||
self.phase = "liftoff"
|
||||
self._event("liftoff_required", {"next_site_id": ""})
|
||||
return
|
||||
site = self.sites[self._site_index]
|
||||
if self._repeat_index < site.repeats:
|
||||
self._open_repeat()
|
||||
self.phase = "load"
|
||||
return
|
||||
self._site_index += 1
|
||||
self._repeat_index = 0
|
||||
if self._site_index < len(self.sites):
|
||||
self.phase = "liftoff"
|
||||
self._event("liftoff_required", {"next_site_id": self.sites[self._site_index].site_id})
|
||||
return
|
||||
self._finish_session("complete")
|
||||
self.phase = "done"
|
||||
|
||||
def confirm_liftoff(self) -> None:
|
||||
if self.phase != "liftoff":
|
||||
raise RuntimeError("liftoff confirmation is only required between points")
|
||||
if self._single_site:
|
||||
self._event("liftoff_confirmed", {
|
||||
"next_site_id": "",
|
||||
"measured_clearance": "not_available",
|
||||
})
|
||||
self._single_site = False
|
||||
self._finish_session(self._session_status())
|
||||
self.phase = "done"
|
||||
return
|
||||
self._event("liftoff_confirmed", {
|
||||
"next_site_id": self.sites[self._site_index].site_id,
|
||||
"measured_clearance": "not_available",
|
||||
})
|
||||
self._open_repeat()
|
||||
self.phase = "load"
|
||||
|
||||
def stop(self, reason: str) -> None:
|
||||
if not self.is_active:
|
||||
return
|
||||
self.reason = reason or "stopped"
|
||||
if self._active is not None:
|
||||
step_index, branch, target, force, start = self._active
|
||||
now = time.perf_counter_ns()
|
||||
self._reference.writerow(self._reference_values(
|
||||
step_index, branch, target, start, now, force, 0,
|
||||
))
|
||||
self._reference_file.flush()
|
||||
self._active = None
|
||||
self._pending = None
|
||||
if self._trial_dir is not None:
|
||||
self._close_repeat(complete=False)
|
||||
self._finish_session("incomplete")
|
||||
self.phase = "aborted"
|
||||
|
||||
def reacquire(self, site_id: str) -> None:
|
||||
"""Append one more repeat of a point into this same session."""
|
||||
if self.is_active:
|
||||
raise RuntimeError("finish or stop the current point before recording it again")
|
||||
if self.directory is None or self._document is None:
|
||||
raise RuntimeError("start a session before recording a point again")
|
||||
self._site_index = self._index(site_id)
|
||||
self._repeat_index = sum(
|
||||
1 for trial in self._document["trials"] if trial["site_id"] == site_id
|
||||
)
|
||||
self._single_site = True
|
||||
self.reason = ""
|
||||
self._reopen_events()
|
||||
self._document["status"] = "in_progress"
|
||||
self._save()
|
||||
self._open_repeat()
|
||||
self.phase = "load"
|
||||
|
||||
def completion(self) -> list[dict]:
|
||||
counts: dict[str, int] = {}
|
||||
if self._document is not None:
|
||||
for trial in self._document["trials"]:
|
||||
if trial.get("status") == "complete":
|
||||
counts[trial["site_id"]] = counts.get(trial["site_id"], 0) + 1
|
||||
return [
|
||||
{
|
||||
"site_id": site.site_id,
|
||||
"repeats_planned": site.repeats,
|
||||
"repeats_complete": counts.get(site.site_id, 0),
|
||||
"complete": counts.get(site.site_id, 0) >= site.repeats,
|
||||
}
|
||||
for site in self.sites
|
||||
]
|
||||
|
||||
def remaining_s(self) -> float:
|
||||
if self.phase != "hold" or self._hold_started_ns is None:
|
||||
return 0.0
|
||||
elapsed = (time.perf_counter_ns() - self._hold_started_ns) / 1e9
|
||||
return max(0.0, self._hold_s - elapsed)
|
||||
|
||||
def progress(self) -> dict:
|
||||
site = self.sites[self._site_index] if self.sites and self._site_index < len(self.sites) else None
|
||||
step = self._steps[self._step_index] if self._step_index < len(self._steps) else None
|
||||
return {
|
||||
"phase": self.phase,
|
||||
"site_id": site.site_id if site else "",
|
||||
"site_index": self._site_index + 1 if site else 0,
|
||||
"site_count": len(self.sites),
|
||||
"position": site.position() if site else None,
|
||||
"repeat_index": self._repeat_index + 1 if site and self.is_active else 0,
|
||||
"repeats": site.repeats if site else 0,
|
||||
"branch": step[0] if step and self.phase in {"load", "hold"} else "",
|
||||
"target_n": step[1] if step and self.phase in {"load", "hold"} else None,
|
||||
"done": self._completed_holds,
|
||||
"total": self._total_holds(),
|
||||
"directory": str(self.directory) if self.directory else "",
|
||||
"reason": self.reason,
|
||||
}
|
||||
|
||||
def attach_capture(self, worker, *, fps: float) -> None:
|
||||
"""Record images into the open trial. Does not move the platform."""
|
||||
if self._trial_dir is None:
|
||||
raise RuntimeError("no trial is open")
|
||||
packet = None if worker is None else worker.latest()
|
||||
if worker is None or not worker.is_running or packet is None or packet.capture_mono_ns is None:
|
||||
raise RuntimeError("start the camera and wait for a frame before live capture")
|
||||
self._pending_worker = worker
|
||||
self._fps = float(fps)
|
||||
self._start_images(packet)
|
||||
|
||||
def _spec(self, site_id, x, y, z, levels, hold_s, repeats, max_force_n, min_force_n: float = 0.0) -> SiteSpec:
|
||||
hold = _finite(hold_s, "hold time")
|
||||
count = int(repeats)
|
||||
if hold <= 0:
|
||||
raise ValueError("hold time must be greater than 0")
|
||||
if count < 1:
|
||||
raise ValueError("repeat count must be at least 1")
|
||||
steps = load_steps(list(levels), float(max_force_n), min_force_n=float(min_force_n))
|
||||
return SiteSpec(
|
||||
site_id=site_id,
|
||||
x=_finite(x, "x"),
|
||||
y=_finite(y, "y"),
|
||||
z=_finite(z, "z"),
|
||||
levels_n=tuple(target for branch, target in steps if branch == "load"),
|
||||
hold_s=hold,
|
||||
repeats=count,
|
||||
)
|
||||
|
||||
def _require_idle(self) -> None:
|
||||
if self.is_active:
|
||||
raise RuntimeError("stop the acquisition before editing points")
|
||||
|
||||
def _index(self, site_id: str) -> int:
|
||||
for index, site in enumerate(self.sites):
|
||||
if site.site_id == site_id:
|
||||
return index
|
||||
raise ValueError("that point is not in the list")
|
||||
|
||||
def _site_record(self, site: SiteSpec) -> dict:
|
||||
return {
|
||||
"site_id": site.site_id,
|
||||
"position": site.position(),
|
||||
"coordinate_frame": site.position_frame or self.coordinate_frame,
|
||||
"position_frame": site.position_frame or self.coordinate_frame,
|
||||
"normal_sensor": None if site.normal is None else {
|
||||
"x": site.normal[0], "y": site.normal[1], "z": site.normal[2],
|
||||
},
|
||||
"pose": site.pose,
|
||||
"uv_px": None if site.uv_px is None else {"u": site.uv_px[0], "v": site.uv_px[1]},
|
||||
"levels_n": list(site.levels_n),
|
||||
"hold_s": site.hold_s,
|
||||
"repeats": site.repeats,
|
||||
}
|
||||
|
||||
def _total_holds(self) -> int:
|
||||
if self.max_force_n is None:
|
||||
return 0
|
||||
return sum(
|
||||
site.repeats * len(self._steps_for(site)) for site in self.sites
|
||||
)
|
||||
|
||||
def _open_repeat(self) -> None:
|
||||
site = self.sites[self._site_index]
|
||||
self._steps = self._steps_for(site)
|
||||
self._step_index = 0
|
||||
self._hold_s = site.hold_s
|
||||
self._trial_serial += 1
|
||||
repeat_number = self._repeat_index + 1
|
||||
folder = _path_part(site.site_id)
|
||||
trial_id = f"{folder}-repeat-{repeat_number:03d}"
|
||||
directory = self.directory / "sites" / folder / f"repeat-{repeat_number:03d}"
|
||||
directory.mkdir(parents=True)
|
||||
self._trial_dir = directory
|
||||
metadata = {
|
||||
"trial_id": trial_id,
|
||||
"site_id": site.site_id,
|
||||
"position": site.position(),
|
||||
"position_frame": site.position_frame or self.coordinate_frame,
|
||||
"normal_sensor": None if site.normal is None else {
|
||||
"x": site.normal[0], "y": site.normal[1], "z": site.normal[2],
|
||||
},
|
||||
"pose": site.pose,
|
||||
"uv_px": None if site.uv_px is None else {"u": site.uv_px[0], "v": site.uv_px[1]},
|
||||
"coordinate_frame": site.position_frame or self.coordinate_frame,
|
||||
"repeat_index": repeat_number,
|
||||
"repeat_count": site.repeats,
|
||||
"levels_n": list(site.levels_n),
|
||||
"hold_s": site.hold_s,
|
||||
"steps": [
|
||||
{"step_index": i, "branch": branch, "target_load_n": target}
|
||||
for i, (branch, target) in enumerate(self._steps)
|
||||
],
|
||||
"sync": SYNC,
|
||||
"images": {
|
||||
"captured": False,
|
||||
"reason": "simulation does not open a camera" if self.simulate else "camera not attached yet",
|
||||
},
|
||||
"status": "incomplete",
|
||||
}
|
||||
write_json(directory / "session.json", metadata)
|
||||
reference_file = (directory / "reference.csv").open("w", newline="", encoding="utf-8")
|
||||
writer = csv.writer(reference_file)
|
||||
writer.writerow(REFERENCE_HEADER)
|
||||
reference_file.flush()
|
||||
self._reference_file = reference_file
|
||||
self._reference = writer
|
||||
events = (directory / "events.csv").open("w", newline="", encoding="utf-8")
|
||||
event_writer = csv.writer(events)
|
||||
event_writer.writerow(("t_host_mono_ns", "t_host_utc_ns", "kind", "detail"))
|
||||
events.flush()
|
||||
self._events_file = events
|
||||
self._events = event_writer
|
||||
self._event("trial_started", {"site_id": site.site_id, "repeat_index": self._repeat_index + 1})
|
||||
if self._pending_worker is not None:
|
||||
self._start_images(self._pending_worker.latest())
|
||||
self._document["trials"].append({
|
||||
"trial_id": trial_id,
|
||||
"site_id": site.site_id,
|
||||
"repeat_index": self._repeat_index + 1,
|
||||
"directory": f"sites/{folder}/repeat-{repeat_number:03d}",
|
||||
"status": "incomplete",
|
||||
})
|
||||
self._save()
|
||||
|
||||
def _close_repeat(self, *, complete: bool) -> None:
|
||||
self._release_images()
|
||||
if self._reference_file is not None:
|
||||
self._reference_file.close()
|
||||
self._reference_file = None
|
||||
if self._events_file is not None:
|
||||
self._events_file.close()
|
||||
self._events_file = None
|
||||
self._events = None
|
||||
if self._trial_dir is None:
|
||||
return
|
||||
path = self._trial_dir / "session.json"
|
||||
metadata = json.loads(path.read_text(encoding="utf-8"))
|
||||
metadata["status"] = "complete" if complete else "incomplete"
|
||||
metadata["reason"] = "" if complete else self.reason
|
||||
write_json(path, metadata)
|
||||
self._document["trials"][-1]["status"] = metadata["status"]
|
||||
self._document["trials"][-1]["reason"] = metadata["reason"]
|
||||
self._trial_dir = None
|
||||
self._save()
|
||||
|
||||
def _finish_session(self, status: str) -> None:
|
||||
self._document["status"] = status
|
||||
self._document["reason"] = self.reason
|
||||
self._document["ended_at"] = datetime.now(timezone.utc).isoformat()
|
||||
self._save()
|
||||
if self._session_events_file is not None:
|
||||
self._session_events_file.close()
|
||||
self._session_events_file = None
|
||||
self._session_events = None
|
||||
|
||||
def _save(self) -> None:
|
||||
if self._document is not None:
|
||||
self._document["completion"] = self.completion()
|
||||
write_json(self.directory / "session.json", self._document)
|
||||
|
||||
def _steps_for(self, site: SiteSpec) -> list[tuple[str, float]]:
|
||||
return load_steps(
|
||||
list(site.levels_n), float(self.max_force_n), min_force_n=self.min_force_n,
|
||||
)
|
||||
|
||||
def _session_status(self) -> str:
|
||||
rows = self.completion()
|
||||
if rows and all(row["complete"] for row in rows):
|
||||
return "complete"
|
||||
if any(row["repeats_complete"] for row in rows):
|
||||
return "partial"
|
||||
return "incomplete"
|
||||
|
||||
def _reopen_events(self) -> None:
|
||||
if self._session_events is not None:
|
||||
return
|
||||
handle = (self.directory / "events.csv").open("a", newline="", encoding="utf-8")
|
||||
self._session_events_file = handle
|
||||
self._session_events = csv.writer(handle)
|
||||
|
||||
def _reference_values(self, step_index, branch, target, start, end, force, complete) -> tuple:
|
||||
site = self.sites[self._site_index]
|
||||
pending = self._pending or {"source": "", "fx": "", "fy": "", "fz": ""}
|
||||
pose = site.pose or {}
|
||||
normal = site.normal or ("", "", "")
|
||||
rotation = ""
|
||||
if pending.get("source") == "wrench" and self._rotation is not None:
|
||||
rotation = " ".join(str(value) for row in self._rotation for value in row)
|
||||
return (
|
||||
step_index, branch, target, start, end, force,
|
||||
site.site_id, site.x, site.y, site.z,
|
||||
site.position_frame or self.coordinate_frame,
|
||||
self._repeat_index + 1, site.repeats, complete,
|
||||
pending.get("source", ""), pending.get("fx", ""), pending.get("fy", ""), pending.get("fz", ""),
|
||||
rotation, normal[0], normal[1], normal[2],
|
||||
pose.get("x", ""), pose.get("y", ""), pose.get("yaw", ""),
|
||||
pose.get("pitch", ""), pose.get("z", ""),
|
||||
pose.get("normal_aligned_to", ""),
|
||||
)
|
||||
|
||||
def _event(self, kind: str, detail: dict) -> None:
|
||||
now = time.perf_counter_ns()
|
||||
utc = time.time_ns() + now - time.perf_counter_ns()
|
||||
row = (now, utc, kind, json.dumps(detail, ensure_ascii=False))
|
||||
if self._session_events is not None:
|
||||
self._session_events.writerow(row)
|
||||
self._session_events_file.flush()
|
||||
if self._events is not None:
|
||||
self._events.writerow(row)
|
||||
self._events_file.flush()
|
||||
|
||||
def _start_images(self, packet) -> None:
|
||||
if packet is None or packet.capture_mono_ns is None:
|
||||
raise RuntimeError("start the camera and wait for a frame before live capture")
|
||||
from orisys_app_shared.record import ExperimentRecorder, RecordConfig
|
||||
|
||||
self._release_images()
|
||||
recorder = ExperimentRecorder(
|
||||
RecordConfig(fps=self._fps, segment_seconds=24 * 3600.0, preserve_all_frames=True)
|
||||
)
|
||||
recorder.start(
|
||||
root=self._trial_dir,
|
||||
session_id=int(packet.session_id),
|
||||
metadata={"sync": SYNC, "images_only": True},
|
||||
directory_name="capture",
|
||||
)
|
||||
self._pending_worker.add_sink(recorder.on_packet)
|
||||
self._recorder = recorder
|
||||
self._image_worker = self._pending_worker
|
||||
trial = self._document["trials"][-1]
|
||||
trial["images"] = {"captured": True, "directory": f"{trial['directory']}/capture"}
|
||||
self._save()
|
||||
|
||||
def _release_images(self) -> None:
|
||||
recorder, worker = self._recorder, self._image_worker
|
||||
self._recorder = None
|
||||
self._image_worker = None
|
||||
if worker is not None and recorder is not None:
|
||||
worker.remove_sink(recorder.on_packet)
|
||||
if recorder is not None:
|
||||
recorder.stop(wait=True)
|
||||
|
||||
|
||||
def _path_part(value: str) -> str:
|
||||
cleaned = re.sub(r"[^\w-]+", "-", value).strip("-_")[:48]
|
||||
if not cleaned:
|
||||
raise ValueError("sensor model and SN need letters or numbers")
|
||||
return cleaned
|
||||
|
||||
|
||||
def _session_directory(root: Path, model: str, serial: str) -> Path:
|
||||
parts = [_path_part(model), _path_part(serial)]
|
||||
stamp = datetime.now(timezone.utc).strftime("%Y%m%dT%H%M%SZ")
|
||||
directory = root / f"{'_'.join(parts)}_{stamp}_{uuid4().hex[:12]}"
|
||||
directory.mkdir(parents=True, exist_ok=False)
|
||||
return directory
|
||||
@@ -118,6 +118,8 @@ def review_session(directory: Path, *, selections: dict | None = None) -> dict:
|
||||
"""
|
||||
root = Path(directory).resolve()
|
||||
manifest = json.loads((root / "session.json").read_text(encoding="utf-8"))
|
||||
if manifest.get("format_version") == "autocal-multipoint-1.0":
|
||||
raise ValueError("multi-point sessions are raw acquisition folders; this review only opens finger sessions")
|
||||
if manifest.get("format_version") != "autocal-session-1.0":
|
||||
raise ValueError("select a calibration session root, not an individual trial")
|
||||
windows = []
|
||||
|
||||
@@ -31,6 +31,7 @@ from ..sensor.uv_flat import site_at, uv_origin
|
||||
from ..sensor.uv_outline import outline_snapshot
|
||||
from ..trial import ManualTrial, load_path
|
||||
from .live_force_view import LiveForceView
|
||||
from .multipoint_panel import MultipointPanel
|
||||
from .sensor_panel import FlatSurfaceView
|
||||
|
||||
#: Product models a session can be recorded under. The stored value is this id.
|
||||
@@ -103,6 +104,8 @@ class CalibrationPanel(QWidget):
|
||||
site_layout.addWidget(self.bundle_button)
|
||||
self.views.addTab(self.site_view, "")
|
||||
self.views.addTab(self.source_panel, "")
|
||||
self.multipoint = MultipointPanel(self, self.views)
|
||||
self.views.addTab(self.multipoint, "")
|
||||
body.addWidget(self.views, 3)
|
||||
|
||||
controls = QGroupBox(self)
|
||||
@@ -256,6 +259,8 @@ class CalibrationPanel(QWidget):
|
||||
|
||||
def _start(self) -> None:
|
||||
try:
|
||||
if self.multipoint.is_active:
|
||||
raise RuntimeError(t("multipoint.finger_busy"))
|
||||
if self._site is None:
|
||||
raise ValueError(t("calibration.invalid_site"))
|
||||
if not self.output_path.text().strip():
|
||||
@@ -299,6 +304,9 @@ class CalibrationPanel(QWidget):
|
||||
self._sync()
|
||||
|
||||
def _confirm(self) -> None:
|
||||
if self.multipoint.campaign.phase == "load":
|
||||
self.multipoint.confirm_loaded()
|
||||
return
|
||||
try:
|
||||
if not self.reference.text().strip():
|
||||
raise ValueError(t("calibration.reference_required"))
|
||||
@@ -375,6 +383,7 @@ class CalibrationPanel(QWidget):
|
||||
try:
|
||||
if self.trial.status == "recording":
|
||||
self.trial.finish(aborted=True, reason="application closed")
|
||||
self.multipoint.shutdown()
|
||||
if self.trial.status == "closing" and self.trial.recorder is not None:
|
||||
self.trial.recorder.stop(wait=True)
|
||||
self.trial.poll()
|
||||
@@ -395,13 +404,19 @@ class CalibrationPanel(QWidget):
|
||||
self.bundle_button,
|
||||
):
|
||||
widget.setEnabled(not locked)
|
||||
campaign_busy = self.multipoint.is_active
|
||||
for widget in (self.sensor_sn, self.sensor_model, self.output_path,
|
||||
self.output_browse, self.bundle_button):
|
||||
widget.setEnabled(not locked and self.trial.session is None)
|
||||
self.start_button.setEnabled(not locked and self._site is not None)
|
||||
widget.setEnabled(not locked and self.trial.session is None and not campaign_busy)
|
||||
self.start_button.setEnabled(
|
||||
not locked and self._site is not None and not campaign_busy
|
||||
)
|
||||
can_confirm = recording and self.trial.next_step is not None and not self.trial.holding
|
||||
self.confirm_button.setEnabled(can_confirm)
|
||||
self.reference.setEnabled(can_confirm)
|
||||
multipoint_load = self.multipoint.campaign.phase == "load"
|
||||
self.confirm_button.setEnabled(can_confirm or multipoint_load)
|
||||
self.reference.setEnabled(
|
||||
can_confirm or (multipoint_load and self.multipoint.source.currentData() != "wrench")
|
||||
)
|
||||
self.finish_button.setEnabled(recording and self.trial.next_step is None)
|
||||
self.abort_button.setEnabled(recording)
|
||||
step = self.trial.next_step
|
||||
@@ -417,6 +432,8 @@ class CalibrationPanel(QWidget):
|
||||
target_text = t("calibration.before_start")
|
||||
else:
|
||||
target_text = t("calibration.no_step")
|
||||
if self.multipoint.campaign.phase == "load":
|
||||
target_text = t("multipoint.right_confirm")
|
||||
self.target_label.setText(target_text)
|
||||
self.progress_label.setText(
|
||||
t("calibration.progress").format(
|
||||
@@ -466,6 +483,8 @@ class CalibrationPanel(QWidget):
|
||||
self.review_button.setText(t("review.open"))
|
||||
self.views.setTabText(0, t("calibration.site_view"))
|
||||
self.views.setTabText(1, t("calibration.live_view"))
|
||||
self.views.setTabText(2, t("multipoint.tab"))
|
||||
self.multipoint.retranslate()
|
||||
self.bundle_button.setText(t("finger.load"))
|
||||
self.coords_label.setText(t("calibration.uv"))
|
||||
self.position_label.setText(t("calibration.position"))
|
||||
|
||||
@@ -0,0 +1,591 @@
|
||||
"""02 multi-point acquisition. Manual loads only; this page never moves the platform."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import numpy as np
|
||||
from PySide6.QtCore import Qt, QTimer, Signal
|
||||
from PySide6.QtWidgets import (
|
||||
QComboBox,
|
||||
QGridLayout,
|
||||
QHBoxLayout,
|
||||
QHeaderView,
|
||||
QLabel,
|
||||
QLineEdit,
|
||||
QPushButton,
|
||||
QScrollArea,
|
||||
QSizePolicy,
|
||||
QSpinBox,
|
||||
QTableWidget,
|
||||
QTableWidgetItem,
|
||||
QVBoxLayout,
|
||||
QWidget,
|
||||
)
|
||||
from orisys_app_shared.i18n import t
|
||||
|
||||
from ..multipoint import Campaign, next_point_id
|
||||
from ..sensor.uv_flat import site_at
|
||||
|
||||
|
||||
class _WrappingLabel(QLabel):
|
||||
"""Keep wrapped text inside its own row instead of painting over the next control."""
|
||||
|
||||
def __init__(self, parent: QWidget | None = None) -> None:
|
||||
super().__init__(parent)
|
||||
self.setWordWrap(True)
|
||||
self.setSizePolicy(QSizePolicy.Policy.Preferred, QSizePolicy.Policy.Minimum)
|
||||
|
||||
def resizeEvent(self, event) -> None:
|
||||
super().resizeEvent(event)
|
||||
height = self.heightForWidth(self.width())
|
||||
if height > 0 and self.minimumHeight() != height:
|
||||
self.setMinimumHeight(height)
|
||||
self.updateGeometry()
|
||||
|
||||
|
||||
class MultipointPanel(QWidget):
|
||||
"""Add points, then record them in order after each load is confirmed by hand."""
|
||||
|
||||
alignRequested = Signal(object)
|
||||
|
||||
def __init__(self, host: QWidget, parent: QWidget | None = None) -> None:
|
||||
super().__init__(parent)
|
||||
self.host = host
|
||||
self.campaign = Campaign()
|
||||
self._selected = ""
|
||||
self._offered_liftoff = ""
|
||||
self._finger = None
|
||||
|
||||
outer = QVBoxLayout(self)
|
||||
outer.setContentsMargins(0, 0, 0, 0)
|
||||
scroll = QScrollArea(self)
|
||||
scroll.setWidgetResizable(True)
|
||||
scroll.setFrameShape(QScrollArea.Shape.NoFrame)
|
||||
scroll.setHorizontalScrollBarPolicy(Qt.ScrollBarPolicy.ScrollBarAsNeeded)
|
||||
scroll.setVerticalScrollBarPolicy(Qt.ScrollBarPolicy.ScrollBarAsNeeded)
|
||||
outer.addWidget(scroll)
|
||||
page = QWidget(scroll)
|
||||
page.setMinimumWidth(560)
|
||||
layout = QVBoxLayout(page)
|
||||
layout.setContentsMargins(12, 12, 12, 12)
|
||||
layout.setSpacing(12)
|
||||
scroll.setWidget(page)
|
||||
|
||||
self.hint = _WrappingLabel(page)
|
||||
layout.addWidget(self.hint)
|
||||
|
||||
self.site_id = QLineEdit(page)
|
||||
self.repeats = QSpinBox(page)
|
||||
self.repeats.setRange(1, 99)
|
||||
self.repeats.setValue(1)
|
||||
self.repeats.setMinimumWidth(160)
|
||||
self.frame = QLineEdit(page)
|
||||
self.max_force = QLineEdit(page)
|
||||
self.min_force = QLineEdit(page)
|
||||
self.id_label = QLabel(page)
|
||||
self.repeats_label = QLabel(page)
|
||||
self.frame_label = QLabel(page)
|
||||
self.force_label = QLabel(page)
|
||||
self.min_label = QLabel(page)
|
||||
form = QGridLayout()
|
||||
form.setHorizontalSpacing(16)
|
||||
form.setVerticalSpacing(10)
|
||||
form.setColumnMinimumWidth(0, 156)
|
||||
form.setColumnStretch(1, 1)
|
||||
fields = (
|
||||
(self.id_label, self.site_id),
|
||||
(self.repeats_label, self.repeats),
|
||||
(self.frame_label, self.frame),
|
||||
(self.min_label, self.min_force),
|
||||
(self.force_label, self.max_force),
|
||||
)
|
||||
for row, (label, field) in enumerate(fields):
|
||||
label.setAlignment(Qt.AlignmentFlag.AlignRight | Qt.AlignmentFlag.AlignVCenter)
|
||||
label.setSizePolicy(QSizePolicy.Policy.Minimum, QSizePolicy.Policy.Preferred)
|
||||
form.addWidget(label, row, 0)
|
||||
form.addWidget(field, row, 1)
|
||||
layout.addLayout(form)
|
||||
|
||||
buttons = QHBoxLayout()
|
||||
buttons.setSpacing(8)
|
||||
self.add_button = QPushButton(page)
|
||||
self.update_button = QPushButton(page)
|
||||
self.remove_button = QPushButton(page)
|
||||
self.up_button = QPushButton(page)
|
||||
self.down_button = QPushButton(page)
|
||||
for button in (
|
||||
self.add_button, self.update_button, self.remove_button,
|
||||
self.up_button, self.down_button,
|
||||
):
|
||||
buttons.addWidget(button, 1)
|
||||
layout.addLayout(buttons)
|
||||
extra = QHBoxLayout()
|
||||
extra.setSpacing(8)
|
||||
self.import_button = QPushButton(page)
|
||||
self.align_button = QPushButton(page)
|
||||
self.reacquire_button = QPushButton(page)
|
||||
for button in (self.import_button, self.align_button, self.reacquire_button):
|
||||
extra.addWidget(button, 1)
|
||||
layout.addLayout(extra)
|
||||
|
||||
self.table = QTableWidget(0, 8, page)
|
||||
self.table.setSelectionBehavior(QTableWidget.SelectionBehavior.SelectRows)
|
||||
self.table.setSelectionMode(QTableWidget.SelectionMode.SingleSelection)
|
||||
self.table.setMinimumHeight(160)
|
||||
self.table.verticalHeader().setVisible(False)
|
||||
self.table.horizontalHeader().setSectionResizeMode(QHeaderView.ResizeMode.Stretch)
|
||||
self.table.horizontalHeader().setStretchLastSection(True)
|
||||
self.table.itemSelectionChanged.connect(self._on_select)
|
||||
layout.addWidget(self.table, 1)
|
||||
|
||||
self.progress = _WrappingLabel(page)
|
||||
self.save_status = _WrappingLabel(page)
|
||||
layout.addWidget(self.progress)
|
||||
layout.addWidget(self.save_status)
|
||||
self.pose_status = _WrappingLabel(page)
|
||||
layout.addWidget(self.pose_status)
|
||||
|
||||
self.source_label = QLabel(page)
|
||||
self.source_label.setAlignment(Qt.AlignmentFlag.AlignRight | Qt.AlignmentFlag.AlignVCenter)
|
||||
self.source_label.setMinimumWidth(156)
|
||||
self.source = QComboBox(page)
|
||||
self.source.addItem("", "weight")
|
||||
self.source.addItem("", "wrench")
|
||||
source_row = QHBoxLayout()
|
||||
source_row.setSpacing(16)
|
||||
source_row.addWidget(self.source_label)
|
||||
source_row.addWidget(self.source, 1)
|
||||
layout.addLayout(source_row)
|
||||
|
||||
self.wrench_box = QWidget(page)
|
||||
wrench = QGridLayout(self.wrench_box)
|
||||
wrench.setContentsMargins(0, 0, 0, 0)
|
||||
wrench.setHorizontalSpacing(8)
|
||||
wrench.setVerticalSpacing(8)
|
||||
self.fx_label = QLabel(self.wrench_box)
|
||||
self.fy_label = QLabel(self.wrench_box)
|
||||
self.fz_label = QLabel(self.wrench_box)
|
||||
self.fx_edit = QLineEdit(self.wrench_box)
|
||||
self.fy_edit = QLineEdit(self.wrench_box)
|
||||
self.fz_edit = QLineEdit(self.wrench_box)
|
||||
self.rotation_label = QLabel(self.wrench_box)
|
||||
self.rotation = QLineEdit(self.wrench_box)
|
||||
wrench.addWidget(self.fx_label, 0, 0)
|
||||
wrench.addWidget(self.fx_edit, 0, 1)
|
||||
wrench.addWidget(self.fy_label, 0, 2)
|
||||
wrench.addWidget(self.fy_edit, 0, 3)
|
||||
wrench.addWidget(self.fz_label, 0, 4)
|
||||
wrench.addWidget(self.fz_edit, 0, 5)
|
||||
wrench.addWidget(self.rotation_label, 1, 0)
|
||||
wrench.addWidget(self.rotation, 1, 1, 1, 5)
|
||||
layout.addWidget(self.wrench_box)
|
||||
|
||||
actions = QGridLayout()
|
||||
actions.setHorizontalSpacing(8)
|
||||
actions.setVerticalSpacing(8)
|
||||
self.simulate_button = QPushButton(page)
|
||||
self.start_button = QPushButton(page)
|
||||
self.liftoff_button = QPushButton(page)
|
||||
self.stop_button = QPushButton(page)
|
||||
actions.addWidget(self.simulate_button, 0, 0)
|
||||
actions.addWidget(self.start_button, 0, 1)
|
||||
actions.addWidget(self.liftoff_button, 1, 0, 1, 2)
|
||||
actions.addWidget(self.stop_button, 2, 0, 1, 2)
|
||||
actions.setColumnStretch(0, 1)
|
||||
actions.setColumnStretch(1, 1)
|
||||
layout.addLayout(actions)
|
||||
self.message = _WrappingLabel(page)
|
||||
layout.addWidget(self.message)
|
||||
|
||||
self.add_button.clicked.connect(self._add)
|
||||
self.update_button.clicked.connect(self._update)
|
||||
self.remove_button.clicked.connect(self._remove)
|
||||
self.up_button.clicked.connect(lambda: self._move(-1))
|
||||
self.down_button.clicked.connect(lambda: self._move(1))
|
||||
self.import_button.clicked.connect(self._import_finger)
|
||||
self.align_button.clicked.connect(self._align)
|
||||
self.reacquire_button.clicked.connect(self._reacquire)
|
||||
self.source.currentIndexChanged.connect(lambda _index: self._refresh())
|
||||
self.simulate_button.clicked.connect(lambda: self._begin(simulate=True))
|
||||
self.start_button.clicked.connect(lambda: self._begin(simulate=False))
|
||||
self.liftoff_button.clicked.connect(self._liftoff)
|
||||
self.stop_button.clicked.connect(self._stop)
|
||||
self._timer = QTimer(self)
|
||||
self._timer.setInterval(200)
|
||||
self._timer.timeout.connect(self._poll)
|
||||
self._timer.start()
|
||||
self._listed: tuple = ()
|
||||
self.retranslate()
|
||||
self.site_id.setText(self._next_site_id())
|
||||
self.frame.setText("sensor")
|
||||
self._refresh()
|
||||
|
||||
@property
|
||||
def is_active(self) -> bool:
|
||||
return self.campaign.is_active
|
||||
|
||||
def bind_finger(self, panel) -> None:
|
||||
"""The finger page whose picked points and solved poses this tab records."""
|
||||
self._finger = panel
|
||||
|
||||
def shutdown(self) -> None:
|
||||
self._timer.stop()
|
||||
if self.campaign.is_active:
|
||||
self.campaign.stop("application closed")
|
||||
|
||||
def retranslate(self) -> None:
|
||||
self.hint.setText(t("multipoint.hint"))
|
||||
self.id_label.setText(t("multipoint.site_id"))
|
||||
self.repeats_label.setText(t("multipoint.repeats"))
|
||||
self.frame_label.setText(t("multipoint.frame"))
|
||||
self.force_label.setText(t("multipoint.max_force"))
|
||||
self.min_label.setText(t("multipoint.min_force"))
|
||||
self.frame.setPlaceholderText(t("multipoint.frame_placeholder"))
|
||||
self.max_force.setPlaceholderText(t("multipoint.force_placeholder"))
|
||||
self.min_force.setPlaceholderText(t("multipoint.min_placeholder"))
|
||||
self.add_button.setText(t("multipoint.add"))
|
||||
self.update_button.setText(t("multipoint.update"))
|
||||
self.remove_button.setText(t("multipoint.remove"))
|
||||
self.up_button.setText(t("multipoint.up"))
|
||||
self.down_button.setText(t("multipoint.down"))
|
||||
self.import_button.setText(t("multipoint.import_points"))
|
||||
self.align_button.setText(t("multipoint.align"))
|
||||
self.reacquire_button.setText(t("multipoint.reacquire"))
|
||||
self.source_label.setText(t("multipoint.source"))
|
||||
self.source.setItemText(0, t("multipoint.source_weight"))
|
||||
self.source.setItemText(1, t("multipoint.source_wrench"))
|
||||
self.fx_label.setText(t("multipoint.fx"))
|
||||
self.fy_label.setText(t("multipoint.fy"))
|
||||
self.fz_label.setText(t("multipoint.fz"))
|
||||
self.rotation_label.setText(t("multipoint.rotation"))
|
||||
self.rotation.setPlaceholderText(t("multipoint.rotation_placeholder"))
|
||||
self.simulate_button.setText(t("multipoint.simulate"))
|
||||
self.start_button.setText(t("multipoint.start"))
|
||||
self.liftoff_button.setText(t("multipoint.liftoff"))
|
||||
self.stop_button.setText(t("multipoint.stop"))
|
||||
self.table.setHorizontalHeaderLabels([
|
||||
t("multipoint.col_id"), t("multipoint.col_x"), t("multipoint.col_y"),
|
||||
t("multipoint.col_z"), t("multipoint.col_levels"), t("multipoint.col_hold"),
|
||||
t("multipoint.col_repeats"), t("multipoint.col_done"),
|
||||
])
|
||||
self._refresh()
|
||||
|
||||
def _next_site_id(self) -> str:
|
||||
return next_point_id([site.site_id for site in self.campaign.sites])
|
||||
|
||||
def _levels(self) -> list[float]:
|
||||
return [float(part.strip()) for part in self.host.levels.text().split(",") if part.strip()]
|
||||
|
||||
def _hold(self) -> float:
|
||||
return float(self.host.hold.value())
|
||||
|
||||
def _solve_uv(self, u: float, v: float):
|
||||
"""Pose for the point currently chosen on 选择测点. Does not move the platform."""
|
||||
flat = getattr(self.host, "_flat", None)
|
||||
if flat is None:
|
||||
raise ValueError(t("multipoint.no_map"))
|
||||
site = site_at(flat, int(round(u)), int(round(v)), origin=getattr(self.host, "_origin", None))
|
||||
sample = flat.sample(*site.raster_xy_px)
|
||||
if sample is None:
|
||||
raise ValueError(t("multipoint.mesh_required"))
|
||||
point, normal, _face = sample
|
||||
finger = self._finger
|
||||
stack = None if finger is None else getattr(finger, "stack", None)
|
||||
if stack is None:
|
||||
raise ValueError(t("multipoint.no_solver"))
|
||||
solved = stack.solve(
|
||||
np.asarray(point, dtype=np.float64),
|
||||
np.asarray(normal, dtype=np.float64),
|
||||
)
|
||||
length = float(np.linalg.norm(normal))
|
||||
unit = tuple(float(value) / length for value in normal) if length >= 1e-9 else tuple(
|
||||
float(value) for value in normal
|
||||
)
|
||||
pose = {
|
||||
"x": float(solved.table_xy_mm[0]),
|
||||
"y": float(solved.table_xy_mm[1]),
|
||||
"yaw": float(solved.yaw_deg),
|
||||
"pitch": float(solved.pitch_deg),
|
||||
"z": float(solved.contact_z_mm),
|
||||
"residual_deg": float(solved.residual_deg),
|
||||
"upright": bool(solved.upright),
|
||||
"in_range": bool(solved.ok and solved.in_range),
|
||||
"ok": bool(solved.ok),
|
||||
"normal_aligned_to": f"tool_axis:{stack.tool_axis}",
|
||||
"reason": str(solved.reason or ""),
|
||||
}
|
||||
return (
|
||||
(float(point[0]), float(point[1]), float(point[2])),
|
||||
unit,
|
||||
pose,
|
||||
(float(site.u_px), float(site.v_px)),
|
||||
)
|
||||
|
||||
def _solve_current(self):
|
||||
return self._solve_uv(float(self.host.u_spin.value()), float(self.host.v_spin.value()))
|
||||
|
||||
def _store_pose(self, site_id: str, point, normal, pose, uv) -> None:
|
||||
site = next(item for item in self.campaign.sites if item.site_id == site_id)
|
||||
site.x, site.y, site.z = point
|
||||
site.normal = normal
|
||||
site.pose = pose
|
||||
site.uv_px = uv
|
||||
site.position_frame = "sensor"
|
||||
|
||||
def _max_force(self) -> float:
|
||||
text = self.max_force.text().strip()
|
||||
if not text:
|
||||
raise ValueError(t("multipoint.force_required"))
|
||||
return float(text)
|
||||
|
||||
def _min_force(self) -> float:
|
||||
text = self.min_force.text().strip()
|
||||
return 0.0 if not text else float(text)
|
||||
|
||||
def _add(self) -> None:
|
||||
def go():
|
||||
if not self.site_id.text().strip():
|
||||
self.site_id.setText(self._next_site_id())
|
||||
point, normal, pose, uv = self._solve_current()
|
||||
self.campaign.add_solved_site(
|
||||
self.site_id.text(), point[0], point[1], point[2], normal, pose,
|
||||
self._levels(), self._hold(), self.repeats.value(),
|
||||
max_force_n=self._max_force(), min_force_n=self._min_force(), uv_px=uv,
|
||||
)
|
||||
self.table.clearSelection()
|
||||
self._selected = ""
|
||||
self.site_id.setText(self._next_site_id())
|
||||
self._run(go)
|
||||
|
||||
def _update(self) -> None:
|
||||
if not self._selected:
|
||||
return
|
||||
def go():
|
||||
point, normal, pose, uv = self._solve_current()
|
||||
new_id = self.site_id.text().strip()
|
||||
self.campaign.update_site(
|
||||
self._selected, point[0], point[1], point[2], self._levels(), self._hold(),
|
||||
self.repeats.value(), max_force_n=self._max_force(), new_id=new_id,
|
||||
)
|
||||
self._store_pose(new_id or self._selected, point, normal, pose, uv)
|
||||
self._run(go)
|
||||
|
||||
def _remove(self) -> None:
|
||||
if self._selected:
|
||||
self._run(lambda: self.campaign.remove_site(self._selected))
|
||||
|
||||
def _move(self, delta: int) -> None:
|
||||
if self._selected:
|
||||
self._run(lambda: self.campaign.move_site(self._selected, delta))
|
||||
|
||||
def _begin(self, *, simulate: bool) -> None:
|
||||
def go():
|
||||
if self.host.trial.status in {"recording", "closing"}:
|
||||
raise RuntimeError(t("multipoint.finger_busy"))
|
||||
if not simulate:
|
||||
worker = self.host.source_panel.worker
|
||||
packet = worker.latest()
|
||||
mode = getattr(self.host.source_panel, "mode", None)
|
||||
if not worker.is_running or packet is None or mode is None:
|
||||
raise RuntimeError(t("multipoint.camera_required"))
|
||||
directory = self.campaign.start(
|
||||
self.host.output_path.text().strip(),
|
||||
sensor_model=self.host.sensor_model.currentText(),
|
||||
sensor_sn=self.host.sensor_sn.text(),
|
||||
coordinate_frame=self.frame.text().strip() or "sensor",
|
||||
max_force_n=self._max_force(),
|
||||
min_force_n=self._min_force(),
|
||||
simulate=simulate,
|
||||
)
|
||||
if not simulate:
|
||||
self.campaign.attach_capture(
|
||||
self.host.source_panel.worker,
|
||||
fps=float(self.host.source_panel.mode.fps),
|
||||
)
|
||||
self.message.setText(str(directory))
|
||||
self._run(go)
|
||||
|
||||
def _confirm(self) -> None:
|
||||
if self.source.currentData() == "wrench":
|
||||
parts = [part for part in self.rotation.text().replace(",", " ").split() if part]
|
||||
self._run(lambda: (
|
||||
self.campaign.set_force_rotation([float(part) for part in parts]),
|
||||
self.campaign.confirm_wrench(
|
||||
float(self.fx_edit.text()), float(self.fy_edit.text()), float(self.fz_edit.text()),
|
||||
),
|
||||
))
|
||||
return
|
||||
self._run(lambda: self.campaign.confirm_reference(float(self.host.reference.text())))
|
||||
|
||||
def confirm_loaded(self) -> None:
|
||||
"""The right-hand confirm button records the current multi-point step."""
|
||||
self._confirm()
|
||||
|
||||
def _import_finger(self) -> None:
|
||||
def go():
|
||||
if self._finger is None or not hasattr(self._finger, "acquisition_points"):
|
||||
raise RuntimeError(t("multipoint.no_finger"))
|
||||
points = self._finger.acquisition_points()
|
||||
if not points:
|
||||
raise RuntimeError(t("multipoint.no_finger_points"))
|
||||
force = self._max_force()
|
||||
lower = self._min_force()
|
||||
for point in points:
|
||||
uv = point.get("u_px"), point.get("v_px")
|
||||
self.campaign.add_solved_site(
|
||||
point["site_id"], point["x"], point["y"], point["z"],
|
||||
(point["nx"], point["ny"], point["nz"]), point["pose"],
|
||||
self._levels(), self._hold(), self.repeats.value(),
|
||||
max_force_n=force, min_force_n=lower,
|
||||
uv_px=None if uv[0] is None else (float(uv[0]), float(uv[1])),
|
||||
)
|
||||
self._run(go)
|
||||
|
||||
def _align(self) -> None:
|
||||
if not self._selected:
|
||||
self.message.setText(t("multipoint.align_select"))
|
||||
return
|
||||
def go():
|
||||
site = next(item for item in self.campaign.sites if item.site_id == self._selected)
|
||||
pose = site.pose
|
||||
if pose is None or not pose.get("in_range"):
|
||||
if site.uv_px is None:
|
||||
raise ValueError(t("multipoint.no_pose"))
|
||||
point, normal, pose, uv = self._solve_uv(*site.uv_px)
|
||||
self._store_pose(site.site_id, point, normal, pose, uv)
|
||||
if not pose.get("ok") or not pose.get("in_range"):
|
||||
raise ValueError(t("multipoint.pose_refused").format(reason=pose.get("reason") or ""))
|
||||
self.alignRequested.emit(self.campaign.command_targets(self._selected))
|
||||
self._run(go)
|
||||
|
||||
def _reacquire(self) -> None:
|
||||
if self._selected:
|
||||
self._run(lambda: self.campaign.reacquire(self._selected))
|
||||
|
||||
def _liftoff(self) -> None:
|
||||
self._run(self.campaign.confirm_liftoff)
|
||||
|
||||
def _stop(self) -> None:
|
||||
self.campaign.stop(t("multipoint.stopped_reason"))
|
||||
self._refresh()
|
||||
|
||||
def _poll(self) -> None:
|
||||
if self.campaign.phase == "hold" and self.campaign.remaining_s() <= 0:
|
||||
try:
|
||||
self.campaign.complete_hold()
|
||||
except (OSError, RuntimeError, ValueError) as exc:
|
||||
self.message.setText(t("calibration.error").format(detail=exc))
|
||||
if self.campaign.is_active:
|
||||
self.campaign.stop(str(exc))
|
||||
self._refresh()
|
||||
|
||||
def _run(self, action) -> None:
|
||||
try:
|
||||
action()
|
||||
except (OSError, RuntimeError, ValueError) as exc:
|
||||
self.message.setText(t("calibration.error").format(detail=exc))
|
||||
self._refresh()
|
||||
|
||||
def _on_select(self) -> None:
|
||||
rows = self.table.selectionModel().selectedRows()
|
||||
if not rows:
|
||||
return
|
||||
site = self.campaign.sites[rows[0].row()]
|
||||
self._selected = site.site_id
|
||||
self.site_id.setText(site.site_id)
|
||||
self.repeats.setValue(site.repeats)
|
||||
if site.uv_px is not None:
|
||||
self.host.u_spin.setValue(int(round(site.uv_px[0])))
|
||||
self.host.v_spin.setValue(int(round(site.uv_px[1])))
|
||||
|
||||
def _refresh(self) -> None:
|
||||
finished = {item["site_id"]: item for item in self.campaign.completion()}
|
||||
listed = tuple(
|
||||
(
|
||||
site.site_id, site.x, site.y, site.z, site.levels_n, site.hold_s, site.repeats,
|
||||
finished[site.site_id]["repeats_complete"],
|
||||
)
|
||||
for site in self.campaign.sites
|
||||
)
|
||||
if listed != self._listed:
|
||||
self._listed = listed
|
||||
self.table.setRowCount(len(self.campaign.sites))
|
||||
for row, site in enumerate(self.campaign.sites):
|
||||
item = finished[site.site_id]
|
||||
values = [
|
||||
site.site_id, f"{site.x:g}", f"{site.y:g}", f"{site.z:g}",
|
||||
", ".join(f"{value:g}" for value in site.levels_n),
|
||||
f"{site.hold_s:g}", str(site.repeats),
|
||||
f"{item['repeats_complete']}/{item['repeats_planned']}",
|
||||
]
|
||||
for column, value in enumerate(values):
|
||||
self.table.setItem(row, column, QTableWidgetItem(value))
|
||||
active = self.campaign.is_active
|
||||
wrench = self.source.currentData() == "wrench"
|
||||
self.wrench_box.setVisible(wrench)
|
||||
for widget in (
|
||||
self.add_button, self.update_button, self.remove_button, self.up_button,
|
||||
self.down_button, self.import_button, self.simulate_button, self.start_button,
|
||||
self.site_id, self.repeats, self.frame, self.max_force, self.min_force, self.source,
|
||||
):
|
||||
widget.setEnabled(not active)
|
||||
for widget in (self.fx_edit, self.fy_edit, self.fz_edit, self.rotation):
|
||||
widget.setEnabled(self.campaign.phase == "load" and wrench)
|
||||
self.liftoff_button.setEnabled(self.campaign.phase == "liftoff")
|
||||
self.stop_button.setEnabled(active)
|
||||
self.reacquire_button.setEnabled(
|
||||
not active and bool(self._selected) and self.campaign.directory is not None
|
||||
)
|
||||
state = self.campaign.progress()
|
||||
next_id = state["site_id"] if self.campaign.phase == "liftoff" else ""
|
||||
if next_id and next_id != self._offered_liftoff:
|
||||
self._offered_liftoff = next_id
|
||||
for row, site in enumerate(self.campaign.sites):
|
||||
if site.site_id == next_id:
|
||||
self.table.selectRow(row)
|
||||
break
|
||||
elif self.campaign.phase != "liftoff":
|
||||
self._offered_liftoff = ""
|
||||
self.align_button.setEnabled(self.campaign.phase != "hold" and bool(self._selected))
|
||||
branch = {
|
||||
"baseline": t("multipoint.branch_baseline"),
|
||||
"load": t("multipoint.branch_load"),
|
||||
"unload": t("multipoint.branch_unload"),
|
||||
}.get(state["branch"], state["branch"])
|
||||
if self.campaign.phase == "liftoff":
|
||||
self.progress.setText(t("multipoint.liftoff_wait").format(
|
||||
site=state["site_id"], done=state["done"], total=state["total"],
|
||||
))
|
||||
elif state["phase"] in {"load", "hold"}:
|
||||
self.progress.setText(t("multipoint.progress").format(
|
||||
site=state["site_id"], site_index=state["site_index"],
|
||||
site_count=state["site_count"], branch=branch,
|
||||
load=state["target_n"], repeat=state["repeat_index"],
|
||||
repeats=state["repeats"], done=state["done"], total=state["total"],
|
||||
seconds=self.campaign.remaining_s(),
|
||||
))
|
||||
else:
|
||||
self.progress.setText(t("multipoint.idle"))
|
||||
shown = next(
|
||||
(site for site in self.campaign.sites if site.site_id == (state["site_id"] or self._selected)),
|
||||
None,
|
||||
)
|
||||
pose = None if shown is None else shown.pose
|
||||
if pose:
|
||||
text = t("multipoint.pose").format(
|
||||
x=pose["x"], y=pose["y"], yaw=pose["yaw"], pitch=pose["pitch"],
|
||||
z=pose["z"], residual=float(pose.get("residual_deg") or 0.0),
|
||||
)
|
||||
if not pose.get("in_range"):
|
||||
text += "\n" + t("multipoint.pose_refused").format(reason=pose.get("reason") or "")
|
||||
self.pose_status.setText(text)
|
||||
else:
|
||||
self.pose_status.setText(t("multipoint.no_pose"))
|
||||
if self.campaign.phase == "done":
|
||||
save = t("multipoint.save_done")
|
||||
elif self.campaign.phase == "aborted":
|
||||
save = t("multipoint.save_kept")
|
||||
elif active:
|
||||
save = t("multipoint.save_writing")
|
||||
else:
|
||||
save = t("multipoint.save_idle")
|
||||
self.save_status.setText(t("multipoint.save").format(status=save, path=state["directory"] or "—"))
|
||||
@@ -814,6 +814,43 @@ class FingerPanel(QGroupBox):
|
||||
"""Every picked point, at full precision, for a later export."""
|
||||
return [dict(row) for row in self._picked]
|
||||
|
||||
def acquisition_points(self) -> list[dict]:
|
||||
"""Picked points with a freshly solved pose for 02 acquisition.
|
||||
|
||||
The pose is the same ``PlatformStack.solve`` result the execute button uses.
|
||||
Calling this does not move the platform.
|
||||
"""
|
||||
points = []
|
||||
for index, record in enumerate(self._picked, 1):
|
||||
point = np.array([record["x"], record["y"], record["z"]], dtype=np.float64)
|
||||
normal = np.array([record["nx"], record["ny"], record["nz"]], dtype=np.float64)
|
||||
pose = self._stack.solve(point, normal)
|
||||
points.append({
|
||||
"site_id": f"site-{index:03d}",
|
||||
"x": float(point[0]),
|
||||
"y": float(point[1]),
|
||||
"z": float(point[2]),
|
||||
"nx": float(normal[0]),
|
||||
"ny": float(normal[1]),
|
||||
"nz": float(normal[2]),
|
||||
"u_px": record.get("u_px"),
|
||||
"v_px": record.get("v_px"),
|
||||
"pose": {
|
||||
"x": float(pose.table_xy_mm[0]),
|
||||
"y": float(pose.table_xy_mm[1]),
|
||||
"yaw": float(pose.yaw_deg),
|
||||
"pitch": float(pose.pitch_deg),
|
||||
"z": float(pose.contact_z_mm),
|
||||
"residual_deg": float(pose.residual_deg),
|
||||
"upright": bool(pose.upright),
|
||||
"in_range": bool(pose.ok and pose.in_range),
|
||||
"ok": bool(pose.ok),
|
||||
"normal_aligned_to": f"tool_axis:{self._stack.tool_axis}",
|
||||
"reason": pose.reason,
|
||||
},
|
||||
})
|
||||
return points
|
||||
|
||||
def export_sites(self, path: str | Path) -> Path:
|
||||
"""Export the displayed order, with the same UV convention as calibration."""
|
||||
flat = self.view.flat
|
||||
|
||||
@@ -369,6 +369,8 @@ class AutoCalMainWindow(QMainWindow):
|
||||
self.finger_panel.executeRequested.connect(self._on_execute_pose)
|
||||
self.finger_panel.stopRequested.connect(self._on_stop_all)
|
||||
self.finger_panel.confirmChanged.connect(self._on_confirm_moves_changed)
|
||||
self.calibration_panel.multipoint.bind_finger(self.finger_panel)
|
||||
self.calibration_panel.multipoint.alignRequested.connect(self._on_align_multipoint)
|
||||
self.run_bar.runRequested.connect(self._on_run_requested)
|
||||
self.run_bar.pauseRequested.connect(self._on_run_pause)
|
||||
self.run_bar.abortRequested.connect(self._on_run_abort)
|
||||
@@ -1086,17 +1088,37 @@ class AutoCalMainWindow(QMainWindow):
|
||||
)
|
||||
self._start_move(plan, t("move.execute"))
|
||||
|
||||
def _on_align_multipoint(self, targets: dict) -> None:
|
||||
"""Same five-axis move as the finger screen, for the point chosen on 选择测点."""
|
||||
plan = plan_pose_move(
|
||||
self.poller.service,
|
||||
targets,
|
||||
title=t("multipoint.align"),
|
||||
hold=(),
|
||||
together=True,
|
||||
)
|
||||
self._start_move(
|
||||
plan,
|
||||
t("multipoint.align"),
|
||||
report=self.calibration_panel.multipoint.message.setText,
|
||||
)
|
||||
|
||||
def _on_return_to_initial(self) -> None:
|
||||
"""Yaw, pitch, table X/Y and the press axis back to zero together."""
|
||||
plan = plan_return_to_initial(self.poller.service)
|
||||
self._start_move(plan, t("move.return_initial"))
|
||||
|
||||
def _start_move(self, plan, title: str) -> None:
|
||||
def _start_move(self, plan, title: str, *, report=None) -> None:
|
||||
def say(text: str) -> None:
|
||||
if report is not None:
|
||||
report(text)
|
||||
|
||||
service = self.poller.service
|
||||
refusal = service.check_axes_move(
|
||||
{step.role: step.target for step in plan.steps}
|
||||
)
|
||||
if refusal is not None:
|
||||
say(refusal.message)
|
||||
self._refuse_move(refusal.message)
|
||||
return
|
||||
# Confirm before the platform moves, with the numbers rather than a summary of
|
||||
@@ -1131,10 +1153,13 @@ class AutoCalMainWindow(QMainWindow):
|
||||
QMessageBox.StandardButton.No,
|
||||
)
|
||||
if answer != QMessageBox.StandardButton.Yes:
|
||||
say(t("move.cancelled"))
|
||||
self.finger_panel.set_move_status(t("move.cancelled"))
|
||||
return
|
||||
if not self.mover.start(plan).ok:
|
||||
self._refuse_move(self.mover.state.failure or t("move.busy"))
|
||||
reason = self.mover.state.failure or t("move.busy")
|
||||
say(reason)
|
||||
self._refuse_move(reason)
|
||||
return
|
||||
self.finger_panel.set_move_status(plan.describe())
|
||||
self.status_strip.set_action_result(f"{title}: {plan.describe()}", True)
|
||||
|
||||
Reference in New Issue
Block a user