fallensigh/craic
2
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

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

Browse Source 
- 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
This commit is contained in:
FallenSigh
2026-06-15 21:33:47 +08:00
parent f343a01372
commit 7accfc617e
4 changed files with 316 additions and 60 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
tools/.udp_control_state.json
View File

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

239
tools/camera_capture.py Normal file
View File

@@ -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())

123
tools/udp_control.py
View File

@@ -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")