# --------------------------------------------------------------------------------------------------------
# step constructors
# --------------------------------------------------------------------------------------------------------


def MV(p, yaw=0.0, pitch=0.0, g=-1.0, v=1.0, tol=0.006, settle=False, rtol=0.06, nmin=0):
    """Move the grip site to p with hand orientation (yaw, pitch); g = gripper command; v = action cap."""
    return dict(kind="move", p=np.array(p, dtype=np.float64), yaw=float(yaw), pitch=float(pitch), g=float(g),
                v=float(v), tol=float(tol), settle=bool(settle), rtol=float(rtol), nmin=int(nmin))


def HOLD(g, n):
    """Keep the last commanded pose for n steps with gripper command g."""
    return dict(kind="hold", g=float(g), n=int(n))


# --------------------------------------------------------------------------------------------------------
# task geometry (world frame). Derivations: AUDIT.md "Derived facts" and REPORT_v1.md.
# --------------------------------------------------------------------------------------------------------
BOWL_RIM_R = 0.0485         # mid-wall radius of the bowl 1.3 cm below the rim (work/bowl_tilt.py)
BOWL_RIM_TOP = 0.0521       # bowl rim above the bowl origin (collision geometry)
BOWL_BOTTOM = 0.0016        # lowest bowl collision point above the bowl origin
BOWL_GRIP_BELOW_RIM = 0.013
# A bowl pinched by its 14.7-deg wall hangs tilted (far side down): its far bottom edge is 5.7 cm below the
# pinch point and its centre 3.7 cm from it; a level bowl would have 3.9 cm and 4.85 cm. The release height
# is set for the tilted case (safe for both); the horizontal offset is the mean of the two cases.
BOWL_LOW_BELOW_GRIP = 0.057
BOWL_PLACE_OFFSET = 0.042
PLATE_XY = (0.06, 0.20)     # plate_region centre (libero_spatial)
PLATE_FLOOR = 0.9079        # top of the plate's flat centre: 0.90 + 0.0025 + 0.0054
CAN_GRIP_H = 0.055          # grip-site height above the support for the 7.6 cm cans: pads at 5.1-6.7 cm, palm at 8.6 cm
CAN_Y_OFF = -0.0035         # the cans' collision cylinder is centred 3.5 mm to -y of the body origin
MILK_GRIP_H = 0.105         # pads 0.9 cm onto the carton walls, palm 4 mm above the roof ridge
BASKET_RIM = 0.1457 - 0.0042        # basket rim above its support (collision top - resting offset)
BASKET_XY = (0.0, 0.26)


def _pick_place_bowl(bowl_xy, rim_top_z, u, half_open_steps, approach_z, lift_z, yaw):
    """Rim grasp of a bowl at bowl_xy + BOWL_RIM_R*u (fingers along u), then release centred over the plate."""
    u = np.array(u, dtype=np.float64)
    u = u / np.linalg.norm(u)
    gx, gy = bowl_xy[0] + BOWL_RIM_R * u[0], bowl_xy[1] + BOWL_RIM_R * u[1]
    gz = rim_top_z - BOWL_GRIP_BELOW_RIM
    px, py = PLATE_XY[0] + BOWL_PLACE_OFFSET * u[0], PLATE_XY[1] + BOWL_PLACE_OFFSET * u[1]
    place_z = PLATE_FLOOR + 0.005 + BOWL_LOW_BELOW_GRIP          # lowest bowl point 5 mm above the plate
    g_pre = 0.0 if half_open_steps > 0 else -1.0
    steps = []
    if half_open_steps > 0:
        steps.append(HOLD(1.0, half_open_steps))                 # partially close: finger target 0.04 - 0.005*n
    steps += [
        MV((gx, gy, approach_z), yaw=yaw, g=g_pre, v=1.0, tol=0.006),
        MV((gx, gy, gz), yaw=yaw, g=g_pre, v=0.8, tol=0.004, settle=True),
        HOLD(1.0, 11),
        MV((gx, gy, lift_z), yaw=yaw, g=1.0, v=1.0, tol=0.012),
        MV((px, py, lift_z), yaw=yaw, g=1.0, v=1.0, tol=0.008),
        MV((px, py, place_z + 0.03), yaw=yaw, g=1.0, v=1.0, tol=0.008),
        MV((px, py, place_z), yaw=yaw, g=1.0, v=0.35, tol=0.004, settle=True),
        HOLD(-1.0, 8),
        MV((px, py, place_z + 0.12), yaw=yaw, g=-1.0, v=1.0, tol=0.012),
    ]
    return steps


def plan_task0():
    # bowl in the open top drawer of the wooden cabinet -> plate
    cab = np.array([0.03, -0.27])
    th = 0.5 * (2.6613777765410678 + 2.7242096296128633)
    opening = 0.15
    c, s = math.cos(th), math.sin(th)
    local = np.array([0.00328, 0.01128 - opening])            # top_region site xy in the cabinet frame, drawer open
    bowl = cab + np.array([c * local[0] - s * local[1], s * local[0] + c * local[1]])
    rim = 0.905 + 0.1592 - BOWL_BOTTOM + BOWL_RIM_TOP          # cabinet z + drawer floor top
    u = np.array([c, s])                                       # +x of the cabinet: toward the robot
    yaw = math.atan2(u[1], u[0]) - 0.5 * math.pi
    while yaw > 0.5 * math.pi:
        yaw -= math.pi
    while yaw <= -0.5 * math.pi:
        yaw += math.pi
    # lift to 1.25: the tilted bowl's lowest point (1.193) clears the drawer walls (1.122) and cabinet top (1.128)
    return _pick_place_bowl(bowl, rim, u, half_open_steps=4, approach_z=1.20, lift_z=1.25, yaw=yaw)


def plan_task1():
    # bowl next to the plate (next_to_plate_region centre) -> plate; grasp the rim on the robot side
    rim = Z_TABLE - BOWL_BOTTOM + BOWL_RIM_TOP
    return _pick_place_bowl((0.01, 0.31), rim, (-1.0, 0.0), 0, approach_z=1.03, lift_z=1.07, yaw=0.5 * math.pi)


def plan_task2():
    # bowl nested on the ramekin (ramekin_region centre) -> plate
    # bowl cone (r 0.027@z0.002 .. 0.040@z0.017) rests on the ramekin's inner top edge (r 0.0356, z 0.0425)
    bowl_origin = Z_TABLE + 0.0425 - 0.0119 + 0.0014
    rim = bowl_origin + BOWL_RIM_TOP
    return _pick_place_bowl((-0.20, 0.20), rim, (-1.0, 0.0), 0, approach_z=1.07, lift_z=1.09, yaw=0.5 * math.pi)


def _pick_to_basket(obj_xy, z0, grip_h, top_h, drop_xy, release_h, yaw=0.0):
    """Top-down grasp and release above the basket (fingertips stay above the rim)."""
    gz = z0 + grip_h
    approach = z0 + top_h + TIP_BELOW + 0.045
    carry = z0 + BASKET_RIM + 0.035 + grip_h                    # object bottom 3.5 cm above the rim
    rel = z0 + release_h
    return [
        MV((obj_xy[0], obj_xy[1], approach), yaw=yaw, g=-1.0, v=1.0, tol=0.006),
        MV((obj_xy[0], obj_xy[1], gz), yaw=yaw, g=-1.0, v=0.6, tol=0.004, settle=True),
        HOLD(1.0, 14),
        MV((obj_xy[0], obj_xy[1], carry), yaw=yaw, g=1.0, v=0.8, tol=0.012),
        MV((drop_xy[0], drop_xy[1], carry), yaw=yaw, g=1.0, v=1.0, tol=0.008),
        MV((drop_xy[0], drop_xy[1], rel), yaw=yaw, g=1.0, v=0.6, tol=0.006, settle=True),
        HOLD(-1.0, 12),
        MV((drop_xy[0], drop_xy[1], carry), yaw=yaw, g=-1.0, v=0.8, tol=0.012),
    ]


CAN_RELEASE_H = BASKET_RIM + 0.012 + TIP_BELOW                  # fingertips 1.2 cm above the basket rim
MILK_RELEASE_H = BASKET_RIM + 0.035 + TIP_BELOW


def plan_task3():
    return _pick_to_basket((-0.12, -0.24 + CAN_Y_OFF), Z_FLOOR, CAN_GRIP_H, 0.0762, BASKET_XY, CAN_RELEASE_H)


def plan_task4():
    return _pick_to_basket((-0.12, -0.24), Z_FLOOR, MILK_GRIP_H, 0.1318, BASKET_XY, MILK_RELEASE_H)


def plan_task5():
    # fingers along x: with fingers along y the 0.208 m palm would overlap the orange-juice carton at (0.10,-0.20)
    return _pick_to_basket((0.05, -0.10 + CAN_Y_OFF), Z_FLOOR, CAN_GRIP_H, 0.0762, BASKET_XY, CAN_RELEASE_H,
                           yaw=0.5 * math.pi)


# ---- task 6: wine bottle on the rack -------------------------------------------------------------------
BOTTLE_XY = (-0.20, -0.05)
BOTTLE_Z0 = Z_TABLE - 0.0011
BOTTLE_PINCH = 0.132            # pinch height on the neck above the bottle base (bottle is 0.158 tall)
RACK_XY = (-0.26, -0.26)
RACK_Z = 0.92                   # rack origin: 0.90 - 0.04 + 0.06
T6_PITCH = 50.0 * D2R


def plan_task6(bottle_xy):
    bx, by = bottle_xy
    gz = BOTTLE_Z0 + BOTTLE_PINCH
    # resting axis of a bottle lying against the stopper of the upper shelf (rack frame y=-0.0650, z=0.2452),
    # rack yaw = pi  ->  world y = rack_y + 0.0650, z = RACK_Z + 0.2452; uphill direction (world y,z) = (-0.855, 0.519)
    rest_y = RACK_XY[1] + 0.0650
    rest_z = RACK_Z + 0.2452
    base = np.array([RACK_XY[0] - 0.03, rest_y - 0.012 * 0.855, rest_z + 0.012 * 0.519])
    a = np.array([math.sin(T6_PITCH), 0.0, math.cos(T6_PITCH)])     # bottle axis (base -> top) when pitched
    rel = base + BOTTLE_PINCH * a
    return [
        MV((bx, by, gz + 0.075), g=-1.0, v=1.0, tol=0.005),
        MV((bx, by, gz), g=-1.0, v=0.5, tol=0.004, settle=True),
        HOLD(1.0, 14),
        MV((bx, by, gz + 0.16), g=1.0, v=0.6, tol=0.012),
        MV((rel[0], rel[1] + 0.06, rel[2] + 0.07), pitch=T6_PITCH, g=1.0, v=0.6, tol=0.008, settle=True),
        MV((rel[0], rel[1], rel[2] + 0.07), pitch=T6_PITCH, g=1.0, v=0.5, tol=0.006, settle=True),
        MV((rel[0], rel[1], rel[2]), pitch=T6_PITCH, g=1.0, v=0.3, tol=0.004, settle=True),
        HOLD(-1.0, 14),
        MV((rel[0] + 0.02, rel[1], rel[2] + 0.06), pitch=T6_PITCH, g=-1.0, v=0.5, tol=0.012),
    ]


# ---- task 7: turn on the stove -------------------------------------------------------------------------
STOVE7_XY = (-0.41, 0.21)       # stove_region centre (libero_goal); stove origin z = 0.905; knob axis at the origin
T7_PITCH = 20.0 * D2R


def plan_task7():
    # Push the +x end of the knob ridge toward +y (positive hinge rotation) with the closed fingers.
    # Hand pitched 20 deg about y (wrist away from the base: static IK margin 0.34 rad on joint 4; a vertical
    # hand leaves 0.1-0.16).  Closed-form finger geometry at 20 deg, relative to the grip site (x, z):
    #   lowest corner A = (+0.0067, -0.0122), inner corner B = (-0.0130, -0.0051), palm corner = (+0.0403, +0.0183).
    # Site at stove (x +0.032, z 0.9451): B at x_rel 0.019 / z 0.940 (ridge tip reaches x_rel 0.028-0.041 there),
    # A at z 0.9329 (2.4 mm above the burner plate, which starts at x_rel 0.055), palm 0.963 > ridge top 0.955.
    x = STOVE7_XY[0] + 0.032
    z = 0.9451
    y0 = STOVE7_XY[1] - 0.060
    y1 = STOVE7_XY[1] + 0.080       # contact lever 0.009-0.029 m  ->  knob angle 0.75-1.25 rad (needs >= 0.5)
    return [
        MV((x, y0, z + 0.09), pitch=T7_PITCH, g=1.0, v=1.0, tol=0.006),
        MV((x, y0, z), pitch=T7_PITCH, g=1.0, v=0.5, tol=0.004, settle=True),
        MV((x, y1, z), pitch=T7_PITCH, g=1.0, v=1.0, tol=0.005, nmin=150),
        MV((x, y1, z + 0.12), pitch=T7_PITCH, g=1.0, v=0.8, tol=0.012),
    ]


# ---- task 8: both moka pots on the stove ---------------------------------------------------------------
STOVE8_XY = (-0.20, -0.20)
BURNER8 = (STOVE8_XY[0] + 0.15, STOVE8_XY[1])
BURNER_TOP = 0.905 + 0.0255
POT_BOTTOM = 0.0661             # pot origin above its bottom
POT_HANDLE_Y = 0.068            # handle top-bar grasp point: 0.068 toward -y of the pot origin (yaw 180 deg)
POT_HANDLE_Z = 0.0544           # grip-site height above the pot origin (pads on the top bar, z 0.053..0.067)
POT1_XY = (-0.05, 0.25)         # moka_pot_1 ("right" region)
POT2_XY = (0.05, 0.05)          # moka_pot_2 ("left" region)


def _pot_pick_place(pot_xy, slot_xy):
    yaw = 0.5 * math.pi         # fingers along x
    hx, hy = pot_xy[0], pot_xy[1] - POT_HANDLE_Y
    gz = Z_TABLE + POT_BOTTOM + POT_HANDLE_Z
    sx, sy = slot_xy[0], slot_xy[1] - POT_HANDLE_Y
    rel = BURNER_TOP + POT_BOTTOM + POT_HANDLE_Z + 0.004
    hi = 1.13
    return [
        MV((hx, hy, hi), yaw=yaw, g=-1.0, v=1.0, tol=0.006),
        MV((hx, hy, gz), yaw=yaw, g=-1.0, v=0.6, tol=0.004, settle=True),
        HOLD(1.0, 14),
        MV((hx, hy, hi), yaw=yaw, g=1.0, v=0.7, tol=0.012),
        MV((sx, hy, hi), yaw=yaw, g=1.0, v=1.0, tol=0.008),          # x first: never sweep across the other pot
        MV((sx, sy, hi), yaw=yaw, g=1.0, v=1.0, tol=0.006, settle=True),
        MV((sx, sy, rel + 0.03), yaw=yaw, g=1.0, v=0.6, tol=0.006),
        MV((sx, sy, rel), yaw=yaw, g=1.0, v=0.3, tol=0.004, settle=True),
        HOLD(-1.0, 12),
        MV((sx, sy, hi + 0.02), yaw=yaw, g=-1.0, v=0.7, tol=0.012),
    ]


def plan_task8(pot1_xy, pot2_xy):
    steps = _pot_pick_place(pot2_xy, (BURNER8[0] + 0.047, BURNER8[1]))
    steps += _pot_pick_place(pot1_xy, (BURNER8[0] - 0.047, BURNER8[1]))
    return steps


# ---- task 9: alphabet soup and tomato sauce into the basket (living room) ------------------------------
SOUP9_XY = (-0.10, -0.15)
TOMATO9_XY = (-0.10, 0.05)


def plan_task9(soup_xy, tomato_xy):
    d1 = (BASKET_XY[0], BASKET_XY[1] + 0.028)
    d2 = (BASKET_XY[0], BASKET_XY[1] - 0.028)
    steps = _pick_to_basket((tomato_xy[0], tomato_xy[1] + CAN_Y_OFF), Z_LIVING, CAN_GRIP_H, 0.0762, d1, CAN_RELEASE_H)
    steps += _pick_to_basket((soup_xy[0], soup_xy[1] + CAN_Y_OFF), Z_LIVING, CAN_GRIP_H, 0.0762, d2, CAN_RELEASE_H)
    return steps


# --------------------------------------------------------------------------------------------------------
# instruction -> task
# --------------------------------------------------------------------------------------------------------
INSTRUCTIONS = [
    "pick up the black bowl in the top drawer of the wooden cabinet and place it on the plate",
    "pick up the black bowl next to the plate and place it on the plate",
    "pick up the black bowl on the ramekin and place it on the plate",
    "pick up the alphabet soup and place it in the basket",
    "pick up the milk and place it in the basket",
    "pick up the tomato sauce and place it in the basket",
    "put the wine bottle on the rack",
    "turn on the stove",
    "put both moka pots on the stove",
    "put both the alphabet soup and the tomato sauce in the basket",
]


def task_index(instruction):
    s = " ".join(str(instruction).lower().split())
    if s in INSTRUCTIONS:
        return INSTRUCTIONS.index(s)
    if "drawer" in s:
        return 0
    if "next to the plate" in s:
        return 1
    if "ramekin" in s:
        return 2
    if "moka" in s:
        return 8
    if "both" in s:
        return 9
    if "milk" in s:
        return 4
    if "alphabet" in s:
        return 3
    if "tomato" in s:
        return 5
    if "wine" in s or "rack" in s:
        return 6
    if "stove" in s:
        return 7
    return 1


_SHAPES = None


def _shapes():
    global _SHAPES
    if _SHAPES is None:
        try:
            z = np.load(os.path.join(HERE, "shapes.npz"))
            _SHAPES = {k: np.array(z[k], dtype=np.float64) for k in z.files}
        except Exception:
            _SHAPES = {}
    return _SHAPES


R_Y_UP = _mat_from_wxyz((0.0, 0.0, math.sqrt(0.5), math.sqrt(0.5)))    # soup / tomato sauce: mesh y -> world z
R_YAW180 = _rz(math.pi)                                                  # turbosquid objects

STEP_M = 0.0129        # eef travel per policy step at action 1.0 (closed form: OSC kp=150, critically damped, 20 Hz)
STEP_RAD = 0.064       # hand rotation per policy step at the capped rotation action (0.25 rad goal offset)


def _level(R):
    """Pointing-down frame that keeps the measured heading of the hand x-axis (robust to a 180 deg flip)."""
    x = np.array([R[0, 0], R[1, 0], 0.0])
    n = float(np.linalg.norm(x))
    if not np.isfinite(n) or n < 0.3:
        return R_DOWN.copy()
    x = x / n
    z = np.array([0.0, 0.0, -1.0])
    y = np.cross(z, x)
    return np.stack([x, y, z], axis=1)


# --------------------------------------------------------------------------------------------------------
# the policy
# --------------------------------------------------------------------------------------------------------
class Policy:
    def __init__(self, instruction):
        self.task = task_index(instruction)
        self.t = 0
        self.plan = None
        self.i = 0
        self.k = 0
        self.n_lim = 0
        self.c_in = 0
        self.R_base = R_DOWN.copy()
        self.last_p = None
        self.last_yaw = 0.0
        self.last_pitch = 0.0
        self.last_g = -1.0
        self.vision = {}
        _shapes()

    def _R(self, yaw, pitch):
        return _ry(pitch) @ _rz(yaw) @ self.R_base

    # ---- perception (first frame only) ------------------------------------------------------------------
    def _refine(self, image, cam, name, shape, R_obj, p_nom, search_m, z_support, min_score=0.45, min_inside=0.50):
        pts = _shapes().get(shape)
        if pts is None:
            self.vision[name] = None
            return np.array(p_nom[:2], dtype=np.float64)
        P = pts @ R_obj.T + np.array(p_nom, dtype=np.float64)
        P = P[P[:, 2] >= z_support]                       # parts below the support surface are not visible
        ref = P.mean(axis=0)
        res = locate(image, cam, P, ref, search_m, min_score, min_inside)
        self.vision[name] = res
        if res is None:
            return np.array(p_nom[:2], dtype=np.float64)
        return np.array([p_nom[0] + res[0], p_nom[1] + res[1]])

    def _build_plan(self, image):
        t = self.task
        if t == 0:
            return plan_task0()
        if t == 1:
            return plan_task1()
        if t == 2:
            return plan_task2()
        if t == 3:
            return plan_task3()
        if t == 4:
            return plan_task4()
        if t == 5:
            return plan_task5()
        if t == 6:
            cam = _Cam(*CAM_TABLE)
            b = self._refine(image, cam, "bottle", "bottle", R_YAW180,
                             (BOTTLE_XY[0], BOTTLE_XY[1], BOTTLE_Z0), 0.015, Z_TABLE)
            return plan_task6(b)
        if t == 7:
            return plan_task7()
        if t == 8:
            cam = _Cam(*CAM_TABLE)
            z = Z_TABLE + POT_BOTTOM
            p1 = self._refine(image, cam, "pot1", "moka", R_YAW180, (POT1_XY[0], POT1_XY[1], z), 0.025, Z_TABLE)
            p2 = self._refine(image, cam, "pot2", "moka", R_YAW180, (POT2_XY[0], POT2_XY[1], z), 0.025, Z_TABLE)
            return plan_task8(p1, p2)
        cam = _Cam(*CAM_LIVING)
        z = Z_LIVING + 0.0386
        # nominal placement (+-2.5 cm) is outside the +-0.65 cm finger clearance most of the time, so even a
        # weaker match is worth using here: looser gates than for tasks 6 and 8
        sp = self._refine(image, cam, "soup", "soup", R_Y_UP, (SOUP9_XY[0], SOUP9_XY[1], z), 0.025, Z_LIVING, 0.30, 0.40)
        tm = self._refine(image, cam, "tomato", "tomato", R_Y_UP, (TOMATO9_XY[0], TOMATO9_XY[1], z), 0.025, Z_LIVING, 0.30, 0.40)
        return plan_task9(sp, tm)

    # ---- control ----------------------------------------------------------------------------------------
    def act(self, image, proprio, prev_proprio):
        pr = np.asarray(proprio, dtype=np.float64).reshape(-1)
        pp = np.asarray(prev_proprio, dtype=np.float64).reshape(-1)
        if pr.shape[0] < 7 or not np.all(np.isfinite(pr[:7])):
            return [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, float(self.last_g)]
        pos = pr[0:3]
        R_cur = _mat_from_xyzw(pr[3:7])
        if pp.shape[0] >= 3 and np.all(np.isfinite(pp[:3])):
            speed = float(np.linalg.norm(pos - pp[0:3]))
        else:
            speed = 0.0

        if self.plan is None:
            self.R_base = _level(R_cur)
            self.plan = self._build_plan(image)
            self.i = 0
            self.k = 0
            self.last_p = pos.copy()
        self.t += 1

        # advance over finished steps
        for _ in range(4):
            if self.i >= len(self.plan):
                break
            st = self.plan[self.i]
            if st["kind"] == "hold":
                if self.k >= st["n"]:
                    self.i += 1
                    self.k = 0
                    continue
                break
            err = st["p"] - pos
            rv = _rotvec(self._R(st["yaw"], st["pitch"]) @ R_cur.T)
            if self.k == 0:
                # time-out from the distance to go (1.6x the closed-form travel time + slack); never blocks forever
                dist = float(np.max(np.abs(err)))
                self.n_lim = (int(math.ceil(1.6 * dist / (STEP_M * max(st["v"], 0.1))))
                              + int(math.ceil(1.6 * float(np.linalg.norm(rv)) / STEP_RAD)) + 15)
                self.n_lim = max(self.n_lim, st["nmin"])
                self.c_in = 0
            in_tol = float(np.linalg.norm(err)) < st["tol"] and float(np.linalg.norm(rv)) < st["rtol"]
            self.c_in = self.c_in + 1 if in_tol else 0
            reached = in_tol and self.k >= 1 and ((not st["settle"]) or speed < 0.0025 or self.c_in >= 6)
            if reached or self.k >= self.n_lim:
                self.last_p = st["p"].copy()
                self.last_yaw = st["yaw"]
                self.last_pitch = st["pitch"]
                self.last_g = st["g"]
                self.i += 1
                self.k = 0
                continue
            break

        if self.i >= len(self.plan):
            tgt, yaw, pitch, g, vmax = self.last_p, self.last_yaw, self.last_pitch, self.last_g, 0.5
        else:
            st = self.plan[self.i]
            if st["kind"] == "hold":
                tgt, yaw, pitch, g, vmax = self.last_p, self.last_yaw, self.last_pitch, st["g"], 0.5
                self.last_g = st["g"]
            else:
                tgt, yaw, pitch, g, vmax = st["p"], st["yaw"], st["pitch"], st["g"], st["v"]
            self.k += 1

        a = np.zeros(7)
        # per-axis clip (not norm clip): axes with a small error keep their full position gain, so the
        # hand holds its lateral position while it pushes or descends along another axis
        a[0:3] = np.clip((tgt - pos) / 0.05, -vmax, vmax)
        rv = _rotvec(self._R(yaw, pitch) @ R_cur.T)
        nr = float(np.linalg.norm(rv))
        if nr > 0.004:
            w = rv / 0.5 * 0.9
            nw = float(np.linalg.norm(w))
            if nw > 0.5:
                w = w * (0.5 / nw)
            a[3:6] = w
        a[6] = g
        a = np.clip(np.nan_to_num(a, nan=0.0, posinf=0.0, neginf=0.0), -1.0, 1.0)
        return [float(x) for x in a]
