source/bimanual/yam/solvers/stack.py

"""Stack objects one on top of another on a target region."""
import numpy as np

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


def solve(env, objects, target, arm="right", jaw="auto", max_gap=0.045):
    a = env.arms[arm]
    rec = env.recorder
    reg = env.scene.regions[target]
    out = {}
    for layer, name in enumerate(objects):
        pre = f"[{layer+1}] "
        res, ext = approach_and_grasp(env, a, name, jaw=jaw, max_gap=max_gap, prefix=pre)
        if not res.ok:
            out[name] = {"grasped": False, "reason": res.reason}
            continue
        z = TABLE_TOP+0.002+float(ext[2])*(layer+0.5)
        place = a.to_root(np.array([reg["xy"][0], reg["xy"][1], z], np.float32))
        above = place+np.array([0, 0, 0.12], np.float32)
        rec.phase = f"{pre}4. CARRY over the target"
        a.flow(fillet([a._seg_start(), res.hold_pose+np.array([0, 0, 0.16], np.float32), above])[1:], CLOSE)
        # stacking is the precision case: an open-loop drop misses by more than a block width
        rec.phase = f"{pre}5. PLACE (closed loop)"
        err = a.move_to(place, CLOSE, tol=0.005, max_steps=170)
        rec.phase = f"{pre}6. RELEASE"
        a.move_to(place, OPEN, tol=0.02, max_steps=40)
        rec.phase = f"{pre}7. RETREAT"
        a.flow([above], OPEN)
        out[name] = {"grasped": True, "place_err": err}
    return out