armcade

"""Regression tests for the release-before-move gate in CartesianController.control_step.

Reproduces the live "took control, session started, arm never moved" bug and proves the fix.

Root cause: go_home()/go_rest() queue the joint move in ``_pending_path`` and hold the arm until the
gripper has opened. The gate used to slew ``self.gripper`` toward the operator's LIVE
``commanded_gripper``; a held grip-CLOSE jog during the transition dragged that target below the release
threshold, so the gate never fired, the move never started, and the arm sat wedged forever (with
is_settling() stuck True, which also wedged the one-arm-at-a-time cross-arm gate). The fix slews toward a
FIXED open target and promotes after a bounded timeout regardless.

No hardware: builds a controller over the Tom URDF and drives control_step directly.
Run: ``python -m tomctl.tests.test_release_gate`` (from robot/).
"""

from __future__ import annotations

from pathlib import Path

from tomctl import control as cc

OK = True


def check(name: str, cond: bool) -> None:
    global OK
    print(f"  [{'PASS' if cond else 'FAIL'}] {name}")
    OK = OK and bool(cond)


def _build_ctrl() -> cc.CartesianController:
    urdf = Path(cc.__file__).resolve().parent.parent / "lerobot" / "robots" / "tom" / "tom.urdf"
    return cc.CartesianController(cc.TomKinematics(str(urdf)))


def _run_until_promote(ctrl, *, grip_close_hold: bool, dt: float = 0.02, max_s: float = 6.0):
    """Tick control_step until the queued _pending_path promotes to a real move (or max_s elapses).
    grip_close_hold mimics an operator holding the CLOSE-gripper key throughout the transition — the
    per-tick command_delta runs BEFORE control_step, exactly as the main loop orders apply()+control_step.
    Returns (promoted: bool, elapsed_s: float, commanded_gripper_at_promote: float)."""
    t = 0.0
    steps = int(max_s / dt)
    for _ in range(steps):
        if grip_close_hold:
            ctrl.command_delta(0.0, 0.0, 0.0, -100.0)  # drive commanded_gripper hard toward CLOSED
        ctrl.control_step(dt)
        t += dt
        if ctrl._pending_path is None:  # gate promoted -> the move has started
            return True, t, ctrl.commanded_gripper
    return False, t, ctrl.commanded_gripper


def main() -> int:
    open_to = cc.GRIPPER_LIMITS[1]
    thresh = open_to - 0.5

    # 1) Clean release: no adverse input -> gate promotes quickly (well under the timeout).
    c = _build_ctrl()
    c.gripper = cc.GRIPPER_LIMITS[0]  # start fully CLOSED (holding something)
    c.go_home()
    check("go_home queues a release-before-move (_pending_path set, is_settling True)",
          c._pending_path is not None and c.is_settling())
    promoted, elapsed, _ = _run_until_promote(c, grip_close_hold=False)
    check("clean release promotes the move", promoted)
    check("clean release is fast (< 1s, not the timeout)", promoted and elapsed < 1.0)

    # 2) THE BUG: hold grip-CLOSE through the whole home transition. The old gate slewed toward the live
    #    commanded_gripper, which this drives to ~0 -> it could never reach the threshold -> permanent
    #    stall. The fix slews toward the FIXED open target, so it still promotes.
    c = _build_ctrl()
    c.gripper = cc.GRIPPER_LIMITS[0]
    c.go_home()
    promoted, elapsed, cmd_at_promote = _run_until_promote(c, grip_close_hold=True)
    check("grip-CLOSE held during home STILL promotes the move (arm moves)", promoted)
    # Proof the old commanded_gripper-based gate would have stalled here: commanded_gripper is far below
    # the release threshold at the moment the (fixed-target) gate fired.
    check("repro is valid: commanded_gripper was driven below the old threshold",
          cmd_at_promote < thresh)
    check("grip-CLOSE promotion still bounded (<= timeout + margin)",
          promoted and elapsed <= cc.PENDING_RELEASE_TIMEOUT_S + 0.5)

    # 3) Timeout backstop: freeze the gripper slew entirely (speed 0) so the position can NEVER reach the
    #    threshold. The gate must still promote via the bounded timeout, not wait forever.
    saved = cc.GRIPPER_SPEED_PPS
    cc.GRIPPER_SPEED_PPS = 0.0
    try:
        c = _build_ctrl()
        c.gripper = cc.GRIPPER_LIMITS[0]
        c.go_home()
        promoted, elapsed, _ = _run_until_promote(c, grip_close_hold=False)
        check("frozen gripper still promotes via timeout backstop", promoted)
        check("timeout fires around PENDING_RELEASE_TIMEOUT_S",
              promoted and cc.PENDING_RELEASE_TIMEOUT_S <= elapsed <= cc.PENDING_RELEASE_TIMEOUT_S + 0.3)
    finally:
        cc.GRIPPER_SPEED_PPS = saved

    print("RELEASE-GATE TESTS:", "PASS" if OK else "FAIL")
    return 0 if OK else 1


if __name__ == "__main__":
    raise SystemExit(main())