Add arm UDP controller with IK and interpolation

docs/craic.md

@@ -0,0 +1,113 @@
+这是一个非常漂亮且实用的自定义机械臂！从你的描述和图片来看，这属于一个**类 SCARA（水平多关节）构型**的机械臂，带有一个直线 Z 轴和三个平面的旋转关节。
+
+为了让数学表达更清晰，我们先统一一下坐标系和变量的定义。
+
+### 变量与坐标系定义
+
+我们将基坐标系原点设在红色滑轨的零点位置。
+
+* **$d_1$**: 关节 1（高度滑块）的位置变量。
+* **$\theta_2$**: 关节 2 的旋转角度（相对于基坐标系 X 轴）。
+* **$\theta_3$**: 关节 3 的旋转角度（相对于上一连杆 $L_1$）。
+* **$\theta_4$**: 关节 4 的旋转角度（相对于上一连杆 $L_2$）。
+* **$(X, Y, Z, \Phi)$**: 夹爪末端（TCP - Tool Center Point）在基坐标系下的位姿。其中 $(X, Y, Z)$ 是空间坐标，$\Phi$ 是夹爪在水平面上的总朝向角（偏航角）。
+* **已知常量**: $L_1$（二三关节距离）, $L_2$（三四关节距离）, $x_4$（夹爪相对关节四的 X 偏移）, $z_4$（夹爪相对关节四的高度偏移）。
+
+---
+
+### 1. 运动学正解 (Forward Kinematics)
+
+正解的目的是：**已知各个电机的角度和滑块高度 $(d_1, \theta_2, \theta_3, \theta_4)$，求夹爪末端的位置 $(X, Y, Z, \Phi)$。**
+
+因为 Z 轴的直线运动与 XY 平面的旋转运动是完全解耦的，我们可以分别计算：
+
+**高度 (Z 轴):**
+
+
+$$Z = d_1 + z_4$$
+
+**平面朝向角 (偏航角 $\Phi$):**
+
+
+$$\Phi = \theta_2 + \theta_3 + \theta_4$$
+
+**平面坐标 (X, Y):**
+
+
+$$X = L_1 \cos(\theta_2) + L_2 \cos(\theta_2 + \theta_3) + x_4 \cos(\theta_2 + \theta_3 + \theta_4)$$
+
+$$Y = L_1 \sin(\theta_2) + L_2 \sin(\theta_2 + \theta_3) + x_4 \sin(\theta_2 + \theta_3 + \theta_4)$$
+
+*(注意：在实际编程中，如果你的电机零点不是一条直线，需要在角度上加上相应的初始偏置)*
+
+---
+
+### 2. 运动学逆解 (Inverse Kinematics)
+
+逆解的目的是：**给出夹爪期望到达的目标位置和朝向 $(X, Y, Z, \Phi)$，求出各关节需要运动到的目标值 $(d_1, \theta_2, \theta_3, \theta_4)$。**
+
+> **注意：** 你的机械臂在 XY 平面上有 3 个旋转自由度，但平面位置只需要 2 个自由度 $(X,Y)$。这意味着如果只给定目标坐标，机械臂有无数种姿态可以到达（冗余）。因此，**为了得到唯一解，必须同时指定夹爪的最终期望朝向角 $\Phi$**。
+
+下面是逆解的推导步骤，非常适合直接转化为固件中的控制代码：
+
+#### 第一步：求解滑块高度 $d_1$
+
+高度依然是解耦的，直接通过目标 $Z$ 坐标和常量偏移计算：
+
+
+$$d_1 = Z - z_4$$
+
+#### 第二步：反推关节 4 的坐标 $(X_4, Y_4)$
+
+既然我们知道末端目标的坐标 $(X, Y)$ 和总朝向 $\Phi$，我们可以把夹爪的偏置 $x_4$ “剥离”掉，求出关节 4 中轴线在空间中的位置：
+
+
+$$X_4 = X - x_4 \cos(\Phi)$$
+
+$$Y_4 = Y - x_4 \sin(\Phi)$$
+
+#### 第三步：求解关节 3 的角度 $\theta_3$
+
+现在问题简化为了一个标准的双连杆（两轴）平面机械臂求逆解问题。目标点是 $(X_4, Y_4)$，连杆是 $L_1$ 和 $L_2$。
+根据余弦定理，设目标点到原点的距离平方为 $r^2 = X_4^2 + Y_4^2$，有：
+
+
+$$\cos(\theta_3) = \frac{X_4^2 + Y_4^2 - L_1^2 - L_2^2}{2 L_1 L_2}$$
+
+设 $C_3 = \cos(\theta_3)$。在实际控制代码中，必须在这里做合法性检查：如果 $C_3 > 1$ 或 $C_3 < -1$，说明目标点超出了机械臂的物理工作空间（够不到）。
+如果合法，则 $\theta_3$ 的正弦值为：
+
+
+$$S_3 = \pm \sqrt{1 - C_3^2}$$
+
+
+*(这里的 $\pm$ 代表机械臂的两种姿态：“左手系/右臂”或“右手系/左臂”，也就是俗称的“手肘朝左”还是“手肘朝右”。你可以根据防碰撞需求或当前姿态选择其中一个)*
+
+最终使用反正切函数求解 $\theta_3$：
+
+
+$$\theta_3 = \text{atan2}(S_3, C_3)$$
+
+#### 第四步：求解关节 2 的角度 $\theta_2$
+
+利用几何关系和已经求出的 $\theta_3$，可以通过组合角度直接求出 $\theta_2$：
+
+
+$$\theta_2 = \text{atan2}(Y_4, X_4) - \text{atan2}(L_2 S_3, L_1 + L_2 C_3)$$
+
+#### 第五步：求解关节 4 的角度 $\theta_4$
+
+因为总朝向 $\Phi = \theta_2 + \theta_3 + \theta_4$，所以：
+
+
+$$\theta_4 = \Phi - \theta_2 - \theta_3$$
+
+---
+
+### 💡 嵌入式固件实现建议
+
+由于你很可能需要将这些公式写入 MCU（比如利用 C/C++ 或 Rust 编写固件），这里有几个实践建议：
+
+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$ 进行归一化和软限位拦截。

ros2/src/udp_teleop/config/params.yaml

@@ -1,6 +1,6 @@
 keyboard_udp_control:
   ros__parameters:
-    udp_ip: "127.0.0.1"
+    udp_ip: "192.168.4.1"
     udp_port: 8888
     chassis_linear_speed: 100
     chassis_angular_speed: 45

ros2/src/udp_teleop/udp_teleop/keyboard_control.py

@@ -61,7 +61,7 @@ class KeyboardUdpControlNode(Node):
     def __init__(self):
         super().__init__("keyboard_udp_control")
 
-        self.declare_parameter("udp_ip", "192.168.233.67")
+        self.declare_parameter("udp_ip", "192.168.4.1")
         self.declare_parameter("udp_port", 8888)
         self.declare_parameter("chassis_linear_speed", 100)
         self.declare_parameter("chassis_angular_speed", 45)
@@ -283,7 +283,7 @@ class KeyboardUdpControlNode(Node):
             self.arm_height = min(self.arm_height + self.arm_height_step, -10)
             arm_changed = True
         if KEY_DOWN in keys:
-            self.arm_height = max(self.arm_height - self.arm_height_step, -280)
+            self.arm_height = max(self.arm_height - self.arm_height_step, -285)
             arm_changed = True
 
         joint_index = self.arm_selected_joint

tools/.udp_control_state.json

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

tools/udp_control.py

@@ -0,0 +1,771 @@
+#!/usr/bin/env python3
+"""CRAIC mechanical arm UDP controller.
+
+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``.
+
+Supports two control modes:
+1. Direct joint command mode.
+2. Cartesian TCP pose mode using the inverse kinematics from ``docs/craic.md``.
+"""
+
+from __future__ import annotations
+
+import argparse
+import json
+import math
+import socket
+import sys
+import time
+from dataclasses import dataclass
+from pathlib import Path
+
+
+DEFAULT_UDP_IP = "192.168.4.1"
+DEFAULT_UDP_PORT = 8888
+
+DEFAULT_HEIGHT_MIN = -285
+DEFAULT_HEIGHT_MAX = -10
+DEFAULT_JOINT_MIN = -180
+DEFAULT_JOINT_MAX = 180
+DEFAULT_FIXED_J5 = 81
+DEFAULT_FIXED_J6 = 0
+DEFAULT_ZERO_J2 = 3
+DEFAULT_ZERO_J3 = 7
+DEFAULT_ZERO_J4 = 25
+DEFAULT_INTERP_DURATION = 1.0
+DEFAULT_INTERP_RATE = 20.0
+STATE_FILE = Path(__file__).with_name(".udp_control_state.json")
+
+
+class ArmControlError(ValueError):
+    """Raised when the requested arm pose is invalid."""
+
+
+@dataclass(frozen=True)
+class ArmGeometry:
+    l1: float
+    l2: float
+    x4: float
+    z4: float
+
+
+@dataclass(frozen=True)
+class ArmLimits:
+    height_min: int = DEFAULT_HEIGHT_MIN
+    height_max: int = DEFAULT_HEIGHT_MAX
+    joint_min: int = DEFAULT_JOINT_MIN
+    joint_max: int = DEFAULT_JOINT_MAX
+
+
+@dataclass(frozen=True)
+class ArmZeroOffsets:
+    j2: int = DEFAULT_ZERO_J2
+    j3: int = DEFAULT_ZERO_J3
+    j4: int = DEFAULT_ZERO_J4
+
+
+@dataclass(frozen=True)
+class ArmJointState:
+    height: int
+    j2: int
+    j3: int
+    j4: int
+    j5: int = DEFAULT_FIXED_J5
+    j6: int = DEFAULT_FIXED_J6
+
+    def to_udp_message(self) -> bytes:
+        return (
+            f"JXB:{self.height}:{self.j2}:{self.j3}:{self.j4}:"
+            f"{self.j5}:{self.j6}:0:0:EZHY\n"
+        ).encode("utf-8")
+
+
+@dataclass(frozen=True)
+class ArmPose:
+    x: float
+    y: float
+    z: float
+    phi_deg: float
+
+
+@dataclass(frozen=True)
+class ArmMathState:
+    d1: float
+    theta2_deg: float
+    theta3_deg: float
+    theta4_deg: float
+
+
+@dataclass(frozen=True)
+class Joint4Center:
+    x: float
+    y: float
+    z: float
+
+
+def default_command_state() -> ArmJointState:
+    return ArmJointState(
+        height=DEFAULT_HEIGHT_MAX,
+        j2=DEFAULT_ZERO_J2,
+        j3=DEFAULT_ZERO_J3,
+        j4=DEFAULT_ZERO_J4,
+        j5=DEFAULT_FIXED_J5,
+        j6=DEFAULT_FIXED_J6,
+    )
+
+
+def clamp_int(value: float, lower: int, upper: int, name: str) -> int:
+    rounded = int(round(value))
+    if rounded < lower or rounded > upper:
+        raise ArmControlError(
+            f"{name}={rounded} 超出范围 [{lower}, {upper}]"
+        )
+    return rounded
+
+
+def normalize_angle_deg(angle_deg: float) -> float:
+    normalized = (angle_deg + 180.0) % 360.0 - 180.0
+    if normalized == -180.0 and angle_deg > 0:
+        return 180.0
+    return normalized
+
+
+def lerp(start: float, end: float, t: float) -> float:
+    return start + (end - start) * t
+
+
+def lerp_angle_deg(start_deg: float, end_deg: float, t: float) -> float:
+    delta = normalize_angle_deg(end_deg - start_deg)
+    return normalize_angle_deg(start_deg + delta * t)
+
+
+def tcp_to_joint4_center(geometry: ArmGeometry, pose: ArmPose) -> Joint4Center:
+    """Project the TCP target back to the J4 rotation center.
+
+    ``x4`` and ``z4`` only affect position conversion. They do not affect ``phi``.
+    """
+
+    phi = math.radians(pose.phi_deg)
+    return Joint4Center(
+        x=pose.x - geometry.x4 * math.cos(phi),
+        y=pose.y - geometry.x4 * math.sin(phi),
+        z=pose.z - geometry.z4,
+    )
+
+
+def forward_kinematics(geometry: ArmGeometry, state: ArmJointState) -> ArmPose:
+    theta2 = math.radians(state.theta2_deg)
+    theta3 = math.radians(state.theta3_deg)
+    theta4 = math.radians(state.theta4_deg)
+    phi = theta2 + theta3 + theta4
+    j4_center_x = (
+        geometry.l1 * math.cos(theta2)
+        + geometry.l2 * math.cos(theta2 + theta3)
+    )
+    j4_center_y = (
+        geometry.l1 * math.sin(theta2)
+        + geometry.l2 * math.sin(theta2 + theta3)
+    )
+    x = j4_center_x + geometry.x4 * math.cos(phi)
+    y = j4_center_y + geometry.x4 * math.sin(phi)
+    z = state.d1 + geometry.z4
+    return ArmPose(x=x, y=y, z=z, phi_deg=math.degrees(phi))
+
+
+def inverse_kinematics(
+    geometry: ArmGeometry,
+    pose: ArmPose,
+    limits: ArmLimits,
+    elbow_up: bool,
+    j5: int,
+    j6: int,
+) -> ArmJointState:
+    joint4_center = tcp_to_joint4_center(geometry, pose)
+    d1 = joint4_center.z
+
+    r2 = joint4_center.x * joint4_center.x + joint4_center.y * joint4_center.y
+    if r2 < 1e-9:
+        raise ArmControlError("目标点过于接近奇异点，无法稳定求解 J2。")
+
+    denom = 2.0 * geometry.l1 * geometry.l2
+    if abs(denom) < 1e-9:
+        raise ArmControlError("机械臂几何参数无效：L1 和 L2 不能为 0。")
+
+    c3 = (r2 - geometry.l1 * geometry.l1 - geometry.l2 * geometry.l2) / denom
+    if c3 < -1.0 - 1e-9 or c3 > 1.0 + 1e-9:
+        reach = math.sqrt(r2)
+        raise ArmControlError(
+            f"目标超出工作空间，关节 4 投影距离为 {reach:.3f}。"
+        )
+    c3 = max(-1.0, min(1.0, c3))
+
+    s3_abs = math.sqrt(max(0.0, 1.0 - c3 * c3))
+    s3 = -s3_abs if elbow_up else s3_abs
+
+    theta3 = math.atan2(s3, c3)
+    theta2 = math.atan2(joint4_center.y, joint4_center.x) - math.atan2(
+        geometry.l2 * s3,
+        geometry.l1 + geometry.l2 * c3,
+    )
+    phi = math.radians(pose.phi_deg)
+    theta4 = phi - theta2 - theta3
+
+    return ArmMathState(
+        d1=d1,
+        theta2_deg=normalize_angle_deg(math.degrees(theta2)),
+        theta3_deg=normalize_angle_deg(math.degrees(theta3)),
+        theta4_deg=normalize_angle_deg(math.degrees(theta4)),
+    )
+
+
+def command_to_math_state(
+    command_state: ArmJointState,
+    zero_offsets: ArmZeroOffsets,
+) -> ArmMathState:
+    return ArmMathState(
+        d1=command_state.height,
+        theta2_deg=command_state.j2 - zero_offsets.j2,
+        theta3_deg=command_state.j3 - zero_offsets.j3,
+        theta4_deg=command_state.j4 - zero_offsets.j4,
+    )
+
+
+def math_to_command_state(
+    math_state: ArmMathState,
+    zero_offsets: ArmZeroOffsets,
+    limits: ArmLimits,
+    j5: int,
+    j6: int,
+) -> ArmJointState:
+    return ArmJointState(
+        height=clamp_int(
+            math_state.d1,
+            limits.height_min,
+            limits.height_max,
+            "height(cmd)",
+        ),
+        j2=clamp_int(
+            math_state.theta2_deg + zero_offsets.j2,
+            limits.joint_min,
+            limits.joint_max,
+            "J2(cmd)",
+        ),
+        j3=clamp_int(
+            math_state.theta3_deg + zero_offsets.j3,
+            limits.joint_min,
+            limits.joint_max,
+            "J3(cmd)",
+        ),
+        j4=clamp_int(
+            math_state.theta4_deg + zero_offsets.j4,
+            limits.joint_min,
+            limits.joint_max,
+            "J4(cmd)",
+        ),
+        j5=clamp_int(j5, limits.joint_min, limits.joint_max, "J5"),
+        j6=clamp_int(j6, limits.joint_min, limits.joint_max, "J6"),
+    )
+
+
+def load_cached_command_state(limits: ArmLimits) -> ArmJointState | None:
+    if not STATE_FILE.exists():
+        return 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"),
+            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"),
+            j5=clamp_int(payload.get("j5", DEFAULT_FIXED_J5), limits.joint_min, limits.joint_max, "J5"),
+            j6=clamp_int(payload.get("j6", DEFAULT_FIXED_J6), limits.joint_min, limits.joint_max, "J6"),
+        )
+    except (OSError, json.JSONDecodeError, KeyError, TypeError, ArmControlError):
+        return None
+
+
+def save_cached_command_state(state: ArmJointState) -> None:
+    STATE_FILE.write_text(
+        json.dumps(
+            {
+                "height": state.height,
+                "j2": state.j2,
+                "j3": state.j3,
+                "j4": state.j4,
+                "j5": state.j5,
+                "j6": state.j6,
+            },
+            ensure_ascii=True,
+            indent=2,
+        ),
+        encoding="utf-8",
+    )
+
+
+def resolve_start_command_state(
+    limits: ArmLimits,
+    use_state_cache: bool,
+) -> ArmJointState:
+    if use_state_cache:
+        cached_state = load_cached_command_state(limits)
+        if cached_state is not None:
+            return cached_state
+    return default_command_state()
+
+
+def interpolate_command_states(
+    start: ArmJointState,
+    end: ArmJointState,
+    steps: int,
+) -> list[ArmJointState]:
+    if steps <= 1:
+        return [end]
+
+    states: list[ArmJointState] = []
+    for step_index in range(1, steps + 1):
+        t = step_index / steps
+        states.append(
+            ArmJointState(
+                height=int(round(lerp(start.height, end.height, t))),
+                j2=int(round(lerp(start.j2, end.j2, t))),
+                j3=int(round(lerp(start.j3, end.j3, t))),
+                j4=int(round(lerp(start.j4, end.j4, t))),
+                j5=int(round(lerp(start.j5, end.j5, t))),
+                j6=int(round(lerp(start.j6, end.j6, t))),
+            )
+        )
+    return states
+
+
+def interpolate_pose(
+    start: ArmPose,
+    end: ArmPose,
+    t: float,
+) -> ArmPose:
+    return ArmPose(
+        x=lerp(start.x, end.x, t),
+        y=lerp(start.y, end.y, t),
+        z=lerp(start.z, end.z, t),
+        phi_deg=lerp_angle_deg(start.phi_deg, end.phi_deg, t),
+    )
+
+
+def build_pose_command_path(
+    start_pose: ArmPose,
+    target_pose: ArmPose,
+    steps: int,
+    geometry: ArmGeometry,
+    limits: ArmLimits,
+    zero_offsets: ArmZeroOffsets,
+    elbow_up: bool,
+    j5: int,
+    j6: int,
+) -> list[ArmJointState]:
+    if steps <= 1:
+        math_state = inverse_kinematics(
+            geometry=geometry,
+            pose=target_pose,
+            limits=limits,
+            elbow_up=elbow_up,
+            j5=j5,
+            j6=j6,
+        )
+        return [math_to_command_state(math_state, zero_offsets, limits, j5, j6)]
+
+    path: list[ArmJointState] = []
+    for step_index in range(1, steps + 1):
+        t = step_index / steps
+        pose = interpolate_pose(start_pose, target_pose, t)
+        math_state = inverse_kinematics(
+            geometry=geometry,
+            pose=pose,
+            limits=limits,
+            elbow_up=elbow_up,
+            j5=j5,
+            j6=j6,
+        )
+        path.append(math_to_command_state(math_state, zero_offsets, limits, j5, j6))
+    return path
+
+
+def compute_interpolation_steps(duration: float, rate: float) -> int:
+    if duration <= 0.0 or rate <= 0.0:
+        return 1
+    return max(1, int(math.ceil(duration * rate)))
+
+
+def send_udp_commands(
+    ip: str,
+    port: int,
+    states: list[ArmJointState],
+    dry_run: bool,
+    duration: float,
+) -> None:
+    if not states:
+        return
+
+    delay = duration / len(states) if len(states) > 1 and duration > 0.0 else 0.0
+
+    if dry_run:
+        for state in states:
+            print(state.to_udp_message().decode("utf-8").strip())
+        return
+
+    with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as sock:
+        for index, state in enumerate(states):
+            sock.sendto(state.to_udp_message(), (ip, port))
+            if delay > 0.0 and index < len(states) - 1:
+                time.sleep(delay)
+
+
+def build_parser() -> argparse.ArgumentParser:
+    parser = argparse.ArgumentParser(
+        description="CRAIC 机械臂 UDP 控制程序"
+    )
+    parser.add_argument("--ip", default=DEFAULT_UDP_IP, help="目标 UDP IP")
+    parser.add_argument("--port", type=int, default=DEFAULT_UDP_PORT, help="目标 UDP 端口")
+    parser.add_argument(
+        "--dry-run",
+        action="store_true",
+        help="只打印指令，不实际发送 UDP",
+    )
+    parser.add_argument(
+        "--show-fk",
+        action="store_true",
+        help="输出对应关节角的 TCP 正运动学结果",
+    )
+    parser.add_argument(
+        "--duration",
+        type=float,
+        default=DEFAULT_INTERP_DURATION,
+        help="插值总时长（秒），默认 1.0；设为 0 则直接发送",
+    )
+    parser.add_argument(
+        "--rate",
+        type=float,
+        default=DEFAULT_INTERP_RATE,
+        help="插值发送频率（Hz），默认 20",
+    )
+    parser.add_argument(
+        "--no-state-cache",
+        action="store_true",
+        help="不读取或更新上次发送的关节命令缓存",
+    )
+    parser.add_argument(
+        "--height-min",
+        type=int,
+        default=DEFAULT_HEIGHT_MIN,
+        help="高度下限，默认 -285",
+    )
+    parser.add_argument(
+        "--height-max",
+        type=int,
+        default=DEFAULT_HEIGHT_MAX,
+        help="UDP 指令高度上限，默认 -10",
+    )
+    parser.add_argument(
+        "--joint-min",
+        type=int,
+        default=DEFAULT_JOINT_MIN,
+        help="关节角下限，默认 -180",
+    )
+    parser.add_argument(
+        "--joint-max",
+        type=int,
+        default=DEFAULT_JOINT_MAX,
+        help="关节角上限，默认 180",
+    )
+
+    subparsers = parser.add_subparsers(dest="mode", required=True)
+
+    joints = subparsers.add_parser("joints", help="直接发送关节角")
+    joints.add_argument(
+        "--dry-run",
+        action="store_true",
+        help="只打印指令，不实际发送 UDP",
+    )
+    joints.add_argument(
+        "--show-fk",
+        action="store_true",
+        help="输出对应关节角的 TCP 正运动学结果",
+    )
+    joints.add_argument(
+        "--duration",
+        type=float,
+        default=DEFAULT_INTERP_DURATION,
+        help="插值总时长（秒），默认 1.0；设为 0 则直接发送",
+    )
+    joints.add_argument(
+        "--rate",
+        type=float,
+        default=DEFAULT_INTERP_RATE,
+        help="插值发送频率（Hz），默认 20",
+    )
+    joints.add_argument(
+        "--no-state-cache",
+        action="store_true",
+        help="不读取或更新上次发送的关节命令缓存",
+    )
+    joints.add_argument("--height", type=int, required=True, help="UDP 指令里的升降高度命令值")
+    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("--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")
+
+    pose = subparsers.add_parser("pose", help="根据末端位姿逆解后发送")
+    pose.add_argument(
+        "--dry-run",
+        action="store_true",
+        help="只打印指令，不实际发送 UDP",
+    )
+    pose.add_argument(
+        "--show-fk",
+        action="store_true",
+        help="输出对应关节角的 TCP 正运动学结果",
+    )
+    pose.add_argument(
+        "--duration",
+        type=float,
+        default=DEFAULT_INTERP_DURATION,
+        help="插值总时长（秒），默认 1.0；设为 0 则直接发送",
+    )
+    pose.add_argument(
+        "--rate",
+        type=float,
+        default=DEFAULT_INTERP_RATE,
+        help="插值发送频率（Hz），默认 20",
+    )
+    pose.add_argument(
+        "--no-state-cache",
+        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("--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(
+        "--elbow-up",
+        action="store_true",
+        help="使用肘部向上分支，默认使用肘部向下分支",
+    )
+    pose.add_argument("--j5", type=int, default=DEFAULT_FIXED_J5, help="附加发送的 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:
+    return ArmGeometry(
+        l1=float(args.l1),
+        l2=float(args.l2),
+        x4=float(args.x4),
+        z4=float(args.z4),
+    )
+
+
+def limits_from_args(args: argparse.Namespace) -> ArmLimits:
+    if args.height_min > args.height_max:
+        raise ArmControlError("height-min 不能大于 height-max。")
+    if args.joint_min > args.joint_max:
+        raise ArmControlError("joint-min 不能大于 joint-max。")
+    return ArmLimits(
+        height_min=args.height_min,
+        height_max=args.height_max,
+        joint_min=args.joint_min,
+        joint_max=args.joint_max,
+    )
+
+
+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"),
+        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"),
+        j6=clamp_int(args.j6, limits.joint_min, limits.joint_max, "J6"),
+    )
+
+
+def print_joint_summary(state: ArmJointState) -> None:
+    print(
+        "UDP command joints:",
+        f"height={state.height}",
+        f"J2={state.j2}",
+        f"J3={state.j3}",
+        f"J4={state.j4}",
+        f"J5={state.j5}",
+        f"J6={state.j6}",
+    )
+
+
+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"phi={pose.phi_deg:.3f}deg",
+    )
+
+
+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}",
+    )
+
+
+def print_math_summary(state: ArmMathState) -> None:
+    print(
+        "Math joints:",
+        f"d1={state.d1:.3f}",
+        f"J2={state.theta2_deg:.3f}",
+        f"J3={state.theta3_deg:.3f}",
+        f"J4={state.theta4_deg:.3f}",
+    )
+
+
+def print_interpolation_summary(
+    duration: float,
+    rate: float,
+    steps: int,
+    use_state_cache: bool,
+) -> None:
+    cache_mode = "on" if use_state_cache else "off"
+    print(
+        "Interpolation:",
+        f"duration={duration:.3f}s",
+        f"rate={rate:.3f}Hz",
+        f"steps={steps}",
+        f"state_cache={cache_mode}",
+    )
+
+
+def main() -> int:
+    parser = build_parser()
+    args = parser.parse_args()
+
+    try:
+        limits = limits_from_args(args)
+        zero_offsets = ArmZeroOffsets()
+        use_state_cache = not args.no_state_cache
+        steps = compute_interpolation_steps(args.duration, args.rate)
+        print_interpolation_summary(args.duration, args.rate, steps, use_state_cache)
+
+        if args.mode == "joints":
+            start_command_state = resolve_start_command_state(limits, use_state_cache)
+            command_state = state_from_joint_args(args, limits)
+            math_state = command_to_math_state(command_state, zero_offsets)
+            start_math_state = command_to_math_state(start_command_state, zero_offsets)
+            command_path = interpolate_command_states(start_command_state, command_state, steps)
+            print("Start state source:", "cache/default")
+            print_joint_summary(start_command_state)
+            print_math_summary(start_math_state)
+            print_joint_summary(command_state)
+            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)
+                print("Start FK:")
+                print_pose_summary(start_pose)
+                pose = forward_kinematics(geometry_from_args(args), math_state)
+                print_pose_summary(pose)
+
+        elif args.mode == "pose":
+            geometry = geometry_from_args(args)
+            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,
+                phi_deg=args.phi,
+            )
+            joint4_center = tcp_to_joint4_center(geometry, target_pose)
+            math_state = inverse_kinematics(
+                geometry=geometry,
+                pose=target_pose,
+                limits=limits,
+                elbow_up=args.elbow_up,
+                j5=args.j5,
+                j6=args.j6,
+            )
+            command_state = math_to_command_state(
+                math_state,
+                zero_offsets,
+                limits,
+                j5=args.j5,
+                j6=args.j6,
+            )
+            command_path = build_pose_command_path(
+                start_pose=start_pose,
+                target_pose=target_pose,
+                steps=steps,
+                geometry=geometry,
+                limits=limits,
+                zero_offsets=zero_offsets,
+                elbow_up=args.elbow_up,
+                j5=args.j5,
+                j6=args.j6,
+            )
+            print("Start state source:", "cache/default")
+            print_joint_summary(start_command_state)
+            print_math_summary(start_math_state)
+            print("Start FK:")
+            print_pose_summary(start_pose)
+            print_pose_summary(target_pose)
+            print_joint4_center_summary(joint4_center)
+            print_math_summary(math_state)
+            print_joint_summary(command_state)
+
+            if args.show_fk:
+                solved_pose = forward_kinematics(geometry, math_state)
+                print("Solved FK check:")
+                print_pose_summary(solved_pose)
+
+        else:
+            raise ArmControlError(f"未知模式: {args.mode}")
+
+        final_payload = command_state.to_udp_message()
+        print("Final UDP payload:", final_payload.decode("utf-8").strip())
+        send_udp_commands(args.ip, args.port, command_path, args.dry_run, args.duration)
+        if use_state_cache and not args.dry_run:
+            save_cached_command_state(command_state)
+        if not args.dry_run:
+            print(f"Sent to {args.ip}:{args.port}")
+        return 0
+
+    except ArmControlError as exc:
+        print(f"错误: {exc}", file=sys.stderr)
+        return 2
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())