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

优化传感器对准压头

Browse Source
This commit is contained in:
yijian
2026-10-15 17:13:04 +08:00
parent ad049d12e4
commit 2a154307fd
15 changed files with 565 additions and 172 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/autocal/config/locales/en_US.json
+6 -1
View File
@@ -59,7 +59,7 @@
"stop": "Stop",
"stop_hint": "Stop every axis now (the same command as 全部停止 in the strip).",
"return_initial": "Return to zero",
"return_hint": "Yaw, pitch and press axis Z return to 0. The table parks at its initial position: X at -1.0 mm, Y 15.0 mm forward.",
"return_hint": "Yaw, pitch and press axis Z return to 0. The table parks at its initial position: X at 0 mm, Y at 0 mm.",
"confirm_title": "Confirm motion",
"confirm_execute": "Will move these axes together:\n\n{steps}\n\nCheck this against the machine before continuing.",
"confirm_return": "Will move these axes back to zero together:\n\n{steps}",
@@ -416,6 +416,10 @@
"row_table_move": "Table move X / Y",
"row_press": "Press-axis target",
"row_residual": "Alignment residual",
"row_match": "Point match",
"match_none": "Pick a point on the flat image first",
"match_offline": "Platform not connected",
"match_value": "Selected X {sx} Y {sy} mm, yaw {syaw}°, pitch {spitch}°. Actual X {ax} Y {ay} mm, yaw {ayaw}°, pitch {apitch}°. Difference X {dx} Y {dy} mm, yaw {dyaw}°, pitch {dpitch}°",
"row_frames": "Frame mapping",
"branch_none": "—",
"branch_value": "{reason} (this pose {travel}° total, the other exact pose {other}°)",
@@ -424,6 +428,7 @@
"branch_in_range": "inside travel",
"branch_upright": "face still up",
"branch_smallest": "smallest turn",
"branch_crown": "already facing up; translate only",
"press_unset": "Zero not measured: this move holds the press axis (enter it in Device setup → Press zero)",
"press_target": "{target} mm (geometric contact height {height} mm)",
"tool_xy": "Tool X / Y",
apps/autocal/config/locales/zh_CN.json
+6 -1
View File
@@ -59,7 +59,7 @@
"stop": "停止",
"stop_hint": "立即停止所有轴（与顶栏「全部停止」相同）。",
"return_initial": "回初始位",
"return_hint": "回转、俯仰、压轴 Z 回到 0。工作台停在初始位置：X 为 -1.0 mm，Y 为向前 15.0 mm。",
"return_hint": "回转、俯仰、压轴 Z 回到 0。工作台停在初始位置：X 为 0 mm，Y 为 0 mm。",
"confirm_title": "确认运动",
"confirm_execute": "将同时移动：\n\n{steps}\n\n请确认仿真与机床一致。",
"confirm_return": "将同时移回零点：\n\n{steps}\n\n请确认机床上没有干涉。",
@@ -416,6 +416,10 @@
"row_table_move": "工作台移动 X / Y",
"row_press": "压轴目标",
"row_residual": "对齐残差",
"row_match": "点位匹配",
"match_none": "先在展开图上选一个点",
"match_offline": "未连接平台",
"match_value": "选择 X {sx} Y {sy} mm，回转 {syaw}°，俯仰 {spitch}°。实际 X {ax} Y {ay} mm，回转 {ayaw}°，俯仰 {apitch}°。差 X {dx} Y {dy} mm，回转 {dyaw}°，俯仰 {dpitch}°",
"row_frames": "坐标映射",
"branch_none": "—",
"branch_value": "{reason}（本解合计 {travel}°，另一精确解 {other}°）",
@@ -424,6 +428,7 @@
"branch_in_range": "行程内",
"branch_upright": "传感面仍朝上",
"branch_smallest": "转动量最小",
"branch_crown": "已朝上，只平移",
"press_unset": "未标定零位：本动作保持压轴不动（在 设备设置 → 压轴零位 填入实测零位）",
"press_target": "{target} mm（几何接触高度 {height} mm）",
"tool_xy": "刀具 X / Y",
apps/autocal/docs/inverse-kinematics.md
+12 -11
View File
@@ -16,10 +16,10 @@ the contact point ends up, and Z is reported, never solved.
| Item | Value | Where |
|---|---|---|
| Tool axis | `z` | `motion/frames.py` |
| Yaw axis | `z`, travel **±180°** | `YAW_AXIS`, `finger_yaw_limit_deg` |
| Pitch axis | `y`, travel **90°** | `PITCH_AXIS`, `finger_pitch_limit_deg` |
| Yaw axis | `z`, travel **±180°**, positive is clockwise from above | `YAW_AXIS`, `YAW_COMMAND_SIGN` |
| Pitch axis | `x`, travel **90°**, positive nods the dome toward `−Y` | `PITCH_AXIS`, `PITCH_COMMAND_SIGN` |
| Stage order | `R_pitch @ R_yaw` | `stage_rotation` |
| Mount map | sensor `x→x, y→z, z→-y` | `AXIS_MAP` |
| Mount map | sensor `x→-x, y→z, z→y` | `AXIS_MAP` |
| Aim direction | `(0, 0, +1)` in the platform frame | `approach_sign = +1` |
### The face is aimed at +Z, and this was wrong
@@ -135,20 +135,21 @@ All 1032 vertices are **in range**, and all 1032 report `upright` under the
tool-axis reference task 1 introduced. Reachability is the yaw limit, not the rim:
see *the face is aimed at +Z* above and *the two exact poses of one normal* below.
### The two pitch bindings are one family
### The pitch axis is the left-right axis
`PITCH_AXIS` is `y`, while the trunnion drawing shows the pitch arc about `x`.
This turns out not to matter:
The right-hand identity
```
R_y(p) · R_z(t) ≡ R_x(p) · R_z(t − 90°)
```
agreeing to machine precision in alignment for every yaw and pitch tried. So the
axis binding is a **naming and zero-offset choice**: the two reach exactly the
same set of alignments, and the 90° difference is absorbed when the yaw stage is
zeroed at commissioning. `PITCH_AXIS` is documentation, not a correctness
question — which is why the tests pin the identity rather than a value.
still says the two bindings can reach the same alignments. It does not say the
drive can use either one. Measured from the commissioned zero on 2026-10-15:
pitch is about platform `x`. An executed pose matched the preview only after
the pitch sign was reversed, so positive pitch is the right-hand sense about
`+X` and nods the dome toward the operator. Axis 2 at `+10°` turned the tip clockwise from above, so positive yaw is
the opposite of the right-hand sense about `+Z`. The solver, the preview and
the number sent to the drive are that commanded angle.
## What the solver reports
apps/autocal/src/orisys_autocal/motion/kinematics.py
+166 -60
View File
@@ -33,8 +33,11 @@ import numpy as np
from ..sensor.frames import (
AXIS_MAP,
PITCH_AXIS,
PITCH_COMMAND_SIGN,
TOOL_AXIS,
YAW_AXIS,
YAW_COMMAND_SIGN,
machine_table_xy,
mount_rotation,
to_platform,
to_sensor,
@@ -43,6 +46,13 @@ from ..sensor.frames import (
#: A normal this close to the tool axis counts as already aligned.
ALIGNED_TOL_DEG = 0.01
#: A normal already within this many degrees of the tool axis is not yawed.
#: Pitch nods about the left-right axis, so a leftover lean along that axis can
#: only be removed by yawing a quarter-turn first. On the crown that leftover is
#: about 1.6°, and the quarter-turn turns a fore-aft table move into a sideways
#: one. Before the drive axes were measured, the same point was yaw 0 and a Y move.
CROWN_RESIDUAL_DEG = 3.0
#: Sweep step, degrees, for each stage of the alignment search. The sweep only
#: has to land in the right basin — the polish that follows refines it to 1e-4 —
#: so this trades a little precision for a sweep the eye can be told about.
@@ -214,17 +224,24 @@ class Pose:
False means the geometry cannot bring the tool axis onto the normal, no
matter the angles — a statement about the machine, not about the search.
A crown point kept upright on purpose is reachable: the quarter-turn that
would erase the last couple of degrees is declined, not impossible.
"""
return self.ok and self.residual_deg <= 0.5
if not self.ok:
return False
if self.branch_reason == "crown":
return True
return self.residual_deg <= 0.5
def describe(self) -> str:
if not self.ok:
return f"no pose: {self.reason}"
yaw = f"{self.yaw_deg:+.3f}" if self.yaw_determined else "indeterminate"
move_x, move_y = machine_table_xy(self.table_xy_mm)
return (
f"X {self.target_xy_mm[0]:+.3f} Y {self.target_xy_mm[1]:+.3f} "
f"Z {self.contact_z_mm:+.3f} move {self.table_xy_mm[0]:+.3f} "
f"{self.table_xy_mm[1]:+.3f} yaw {yaw} pitch {self.pitch_deg:+.3f} "
f"Z {self.contact_z_mm:+.3f} move {move_x:+.3f} "
f"{move_y:+.3f} yaw {yaw} pitch {self.pitch_deg:+.3f} "
f"residual {self.residual_deg:.4f} deg"
)
@@ -236,6 +253,12 @@ class PlatformStack:
axis_map: dict[str, str] = field(default_factory=lambda: dict(AXIS_MAP))
yaw_axis: str = YAW_AXIS
pitch_axis: str = PITCH_AXIS
#: Commanded degrees times this is the right-hand angle about ``yaw_axis``.
#: ``-1`` matches the drive: positive yaw is clockwise from above.
yaw_sign: float = YAW_COMMAND_SIGN
#: Commanded degrees times this is the right-hand angle about ``pitch_axis``.
#: ``+1`` matches the drive: positive pitch nods the dome toward platform −Y.
pitch_sign: float = PITCH_COMMAND_SIGN
tool_axis: str = TOOL_AXIS
#: Which way along ``tool_axis`` the sensing face is aimed, relative to the
#: normal the mesh stores. It is ``+1``: the sensor sits at the bottom of its
@@ -294,6 +317,9 @@ class PlatformStack:
for axis in (self.yaw_axis, self.pitch_axis, self.tool_axis):
if axis not in ("x", "y", "z"):
raise ValueError(f"unknown axis {axis!r}")
for name, sign in (("yaw_sign", self.yaw_sign), ("pitch_sign", self.pitch_sign)):
if float(sign) not in (1.0, -1.0):
raise ValueError(f"{name} must be +1 or -1, got {sign}")
# ------------------------------------------------------------------ frames
@@ -381,16 +407,31 @@ class PlatformStack:
length = float(np.linalg.norm(normal))
return normal if length < 1e-12 else normal / length
def _command_sign(self, axis: str) -> float:
"""``+1`` or ``-1``: commanded degrees to the right-hand angle about ``axis``."""
if axis == self.yaw_axis:
return float(self.yaw_sign)
if axis == self.pitch_axis:
return float(self.pitch_sign)
return 1.0
def yaw_rotation(self, degrees: float) -> np.ndarray:
"""The rotation a yaw command of ``degrees`` produces on the drive."""
return rotation_about(self.yaw_axis, self.yaw_sign * float(degrees))
def pitch_rotation(self, degrees: float) -> np.ndarray:
"""The rotation a pitch command of ``degrees`` produces on the drive."""
return rotation_about(self.pitch_axis, self.pitch_sign * float(degrees))
def platform_rotation(self, yaw_deg: float, pitch_deg: float) -> np.ndarray:
"""``R_pitch · R_yaw``: the stage rotation written in the table frame.
:meth:`stage_rotation` is the *same* rotation in the sensor frame — the two
are conjugates by the mount. Anything that reasons about where a point of
the table ends up (the contact, the table's own frame) needs this one.
Both angles are the numbers sent to the drives. :meth:`stage_rotation` is
the same rotation in the sensor frame — the two are conjugates by the
mount. Anything that reasons about where a point of the table ends up
(the contact, the table's own frame) needs this one.
"""
return rotation_about(self.pitch_axis, pitch_deg) @ rotation_about(
self.yaw_axis, yaw_deg
)
return self.pitch_rotation(pitch_deg) @ self.yaw_rotation(yaw_deg)
# -------------------------------------------------------------------- solve
@@ -448,6 +489,19 @@ class PlatformStack:
(branch.travel_deg for branch in usable if branch is not chosen),
default=0.0,
)
yaw_deg, pitch_deg, residual, upright, branch_reason, branch_travel, other_travel = (
self._keep_a_crown_upright(
unit,
tool,
yaw_deg,
pitch_deg,
residual,
upright,
branch_reason,
branch_travel,
other_travel,
)
)
elif branches:
# Every exact pose needs more travel than the stage has. Report the
# best pose the limits *do* allow, with the residual it really
@@ -481,7 +535,7 @@ class PlatformStack:
reason = "the normal lies on the tool axis; yaw is indeterminate"
elif not in_range:
reason = "the pose is outside the stage travel"
elif residual > self.tolerance_deg:
elif branch_reason != "crown" and residual > self.tolerance_deg:
reason = "no pose meets the tolerance"
# Three points, and they are three different places:
@@ -520,6 +574,59 @@ class PlatformStack:
other_travel_deg=other_travel,
)
def _keep_a_crown_upright(
self,
unit: np.ndarray,
tool: np.ndarray,
yaw_deg: float,
pitch_deg: float,
residual: float,
upright: bool,
branch_reason: str,
branch_travel: float,
other_travel: float,
) -> tuple[float, float, float, bool, str, float, float]:
"""Leave a nearly vertical normal unyawed, and move the table instead.
The exact pose of a crown point yaws about 90° so the pitch stage can nod
away a degree or two of sideways lean. That turns the point's fore-aft
offset into a sideways move. Yaw 0 and the pitch that actually helps —
none, when the lean is sideways — put the move back on Y.
"""
if abs(yaw_deg) <= 45.0:
return (
yaw_deg,
pitch_deg,
residual,
upright,
branch_reason,
branch_travel,
other_travel,
)
rest_pitch = self._pitch_for(unit, tool)
limit = self._pitch_limit_effective()
rest_pitch = max(-limit, min(limit, rest_pitch))
rest_residual = self._residual(unit, 0.0, rest_pitch)
if rest_residual > CROWN_RESIDUAL_DEG:
return (
yaw_deg,
pitch_deg,
residual,
upright,
branch_reason,
branch_travel,
other_travel,
)
return (
0.0,
rest_pitch,
rest_residual,
self.uprightness(0.0, rest_pitch) >= 0.0,
"crown",
abs(rest_pitch),
abs(yaw_deg) + abs(pitch_deg),
)
# ------------------------------------------------------- closed-form solve
def alignment_branches(
@@ -547,7 +654,7 @@ class PlatformStack:
"""
unit = np.asarray(unit, dtype=np.float64)
tool = np.asarray(tool, dtype=np.float64)
pitch_stationary = self._rotation_coefficients(self.pitch_axis)[0]
pitch_stationary = self._signed_coefficients(self.pitch_axis)[0]
yaw_stationary = self._yaw_stationary
if pitch_stationary == yaw_stationary: # pragma: no cover - axes perpendicular
return []
@@ -571,7 +678,7 @@ class PlatformStack:
if any(abs(yaw_deg - prior) < 1e-6 for prior in seen):
continue
seen.add(yaw_deg)
yawed = rotation_about(self.yaw_axis, yaw_deg) @ unit
yawed = self.yaw_rotation(yaw_deg) @ unit
pitch_deg = self._pitch_for(yawed, tool)
branches.append(
Branch(
@@ -604,7 +711,7 @@ class PlatformStack:
to transcribe wrong.
"""
at_zero = np.asarray(unit, dtype=np.float64)
at_ninety = rotation_about(self.yaw_axis, 90.0) @ at_zero
at_ninety = self.yaw_rotation(90.0) @ at_zero
a = float(at_zero[pinned])
b = float(at_ninety[pinned])
@@ -641,7 +748,7 @@ class PlatformStack:
best: tuple[float, float, float] | None = None
step = max(SWEEP_STEP, (2.0 * self._yaw_limit_effective()) / max(samples - 1, 1))
for yaw_deg in np.arange(-self._yaw_limit_effective(), self._yaw_limit_effective() + step, step):
yawed = rotation_about(self.yaw_axis, float(yaw_deg)) @ unit
yawed = self.yaw_rotation(float(yaw_deg)) @ unit
pitch_deg = self._pitch_for(yawed, tool)
pitch_deg = max(-pitch_limit, min(pitch_limit, pitch_deg))
residual = self._residual(unit, float(yaw_deg), pitch_deg)
@@ -657,16 +764,19 @@ class PlatformStack:
two ``atan2`` arguments are the dot and cross of those two components
against the tool's.
"""
_, first, second, sign_first, sign_second = self._rotation_coefficients(
self.pitch_axis
)
u1, u2 = float(yawed[first]), float(yawed[second])
w1, w2 = float(tool[first]), float(tool[second])
# turned[first] = u1·c + sign_first ·u2·s
# turned[second] = sign_second·u1·s + u2·c
cosine_term = u1 * w1 + sign_second * u2 * w2
sine_term = sign_first * u2 * w1 - u1 * w2
return math.degrees(math.atan2(sine_term, cosine_term))
stationary = self._signed_coefficients(self.pitch_axis)[0]
vector = np.asarray(yawed, dtype=np.float64)
# R(θ) = cos(θ)·I + sin(θ)·R(90) on the plane this stage turns, so the
# angle that lands on the tool is the atan2 of those two probes. The
# probes are the commanded rotation, which is what keeps a sign change
# on the drive from needing its own copy of the matrix.
before = vector.copy()
after = self.pitch_rotation(90.0) @ vector
before[stationary] = 0.0
after[stationary] = 0.0
target = np.asarray(tool, dtype=np.float64).copy()
target[stationary] = 0.0
return math.degrees(math.atan2(float(after @ target), float(before @ target)))
def reachable(self, unit: np.ndarray, tool: np.ndarray) -> tuple[bool, str]:
"""Whether any pose can align this normal, and why not if none can.
@@ -693,15 +803,27 @@ class PlatformStack:
@property
def _yaw_stationary(self) -> int:
"""The component the yaw leaves alone (its own axis)."""
return self._rotation_coefficients(self.yaw_axis)[0]
return self._signed_coefficients(self.yaw_axis)[0]
@property
def _yaw_sign_first(self) -> float:
return self._rotation_coefficients(self.yaw_axis)[3]
return self._signed_coefficients(self.yaw_axis)[3]
@property
def _yaw_sign_second(self) -> float:
return self._rotation_coefficients(self.yaw_axis)[4]
return self._signed_coefficients(self.yaw_axis)[4]
def _signed_coefficients(
self, axis: str
) -> tuple[int, int, int, float, float]:
"""Right-hand coefficients with the drive's command sign folded in.
A command sign of ``-1`` flips the two sine terms, which is the same
rotation as feeding ``rotation_about`` the negated angle.
"""
stationary, first, second, sign_first, sign_second = self._rotation_coefficients(axis)
sign = self._command_sign(axis)
return stationary, first, second, sign_first * sign, sign_second * sign
def _rotation_coefficients(
self, axis: str
@@ -733,7 +855,7 @@ class PlatformStack:
yaw = math.radians(yaw_deg)
yaw_cos, yaw_sin = math.cos(yaw), math.sin(yaw)
yawed = [0.0, 0.0, 0.0]
stationary, first, second, sign_first, sign_second = self._rotation_coefficients(
stationary, first, second, sign_first, sign_second = self._signed_coefficients(
self.yaw_axis
)
yawed[stationary] = unit[stationary]
@@ -743,7 +865,7 @@ class PlatformStack:
pitch = math.radians(pitch_deg)
pitch_cos, pitch_sin = math.cos(pitch), math.sin(pitch)
turned = [0.0, 0.0, 0.0]
stationary, first, second, sign_first, sign_second = self._rotation_coefficients(
stationary, first, second, sign_first, sign_second = self._signed_coefficients(
self.pitch_axis
)
turned[stationary] = yawed[stationary]
@@ -777,7 +899,7 @@ class PlatformStack:
"""
angle = np.radians(np.asarray(angle_deg, dtype=np.float64))
vector = np.asarray(vector, dtype=np.float64)
stationary, first, second, sign_first, sign_second = self._rotation_coefficients(axis)
stationary, first, second, sign_first, sign_second = self._signed_coefficients(axis)
cosine, sine = np.cos(angle), np.sin(angle)
turned = np.empty((angle.size, 3), dtype=np.float64)
@@ -839,27 +961,20 @@ class PlatformStack:
``O(n_yaw)`` instead of ``O(n_yaw * n_pitch)``, which is the whole cost of
the exhaustive grid below.
The coefficients are written out by axis name rather than through the
``first``/``second`` rotation ordering, because it is the *cross* term
that carries the sign and getting that backwards returns the antipode —
an angle 180 deg away that looks entirely plausible. The signs here were
fixed against a direct numeric scan, and
``test_the_closed_form_pitch_beats_a_direct_scan`` keeps them that way.
The sine coefficient is the same probe :meth:`_pitch_for` uses: a
commanded 90° pitch. Writing it out per axis name returned the antipode
once the pitch axis moved off ``y``.
"""
tool = np.asarray(self.tool_direction, dtype=np.float64)
axis = self.pitch_axis
others = [name for name in ("x", "y", "z") if name != axis]
leading, trailing = others[0], others[1]
lead, trail = ("x", "y", "z").index(leading), ("x", "y", "z").index(trailing)
yawed = self._turn(self.yaw_axis, yaws, unit)
cosine_coefficient = (
yawed[:, lead] * tool[lead] + yawed[:, trail] * tool[trail]
)
sine_coefficient = (
yawed[:, trail] * tool[lead] - yawed[:, lead] * tool[trail]
)
return np.degrees(np.arctan2(sine_coefficient, cosine_coefficient))
stationary = self._signed_coefficients(self.pitch_axis)[0]
before = np.array(yawed, copy=True)
after = yawed @ self.pitch_rotation(90.0).T
before[:, stationary] = 0.0
after[:, stationary] = 0.0
target = np.array(tool, copy=True)
target[stationary] = 0.0
return np.degrees(np.arctan2(after @ target, before @ target))
def _grid_reference(
self, unit: np.ndarray, yaws: np.ndarray, pitches: np.ndarray
@@ -884,7 +999,7 @@ class PlatformStack:
yawed = self._turn(self.yaw_axis, yaws, unit)
pitch = np.radians(np.asarray(pitches, dtype=np.float64))
cosine, sine = np.cos(pitch), np.sin(pitch)
stationary, first, second, sign_first, sign_second = self._rotation_coefficients(
stationary, first, second, sign_first, sign_second = self._signed_coefficients(
self.pitch_axis
)
turned = np.empty_like(yawed)
@@ -1021,12 +1136,7 @@ class PlatformStack:
this way. One definition, so the two cannot disagree.
"""
mount = mount_rotation(self.axis_map)
return (
mount.T
@ rotation_about(self.pitch_axis, pitch_deg)
@ rotation_about(self.yaw_axis, yaw_deg)
@ mount
)
return mount.T @ self.platform_rotation(yaw_deg, pitch_deg) @ mount
def uprightness(self, yaw_deg: float, pitch_deg: float) -> float:
"""How much the sensing face still points up, in -1..1.
@@ -1236,11 +1346,7 @@ def residual_for(
normal = stack.normal_to_platform(np.asarray(normal_sensor, dtype=np.float64))
length = max(float(np.linalg.norm(normal)), 1e-12)
unit = normal / length
rotated = (
rotation_about(stack.pitch_axis, pitch_deg)
@ rotation_about(stack.yaw_axis, yaw_deg)
@ unit
)
rotated = stack.platform_rotation(yaw_deg, pitch_deg) @ unit
return float(
np.degrees(np.arccos(np.clip(float(rotated @ stack.tool_direction), -1.0, 1.0)))
)
apps/autocal/src/orisys_autocal/preferences.py
+11 -4
View File
@@ -248,14 +248,17 @@ def default_preferences() -> dict[str, Any]:
#: lifts the sensing face and does nothing else, and two fields for one
#: number drift apart. The bench plate under the finger is 8 mm thick.
"finger_sensor_offset_mm": [0.0, 0.0, 8.0],
# Tip points forward, away from the operator. 180° turned it back toward
# the operator; the preview then showed the green tip on the near side.
"finger_sensor_yaw_deg": 0.0,
#: Where the tool is, in the table frame, mm. A tool-tip offset belongs
#: here. The bench X shift is the initial position, not this number.
"finger_tool_xy_mm": [0.0, 3.0],
#: Where 「回初始位」 parks the table, mm. X is 1 mm toward the negative
#: direction. Y is 15.0 mm forward of the old zero.
"table_initial_x_mm": -1.0,
"table_initial_y_mm": 15.0,
#: Where 「回初始位」 parks the table, mm. After the power cycle the
#: coordinate origin sits on the previous park, so this is the origin:
#: one millimetre back in X and 15 mm back in Y from the old target.
"table_initial_x_mm": 0.0,
"table_initial_y_mm": 0.0,
#: Stage travel, degrees. Yaw turns ±180. Pitch is held to ±90 because the
#: fixture interferes outside that.
"finger_yaw_limit_deg": 180.0,
@@ -312,6 +315,10 @@ def load_preferences() -> dict[str, Any]:
if not offset[2]:
offset[2] = float(defaults["finger_sensor_offset_mm"][2])
prefs["finger_sensor_offset_mm"] = offset
# The mounted shell needs no extra yaw: the tip points forward, away from
# the operator. A running session still has 180° in memory and writes it
# back on close, which would turn the tip toward the operator again.
prefs["finger_sensor_yaw_deg"] = float(defaults["finger_sensor_yaw_deg"])
# The tool tip offset used to be a vector added to the reported coordinates
# while the drawing ignored it, so the picture and the numbers disagreed by
apps/autocal/src/orisys_autocal/sensor/export_bundle.py
+40 -2
View File
@@ -200,14 +200,21 @@ def load_bundle(
housing_vertices, housing_faces, part_names = _load_housing(root / "housing.npz", warnings)
camera_k, camera_size = _load_intrinsics(root / "camera_intrinsics.npz")
vertices_mm = vertices * mm_per_unit
housing_mm = housing_vertices * mm_per_unit
# The mesh zero sits near the tail. Move it onto the tail's mounting-face
# midpoint, which is the point aligned with the platform origin.
origin = housing_tail_origin_mm(housing_mm, vertices_mm)
vertices_mm = vertices_mm - origin
housing_mm = housing_mm - origin
return SensorBundle(
path=root,
vertices_mm=vertices * mm_per_unit,
vertices_mm=vertices_mm,
faces=faces,
uv_data=uv_data,
loop_to_vertex=loop_to_vertex,
housing_vertices_mm=housing_vertices * mm_per_unit,
housing_vertices_mm=housing_mm,
housing_faces=housing_faces,
housing_part_names=part_names,
camera_k=camera_k,
@@ -218,6 +225,37 @@ def load_bundle(
)
def housing_tail_origin_mm(housing_mm: np.ndarray, sensing_mm: np.ndarray) -> np.ndarray:
"""Mounting-face center of the housing end the sensing surface does not cover.
That end is the shell tail. Its base midpoint is the sensor origin: the
point placed on the platform's rotation axis.
"""
housing_mm = np.asarray(housing_mm, dtype=np.float64)
sensing_mm = np.asarray(sensing_mm, dtype=np.float64)
if housing_mm.size == 0 or sensing_mm.size == 0:
return np.zeros(3, dtype=np.float64)
depth = housing_mm[:, 2]
low = housing_mm[depth <= float(depth.min()) + 1.0]
high = housing_mm[depth >= float(depth.max()) - 1.0]
sense_depth = sensing_mm[:, 2]
low_gap = float(sense_depth.min() - low[:, 2].mean())
high_gap = float(high[:, 2].mean() - sense_depth.max())
end = low if low_gap >= high_gap else high
base_height = float(end[:, 1].min())
base = end[end[:, 1] <= base_height + 0.3]
if base.size == 0:
base = end
return np.array(
[
0.5 * (float(base[:, 0].min()) + float(base[:, 0].max())),
float(np.median(base[:, 1])),
float(np.median(base[:, 2])),
],
dtype=np.float64,
)
def mm_per_unit_from_pose(world_to_cam: np.ndarray) -> float:
"""The millimetres-per-mesh-unit factor, derived from the pose.
apps/autocal/src/orisys_autocal/sensor/frames.py
+52 -5
View File
@@ -16,14 +16,47 @@ from __future__ import annotations
import numpy as np
#: Which platform axis each sensor axis points along, and in which sign.
#: ``"z": "-y"`` keeps the frame right-handed: sensor (x, y, z) → platform
#: (x, z, -y), whose determinant is +1 (verified by a test).
AXIS_MAP: dict[str, str] = {"x": "x", "y": "z", "z": "-y"}
#: The bench (2026-10-15) showed both table commands reversed: a point further
#: along the shell (sensor +Z, toward the tip) must move the table toward
#: machine −Y, and a point on sensor +X must move it toward machine +X.
#: Sensor +Z is machine +Y and sensor +Y is machine +Z. Sensor +X is machine
#: −X, which is what keeps this a rotation (det +1) rather than a mirror.
#: Sensor (x, y, z) → platform (−x, z, y).
AXIS_MAP: dict[str, str] = {"x": "-x", "y": "z", "z": "y"}
#: Which platform axis each stage turns about. The stages are stacked on the XY
#: table, so both are expressed in the *platform* frame.
#:
#: The positive direction is the controller's, measured 2026-10-15 from the
#: commissioned zero (face up, tip toward platform +Y):
#:
#: * yaw (axis 2) about ``z``. ``+10°`` turned the tip clockwise viewed from
#: above, which is the opposite of the right-hand sense about ``+Z``.
#: * pitch (axis 3) about ``x``, the left-right axis. It is not a roll about
#: the tip axis. An executed pose matched the preview only with this sign
#: reversed, so positive pitch is the right-hand sense about ``+X``: the dome
#: nods toward the operator (platform ``−Y``).
#:
#: ``YAW_COMMAND_SIGN`` and ``PITCH_COMMAND_SIGN`` convert a commanded angle into
#: the right-hand angle :func:`orisys_autocal.motion.kinematics.rotation_about`
#: expects. The preview, the solver and the drive all use the commanded angle.
YAW_AXIS = "z"
PITCH_AXIS = "y"
YAW_COMMAND_SIGN = -1.0
PITCH_AXIS = "x"
PITCH_COMMAND_SIGN = 1.0
#: Model table X to the X motor. Yaw and pitch already match the drives, and Y
#: was checked (+5 mm toward the gantry). The X slide did not: cancelling the
#: yaw swing in model +X pushed the contact further off the probe. The preview
#: keeps the model move, so the drawn contact stays on the tool; only the number
#: sent to the drive, and the readout compared with the live axis, uses this sign.
TABLE_X_COMMAND_SIGN = -1.0
#: How much of the model X slide the motor should actually travel. On the two
#: side points already run (table X about 11 mm) the probe stopped about 1 mm
#: past the contact, on both sides, in the direction of that slide. 10/11 takes
#: that millimetre off. The preview still draws the full model slide.
TABLE_X_GAIN = 10.0 / 11.0
#: Platform axes, in the order the pose reports them.
PLATFORM_AXES: tuple[str, ...] = ("x", "y", "z")
@@ -81,8 +114,22 @@ def to_sensor(vectors_mm: np.ndarray, axis_map: dict[str, str] | None = None) ->
return array @ matrix
def machine_table_xy(table_xy_mm: tuple[float, float]) -> tuple[float, float]:
"""The table move the drives must make, in millimetres.
``table_xy_mm`` is the solver's move in the model frame. X is multiplied by
:data:`TABLE_X_COMMAND_SIGN` because the motor's positive direction is
opposite that frame, then by :data:`TABLE_X_GAIN` because that slide runs
about a millimetre past the real contact. Y is unchanged.
"""
return (
TABLE_X_COMMAND_SIGN * TABLE_X_GAIN * float(table_xy_mm[0]),
float(table_xy_mm[1]),
)
def describe(axis_map: dict[str, str] | None = None) -> str:
"""``sensor x→platform x, y→z, z→-y`` — shown in the UI so a flip is visible."""
"""``sensor x→-x, y→z, z→y`` — shown in the UI so a flip is visible."""
mapping = dict(axis_map or AXIS_MAP)
return "sensor " + ", ".join(
f"{axis}->{mapping.get(axis, '?')}" for axis in ("x", "y", "z")
apps/autocal/src/orisys_autocal/ui/multipoint_panel.py
+4 -2
View File
@@ -23,6 +23,7 @@ from PySide6.QtWidgets import (
from orisys_app_shared.i18n import t
from ..multipoint import Campaign, next_point_id
from ..sensor.frames import machine_table_xy
from ..sensor.uv_flat import site_at
#: A stored point and the mapping's answer for the same UV are the same point when
@@ -291,9 +292,10 @@ class MultipointPanel(QWidget):
@staticmethod
def _pose_of(solved, stack) -> dict:
"""The record of one solve: the targets, and what the solver said of them."""
move_x, move_y = machine_table_xy(solved.table_xy_mm)
return {
"x": float(solved.table_xy_mm[0]),
"y": float(solved.table_xy_mm[1]),
"x": float(move_x),
"y": float(move_y),
"yaw": float(solved.yaw_deg),
"pitch": float(solved.pitch_deg),
"z": float(solved.contact_z_mm),
apps/autocal/src/orisys_autocal/ui/sensor_panel.py
+58 -7
View File
@@ -48,6 +48,7 @@ from ..motion.kinematics import PlatformStack
from ..motion.pose_move import PressAxis, press_targets
from ..sensor.export_bundle import SensorBundle
from ..sensor.frames import describe as describe_frames
from ..sensor.frames import machine_table_xy
from ..sensor.uv_flat import FlatSurface, mapping_sha256, site_at, uv_origin
#: How many picked points the list keeps before the oldest is dropped.
@@ -377,6 +378,7 @@ class FingerPanel(QGroupBox):
"table_move",
"press",
"residual",
"match",
"frames",
)
for row, key in enumerate(rows):
@@ -386,7 +388,7 @@ class FingerPanel(QGroupBox):
value.setObjectName("statusValue")
value.setWordWrap(True)
value.setTextInteractionFlags(Qt.TextInteractionFlag.TextSelectableByMouse)
# These values are long ("sensor x->x, y->z, z->-y"). Allowed to drive
# These values are long ("sensor x->-x, y->z, z->y"). Allowed to drive
# their own width they claimed most of the row and squeezed the UV
# picker beside them down to a thumbnail; wrapping keeps them readable
# at whatever width the row can spare.
@@ -771,7 +773,7 @@ class FingerPanel(QGroupBox):
pose = self._last_pose
if pose is None:
return None
table_x, table_y = pose.table_xy_mm
table_x, table_y = machine_table_xy(pose.table_xy_mm)
targets = {
"x": float(table_x),
"y": float(table_y),
@@ -868,6 +870,7 @@ class FingerPanel(QGroupBox):
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)
move_x, move_y = machine_table_xy(pose.table_xy_mm)
points.append({
"site_id": f"site-{index:03d}",
"x": float(point[0]),
@@ -879,8 +882,8 @@ class FingerPanel(QGroupBox):
"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]),
"x": float(move_x),
"y": float(move_y),
"yaw": float(pose.yaw_deg),
"pitch": float(pose.pitch_deg),
"z": float(pose.contact_z_mm),
@@ -1246,6 +1249,7 @@ class FingerPanel(QGroupBox):
_last_pose = None
_last_face = -1
_actual_pose: tuple[float, float, float, float] | None = None
def refresh_readouts(self, point=None, normal=None, pose=None, face: int = -1) -> None:
self._readout_labels["frames"].setText(describe_frames(self._stack.axis_map))
@@ -1278,10 +1282,9 @@ class FingerPanel(QGroupBox):
self._readout_labels["platform_point"].setText(
" ".join(f"{value:+.3f}" for value in platform) + " mm"
)
move_x, move_y = (0.0, 0.0) if pose is None else machine_table_xy(pose.table_xy_mm)
self._readout_labels["table_move"].setText(
"—"
if pose is None
else f"{pose.table_xy_mm[0]:+.3f} {pose.table_xy_mm[1]:+.3f} mm"
"—" if pose is None else f"{move_x:+.3f} {move_y:+.3f} mm"
)
if pose is not None:
self._readout_labels["pose"].setText(pose.describe())
@@ -1298,8 +1301,55 @@ class FingerPanel(QGroupBox):
),
)
)
self._refresh_match()
del face
def set_actual_pose(
self, pose: tuple[float, float, float, float] | None
) -> None:
"""Live platform pose, ``(x_mm, y_mm, yaw_deg, pitch_deg)``, or none."""
self._actual_pose = pose
self._refresh_match()
def _refresh_match(self) -> None:
"""Selected pose against the live axes. The tail origin is on the platform axis."""
label = self._readout_labels.get("match")
if label is None:
return
pose = self._last_pose
actual = self._actual_pose
if pose is None:
label.setText(t("finger.match_none"))
return
if actual is None:
label.setText(t("finger.match_offline"))
return
move_x, move_y = machine_table_xy(pose.table_xy_mm)
selected = (
float(move_x),
float(move_y),
float(pose.yaw_deg),
float(pose.pitch_deg),
)
delta = tuple(actual[index] - selected[index] for index in range(4))
label.setText(
t(
"finger.match_value",
sx=f"{selected[0]:+.3f}",
sy=f"{selected[1]:+.3f}",
syaw=f"{selected[2]:+.3f}",
spitch=f"{selected[3]:+.3f}",
ax=f"{actual[0]:+.3f}",
ay=f"{actual[1]:+.3f}",
ayaw=f"{actual[2]:+.3f}",
apitch=f"{actual[3]:+.3f}",
dx=f"{delta[0]:+.3f}",
dy=f"{delta[1]:+.3f}",
dyaw=f"{delta[2]:+.3f}",
dpitch=f"{delta[3]:+.3f}",
)
)
def _refresh_path(self) -> None:
"""Report what the planned motion does, and where it actually arrives.
@@ -1377,6 +1427,7 @@ class FingerPanel(QGroupBox):
"table_move": "finger.row_table_move",
"press": "finger.row_press",
"residual": "finger.row_residual",
"match": "finger.row_match",
"frames": "finger.row_frames",
}
grid = self._readout_box.layout()
apps/autocal/src/orisys_autocal/ui/window.py
+35 -2
View File
@@ -10,6 +10,7 @@ the ctypes handle single-threaded.
from __future__ import annotations
import logging
import math
import subprocess
import sys
from pathlib import Path
@@ -47,7 +48,7 @@ from ..motion.pose_move import (
press_targets,
)
from ..motion.service import clamp_bas_task
from ..motion.units import DEFAULT_UNITS
from ..motion.units import DEFAULT_UNITS, user_units_to_deg, user_units_to_mm
from ..preferences import load_preferences, save_preferences
from ..sensor import (
BundleError,
@@ -869,6 +870,36 @@ class AutoCalMainWindow(QMainWindow):
"""Signal handler for the poller's periodic state read."""
self.update_snapshot(snapshot)
def _report_point_match(self, snapshot: Snapshot) -> None:
"""Selected pose versus the live axes. Offsets are actual minus selected."""
if not snapshot.connected:
self.finger_panel.set_actual_pose(None)
return
roles = self.prefs.get("axis_roles") or {}
def reading(role: str, rotary: bool) -> float | None:
axis = roles.get(role)
if axis is None:
return None
state = snapshot.axis(int(axis))
if state is None or not state.read_ok:
return None
if rotary:
return user_units_to_deg(int(axis), state.dpos)
units = state.units or float(self.prefs.get("units_per_mm") or DEFAULT_UNITS)
return user_units_to_mm(state.dpos, units=units)
values = (
reading("x", False),
reading("y", False),
reading("yaw", True),
reading("pitch", True),
)
if any(value is None or not math.isfinite(value) for value in values):
self.finger_panel.set_actual_pose(None)
return
self.finger_panel.set_actual_pose(tuple(float(value) for value in values)) # type: ignore[arg-type]
def update_snapshot(self, snapshot: Snapshot) -> None:
self._last_snapshot = snapshot
self.connection_panel.set_connected(
@@ -894,6 +925,7 @@ class AutoCalMainWindow(QMainWindow):
self.finger_panel.set_move_available(
self._move_available(), moving=self.mover.running
)
self._report_point_match(snapshot)
# The strip is outside the tabs, so it is fed on every poll regardless
# of which tab is showing.
self.status_strip.set_snapshot(snapshot)
@@ -1103,7 +1135,8 @@ class AutoCalMainWindow(QMainWindow):
when the bench has measured how its coordinate relates to a contact
height; otherwise the panel hands back that role as held, so the move
carries the roles it can justify and says which one it left alone. The
numbers in the confirmation are the ones the preview drew.
confirmation shows those drive targets. Table X carries the motor sign;
the preview keeps the model slide, which is what puts the contact on the tool.
"""
solve = self.finger_panel.solved_pose()
if solve is None:
apps/autocal/tests/test_finger_panel.py
+12 -15
View File
@@ -169,19 +169,14 @@ def test_the_initial_view_is_the_commissioned_orientation(qt_app, bundle):
f"sensor +Y must point up the screen, but moved {tip[1] - origin[1]:+.1f} px"
)
# Sensor +Z is the sensing face's own axis and must face the viewer. It is
# the platform's -Y on this mount. ``forward`` points from the eye into the
# scene, so a surface normal aimed *at* the camera has a positive dot with
# it — the opposite of what the depth term in a projection does, and the
# sign this assertion got wrong first time.
# The commissioned camera sits in front of the shell and a little below
# it, looking back toward platform −Y. That is the ad049d12 preview.
eye, _distance = preview._camera() # noqa: SLF001
centre = preview._view_target() # noqa: SLF001
_centre, forward, _right, _up = preview._view_basis(eye) # noqa: SLF001
lens_axis = to_sensor(np.array([0.0, -1.0, 0.0]), stack.axis_map)
lens_axis = lens_axis / np.linalg.norm(lens_axis)
assert float(lens_axis @ forward) > 0.2, (
f"sensor +Z must face the viewer, but points away: "
f"{float(lens_axis @ forward):+.3f}"
)
assert eye[1] > centre[1]
assert eye[2] < centre[2]
assert forward[1] < -0.5, f"the view must look toward −Y, but forward is {forward}"
finally:
preview.deleteLater()
@@ -1253,8 +1248,8 @@ def test_the_tool_position_sets_the_move_and_not_the_reading(panel):
def test_the_readouts_name_the_frame_mapping(panel):
"""The one line the bench will want to check when x and y are interchanged."""
text = panel._readout_labels["frames"].text() # noqa: SLF001
assert "x" in text and "z" in text
assert "-y" in text or "−y" in text
assert "x->-x" in text
assert "z->y" in text
def test_the_readout_names_the_chosen_pose_and_the_key_that_chose_it(panel):
@@ -1271,7 +1266,7 @@ def test_the_readout_names_the_chosen_pose_and_the_key_that_chose_it(panel):
pose = panel._last_pose # noqa: SLF001
text = panel._readout_labels["branch"].text() # noqa: SLF001
assert pose.branch_reason in {"only", "alignment", "in_range", "upright", "smallest"}
assert pose.branch_reason in {"only", "alignment", "in_range", "upright", "smallest", "crown"}
assert t(f"finger.branch_{pose.branch_reason}") in text
# And the alternative's cost is on screen too, not just the chosen one's.
assert f"{pose.other_travel_deg:.1f}" in text
@@ -1294,7 +1289,9 @@ def test_the_press_readout_says_why_it_is_held_and_then_names_the_target(panel):
targets, held = panel.solved_pose()
assert held == ("z",)
assert "z" not in targets
assert targets["x"] == pytest.approx(pose.table_xy_mm[0])
from orisys_autocal.sensor.frames import TABLE_X_GAIN
assert targets["x"] == pytest.approx(-TABLE_X_GAIN * pose.table_xy_mm[0])
# With the bench's zero entered, the same contact height becomes a coordinate.
panel.set_press(PressAxis(zero_mm=2.0, rising=True, calibrated=True))
apps/autocal/tests/test_ik_analytical.py
+5 -10
View File
@@ -39,12 +39,8 @@ def _random_units(count: int, seed: int) -> list[np.ndarray]:
def _apply(stack: PlatformStack, unit: np.ndarray, yaw_deg: float, pitch_deg: float) -> np.ndarray:
"""Re-apply a returned pose with the library's own rotations."""
return (
rotation_about(stack.pitch_axis, pitch_deg)
@ rotation_about(stack.yaw_axis, yaw_deg)
@ unit
)
"""Re-apply a returned pose with the same rotation the drives execute."""
return stack.platform_rotation(yaw_deg, pitch_deg) @ unit
# ------------------------------------------------------------------ exactness
@@ -108,11 +104,10 @@ def test_the_two_methods_agree_inside_the_envelope():
def test_the_two_pitch_bindings_are_the_same_family():
"""Confirmed at the bench: a 90 degree yaw takes one binding to the other.
"""``R_y(p)·R_z(t)`` and ``R_x(p)·R_z(t − 90)`` reach the same alignments.
``R_y(p)·R_z(t)`` and ``R_x(p)·R_z(t − 90)`` agree to machine precision. So
which axis is called pitch is a naming and zero-offset choice, not a
correctness question — the two reach exactly the same set of alignments.
This is a property of the right-hand matrices. The drive does not get to
pick either binding: pitch is about ``x`` with the measured command sign.
"""
unit = np.array([0.3, 0.5, -0.81])
unit = unit / np.linalg.norm(unit)
apps/autocal/tests/test_kinematics.py
+132 -50
View File
@@ -59,9 +59,9 @@ def _brute_force_best(
pitch_limit = min(360.0, stack.pitch_limit_deg)
best = 1e9
for yaw in np.arange(-yaw_limit, yaw_limit + step, step):
turned = rotation_about(stack.yaw_axis, float(yaw)) @ unit_platform
turned = stack.yaw_rotation(float(yaw)) @ unit_platform
for pitch in np.arange(-pitch_limit, pitch_limit + step, step):
rotated = rotation_about(stack.pitch_axis, float(pitch)) @ turned
rotated = stack.pitch_rotation(float(pitch)) @ turned
residual = float(np.degrees(np.arccos(np.clip(rotated @ tool, -1.0, 1.0))))
best = min(best, residual)
return best
@@ -77,13 +77,78 @@ def test_the_axis_map_is_a_rotation():
assert np.allclose(matrix @ matrix.T, np.eye(3))
def test_a_crown_point_moves_along_y_and_does_not_yaw():
"""The top face used to be a Y move. A 1.6° sideways lean must not yaw 90°."""
stack = PlatformStack(sensor_offset_mm=(0.0, 0.0, 8.0), pitch_limit_deg=90.0)
point = np.array([0.150, 8.355, 5.483])
normal = np.array([0.028, 1.0, 0.0])
normal = normal / np.linalg.norm(normal)
pose = stack.solve(point, normal)
assert pose.yaw_deg == pytest.approx(0.0, abs=1e-6)
assert pose.pitch_deg == pytest.approx(0.0, abs=1e-6)
assert pose.table_xy_mm[1] == pytest.approx(-5.483, abs=0.02)
assert abs(pose.table_xy_mm[0]) < 0.5
assert pose.branch_reason == "crown"
assert pose.reachable
assert pose.reason == ""
def test_a_sideways_normal_still_yaws():
"""A lean the pitch stage cannot nod away still takes the exact yaw."""
stack = PlatformStack()
normal = np.array([0.7, 0.7, 0.1])
normal = normal / np.linalg.norm(normal)
pose = stack.solve(np.zeros(3), normal)
assert pose.residual_deg < 1e-4
assert abs(pose.yaw_deg) > 20.0
assert pose.branch_reason != "crown"
def test_positive_stage_commands_match_the_drives():
"""Bench 2026-10-15, one axis at a time from the commissioned zero.
Yaw +10° turned the tip clockwise viewed from above. Positive pitch nods the
dome toward the operator (platform −Y). The preview uses this same rotation,
so a commanded angle draws the motion the drive will make.
"""
stack = PlatformStack()
assert stack.yaw_axis == "z" and stack.pitch_axis == "x"
assert np.allclose(stack.platform_rotation(0.0, 0.0), np.eye(3))
tip = stack.platform_rotation(10.0, 0.0) @ np.array([0.0, 1.0, 0.0])
# Clockwise from above: the tip, pointing at +Y, swings toward +X.
assert tip[0] > 0.0
assert tip[1] > 0.0
assert abs(tip[2]) < 1e-12
dome = stack.platform_rotation(0.0, 10.0) @ np.array([0.0, 0.0, 1.0])
# A nod, not a roll: the face leans toward −Y and does not pick up +X.
assert dome[1] < 0.0
assert abs(dome[0]) < 1e-12
assert dome[2] > 0.0
def test_the_sensor_y_maps_to_the_platform_z():
"""The bench's frame relation, stated once so it can be corrected once."""
assert np.allclose(to_platform(np.array([0.0, 1.0, 0.0])), [0.0, 0.0, 1.0])
def test_the_sensor_x_maps_to_the_platform_x():
assert np.allclose(to_platform(np.array([1.0, 0.0, 0.0])), [1.0, 0.0, 0.0])
def test_the_sensor_x_maps_to_the_platform_minus_x():
"""Both table axes were reversed on the bench. −X keeps the map a rotation."""
assert np.allclose(to_platform(np.array([1.0, 0.0, 0.0])), [-1.0, 0.0, 0.0])
def test_the_sensor_z_maps_to_the_platform_y():
"""Shell length and machine Y point the same way, so the tip is +Y."""
assert np.allclose(to_platform(np.array([0.0, 0.0, 1.0])), [0.0, 1.0, 0.0])
def test_a_tip_side_point_commands_negative_table_y():
"""Bringing a point toward the tip under the tool moves the table to −Y."""
pose = PlatformStack().solve(np.array([0.0, 8.0, 6.0]), np.array([0.0, 1.0, 0.0]))
assert pose.table_xy_mm[1] == pytest.approx(-6.0)
side = PlatformStack().solve(np.array([2.0, 8.0, 0.0]), np.array([0.0, 1.0, 0.0]))
assert side.table_xy_mm[0] == pytest.approx(2.0)
def test_the_mount_round_trips():
@@ -105,7 +170,7 @@ def test_an_incomplete_map_is_refused():
def test_the_mapping_is_described_for_the_ui():
assert describe() == "sensor x->x, y->z, z->-y"
assert describe() == "sensor x->-x, y->z, z->y"
def test_swapping_the_mapping_changes_the_answer():
@@ -155,9 +220,7 @@ def test_the_nominal_normal_needs_no_rotation_and_lands_on_plus_z():
unit = unit / np.linalg.norm(unit)
pose = stack.solve(np.array([0.0, 5.0, 0.0]), nominal)
landed = rotation_about(stack.pitch_axis, pose.pitch_deg) @ rotation_about(
stack.yaw_axis, pose.yaw_deg
) @ unit
landed = stack.platform_rotation(pose.yaw_deg, pose.pitch_deg) @ unit
landed = landed / np.linalg.norm(landed)
assert landed[2] > 0.0, f"the face was aimed at {np.round(landed, 3)}, not +Z"
# And it gets there without turning: the commissioned pose is the identity.
@@ -241,7 +304,7 @@ def test_the_closed_form_pitch_beats_a_direct_scan():
tool = stack.tool_direction
cosine, sine = np.cos(np.radians(scan)), np.sin(np.radians(scan))
stationary, first, second, sign_first, sign_second = (
stack._rotation_coefficients(stack.pitch_axis) # noqa: SLF001
stack._signed_coefficients(stack.pitch_axis) # noqa: SLF001
)
dot = (
yawed[stationary] * tool[stationary]
@@ -262,28 +325,11 @@ def _independent_residual(
"""The residual worked out from raw matrices built here, not by the solver.
``residual_for`` shares ``rotation_about`` with the solver, so it cannot catch
an error in that shared helper. This builds the two 3x3 matrices from the
rotation formulas directly, so the two paths agree only if both are right.
an error in that shared helper. The matrices live in ``_independent_rotation``.
"""
normal = stack.normal_to_platform(np.asarray(normal_sensor, dtype=np.float64))
unit = normal / np.linalg.norm(normal)
yaw, pitch = np.radians(yaw_deg), np.radians(pitch_deg)
about_yaw = np.array(
[
[np.cos(yaw), -np.sin(yaw), 0.0],
[np.sin(yaw), np.cos(yaw), 0.0],
[0.0, 0.0, 1.0],
]
)
about_pitch = np.array(
[
[np.cos(pitch), 0.0, np.sin(pitch)],
[0.0, 1.0, 0.0],
[-np.sin(pitch), 0.0, np.cos(pitch)],
]
)
turned = about_pitch @ about_yaw @ unit
turned = _independent_rotation(yaw_deg, pitch_deg) @ unit
return float(
np.degrees(np.arccos(np.clip(float(turned @ stack.tool_direction), -1.0, 1.0)))
)
@@ -368,10 +414,10 @@ def test_the_full_yaw_travel_keeps_the_second_solution_of_a_measured_point():
"""±180 is what keeps "cannot reach" from being reported for an aligned pose.
``docs/motion-correction-plan.zh.md`` measures a point at (-2.698, 5.186, 18.228)
with normal (-0.343, 0.699, 0.627). It has two exact solutions — -61.319°/+45.636°
and +118.681°/-45.636° — and the second sits outside the ±90 note this bench was
once configured with. At ±180 both branches are reachable, so a change in which
branch is preferred cannot strand the point on "cannot reach".
with normal (-0.343, 0.699, 0.627). Under the drive's rotation it has two exact
solutions, -28.681° and +151.319° of yaw, and the second sits outside the ±90
note this bench was once configured with. At ±180 both branches are reachable,
so a change in which branch is preferred cannot strand the point on "cannot reach".
"""
point = np.array([-2.698, 5.186, 18.228])
normal = np.array([-0.343, 0.699, 0.627])
@@ -383,7 +429,7 @@ def test_the_full_yaw_travel_keeps_the_second_solution_of_a_measured_point():
wide_branches = wide.alignment_branches(unit, wide.tool_direction)
narrow_branches = narrow.alignment_branches(unit, narrow.tool_direction)
assert {round(branch.yaw_deg, 3) for branch in wide_branches} == {-61.319, 118.681}
assert {round(branch.yaw_deg, 3) for branch in wide_branches} == {-28.681, 151.319}
assert all(branch.in_range for branch in wide_branches)
assert sum(branch.in_range for branch in narrow_branches) == 1, (
"±90 was expected to drop one of the two exact solutions"
@@ -493,6 +539,20 @@ def test_the_default_tool_is_above_the_table_origin():
assert pose.table_xy_mm[1] == pytest.approx(-pose.target_xy_mm[1])
def test_the_drive_x_is_opposite_the_model_slide_and_a_little_shorter():
"""The X motor is opposite the model, and the slide is 10/11 of it.
The preview keeps the full model slide, so the drawn contact stays on the tool.
"""
from orisys_autocal.sensor.frames import TABLE_X_GAIN, machine_table_xy
pose = PlatformStack().solve(np.array([3.0, 8.0, 1.0]), np.array([0.0, 1.0, 0.0]))
drive_x, drive_y = machine_table_xy(pose.table_xy_mm)
assert drive_x == pytest.approx(-TABLE_X_GAIN * pose.table_xy_mm[0])
assert drive_y == pytest.approx(pose.table_xy_mm[1])
assert f"{drive_x:+.3f}" in pose.describe()
def test_the_table_move_lands_the_contact_on_the_tool():
"""The one invariant the whole machine exists to satisfy."""
stack = PlatformStack(
@@ -531,12 +591,11 @@ def test_the_mounting_offset_moves_the_point_and_the_mounting_yaw_the_whole_fram
moved = shifted.solve(point, normal)
assert moved.target_xy_mm[0] - base.target_xy_mm[0] == pytest.approx(1.5)
assert moved.target_xy_mm[1] - base.target_xy_mm[1] == pytest.approx(-2.5)
# An in-plane offset leaves the *mounting* height alone, but not the height the
# stages present: the same angles now lift a contact that sits further from the
# trunnion axis. This line used to compare contact_z_mm on both sides, which
# only held while that field was reported before the rotation.
# An in-plane offset leaves the mounting height alone. The stages then present
# a different height, because the same angles tilt a contact that sits somewhere
# else relative to the pitch axis.
assert moved.mounted_point_mm[2] == pytest.approx(base.mounted_point_mm[2])
assert moved.contact_z_mm - base.contact_z_mm > 0.1
assert abs(moved.contact_z_mm - base.contact_z_mm) > 0.1
turned = dataclasses.replace(stack, sensor_yaw_deg=35.0)
yawed = turned.solve(point, normal)
@@ -546,9 +605,19 @@ def test_the_mounting_offset_moves_the_point_and_the_mounting_yaw_the_whole_fram
)
assert yawed.contact_z_mm == pytest.approx(base.contact_z_mm)
# The mounting yaw is absorbed by the stage: it has to turn that much less.
difference = (base.yaw_deg - yawed.yaw_deg) % 360.0
assert min(difference, 360.0 - difference) == pytest.approx(35.0, abs=1e-9)
assert yawed.pitch_deg == pytest.approx(base.pitch_deg)
# Mounting yaw is right-handed about Z. Positive stage yaw is the opposite
# sense, so the same-pitch branch's command rises by the mounting angle.
# The shorter branch may be the other one, a half-turn away.
unit = turned.setup_normal(normal)
unit = unit / float(np.linalg.norm(unit))
same_pitch = [
branch
for branch in turned.alignment_branches(unit, turned.tool_direction)
if branch.pitch_deg == pytest.approx(base.pitch_deg)
]
assert len(same_pitch) == 1
shift = (same_pitch[0].yaw_deg - base.yaw_deg + 180.0) % 360.0 - 180.0
assert shift == pytest.approx(35.0, abs=1e-9)
assert yawed.residual_deg == pytest.approx(base.residual_deg, abs=1e-9)
@@ -574,12 +643,22 @@ def test_the_setup_z_is_the_mounting_plate():
)
def test_the_bench_mount_points_the_tip_forward():
"""Yaw 180 put the tip on the near side. Zero yaw points it away."""
stack = PlatformStack(sensor_yaw_deg=0.0)
tip = stack.setup_point(np.array([0.0, 0.0, 1.0]))
cable = stack.setup_point(np.array([0.0, 0.0, -1.0]))
assert tip[:2] == pytest.approx((0.0, 1.0))
assert cable[:2] == pytest.approx((0.0, -1.0))
assert stack.setup_point(np.array([0.0, 1.0, 0.0]))[2] == pytest.approx(1.0)
def test_the_setup_normal_is_the_mounting_yaw_applied_to_the_mount():
stack = PlatformStack(sensor_yaw_deg=90.0)
normal = np.array([1.0, 0.0, 0.0])
# Sensor +x is platform +x; a 90 degree mounting yaw sends it to platform +y.
assert np.allclose(stack.setup_normal(normal), [0.0, 1.0, 0.0], atol=1e-12)
assert stack.normal_to_platform(normal) == pytest.approx([1.0, 0.0, 0.0])
# Sensor +x is platform −x; a +90° mounting yaw about Z sends it to platform −y.
assert np.allclose(stack.setup_normal(normal), [0.0, -1.0, 0.0], atol=1e-12)
assert stack.normal_to_platform(normal) == pytest.approx([-1.0, 0.0, 0.0])
def test_a_non_finite_sensor_yaw_is_refused():
@@ -625,20 +704,23 @@ def test_an_aligned_pose_reports_that_it_reached_tolerance():
def _independent_rotation(yaw_deg: float, pitch_deg: float) -> np.ndarray:
"""``R_pitch @ R_yaw`` from the formulas, not through ``rotation_about``."""
"""``R_pitch @ R_yaw`` for the drive directions, not through ``rotation_about``.
Positive yaw is clockwise from above. Positive pitch nods ``+Z`` toward ``−Y``.
"""
yaw, pitch = np.radians(yaw_deg), np.radians(pitch_deg)
about_yaw = np.array(
[
[np.cos(yaw), -np.sin(yaw), 0.0],
[np.sin(yaw), np.cos(yaw), 0.0],
[np.cos(yaw), np.sin(yaw), 0.0],
[-np.sin(yaw), np.cos(yaw), 0.0],
[0.0, 0.0, 1.0],
]
)
about_pitch = np.array(
[
[np.cos(pitch), 0.0, np.sin(pitch)],
[0.0, 1.0, 0.0],
[-np.sin(pitch), 0.0, np.cos(pitch)],
[1.0, 0.0, 0.0],
[0.0, np.cos(pitch), -np.sin(pitch)],
[0.0, np.sin(pitch), np.cos(pitch)],
]
)
return about_pitch @ about_yaw
apps/autocal/tests/test_sensor_bundle.py
+24
View File
@@ -17,6 +17,7 @@ import numpy as np
import pytest
from orisys_autocal.sensor.export_bundle import (
BundleError,
housing_tail_origin_mm,
load_bundle,
mm_per_unit_from_pose,
)
@@ -116,6 +117,29 @@ def test_the_surface_area_is_physically_plausible(bundle):
assert bundle.surface_area_mm2 == pytest.approx(355.77, abs=1.0)
def test_the_origin_is_the_unsensed_housing_tail():
"""The tail is the housing end the sensing patch does not cover."""
housing = np.array(
[
[-2.0, 0.0, 0.0],
[2.0, 0.0, 0.0],
[0.0, 4.0, 0.2],
[-1.0, 0.1, 20.0],
[1.0, 0.1, 20.2],
]
)
sensing = np.array([[0.0, 2.0, 5.0], [0.0, 2.0, 19.8]])
origin = housing_tail_origin_mm(housing, sensing)
assert origin[0] == pytest.approx(0.0)
assert origin[1] == pytest.approx(0.0)
assert origin[2] == pytest.approx(0.0)
def test_a_loaded_bundle_puts_the_tail_on_the_origin(bundle):
origin = housing_tail_origin_mm(bundle.housing_vertices_mm, bundle.vertices_mm)
assert origin == pytest.approx(np.zeros(3), abs=1e-6)
def test_the_stored_normals_are_coherent(bundle):
"""Mixed winding would make 'the normal' meaningless."""
normals = bundle.face_normals
apps/autocal/tools/layout_mockup.py
+2 -2
View File
@@ -193,7 +193,7 @@ STRINGS = {
["位姿 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],
["坐标系映射", "x→-x, y→z, z→y", None],
],
"finger_tool_label": "刀具 X / Y",
"finger_tool_values": ["0.000", "0.000"],
@@ -349,7 +349,7 @@ STRINGS = {
["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],
["Frame map", "x→-x, y→z, z→y", None],
],
"finger_tool_label": "Tool X / Y",
"finger_tool_values": ["0.000", "0.000"],