Merge branch 'main' of ssh://892887cvmk08.aicp.io:24928/orisys-tech/orisys-app
This commit is contained in:
@@ -311,6 +311,76 @@
|
|||||||
"path": "Trial path (N): {path}",
|
"path": "Trial path (N): {path}",
|
||||||
"invalid_levels": "Enter strictly increasing levels greater than 0 and at most 30 N"
|
"invalid_levels": "Enter strictly increasing levels greater than 0 and at most 30 N"
|
||||||
},
|
},
|
||||||
|
"multipoint": {
|
||||||
|
"tab": "02 multi-point",
|
||||||
|
"hint": "A point comes from the position chosen on Select point. Adding it solves the platform pose so the surface normal faces the tool. Align normal asks before moving the five axes. If the controller is not connected, nothing moves and the reason is shown on this page. Simulation does not move the platform. A weight reference is typed. A six-axis reference stores Fx, Fy and Fz, then projects them with the rotation you enter. One raw axis is not the normal. Confirm the indenter has left before the next point.",
|
||||||
|
"site_id": "Point id",
|
||||||
|
"coordinates": "Position X / Y / Z",
|
||||||
|
"levels": "Load levels for this point (N), excluding baseline",
|
||||||
|
"hold": "Hold time",
|
||||||
|
"repeats": "Repeats",
|
||||||
|
"frame": "Coordinate frame",
|
||||||
|
"frame_placeholder": "Name the frame; it is stored and not converted",
|
||||||
|
"min_force": "Minimum force (N)",
|
||||||
|
"min_placeholder": "Blank means 0; the unloaded baseline is always recorded",
|
||||||
|
"max_force": "Maximum force (N)",
|
||||||
|
"force_placeholder": "Required; nothing is assumed",
|
||||||
|
"force_required": "Enter the maximum force; it is not assumed",
|
||||||
|
"add": "Add point",
|
||||||
|
"update": "Update selected",
|
||||||
|
"remove": "Delete selected",
|
||||||
|
"up": "Move up",
|
||||||
|
"down": "Move down",
|
||||||
|
"import_points": "Import from finger debug",
|
||||||
|
"align": "Align normal",
|
||||||
|
"reacquire": "Record selected again",
|
||||||
|
"source": "Reference source",
|
||||||
|
"source_weight": "Weights",
|
||||||
|
"source_wrench": "Six-axis",
|
||||||
|
"fx": "Fx",
|
||||||
|
"fy": "Fy",
|
||||||
|
"fz": "Fz",
|
||||||
|
"rotation": "Force sensor to tool rotation",
|
||||||
|
"rotation_placeholder": "9 numbers, row major. Required before a normal component can be computed",
|
||||||
|
"reference": "Measured reference (N)",
|
||||||
|
"simulate": "Simulate acquisition",
|
||||||
|
"start": "Start acquisition",
|
||||||
|
"confirm": "At target force, start hold",
|
||||||
|
"liftoff": "Confirm the indenter has left the sensor",
|
||||||
|
"stop": "Stop and keep data",
|
||||||
|
"stopped_reason": "operator stopped",
|
||||||
|
"finger_busy": "A single-point trial is running; finish it before multi-point acquisition",
|
||||||
|
"camera_required": "Start the camera or video input before live acquisition. Use simulation when there is no image; simulation does not open a camera or move the platform.",
|
||||||
|
"liftoff_wait": "The previous point is unloaded. Next point is {site}. Lift the indenter, click Align normal, and confirm before the platform moves so this normal faces up. Then click Confirm the indenter has left the sensor. Progress {done}/{total}",
|
||||||
|
"progress": "Point {site} ({site_index}/{site_count}) · {branch} {load:g} N · repeat {repeat}/{repeats} · progress {done}/{total} · {seconds:.1f} s left",
|
||||||
|
"idle": "Not started. Pick a position on Select point, add it, then simulate or start.",
|
||||||
|
"pose": "Solved pose: table X {x:.3f} mm, Y {y:.3f} mm, yaw {yaw:.3f}°, pitch {pitch:.3f}°, Z {z:.3f} mm, residual {residual:.3f}°. Align normal asks before the platform moves.",
|
||||||
|
"no_pose": "This point has no normal. Pick a valid position on Select point, then click Add point or Update selected.",
|
||||||
|
"no_map": "Load a sensor bundle so Select point shows the surface.",
|
||||||
|
"no_solver": "The pose solver is not connected.",
|
||||||
|
"align_select": "Select the destination point in the table, then click Align normal.",
|
||||||
|
"pose_refused": "This pose cannot be commanded: {reason}",
|
||||||
|
"no_finger": "The finger page is not connected.",
|
||||||
|
"no_finger_points": "The finger page has no picked points.",
|
||||||
|
"branch_baseline": "baseline",
|
||||||
|
"branch_load": "load",
|
||||||
|
"branch_unload": "unload",
|
||||||
|
"mesh_required": "Pick the point with U/V on the right so its mesh coordinate is shown, then add it.",
|
||||||
|
"right_confirm": "This step uses the right side: type the weight in Manual reference. For a six-axis reading, type Fx, Fy and Fz on the left. Then click At target force, start hold.",
|
||||||
|
"save": "Save status: {status}\n{path}",
|
||||||
|
"save_idle": "Nothing saved yet",
|
||||||
|
"save_writing": "Writing",
|
||||||
|
"save_kept": "Stopped; data already collected is kept",
|
||||||
|
"save_done": "Session complete",
|
||||||
|
"col_id": "Id",
|
||||||
|
"col_x": "X",
|
||||||
|
"col_y": "Y",
|
||||||
|
"col_z": "Z",
|
||||||
|
"col_levels": "Levels (N)",
|
||||||
|
"col_hold": "Hold (s)",
|
||||||
|
"col_repeats": "Repeats",
|
||||||
|
"col_done": "Done"
|
||||||
|
},
|
||||||
"finger": {
|
"finger": {
|
||||||
"title": "Finger calibration-point debug",
|
"title": "Finger calibration-point debug",
|
||||||
"bundle_path": "Export bundle",
|
"bundle_path": "Export bundle",
|
||||||
@@ -335,7 +405,7 @@
|
|||||||
"row_residual": "Alignment residual",
|
"row_residual": "Alignment residual",
|
||||||
"row_frames": "Frame mapping",
|
"row_frames": "Frame mapping",
|
||||||
"tool_xy": "Tool X / Y",
|
"tool_xy": "Tool X / Y",
|
||||||
"tool_hint": "Where the tool is in the table frame, straight above the table origin by default. The table move is what carries the contact to this point. Measure it on the real setup.",
|
"tool_hint": "Where the tool is in the table frame. With Y at 3 mm, every point's table Y is the solved move plus 3 mm, so the sensor shell tail stays on the platform centre axis.",
|
||||||
"sensor_xy": "Sensor X / Y",
|
"sensor_xy": "Sensor X / Y",
|
||||||
"sensor_zy": "Sensor Z / yaw",
|
"sensor_zy": "Sensor Z / yaw",
|
||||||
"setup_hint": "How the sensor actually sits on the stack with the machine at zero: X/Y/Z translation and a yaw about Z. Z is the mounting plate, shims included — there is no separate plate field. Measure it on the real setup.",
|
"setup_hint": "How the sensor actually sits on the stack with the machine at zero: X/Y/Z translation and a yaw about Z. Z is the mounting plate, shims included — there is no separate plate field. Measure it on the real setup.",
|
||||||
|
|||||||
@@ -311,6 +311,76 @@
|
|||||||
"path": "本次路径 (N):{path}",
|
"path": "本次路径 (N):{path}",
|
||||||
"invalid_levels": "请输入严格递增的加载档位,每档大于 0 且不超过 30 N"
|
"invalid_levels": "请输入严格递增的加载档位,每档大于 0 且不超过 30 N"
|
||||||
},
|
},
|
||||||
|
"multipoint": {
|
||||||
|
"tab": "02 多点采集",
|
||||||
|
"hint": "测点直接用标定采集「选择测点」里点中的位置。添加时会用法向解出平台姿态,使该点法向朝上。点「对齐法向」会先询问再移动五个轴;未连接控制器时不会动,原因写在本页下方。模拟采集不移动平台。砝码参考由人工填写。六维力参考记录三个分量,再按你填写的旋转投影到刀具轴,不会把某一轴直接当成法向。换点前确认压头已脱离。",
|
||||||
|
"site_id": "测点编号",
|
||||||
|
"coordinates": "位置 X / Y / Z",
|
||||||
|
"levels": "该点加载档位 (N),不含空载",
|
||||||
|
"hold": "保持时间",
|
||||||
|
"repeats": "重复次数",
|
||||||
|
"frame": "坐标所在坐标系",
|
||||||
|
"frame_placeholder": "填写坐标系名称,程序不解释、不换算",
|
||||||
|
"min_force": "最小力 (N)",
|
||||||
|
"min_placeholder": "空着表示 0;空载基线总会记录",
|
||||||
|
"max_force": "最大力 (N)",
|
||||||
|
"force_placeholder": "必须填写,程序不预设",
|
||||||
|
"force_required": "请填写最大力,程序不会替你假设",
|
||||||
|
"add": "添加测点",
|
||||||
|
"update": "更新所选",
|
||||||
|
"remove": "删除所选",
|
||||||
|
"up": "上移",
|
||||||
|
"down": "下移",
|
||||||
|
"import_points": "从手指调试导入",
|
||||||
|
"align": "对齐法向",
|
||||||
|
"reacquire": "补采所选",
|
||||||
|
"source": "参考力来源",
|
||||||
|
"source_weight": "砝码",
|
||||||
|
"source_wrench": "六维力",
|
||||||
|
"fx": "Fx",
|
||||||
|
"fy": "Fy",
|
||||||
|
"fz": "Fz",
|
||||||
|
"rotation": "力传感器到刀具的旋转",
|
||||||
|
"rotation_placeholder": "9 个数,行优先。不填不能计算法向力",
|
||||||
|
"reference": "实测参考力 (N)",
|
||||||
|
"simulate": "模拟采集",
|
||||||
|
"start": "开始采集",
|
||||||
|
"confirm": "已到目标力,开始保持",
|
||||||
|
"liftoff": "确认压头已脱离传感器",
|
||||||
|
"stop": "停止并保留数据",
|
||||||
|
"stopped_reason": "操作员停止",
|
||||||
|
"finger_busy": "单点试验进行中,请先结束再开始多点采集",
|
||||||
|
"camera_required": "开始采集前请先启动相机或视频输入。没有图像时用模拟采集,模拟不会打开相机,也不会移动平台。",
|
||||||
|
"liftoff_wait": "上一点已卸载。下一点是 {site}。先抬起接触头,再点「对齐法向」并确认,平台才移到这个点、法向朝上。到位后再点「确认压头已脱离传感器」。程序不会自动移动。总进度 {done}/{total}",
|
||||||
|
"progress": "测点 {site}({site_index}/{site_count})· {branch} {load:g} N · 重复 {repeat}/{repeats} · 总进度 {done}/{total} · 剩余 {seconds:.1f} s",
|
||||||
|
"idle": "尚未开始。先在「选择测点」里点一个位置,再添加测点,然后模拟采集或开始采集。",
|
||||||
|
"pose": "求解姿态:工作台 X {x:.3f} mm,Y {y:.3f} mm,yaw {yaw:.3f}°,pitch {pitch:.3f}°,Z {z:.3f} mm,残差 {residual:.3f}°。点「对齐法向」后才会询问并移动平台。",
|
||||||
|
"no_pose": "这个测点还没有法向。请在「选择测点」里点一个有效位置,再点「添加测点」或「更新所选」。",
|
||||||
|
"no_map": "请先载入传感器导出包,让「选择测点」出现曲面。",
|
||||||
|
"no_solver": "姿态求解还没有接上。",
|
||||||
|
"align_select": "请先在左边表里点中要去的测点,再点「对齐法向」。",
|
||||||
|
"pose_refused": "这个点的姿态不能执行:{reason}",
|
||||||
|
"no_finger": "手指调试页面还没有接上。",
|
||||||
|
"no_finger_points": "手指调试里还没有选点。",
|
||||||
|
"branch_baseline": "空载",
|
||||||
|
"branch_load": "加载",
|
||||||
|
"branch_unload": "卸载",
|
||||||
|
"mesh_required": "请先在右侧用 U/V 选出测点,出现网格坐标后再添加。",
|
||||||
|
"right_confirm": "这一档用右侧:砝码填写人工参考力;六维力填写左侧 Fx、Fy、Fz。然后点「已到目标力,开始保持」。",
|
||||||
|
"save": "保存状态:{status}\n{path}",
|
||||||
|
"save_idle": "尚未保存",
|
||||||
|
"save_writing": "正在写入",
|
||||||
|
"save_kept": "已停止,已经采集的数据保留在目录中",
|
||||||
|
"save_done": "本场已完成",
|
||||||
|
"col_id": "编号",
|
||||||
|
"col_x": "X",
|
||||||
|
"col_y": "Y",
|
||||||
|
"col_z": "Z",
|
||||||
|
"col_levels": "档位 (N)",
|
||||||
|
"col_hold": "保持 (s)",
|
||||||
|
"col_repeats": "重复",
|
||||||
|
"col_done": "完成"
|
||||||
|
},
|
||||||
"finger": {
|
"finger": {
|
||||||
"title": "手指标定点调试",
|
"title": "手指标定点调试",
|
||||||
"bundle_path": "导出包",
|
"bundle_path": "导出包",
|
||||||
@@ -335,7 +405,7 @@
|
|||||||
"row_residual": "对齐残差",
|
"row_residual": "对齐残差",
|
||||||
"row_frames": "坐标映射",
|
"row_frames": "坐标映射",
|
||||||
"tool_xy": "刀具 X / Y",
|
"tool_xy": "刀具 X / Y",
|
||||||
"tool_hint": "刀具在工作台坐标系中的位置,默认在原点正上方;工作台的移动量就是把接触点送到此处所需的位移。请按实际安装测量填写。",
|
"tool_hint": "刀具在工作台坐标系中的位置。Y 为 3 mm 时,每个测点的工作台 Y 都会在解算结果上再加 3 mm,使传感器壳体尾部与平台原中心同轴。",
|
||||||
"sensor_xy": "传感器 X / Y",
|
"sensor_xy": "传感器 X / Y",
|
||||||
"sensor_zy": "传感器 Z / yaw",
|
"sensor_zy": "传感器 Z / yaw",
|
||||||
"setup_hint": "传感器在转台零位时相对工作台原点的实际安装位姿:X/Y/Z 平移与绕 Z 的偏摆。Z 即安装板厚度(含垫片),无需另填。请按实际安装测量填写。",
|
"setup_hint": "传感器在转台零位时相对工作台原点的实际安装位姿:X/Y/Z 平移与绕 Z 的偏摆。Z 即安装板厚度(含垫片),无需另填。请按实际安装测量填写。",
|
||||||
|
|||||||
@@ -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
|
||||||
@@ -228,7 +228,7 @@ def default_preferences() -> dict[str, Any]:
|
|||||||
#: against it, so it is what makes the reported move match the bench. A tool
|
#: against it, so it is what makes the reported move match the bench. A tool
|
||||||
#: tip offset belongs here — it used to be a separate field, and having two
|
#: tip offset belongs here — it used to be a separate field, and having two
|
||||||
#: of them is what let the picture and the numbers disagree.
|
#: of them is what let the picture and the numbers disagree.
|
||||||
"finger_tool_xy_mm": [0.0, 0.0],
|
"finger_tool_xy_mm": [0.0, 3.0],
|
||||||
#: Stage travel, degrees. Confirmed at the bench: the vertical stage turns
|
#: Stage travel, degrees. Confirmed at the bench: the vertical stage turns
|
||||||
#: a full half-turn either way (±180), and the horizontal stage turns
|
#: a full half-turn either way (±180), and the horizontal stage turns
|
||||||
#: continuously. An earlier value of 90 was wrong and made half the patch
|
#: continuously. An earlier value of 90 was wrong and made half the patch
|
||||||
|
|||||||
@@ -118,6 +118,8 @@ def review_session(directory: Path, *, selections: dict | None = None) -> dict:
|
|||||||
"""
|
"""
|
||||||
root = Path(directory).resolve()
|
root = Path(directory).resolve()
|
||||||
manifest = json.loads((root / "session.json").read_text(encoding="utf-8"))
|
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":
|
if manifest.get("format_version") != "autocal-session-1.0":
|
||||||
raise ValueError("select a calibration session root, not an individual trial")
|
raise ValueError("select a calibration session root, not an individual trial")
|
||||||
windows = []
|
windows = []
|
||||||
|
|||||||
@@ -31,6 +31,7 @@ from ..sensor.uv_flat import site_at, uv_origin
|
|||||||
from ..sensor.uv_outline import outline_snapshot
|
from ..sensor.uv_outline import outline_snapshot
|
||||||
from ..trial import ManualTrial, load_path
|
from ..trial import ManualTrial, load_path
|
||||||
from .live_force_view import LiveForceView
|
from .live_force_view import LiveForceView
|
||||||
|
from .multipoint_panel import MultipointPanel
|
||||||
from .sensor_panel import FlatSurfaceView
|
from .sensor_panel import FlatSurfaceView
|
||||||
|
|
||||||
#: Product models a session can be recorded under. The stored value is this id.
|
#: 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)
|
site_layout.addWidget(self.bundle_button)
|
||||||
self.views.addTab(self.site_view, "")
|
self.views.addTab(self.site_view, "")
|
||||||
self.views.addTab(self.source_panel, "")
|
self.views.addTab(self.source_panel, "")
|
||||||
|
self.multipoint = MultipointPanel(self, self.views)
|
||||||
|
self.views.addTab(self.multipoint, "")
|
||||||
body.addWidget(self.views, 3)
|
body.addWidget(self.views, 3)
|
||||||
|
|
||||||
controls = QGroupBox(self)
|
controls = QGroupBox(self)
|
||||||
@@ -256,6 +259,8 @@ class CalibrationPanel(QWidget):
|
|||||||
|
|
||||||
def _start(self) -> None:
|
def _start(self) -> None:
|
||||||
try:
|
try:
|
||||||
|
if self.multipoint.is_active:
|
||||||
|
raise RuntimeError(t("multipoint.finger_busy"))
|
||||||
if self._site is None:
|
if self._site is None:
|
||||||
raise ValueError(t("calibration.invalid_site"))
|
raise ValueError(t("calibration.invalid_site"))
|
||||||
if not self.output_path.text().strip():
|
if not self.output_path.text().strip():
|
||||||
@@ -299,6 +304,9 @@ class CalibrationPanel(QWidget):
|
|||||||
self._sync()
|
self._sync()
|
||||||
|
|
||||||
def _confirm(self) -> None:
|
def _confirm(self) -> None:
|
||||||
|
if self.multipoint.campaign.phase == "load":
|
||||||
|
self.multipoint.confirm_loaded()
|
||||||
|
return
|
||||||
try:
|
try:
|
||||||
if not self.reference.text().strip():
|
if not self.reference.text().strip():
|
||||||
raise ValueError(t("calibration.reference_required"))
|
raise ValueError(t("calibration.reference_required"))
|
||||||
@@ -375,6 +383,7 @@ class CalibrationPanel(QWidget):
|
|||||||
try:
|
try:
|
||||||
if self.trial.status == "recording":
|
if self.trial.status == "recording":
|
||||||
self.trial.finish(aborted=True, reason="application closed")
|
self.trial.finish(aborted=True, reason="application closed")
|
||||||
|
self.multipoint.shutdown()
|
||||||
if self.trial.status == "closing" and self.trial.recorder is not None:
|
if self.trial.status == "closing" and self.trial.recorder is not None:
|
||||||
self.trial.recorder.stop(wait=True)
|
self.trial.recorder.stop(wait=True)
|
||||||
self.trial.poll()
|
self.trial.poll()
|
||||||
@@ -395,13 +404,19 @@ class CalibrationPanel(QWidget):
|
|||||||
self.bundle_button,
|
self.bundle_button,
|
||||||
):
|
):
|
||||||
widget.setEnabled(not locked)
|
widget.setEnabled(not locked)
|
||||||
|
campaign_busy = self.multipoint.is_active
|
||||||
for widget in (self.sensor_sn, self.sensor_model, self.output_path,
|
for widget in (self.sensor_sn, self.sensor_model, self.output_path,
|
||||||
self.output_browse, self.bundle_button):
|
self.output_browse, self.bundle_button):
|
||||||
widget.setEnabled(not locked and self.trial.session is 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)
|
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
|
can_confirm = recording and self.trial.next_step is not None and not self.trial.holding
|
||||||
self.confirm_button.setEnabled(can_confirm)
|
multipoint_load = self.multipoint.campaign.phase == "load"
|
||||||
self.reference.setEnabled(can_confirm)
|
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.finish_button.setEnabled(recording and self.trial.next_step is None)
|
||||||
self.abort_button.setEnabled(recording)
|
self.abort_button.setEnabled(recording)
|
||||||
step = self.trial.next_step
|
step = self.trial.next_step
|
||||||
@@ -417,6 +432,8 @@ class CalibrationPanel(QWidget):
|
|||||||
target_text = t("calibration.before_start")
|
target_text = t("calibration.before_start")
|
||||||
else:
|
else:
|
||||||
target_text = t("calibration.no_step")
|
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.target_label.setText(target_text)
|
||||||
self.progress_label.setText(
|
self.progress_label.setText(
|
||||||
t("calibration.progress").format(
|
t("calibration.progress").format(
|
||||||
@@ -466,6 +483,8 @@ class CalibrationPanel(QWidget):
|
|||||||
self.review_button.setText(t("review.open"))
|
self.review_button.setText(t("review.open"))
|
||||||
self.views.setTabText(0, t("calibration.site_view"))
|
self.views.setTabText(0, t("calibration.site_view"))
|
||||||
self.views.setTabText(1, t("calibration.live_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.bundle_button.setText(t("finger.load"))
|
||||||
self.coords_label.setText(t("calibration.uv"))
|
self.coords_label.setText(t("calibration.uv"))
|
||||||
self.position_label.setText(t("calibration.position"))
|
self.position_label.setText(t("calibration.position"))
|
||||||
|
|||||||
@@ -0,0 +1,568 @@
|
|||||||
|
"""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.align_button = QPushButton(page)
|
||||||
|
self.reacquire_button = QPushButton(page)
|
||||||
|
for button in (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.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.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 _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.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."""
|
"""Every picked point, at full precision, for a later export."""
|
||||||
return [dict(row) for row in self._picked]
|
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:
|
def export_sites(self, path: str | Path) -> Path:
|
||||||
"""Export the displayed order, with the same UV convention as calibration."""
|
"""Export the displayed order, with the same UV convention as calibration."""
|
||||||
flat = self.view.flat
|
flat = self.view.flat
|
||||||
|
|||||||
@@ -369,6 +369,8 @@ class AutoCalMainWindow(QMainWindow):
|
|||||||
self.finger_panel.executeRequested.connect(self._on_execute_pose)
|
self.finger_panel.executeRequested.connect(self._on_execute_pose)
|
||||||
self.finger_panel.stopRequested.connect(self._on_stop_all)
|
self.finger_panel.stopRequested.connect(self._on_stop_all)
|
||||||
self.finger_panel.confirmChanged.connect(self._on_confirm_moves_changed)
|
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.runRequested.connect(self._on_run_requested)
|
||||||
self.run_bar.pauseRequested.connect(self._on_run_pause)
|
self.run_bar.pauseRequested.connect(self._on_run_pause)
|
||||||
self.run_bar.abortRequested.connect(self._on_run_abort)
|
self.run_bar.abortRequested.connect(self._on_run_abort)
|
||||||
@@ -1086,17 +1088,37 @@ class AutoCalMainWindow(QMainWindow):
|
|||||||
)
|
)
|
||||||
self._start_move(plan, t("move.execute"))
|
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:
|
def _on_return_to_initial(self) -> None:
|
||||||
"""Yaw, pitch, table X/Y and the press axis back to zero together."""
|
"""Yaw, pitch, table X/Y and the press axis back to zero together."""
|
||||||
plan = plan_return_to_initial(self.poller.service)
|
plan = plan_return_to_initial(self.poller.service)
|
||||||
self._start_move(plan, t("move.return_initial"))
|
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
|
service = self.poller.service
|
||||||
refusal = service.check_axes_move(
|
refusal = service.check_axes_move(
|
||||||
{step.role: step.target for step in plan.steps}
|
{step.role: step.target for step in plan.steps}
|
||||||
)
|
)
|
||||||
if refusal is not None:
|
if refusal is not None:
|
||||||
|
say(refusal.message)
|
||||||
self._refuse_move(refusal.message)
|
self._refuse_move(refusal.message)
|
||||||
return
|
return
|
||||||
# Confirm before the platform moves, with the numbers rather than a summary of
|
# Confirm before the platform moves, with the numbers rather than a summary of
|
||||||
@@ -1131,10 +1153,13 @@ class AutoCalMainWindow(QMainWindow):
|
|||||||
QMessageBox.StandardButton.No,
|
QMessageBox.StandardButton.No,
|
||||||
)
|
)
|
||||||
if answer != QMessageBox.StandardButton.Yes:
|
if answer != QMessageBox.StandardButton.Yes:
|
||||||
|
say(t("move.cancelled"))
|
||||||
self.finger_panel.set_move_status(t("move.cancelled"))
|
self.finger_panel.set_move_status(t("move.cancelled"))
|
||||||
return
|
return
|
||||||
if not self.mover.start(plan).ok:
|
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
|
return
|
||||||
self.finger_panel.set_move_status(plan.describe())
|
self.finger_panel.set_move_status(plan.describe())
|
||||||
self.status_strip.set_action_result(f"{title}: {plan.describe()}", True)
|
self.status_strip.set_action_result(f"{title}: {plan.describe()}", True)
|
||||||
|
|||||||
@@ -0,0 +1,238 @@
|
|||||||
|
"""02 multi-point acquisition without a camera or a motion controller."""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import csv
|
||||||
|
import json
|
||||||
|
|
||||||
|
import pytest
|
||||||
|
|
||||||
|
from datetime import date
|
||||||
|
|
||||||
|
from orisys_autocal.multipoint import Campaign, next_point_id
|
||||||
|
|
||||||
|
|
||||||
|
def _campaign() -> Campaign:
|
||||||
|
campaign = Campaign()
|
||||||
|
campaign.add_site("A", 1, 2, 3, [5, 10], 1, 1, max_force_n=20)
|
||||||
|
campaign.add_site("B", 4, 5, 6, [5], 1, 2, max_force_n=20)
|
||||||
|
return campaign
|
||||||
|
|
||||||
|
|
||||||
|
def _finish_open_steps(campaign: Campaign) -> None:
|
||||||
|
site_index = campaign._site_index
|
||||||
|
repeat_index = campaign._repeat_index
|
||||||
|
while (
|
||||||
|
campaign.phase == "load"
|
||||||
|
and campaign._site_index == site_index
|
||||||
|
and campaign._repeat_index == repeat_index
|
||||||
|
):
|
||||||
|
_branch, target = campaign._steps[campaign._step_index]
|
||||||
|
campaign.confirm_reference(target)
|
||||||
|
campaign.complete_hold(now_ns=campaign._hold_started_ns + int(campaign._hold_s * 1e9))
|
||||||
|
|
||||||
|
|
||||||
|
def test_default_point_ids_use_the_date_and_increment():
|
||||||
|
today = date(2026, 10, 13)
|
||||||
|
assert next_point_id([], today) == "20261013point0"
|
||||||
|
assert next_point_id(["20261013point0"], today) == "20261013point1"
|
||||||
|
assert next_point_id(["20261013point0", "custom", "20261013point2"], today) == "20261013point3"
|
||||||
|
assert next_point_id(["20261012point4"], today) == "20261013point0"
|
||||||
|
|
||||||
|
|
||||||
|
def test_points_can_be_added_viewed_reordered_and_removed():
|
||||||
|
campaign = Campaign()
|
||||||
|
campaign.add_site("A", 0, 0, 1, [5], 10, 1, max_force_n=30)
|
||||||
|
campaign.add_site("B", 2, 0, 1, [5, 8], 12, 3, max_force_n=30)
|
||||||
|
assert [site.site_id for site in campaign.sites] == ["A", "B"]
|
||||||
|
assert campaign.sites[1].repeats == 3
|
||||||
|
campaign.move_site("B", -1)
|
||||||
|
assert [site.site_id for site in campaign.sites] == ["B", "A"]
|
||||||
|
campaign.remove_site("A")
|
||||||
|
assert [site.site_id for site in campaign.sites] == ["B"]
|
||||||
|
with pytest.raises(ValueError, match="unique"):
|
||||||
|
campaign.add_site("B", 0, 0, 0, [5], 10, 1, max_force_n=30)
|
||||||
|
|
||||||
|
|
||||||
|
def test_levels_above_the_entered_maximum_are_refused():
|
||||||
|
campaign = Campaign()
|
||||||
|
with pytest.raises(ValueError, match="force range"):
|
||||||
|
campaign.add_site("A", 0, 0, 0, [40], 10, 1, max_force_n=30)
|
||||||
|
|
||||||
|
|
||||||
|
def test_the_next_point_waits_until_the_indenter_is_confirmed_clear(tmp_path):
|
||||||
|
campaign = _campaign()
|
||||||
|
directory = campaign.start(
|
||||||
|
tmp_path, sensor_model="finger", sensor_sn="SN1",
|
||||||
|
coordinate_frame="fixture", max_force_n=20, simulate=True,
|
||||||
|
)
|
||||||
|
_finish_open_steps(campaign)
|
||||||
|
assert campaign.phase == "liftoff"
|
||||||
|
with pytest.raises(RuntimeError, match="left the sensor"):
|
||||||
|
campaign.confirm_reference(0)
|
||||||
|
campaign.confirm_liftoff()
|
||||||
|
assert campaign.phase == "load"
|
||||||
|
assert campaign.progress()["site_id"] == "B"
|
||||||
|
_finish_open_steps(campaign)
|
||||||
|
assert campaign.phase == "load"
|
||||||
|
assert campaign.progress()["repeat_index"] == 2
|
||||||
|
_finish_open_steps(campaign)
|
||||||
|
assert campaign.phase == "done"
|
||||||
|
|
||||||
|
session = json.loads((directory / "session.json").read_text(encoding="utf-8"))
|
||||||
|
assert session["status"] == "complete"
|
||||||
|
assert session["automatic_force_control"] is False
|
||||||
|
assert session["platform_motion"] is False
|
||||||
|
assert session["sync"]["association"].startswith("A frame belongs")
|
||||||
|
assert session["coordinate_frame"] == "fixture"
|
||||||
|
assert [site["site_id"] for site in session["sites"]] == ["A", "B"]
|
||||||
|
assert session["sites"][0]["position"] == {"x": 1.0, "y": 2.0, "z": 3.0}
|
||||||
|
assert all(trial["status"] == "complete" for trial in session["trials"])
|
||||||
|
assert directory.name.startswith("finger_SN1_")
|
||||||
|
from orisys_autocal.session_review import review_session
|
||||||
|
with pytest.raises(ValueError, match="finger sessions"):
|
||||||
|
review_session(directory)
|
||||||
|
|
||||||
|
first = directory / "sites" / "A" / "repeat-001" / "reference.csv"
|
||||||
|
rows = list(csv.DictReader(first.open(encoding="utf-8")))
|
||||||
|
assert [row["branch"] for row in rows] == ["baseline", "load", "load", "unload", "unload"]
|
||||||
|
assert rows[1]["site_id"] == "A"
|
||||||
|
assert [row["target_load_n"] for row in rows] == ["0.0", "5.0", "10.0", "5.0", "0.0"]
|
||||||
|
assert rows[2]["F_ref_normal_N"] == "10.0"
|
||||||
|
assert rows[2]["site_id"] == "A"
|
||||||
|
assert rows[0]["repeat_index"] == "1"
|
||||||
|
assert int(rows[0]["t_end_host_mono_ns"]) > int(rows[0]["t_start_host_mono_ns"])
|
||||||
|
events = (directory / "sites" / "A" / "repeat-001" / "events.csv").read_text(encoding="utf-8")
|
||||||
|
assert "liftoff_confirmed" not in events
|
||||||
|
later = (directory / "sites" / "B" / "repeat-001" / "events.csv").read_text(encoding="utf-8")
|
||||||
|
assert "trial_started" in later
|
||||||
|
session_events = (directory / "events.csv").read_text(encoding="utf-8")
|
||||||
|
assert "liftoff_required" in session_events
|
||||||
|
assert "liftoff_confirmed" in session_events
|
||||||
|
|
||||||
|
|
||||||
|
def test_stopping_keeps_the_points_already_recorded(tmp_path):
|
||||||
|
campaign = _campaign()
|
||||||
|
directory = campaign.start(
|
||||||
|
tmp_path, sensor_model="finger", sensor_sn="SN1",
|
||||||
|
coordinate_frame="fixture", max_force_n=20, simulate=True,
|
||||||
|
)
|
||||||
|
_finish_open_steps(campaign)
|
||||||
|
campaign.confirm_liftoff()
|
||||||
|
campaign.confirm_reference(0)
|
||||||
|
campaign.stop("operator stopped")
|
||||||
|
session = json.loads((directory / "session.json").read_text(encoding="utf-8"))
|
||||||
|
assert session["status"] == "incomplete"
|
||||||
|
assert session["trials"][0]["status"] == "complete"
|
||||||
|
assert session["trials"][1]["status"] == "incomplete"
|
||||||
|
kept = directory / "sites" / "A" / "repeat-001" / "reference.csv"
|
||||||
|
assert kept.is_file() and len(kept.read_text(encoding="utf-8").splitlines()) > 1
|
||||||
|
assert campaign.phase == "aborted"
|
||||||
|
|
||||||
|
|
||||||
|
def test_a_missing_frame_name_or_model_refuses_to_start(tmp_path):
|
||||||
|
campaign = _campaign()
|
||||||
|
with pytest.raises(ValueError, match="coordinate frame"):
|
||||||
|
campaign.start(
|
||||||
|
tmp_path, sensor_model="finger", sensor_sn="SN1",
|
||||||
|
coordinate_frame="", max_force_n=20, simulate=True,
|
||||||
|
)
|
||||||
|
with pytest.raises(ValueError, match="finger"):
|
||||||
|
campaign.start(
|
||||||
|
tmp_path, sensor_model="other", sensor_sn="SN1",
|
||||||
|
coordinate_frame="fixture", max_force_n=20, simulate=True,
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
_ROTATION = [0, 0, 1, 0, 1, 0, -1, 0, 0]
|
||||||
|
|
||||||
|
|
||||||
|
def test_six_axis_normal_uses_the_entered_rotation_not_one_axis(tmp_path):
|
||||||
|
campaign = Campaign()
|
||||||
|
campaign.add_site("A", 0, 0, 1, [5], 1, 1, max_force_n=20)
|
||||||
|
campaign.start(
|
||||||
|
tmp_path, sensor_model="finger", sensor_sn="SN1",
|
||||||
|
coordinate_frame="fixture", max_force_n=20, simulate=True,
|
||||||
|
)
|
||||||
|
with pytest.raises(ValueError, match="rotation"):
|
||||||
|
campaign.confirm_wrench(-5, 1, 9)
|
||||||
|
with pytest.raises(ValueError, match="determinant"):
|
||||||
|
campaign.set_force_rotation([1, 0, 0, 0, 1, 0, 0, 0, -1])
|
||||||
|
campaign.set_force_rotation(_ROTATION)
|
||||||
|
campaign.confirm_wrench(0, 0, 0)
|
||||||
|
campaign.complete_hold(now_ns=campaign._hold_started_ns + int(campaign._hold_s * 1e9))
|
||||||
|
campaign.confirm_wrench(-5, 1, 9)
|
||||||
|
campaign.complete_hold(now_ns=campaign._hold_started_ns + int(campaign._hold_s * 1e9))
|
||||||
|
campaign.confirm_wrench(0, 0, 0)
|
||||||
|
campaign.complete_hold(now_ns=campaign._hold_started_ns + int(campaign._hold_s * 1e9))
|
||||||
|
rows = list(csv.DictReader(
|
||||||
|
(campaign.directory / "sites" / "A" / "repeat-001" / "reference.csv").open(encoding="utf-8")
|
||||||
|
))
|
||||||
|
assert rows[1]["reference_source"] == "wrench"
|
||||||
|
assert rows[1]["branch"] == "load"
|
||||||
|
assert float(rows[1]["F_ref_normal_N"]) == pytest.approx(5.0)
|
||||||
|
assert float(rows[1]["fx_N"]) == pytest.approx(-5.0)
|
||||||
|
assert float(rows[1]["fy_N"]) == pytest.approx(1.0)
|
||||||
|
assert float(rows[1]["fz_N"]) == pytest.approx(9.0)
|
||||||
|
assert float(rows[1]["F_ref_normal_N"]) != pytest.approx(float(rows[1]["fx_N"]))
|
||||||
|
assert float(rows[1]["F_ref_normal_N"]) != pytest.approx(float(rows[1]["fy_N"]))
|
||||||
|
assert float(rows[1]["F_ref_normal_N"]) != pytest.approx(float(rows[1]["fz_N"]))
|
||||||
|
assert campaign._document["normal_projection"].startswith("F_tool = R @ F_sensor")
|
||||||
|
|
||||||
|
|
||||||
|
def test_solved_pose_is_stored_and_a_point_can_be_recorded_again(tmp_path):
|
||||||
|
from orisys_autocal.motion.kinematics import PlatformStack
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
stack = PlatformStack()
|
||||||
|
point = np.array([1.0, 2.0, 3.0])
|
||||||
|
normal = np.array([0.0, 1.0, 0.0])
|
||||||
|
pose = stack.solve(point, normal)
|
||||||
|
assert pose.ok
|
||||||
|
campaign = Campaign()
|
||||||
|
campaign.add_solved_site(
|
||||||
|
"P1", float(point[0]), float(point[1]), float(point[2]),
|
||||||
|
(float(normal[0]), float(normal[1]), float(normal[2])),
|
||||||
|
{
|
||||||
|
"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.in_range),
|
||||||
|
"ok": bool(pose.ok),
|
||||||
|
"normal_aligned_to": f"tool_axis:{stack.tool_axis}",
|
||||||
|
},
|
||||||
|
[4], 1, 1, max_force_n=10,
|
||||||
|
)
|
||||||
|
targets = campaign.command_targets("P1")
|
||||||
|
assert targets["x"] == pytest.approx(pose.table_xy_mm[0])
|
||||||
|
assert targets["y"] == pytest.approx(pose.table_xy_mm[1])
|
||||||
|
assert targets["yaw"] == pytest.approx(pose.yaw_deg)
|
||||||
|
assert targets["pitch"] == pytest.approx(pose.pitch_deg)
|
||||||
|
assert targets["z"] == pytest.approx(pose.contact_z_mm)
|
||||||
|
directory = campaign.start(
|
||||||
|
tmp_path, sensor_model="finger", sensor_sn="SN9",
|
||||||
|
coordinate_frame="sensor", max_force_n=10, min_force_n=0, simulate=True,
|
||||||
|
)
|
||||||
|
assert campaign._document["acquisition_commands_motion"] is False
|
||||||
|
_finish_open_steps(campaign)
|
||||||
|
assert campaign.phase == "done"
|
||||||
|
saved = (directory / "sites" / "P1" / "repeat-001" / "reference.csv").read_text(encoding="utf-8")
|
||||||
|
assert "pose_yaw_deg" in saved.splitlines()[0]
|
||||||
|
row = list(csv.DictReader(saved.splitlines()))[1]
|
||||||
|
assert float(row["pose_yaw_deg"]) == pytest.approx(pose.yaw_deg)
|
||||||
|
assert float(row["nx"]) == pytest.approx(0.0)
|
||||||
|
assert row["reference_source"] == "weight"
|
||||||
|
assert row["branch"] == "load"
|
||||||
|
campaign.reacquire("P1")
|
||||||
|
_finish_open_steps(campaign)
|
||||||
|
assert campaign.phase == "liftoff"
|
||||||
|
campaign.confirm_liftoff()
|
||||||
|
assert campaign.phase == "done"
|
||||||
|
assert (directory / "sites" / "P1" / "repeat-002" / "reference.csv").is_file()
|
||||||
|
session = json.loads((directory / "session.json").read_text(encoding="utf-8"))
|
||||||
|
assert session["completion"][0]["repeats_complete"] == 2
|
||||||
|
assert session["completion"][0]["complete"] is True
|
||||||
Reference in New Issue
Block a user