move_above: TSR reachability depends on the pre-tilt (no fallback)

[siddhss5]

Context

The TSR-based move_above (PR #29) freezes the articutool's joints at planning time — T_ee_to_fork_tip is captured once into the TSR's Tw_e, and only the 6 arm joints are in the planning DOFs.

That means the planner can't compensate for a bad tilt by curling the articutool. The articutool tilt is an input that the caller picks (via tilt_fork(schema.tilt_angle) in task.py); planning either succeeds for that tilt or fails outright with PLANNING_FAILED: no_path / AllGoalConfigurationsInvalid.

Observed sensitivity

strawberry_0 at world (0.113, −0.480, 0.774), JACO2_RESTING start:

articutool tilt	EE-to-tip distance	TSR sample IK success	plan_to_tsrs
0 (default)	0.42 m	0 / 100	fails (100 IK unreachable)
−π/4 (straight_skewer)	0.38 m	~4 / 10	succeeds

The straight_skewer tilt was tuned to be feasible. A neighborhood around it is fine. Outside that, planning silently fails.

What we don't currently handle

1. Foods near the arm's reach boundary. Even at the schema's tilt, food positions at the far edge of the plate may have no reachable TSR.
2. Schema/tilt mismatches. A new schema with a different tilt (say +π/3 for a funky food shape) may not be reachable for the chosen food, and the failure mode is silent.
3. Per-attempt re-tilt. No mechanism right now for "if this tilt didn't work, try another."

Possible directions

1. Better diagnostics on failure — when plan_to_tsrs fails, sample a few EE goals from the TSR and report which arm-IK constraint they violated (limits, reach, self-collision). Cheap, helps debugging, doesn't change the design.
2. Articutool tilt as a planning DOF — add the 2 articutool joints to the IK / TSR machinery so the optimizer picks arm + tilt jointly. Most general fix, but loses the deterministic "skewer angle = schema.tilt_angle" semantic, and requires changes in ForkTSR, the IK solver, and plan_to_tsrs.
3. Re-plan loop in move_above — try the schema tilt first; if planning fails, sweep nearby tilts and re-plan. Cheap, keeps the schema-as-input contract, but adds latency on failures.

My lean: start with (1) — it's small and helps everywhere — and only invest in (2) or (3) if real failures show up.

Related

• Depends on the design decision in move_above: plan to a ForkTSR placement TSR over the food #29 (snapshot articutool, plan arm only).
• Adjacent to tilt_fork doesn't settle in physics mode — breaks move_above TSR planning #28 (tilt_fork doesn't settle in physics) — that one breaks even the happy path.

[siddhss5]

Update after further investigation: the root cause of the tilt-sensitive reachability is JACO 2 IK falling all the way to mink, not the TSR-as-design.

EAIK has no analytical decomposition for JACO 2's non-Pieper 6R geometry (the runtime emits EAIK has no known decomposition for this arm: 6R-Unknown Kinematic Class on every ADA() construction). The factory then falls back to mink, which is a single-seed numerical Jacobian solver — it converges to one IK per sample, and which one depends on the seed. That explains everything we've been seeing:

• IK success rate on TSR samples changes dramatically with the chosen articutool tilt (was 0/100 at tilt=0, 4/10 at −π/4, 19/20 at +π/2 from STAGING).
• The "right tilt" varies by start config too, because the seed changes.
• No branch enumeration: each TSR sample asks mink for one IK, and the planner discards the sample if that one IK fails — even when other branches would have been reachable.

Tracking the fix upstream as mj_manipulator#158 — wire up the ssik analytical IK package (ssik covers exactly the non-Pieper 6R and 7R geometries EAIK refuses, returns every analytical branch per pose, supports q_seed for nearest-branch teleop). Dispatch order becomes EAIK → ssik → mink, with ssik picking up JACO 2.

Once ssik is wired:

• The feeding demo's TSR-based behaviors (move_above, transfer_to_mouth) should be reachability-deterministic.
• Per-tilt reachability sweeps become uninteresting (every reasonable tilt yields full branch counts).
• The diagnostic-quality suggestion in this issue can be deferred — the underlying failure mode largely disappears.

I'll come back here once mj_manipulator#158 lands to either close this issue or pare it back to whatever residual reachability concerns remain.