# 机械臂运动学推导

6-DOF 机械臂逆运动学和正运动学完整推导。

## 机械臂结构

### 关节配置

```
J1: 线性滑轨（垂直运动，高度 d1）
J2: 基座旋转（绕 Z 轴，XY 平面）
J3: 肘关节（绕 Z 轴，XY 平面）
J4: 腕关节（绕 Z 轴，XY 平面）
J5: 末端俯仰（0° = 水平）
J6: 夹爪旋转
```

### 几何参数

| 参数 | 值 (mm) | 说明 |
|------|---------|------|
| L1 | 125 | J2-J3 连杆长度 |
| L2 | 125 | J3-J4 连杆长度 |
| x4 | 110 | J4-TCP 水平偏移 |
| z4 | 80 | J4-TCP 垂直偏移（夹爪状态相关：张开 55mm，闭合 -100mm） |

### 坐标系

**基坐标系**（右手系，Z 轴朝上）：
```
        Z↑ (高度)
        |
        |
        o----→ X (前)
       /
      /
     Y (左)
```

- 原点：J1 滑轨底部
- d1 范围：[-290, 0] mm（负值表示在基准面以下）

## 正运动学（FK）

已知关节状态 → 计算 TCP 位姿

### 输入

- `d1`: 高度（J1 线性位移）
- `θ2, θ3, θ4`: J2/J3/J4 角度（度）

### 输出

- `(x, y, z)`: TCP 位置（mm）
- `φ`: TCP 偏航角（度）

### 推导

**步骤 1**：计算 J4 中心位置（XY 平面）

J2 和 J3 形成二连杆机构：

$$
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 偏航角

$$
\phi = \theta_2 + \theta_3 + \theta_4
$$

**步骤 3**：计算 TCP 位置

从 J4 中心沿 φ 方向偏移 x4，垂直偏移 z4：

$$
x = x_{j4} + x_4 \cos\phi
$$

$$
y = y_{j4} + x_4 \sin\phi
$$

$$
z = d_1 - z_4
$$

### Python 实现

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

## 逆运动学（IK）

已知 TCP 目标位姿 → 计算关节角度

### 输入

- `(x, y, z)`: 目标位置（mm）
- `φ`: 目标偏航角（度）

### 输出

- `d1`: 高度
- `θ2, θ3, θ4`: 关节角度（度）

### 推导

**步骤 1**：计算 J4 中心目标位置

从 TCP 位置反向计算 J4 位置：

$$
x_{j4} = x - x_4 \cos\phi
$$

$$
y_{j4} = y - x_4 \sin\phi
$$

$$
z_{j4} = z + z_4
$$

$$
d_1 = z_{j4}
$$

**步骤 2**：计算平面距离

$$
r = \sqrt{x_{j4}^2 + y_{j4}^2}
$$

**步骤 3**：求解 θ3（余弦定理）

二连杆机构的标准解法：

$$
\cos\theta_3 = \frac{r^2 - L_1^2 - L_2^2}{2L_1L_2}
$$

检查工作空间：

$$
-1 \leq \cos\theta_3 \leq 1 \quad \Rightarrow \quad |L_1 - L_2| \leq r \leq L_1 + L_2
$$

否则目标超出工作空间。

有两个解（肘部朝上 / 朝下）：

$$
\theta_3 = \pm \arccos\left(\frac{r^2 - L_1^2 - L_2^2}{2L_1L_2}\right)
$$

**步骤 4**：求解 θ2

$$
\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
$$

### Python 实现

```python
def inverse_kinematics(x, y, z, phi_deg, elbow_up=False, L1=125, L2=125, x4=110, z4=80):
    """逆运动学"""
    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
```