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

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