Refactor arm UDP controller with user-space height coordinates and gripper toggle

- Switch height to user coordinates (0=top, 290=bottom mm), invert sign
  internally for UDP protocol
- Replace raw --j5 arg with --up flag (gripper up/down toggle)
- Add default geometry constants (L1, L2, X4, Z4) and gripper
  open/closed angles
- Update --show-fk and pose mode to derive Z4 from gripper state
- Add mm unit annotations to all CLI help text and output
- Update cached joint state to match new coordinate convention
- Add camera_capture.py: single-shot JPEG grabber from ESP32 MJPEG stream
- Fix missing newline at EOF in craic.md

docs/craic.md

@@ -2,6 +2,8 @@
 
 为了让数学表达更清晰，我们先统一一下坐标系和变量的定义。
 
+除角度外，所有线性长度和坐标统一使用 **mm**。
+
 ### 变量与坐标系定义
 
 我们将基坐标系原点设在红色滑轨的零点位置。

tools/.udp_control_state.json

@@ -1,8 +1,8 @@
 {
-  "height": -252,
+  "height": -290,
-  "j2": 40,
+  "j2": -66,
-  "j3": 64,
+  "j3": 145,
-  "j4": -69,
+  "j4": -44,
-  "j5": 81,
+  "j5": -100,
   "j6": 0
 }

tools/camera_capture.py

@@ -0,0 +1,239 @@
+#!/usr/bin/env python3
+"""Capture a JPEG frame from the CRAIC ESP32-S3 camera via HTTP MJPEG stream.
+
+Usage:
+    python tools/camera_capture.py                        # auto-detect or default IP
+    python tools/camera_capture.py --ip 192.168.x.x       # specify IP
+    python tools/camera_capture.py -o frame.jpg           # custom output
+    python tools/camera_capture.py --scan                 # scan local subnet for ESP32
+"""
+
+from __future__ import annotations
+
+import argparse
+import os
+import socket
+import sys
+import time
+from datetime import datetime
+
+
+def find_esp32_on_subnet(subnet_prefix: str) -> list[str]:
+    """Ping sweep a /24 subnet and return IPs with port 80 open."""
+    candidates: list[str] = []
+    print(f"Scanning {subnet_prefix}.0/24 ...")
+    for i in range(1, 255):
+        ip = f"{subnet_prefix}.{i}"
+        r = os.system(f"ping -c 1 -W 1 {ip} >/dev/null 2>&1")
+        if r == 0:
+            try:
+                sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+                sock.settimeout(0.5)
+                result = sock.connect_ex((ip, 80))
+                sock.close()
+                if result == 0:
+                    candidates.append(ip)
+            except:
+                pass
+        if i % 50 == 0:
+            print(f"  ... scanned {i}/254")
+    return candidates
+
+
+def probe_esp32_status(ip: str, timeout: float = 3) -> bool:
+    """Check if an IP serves the ESP32 camera JSON status endpoint."""
+    try:
+        sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+        sock.settimeout(timeout)
+        sock.connect((ip, 80))
+        sock.sendall(
+            b"GET /status HTTP/1.1\r\n"
+            b"Host: " + ip.encode() + b"\r\n"
+            b"User-Agent: Mozilla/5.0\r\n"
+            b"Accept: */*\r\n"
+            b"Connection: close\r\n\r\n"
+        )
+        data = b""
+        while True:
+            try:
+                chunk = sock.recv(4096)
+                if not chunk:
+                    break
+                data += chunk
+            except socket.timeout:
+                break
+            except:
+                break
+        sock.close()
+        # ESP32 /status returns JSON with these keys
+        return b"capture_fps" in data or b"has_client" in data
+    except:
+        return False
+
+
+def grab_jpeg_frame(ip: str, port: int = 80, timeout: float = 10) -> bytes | None:
+    """Connect to MJPEG stream, read raw bytes, extract first valid JPEG frame."""
+    sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+    sock.settimeout(timeout)
+    try:
+        sock.connect((ip, port))
+        sock.sendall(
+            f"GET /stream HTTP/1.1\r\n"
+            f"Host: {ip}\r\n"
+            f"User-Agent: Mozilla/5.0\r\n"
+            f"Accept: */*\r\n"
+            f"Connection: close\r\n\r\n".encode()
+        )
+
+        buf = b""
+        start = time.time()
+        boundary = b"--frame"
+
+        while time.time() - start < timeout:
+            try:
+                chunk = sock.recv(4096)
+                if not chunk:
+                    break
+                buf += chunk
+            except socket.timeout:
+                continue
+            except:
+                break
+
+            # Scan buffer for a complete JPEG frame
+            # Format: --frame\r\n...headers...\r\n\r\n<JPEG>\r\n--frame
+            while True:
+                # Find JPEG start-of-image marker
+                soi = buf.find(b"\xff\xd8")
+                if soi == -1:
+                    break
+                # Find JPEG end-of-image marker
+                eoi = buf.find(b"\xff\xd9", soi + 2)
+                if eoi == -1:
+                    break
+
+                jpeg = buf[soi : eoi + 2]
+
+                # Verify it's realistically sized (>= 1KB) and preceded by boundary
+                if len(jpeg) >= 1024:
+                    return jpeg
+
+                # False positive — keep scanning past this SOI
+                buf = buf[soi + 2:]
+
+        return None
+
+    except socket.timeout:
+        print(f"  Timeout connecting to {ip}:{port}", file=sys.stderr)
+        return None
+    except ConnectionRefusedError:
+        return None
+    except Exception as e:
+        print(f"  Socket error: {e}", file=sys.stderr)
+        return None
+    finally:
+        try:
+            sock.close()
+        except:
+            pass
+
+
+def resolve_camera() -> str | None:
+    """Try to find the ESP32 camera on local networks."""
+    # Get local IP
+    s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+    try:
+        s.connect(("8.8.8.8", 80))
+        local_ip = s.getsockname()[0]
+        s.close()
+    except:
+        local_ip = "127.0.0.1"
+
+    prefix = ".".join(local_ip.split(".")[:3])
+
+    # Collect candidate IPs to try
+    candidates: list[str] = []
+
+    # 1. Gateway (common for ESP32 AP mode)
+    candidates.append(f"{prefix}.1")
+    # 2. Common DHCP range
+    for i in range(100, 111):
+        candidates.append(f"{prefix}.{i}")
+    # 3. Some other common patterns
+    for i in [50, 51, 20, 30, 200, 201]:
+        candidates.append(f"{prefix}.{i}")
+
+    # Deduplicate
+    seen: set[str] = set()
+    unique: list[str] = []
+    for ip in candidates:
+        if ip not in seen:
+            seen.add(ip)
+            unique.append(ip)
+
+    print(f"Probing {len(unique)} likely IPs on {prefix}.0/24 ...")
+    for ip in unique:
+        print(f"  {ip} ... ", end="", flush=True)
+        if probe_esp32_status(ip):
+            print("ESP32 camera found!")
+            return ip
+        print("no")
+
+    # Broader subnet scan
+    print(f"\nNo camera at common IPs. Scanning full subnet {prefix}.0/24 ...")
+    port80_hosts = find_esp32_on_subnet(prefix)
+    for ip in port80_hosts:
+        if probe_esp32_status(ip):
+            print(f"ESP32 camera found at {ip}")
+            return ip
+
+    return None
+
+
+def main() -> int:
+    parser = argparse.ArgumentParser(
+        description="从 CRAIC ESP32-S3 摄像头抓取 JPEG 帧"
+    )
+    parser.add_argument("--ip", help="ESP32 IP 地址 (默认自动检测)")
+    parser.add_argument("--port", type=int, default=80)
+    parser.add_argument("-o", "--output", help="输出文件路径")
+    parser.add_argument("--scan", action="store_true", help="扫描子网查找 ESP32")
+    args = parser.parse_args()
+
+    # ---- resolve IP ----
+    ip = args.ip
+    if args.scan:
+        ip = resolve_camera()
+    elif ip is None:
+        ip = resolve_camera()
+
+    if ip is None:
+        print(
+            "ERROR: 找不到 ESP32 摄像头，请确保:\n"
+            "  1. ESP32 已上电\n"
+            "  2. ESP32 已连接 WiFi (STA 模式, SSID: FS)\n"
+            "  3. 本机与 ESP32 在同一子网\n"
+            "  或使用 --ip 直接指定地址",
+            file=sys.stderr,
+        )
+        return 1
+
+    # ---- capture ----
+    ts = datetime.now().strftime("%Y%m%d_%H%M%S")
+    out = args.output or f"capture_{ts}.jpg"
+
+    print(f"Connecting to http://{ip}:{args.port}/stream ...")
+    jpeg = grab_jpeg_frame(ip, args.port)
+
+    if jpeg is None:
+        print("ERROR: 无法获取 JPEG 帧。", file=sys.stderr)
+        return 1
+
+    with open(out, "wb") as f:
+        f.write(jpeg)
+    print(f"Saved {out}  ({len(jpeg)} bytes)")
+    return 0
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())

tools/udp_control.py

@@ -5,6 +5,7 @@ Mechanical structure used here:
 1. J2/J3/J4 rotate around the Z axis in the XY plane.
 2. The gripper TCP has a fixed offset relative to the J4 frame: (x4, 0, z4).
 3. ``phi`` is the TCP yaw in the XY plane and is always ``J2 + J3 + J4``.
+4. All linear pose and geometry values are millimeters.
 
 Supports two control modes:
 1. Direct joint command mode.
@@ -26,15 +27,23 @@ from pathlib import Path
 DEFAULT_UDP_IP = "192.168.4.1"
 DEFAULT_UDP_PORT = 8888
 
-DEFAULT_HEIGHT_MIN = -285
+DEFAULT_HEIGHT_MIN = 0      # 用户坐标系：顶部，单位 mm
-DEFAULT_HEIGHT_MAX = -10
+DEFAULT_HEIGHT_MAX = 290    # 用户坐标系：底部，单位 mm
 DEFAULT_JOINT_MIN = -180
 DEFAULT_JOINT_MAX = 180
-DEFAULT_FIXED_J5 = 81
+J5_OPEN = 81
+J5_CLOSED = -100
+Z4_OPEN = -55
+Z4_CLOSED = 100
+DEFAULT_FIXED_J5 = J5_OPEN
 DEFAULT_FIXED_J6 = 0
 DEFAULT_ZERO_J2 = 3
 DEFAULT_ZERO_J3 = 7
 DEFAULT_ZERO_J4 = 25
+DEFAULT_L1 = 125.0
+DEFAULT_L2 = 125.0
+DEFAULT_X4 = 110.0
+DEFAULT_Z4 = -80.0
 DEFAULT_INTERP_DURATION = 1.0
 DEFAULT_INTERP_RATE = 20.0
 STATE_FILE = Path(__file__).with_name(".udp_control_state.json")
@@ -108,7 +117,7 @@ class Joint4Center:
 
 def default_command_state() -> ArmJointState:
     return ArmJointState(
-        height=DEFAULT_HEIGHT_MAX,
+        height=-DEFAULT_HEIGHT_MIN,
         j2=DEFAULT_ZERO_J2,
         j3=DEFAULT_ZERO_J3,
         j4=DEFAULT_ZERO_J4,
@@ -156,7 +165,7 @@ def tcp_to_joint4_center(geometry: ArmGeometry, pose: ArmPose) -> Joint4Center:
     )
 
 
-def forward_kinematics(geometry: ArmGeometry, state: ArmJointState) -> ArmPose:
+def forward_kinematics(geometry: ArmGeometry, state: ArmMathState) -> ArmPose:
     theta2 = math.radians(state.theta2_deg)
     theta3 = math.radians(state.theta3_deg)
     theta4 = math.radians(state.theta4_deg)
@@ -182,7 +191,7 @@ def inverse_kinematics(
     elbow_up: bool,
     j5: int,
     j6: int,
-) -> ArmJointState:
+) -> ArmMathState:
     joint4_center = tcp_to_joint4_center(geometry, pose)
     d1 = joint4_center.z
 
@@ -198,7 +207,7 @@ def inverse_kinematics(
     if c3 < -1.0 - 1e-9 or c3 > 1.0 + 1e-9:
         reach = math.sqrt(r2)
         raise ArmControlError(
-            f"目标超出工作空间，关节 4 投影距离为 {reach:.3f}。"
+            f"目标超出工作空间，关节 4 投影距离为 {reach:.3f}mm。"
         )
     c3 = max(-1.0, min(1.0, c3))
 
@@ -241,8 +250,8 @@ def math_to_command_state(
     j6: int,
 ) -> ArmJointState:
     return ArmJointState(
-        height=clamp_int(
+        height=-clamp_int(
-            math_state.d1,
+            round(-math_state.d1),
             limits.height_min,
             limits.height_max,
             "height(cmd)",
@@ -277,7 +286,7 @@ def load_cached_command_state(limits: ArmLimits) -> ArmJointState | None:
     try:
         payload = json.loads(STATE_FILE.read_text(encoding="utf-8"))
         return ArmJointState(
-            height=clamp_int(payload["height"], limits.height_min, limits.height_max, "height"),
+            height=-clamp_int(-payload["height"], limits.height_min, limits.height_max, "height(cmd)"),
             j2=clamp_int(payload["j2"], limits.joint_min, limits.joint_max, "J2"),
             j3=clamp_int(payload["j3"], limits.joint_min, limits.joint_max, "J3"),
             j4=clamp_int(payload["j4"], limits.joint_min, limits.joint_max, "J4"),
@@ -459,13 +468,13 @@ def build_parser() -> argparse.ArgumentParser:
         "--height-min",
         type=int,
         default=DEFAULT_HEIGHT_MIN,
-        help="高度下限，默认 -285",
+        help=f"高度下限 (用户坐标, mm)，默认 {DEFAULT_HEIGHT_MIN}",
     )
     parser.add_argument(
         "--height-max",
         type=int,
         default=DEFAULT_HEIGHT_MAX,
-        help="UDP 指令高度上限，默认 -10",
+        help=f"高度上限 (用户坐标, mm)，默认 {DEFAULT_HEIGHT_MAX}",
     )
     parser.add_argument(
         "--joint-min",
@@ -510,16 +519,16 @@ def build_parser() -> argparse.ArgumentParser:
         action="store_true",
         help="不读取或更新上次发送的关节命令缓存",
     )
-    joints.add_argument("--height", type=int, required=True, help="UDP 指令里的升降高度命令值")
+    joints.add_argument("--height", type=int, required=True, help="升降高度 mm (0=顶部, 290=底部)")
     joints.add_argument("--j2", type=int, required=True, help="UDP 指令里的 J2 命令值")
     joints.add_argument("--j3", type=int, required=True, help="UDP 指令里的 J3 命令值")
     joints.add_argument("--j4", type=int, required=True, help="UDP 指令里的 J4 命令值")
-    joints.add_argument("--j5", type=int, default=DEFAULT_FIXED_J5, help="UDP 指令里的 J5 命令值，默认固定 81")
+    joints.add_argument("--up", action="store_true", help="夹爪抬起 (J5=-100°)，默认放下 (J5=81°)")
     joints.add_argument("--j6", type=int, default=DEFAULT_FIXED_J6, help="UDP 指令里的 J6 命令值，默认固定 0")
-    joints.add_argument("--l1", type=float, help="用于 --show-fk 的 L1")
+    joints.add_argument("--l1", type=float, default=DEFAULT_L1, help=f"J2 到 J3 的连杆长度 mm (默认 {DEFAULT_L1})")
-    joints.add_argument("--l2", type=float, help="用于 --show-fk 的 L2")
+    joints.add_argument("--l2", type=float, default=DEFAULT_L2, help=f"J3 到 J4 的连杆长度 mm (默认 {DEFAULT_L2})")
-    joints.add_argument("--x4", type=float, help="J4 坐标系到 TCP 的 X 偏移，用于 --show-fk")
+    joints.add_argument("--x4", type=float, default=DEFAULT_X4, help=f"J4 到 TCP 的 X 偏移 mm (默认 {DEFAULT_X4})")
-    joints.add_argument("--z4", type=float, help="J4 坐标系到 TCP 的 Z 偏移，用于 --show-fk")
+    joints.add_argument("--z4", type=float, default=DEFAULT_Z4, help=f"J4 到 TCP 的 Z 偏移 mm (默认 {DEFAULT_Z4})")
 
     pose = subparsers.add_parser("pose", help="根据末端位姿逆解后发送")
     pose.add_argument(
@@ -549,31 +558,31 @@ def build_parser() -> argparse.ArgumentParser:
         action="store_true",
         help="不读取或更新上次发送的关节命令缓存",
     )
-    pose.add_argument("--x", type=float, required=True, help="TCP X 坐标")
+    pose.add_argument("--x", type=float, required=True, help="TCP X 坐标 mm")
-    pose.add_argument("--y", type=float, required=True, help="TCP Y 坐标")
+    pose.add_argument("--y", type=float, required=True, help="TCP Y 坐标 mm")
-    pose.add_argument("--z", type=float, required=True, help="TCP Z 坐标")
+    pose.add_argument("--z", type=float, required=True, help="TCP Z 坐标 mm (0=顶部, 正值向下)")
     pose.add_argument("--phi", type=float, required=True, help="TCP 偏航角，单位度；等于 J2+J3+J4")
-    pose.add_argument("--l1", type=float, required=True, help="J2 到 J3 的连杆长度 L1")
+    pose.add_argument("--l1", type=float, default=DEFAULT_L1, help=f"J2 到 J3 的连杆长度 mm (默认 {DEFAULT_L1})")
-    pose.add_argument("--l2", type=float, required=True, help="J3 到 J4 的连杆长度 L2")
+    pose.add_argument("--l2", type=float, default=DEFAULT_L2, help=f"J3 到 J4 的连杆长度 mm (默认 {DEFAULT_L2})")
-    pose.add_argument("--x4", type=float, default=0.0, help="J4 坐标系到 TCP 的固定 X 偏移 x4")
+    pose.add_argument("--x4", type=float, default=DEFAULT_X4, help=f"J4 到 TCP 的 X 偏移 mm (默认 {DEFAULT_X4})")
-    pose.add_argument("--z4", type=float, default=0.0, help="J4 坐标系到 TCP 的固定 Z 偏移 z4")
+    pose.add_argument("--z4", type=float, default=DEFAULT_Z4, help=f"J4 到 TCP 的 Z 偏移 mm (默认 {DEFAULT_Z4})")
     pose.add_argument(
         "--elbow-up",
         action="store_true",
         help="使用肘部向上分支，默认使用肘部向下分支",
     )
-    pose.add_argument("--j5", type=int, default=DEFAULT_FIXED_J5, help="附加发送的 J5 命令值，默认固定 81")
+    pose.add_argument("--up", action="store_true", help="夹爪抬起 (J5=-100°)，默认放下 (J5=81°)")
     pose.add_argument("--j6", type=int, default=DEFAULT_FIXED_J6, help="附加发送的 J6 命令值，默认固定 0")
 
     return parser
 
 
-def geometry_from_args(args: argparse.Namespace) -> ArmGeometry:
+def geometry_from_args(args: argparse.Namespace, z4: float | None = None) -> ArmGeometry:
     return ArmGeometry(
         l1=float(args.l1),
         l2=float(args.l2),
         x4=float(args.x4),
-        z4=float(args.z4),
+        z4=float(args.z4) if z4 is None else z4,
     )
 
 
@@ -590,13 +599,21 @@ def limits_from_args(args: argparse.Namespace) -> ArmLimits:
     )
 
 
+def resolve_j5(up: bool) -> int:
+    return J5_CLOSED if up else J5_OPEN
+
+
+def resolve_z4(up: bool) -> float:
+    return Z4_CLOSED if up else Z4_OPEN
+
+
 def state_from_joint_args(args: argparse.Namespace, limits: ArmLimits) -> ArmJointState:
     return ArmJointState(
-        height=clamp_int(args.height, limits.height_min, limits.height_max, "height"),
+        height=-clamp_int(args.height, limits.height_min, limits.height_max, "height(cmd)"),
         j2=clamp_int(args.j2, limits.joint_min, limits.joint_max, "J2"),
         j3=clamp_int(args.j3, limits.joint_min, limits.joint_max, "J3"),
         j4=clamp_int(args.j4, limits.joint_min, limits.joint_max, "J4"),
-        j5=clamp_int(args.j5, limits.joint_min, limits.joint_max, "J5"),
+        j5=clamp_int(resolve_j5(args.up), limits.joint_min, limits.joint_max, "J5"),
         j6=clamp_int(args.j6, limits.joint_min, limits.joint_max, "J6"),
     )
 
@@ -604,7 +621,7 @@ def state_from_joint_args(args: argparse.Namespace, limits: ArmLimits) -> ArmJoi
 def print_joint_summary(state: ArmJointState) -> None:
     print(
         "UDP command joints:",
-        f"height={state.height}",
+        f"height={-state.height}mm",
         f"J2={state.j2}",
         f"J3={state.j3}",
         f"J4={state.j4}",
@@ -616,9 +633,9 @@ def print_joint_summary(state: ArmJointState) -> None:
 def print_pose_summary(pose: ArmPose) -> None:
     print(
         "TCP pose:",
-        f"x={pose.x:.3f}",
+        f"x={pose.x:.3f}mm",
-        f"y={pose.y:.3f}",
+        f"y={pose.y:.3f}mm",
-        f"z={pose.z:.3f}",
+        f"z={-pose.z:.3f}mm",
         f"phi={pose.phi_deg:.3f}deg",
     )
 
@@ -626,16 +643,16 @@ def print_pose_summary(pose: ArmPose) -> None:
 def print_joint4_center_summary(center: Joint4Center) -> None:
     print(
         "J4 center:",
-        f"x={center.x:.3f}",
+        f"x={center.x:.3f}mm",
-        f"y={center.y:.3f}",
+        f"y={center.y:.3f}mm",
-        f"z={center.z:.3f}",
+        f"z={-center.z:.3f}mm",
     )
 
 
 def print_math_summary(state: ArmMathState) -> None:
     print(
         "Math joints:",
-        f"d1={state.d1:.3f}",
+        f"d1={-state.d1:.3f}mm",
         f"J2={state.theta2_deg:.3f}",
         f"J3={state.theta3_deg:.3f}",
         f"J4={state.theta4_deg:.3f}",
@@ -682,30 +699,28 @@ def main() -> int:
             print_math_summary(math_state)
 
             if args.show_fk:
-                missing = [
+                z4 = resolve_z4(args.up)
-                    name for name in ("l1", "l2", "x4", "z4")
+                start_pose = forward_kinematics(
-                    if getattr(args, name) is None
+                    geometry_from_args(args, z4=z4), start_math_state,
-                ]
-                if missing:
-                    raise ArmControlError(
-                        "--show-fk 需要同时提供 "
-                        + ", ".join(f"--{name}" for name in missing)
                 )
-                start_pose = forward_kinematics(geometry_from_args(args), start_math_state)
                 print("Start FK:")
                 print_pose_summary(start_pose)
-                pose = forward_kinematics(geometry_from_args(args), math_state)
+                pose = forward_kinematics(
+                    geometry_from_args(args, z4=z4), math_state,
+                )
                 print_pose_summary(pose)
 
         elif args.mode == "pose":
-            geometry = geometry_from_args(args)
+            z4 = resolve_z4(args.up)
+            geometry = geometry_from_args(args, z4=z4)
+            j5 = resolve_j5(args.up)
             start_command_state = resolve_start_command_state(limits, use_state_cache)
             start_math_state = command_to_math_state(start_command_state, zero_offsets)
             start_pose = forward_kinematics(geometry, start_math_state)
             target_pose = ArmPose(
                 x=args.x,
                 y=args.y,
-                z=args.z,
+                z=-args.z,
                 phi_deg=args.phi,
             )
             joint4_center = tcp_to_joint4_center(geometry, target_pose)
@@ -714,14 +729,14 @@ def main() -> int:
                 pose=target_pose,
                 limits=limits,
                 elbow_up=args.elbow_up,
-                j5=args.j5,
+                j5=j5,
                 j6=args.j6,
             )
             command_state = math_to_command_state(
                 math_state,
                 zero_offsets,
                 limits,
-                j5=args.j5,
+                j5=j5,
                 j6=args.j6,
             )
             command_path = build_pose_command_path(
@@ -732,7 +747,7 @@ def main() -> int:
                 limits=limits,
                 zero_offsets=zero_offsets,
                 elbow_up=args.elbow_up,
-                j5=args.j5,
+                j5=j5,
                 j6=args.j6,
             )
             print("Start state source:", "cache/default")