level_fork is a TODO stub — blocks transfer_to_mouth post-acquisition

[siddhss5]

Symptom

The full feed cycle (feed()) reliably reaches the acquisition phase post-ssik (#33) but fails at transfer_to_mouth with AllGoalConfigurationsInvalid: 100 IK unreachable — the approach_mouth TSR has no reachable goals from the post-extract configuration.

Reproduction:

# In: uv run python -m ada_mj --demo feeding --viser
food_items()                    # ok
move_above_food(food_items()[0])   # ok (deterministic, ssik)
robot.go_to("staging"); robot.articutool.set_tilt(np.pi/2); transfer()  # ok!
# But feed() (full cycle) hits the same TSR-unreachable failure as transfer() without the pre-tilt.
feed()                          # fails at step 8 (transfer_to_mouth)

Root cause

level_fork in feeding/behaviors.py is a placeholder:

def level_fork(*, arm, ctx):
    # TODO: compute gravity-compensating tilt from arm FK.
    # ...
    # For now, set tilt to 0 (horizontal).
    return tilt_fork(0.0, ctx=ctx)

It hardcodes tilt = 0. But for the mouth approach TSR to be reachable from the post-acquisition arm config, the articutool needs to be tilted to roughly +π/2 (confirmed by sweeping tilts: +π/2 → 18/20 IK successes, 0.0 → 0/20). The "level" name is aspirational; the implementation isn't doing the geometric leveling its docstring describes.

What "level" should actually mean

Per the docstring, level_fork is supposed to keep the food horizontal during transport — i.e., make the fork tine parallel to the ground regardless of the arm's current EE pose. That's a function of the EE pitch:

articutool_tilt_target = - (EE pitch around world horizontal axis) - (fork-tip-z offset from articutool frame)

The exact formula depends on the articutool's kinematics (joint axes, link offsets) and which axis "horizontal" means in the user's frame. The current stub punts on this.

Recommended fix

Compute the gravity-compensating tilt from arm.get_ee_pose():

1. Take the current EE rotation matrix.
2. Extract the rotation between EE's z-axis and world's −z axis (pitch).
3. Set articutool/atool_joint1 to a tilt that places the fork tine parallel to world's horizontal plane.

A simple approximation that's likely good enough: solve for tilt such that the fork-tip site's z-axis in world frame is horizontal. Can be done analytically given the articutool's kinematics or numerically (one Jacobian step).

Workarounds in the meantime

• Demo helper (ada_mj/demos/feeding.py): the transfer() helper documents the manual tilt step.
• Schemas: an AcquisitionSchema could carry a transfer_tilt: float field set by the operator who designed the schema, sidestepping the geometry. Same place tilt_angle lives. Cleanest if we don't want to compute leveling automatically.

Related

• Surfaced after jaco2: wire ssik analytical IK (replaces mink fallback) #33 (ssik) made the IK deterministic enough to actually pinpoint this as the next bottleneck. Pre-ssik this was masked by mink's tilt-sensitive flaky behavior.
• See also move_above: TSR reachability depends on the pre-tilt (no fallback) #30 (per-tilt reachability) — that issue described the symptom; this one identifies the specific behavior that triggers it in the happy path.