把存储的 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
+36
View File
@@ -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: