"""Drive a jointed fixture: swing a hinged door/lid open, or draw a slider out. The distinction that matters is the PATH the hand must follow, not the grasp. A drawer front travels in a straight line, so a straight pull works. A door edge travels on a circle about its hinge, so pulling straight back binds it after a few centimetres -- the hand has to follow the arc. Both are handled here so a task file only says which kind of joint it is. """ import numpy as np from ..motion.arm import OPEN, CLOSE, grasp_quat from ..motion.planner import plan_path from ..envs.scene import TABLE_TOP def _arc(pivot, start, sweep, n=26): """Points along a circle about `pivot` from `start`, turning by `sweep` radians.""" r = start[:2]-pivot[:2] out = [] for k in range(1, n+1): t = sweep*k/n c, s = np.cos(t), np.sin(t) p = pivot[:2]+np.array([c*r[0]-s*r[1], s*r[0]+c*r[1]]) out.append(np.array([p[0], p[1], start[2]], np.float32)) return out def solve(env, fixture, kind="hinge", arm="right", grip_xy=None, grip_z=0.06, pivot_xy=None, sweep_deg=75.0, distance=0.14, direction=(1.0, 0.0), approach_from=(0.0, -1.0), open_jaw=True, press=0.0, grip_tilt=0.0, jaw="y"): """Pull a door/lid/drawer open. `grip_xy` where on the fixture to take hold (world xy, env-relative). This is the handle or the free edge -- NOT the fixture's centre, which is inside the body. `pivot_xy` the hinge line, required for kind="hinge". `open_jaw` hook the OPEN jaw behind the edge and drag, instead of clamping. Most of these panels are thin plates with no graspable handle, and a clamp on a 5 mm plate either misses or wedges; a hooked jaw pulls just as well and never slips off. """ a = env.arms[arm] rec = env.recorder # Baseline the joints HERE, the instant before the robot touches anything. Recording them at # build time counts the fixture's own settling under gravity as progress -- switch_toggle # "passed" on 0.44 rad of falling and 0.008 rad of actual work. # Step first: joint_pos is a cached buffer, and reading it without a step returns the value # from build time (~0). That stale zero is why switch_toggle kept "passing" -- the check saw # 0 -> 0.487 when the robot had actually moved the joint 0.049. for _ in range(4): env.step() env._start_joints = env._joint_state() grip = np.asarray(grip_xy, float) hold = OPEN if open_jaw else CLOSE z = TABLE_TOP+float(grip_z) if abs(grip_tilt) > 1e-6: # LEAN THE WRIST. A drawer front is a vertical face: a top-down hand can only press on # its top lip, and pressing down is nearly useless for pulling. Tilted toward horizontal # the fingers straddle the lip -- one in front, one behind -- so the pull has something # to act against instead of relying on friction. a.quat = grasp_quat(jaw, tilt_deg=abs(grip_tilt), tilt_sign=1.0 if grip_tilt > 0 else -1.0) stand = grip+np.asarray(approach_from, float)*0.06 rec.phase = "1. APPROACH the panel edge" a.flow(plan_path(a.eef(), a.to_root(np.array([stand[0], stand[1], z+0.14], np.float32)), a.root), hold) rec.phase = "2. LOWER alongside the edge" a.move_to(a.to_root(np.array([stand[0], stand[1], z], np.float32)), hold, tol=0.010, max_steps=130) rec.phase = "3. HOOK behind the edge" a.move_to(a.to_root(np.array([grip[0], grip[1], z], np.float32)), hold, tol=0.010, max_steps=110) if press > 0.0: # A drawer front is flat with no handle, so there is nothing to clamp. What it does have # is a lip standing ~1.6 cm proud of the cabinet: press the closed jaw down onto that lip # and drag. The joint has zero stiffness and light damping, so friction is enough. rec.phase = "3b. PRESS DOWN onto the lip" a.move_to(a.to_root(np.array([grip[0], grip[1], z-press], np.float32)), CLOSE, tol=0.006, max_steps=90) hold = CLOSE elif not open_jaw: a.grasp(lambda: z, max_gap=0.05, verify_lift=0.0) q0 = env.scene.joint_pos(fixture, 0) if kind == "press": # A toggle/button rotates about a HORIZONTAL axis, so it flips in the vertical plane and # an in-plane arc sweep does nothing to it. The motion is a straight push down onto the # button face -- which is also what a finger does. rec.phase = "4. PRESS the button straight down" a.move_to(a.to_root(np.array([grip[0], grip[1], z-float(distance)], np.float32)), CLOSE, tol=0.004, max_steps=180) a.hold(25, CLOSE) elif kind == "hinge": if pivot_xy is None: raise ValueError("kind='hinge' needs pivot_xy (the hinge line)") pivot = np.array([pivot_xy[0], pivot_xy[1], z], float) rec.phase = f"4. SWING the door open ({sweep_deg:.0f} deg about its hinge)" # sign the sweep so the hand turns AWAY from the fixture body, whichever side the # hinge is on r = grip-np.asarray(pivot_xy, float) sign = 1.0 if np.cross(r, np.asarray(direction, float)) > 0 else -1.0 path = _arc(pivot, np.array([grip[0], grip[1], z], np.float32), sign*np.deg2rad(sweep_deg)) a.flow([a.to_root(p) for p in path], hold, speed=0.006) else: d = np.asarray(direction, float) d = d/(np.linalg.norm(d)+1e-9) rec.phase = "4. DRAW the drawer straight out" target = np.array([grip[0]+d[0]*distance, grip[1]+d[1]*distance, z], np.float32) a.move_to(a.to_root(target), hold, tol=0.012, max_steps=220) q1 = env.scene.joint_pos(fixture, 0) rec.phase = "5. WITHDRAW" a.flow([a._seg_start()+np.array([0, 0, 0.16], np.float32)], OPEN) # Record what THIS action did, and hand it to the checker. Comparing against a baseline the # env took earlier counts anything that happened in between -- laptop_close "passed" on a # joint that had already blown to -7.99 rad (its limit is -2.34) before the robot moved. if not hasattr(env, "_joint_delta"): env._joint_delta = {} env._joint_delta[(fixture, 0)] = abs(q1-q0) lim = env.scene.articulations[fixture].data.joint_pos_limits[0, 0].cpu().numpy() sane = float(lim[0])-0.05 <= q1 <= float(lim[1])+0.05 print(f"[solver] {fixture} joint 0: {q0:+.3f} -> {q1:+.3f} (moved {abs(q1-q0):.3f})" + ("" if sane else f" !! OUTSIDE its limits {np.round(lim,3)} -- the joint blew up, " "this is not a real result"), flush=True) return {"joint_start": q0, "joint_end": q1, "moved": abs(q1-q0), "in_limits": sane}