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

added motion trajectory simulation

Browse Source
This commit is contained in:
ykcheungab
2026-09-23 09:58:00 +08:00
parent f1bec62ca4
commit 3347d17a20
25 changed files with 5198 additions and 1762 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/autocal/tests/test_ik_analytical.py
+334
View File
@@ -0,0 +1,334 @@
"""The closed-form solve: exactness, agreement with the sweep, and what it reports.
The sweep in ``test_kinematics.py`` is the reference this path is checked against.
It is slow but line-by-line verifiable, and solvers in this file have been
confidently wrong five times; a fast path with no reference is how that happens.
"""
from __future__ import annotations
import math
from pathlib import Path
import numpy as np
import pytest
from orisys_autocal.motion.kinematics import (
PlatformStack,
Branch,
rotation_about,
)
from orisys_autocal.sensor import load_bundle
#: The revision the bench confirmed. Skipped rather than failed when absent: the
#: export lives in a sibling design repo, and a machine without it has not broken
#: AutoCal.
BUNDLE = Path(r"D:\orisys_ws\orisys-sensor-design\build\export\finger\r01\compute")
@pytest.fixture(scope="module")
def bundle():
if not BUNDLE.is_dir():
pytest.skip(f"no sensor export at {BUNDLE}")
return load_bundle(BUNDLE)
def _random_units(count: int, seed: int) -> list[np.ndarray]:
generator = np.random.default_rng(seed)
vectors = generator.normal(size=(count, 3))
return [vector / np.linalg.norm(vector) for vector in vectors]
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
)
# ------------------------------------------------------------------ exactness
def test_the_closed_form_is_exact_on_random_normals():
"""Two atan2 calls should land on the answer, not near it."""
stack = PlatformStack()
worst = 0.0
for unit in _random_units(2000, seed=20240517):
branches = stack.alignment_branches(unit, stack.tool_direction)
assert branches, f"no branch found for {unit}"
worst = max(worst, min(branch.residual_deg for branch in branches))
assert worst < 1e-4, f"closed form left {worst:.3e} deg on the table"
def test_every_branch_reproduces_its_own_claimed_residual():
"""The number reported must be the number the rotations actually give."""
stack = PlatformStack()
for unit in _random_units(200, seed=7):
for branch in stack.alignment_branches(unit, stack.tool_direction):
landed = _apply(stack, unit, branch.yaw_deg, branch.pitch_deg)
measured = math.degrees(
math.acos(
max(-1.0, min(1.0, float(landed @ stack.tool_direction)))
)
)
# ``acos`` near 1 loses about sqrt(eps) in radians because the
# derivative blows up there; 1e-5 deg is the floating-point floor,
# not slack in the answer.
assert measured == pytest.approx(branch.residual_deg, abs=1e-5)
# ------------------------------------------------------------------ agreement
def test_the_closed_form_is_no_worse_than_the_sweep():
"""The sweep is the reference; the fast path may beat it but never lose."""
stack = PlatformStack()
for unit in _random_units(400, seed=99):
fast = stack._search_alignment(unit, stack.tool_direction) # noqa: SLF001
branches = stack.alignment_branches(unit, stack.tool_direction)
best = min(branch.residual_deg for branch in branches)
assert best <= fast[2] + 1e-9, (
f"closed form left {best:.6f} deg where the sweep found {fast[2]:.6f}"
)
def test_the_two_methods_agree_inside_the_envelope():
stack = PlatformStack()
for unit in _random_units(200, seed=1234):
analytical = stack.solve(np.zeros(3), unit, method="analytical")
search = stack.solve(np.zeros(3), unit, method="search")
assert analytical.residual_deg <= search.residual_deg + 1e-6
for pose in (analytical, search):
assert abs(pose.yaw_deg) <= stack.yaw_limit_deg + 1e-6
assert abs(pose.pitch_deg) <= stack.pitch_limit_deg + 1e-6
# ----------------------------------------------------------- axis equivalence
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)`` 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.
"""
unit = np.array([0.3, 0.5, -0.81])
unit = unit / np.linalg.norm(unit)
tool = np.array([0.0, 0.0, -1.0])
worst = 0.0
for yaw in (-120.0, -45.0, 0.0, 37.0, 90.0, 150.0):
for pitch in (-90.0, -20.0, 0.0, 15.0, 75.0):
about_y = rotation_about("y", pitch) @ rotation_about("z", yaw) @ unit
about_x = rotation_about("x", pitch) @ rotation_about("z", yaw - 90.0) @ unit
worst = max(worst, abs(float(about_y @ tool) - float(about_x @ tool)))
assert worst == pytest.approx(0.0, abs=1e-12), (
f"the two pitch bindings diverge in alignment by {worst:.3e}"
)
def test_the_offset_between_the_bindings_is_a_constant_ninety_degrees():
"""A constant shift, so it is a commissioning zero rather than a correction."""
unit = np.array([0.3, 0.5, -0.81])
unit = unit / np.linalg.norm(unit)
tool = np.array([0.0, 0.0, -1.0])
for yaw in (-120.0, -30.0, 0.0, 45.0, 120.0):
for pitch in (-90.0, 0.0, 25.0, 75.0):
about_y = rotation_about("y", pitch) @ rotation_about("z", yaw) @ unit
same = rotation_about("x", pitch) @ rotation_about("z", yaw - 90.0) @ unit
assert float(about_y @ tool) == pytest.approx(float(same @ tool), abs=1e-12)
# ---------------------------------------------------------------- reporting
def test_the_normal_is_aligned_to_plus_z_not_minus_z(bundle):
"""The regression that would have caught the sensor being turned over.
``approach_sign`` was ``-1``, which asked the solver to bring every normal onto
**-Z** — the sensing face is the *top* surface of a sensor mounted at the bottom
of its shell, so it points up, and the tool feeds down onto it. Nothing caught
it because ``arccos`` is sign-blind: a face turned fully over still scored
``0.0000`` degrees, and every test here asserted only that the residual was
small. The failure showed up exclusively as the model flipping upside down.
So this asserts the *direction* the normal lands in, which is the thing the
residual cannot express.
"""
stack = PlatformStack()
assert stack.approach_sign == 1
assert np.allclose(stack.tool_direction, [0.0, 0.0, 1.0])
for normal in bundle.vertex_normals():
unit = stack.normal_to_platform(normal)
unit = unit / np.linalg.norm(unit)
pose = stack.solve(np.zeros(3), normal)
# Rotate in the **platform** frame, which is where the residual is measured.
# ``stage_rotation`` is the same rotation conjugated into the mesh's frame,
# so using it here compares a sensor-frame vector against a platform-frame
# target and reports a mismatch that does not exist.
landed = _apply(stack, unit, pose.yaw_deg, pose.pitch_deg)
landed = landed / np.linalg.norm(landed)
# Aligned with the face's aim, not merely parallel to its line.
assert landed[2] > 0.0, (
f"the normal landed on {np.round(landed, 3)} — pointing away from +Z"
)
def test_the_nominal_pose_is_upright(bundle):
"""No rotation is the commissioned pose, and it must read as upright."""
stack = PlatformStack()
assert stack.uprightness(0.0, 0.0) == pytest.approx(1.0)
def test_the_surface_normals_point_up_so_the_face_faces_the_tool(bundle):
"""A sanity check on the geometry the polarity has to agree with.
The mean normal has a positive platform-Z component: the patch faces upward.
If this ever inverted, ``approach_sign`` would need revisiting rather than the
solver.
"""
stack = PlatformStack()
platform = np.array(
[stack.normal_to_platform(n) for n in bundle.vertex_normals()]
)
platform /= np.linalg.norm(platform, axis=1)[:, None]
assert float(platform[:, 2].mean()) > 0.0, (
f"mean platform-Z component of the normals is {platform[:, 2].mean():+.3f}"
)
def test_few_vertices_end_turned_over_and_those_are_edge_on(bundle):
"""Turning the face over is the failure mode; measure it and bound it.
The remainder are vertices whose normals are 60-90 degrees off the tool axis —
the rim of the dome. Aligning an edge-on face has to roll the body about that
normal, and the sign is decided by rounding, so they cannot be pushed to zero
and should not be asserted away.
"""
stack = PlatformStack()
normals = bundle.vertex_normals()
flipped = 0
for normal in normals:
pose = stack.solve(np.zeros(3), normal)
if stack.uprightness(pose.yaw_deg, pose.pitch_deg) < -0.5:
flipped += 1
assert flipped <= len(normals) * 0.03, (
f"{flipped} of {len(normals)} vertices ended turned over"
)
def test_a_pose_reports_whether_it_is_upright():
"""A pose can align the normal and still turn the sensor over."""
stack = PlatformStack()
seen = {True: 0, False: 0}
for unit in _random_units(300, seed=3):
pose = stack.solve(np.zeros(3), unit)
seen[pose.upright] += 1
# Whatever it says must match the measured orientation.
assert pose.upright == (stack.uprightness(pose.yaw_deg, pose.pitch_deg) >= 0.0)
assert seen[True] > 0
def test_an_out_of_range_pose_is_reported_and_never_silently_clamped():
"""Tight travel must give a measured residual, not a pose the stage cannot reach."""
stack = PlatformStack(pitch_limit_deg=45.0)
for unit in _random_units(50, seed=5):
pose = stack.solve(np.zeros(3), unit)
assert abs(pose.pitch_deg) <= 45.0 + 1e-6
assert abs(pose.yaw_deg) <= stack.yaw_limit_deg + 1e-6
if not pose.in_range:
assert "travel" in pose.reason
def test_nothing_is_reported_out_of_range_when_the_travel_is_ample():
stack = PlatformStack()
for unit in _random_units(300, seed=11):
pose = stack.solve(np.zeros(3), unit)
assert pose.in_range, f"{unit} reported out of range at full travel"
assert pose.reason == "" or "indeterminate" in pose.reason
def test_reachability_agrees_with_the_envelope():
"""Every normal is reachable when the yaw can turn a full half-turn."""
stack = PlatformStack()
for unit in _random_units(500, seed=13):
reachable, why = stack.reachable(unit, stack.tool_direction)
assert reachable, f"{unit} unreachable: {why}"
# And with the pitch stage able to turn, no normal is rejected either.
assert stack.reachable(
np.array([0.0, 0.0, 1.0]), stack.tool_direction
)[0]
# --------------------------------------------------------------------- poles
@pytest.mark.parametrize(
"normal",
[
np.array([0.0, 1.0, 0.0]), # on the tool axis
np.array([0.0, -1.0, 0.0]), # on the tool axis, other way
np.array([1.0, 0.0, 0.0]), # on the yaw plane
np.array([0.0, 0.0, 1.0]), # on the yaw axis
],
)
def test_poles_still_return_a_usable_pose(normal):
stack = PlatformStack()
pose = stack.solve(np.zeros(3), normal)
assert pose.ok
assert math.isfinite(pose.yaw_deg) and math.isfinite(pose.pitch_deg)
landed = _apply(stack, stack.normal_to_platform(normal) / np.linalg.norm(
stack.normal_to_platform(normal)
), pose.yaw_deg, pose.pitch_deg)
measured = math.degrees(
math.acos(max(-1.0, min(1.0, float(landed @ stack.tool_direction))))
)
assert measured <= stack.tolerance_deg + 1e-6
def test_the_degenerate_normal_is_still_refused():
stack = PlatformStack()
pose = stack.solve(np.zeros(3), np.zeros(3))
assert not pose.ok
assert "degenerate" in pose.reason
# ----------------------------------------------------------------- selection
def test_select_branch_prefers_in_range_then_upright_then_small():
reachable_upright = Branch(10.0, 5.0, 1e-6, True, True)
reachable_inverted = Branch(10.0, 5.0, 1e-6, False, True)
unreachable_upright = Branch(179.0, 5.0, 1e-6, True, False)
# In range beats everything: an inverted pose the machine can reach is worth
# more than an upright one it cannot.
assert PlatformStack.select_branch(
[unreachable_upright, reachable_inverted, reachable_upright]
) is reachable_upright
# Among reachable poses, upright wins.
assert PlatformStack.select_branch(
[reachable_inverted, reachable_upright]
) is reachable_upright
# Among poses that are out of range and one of them upright, upright wins.
unreachable_inverted = Branch(179.0, 5.0, 1e-6, False, False)
assert PlatformStack.select_branch(
[unreachable_inverted, unreachable_upright]
) is unreachable_upright
# A materially better alignment beats an upright pose.
much_better = Branch(10.0, 5.0, 0.0, False, True)
worse_upright = Branch(10.0, 5.0, 5.0, True, True)
assert PlatformStack.select_branch([much_better, worse_upright]) is much_better
# And with everything else equal, the smaller move wins.
smaller = Branch(5.0, 5.0, 1e-6, True, True)
larger = Branch(60.0, 5.0, 1e-6, True, True)
assert PlatformStack.select_branch([larger, smaller]) is smaller
def test_an_unknown_method_is_refused():
stack = PlatformStack()
with pytest.raises(ValueError):
stack.solve(np.zeros(3), np.array([0.0, 1.0, 0.0]), method="nonsense")