source/bimanual/yam/solvers/handover.py

"""Left arm picks the object, passes it to the right arm, right arm sets it down."""
import numpy as np

from ..motion.arm import OPEN, CLOSE, fillet, grasp_quat
from ..envs.scene import TABLE_TOP
from .base import approach_and_grasp, place_at


def solve(env, obj, target, handover_xy=(0.06, 0.0), max_gap=0.045, rim_grasp=False,
          rim_margin=0.010, grasp_top=None, jaw="auto", world_half_y=None):
    """`rim_grasp` takes the object by the EDGE nearest each arm instead of across its middle.

    A basket is 18.9 x 14.3 cm and the jaw opens 9.4 cm, so there is no across-the-middle grasp
    at all: the jaws stall at full opening and the clamp check (correctly) rejects it. Gripping
    the near rim closes across a ~1 cm wall, and it is also what a person does.
    """
    rec = env.recorder
    L, R = env.arms["left"], env.arms["right"]
    ext0 = env.scene.object_size(obj)
    # left arm sits at +y, so its rim is the +y edge; the right arm takes the -y edge later
    off_l = (0.0, float(ext0[1])/2.0-rim_margin) if rim_grasp else (0.0, 0.0)
    res, ext = approach_and_grasp(env, L, obj, max_gap=max_gap, grasp_offset=off_l,
                                  grasp_top=grasp_top, jaw="y" if rim_grasp else jaw,
                                  prefix="[L] ")
    if not res.ok:
        rec.phase = "ABORT: left arm never grasped it"
        return {"handed": False, "reason": res.reason}

    rec.phase = "[L] 4. CARRY to the handover pose"
    mid = L.to_root(np.array([handover_xy[0], handover_xy[1], TABLE_TOP+0.18], np.float32))
    L.flow(fillet([L._seg_start(), L._seg_start()+np.array([0, 0, 0.14], np.float32), mid])[1:], CLOSE)

    live = env.scene.object_pos(obj)
    # object_size() is the AUTHORED bbox and ignores yaw, so a yawed object's world extent is
    # not ext[]. The task passes the rotated half when it has yawed the object.
    half = float(world_half_y) if world_half_y is not None else float(ext[1])/2.0
    # The second hand must close along the SAME axis as the first -- the other axis is the
    # object's length and does not fit the jaw at all (11.7 cm against a 9.4 cm opening, which
    # is why the right hand kept closing on air). It takes a different SPOT along that length:
    # the end nearer itself, so the two hands are never fighting for the same few centimetres.
    R.quat = grasp_quat(jaw if jaw in ("x", "y") else "y")
    grab_y = live[1]-(half*0.55 if not rim_grasp else half-rim_margin)
    Rg = R.to_root(np.array([live[0], grab_y, live[2]], np.float32))
    print(f"[solver] right hand takes the {jaw}-axis grip at y={grab_y:+.3f} "
          f"(object centre {live[1]:+.3f}, half-length {half:.3f})", flush=True)
    rec.phase = "[R] 5. RIGHT approaches the far rim"
    R.move_to(Rg+np.array([0, 0, 0.12], np.float32), OPEN, tol=0.02, max_steps=120)
    R.move_to(Rg, OPEN, tol=0.01, max_steps=100)
    rec.phase = "[R] 6. RIGHT closes (both hold)"
    # verify_lift=0: the object cannot rise while the left hand still holds it
    gr = R.grasp(lambda: env.scene.object_pos(obj)[2], max_gap=max_gap, verify_lift=0.0)
    rec.phase = "[L] 7. LEFT releases"
    for _ in range(45):
        L._drive(L._seg_start(), OPEN)
    L.flow([L._seg_start()+np.array([0, 0.10, 0.10], np.float32)], OPEN)
    if not gr.ok:
        rec.phase = "ABORT: right arm never took it"
        return {"handed": False, "reason": gr.reason}
    held = float(env.scene.object_pos(obj)[2]) > TABLE_TOP+0.05
    print(f"[solver] handover: right holds it above the table = {held}", flush=True)
    err = place_at(env, R, target, float(ext[2])/2.0, prefix="[R] ")
    return {"handed": held, "place_err": err}