insert.py

#!/usr/bin/env python3
"""Insert Script — Pick up rod, transit to insertion pose via waypoints, drop, return home.

Full pipeline: detect rod → approach → open gripper → descend → close gripper
→ lift → transit to insertion target via 6 slow waypoints (Enter between each)
→ open gripper (drop) → return to home pose.

Usage:
    python insert.py [--base-url http://localhost:8000]
                     [--calibration calibration.json]
                     [--camera 0]
                     [--offsets calibrate.yaml]
                     [--pick-height 0.13]
                     [--waypoints 6]
"""

import argparse
import json
import math
import os
import sys
import time

import cv2
import numpy as np
import yaml
from ultralytics import YOLO

# ── Constants ────────────────────────────────────────────────────────────────

APPROACH_HEIGHT = 0.23    # m above pallet — approach height for the TCP
PICK_HEIGHT = 0.13        # m — gripper descends to this Z to grab rod
LIFT_HEIGHT = 0.23        # m — lift back up after grip (same as APPROACH_HEIGHT)
GRIPPER_SETTLE_TIME = 0.5 # seconds to wait after gripper command

# Insertion target pose — defaults in mm / degrees (copy-paste from robot UI)
# Override via CLI: --insert-x 661.2 --insert-y 14.9 --insert-z 651.9 ...
INSERT_X_MM = 661.2
INSERT_Y_MM = 14.9
INSERT_Z_MM = 651.9
INSERT_RX_DEG = 175.2
INSERT_RY_DEG = 0.4
INSERT_RZ_DEG = 109.9

NUM_WAYPOINTS = 6         # number of transit waypoints (to pre-insert point)
PRE_INSERT_X_OFFSET = 0.05  # m — penultimate waypoint is this far back in X

FLOW_ID = "homing_move"
POLL_INTERVAL = 0.3       # seconds between status polls
FLOW_TIMEOUT = 30         # seconds max wait per move
MOVE_VELOCITY = 0.1       # m/s — used for pick moves
MOVE_ACCELERATION = 0.1   # m/s²
TRANSIT_VELOCITY = 0.05   # m/s — slower for waypoint transit
TRANSIT_ACCELERATION = 0.05  # m/s²

# Gripper flow JSONs — used as fallback if not already on backend
OPEN_GRIPPER_FLOW = {
    "id": "open_gripper",
    "name": "Open Gripper",
    "initial_state": "open",
    "loop": False,
    "variables": {},
    "states": [
        {
            "name": "open",
            "steps": [
                {"id": "d1_off", "skill": "set_tool_output", "executor": "robot",
                 "params": {"index": 1, "status": 0}, "timeout_ms": 3000},
                {"id": "d2_on", "skill": "set_tool_output", "executor": "robot",
                 "params": {"index": 2, "status": 1}, "timeout_ms": 3000},
            ],
        }
    ],
    "transitions": [],
}

CLOSE_GRIPPER_FLOW = {
    "id": "close_gripper",
    "name": "Close Gripper",
    "initial_state": "close",
    "loop": False,
    "variables": {},
    "states": [
        {
            "name": "close",
            "steps": [
                {"id": "d2_off", "skill": "set_tool_output", "executor": "robot",
                 "params": {"index": 2, "status": 0}, "timeout_ms": 3000},
                {"id": "d1_on", "skill": "set_tool_output", "executor": "robot",
                 "params": {"index": 1, "status": 1}, "timeout_ms": 3000},
            ],
        }
    ],
    "transitions": [],
}


# ── Extended Kalman Filter for static target tracking ────────────────────────

class PipeEKF:
    """2D EKF for a static target observed via noisy measurements."""

    def __init__(self, R: np.ndarray):
        self.x: np.ndarray | None = None   # state [rod_x, rod_y]
        self.P: np.ndarray | None = None   # covariance 2x2
        self.R = R                         # measurement noise 2x2
        self.Q = np.diag([1e-8, 1e-8])    # process noise (static target)
        self.n_updates = 0

    @property
    def initialized(self) -> bool:
        return self.x is not None

    def initialize(self, z: np.ndarray) -> None:
        """Set state from first measurement; covariance starts at R."""
        self.x = z.copy()
        self.P = self.R.copy()

    def predict(self) -> None:
        """Static process model: x_{k+1} = x_k, P_{k+1} = P_k + Q."""
        self.P = self.P + self.Q

    def mahalanobis(self, z: np.ndarray) -> float:
        """Mahalanobis distance of measurement z from current estimate."""
        S = self.P + self.R  # innovation covariance
        y = z - self.x       # innovation
        return float(np.sqrt(y @ np.linalg.inv(S) @ y))

    def update(self, z: np.ndarray) -> None:
        """Standard Kalman update with measurement z."""
        S = self.P + self.R
        K = self.P @ np.linalg.inv(S)  # Kalman gain
        y = z - self.x
        self.x = self.x + K @ y
        self.P = (np.eye(2) - K) @ self.P
        self.n_updates += 1

    def std(self) -> tuple[float, float]:
        """Return (std_x, std_y) in the same units as the state."""
        return float(np.sqrt(self.P[0, 0])), float(np.sqrt(self.P[1, 1]))


# ── Flow builder ─────────────────────────────────────────────────────────────

def build_move_flow(x: float, y: float, z: float,
                    rx: float, ry: float, rz: float,
                    flow_id: str = FLOW_ID,
                    velocity: float = MOVE_VELOCITY,
                    acceleration: float = MOVE_ACCELERATION) -> dict:
    """Return a flow JSON dict with a single move_linear step."""
    return {
        "id": flow_id,
        "name": "Homing Move",
        "initial_state": "m",
        "loop": False,
        "variables": {},
        "states": [
            {
                "name": "m",
                "steps": [
                    {
                        "id": "go",
                        "skill": "move_linear",
                        "executor": "robot",
                        "params": {
                            "target_pose": [x, y, z, rx, ry, rz],
                            "velocity": velocity,
                            "acceleration": acceleration,
                        },
                        "store_result": None,
                        "error_handling": {
                            "strategy": "stop",
                            "max_retries": 3,
                            "retry_delay_ms": 1000,
                            "fallback_skill": None,
                        },
                        "timeout_ms": FLOW_TIMEOUT * 1000,
                    }
                ],
            }
        ],
        "transitions": [],
    }


# ── API helpers ──────────────────────────────────────────────────────────────

def send_and_run_flow(session, base_url: str, flow: dict) -> None:
    """Save the flow, start it, and poll until done or timeout."""
    flow_id = flow["id"]
    resp = session.post(f"{base_url}/api/flows", json=flow)
    resp.raise_for_status()

    resp = session.post(f"{base_url}/api/flows/{flow_id}/start")
    resp.raise_for_status()

    deadline = time.monotonic() + FLOW_TIMEOUT
    while time.monotonic() < deadline:
        time.sleep(POLL_INTERVAL)
        resp = session.get(f"{base_url}/api/flows/status")
        resp.raise_for_status()
        status = resp.json().get("status", "")
        if status in ("completed", "idle"):
            return
        if status in ("error", "aborted"):
            raise RuntimeError(
                f"Flow ended with status '{status}': "
                f"{resp.json().get('error_message', 'unknown')}"
            )
    raise TimeoutError(f"Flow did not complete within {FLOW_TIMEOUT}s")


def start_and_wait_flow(session, base_url: str, flow_id: str,
                        fallback_flow: dict | None = None) -> None:
    """Start an existing flow by ID and poll until done.

    If the flow doesn't exist yet (start returns 404), POST the fallback_flow
    JSON first, then retry.
    """
    resp = session.post(f"{base_url}/api/flows/{flow_id}/start")
    if resp.status_code == 404 and fallback_flow is not None:
        print(f"  Flow '{flow_id}' not found, registering...")
        reg = session.post(f"{base_url}/api/flows", json=fallback_flow)
        reg.raise_for_status()
        resp = session.post(f"{base_url}/api/flows/{flow_id}/start")
    resp.raise_for_status()

    deadline = time.monotonic() + FLOW_TIMEOUT
    while time.monotonic() < deadline:
        time.sleep(POLL_INTERVAL)
        resp = session.get(f"{base_url}/api/flows/status")
        resp.raise_for_status()
        status = resp.json().get("status", "")
        if status in ("completed", "idle"):
            return
        if status in ("error", "aborted"):
            raise RuntimeError(
                f"Flow '{flow_id}' ended with status '{status}': "
                f"{resp.json().get('error_message', 'unknown')}"
            )
    raise TimeoutError(f"Flow '{flow_id}' did not complete within {FLOW_TIMEOUT}s")


def get_current_tcp_pose(session, base_url: str) -> list[float]:
    """Read the robot's current TCP pose from /api/robot/state."""
    resp = session.get(f"{base_url}/api/robot/state")
    resp.raise_for_status()
    state = resp.json()
    tcp = state["tcp_pose"]
    print(f"  Current TCP pose: [{', '.join(f'{v:.4f}' for v in tcp)}]")
    return tcp


# ── Camera helpers ───────────────────────────────────────────────────────────

def open_camera(device: int = 0) -> cv2.VideoCapture:
    """Open an OpenCV camera and configure it at 1280x720."""
    cap = cv2.VideoCapture(device)
    if not cap.isOpened():
        raise RuntimeError(f"Cannot open camera device {device}")
    cap.set(cv2.CAP_PROP_FPS, 15)
    cap.set(cv2.CAP_PROP_FRAME_WIDTH, 1280)
    cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 720)
    for _ in range(5):
        cap.read()
    return cap


def capture_snapshot(cap: cv2.VideoCapture) -> np.ndarray:
    """Grab a fresh frame from the OpenCV camera."""
    for _ in range(3):
        cap.grab()
    ret, frame = cap.read()
    if not ret or frame is None:
        raise RuntimeError("Failed to capture frame from camera")
    return frame


# ── Calibration loader + pixel→robot conversion ─────────────────────────────

def load_calibration(path: str) -> dict:
    """Load calibration.json and return the interpolation dict."""
    with open(path) as f:
        data = json.load(f)
    interp = data["interpolation"]
    method = interp.get("method")
    print(f"Loaded calibration from {path}")
    print(f"  Method:  {method}")
    if method == "homography_floor_corrected":
        print(f"  Z pass 1:  {interp['z_h1']*1000:.0f}mm  ({interp.get('n_inliers_h1', '?')} inliers)")
        print(f"  Z pass 2:  {interp['z_h2']*1000:.0f}mm  ({interp.get('n_inliers_h2', '?')} inliers)")
        print(f"  Z target:  {interp['z_target']*1000:.0f}mm (floor)")
    elif method == "homography":
        print(f"  Points:  {interp['n_points']} ({interp.get('n_inliers', '?')} inliers)")
    else:
        print(f"  Polynomial terms: {interp['n_points']}, max degree: {interp['max_degree']}")
        print(f"  Pixel U range:    [{interp['pixel_u_range'][0]:.1f}, {interp['pixel_u_range'][1]:.1f}]")
        print(f"  Pixel V range:    [{interp['pixel_v_range'][0]:.1f}, {interp['pixel_v_range'][1]:.1f}]")
    return interp


def _eval_homography(interp: dict, cx: float, cy: float) -> tuple[float, float]:
    H = np.array(interp["H"])
    p = H @ np.array([cx, cy, 1.0])
    return float(p[0] / p[2]), float(p[1] / p[2])


def _eval_lagrangian(interp: dict, cx: float, cy: float) -> tuple[float, float]:
    u_min, u_max = interp["pixel_u_range"]
    v_min, v_max = interp["pixel_v_range"]
    if not (u_min <= cx <= u_max) or not (v_min <= cy <= v_max):
        print(f"  WARNING: pixel ({cx:.1f}, {cy:.1f}) is outside calibrated range — "
              f"extrapolating (may be inaccurate)")
    u_n = 2.0 * (cx - u_min) / (u_max - u_min) - 1.0
    v_n = 2.0 * (cy - v_min) / (v_max - v_min) - 1.0
    robot_x = sum(c * (u_n ** a) * (v_n ** b)
                  for c, (a, b) in zip(interp["coeff_x"], interp["exponents"]))
    robot_y = sum(c * (u_n ** a) * (v_n ** b)
                  for c, (a, b) in zip(interp["coeff_y"], interp["exponents"]))
    return float(robot_x), float(robot_y)


def pixel_to_robot(interp: dict, cx: float, cy: float) -> tuple[float, float]:
    """Convert pixel (cx, cy) → (robot_x, robot_y) using whichever method was calibrated."""
    method = interp.get("method")
    if method in ("homography", "homography_floor_corrected"):
        return _eval_homography(interp, cx, cy)
    return _eval_lagrangian(interp, cx, cy)


# ── Offset loader ────────────────────────────────────────────────────────────

def load_offsets(path: str) -> dict:
    """Load XY offsets from calibrate.yaml.  Returns zeros if file missing."""
    defaults = {"x_offset_mm": 0.0, "y_offset_mm": 0.0, "wrist_angle_offset_deg": 0.0}
    if not os.path.exists(path):
        print(f"  Offsets file '{path}' not found — using zeros")
        return defaults
    with open(path) as f:
        data = yaml.safe_load(f) or {}
    for k in defaults:
        if k not in data:
            data[k] = defaults[k]
    return data


# ── Pipe detection with angle ────────────────────────────────────────────────

def detect_pipe_with_angle(model: YOLO, frame: np.ndarray) -> tuple[float, float, float] | None:
    """Run YOLO segmentation, return (cx, cy, angle_deg) or None."""
    results = model(frame, verbose=False)[0]

    if results.masks is None or len(results.masks) == 0:
        return None

    best_idx = int(results.boxes.conf.argmax())
    best_conf = float(results.boxes.conf[best_idx])
    best_cls = int(results.boxes.cls[best_idx])
    best_name = model.names[best_cls]

    print(f"  Detection: {best_name} (conf={best_conf:.3f})")

    mask = results.masks.data[best_idx].cpu().numpy().astype(np.uint8)
    mask = cv2.resize(mask, (frame.shape[1], frame.shape[0]))

    contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
    if not contours:
        return None

    largest = max(contours, key=cv2.contourArea)
    moments = cv2.moments(largest)
    if moments["m00"] == 0:
        return None

    cx = moments["m10"] / moments["m00"]
    cy = moments["m01"] / moments["m00"]

    rect = cv2.minAreaRect(largest)
    _, (w, h), angle = rect
    if w < h:
        angle += 90.0

    return cx, cy, angle


def circular_mean_angle(angles: list[float]) -> float:
    """Compute circular mean handling 180° ambiguity (rod is symmetric)."""
    doubled = [2.0 * math.radians(a) for a in angles]
    mean_2a = math.atan2(
        sum(math.sin(d) for d in doubled),
        sum(math.cos(d) for d in doubled),
    )
    return math.degrees(mean_2a / 2.0)


# ── Waypoint interpolation ──────────────────────────────────────────────────

def interpolate_poses(start: list[float], end: list[float],
                      n: int) -> list[list[float]]:
    """Generate n evenly-spaced waypoints between start and end (exclusive of
    start, inclusive of end).

    Each pose is [x, y, z, rx, ry, rz].  Linear interpolation on all 6 DOF.
    """
    waypoints = []
    for i in range(1, n + 1):
        t = i / n
        wp = [s + t * (e - s) for s, e in zip(start, end)]
        waypoints.append(wp)
    return waypoints


# ── Main ─────────────────────────────────────────────────────────────────────

def main() -> None:
    parser = argparse.ArgumentParser(
        description="Full pipeline: pick up rod, transit to insertion pose, drop, return home."
    )
    parser.add_argument("--base-url", default="http://localhost:8000",
                        help="Backend base URL (default: http://localhost:8000)")
    parser.add_argument("--calibration", default="calibration.json",
                        help="Calibration JSON file (default: calibration.json)")
    parser.add_argument("--camera", type=int, default=0,
                        help="OpenCV camera device index (default: 0)")
    parser.add_argument("--model", default="best.pt",
                        help="YOLO model path (default: best.pt)")
    parser.add_argument("--meas-noise-mm", type=float, default=2.0,
                        help="Measurement noise std dev in mm (default: 2.0)")
    parser.add_argument("--min-frames", type=int, default=10,
                        help="Min accepted measurements before convergence (default: 10)")
    parser.add_argument("--max-frames", type=int, default=100,
                        help="Max frames to process before timeout (default: 100)")
    parser.add_argument("--offsets", default="calibrate.yaml",
                        help="YAML file with XY/angle offsets (default: calibrate.yaml)")
    parser.add_argument("--pick-height", type=float, default=PICK_HEIGHT,
                        help=f"Z height for gripping (default: {PICK_HEIGHT})")
    parser.add_argument("--waypoints", type=int, default=NUM_WAYPOINTS,
                        help=f"Number of transit waypoints (default: {NUM_WAYPOINTS})")
    # Insertion target — in mm and degrees so you can copy-paste from robot UI
    parser.add_argument("--insert-x", type=float, default=INSERT_X_MM,
                        help=f"Insertion X in mm (default: {INSERT_X_MM})")
    parser.add_argument("--insert-y", type=float, default=INSERT_Y_MM,
                        help=f"Insertion Y in mm (default: {INSERT_Y_MM})")
    parser.add_argument("--insert-z", type=float, default=INSERT_Z_MM,
                        help=f"Insertion Z in mm (default: {INSERT_Z_MM})")
    parser.add_argument("--insert-rx", type=float, default=INSERT_RX_DEG,
                        help=f"Insertion RX in degrees (default: {INSERT_RX_DEG})")
    parser.add_argument("--insert-ry", type=float, default=INSERT_RY_DEG,
                        help=f"Insertion RY in degrees (default: {INSERT_RY_DEG})")
    parser.add_argument("--insert-rz", type=float, default=INSERT_RZ_DEG,
                        help=f"Insertion RZ in degrees (default: {INSERT_RZ_DEG})")
    args = parser.parse_args()

    base_url = args.base_url.rstrip("/")
    pick_height = args.pick_height
    n_waypoints = args.waypoints

    # Convert insertion target from mm/degrees to m/radians
    ins_x = args.insert_x / 1000.0
    ins_y = args.insert_y / 1000.0
    ins_z = args.insert_z / 1000.0
    ins_rx = math.radians(args.insert_rx)
    ins_ry = math.radians(args.insert_ry)
    ins_rz = math.radians(args.insert_rz)

    print(f"Insertion target: X={args.insert_x:.1f}mm Y={args.insert_y:.1f}mm "
          f"Z={args.insert_z:.1f}mm RX={args.insert_rx:.1f}° "
          f"RY={args.insert_ry:.1f}° RZ={args.insert_rz:.1f}°")

    import requests
    session = requests.Session()

    # 1. Load calibration
    interp = load_calibration(args.calibration)

    # 2. Read current robot TCP pose — this is also used as home pose at the end
    print("Reading current robot state (this will be the HOME pose)...")
    home_pose = get_current_tcp_pose(session, base_url)
    tcp_pose = home_pose

    # 3. Load YOLO model
    print(f"Loading YOLO model: {args.model}")
    model = YOLO(args.model)
    print("  Model loaded.")

    # 4. Open camera
    print(f"Opening camera (device {args.camera})...")
    cap = open_camera(args.camera)
    actual_w = int(cap.get(cv2.CAP_PROP_FRAME_WIDTH))
    actual_h = int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT))
    print(f"  Camera ready — {actual_w}x{actual_h}")

    # 5. Set up EKF
    sigma = args.meas_noise_mm / 1000.0
    R = np.diag([sigma ** 2, sigma ** 2])
    ekf = PipeEKF(R)

    convergence_mm = 0.5
    max_consecutive_misses = 5
    min_accepted_for_occlusion = 5
    angle_samples: list[float] = []

    print(f"\n{'='*60}")
    print(f"EKF TRACKING PHASE (with angle collection)")
    print(f"  Measurement noise σ = {args.meas_noise_mm:.1f} mm")
    print(f"  Convergence threshold = {convergence_mm:.1f} mm")
    print(f"  Min frames = {args.min_frames}, Max frames = {args.max_frames}")
    print(f"{'='*60}\n")

    try:
        consecutive_misses = 0
        stop_reason = "timeout"

        for frame_idx in range(args.max_frames):
            frame = capture_snapshot(cap)
            detection = detect_pipe_with_angle(model, frame)

            if detection is None:
                consecutive_misses += 1
                status = f"  Frame {frame_idx:3d}: NO DETECTION (misses={consecutive_misses})"
                if ekf.initialized:
                    sx, sy = ekf.std()
                    status += f"  |  est=({ekf.x[0]:.4f}, {ekf.x[1]:.4f}) ±({sx*1000:.2f}, {sy*1000:.2f})mm"
                print(status)
                if (consecutive_misses > max_consecutive_misses
                        and ekf.initialized
                        and ekf.n_updates >= min_accepted_for_occlusion):
                    stop_reason = "occlusion"
                    break
                continue

            cx, cy, angle_deg = detection
            robot_x, robot_y = pixel_to_robot(interp, cx, cy)
            z = np.array([robot_x, robot_y])

            if not ekf.initialized:
                ekf.initialize(z)
                consecutive_misses = 0
                angle_samples.append(angle_deg)
                sx, sy = ekf.std()
                print(f"  Frame {frame_idx:3d}: INIT  pixel=({cx:.1f},{cy:.1f})  "
                      f"robot=({robot_x:.4f},{robot_y:.4f})  "
                      f"±({sx*1000:.2f},{sy*1000:.2f})mm  "
                      f"angle={angle_deg:.1f}°")
                continue

            d = ekf.mahalanobis(z)
            if d > 3.0:
                consecutive_misses += 1
                sx, sy = ekf.std()
                print(f"  Frame {frame_idx:3d}: REJECT d={d:.2f}  "
                      f"pixel=({cx:.1f},{cy:.1f})  "
                      f"est=({ekf.x[0]:.4f},{ekf.x[1]:.4f})  "
                      f"±({sx*1000:.2f},{sy*1000:.2f})mm")
                if (consecutive_misses > max_consecutive_misses
                        and ekf.n_updates >= min_accepted_for_occlusion):
                    stop_reason = "occlusion"
                    break
                continue

            consecutive_misses = 0
            ekf.predict()
            ekf.update(z)
            angle_samples.append(angle_deg)

            sx, sy = ekf.std()
            print(f"  Frame {frame_idx:3d}: OK    pixel=({cx:.1f},{cy:.1f})  "
                  f"robot=({ekf.x[0]:.4f},{ekf.x[1]:.4f})  "
                  f"±({sx*1000:.2f},{sy*1000:.2f})mm  "
                  f"n={ekf.n_updates}  angle={angle_deg:.1f}°")

            if (sx * 1000 < convergence_mm
                    and sy * 1000 < convergence_mm
                    and ekf.n_updates >= args.min_frames):
                stop_reason = "converged"
                break

        # ── Results ──────────────────────────────────────────────────────
        if not ekf.initialized:
            print("\nERROR: No pipe detected in any frame.")
            sys.exit(1)

        sx, sy = ekf.std()
        print(f"\n{'='*60}")
        print(f"TRACKING COMPLETE — {stop_reason}")
        print(f"  Accepted measurements: {ekf.n_updates}")
        print(f"  Final estimate: ({ekf.x[0]:.4f}, {ekf.x[1]:.4f}) m")
        print(f"  Uncertainty:    ±({sx*1000:.2f}, {sy*1000:.2f}) mm")
        print(f"  Angle samples:  {len(angle_samples)}")

        if angle_samples:
            mean_angle = circular_mean_angle(angle_samples)
            print(f"  Mean rod angle: {mean_angle:.1f}°")
        else:
            mean_angle = 0.0
            print(f"  No angle samples — using 0°")
        print(f"{'='*60}\n")

        # 7. Apply offsets
        target_x, target_y = float(ekf.x[0]), float(ekf.x[1])
        offsets = load_offsets(args.offsets)
        target_x += offsets["x_offset_mm"] / 1000.0
        target_y += offsets["y_offset_mm"] / 1000.0
        print(f"Applying offsets: x={offsets['x_offset_mm']:.1f}mm, "
              f"y={offsets['y_offset_mm']:.1f}mm, "
              f"wrist_angle={offsets['wrist_angle_offset_deg']:.1f}°")

        rx, ry, rz = tcp_pose[3], tcp_pose[4], tcp_pose[5]
        adjusted_rz = rz + math.radians(mean_angle + offsets["wrist_angle_offset_deg"])
        print(f"Wrist rotation: base_rz={math.degrees(rz):.1f}° + "
              f"rod={mean_angle:.1f}° + offset={offsets['wrist_angle_offset_deg']:.1f}° "
              f"→ rz={math.degrees(adjusted_rz):.1f}°")

        # ══════════════════════════════════════════════════════════════════
        # PHASE 1: PICK UP
        # ══════════════════════════════════════════════════════════════════

        print(f"\n{'='*60}")
        print("PHASE 1: PICK UP")
        print(f"{'='*60}")

        # ── Step 1: Move to approach position ────────────────────────────
        print(f"\n--- Step 1: Move to approach position ---")
        approach_flow = build_move_flow(
            target_x, target_y, APPROACH_HEIGHT, rx, ry, adjusted_rz,
            flow_id="homing_move",
        )
        print(f"Target: ({target_x:.4f}, {target_y:.4f}, z={APPROACH_HEIGHT}) "
              f"with rz={math.degrees(adjusted_rz):.1f}°")
        input(">>> Press Enter to MOVE to approach position...")
        send_and_run_flow(session, base_url, approach_flow)
        print("  Approach position reached.")

        # ── Step 2: Open gripper ─────────────────────────────────────────
        print(f"\n--- Step 2: Open gripper ---")
        input(">>> Press Enter to OPEN gripper...")
        start_and_wait_flow(session, base_url, "open_gripper",
                            fallback_flow=OPEN_GRIPPER_FLOW)
        time.sleep(GRIPPER_SETTLE_TIME)
        print("  Gripper open.")

        # ── Step 3: Descend to pick height ───────────────────────────────
        print(f"\n--- Step 3: Descend to pick height ({pick_height}m) ---")
        input(">>> Press Enter to DESCEND...")
        descend_flow = build_move_flow(
            target_x, target_y, pick_height, rx, ry, adjusted_rz,
            flow_id="pick_descend",
        )
        send_and_run_flow(session, base_url, descend_flow)
        print("  At pick height.")

        # ── Step 4: Close gripper ────────────────────────────────────────
        print(f"\n--- Step 4: Close gripper ---")
        input(">>> Press Enter to CLOSE gripper...")
        start_and_wait_flow(session, base_url, "close_gripper",
                            fallback_flow=CLOSE_GRIPPER_FLOW)
        time.sleep(GRIPPER_SETTLE_TIME)
        print("  Gripper closed.")

        # ── Step 5: Lift ─────────────────────────────────────────────────
        print(f"\n--- Step 5: Lift to {LIFT_HEIGHT}m ---")
        input(">>> Press Enter to LIFT...")
        lift_flow = build_move_flow(
            target_x, target_y, LIFT_HEIGHT, rx, ry, adjusted_rz,
            flow_id="pick_lift",
        )
        send_and_run_flow(session, base_url, lift_flow)
        print("  Lift complete.")

        print(f"\n{'='*60}")
        print("Pick complete — rod in gripper.")
        print(f"{'='*60}")

        # ══════════════════════════════════════════════════════════════════
        # PHASE 2: TRANSIT TO INSERTION POSE VIA WAYPOINTS
        # ══════════════════════════════════════════════════════════════════

        # Pre-insert point: same as final but X pulled back by 5cm
        pre_ins_x = ins_x - PRE_INSERT_X_OFFSET
        final_pose = [ins_x, ins_y, ins_z, ins_rx, ins_ry, ins_rz]
        pre_insert_pose = [pre_ins_x, ins_y, ins_z, ins_rx, ins_ry, ins_rz]
        total_steps = n_waypoints + 1  # waypoints to pre-insert + final insertion

        print(f"\n{'='*60}")
        print("PHASE 2: TRANSIT TO INSERTION POSE")
        print(f"  From: ({target_x:.4f}, {target_y:.4f}, {LIFT_HEIGHT}) "
              f"rz={math.degrees(adjusted_rz):.1f}°")
        print(f"  Pre-insert (wp {n_waypoints}): "
              f"({pre_ins_x:.4f}, {ins_y:.4f}, {ins_z:.4f}) "
              f"rz={math.degrees(ins_rz):.1f}°  [X - 50mm]")
        print(f"  Final (wp {total_steps}):      "
              f"({ins_x:.4f}, {ins_y:.4f}, {ins_z:.4f}) "
              f"rz={math.degrees(ins_rz):.1f}°")
        print(f"  Transit velocity: {TRANSIT_VELOCITY} m/s")
        print(f"{'='*60}")

        # Waypoints 1..N: interpolate from lift pose to pre-insert pose
        start_pose = [target_x, target_y, LIFT_HEIGHT, rx, ry, adjusted_rz]
        waypoints = interpolate_poses(start_pose, pre_insert_pose, n_waypoints)

        for i, wp in enumerate(waypoints, 1):
            wx, wy, wz, wrx, wry, wrz = wp
            print(f"\n--- Waypoint {i}/{total_steps} ---")
            print(f"  Position: ({wx:.4f}, {wy:.4f}, {wz:.4f})")
            print(f"  Rotation: rx={math.degrees(wrx):.1f}° ry={math.degrees(wry):.1f}° "
                  f"rz={math.degrees(wrz):.1f}°")
            input(f">>> Press Enter to move to waypoint {i}/{total_steps}...")

            wp_flow = build_move_flow(
                wx, wy, wz, wrx, wry, wrz,
                flow_id=f"transit_wp{i}",
                velocity=TRANSIT_VELOCITY,
                acceleration=TRANSIT_ACCELERATION,
            )
            send_and_run_flow(session, base_url, wp_flow)
            print(f"  Waypoint {i} reached.")

        # Final waypoint: the actual insertion point (X + 5cm forward)
        print(f"\n--- Waypoint {total_steps}/{total_steps} (FINAL INSERT) ---")
        print(f"  Position: ({ins_x:.4f}, {ins_y:.4f}, {ins_z:.4f})")
        print(f"  Rotation: rx={math.degrees(ins_rx):.1f}° ry={math.degrees(ins_ry):.1f}° "
              f"rz={math.degrees(ins_rz):.1f}°")
        input(f">>> Press Enter to move to FINAL insertion point...")

        final_flow = build_move_flow(
            ins_x, ins_y, ins_z, ins_rx, ins_ry, ins_rz,
            flow_id="transit_final",
            velocity=TRANSIT_VELOCITY,
            acceleration=TRANSIT_ACCELERATION,
        )
        send_and_run_flow(session, base_url, final_flow)
        print(f"  Insertion point reached.")

        print(f"\n{'='*60}")
        print("At insertion pose.")
        print(f"{'='*60}")

        # ══════════════════════════════════════════════════════════════════
        # PHASE 3: DROP OFF
        # ══════════════════════════════════════════════════════════════════

        print(f"\n{'='*60}")
        print("PHASE 3: DROP OFF")
        print(f"{'='*60}")

        print(f"\n--- Open gripper to release rod ---")
        input(">>> Press Enter to OPEN gripper (drop rod)...")
        start_and_wait_flow(session, base_url, "open_gripper",
                            fallback_flow=OPEN_GRIPPER_FLOW)
        time.sleep(GRIPPER_SETTLE_TIME)
        print("  Rod released.")

        # ══════════════════════════════════════════════════════════════════
        # PHASE 4: RETURN HOME
        # ══════════════════════════════════════════════════════════════════

        print(f"\n{'='*60}")
        print("PHASE 4: RETURN HOME")
        print(f"  Home pose: [{', '.join(f'{v:.4f}' for v in home_pose)}]")
        print(f"{'='*60}")

        input(">>> Press Enter to RETURN to home pose...")
        home_flow = build_move_flow(
            home_pose[0], home_pose[1], home_pose[2],
            home_pose[3], home_pose[4], home_pose[5],
            flow_id="return_home",
        )
        send_and_run_flow(session, base_url, home_flow)
        print("  Home pose reached.")

        print(f"\n{'='*60}")
        print("Insert complete!")
        print(f"{'='*60}")

    finally:
        cap.release()


if __name__ == "__main__":
    main()