把存储的 yaw 上限归一到 ±180
yaw 台转 ±180,实测点的第二组精确解落在 118° 之外:旧文件里的 ±90 会让求解器把位姿算成可达、运动守卫却拒绝这段 yaw,操作员看到的就是“到不了”。加载时把求解器与守卫两种写法的 yaw 上限一起归一,pitch 包络此前已按同一理由处理。
This commit is contained in:
@@ -322,6 +322,13 @@ def load_preferences() -> dict[str, Any]:
|
||||
# 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.
|
||||
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.
|
||||
prefs["bas_task"] = clamp_bas_task(prefs.get("bas_task"))
|
||||
prefs = _migrate_axis_roles(prefs)
|
||||
|
||||
@@ -363,6 +363,42 @@ def test_the_yaw_limit_is_respected():
|
||||
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():
|
||||
stack = PlatformStack(pitch_limit_deg=45.0)
|
||||
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"]
|
||||
|
||||
|
||||
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 = ""):
|
||||
for key, value in node.items():
|
||||
path = f"{prefix}{key}"
|
||||
|
||||
@@ -238,6 +238,23 @@ def test_the_guard_names_an_unknown_direction():
|
||||
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
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user