Skip to content

Spec: ikgeo.general_6r — numeric Raghavan-Roth 6R solver

Status: draft, pre-implementation. Review before any code lands. Strategic role: the universal-6R closed-form-style solver that closes the EAIK coverage gap (JACO 2 classical, Agilex Piper, custom non-Pieper 6R; via jointlock.seven_r, also Rizon 4 and other non-SRS 7R arms). Replaces the current grid-search gen_six_dof as the production tier-2 path; the grid version is retained, vectorized and renamed gen_six_dof_oracle, as the cross-check.

Algorithm — clean-room from Tsai 1999 App. C + Manocha-Canny 1994

References (open-access PDFs cached in /tmp/rrk/): - Tsai App. C (METR4202 OCR) — derivation of the 14-equation system, the 6×9 / 14×8 split, elimination of (q₁, q₂), substitution of half-angles, construction of the 12×12 matrix M(x₃). - Manocha-Canny 1994 (Berkeley preprint) — Theorem 1 (24×24 companion matrix), Theorem 2 (the generalized-eigenvalue pencil the solver uses), eigenvector → (x₄, x₅), Newton refinement.

Provenance: clean-room from Tsai App. C and Manocha–Canny 1994. The RR algorithm predates IKFast by 17 years; ssik's implementation is original work, no source-code lineage from any prior implementation.

Step 1 — DH normalization

Input: POE-normalized KinBody with 6 revolute joints. Convert to standard DH parameters (αᵢ, aᵢ, dᵢ) for i ∈ 1..6 by reading off T_left, axes, and joint origins. (POE → DH conversion already exists in ssik.kinematics.poe; verify it covers the non-orthogonal-twist case used by JACO 2's 55° offsets.)

Step 2 — Build the 14×9 / 14×8 system numerically

Substitute target pose T_target and DH params into the closed-form expressions for P (14×9, entries linear in s₃, c₃, 1) and Q (14×8, entries constant per solve) from Tsai Eq. (C.8) / MC Eq. (4). The 14 rows = 6 from columns 3,4 of the matrix loop-closure equation + 8 from the (a·a, a·b, a×b, (a·a)b−2(a·b)a) vector identities. Code this once symbolically (sympy at module import) and emit a NumPy callable build_PQ(dh, T_target) -> (P, Q).

Step 3 — Eliminate (q₁, q₂)

SVD-rank Q (Manocha-Canny §IV-B). If rank(Q) = 8, do Gaussian elimination with complete pivoting to write the 8 right-side monomials in terms of the 9 left-side monomials, leaving 6 equations in (s₄, c₄, s₅, c₅) only. If rank(Q) < 8 (degenerate arm — e.g. Puma with d₆ = 0 collapses to ≤8 solutions), drop ill-conditioned rows; downstream still handles it.

Step 4 — Substitute half-angles, build M(x₃)

Apply Weierstrass sᵢ = 2xᵢ/(1+xᵢ²), cᵢ = (1−xᵢ²)/(1+xᵢ²) for i = 4, 5, clear (1+x₄²)(1+x₅²). Apply for i = 3, multiply first 4 rows by (1+x₃²). Stack [[E'', 0], [0, E''·x₄]] to get the 12×12 polynomial matrix M(x₃) = A·x₃² + B·x₃ + C. Each of A, B, C is a fully-numeric 12×12 matrix. The 12-monomial vector is v = (x₄²x₅², x₄²x₅, x₄², x₄x₅², x₄x₅, x₄, x₅², x₅, 1, x₄x₅², x₄x₅, x₄) (verify exact ordering against Tsai Eq. C.13–C.15 + MC Eq. 18 on first implementation pass).

Step 5 — Roots: QZ on the pencil (MC Theorem 2)

Equilibrate (A, B, C) and solve the 24×24 pencil M₁ − x·M₂ (M₁ = [[I, 0], [0, C]], M₂ = [[0, I], [−A, −B]]) by QZ (LAPACK dggev; Eigen's GeneralizedEigenSolver on native). QZ never inverts A, so a singular leading matrix needs no fallback: its roots come back as eigenvalue pairs with β = 0, the root at infinity (the joint at π, #571), which is kept whatever the phase of α. Drop the 8 spurious roots near ±i (from the (1+x₃²)⁴ factor) and roots that are not real on the projective line: |Im x| > 1e-3 · max(|x|², 1), i.e. Im x above 1e-3 inside the unit disc and Im(1/x) above it outside, the same bound on Im q at every angle (#673). A near-double real root close to q = π splits under round-off into a pair whose Im x is large next to Re x but small next to |x|²; a bound relative to Re x dropped it. A QZ that does not converge yields no roots, never an exception (#599, #658).

The 24×24 companion route of Theorem 1 (Σ = [[0, I], [−A⁻¹C, −A⁻¹B]], solve_x2_roots) is kept as a reference only: its error is amplified through A⁻¹, and at close roots it places them 1e-6 to 3e-3 off, more than their separation, so the null vector read there mixes branches (#640). QZ places them to ~1e-12.

Step 6 — Null vectors and repeated roots (#595, #640)

Each real root x reads its monomial vector as the null vector of M(x) (A at infinity): SVD on native, two steps of inverse iteration in Python. Roots with another root within chordal distance 1e-3, or every root when the pencil is ill-conditioned (#642's probe), take the multiplicity test: k = the number of singular values of M(x) at most √ε · s₀. A root with k ≥ 2 may carry up to k branches; it emits its own null vector and every branch vector the x_lb0 shift and the x_lb1 shift of the monomial vector determine in its own null space: one per real, simple eigenvalue of each shift's projected k × k pencil. A genuine branch vector satisfies its shift relation on every row, so it is an eigenvector of the projected pencil whatever the rest of the space holds. Next to a degenerate pose the space also holds directions with no monomial structure, and branches that share x_lb0, so a shift that does not give k distinct real values still reads the branches it can (#673). No root ever takes a vector from a nearby root's space; FK certification (Step 9) keeps the real branches and the same-root dedup merges repeats. Python and native apply the same rules.

Step 7 — Back-substitution (MC §IV-C/D)

Each eigenvector of Σ has structure V = [v; x₃·v]. Per root: - Pick the top half if |x₃| ≤ 1, bottom half otherwise (smaller relative error — MC Eq. 15 footer). - Recover (x₄, x₅) as ratios of two entries of v. Use entries with largest magnitudes for numerator/denominator. Cross-check via redundant ratios (e.g. v[5]/v[8] = x₄, v[1]/v[5] = x₅). - Recover (q₁, q₂): solve the 8-row linear system from Eq. (11) (Tsai Eq. C.7's right block) for the 8 monomials {s₁s₂, s₁c₂, c₁s₂, c₁c₂, s₁, c₁, s₂, c₂}. Two angles via atan2(s₁c₂, c₁c₂) etc. - Recover q₆ via atan2 on row entries of the original loop-closure equation Eq. (3) once q₁..q₅ are known.

Step 8 — Newton refinement (MC §V-E)

For each candidate q ∈ ℝ⁶, run 1–2 Newton steps on the 14-equation residual (Tsai Eq. C.8) — this lifts ~6 digit eigenvalue precision to ~10–11 digits. Step clipped to π/4, wrap-to-pi each step (same pattern as SP5/SP6 GN).

Step 9 — Verification

FK each refined q via the existing _kinbody-aware FK, compare to T_target Frobenius norm. Drop any q failing ‖FK(q) − T_target‖ < policy.subproblem_numerical. Dedup with the standard wrap-to-pi q_close predicate.

Performance budget

  • Step 2 (build P, Q): ~50 µs after sympy precompute (per-arm, cached).
  • Step 5 (np.linalg.eig on 24×24): ~30 µs in NumPy.
  • Step 7 (back-sub × 16 roots): ~200 µs (linear solves are tiny).
  • Step 8 (Newton × 16): ~500 µs.
  • Total: ~1–3 ms per IK in pure Python. ~20× faster than the symbolic IKFast path. After Phase M codegen: ~100 µs.

Validation plan

Bulletproof discipline — same standard as spherical_two_parallel:

  1. Synthetic 16-solution fixture — MC's Table I example (DH params + 16 real solutions). Hand-verified ground truth. Assert len(solutions) == 16, each FK-matches at 1e-10.
  2. Real-arm fixtures: JACO 2 classical (55° non-orthogonal DH), Agilex Piper (mujoco_menagerie URDF). Assert FK-closure on 100 random poses.
  3. Cross-check vs vectorized gen_six_dof_oracle on 500 hypothesis poses per arm. Solver-set agreement: every solution from one must appear in the other (within 1e-6 wrap-to-pi). Catches missing branches.
  4. Pieper-class regression: run on UR5, Puma 560 (where tier-0 already solves). Solution sets must match three_parallel / spherical_two_parallel at machine precision. Catches algorithm bugs that only manifest on degenerate Q rank.
  5. Hypothesis fuzz: 500 random POE-normalized 6R chains × random poses. FK-closure on every returned solution.
  6. Conditioning stress: poses near q₃ = π (drives x₃ → ∞); confirm the QZ root at infinity is recovered, no NaN/Inf leakage.

Risks and mitigations

  • Q-rank degeneracy on Pieper arms. Puma's Q has rank ≤7 for some poses; rare but real. Mitigation: SVD with explicit rank threshold matching MC §IV-B.
  • Singular pencils (det M(x) ≡ 0): the regular branches at hc10 near_singular/9 sit at roots whose null space is 3-dimensional and are read by the x_lb1 shift (#673); the pose's singular branches are a solution continuum (#662).
  • POE → DH conversion correctness. Already shipped in kinematics.poe, but the JACO 2 fixture will be the first non-orthogonal-twist exercise; audit before relying on it.
  • Newton non-convergence. Cap at 5 iterations; if residual still > 1e-6, drop the candidate and flag in diagnostics.

Files to create

  • src/ssik/solvers/ikgeo/general_6r.py — solver module.
  • src/ssik/solvers/ikgeo/_raghavan_roth.py — private: build_PQ, eliminate_q1_q2, build_M_matrix, extract_x4_x5, back_substitute. Sympy used at module import for symbolic P, Q derivation; pure NumPy at runtime.
  • tests/test_solver_general_6r.py — full bulletproof suite per validation plan above.
  • tests/fixtures/jaco2.py, tests/fixtures/agilex_piper.py — DH / mujoco_menagerie URDFs.

Files to modify

  • src/ssik/solvers/ikgeo/gen_six_dof.py → rename module to gen_six_dof_oracle.py. Vectorize the inner SP5 loop (separate PR after this one lands; ~10× faster, useful as the validation oracle).
  • README.md "Supported arms & solver coverage" section — promote JACO 2, Piper, Rizon 4 to ✅-with-tier-2 / ✅-with-jointlock.

Out of scope (track as separate issues)

  • Per-robot codegen (Phase M). Numeric RR's runtime is the constraint; codegen drops it to ~100 µs but isn't blocking v0.1.
  • Husty-Pfurner alternative path (paywalled paper; deferred).
  • Li-Woernle-Hiller cross-check oracle (Angeles 2014 §9; nice-to-have).

Estimated effort

5–7 days, broken roughly: - Day 1–2: sympy derivation of P, Q symbolic forms + numerical build_PQ callable + unit tests against MC Table I. - Day 3: matrix construction, eigenvalue route, back-substitution. - Day 4: conditioning fallback (Möbius + generalized-eigenvalue). - Day 5: Newton refinement, verification, dedup. - Day 6–7: bulletproof validation suite, JACO 2 + Piper fixtures, cross-solver agreement on 500 hypothesis poses, ship PR.