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/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