diff --git a/docs/craic.md b/docs/craic.md index b9febc1..c0aef70 100644 --- a/docs/craic.md +++ b/docs/craic.md @@ -2,6 +2,8 @@ 为了让数学表达更清晰，我们先统一一下坐标系和变量的定义。 +除角度外，所有线性长度和坐标统一使用 **mm**。 + ### 变量与坐标系定义 我们将基坐标系原点设在红色滑轨的零点位置。 @@ -110,4 +112,4 @@ $$\theta_4 = \Phi - \theta_2 - \theta_3$$ 1. **使用 `atan2` 替代 `asin/acos**`：上面逆解公式中我全部使用了 $\text{atan2}(y, x)$。在标准库中，`atan2` 能够自动处理四个象限的符号问题，且能避免 $x=0$ 时的除零错误，这在底层驱动中至关重要。 2. **死区与奇异点保护**：当 $X_4^2 + Y_4^2 \approx 0$ 时（关节 4 缩回到了原点正上方），此时 $\theta_2$ 会失去意义（奇异点）。在代码中应当加入对 $X_4^2 + Y_4^2 < \epsilon$（一个极小值）的判断，防止产生 NaN。 -3. **角度范围归一化**：计算出的角度可能会超出电机支持的物理限位范围（例如超出了 $[-180^\circ, +180^\circ]$），在下发脉冲或指令前，记得对 $\theta_2, \theta_3, \theta_4$ 进行归一化和软限位拦截。 \ No newline at end of file +3. **角度范围归一化**：计算出的角度可能会超出电机支持的物理限位范围（例如超出了 $[-180^\circ, +180^\circ]$），在下发脉冲或指令前，记得对 $\theta_2, \theta_3, \theta_4$ 进行归一化和软限位拦截。 diff --git a/tools/.udp_control_state.json b/tools/.udp_control_state.json index e163950..c18083c 100644 --- a/tools/.udp_control_state.json +++ b/tools/.udp_control_state.json @@ -1,8 +1,8 @@ { - "height": -252, - "j2": 40, - "j3": 64, - "j4": -69, - "j5": 81, + "height": -290, + "j2": -66, + "j3": 145, + "j4": -44, + "j5": -100, "j6": 0 } \ No newline at end of file diff --git a/tools/camera_capture.py b/tools/camera_capture.py new file mode 100644 index 0000000..d67354f --- /dev/null +++ b/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\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()) diff --git a/tools/udp_control.py b/tools/udp_control.py index bb1a937..6665c09 100644 --- a/tools/udp_control.py +++ b/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_MAX = -10 +DEFAULT_HEIGHT_MIN = 0 # 用户坐标系：顶部，单位 mm +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( - math_state.d1, + height=-clamp_int( + 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("--l2", type=float, help="用于 --show-fk 的 L2") - joints.add_argument("--x4", type=float, help="J4 坐标系到 TCP 的 X 偏移，用于 --show-fk") - joints.add_argument("--z4", type=float, help="J4 坐标系到 TCP 的 Z 偏移，用于 --show-fk") + joints.add_argument("--l1", type=float, default=DEFAULT_L1, help=f"J2 到 J3 的连杆长度 mm (默认 {DEFAULT_L1})") + joints.add_argument("--l2", type=float, default=DEFAULT_L2, help=f"J3 到 J4 的连杆长度 mm (默认 {DEFAULT_L2})") + joints.add_argument("--x4", type=float, default=DEFAULT_X4, help=f"J4 到 TCP 的 X 偏移 mm (默认 {DEFAULT_X4})") + 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("--y", type=float, required=True, help="TCP Y 坐标") - pose.add_argument("--z", type=float, required=True, help="TCP Z 坐标") + pose.add_argument("--x", type=float, required=True, help="TCP X 坐标 mm") + pose.add_argument("--y", type=float, required=True, help="TCP Y 坐标 mm") + 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("--l2", type=float, required=True, help="J3 到 J4 的连杆长度 L2") - pose.add_argument("--x4", type=float, default=0.0, help="J4 坐标系到 TCP 的固定 X 偏移 x4") - pose.add_argument("--z4", type=float, default=0.0, help="J4 坐标系到 TCP 的固定 Z 偏移 z4") + pose.add_argument("--l1", type=float, default=DEFAULT_L1, help=f"J2 到 J3 的连杆长度 mm (默认 {DEFAULT_L1})") + pose.add_argument("--l2", type=float, default=DEFAULT_L2, help=f"J3 到 J4 的连杆长度 mm (默认 {DEFAULT_L2})") + pose.add_argument("--x4", type=float, default=DEFAULT_X4, help=f"J4 到 TCP 的 X 偏移 mm (默认 {DEFAULT_X4})") + 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"y={pose.y:.3f}", - f"z={pose.z:.3f}", + f"x={pose.x:.3f}mm", + f"y={pose.y:.3f}mm", + 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"y={center.y:.3f}", - f"z={center.z:.3f}", + f"x={center.x:.3f}mm", + f"y={center.y:.3f}mm", + 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 = [ - name for name in ("l1", "l2", "x4", "z4") - if getattr(args, name) is None - ] - if missing: - raise ArmControlError( - "--show-fk 需要同时提供 " - + ", ".join(f"--{name}" for name in missing) - ) - start_pose = forward_kinematics(geometry_from_args(args), start_math_state) + z4 = resolve_z4(args.up) + start_pose = forward_kinematics( + geometry_from_args(args, z4=z4), 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")