SRS/jointlock: reachable in-limits poses can return [] on tight-limit redundant 7R (swivel-vs-limits coverage)

[siddhss5]

Problem

For a redundant 7R arm with narrow joint limits, solve(T, respect_limits=True) (the default) can return [] on a reachable, in-limits pose. Measured on OpenArm (joints 5,6 limited to ±0.785 rad):

Arm	empty rate, 200 in-limits poses, respect_limits=True
OpenArm L (seven_r.srs, 16 swivels)	~28%
OpenArm L (seven_r.srs, 48 swivels)	~26%
OpenArm L (jointlock.seven_r)	~62%
iiwa14 (wide limits)	0%

Root cause: the redundancy (elbow swivel) is sampled at 16 points over [-π, π]. When the arm's limits admit only a narrow swivel arc, the sampled solutions all fall outside limits and get filtered, even though an in-limits IK exists (the pose was generated from an in-limits q). Denser swivel sampling barely helps (16→48: 28%→26%), so it's not pure undersampling — the in-limits arc is genuinely narrow for many poses.

Not a regression, not the rescue's job

• Pre-existing / not Generalize native SRS solver to any concurrent-axis 7R (#354) #356: SRS (21/60 empty) is better than the old jointlock path (37/60) this arm used before Generalize native SRS solver to any concurrent-axis 7R (#354) #356. iiwa is unaffected (wide limits).
• The Coverage-gap sweep: nail down all 8 outstanding 0-sols / precision-floor xfails #319/Bulletproof: wire the T-perturbation rescue into the thin-wrapper (SRS-family) arms #358 T-perturbation rescue does NOT fix it (56→53/200): the rescue recovers ridge poses (raw solver returns 0), but here the raw solver returns 128 out-of-limits sols — a redundancy-vs-limits problem, not a rank-deficiency ridge. Wiring the rescue to fire post-limits would be harmful (it'd fire on ~28% of OpenArm solves at ~seconds each, without helping).

Possible directions

• Redundancy resolution toward the in-limits swivel arc: solve for the swivel value(s) that place the limit-binding joints inside limits (an interval intersection over the swivel circle), then sample within that arc instead of uniformly over [-π, π].
• Or a targeted densification pass when the uniform sweep yields no in-limits solution.
• Document as a completeness caveat in the interim (matches the existing per-solver caveat pattern).

Discovered while wiring #358.

[siddhss5]

Now surfaced in the suite

Added test_prebuilt_7r_in_limits_default_path (test_prebuilt_uniform_fuzz.py): samples q strictly within each arm's joint limits, FK, and asserts the default solve() (respect_limits=True) returns ≥1 IK. This is the path users take — the existing round-trip fuzz uses respect_limits=False, which is why this gap was invisible. The 8 non-gap 7R arms now have a real 0-empty guarantee; OpenArm L/R + R1 Pro L/R are xfailed here, tied to this issue.

Measured rate (uniform in-limits sampling): OpenArm ~0.8%, R1 Pro ~0.1% empty (the earlier "28%" was a [-0.7,0.7] hotspot, not the global rate). Small but nonzero, and it's a []-on-reachable, so it violates the bulletproof bar.

Approach

The redundancy is the elbow swivel θ; each joint value is a function q_i(θ). Today the solver samples θ uniformly over [-π, π] then filters by limits after — so a narrow in-limits θ-arc gets missed. Fix: sample within the admissible θ-arcs.

Options, cheapest first:

1. Adaptive densification (pragmatic): when the uniform sweep yields no in-limits solution but the pose is within the reach-sphere, bisect/refine θ locally around samples that come closest to limits. The in-limits arc is narrow but nonzero, so a local refinement finds it. Smallest change; bounded extra cost only in the rare empty branch.
2. Admissible-interval construction (principled): for each joint i, find the θ where q_i(θ) hits lo_i / hi_i (sign-change bracketing on the θ grid + Brent). The admissible set is [-π,π] minus the union of violated intervals; sample within it. Deterministic coverage of the in-limits arc; more work per solve.
3. Seed-directed: when a q_seed is supplied, solve for the θ nearest the seed's elbow position first (often already in-limits).

Recommend starting with (1) — it's a localized fallback in the existing empty branch, low-risk, and directly removes the []-on-reachable. Escalate to (2) if (1) leaves residual misses. When fixed, drop the arms from _LIMITS_GAP_7R and the test enforces the guarantee.

[siddhss5]

Design: exact feasible-swivel resolution (not denser sampling)

Densification is the wrong lever. Measured: 16→64 swivels closes the uniform in-limits gap but plateaus at ~26% on a narrow-arc hotspot, at 4× compute on every solve. Where the feasible ψ-arc is narrow, no finite sampling density is a guarantee. The redundancy is a 1-DOF circle where every joint is a closed-form function of the swivel ψ, so the feasible set is computable exactly.

Verified structural fact (machine precision, on the general non-ZYZ R1 Pro)

The swivel is a rotation of the elbow about the shoulder–wrist axis, so
R_sh(ψ) = Rot(û_sw, ψ) · R_sh(0) = A·sinψ + B·cosψ + C, constant A=[û_sw]_× R_sh(0), B=-[û_sw]_ײ R_sh(0), C=û_sw û_swᵀ R_sh(0). Checked: Rot(û_sw,-ψ)·R_sh(ψ) is constant to 2.9e-15 across the swivel family. Same for the wrist.

Because decompose_3axis extracts each joint as an atan2/arccos of quantities linear in the entries of R_sh(ψ), every joint is one of:

• cosine-type (middle angles): cos qᵢ(ψ) = a sinψ + b cosψ + c
• tangent-type (outer angles): qᵢ(ψ) = atan2(aₙ sinψ+bₙ cosψ+cₙ, a_d sinψ+…)
• elbow q₃ = const (independent of ψ).

This is Shimizu, Kim, Kakuya & Freeman, "Analytical IK for 7-DOF Redundant Manipulators with Joint Limits," IEEE T-RO 24(5), 2008 — stated there for ZYZ; #356's general extraction is what makes the same A/B/C form hold for arbitrary concurrent-axis arms.

Algorithm

1. Per (target, elbow-branch): build A,B,C (shoulder + wrist) from the ψ=0 reference the solver already computes.
2. Per joint, invert [loᵢ,hiᵢ] → feasible ψ-arcs. Both types reduce to solving a single-frequency sinusoid a sinψ + b cosψ = const (≤2 roots in (-π,π]); classify monotone vs. stationary via dqᵢ/dψ. Each joint → a union of arcs on S¹.
3. Intersect the 7 arc-sets → exact feasible ψ-set; union over the 8 discrete branches.
4. Pick ψ: center of the widest arc (max limit-margin), or nearest-to-seed, or optimize a secondary objective within the feasible arc.

Why this is the right answer

• Exact — finds an arbitrarily narrow feasible arc; sampling can't.
• Subsumes redundancy resolution — the whole feasible interval is available, so posture/manipulability/seed-tracking become optimal-constrained selection over a provably-nonempty set.
• O(1) closed-form per joint, vs. 4× sampling that still isn't a guarantee.
• Composes with Generalize native SRS solver to any concurrent-axis 7R (#354) #356; can replace the swivel sweep outright.

Plan

1. Prototype (validation-first): compute A/B/C + per-joint feasible arcs for R1 Pro / OpenArm; show the feasible ψ-set is non-empty exactly on the poses the sweep misses, and every picked ψ is in-limits + FK-closes. Cross-check the closed-form qᵢ(ψ) against the solver's numeric qᵢ over a ψ grid (machine precision).
2. Wire into srs.solve: a resolve_swivel_in_limits path that returns the exact in-limits solution(s); fall back to the sweep only when the feasible set is empty (truly out-of-limits-reachable). Keep the canonical fast path for callers that don't pass limits.
3. Drop the 5 arms from _LIMITS_GAP_7R; the default-path test then enforces the guarantee.

[siddhss5]

Prototype validation (scratch/feasible_swivel_proto.py)

Proven at machine precision:

• R_sh(ψ) = Rot(û_sw,ψ)·R_sh(0) — deviation 2.9e-15 across the swivel family (general non-ZYZ R1 Pro).
• Middle joints exactly cos(qᵢ − δᵢ) = a sinψ + b cosψ + c (resid ~5e-16), δᵢ = atan2(n0·(n1×n2), n0·n2); outer joints smooth with ≤2 stationary points.
• All 8 branches' closed-form q(ψ) FK-close to ~6e-16 (2 elbow × 2 shoulder-sign × 2 wrist-sign; enumeration complete, no sign errors).
• Feasible-interval computation exactly matches brute-force feasibility: 0 disagreements / 3000 ψ on a normal pose.
• Recovers gap poses (that the 16-sweep drops) at machine precision, FK ~1e-15.

Key implementation notes learned:

• Per-joint feasibility must be posed as φᵢ(ψ) = cos(qᵢ(ψ) − centerᵢ) − cos(halfᵢ) ≥ 0 — cos removes the atan2 branch-wrap discontinuity, making the feasibility function smooth (its sign zeros are the exact arc boundaries). Using raw qᵢ(ψ) breaks on the ±π wrap.
• Branch seeding: back-compute each branch's R_sh(0) = Rot(û_sw,−ψ)·R_sh(ψ) (ψ from the elbow position on the swivel circle) — invariant across ψ.

Honest open tail: standalone-prototype gap recovery is ~85% (OpenArm 21/25) / ~45% (R1 Pro 3/7). The misses are NOT the core math (feasibility matches brute exactly), NOT grid resolution (m=96..720 identical), NOT branch enumeration/signs (all 8 branches FK-close). They trace to arc-assembly / feasible_set edge cases in the quick prototype for specific gap configurations. To resolve in the production build, where the branch R_sh(0)/q3 are computed natively inside srs.solve (not back-inferred) and the arc algebra gets the stationary-point-segmented exact treatment.

Production plan

1. Native swivel_family(kb, T, elbow_branch) producing (A,B,C, q3, u_sw) directly from the solver's existing swivel-circle setup (no back-inference).
2. feasible_arcs via φ-formulation with stationary-point segmentation (exact for arbitrarily narrow arcs); middle joints get the closed-form sinusoid inversion.
3. resolve_swivel_in_limits(kb, T, limits, seed=None) → exact in-limits solution(s); wire into srs.solve / the prebuilt solve() as the respect_limits path. Fall back to sweep only when the feasible set is provably empty.
4. Drop the 5 arms from _LIMITS_GAP_7R; the default-path test enforces the guarantee.

[siddhss5]

Tail resolved — method recovers 100% at machine precision

The recovery misses were a harness bug, not the method: my validation check compared raw q_i to limits without wrapping mod 2π, but the joint is feasible at its 2π-equivalent angle (e.g. q1 = −4.385 ≡ 1.898 ∈ [−0.17, 3.14]) — exactly what wrap_to_limits handles in the real solve(). With the wrap in the check:

• R1 Pro: 7/7 gap poses recovered (was 3/7)
• OpenArm: 25/25 (was 21/25)
• worst FK 1e-12, typically 1e-15

So the feasible-interval method is fully validated end-to-end: exact feasibility vs brute (0/3000), all 8 branches FK-close (6e-16), 100% in-limits recovery at machine precision. Building production now.