source/bimanual/yam/solvers/pull.py

"""Pull a bar straight out of a slotted block: grasp the protruding end, draw it along one axis."""
import numpy as np

from ..motion.arm import OPEN, CLOSE
from .base import approach_and_grasp


def solve(env, obj, direction=(1.0, 0.0), distance=0.16, arm="right", max_gap=0.03,
          grasp_offset=(0.0, 0.0)):
    a = env.arms[arm]
    rec = env.recorder
    start = env.scene.object_pos(obj).copy()
    # verify_lift=0: a bar captive in a channel CANNOT rise, so a lift check always "fails" it.
    # The pull distance below is the real verification.
    res, ext = approach_and_grasp(env, a, obj, jaw="y", max_gap=max_gap,
                                  grasp_offset=grasp_offset, verify_lift=0.0)
    if not res.ok:
        rec.phase = "ABORT: the bar was never grasped"
        return {"pulled": False, "reason": res.reason}
    d = np.asarray(direction, float)
    d = d/(np.linalg.norm(d)+1e-9)
    rec.phase = "4. PULL straight out along the slot"
    # constrained motion: no lift, no turn -- any off-axis component jams the bar in the channel
    target = res.hold_pose+np.array([d[0]*distance, d[1]*distance, 0.0], np.float32)
    err = a.move_to(target, CLOSE, tol=0.01, max_steps=200)
    moved = float(np.linalg.norm(env.scene.object_pos(obj)[:2]-start[:2]))
    rec.phase = "5. RELEASE"
    a.move_to(target, OPEN, tol=0.02, max_steps=40)
    rec.phase = "6. RETREAT"
    a.flow([target+np.array([0, 0, 0.12], np.float32)], OPEN)
    print(f"[solver] bar travelled {moved:.3f} m (track err {err:.3f})", flush=True)
    return {"pulled": moved > distance*0.55, "moved": moved}