File size: 1,599 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 | """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}
|