From 5339624ffa6df4415fe8d8cae3ef25c185f9b400 Mon Sep 17 00:00:00 2001 From: ykcheungab Date: Wed, 14 Oct 2026 20:27:21 +0800 Subject: [PATCH] =?UTF-8?q?=E6=8A=8A=E5=AD=98=E5=82=A8=E7=9A=84=20yaw=20?= =?UTF-8?q?=E4=B8=8A=E9=99=90=E5=BD=92=E4=B8=80=E5=88=B0=20=C2=B1180?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit yaw 台转 ±180，实测点的第二组精确解落在 118° 之外：旧文件里的 ±90 会让求解器把位姿算成可达、运动守卫却拒绝这段 yaw，操作员看到的就是“到不了”。加载时把求解器与守卫两种写法的 yaw 上限一起归一，pitch 包络此前已按同一理由处理。 --- .../autocal/src/orisys_autocal/preferences.py | 7 ++++ apps/autocal/tests/test_kinematics.py | 36 +++++++++++++++++++ apps/autocal/tests/test_locales_autocal.py | 35 ++++++++++++++++++ apps/autocal/tests/test_pose_move.py | 17 +++++++++ 4 files changed, 95 insertions(+) diff --git a/apps/autocal/src/orisys_autocal/preferences.py b/apps/autocal/src/orisys_autocal/preferences.py index 3f9603a..f9dd66d 100644 --- a/apps/autocal/src/orisys_autocal/preferences.py +++ b/apps/autocal/src/orisys_autocal/preferences.py @@ -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) diff --git a/apps/autocal/tests/test_kinematics.py b/apps/autocal/tests/test_kinematics.py index 06820c0..9604313 100644 --- a/apps/autocal/tests/test_kinematics.py +++ b/apps/autocal/tests/test_kinematics.py @@ -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: diff --git a/apps/autocal/tests/test_locales_autocal.py b/apps/autocal/tests/test_locales_autocal.py index 11d6b8e..1861992 100644 --- a/apps/autocal/tests/test_locales_autocal.py +++ b/apps/autocal/tests/test_locales_autocal.py @@ -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}" diff --git a/apps/autocal/tests/test_pose_move.py b/apps/autocal/tests/test_pose_move.py index b5831df..7dfc458 100644 --- a/apps/autocal/tests/test_pose_move.py +++ b/apps/autocal/tests/test_pose_move.py @@ -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