| """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 |
| JAW_SAFE = 0.078 |
|
|
|
|
| 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) |
| |
| |
| err = float(np.linalg.norm(arm.eef()-place)) |
| rec.phase = f"{prefix}6. RETREAT" |
| arm.flow([above], OPEN) |
| return err |
|
|
