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

added motion trajectory simulation

Browse Source
This commit is contained in:
ykcheungab
2026-09-23 09:58:00 +08:00
parent f1bec62ca4
commit 3347d17a20
25 changed files with 5198 additions and 1762 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/autocal/tools/layout_mockup.py
+229 -58
View File
@@ -25,6 +25,7 @@ from __future__ import annotations
import argparse
import math
import math
import os
import sys
from pathlib import Path
@@ -41,8 +42,16 @@ if sys.platform == "win32" and "QT_QPA_FONTDIR" not in os.environ:
if _fonts.is_dir():
os.environ["QT_QPA_FONTDIR"] = str(_fonts)
from PySide6.QtCore import QPoint, QRect, Qt # noqa: E402
from PySide6.QtGui import QColor, QFont, QPainter, QPen, QPixmap, QPolygon # noqa: E402
from PySide6.QtCore import QPoint, QPointF, QRect, Qt # noqa: E402
from PySide6.QtGui import ( # noqa: E402
QColor,
QFont,
QPainter,
QPainterPath,
QPen,
QPixmap,
QPolygon,
)
from PySide6.QtWidgets import QApplication # noqa: E402
# ---------------------------------------------------------------- geometry ---
@@ -157,7 +166,6 @@ STRINGS = {
"finger_initial_view": "初始视角",
"finger_reset": "复位视角",
"finger_textured_note": "UV 贴图：patch_texture_compute.png",
"finger_axes_note": "黄色为工具轴 · X/Y 为工作台，Z 为工具行程 · 绿=对齐，红=不可对齐",
"finger_bundle_summary": "1032 顶点 · 1974 三角面 · 15.00 × 7.50 × 15.81 mm · 0.018388 mm/单位",
"finger_flat_title": "展平图像（点击取点）",
"finger_pick_hint": "点击传感面上的像素即可取点；落在 UV 缝隙上的像素会被拒绝。",
@@ -166,21 +174,32 @@ STRINGS = {
["传感器 (mm)", "2.841, 3.512, 0.940", None],
["法向", "-0.18, -0.88, 0.43", None],
["平台 (mm)", "2.841, 0.940, -3.512", None],
["X / Y (mm)", "2.841, 0.940", None],
["yaw / pitch", "-78.42° / 23.10°", None],
["位姿 X / Y / yaw / pitch", "-2.841 0.940 -78.42 23.10", None],
["工作台移动 X / Y", "+2.841 -0.940 mm", None],
["对轴残差", "0.03° 可对轴", "#12b76a"],
["坐标系映射", "x→x, y→z, z→-y", None],
],
"finger_plate_label": "安装板厚 (mm)",
"finger_plate_value": "0.000",
"finger_tool_label": "刀具 X / Y",
"finger_tool_values": ["0.000", "0.000"],
"finger_sensor_xy_label": "传感器 X / Y",
"finger_sensor_xy_values": ["0.000", "0.000"],
"finger_sensor_zy_label": "传感器 Z / yaw",
"finger_sensor_zy_values": ["0.000", "0.000"],
"finger_preview_title": "三维预览",
"finger_preview_legend": "曲面颜色 = 对轴残差（绿为对正，红为对不上） · 黄色为 Z 轴",
"finger_point_cols": ["#", "像素 u, v", "sensor x", "sensor y", "sensor z", "nx", "ny", "nz", "X / Y", "Z", "yaw", "pitch"],
"finger_point_rows": [
["1", "0.35, 0.40", "2.841", "3.512", "0.940", "-0.18", "-0.88", "0.43", "2.841 / 0.940", "7.958", "-78.42", "23.10"],
["2", "0.62, 0.55", "5.113", "2.204", "-1.870", "0.21", "0.55", "-0.81", "5.113 / -1.870", "7.958", "12.05", "8.44"],
["3", "0.50, 0.72", "3.902", "6.118", "2.405", "0.55", "0.19", "0.81", "3.902 / 2.405", "7.958", "-90.00", "51.66"],
"finger_preview_checks": ["显示运动轴", "显示转台原点平面", "显示刀具路径"],
"finger_play": "播放动作",
"finger_preview_notes": [
"颜色含义：绿→红 = 表面对齐残差；红/绿/蓝三色轴 = 工作台 X/Y/Z（位于原点）；橙点 = 工作台原点。",
"黄色直线 = 刀具轴（+Z，位置即刀具 X/Y）；黄色方框 = 传感器安装基面（高度 = 传感器 Z，含安装板）。",
"青色线 = 规划路径；紫圈 = 转台当前姿态、浅紫圈 = 结束姿态，与固定方格网对比即看出整个变换；白点+蓝线 = 接触点及法向。",
],
"finger_point_cols": ["sensor x", "sensor y", "sensor z", "nx", "ny", "nz", "X", "Y", "Z", "yaw", "pitch", "残差"],
"finger_point_rows": [
["2.841", "3.512", "0.940", "-0.18", "-0.88", "0.43", "2.841", "0.940", "7.958", "-78.42", "23.10", "0.03"],
["5.113", "2.204", "-1.870", "0.21", "0.55", "-0.81", "5.113", "-1.870", "7.958", "12.05", "8.44", "0.02"],
["3.902", "6.118", "2.405", "0.55", "0.19", "0.81", "3.902", "2.405", "7.958", "-90.00", "51.66", "0.02"],
],
"finger_table_buttons": [("删除所选", False), ("上移", False), ("下移", False), ("清空标定点", False)],
"finger_bad_word": "撞 yaw 行程限位",
"finger_point_count": "3 个取点 · 展平 1024 × 1024 · 有效像素 72.9%",
"sensor_note": "设备与信号源、实时图像、力曲线",
@@ -284,7 +303,6 @@ STRINGS = {
"finger_initial_view": "Initial view",
"finger_reset": "Reset view",
"finger_textured_note": "UV texture: patch_texture_compute.png",
"finger_axes_note": "Yellow is the tool axis · X/Y are the table, Z the tool travel · green aligned, red not",
"finger_bundle_summary": "1032 vertices · 1974 triangles · 15.00 × 7.50 × 15.81 mm · 0.018388 mm per unit",
"finger_flat_title": "Flat image (click to pick)",
"finger_pick_hint": "Click a pixel on the sensing surface to pick a point; pixels in a UV gap are refused.",
@@ -293,21 +311,33 @@ STRINGS = {
["Sensor (mm)", "2.841, 3.512, 0.940", None],
["Normal", "-0.18, -0.88, 0.43", None],
["Platform (mm)", "2.841, 0.940, -3.512", None],
["X / Y (mm)", "2.841, 0.940", None],
["yaw / pitch", "-78.42° / 23.10°", None],
["Pose X / Y / yaw / pitch", "-2.841 0.940 -78.42 23.10", None],
["Table move X / Y", "+2.841 -0.940 mm", None],
["Residual", "0.03° can align", "#12b76a"],
["Frame map", "x→x, y→z, z→-y", None],
],
"finger_plate_label": "Mounting plate (mm)",
"finger_plate_value": "0.000",
"finger_tool_label": "Tool X / Y",
"finger_tool_values": ["0.000", "0.000"],
"finger_sensor_xy_label": "Sensor X / Y",
"finger_sensor_xy_values": ["0.000", "0.000"],
"finger_sensor_zy_label": "Sensor Z / yaw",
"finger_sensor_zy_values": ["0.000", "0.000"],
"finger_preview_title": "3D preview",
"finger_preview_legend": "Surface colour = alignment residual (green aligned, red not) · yellow is the Z axis",
"finger_point_cols": ["#", "pixel u, v", "sensor x", "sensor y", "sensor z", "nx", "ny", "nz", "X / Y", "Z", "yaw", "pitch"],
"finger_point_rows": [
["1", "0.35, 0.40", "2.841", "3.512", "0.940", "-0.18", "-0.88", "0.43", "2.841 / 0.940", "7.958", "-78.42", "23.10"],
["2", "0.62, 0.55", "5.113", "2.204", "-1.870", "0.21", "0.55", "-0.81", "5.113 / -1.870", "7.958", "12.05", "8.44"],
["3", "0.50, 0.72", "3.902", "6.118", "2.405", "0.55", "0.19", "0.81", "3.902 / 2.405", "7.958", "-90.00", "51.66"],
"finger_preview_checks": ["Show motion axes", "Show table origin plane", "Show tool path"],
"finger_play": "Play motion",
"finger_preview_notes": [
"What the colours mean: green→red = the surface's alignment residual; the red/green/blue triad is the table's X/Y/Z at the origin; the orange dot is the origin.",
"Yellow line = the tool axis (+Z, at the tool's own X/Y); the amber outline = the sensor base plane, at the sensor's Z offset (the plate).",
"Cyan line = the planned path; the violet ring is the table's pose now and the faint one where it finishes, against the fixed grid that is where it started; white dot with blue line = the contact and its normal.",
],
"finger_preview_legend": "Surface colour = alignment residual (green aligned, red not) · yellow is the Z axis",
"finger_point_cols": ["sensor x", "sensor y", "sensor z", "nx", "ny", "nz", "X", "Y", "Z", "yaw", "pitch", "residual"],
"finger_point_rows": [
["2.841", "3.512", "0.940", "-0.18", "-0.88", "0.43", "2.841", "0.940", "7.958", "-78.42", "23.10", "0.03"],
["5.113", "2.204", "-1.870", "0.21", "0.55", "-0.81", "5.113", "-1.870", "7.958", "12.05", "8.44", "0.02"],
["3.902", "6.118", "2.405", "0.55", "0.19", "0.81", "3.902", "2.405", "7.958", "-90.00", "51.66", "0.02"],
],
"finger_table_buttons": [("Remove selected", False), ("Move up", False), ("Move down", False), ("Clear points", False)],
"finger_bad_word": "hits yaw travel limit",
"finger_point_count": "3 picked points · flat 1024 × 1024 · 72.9% valid pixels",
"sensor_note": "Device and source, live image, force curves",
@@ -352,6 +382,28 @@ def _font(size: int = 11, *, bold: bool = False) -> QFont:
return font
def _arrowhead(painter: QPainter, path: QPainterPath, share: float, colour: QColor, size: int) -> None:
"""An arrowhead on ``path`` at ``share``, pointing the way the path runs.
The direction comes from two nearby points on the path rather than from
``angleAtPercent``: that angle is measured counter-clockwise from +x while Qt's
y axis grows downward, and every attempt to reconcile the two by hand has
produced an arrow pointing backwards.
"""
tip = path.pointAtPercent(share)
ahead = path.pointAtPercent(min(1.0, share + 0.01))
dx, dy = ahead.x() - tip.x(), ahead.y() - tip.y()
length = math.hypot(dx, dy) or 1.0
ux, uy = dx / length, dy / length
painter.setPen(QPen(colour, 2))
for sign in (1.0, -1.0):
barb = QPointF(
tip.x() - ux * size + sign * -uy * size * 0.6,
tip.y() - uy * size + sign * ux * size * 0.6,
)
painter.drawLine(tip, barb)
def _panel(painter: QPainter, rect: QRect, palette: Palette) -> QRect:
painter.fillRect(rect, palette.panel)
painter.setPen(QPen(palette.border, 1))
@@ -1008,45 +1060,141 @@ def draw_finger_tab(painter: QPainter, palette: Palette, s: dict, rect: QRect) -
_text(painter, QRect(inner_read.x(), row_y, label_w, 20), label, palette, size=9, muted=True)
_text(painter, QRect(inner_read.x() + label_w, row_y, inner_read.width() - label_w, 20), value, palette, size=9, colour=QColor(colour) if colour else None)
row_y += 20
_text(painter, QRect(inner_read.x(), inner_read.bottom() - 36, inner_read.width(), 18), s["finger_plate_label"], palette, size=9, muted=True)
_field(painter, QRect(inner_read.x(), inner_read.bottom() - 20, inner_read.width(), 20), s["finger_plate_value"], palette)
# The setup and tool fields: pairs of numbers, as in the app, where they are
# spin boxes two to a row because they get dialled in against the real machine.
pairs = (
("finger_tool_label", "finger_tool_values"),
("finger_sensor_xy_label", "finger_sensor_xy_values"),
("finger_sensor_zy_label", "finger_sensor_zy_values"),
)
pair_y = row_y + 4
for label_key, values_key in pairs:
_text(painter, QRect(inner_read.x(), pair_y, label_w, 20), s[label_key], palette, size=9, muted=True)
field_w = int((inner_read.width() - label_w - 6) / 2)
for index, value in enumerate(s[values_key][:2]):
_field(
painter,
QRect(inner_read.x() + label_w + index * (field_w + 6), pair_y, field_w, 20),
value,
palette,
)
pair_y += 22
# 3D preview: the tool axis and a normal with its residual.
# 3D preview: the machine's frame at the table origin, the sensor on it, and the
# planned path. Schematic, but the parts and their colours are the ones the real
# view draws — a proposal that disagrees with the app is worse than no proposal.
inner_preview = _panel(painter, preview, palette)
_text(painter, QRect(inner_preview.x(), inner_preview.y(), inner_preview.width(), 18), s["finger_preview_title"], palette, size=10, bold=True)
view = QRect(inner_preview.x(), inner_preview.y() + 22, inner_preview.width(), inner_preview.height() - 22 - 34)
checks_h = 20
play_h = 28
notes_h = 3 * 14 + 4
view = QRect(
inner_preview.x(),
inner_preview.y() + 22,
inner_preview.width(),
inner_preview.height() - 22 - checks_h - play_h - notes_h,
)
painter.fillRect(view, QColor("#12161c"))
painter.setPen(QPen(QColor("#2a313c"), 1))
for i in range(1, 4):
painter.drawLine(view.x() + i * view.width() // 4, view.y(), view.x() + i * view.width() // 4, view.bottom())
cx, cy = view.center().x(), view.center().y() + 40
axis = QPoint(cx, view.y() + 24)
painter.setPen(QPen(QColor("#f2c94c"), 3))
painter.drawLine(axis, QPoint(cx, cy))
painter.setBrush(QColor("#f2c94c"))
painter.drawEllipse(QPoint(cx, cy), 3, 3)
# The motion axes: the platform frame, drawn in the sensor frame.
cx, cy = view.center().x(), view.center().y() + int(view.height() * 0.12)
# The table origin's plane: a grid, not a filled surface.
half = int(view.width() * 0.30)
floor = QRect(cx - half, cy - int(half * 0.26), 2 * half, int(half * 0.52))
painter.setPen(QPen(QColor("#8c95ab"), 1))
for index in range(5):
fx = floor.x() + index * floor.width() // 4
fy = floor.y() + index * floor.height() // 4
painter.drawLine(fx, floor.y(), fx, floor.bottom())
painter.drawLine(floor.x(), fy, floor.right(), fy)
# The sensor: the sensing face as an arc, the shell below it as a wireframe-ish
# outline. Grey, because the surface carries the residual colour instead.
dome_w = int(view.width() * 0.34)
dome_h = int(view.height() * 0.34)
body = QRect(cx - dome_w // 2, cy - dome_h, dome_w, dome_h + int(dome_h * 0.45))
painter.setPen(QPen(QColor("#5b6273"), 1))
painter.setBrush(QColor("#26b847"))
painter.drawChord(QRect(body.x(), body.y(), body.width(), body.height() * 2 // 3), 0, 180 * 16)
painter.setBrush(Qt.BrushStyle.NoBrush)
painter.drawRect(QRect(body.x(), body.y() + body.height() // 4, body.width(), body.height() // 2))
# The base plane, lifted off the origin by the sensor's z setup offset (the
# mounting plate), and the table's own frame: a circle at the pose it ends in,
# against the square grid that is where it started.
base = QRect(body.x() - 8, body.bottom() - 6, body.width() + 16, int(half * 0.30))
painter.setPen(QPen(QColor("#fab829"), 2))
painter.drawRect(base)
ring = QRect(0, 0, int(half * 1.5), int(half * 0.78))
ring.moveCenter(QPoint(cx + int(half * 0.30), cy + int(half * 0.05)))
painter.setPen(QPen(QColor("#8c6ff7"), 2))
painter.setBrush(Qt.BrushStyle.NoBrush)
painter.drawEllipse(ring)
painter.drawLine(ring.center().x() - ring.width() // 2, ring.center().y(),
ring.center().x() + ring.width() // 2, ring.center().y())
painter.drawLine(ring.center().x(), ring.center().y() - ring.height() // 2,
ring.center().x(), ring.center().y() + ring.height() // 2)
painter.setPen(QPen(QColor(140, 111, 247, 130), 2))
painter.drawEllipse(ring.adjusted(-14, -7, -14, -7))
# The planned path: the stages swing the contact, then the table slides it in.
contact = QPoint(body.x() + 10, body.y() + int(body.height() * 0.28))
landed = QPoint(cx + 6, cy - int(half * 0.30))
path = QPainterPath(contact)
path.quadTo(QPoint(contact.x() + int(half * 0.75), contact.y() - int(half * 0.35)), landed)
painter.setPen(QPen(QColor("#40d1f2"), 2))
painter.drawPath(path)
for share in (0.35, 0.7):
_arrowhead(painter, path, share, QColor("#ccf4ff"), 7)
painter.setBrush(QColor("#40d1f2"))
painter.setPen(Qt.PenStyle.NoPen)
painter.drawEllipse(landed, 5, 5)
# The table triad at the origin: the only full-strength one in the picture.
origin = QPoint(floor.center().x(), floor.center().y() + int(half * 0.12))
for colour, end in (
("#e65454", QPoint(cx - 96, cy + 18)),
("#61cc61", QPoint(cx + 30, cy + 92)),
("#5c8cf2", QPoint(cx + 88, cy - 26)),
("#e64c4c", QPoint(origin.x(), origin.y() + int(half * 0.62))),
("#59c761", QPoint(origin.x() + int(half * 0.85), origin.y())),
("#5787f2", QPoint(origin.x(), origin.y() - int(half * 0.42))),
):
painter.setPen(QPen(QColor(colour), 2))
painter.drawLine(QPoint(cx, cy), end)
painter.setPen(QPen(QColor("#12b76a"), 3))
painter.drawLine(QPoint(cx, cy), QPoint(cx + 26, cy - 96))
painter.setPen(QPen(QColor("#d92d20"), 3))
painter.drawLine(QPoint(cx, cy), QPoint(cx - 74, cy - 78))
painter.drawLine(origin, end)
painter.setBrush(QColor("#fa9933"))
painter.setPen(Qt.PenStyle.NoPen)
painter.drawEllipse(origin, 5, 5)
painter.setBrush(Qt.BrushStyle.NoBrush)
painter.setPen(QPen(QColor("#4a5568"), 1))
painter.drawLine(QPoint(cx - 90, cy), QPoint(cx + 90, cy))
painter.setPen(QPen(QColor("#e65454"), 1))
painter.drawText(QPoint(cx - 108, cy + 24), "X")
painter.setPen(QPen(QColor("#61cc61"), 1))
painter.drawText(QPoint(cx + 34, cy + 100), "Y")
painter.setPen(QPen(QColor("#5c8cf2"), 1))
painter.drawText(QPoint(cx + 92, cy - 28), "Z")
_text(painter, QRect(inner_preview.x(), view.bottom() + 2, inner_preview.width(), 30), s["finger_axes_note"], palette, size=8, muted=True)
# The tool axis, through the point the tool touches.
painter.setPen(QPen(QColor("#f2e04c"), 2))
painter.drawLine(QPoint(landed.x(), view.y() + 16), QPoint(landed.x(), landed.y()))
_text(painter, QRect(landed.x() - 8, view.y() + 2, 90, 16), "+Z", palette, size=8, colour=QColor("#f2e04c"))
# The picked contact and its normal, and the point selected in the table.
painter.setPen(QPen(QColor("#59a6ff"), 2))
painter.drawLine(contact, QPoint(contact.x() - int(half * 0.30), contact.y() - int(half * 0.34)))
painter.setPen(QPen(QColor("#ffffff"), 2))
painter.setBrush(QColor("#ffffff"))
painter.drawEllipse(contact, 4, 4)
painter.setPen(Qt.PenStyle.NoPen)
painter.setBrush(QColor("#eb2133"))
painter.drawEllipse(QPoint(contact.x() + 14, contact.y() + 4), 5, 5)
painter.setBrush(Qt.BrushStyle.NoBrush)
checks = s["finger_preview_checks"]
check_w = inner_preview.width() // max(1, len(checks))
for index, label in enumerate(checks):
box = QRect(inner_preview.x() + index * check_w, view.bottom() + 2, check_w, checks_h)
painter.setPen(QPen(palette.accent, 2))
painter.drawLine(box.x() + 2, box.y() + 9, box.x() + 5, box.y() + 13)
painter.drawLine(box.x() + 5, box.y() + 13, box.x() + 10, box.y() + 5)
_text(painter, QRect(box.x() + 15, box.y(), check_w - 15, checks_h), label, palette, size=9, muted=True)
play = QRect(inner_preview.x(), view.bottom() + checks_h + 2, 110, play_h - 4)
_button(painter, play, s["finger_play"], palette, primary=True)
notes_y = play.bottom() + 4
for text in s["finger_preview_notes"]:
_text(painter, QRect(inner_preview.x(), notes_y, inner_preview.width(), 14), text, palette, size=8, muted=True)
notes_y += 14
# Picked points.
table = QRect(inner.x(), y + body_h + 8, inner.width(), max(60, table_h))
@@ -1055,9 +1203,16 @@ def draw_finger_tab(painter: QPainter, palette: Palette, s: dict, rect: QRect) -
painter.drawRect(table.adjusted(0, 0, -1, -1))
header_h = 24
painter.fillRect(QRect(table.x(), table.y(), table.width(), header_h), palette.panel)
# Leading select column, then the data columns: the real table has a checkbox
# per row so points can be picked out, edited, reordered or removed.
select_w = 34
painter.setPen(QPen(palette.border, 1))
painter.drawLine(table.x() + select_w, table.y(), table.x() + select_w, table.bottom())
columns = s["finger_point_cols"]
widths = [int(table.width() * share) for share in (0.06, 0.09, 0.1, 0.09, 0.09, 0.07, 0.07, 0.07, 0.09, 0.09, 0.09, 0.09)]
x = table.x()
widths = [int((table.width() - select_w) * share)
for share in (0.09, 0.1, 0.09, 0.09, 0.08, 0.08, 0.08, 0.11,
0.07, 0.07, 0.07, 0.07)]
x = table.x() + select_w
for name, width in zip(columns, widths):
_text(painter, QRect(x, table.y(), width, header_h), name, palette, size=9, muted=True, align=Qt.AlignmentFlag.AlignCenter)
x += width
@@ -1065,12 +1220,28 @@ def draw_finger_tab(painter: QPainter, palette: Palette, s: dict, rect: QRect) -
row_y = table.y() + header_h + index * 26
if index % 2 == 0:
painter.fillRect(QRect(table.x() + 1, row_y, table.width() - 2, 25), palette.panel)
x = table.x()
# Tick the first row, as the selected point being tested.
box = QRect(table.x() + select_w // 2 - 6, row_y + 8, 12, 12)
painter.setPen(QPen(palette.muted, 1))
painter.setBrush(Qt.BrushStyle.NoBrush)
painter.drawRect(box)
if index == 0:
painter.setPen(QPen(palette.accent, 2))
painter.drawLine(box.x() + 2, box.y() + 6, box.x() + 5, box.y() + 10)
painter.drawLine(box.x() + 5, box.y() + 10, box.x() + 10, box.y() + 3)
x = table.x() + select_w
for value, width in zip(row, widths):
colour = palette.danger if value == s["finger_bad_word"] else None
_text(painter, QRect(x, row_y, width, 26), value, palette, size=10, align=Qt.AlignmentFlag.AlignCenter, colour=colour)
x += width
_text(painter, QRect(table.x() + 8, table.bottom() + 2, table.width(), 18), s["finger_point_count"], palette, size=9, muted=True)
buttons = s["finger_table_buttons"]
bx = table.x()
by = table.bottom() + 26
for label, primary in buttons:
width = 30 + 8 * len(label)
_button(painter, QRect(bx, by, width, 26), label, palette, primary=primary)
bx += width + 8
TAB_PAINTERS = {