File size: 8,547 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 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 | """Shared grasp/place primitives every solver builds on."""
from __future__ import annotations
import numpy as np
from ..motion.arm import OPEN, CLOSE, grasp_quat, fillet
from ..motion.planner import plan_path
from ..envs.scene import TABLE_TOP
def grasp_pose(env, name, grasp_top=None, offset=(0.0, 0.0), live_z=False):
"""LIVE xy from physics + a height from the measured size. Never the bbox for position.
`offset` shifts the grip along world xy away from the centroid. A long part only presents a
graspable stretch at one end -- a bar sitting in a sleeve has its centroid buried inside the
housing, so gripping the centroid means gripping the housing.
`live_z` takes the height from physics too. The default assumes the object is standing on the
TABLE, which is wrong for anything resting on something else: a lid sitting on a 7.5 cm pot
rim is grasped 7.5 cm too low, so the hand drives into the pot wall and the jaws shut beside
the lid. Pass live_z=True whenever the object starts on a fixture.
"""
live = env.scene.object_pos(name)
ext = env.scene.object_size(name)
base = float(live[2])-float(ext[2])/2.0 if live_z else TABLE_TOP
z = base+float(ext[2])-float(grasp_top) if grasp_top is not None else base+float(ext[2])/2.0
return np.array([live[0]+float(offset[0]), live[1]+float(offset[1]),
max(z, TABLE_TOP+0.010)], np.float32), ext
JAW_MAX = 0.094 # measured full opening of the YAM gripper
JAW_SAFE = 0.078 # beyond this the fingers clip the object while descending
def check_graspable(env, name, jaw=None):
"""Warn when the axis the jaws must close across is too wide for them.
Every "jaw closed beside it" failure in this suite traces back to an object scaled past the
9.4 cm opening: the fingers strike its sides on the way down, the descent stalls a few
centimetres short, and the clamp then shuts on air. It is invisible in the numbers unless
someone divides the bbox by the jaw, so do it here.
"""
ext = env.scene.object_size(name)
axis = jaw if jaw in ("x", "y") else auto_jaw(env, name)
w = float(ext[0] if axis == "x" else ext[1])
if w > JAW_SAFE:
print(f"[solver] WARNING: {name} is {w*100:.1f} cm across the '{axis}' axis, but the jaw "
f"opens {JAW_MAX*100:.1f} cm (safe <= {JAW_SAFE*100:.0f} cm). The fingers will clip "
f"it on the way down -- scale the asset down or grip a narrower feature.", flush=True)
return w
def auto_jaw(env, name):
"""Close across the NARROWER horizontal extent -- the jaw only opens 9.4 cm."""
ext = env.scene.object_size(name)
return "x" if float(ext[0]) < float(ext[1])*0.9 else "y"
def approach_and_grasp(env, arm, name, jaw="auto", grasp_top=None, max_gap=0.045,
grasp_yaw=0.0, grasp_offset=(0.0, 0.0), verify_lift=None, live_z=False,
prefix=""):
"""PRM approach -> closed-loop descend -> clamp -> VERIFIED grasp."""
rec = env.recorder
target, ext = grasp_pose(env, name, grasp_top, offset=grasp_offset, live_z=live_z)
check_graspable(env, name, None if jaw == "auto" else jaw)
arm.quat = grasp_quat(auto_jaw(env, name) if jaw == "auto" else jaw, yaw_deg=grasp_yaw)
goal = arm.to_root(target)
rec.phase = f"{prefix}1. PRM APPROACH"
arm.flow(plan_path(arm.eef(), goal+np.array([0, 0, 0.12], np.float32), arm.root), OPEN)
rec.phase = f"{prefix}2. DESCEND onto {name}"
err = arm.move_to(goal, OPEN, tol=0.008, max_steps=140)
arm.hold(10, OPEN)
print(f"[solver] descend err={err:.3f} target={np.round(goal,3)}", flush=True)
rec.phase = f"{prefix}3. CLOSE-GRASP"
kw = {} if verify_lift is None else {"verify_lift": verify_lift}
res = arm.grasp(lambda: env.scene.object_pos(name)[2], max_gap=max_gap, **kw)
print(f"[solver] grasp: {res.reason}", flush=True)
return res, ext
def place_into(env, arm, region, half_h, approach=(0.0, -1.0), standoff=0.16, tilt_deg=0.0,
prefix=""):
"""Slide an object in through a fixture's open face, optionally TILTED.
place_at() carries over the target and lowers straight down, which is impossible for anything
with a roof: the cupboard's top panel blocks the descent, so the object is released above the
unit and lands 11 cm away on the table. Here the hand rises to shelf height OUTSIDE the
opening, drives in, releases, and backs out the way it came.
`tilt_deg` leans the wrist so the object enters nose-first. A cupboard opening is barely
taller than the box, and a level box catches its top corner on the shelf above; tilted, the
leading edge goes under the lip first and the box levels out as it is released.
"""
rec = env.recorder
reg = env.scene.regions[region]
level_quat = arm.quat.copy()
d = np.asarray(approach, float)
d = d/(np.linalg.norm(d)+1e-9)
z = float(reg.get("shelf_z", reg.get("top_z", TABLE_TOP)))+half_h+0.008
centre = np.array([reg["xy"][0], reg["xy"][1]], float)
outside = centre-d*standoff
rec.phase = f"{prefix}4. RISE to shelf height, clear of the opening"
arm.flow([arm._seg_start()+np.array([0, 0, 0.10], np.float32)], CLOSE)
arm.move_to(arm.to_root(np.array([outside[0], outside[1], z+0.02], np.float32)), CLOSE,
tol=0.012, max_steps=150)
if abs(tilt_deg) > 1e-6:
rec.phase = f"{prefix}4b. TILT the box nose-down to clear the shelf lip"
arm.quat = grasp_quat(auto_jaw(env, env._last_placed) if getattr(env, "_last_placed", None)
else "y", tilt_deg=abs(tilt_deg),
tilt_sign=1.0 if tilt_deg > 0 else -1.0)
arm.hold(18, CLOSE)
rec.phase = f"{prefix}5. SLIDE IN through the open face"
arm.move_to(arm.to_root(np.array([outside[0], outside[1], z], np.float32)), CLOSE,
tol=0.010, max_steps=90)
err = arm.move_to(arm.to_root(np.array([centre[0], centre[1], z], np.float32)), CLOSE,
tol=0.012, max_steps=170)
if abs(tilt_deg) > 1e-6:
rec.phase = f"{prefix}5b. LEVEL OFF so the box sets down flat"
arm.quat = level_quat
arm.hold(22, CLOSE)
rec.phase = f"{prefix}6. RELEASE on the shelf"
arm.hold(20, OPEN)
rec.phase = f"{prefix}7. BACK OUT the way it came in"
arm.move_to(arm.to_root(np.array([outside[0], outside[1], z], np.float32)), OPEN,
tol=0.02, max_steps=140)
arm.flow([arm._seg_start()+np.array([0, 0, 0.14], np.float32)], OPEN)
fin = env.scene.object_pos(env._last_placed) if hasattr(env, "_last_placed") else None
print(f"[solver] shelf insert tracking err {err:.3f}"
+ (f", object at {np.round(fin, 3)}" if fin is not None else ""), flush=True)
return err
def place_at(env, arm, region, half_h, on_top=False, lift=0.16, dx=0.0, prefix=""):
"""Carry to a region and set down as ONE filleted arc, then release.
`dx` spreads multiple objects inside the region. It is applied TANGENTIALLY (perpendicular to
the arm->region direction) rather than along world x: an x-offset pushes one drop point
further from the shoulder than the other, and past ~0.35 m the arm simply cannot get there,
so the second object of a pair would fail while the first succeeded.
"""
rec = env.recorder
reg = env.scene.regions[region]
rim = float(reg.get("top_z", TABLE_TOP))-TABLE_TOP
drop_z = TABLE_TOP+(rim+half_h+0.006 if on_top else max(0.010, rim)+half_h+0.02)
centre = np.array([reg["xy"][0], reg["xy"][1]], np.float64)
radial = centre-np.asarray(arm.root[:2], np.float64)
n = np.linalg.norm(radial)
tangent = np.array([-radial[1], radial[0]])/n if n > 1e-6 else np.array([1.0, 0.0])
spot = centre+tangent*dx
place = arm.to_root(np.array([spot[0], spot[1], drop_z], np.float32))
above = place+np.array([0, 0, 0.10], np.float32)
top = arm._seg_start()+np.array([0, 0, lift], np.float32)
rec.phase = f"{prefix}4. LIFT + CARRY to {region}"
arm.flow(fillet([arm._seg_start(), top, above, place])[1:], CLOSE)
rec.phase = f"{prefix}5. RELEASE"
arm.move_to(place, OPEN, tol=0.01, max_steps=45)
# measure BEFORE retreating: measuring after reports how far the arm backed off, not how
# accurately it placed (that read 0.37 m on an episode that placed the can perfectly)
err = float(np.linalg.norm(arm.eef()-place))
rec.phase = f"{prefix}6. RETREAT"
arm.flow([above], OPEN)
return err
|