把存储的 yaw 上限归一到 ±180

yaw 台转 ±180,实测点的第二组精确解落在 118° 之外:旧文件里的 ±90 会让求解器把位姿算成可达、运动守卫却拒绝这段 yaw,操作员看到的就是“到不了”。加载时把求解器与守卫两种写法的 yaw 上限一起归一,pitch 包络此前已按同一理由处理。
This commit is contained in:
ykcheungab
2026-10-14 20:27:21 +08:00
parent 3f578c26c4
commit 5339624ffa
4 changed files with 95 additions and 0 deletions
@@ -322,6 +322,13 @@ def load_preferences() -> dict[str, Any]:
# Yaw travel is ±180. The same-sign pitch solution sits near ±150° of yaw, # Yaw travel is ±180. The same-sign pitch solution sits near ±150° of yaw,
# and a stored ±90 drops it, leaving the opposite pitch sign on one side. # and a stored ±90 drops it, leaving the opposite pitch sign on one side.
prefs["finger_yaw_limit_deg"] = float(defaults["finger_yaw_limit_deg"]) prefs["finger_yaw_limit_deg"] = float(defaults["finger_yaw_limit_deg"])
# The solver's envelope and the move guard's are one bench fact written twice: a
# measured point has two exact solutions, the second past 118° of yaw, and a file
# saved on the old ±90 would have the solver call a pose reachable while the guard
# refused the 118° the move needs. Normalise both spellings, as the pitch envelope
# above is normalised, so "cannot reach" can only mean the stage really cannot.
if isinstance(limits, dict):
limits["max_yaw_deg"] = float(defaults["safety_limits"]["max_yaw_deg"])
# A hand-edited task number must not reach the controller as typed. # A hand-edited task number must not reach the controller as typed.
prefs["bas_task"] = clamp_bas_task(prefs.get("bas_task")) prefs["bas_task"] = clamp_bas_task(prefs.get("bas_task"))
prefs = _migrate_axis_roles(prefs) prefs = _migrate_axis_roles(prefs)
+36
View File
@@ -363,6 +363,42 @@ def test_the_yaw_limit_is_respected():
assert abs(pose.yaw_deg) <= 90.0 + 1e-6 assert abs(pose.yaw_deg) <= 90.0 + 1e-6
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".
"""
point = np.array([-2.698, 5.186, 18.228])
normal = np.array([-0.343, 0.699, 0.627])
wide = PlatformStack(yaw_limit_deg=180.0, pitch_limit_deg=90.0)
narrow = PlatformStack(yaw_limit_deg=90.0, pitch_limit_deg=90.0)
unit = wide.setup_normal(normal)
unit = unit / float(np.linalg.norm(unit))
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 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"
)
assert wide.solve(point, normal).in_range
def test_a_yaw_limit_past_the_trunnion_travel_is_capped():
"""The stage turns ±180. A stored 360 must not widen the envelope."""
stack = PlatformStack(yaw_limit_deg=360.0, pitch_limit_deg=90.0)
assert stack._yaw_limit_effective() == 180.0 # the cap lives here, not in the caller
for normal in NORMALS:
pose = stack.solve(np.array([0.0, 5.0, 0.0]), normal)
assert abs(pose.yaw_deg) <= 180.0 + 1e-6
def test_the_pitch_limit_is_respected(): def test_the_pitch_limit_is_respected():
stack = PlatformStack(pitch_limit_deg=45.0) stack = PlatformStack(pitch_limit_deg=45.0)
for normal in NORMALS: for normal in NORMALS:
@@ -122,6 +122,41 @@ def test_a_stored_block_without_a_new_limit_still_gets_it():
assert "press_axis" not in prefs["sensor_setup"] assert "press_axis" not in prefs["sensor_setup"]
def test_a_stored_quarter_turn_yaw_limit_is_normalised_to_the_bench_travel():
"""A stored ±90 makes a solvable pose read as unreachable.
The yaw stage turns ±180, and a measured point's second exact solution sits past
118° of yaw: at ±90 the solver is left with one branch and the move guard refuses
the other, so a pose the preview aligned reports "cannot reach". Both spellings of
the limit — the solver's ``finger_yaw_limit_deg`` and the guard's
``safety_limits.max_yaw_deg`` — are the same bench fact, so both are normalised.
"""
from orisys_app_shared.paths import preference_path
from orisys_autocal.preferences import load_preferences
path = preference_path()
path.parent.mkdir(parents=True, exist_ok=True)
path.write_text(
json.dumps(
{
"safety_limits": {"max_yaw_deg": 90.0, "max_pitch_deg": 360.0},
"finger_yaw_limit_deg": 90.0,
"finger_pitch_limit_deg": 360.0,
}
),
encoding="utf-8",
)
prefs = load_preferences()
assert prefs["safety_limits"]["max_yaw_deg"] == 180.0
assert prefs["finger_yaw_limit_deg"] == 180.0
# The pitch envelope keeps the correction it already had, so the two do not drift.
assert prefs["safety_limits"]["max_pitch_deg"] == 90.0
assert prefs["finger_pitch_limit_deg"] == 90.0
def _walk(node, prefix: str = ""): def _walk(node, prefix: str = ""):
for key, value in node.items(): for key, value in node.items():
path = f"{prefix}{key}" path = f"{prefix}{key}"
+17
View File
@@ -238,6 +238,23 @@ def test_the_guard_names_an_unknown_direction():
assert refusal is not None and "roll" in refusal.message assert refusal is not None and "roll" in refusal.message
def test_the_guard_allows_the_yaw_the_solver_calls_reachable():
"""One bench fact, two spellings: the guard must not refuse what the solver solved.
A measured point's second exact solution sits past 118° of yaw — inside the ±180
the trunnion turns, outside the ±90 note this bench was once configured with. The
solver's envelope and the guard's limit therefore have to be the same number.
"""
service = make_service()
assert service.limit_for("yaw") == 180.0
assert service.limit_for("pitch") == 90.0
assert service.check_axes_move({"yaw": 118.681}) is None
past = service.check_axes_move({"yaw": 180.0 + 0.1})
assert past is not None, "a yaw past the trunnion's travel was allowed"
# ------------------------------------------------------------------------ mover # ------------------------------------------------------------------------ mover