把存储的 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,
|
# 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)
|
||||||
|
|||||||
@@ -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}"
|
||||||
|
|||||||
@@ -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
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
Reference in New Issue
Block a user