From 2a154307fd65bec84e248c86a2f863e738bc75bb Mon Sep 17 00:00:00 2001 From: yijian Date: Thu, 15 Oct 2026 17:13:04 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BC=98=E5=8C=96=E4=BC=A0=E6=84=9F=E5=99=A8?= =?UTF-8?q?=E5=AF=B9=E5=87=86=E5=8E=8B=E5=A4=B4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- apps/autocal/config/locales/en_US.json | 7 +- apps/autocal/config/locales/zh_CN.json | 7 +- apps/autocal/docs/inverse-kinematics.md | 23 +- .../src/orisys_autocal/motion/kinematics.py | 226 +++++++++++++----- .../autocal/src/orisys_autocal/preferences.py | 15 +- .../orisys_autocal/sensor/export_bundle.py | 42 +++- .../src/orisys_autocal/sensor/frames.py | 57 ++++- .../src/orisys_autocal/ui/multipoint_panel.py | 6 +- .../src/orisys_autocal/ui/sensor_panel.py | 65 ++++- apps/autocal/src/orisys_autocal/ui/window.py | 37 ++- apps/autocal/tests/test_finger_panel.py | 27 +-- apps/autocal/tests/test_ik_analytical.py | 15 +- apps/autocal/tests/test_kinematics.py | 182 ++++++++++---- apps/autocal/tests/test_sensor_bundle.py | 24 ++ apps/autocal/tools/layout_mockup.py | 4 +- 15 files changed, 565 insertions(+), 172 deletions(-) diff --git a/apps/autocal/config/locales/en_US.json b/apps/autocal/config/locales/en_US.json index 2ad5857..21f49ef 100644 --- a/apps/autocal/config/locales/en_US.json +++ b/apps/autocal/config/locales/en_US.json @@ -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", diff --git a/apps/autocal/config/locales/zh_CN.json b/apps/autocal/config/locales/zh_CN.json index 46e86ff..61269fc 100644 --- a/apps/autocal/config/locales/zh_CN.json +++ b/apps/autocal/config/locales/zh_CN.json @@ -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", diff --git a/apps/autocal/docs/inverse-kinematics.md b/apps/autocal/docs/inverse-kinematics.md index 1fbf5da..22a69d1 100644 --- a/apps/autocal/docs/inverse-kinematics.md +++ b/apps/autocal/docs/inverse-kinematics.md @@ -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 diff --git a/apps/autocal/src/orisys_autocal/motion/kinematics.py b/apps/autocal/src/orisys_autocal/motion/kinematics.py index 9f0df60..447ed36 100644 --- a/apps/autocal/src/orisys_autocal/motion/kinematics.py +++ b/apps/autocal/src/orisys_autocal/motion/kinematics.py @@ -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))) ) diff --git a/apps/autocal/src/orisys_autocal/preferences.py b/apps/autocal/src/orisys_autocal/preferences.py index 32c988f..0ec444e 100644 --- a/apps/autocal/src/orisys_autocal/preferences.py +++ b/apps/autocal/src/orisys_autocal/preferences.py @@ -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 diff --git a/apps/autocal/src/orisys_autocal/sensor/export_bundle.py b/apps/autocal/src/orisys_autocal/sensor/export_bundle.py index 6a0bb79..783528c 100644 --- a/apps/autocal/src/orisys_autocal/sensor/export_bundle.py +++ b/apps/autocal/src/orisys_autocal/sensor/export_bundle.py @@ -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. diff --git a/apps/autocal/src/orisys_autocal/sensor/frames.py b/apps/autocal/src/orisys_autocal/sensor/frames.py index 33e8a15..1b0e847 100644 --- a/apps/autocal/src/orisys_autocal/sensor/frames.py +++ b/apps/autocal/src/orisys_autocal/sensor/frames.py @@ -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") diff --git a/apps/autocal/src/orisys_autocal/ui/multipoint_panel.py b/apps/autocal/src/orisys_autocal/ui/multipoint_panel.py index 90a9c12..1f75b22 100644 --- a/apps/autocal/src/orisys_autocal/ui/multipoint_panel.py +++ b/apps/autocal/src/orisys_autocal/ui/multipoint_panel.py @@ -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), diff --git a/apps/autocal/src/orisys_autocal/ui/sensor_panel.py b/apps/autocal/src/orisys_autocal/ui/sensor_panel.py index 492f332..095c389 100644 --- a/apps/autocal/src/orisys_autocal/ui/sensor_panel.py +++ b/apps/autocal/src/orisys_autocal/ui/sensor_panel.py @@ -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() diff --git a/apps/autocal/src/orisys_autocal/ui/window.py b/apps/autocal/src/orisys_autocal/ui/window.py index 38471e7..689fe82 100644 --- a/apps/autocal/src/orisys_autocal/ui/window.py +++ b/apps/autocal/src/orisys_autocal/ui/window.py @@ -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: diff --git a/apps/autocal/tests/test_finger_panel.py b/apps/autocal/tests/test_finger_panel.py index 012aba7..712f968 100644 --- a/apps/autocal/tests/test_finger_panel.py +++ b/apps/autocal/tests/test_finger_panel.py @@ -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)) diff --git a/apps/autocal/tests/test_ik_analytical.py b/apps/autocal/tests/test_ik_analytical.py index e27a406..549a920 100644 --- a/apps/autocal/tests/test_ik_analytical.py +++ b/apps/autocal/tests/test_ik_analytical.py @@ -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) diff --git a/apps/autocal/tests/test_kinematics.py b/apps/autocal/tests/test_kinematics.py index 1e2050d..abecccb 100644 --- a/apps/autocal/tests/test_kinematics.py +++ b/apps/autocal/tests/test_kinematics.py @@ -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 diff --git a/apps/autocal/tests/test_sensor_bundle.py b/apps/autocal/tests/test_sensor_bundle.py index d765784..35fbf1c 100644 --- a/apps/autocal/tests/test_sensor_bundle.py +++ b/apps/autocal/tests/test_sensor_bundle.py @@ -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 diff --git a/apps/autocal/tools/layout_mockup.py b/apps/autocal/tools/layout_mockup.py index 3ab0b3d..5d2cbe5 100644 --- a/apps/autocal/tools/layout_mockup.py +++ b/apps/autocal/tools/layout_mockup.py @@ -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"],