File size: 3,656 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 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 | """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}
|