File size: 1,437 Bytes
7399b6f | 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 | """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
|