docs: 重写机械臂运动学文档

- 清晰的章节结构：几何参数、正运动学、逆运动学
- 完整的数学推导（带 LaTeX 公式）
- Python 代码实现示例
- 工作空间分析和奇异点说明
- 运动插值算法
- 实用示例和测试命令

替换了原有的混乱结构，现在更易理解和参考

docs/arm.md

@@ -1,115 +1,424 @@
-这是一个非常漂亮且实用的自定义机械臂！从你的描述和图片来看，这属于一个**类 SCARA（水平多关节）构型**的机械臂，带有一个直线 Z 轴和三个平面的旋转关节。
+# 机械臂运动学推导
 
-为了让数学表达更清晰，我们先统一一下坐标系和变量的定义。
+6-DOF 机械臂逆运动学和正运动学完整推导。
 
-除角度外，所有线性长度和坐标统一使用 **mm**。
+## 机械臂结构
 
-### 变量与坐标系定义
+### 关节配置
 
-我们将基坐标系原点设在红色滑轨的零点位置。
+```
+J1: 线性滑轨（垂直运动，高度 d1）
+J2: 基座旋转（绕 Z 轴，XY 平面）
+J3: 肘关节（绕 Z 轴，XY 平面）
+J4: 腕关节（绕 Z 轴，XY 平面）
+J5: 末端俯仰（0° = 水平）
+J6: 夹爪旋转
+```
 
-* **$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$（夹爪相对关节四的高度偏移）。
 
----
+| 参数 | 值 (mm) | 说明 |
+|------|---------|------|
+| L1 | 125 | J2-J3 连杆长度 |
+| L2 | 125 | J3-J4 连杆长度 |
+| x4 | 110 | J4-TCP 水平偏移 |
+| z4 | 80 | J4-TCP 垂直偏移（夹爪状态相关：张开 55mm，闭合 -100mm） |
 
-### 1. 运动学正解 (Forward Kinematics)
+### 坐标系
 
-正解的目的是：**已知各个电机的角度和滑块高度 $(d_1, \theta_2, \theta_3, \theta_4)$，求夹爪末端的位置 $(X, Y, Z, \Phi)$。**
+**基坐标系**（右手系，Z 轴朝上）：
+```
+        Z↑ (高度)
+        |
+        |
+        o----→ X (前)
+       /
+      /
+     Y (左)
+```
 
-因为 Z 轴的直线运动与 XY 平面的旋转运动是完全解耦的，我们可以分别计算：
+- 原点：J1 滑轨底部
+- d1 范围：[-290, 0] mm（负值表示在基准面以下）
 
-**高度 (Z 轴):**
+## 正运动学（FK）
 
+已知关节状态 → 计算 TCP 位姿
 
-$$Z = d_1 + z_4$$
+### 输入
 
-**平面朝向角 (偏航角 $\Phi$):**
+- `d1`: 高度（J1 线性位移）
+- `θ2, θ3, θ4`: J2/J3/J4 角度（度）
 
+### 输出
 
-$$\Phi = \theta_2 + \theta_3 + \theta_4$$
+- `(x, y, z)`: TCP 位置（mm）
+- `φ`: TCP 偏航角（度）
 
-**平面坐标 (X, Y):**
+### 推导
 
+**步骤 1**：计算 J4 中心位置（XY 平面）
 
-$$X = L_1 \cos(\theta_2) + L_2 \cos(\theta_2 + \theta_3) + x_4 \cos(\theta_2 + \theta_3 + \theta_4)$$
+J2 和 J3 形成二连杆机构：
 
-$$Y = L_1 \sin(\theta_2) + L_2 \sin(\theta_2 + \theta_3) + x_4 \sin(\theta_2 + \theta_3 + \theta_4)$$
+$$
+x_{j4} = L_1 \cos\theta_2 + L_2 \cos(\theta_2 + \theta_3)
+$$
 
-*(注意：在实际编程中，如果你的电机零点不是一条直线，需要在角度上加上相应的初始偏置)*
+$$
+y_{j4} = L_1 \sin\theta_2 + L_2 \sin(\theta_2 + \theta_3)
+$$
 
----
+**步骤 2**：计算 TCP 偏航角
 
-### 2. 运动学逆解 (Inverse Kinematics)
+$$
+\phi = \theta_2 + \theta_3 + \theta_4
+$$
 
-逆解的目的是：**给出夹爪期望到达的目标位置和朝向 $(X, Y, Z, \Phi)$，求出各关节需要运动到的目标值 $(d_1, \theta_2, \theta_3, \theta_4)$。**
+**步骤 3**：计算 TCP 位置
 
-> **注意：** 你的机械臂在 XY 平面上有 3 个旋转自由度，但平面位置只需要 2 个自由度 $(X,Y)$。这意味着如果只给定目标坐标，机械臂有无数种姿态可以到达（冗余）。因此，**为了得到唯一解，必须同时指定夹爪的最终期望朝向角 $\Phi$**。
+从 J4 中心沿 φ 方向偏移 x4，垂直偏移 z4：
 
-下面是逆解的推导步骤，非常适合直接转化为固件中的控制代码：
+$$
+x = x_{j4} + x_4 \cos\phi
+$$
 
-#### 第一步：求解滑块高度 $d_1$
+$$
+y = y_{j4} + x_4 \sin\phi
+$$
 
-高度依然是解耦的，直接通过目标 $Z$ 坐标和常量偏移计算：
+$$
+z = d_1 - z_4
+$$
 
+### Python 实现
 
-$$d_1 = Z - z_4$$
+```python
+def forward_kinematics(d1, theta2_deg, theta3_deg, theta4_deg, L1=125, L2=125, x4=110, z4=80):
+    """正运动学"""
+    theta2 = math.radians(theta2_deg)
+    theta3 = math.radians(theta3_deg)
+    theta4 = math.radians(theta4_deg)
+    phi = theta2 + theta3 + theta4
+    
+    # J4 中心
+    x_j4 = L1 * math.cos(theta2) + L2 * math.cos(theta2 + theta3)
+    y_j4 = L1 * math.sin(theta2) + L2 * math.sin(theta2 + theta3)
+    
+    # TCP 位置
+    x = x_j4 + x4 * math.cos(phi)
+    y = y_j4 + x4 * math.sin(phi)
+    z = d1 - z4
+    
+    return x, y, z, math.degrees(phi)
+```
 
-#### 第二步：反推关节 4 的坐标 $(X_4, Y_4)$
+## 逆运动学（IK）
 
-既然我们知道末端目标的坐标 $(X, Y)$ 和总朝向 $\Phi$，我们可以把夹爪的偏置 $x_4$ “剥离”掉，求出关节 4 中轴线在空间中的位置：
+已知 TCP 目标位姿 → 计算关节角度
 
+### 输入
 
-$$X_4 = X - x_4 \cos(\Phi)$$
+- `(x, y, z)`: 目标位置（mm）
+- `φ`: 目标偏航角（度）
 
-$$Y_4 = Y - x_4 \sin(\Phi)$$
+### 输出
 
-#### 第三步：求解关节 3 的角度 $\theta_3$
+- `d1`: 高度
+- `θ2, θ3, θ4`: 关节角度（度）
 
-现在问题简化为了一个标准的双连杆（两轴）平面机械臂求逆解问题。目标点是 $(X_4, Y_4)$，连杆是 $L_1$ 和 $L_2$。
+### 推导
-根据余弦定理，设目标点到原点的距离平方为 $r^2 = X_4^2 + Y_4^2$，有：
 
+**步骤 1**：计算 J4 中心目标位置
 
-$$\cos(\theta_3) = \frac{X_4^2 + Y_4^2 - L_1^2 - L_2^2}{2 L_1 L_2}$$
+从 TCP 位置反向计算 J4 位置：
 
-设 $C_3 = \cos(\theta_3)$。在实际控制代码中，必须在这里做合法性检查：如果 $C_3 > 1$ 或 $C_3 < -1$，说明目标点超出了机械臂的物理工作空间（够不到）。
+$$
-如果合法，则 $\theta_3$ 的正弦值为：
+x_{j4} = x - x_4 \cos\phi
+$$
 
+$$
+y_{j4} = y - x_4 \sin\phi
+$$
 
-$$S_3 = \pm \sqrt{1 - C_3^2}$$
+$$
+z_{j4} = z + z_4
+$$
 
+$$
+d_1 = z_{j4}
+$$
 
-*(这里的 $\pm$ 代表机械臂的两种姿态：“左手系/右臂”或“右手系/左臂”，也就是俗称的“手肘朝左”还是“手肘朝右”。你可以根据防碰撞需求或当前姿态选择其中一个)*
+**步骤 2**：计算平面距离
 
-最终使用反正切函数求解 $\theta_3$：
+$$
+r = \sqrt{x_{j4}^2 + y_{j4}^2}
+$$
 
+**步骤 3**：求解 θ3（余弦定理）
 
-$$\theta_3 = \text{atan2}(S_3, C_3)$$
+二连杆机构的标准解法：
 
-#### 第四步：求解关节 2 的角度 $\theta_2$
+$$
+\cos\theta_3 = \frac{r^2 - L_1^2 - L_2^2}{2L_1L_2}
+$$
 
-利用几何关系和已经求出的 $\theta_3$，可以通过组合角度直接求出 $\theta_2$：
+检查工作空间：
 
+$$
+-1 \leq \cos\theta_3 \leq 1 \quad \Rightarrow \quad |L_1 - L_2| \leq r \leq L_1 + L_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_3 = \pm \arccos\left(\frac{r^2 - L_1^2 - L_2^2}{2L_1L_2}\right)
+$$
 
+**步骤 4**：求解 θ2
 
-$$\theta_4 = \Phi - \theta_2 - \theta_3$$
+$$
+\theta_2 = \arctan2(y_{j4}, x_{j4}) - \arctan2\left(L_2\sin\theta_3, L_1 + L_2\cos\theta_3\right)
+$$
 
----
+**步骤 5**：求解 θ4
 
-### 💡 嵌入式固件实现建议
+$$
+\theta_4 = \phi - \theta_2 - \theta_3
+$$
 
-由于你很可能需要将这些公式写入 MCU（比如利用 C/C++ 或 Rust 编写固件），这里有几个实践建议：
+### Python 实现
 
-1. **使用 `atan2` 替代 `asin/acos**`：上面逆解公式中我全部使用了 $\text{atan2}(y, x)$。在标准库中，`atan2` 能够自动处理四个象限的符号问题，且能避免 $x=0$ 时的除零错误，这在底层驱动中至关重要。
+```python
-2. **死区与奇异点保护**：当 $X_4^2 + Y_4^2 \approx 0$ 时（关节 4 缩回到了原点正上方），此时 $\theta_2$ 会失去意义（奇异点）。在代码中应当加入对 $X_4^2 + Y_4^2 < \epsilon$（一个极小值）的判断，防止产生 NaN。
+def inverse_kinematics(x, y, z, phi_deg, elbow_up=False, L1=125, L2=125, x4=110, z4=80):
-3. **角度范围归一化**：计算出的角度可能会超出电机支持的物理限位范围（例如超出了 $[-180^\circ, +180^\circ]$），在下发脉冲或指令前，记得对 $\theta_2, \theta_3, \theta_4$ 进行归一化和软限位拦截。
+    """逆运动学"""
+    phi = math.radians(phi_deg)
+    
+    # 步骤 1: 计算 J4 中心
+    x_j4 = x - x4 * math.cos(phi)
+    y_j4 = y - x4 * math.sin(phi)
+    z_j4 = z + z4
+    d1 = z_j4
+    
+    # 步骤 2: 平面距离
+    r2 = x_j4**2 + y_j4**2
+    r = math.sqrt(r2)
+    
+    # 步骤 3: 求解 theta3
+    cos_theta3 = (r2 - L1**2 - L2**2) / (2 * L1 * L2)
+    
+    if cos_theta3 < -1 or cos_theta3 > 1:
+        raise ValueError(f"目标超出工作空间，r={r:.1f}mm")
+    
+    # 肘部朝上：负角度，肘部朝下：正角度
+    sin_theta3 = -math.sqrt(1 - cos_theta3**2) if elbow_up else math.sqrt(1 - cos_theta3**2)
+    theta3 = math.atan2(sin_theta3, cos_theta3)
+    
+    # 步骤 4: 求解 theta2
+    theta2 = math.atan2(y_j4, x_j4) - math.atan2(
+        L2 * sin_theta3,
+        L1 + L2 * cos_theta3
+    )
+    
+    # 步骤 5: 求解 theta4
+    theta4 = phi - theta2 - theta3
+    
+    # 角度归一化到 [-180, 180)
+    def normalize(angle):
+        a = (angle + math.pi) % (2 * math.pi) - math.pi
+        return a if a != -math.pi or angle <= 0 else math.pi
+    
+    return (
+        d1,
+        math.degrees(normalize(theta2)),
+        math.degrees(normalize(theta3)),
+        math.degrees(normalize(theta4))
+    )
+```
+
+## 零点偏移
+
+物理零点（机械对齐）与数学零点（直线构型）存在偏差：
+
+| 关节 | 零点偏移 |
+|------|----------|
+| J2 | +3° |
+| J3 | +7° |
+| J4 | +25° |
+
+### 转换关系
+
+**命令角度 ↔ 数学角度**：
+
+$$
+\theta_{\text{command}} = \theta_{\text{math}} + \text{offset}
+$$
+
+$$
+\theta_{\text{math}} = \theta_{\text{command}} - \text{offset}
+$$
+
+**使用**：
+- 正运动学：先减去偏移（命令 → 数学），再计算
+- 逆运动学：先计算（数学），再加上偏移（数学 → 命令）
+
+## 工作空间
+
+### 高度范围
+
+$$
+z \in [-290 - z_4, 0 - z_4] = [-370, -80] \text{ mm}
+$$
+
+### 水平范围
+
+$$
+r_{\min} = |L_1 - L_2| + x_4 = 0 + 110 = 110 \text{ mm}
+$$
+
+$$
+r_{\max} = L_1 + L_2 + x_4 = 125 + 125 + 110 = 360 \text{ mm}
+$$
+
+**可达圆环**：半径 110mm 到 360mm
+
+### 关节限位
+
+| 关节 | 最小值 | 最大值 | 单位 |
+|------|--------|--------|------|
+| height | -290 | 0 | mm |
+| J2 | -110 | 115 | ° |
+| J3 | -120 | 145 | ° |
+| J4 | -90 | 130 | ° |
+| J5 | -180 | 180 | ° |
+| J6 | -180 | 180 | ° |
+
+## 奇异点
+
+### 1. 肩部奇异点
+
+当 J4 中心在原点正上方：
+
+$$
+x_{j4} = y_{j4} = 0 \quad \Rightarrow \quad r = 0
+$$
+
+此时 θ2 无定义（可取任意值）。
+
+**避免**：保持 `r > 10mm`
+
+### 2. 肘部奇异点
+
+当机械臂完全伸直或完全折叠：
+
+$$
+\theta_3 = 0° \text{ 或 } \pm 180°
+$$
+
+此时运动控制不稳定。
+
+**避免**：保持 `|θ3| > 5°`
+
+## 运动插值
+
+关节空间线性插值，避免笛卡尔空间的复杂路径规划。
+
+### 算法
+
+给定起点 `q_start` 和终点 `q_end`，生成 N 个中间点：
+
+$$
+q_i = q_{\text{start}} + \frac{i}{N}(q_{\text{end}} - q_{\text{start}}), \quad i = 1, 2, \ldots, N
+$$
+
+### 参数
+
+- `duration`: 运动时长（秒）
+- `rate`: 插值频率（Hz）
+- `steps = ceil(duration × rate)`: 插值点数
+
+**示例**：`duration=2.0s`, `rate=20Hz` → `steps=40`
+
+### Python 实现
+
+```python
+def interpolate_joints(start, end, duration=2.0, rate=20.0):
+    """关节空间插值"""
+    steps = max(1, int(math.ceil(duration * rate)))
+    trajectory = []
+    
+    for i in range(1, steps + 1):
+        t = i / steps
+        state = {
+            key: int(round(start[key] + t * (end[key] - start[key])))
+            for key in start.keys()
+        }
+        trajectory.append(state)
+    
+    return trajectory
+```
+
+## 完整示例
+
+### 示例 1：前方抓取
+
+**目标**：抓取前方 250mm，右侧 50mm，高度 -150mm 的物体
+
+```python
+# 逆运动学
+d1, theta2, theta3, theta4 = inverse_kinematics(
+    x=250, y=50, z=-150, phi_deg=0
+)
+# 输出: d1=-70, theta2=11.3°, theta3=-48.6°, theta4=37.3°
+
+# 验证：正运动学
+x, y, z, phi = forward_kinematics(d1, theta2, theta3, theta4)
+# 输出: (250.0, 50.0, -150.0, 0.0°) ✓
+```
+
+### 示例 2：多段轨迹
+
+```python
+# 初始位置
+start = {'height': 0, 'j2': 0, 'j3': 0, 'j4': 0, 'j5': 81, 'j6': 30}
+
+# 目标 1: 上方
+_, theta2, theta3, theta4 = inverse_kinematics(200, 100, -50, 45)
+waypoint1 = {'height': -50, 'j2': theta2, 'j3': theta3, 'j4': theta4, 'j5': 81, 'j6': 30}
+
+# 目标 2: 抓取位置
+_, theta2, theta3, theta4 = inverse_kinematics(200, 100, -150, 45)
+waypoint2 = {'height': -150, 'j2': theta2, 'j3': theta3, 'j4': theta4, 'j5': -100, 'j6': -5}
+
+# 生成轨迹
+traj1 = interpolate_joints(start, waypoint1, duration=2.0)      # 2秒到上方
+traj2 = interpolate_joints(waypoint1, waypoint2, duration=1.0)  # 1秒下降抓取
+```
+
+## 参考
+
+### UDP 命令格式
+
+```
+JXB:<height>:<J2>:<J3>:<J4>:<J5>:<J6>:0:0:EZHY\n
+```
+
+- 所有角度值已包含零点偏移
+- 直接发送到 ESP32 的 UDP 端口 8888
+
+### 相关工具
+
+- `tools/udp_control.py` - 命令行控制（支持 `joints` 和 `pose` 模式）
+- `ros2/src/udp_teleop/udp_teleop/arm_control.py` - ROS 节点
+- `docs/vision_calibration_horizontal.md` - 相机坐标变换
+
+### 测试
+
+```bash
+# 测试逆运动学
+python tools/udp_control.py pose --x 200 --y 100 --z -100 --phi 45 --dry-run
+
+# 发送命令
+python tools/udp_control.py pose --x 200 --y 100 --z -100 --phi 45 --duration 2.0
+```