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

修正朝上判断、接触高度与过期姿态目标

Browse Source 
朝上判断误用平台轴、接触高度报旋转前数值、多点对准复用缓存姿态，且多点会话被复核入口拒绝；
现按修正计划改正并接通 autocal-multipoint-1.0 只读复核，缺失帧源如实报告而不补零。

离线验证：新回归在 dc5a6f8 上失败、此处通过，panel_check 通过，两截图点接触高度复现
19.0510 / 21.5781 mm；实机项未做，详见 .scratch/autocal-motion-fix/verification.md。
This commit is contained in:
ykcheungab
2026-10-15 06:52:28 +08:00
parent 5339624ffa
commit bcc55a3b97
29 changed files with 2179 additions and 209 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/autocal/tests/test_kinematics.py
+194 -2
View File
@@ -19,6 +19,7 @@ import numpy as np
import pytest
from orisys_autocal.motion.kinematics import (
ALIGNED_TOL_DEG,
Branch,
PlatformStack,
Pose,
residual_for,
@@ -512,6 +513,10 @@ def test_the_table_move_lands_the_contact_on_the_tool():
final = landed + np.array([pose.table_xy_mm[0], pose.table_xy_mm[1], 0.0])
assert final[0] == pytest.approx(stack.tool_xy_mm[0], abs=1e-9)
assert final[1] == pytest.approx(stack.tool_xy_mm[1], abs=1e-9)
# The reported contact is that same landing point, height included — the
# whole point of the move is to arrive there, not merely at its xy.
assert pose.contact_point_mm == pytest.approx(tuple(final), abs=1e-9)
assert pose.contact_z_mm == pytest.approx(float(final[2]), abs=1e-9)
def test_the_mounting_offset_moves_the_point_and_the_mounting_yaw_the_whole_frame():
@@ -526,7 +531,12 @@ 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)
assert moved.contact_z_mm == pytest.approx(base.contact_z_mm)
# 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.
assert moved.mounted_point_mm[2] == pytest.approx(base.mounted_point_mm[2])
assert moved.contact_z_mm - base.contact_z_mm > 0.1
turned = dataclasses.replace(stack, sensor_yaw_deg=35.0)
yawed = turned.solve(point, normal)
@@ -552,7 +562,11 @@ def test_the_setup_z_is_the_mounting_plate():
lifted = dataclasses.replace(stack, sensor_offset_mm=(0.0, 0.0, 4.25))
raised = lifted.solve(point, normal)
assert raised.contact_z_mm - plain.contact_z_mm == pytest.approx(4.25)
# The plate lifts the mounting pose by exactly its thickness. The height the
# stages present rises by less, because the lift is applied before the stage
# rotation and the rotation tilts it — 4.2285 mm of the 4.25 mm here.
assert raised.mounted_point_mm[2] - plain.mounted_point_mm[2] == pytest.approx(4.25)
assert 0.0 < raised.contact_z_mm - plain.contact_z_mm < 4.25
assert raised.target_xy_mm == pytest.approx(plain.target_xy_mm)
# And it is the same number as the plate parameter it replaced.
assert lifted.setup_point(point)[2] == pytest.approx(
@@ -605,3 +619,181 @@ def test_an_aligned_pose_reports_that_it_reached_tolerance():
assert loose.reachable
far = Pose(ok=True, residual_deg=30.0)
assert not far.reached_tolerance and not far.reachable
# --------------------------------------- the branch, the height and the up axis
def _independent_rotation(yaw_deg: float, pitch_deg: float) -> np.ndarray:
"""``R_pitch @ R_yaw`` from the formulas, not through ``rotation_about``."""
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)],
]
)
return about_pitch @ about_yaw
#: The two measured points from the bench screenshots, in the sensor frame:
#: point, normal, mounting height, and the height the stages present. The mounting
#: height is the plate (8 mm) over the point's own height; the presented height is
#: what the plan records as 19.051 and 21.578 mm.
MEASURED = (
((4.872, 5.444, 16.176), (0.595, 0.673, 0.439), 13.444, 19.051),
((-2.698, 5.186, 18.228), (-0.343, 0.699, 0.627), 13.186, 21.578),
)
def _measured_stack() -> PlatformStack:
"""The bench's mounting: the plate is the mounting pose's z, 8 mm."""
return PlatformStack(sensor_offset_mm=(0.0, 0.0, 8.0))
def test_the_contact_height_is_the_height_after_the_stages_turn():
"""Defect: the pose reported the contact's height *before* the stages turned.
On the bench that is the difference between 13.444 mm and 19.051 mm — the
stages lift the contact several millimetres, and any press-axis target is
derived from the height the contact is *presented* at, not from where it sits
with the stages at zero.
"""
stack = _measured_stack()
for point, normal, mounting, presented in MEASURED:
pose = stack.solve(np.array(point), np.array(normal) / np.linalg.norm(normal))
assert pose.mounted_point_mm[2] == pytest.approx(mounting, abs=5e-4)
assert pose.contact_z_mm == pytest.approx(presented, abs=5e-4)
# The same height again, from a rotation built here rather than by the
# solver: the landing point of the reported angles.
landed = _independent_rotation(pose.yaw_deg, pose.pitch_deg) @ stack.setup_point(
np.array(point)
)
assert pose.contact_z_mm == pytest.approx(float(landed[2]), abs=1e-9)
assert pose.contact_point_mm == pytest.approx(
(stack.tool_xy_mm[0], stack.tool_xy_mm[1], float(landed[2])), abs=1e-9
)
assert pose.contact_point_mm[2] > pose.mounted_point_mm[2]
def test_the_pose_aligns_the_normal_and_keeps_the_body_up():
"""Task-1 acceptance: independent rotation, tool axis and the platform's up.
"Up" is the direction the sensing face nominally aims — platform +Z, which is
where the mount sends the sensor's own up axis — not a horizontal axis of the
platform frame.
"""
stack = _measured_stack()
up = np.array([0.0, 0.0, 1.0])
for point, normal, _mounting, _presented in MEASURED:
pose = stack.solve(np.array(point), np.array(normal) / np.linalg.norm(normal))
rotate = _independent_rotation(pose.yaw_deg, pose.pitch_deg)
aligned = rotate @ stack.setup_normal(np.array(normal))
angle = np.degrees(np.arccos(np.clip(float(aligned @ up), -1.0, 1.0)))
assert angle <= 1e-5
# ``uprightness`` is that same rotation carried onto the up axis: the
# component left pointing up, 1.0 when nothing has tipped at all.
assert stack.uprightness(pose.yaw_deg, pose.pitch_deg) == pytest.approx(
float((rotate @ up)[2]), abs=1e-12
)
assert 0.0 < stack.uprightness(pose.yaw_deg, pose.pitch_deg) <= 1.0
assert pose.upright
def test_both_exact_poses_of_a_measured_point_leave_the_sensor_face_up():
"""Defect: the up test measured the body against a *horizontal* axis.
The two exact poses of a measured point are one pose rolled 180 degrees about
the vertical: same contact, same height, mirrored azimuth. Neither turns the
sensing face over, so flagging one of them inverted was the bug — and the
panel, the preview and the guards all read that flag.
"""
stack = PlatformStack()
first = _independent_rotation(-143.580, 47.692)
second = _independent_rotation(36.420, -47.692)
assert second == pytest.approx(rotation_about("z", 180.0) @ first, abs=1e-9)
for yaw_deg, pitch_deg in (
(36.420, -47.692),
(-143.580, 47.692),
(-61.319, 45.636),
(118.681, -45.636),
):
assert stack.uprightness(yaw_deg, pitch_deg) > 0.0
assert stack.ends_upright(yaw_deg, pitch_deg)
def test_equivalent_poses_are_chosen_by_the_machine_not_by_the_residual():
"""Two exact poses differ by float noise; that must not decide the answer.
The right-hand screenshot point, both ways round: 84 degrees of stage travel
against 191, and the shorter one carries the marginally *larger* residual.
"""
short = Branch(
yaw_deg=36.420, pitch_deg=-47.692, residual_deg=1e-9, upright=True, in_range=True
)
long_way = Branch(
yaw_deg=-143.580, pitch_deg=47.692, residual_deg=0.0, upright=True, in_range=True
)
assert PlatformStack.select_branch([long_way, short]) is short
assert PlatformStack.branch_reason([long_way, short], short) == "smallest"
def test_a_materially_better_alignment_still_wins():
"""Equivalent is a tolerance, not an excuse to ignore the residual."""
coarse = Branch(yaw_deg=0.0, pitch_deg=0.0, residual_deg=3.0, upright=True, in_range=True)
exact = Branch(yaw_deg=80.0, pitch_deg=40.0, residual_deg=0.05, upright=True, in_range=True)
assert PlatformStack.select_branch([coarse, exact]) is exact
assert PlatformStack.branch_reason([coarse, exact], exact) == "alignment"
def test_real_roundoff_does_not_choose_a_half_turn_over_the_shorter_pose():
stack = PlatformStack(pitch_limit_deg=90)
normal = np.array([-0.499590704094966, 0.6734102426572587, -0.5449107940444822])
unit = stack.setup_normal(normal)
branches = stack.alignment_branches(unit, stack.tool_direction)
short = min(branches, key=lambda branch: branch.travel_deg)
for branch in branches:
turned = stack.platform_rotation(branch.yaw_deg, branch.pitch_deg) @ unit
np.testing.assert_allclose(turned, [0, 0, 1], atol=1e-12)
assert stack.select_branch(branches) is short
def test_the_branch_keys_are_applied_in_the_order_the_machine_cares_about():
"""The envelope, then which way the body ends up, then the shorter turn."""
inside = Branch(
yaw_deg=-61.319, pitch_deg=45.636, residual_deg=0.0, upright=True, in_range=True
)
outside = Branch(
yaw_deg=0.0, pitch_deg=0.0, residual_deg=0.0, upright=True, in_range=False
)
assert PlatformStack.select_branch([inside, outside]) is inside
assert PlatformStack.branch_reason([inside, outside], inside) == "in_range"
upright = Branch(
yaw_deg=118.681, pitch_deg=-45.636, residual_deg=0.0, upright=True, in_range=True
)
rolled = Branch(
yaw_deg=10.0, pitch_deg=10.0, residual_deg=0.0, upright=False, in_range=True
)
assert PlatformStack.select_branch([upright, rolled]) is upright
assert PlatformStack.branch_reason([upright, rolled], upright) == "upright"
def test_a_lone_branch_is_its_own_reason_and_an_empty_list_is_refused():
only = Branch(yaw_deg=10.0, pitch_deg=10.0, residual_deg=0.0, upright=True, in_range=True)
assert PlatformStack.select_branch([only]) is only
assert PlatformStack.branch_reason([only], only) == "only"
with pytest.raises(ValueError):
PlatformStack.select_branch([])