source/bimanual/yam/solvers/articulate.py

"""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}