# 机械臂节拍时间优化（Robot Arm Cycle Time Optimization）

## 1. 背景

在现代制造业中——汽车车身焊接、电子 PCB 装配、仓储取放——工业机械臂每天重复同一动作数千次。在单条生产线上，**每节省 1 秒节拍时间**即可转化为每年数百万元的额外产能价值。

本任务对这一核心瓶颈进行建模：给定固定的起始构型和目标构型，找出满足七自由度机械臂所有硬件限制的**最快无碰撞关节空间轨迹**。

## 2. 机器人规格

机器人以 **KUKA LBR iiwa 14 R820** 为原型建模，这是一款广泛应用于工业和科研领域的七自由度协作机械臂。

### 2.1 运动学（标准 DH 参数）

正向运动学遵循标准 Denavit–Hartenberg 约定，每个齐次变换为：

```
T_i = Rz(θ_i) · Tz(d_i) · Tx(a_i) · Rx(α_i)
```

| 关节 | a (m) | d (m)  | α (rad)  | θ 范围 (rad)     |
|------|-------|--------|----------|-----------------|
| 1    | 0.0   | 0.340  | +π/2     | [−2.967, +2.967] |
| 2    | 0.0   | 0.000  | −π/2     | [−2.094, +2.094] |
| 3    | 0.0   | 0.400  | −π/2     | [−2.967, +2.967] |
| 4    | 0.0   | 0.000  | +π/2     | [−2.094, +2.094] |
| 5    | 0.0   | 0.400  | +π/2     | [−2.967, +2.967] |
| 6    | 0.0   | 0.000  | −π/2     | [−2.094, +2.094] |
| 7    | 0.0   | 0.126  |  0.000   | [−3.054, +3.054] |

### 2.2 硬件限制

| 关节 | 最大速度 (rad/s) | 最大加速度 (rad/s²) |
|------|----------------|-------------------|
| 1    | 1.48           | 3.0               |
| 2    | 1.48           | 3.0               |
| 3    | 1.74           | 3.0               |
| 4    | 1.74           | 3.0               |
| 5    | 2.27           | 3.0               |
| 6    | 2.27           | 3.0               |
| 7    | 2.27           | 3.0               |

### 2.3 连杆碰撞几何

每个连杆用**胶囊体**（两端半球形的圆柱）近似，连接相邻关节坐标系原点，各连杆胶囊半径如下：

| 连杆 | 半径 (m) |
|------|---------|
| 1    | 0.10    |
| 2    | 0.10    |
| 3    | 0.10    |
| 4    | 0.08    |
| 5    | 0.08    |
| 6    | 0.08    |
| 7    | 0.06    |

## 3. 任务定义

### 3.1 起始与目标构型

单位：弧度

```
q_start = [0.0,  0.5,  0.0, -1.5,  0.0,  1.0,  0.0]
q_goal  = [1.2, -0.3,  0.8, -0.8,  0.5,  0.8,  1.0]
```

### 3.2 环境障碍物

工作空间中放置一个轴对齐矩形障碍物（AABB）：

- **中心**：[0.45, -0.35, 0.65] m（基坐标系中）
- **半尺寸**：[0.08, 0.20, 0.08] m

机械臂在运动全程中不得与此障碍物发生碰撞。

### 3.3 优化目标

**最小化**总运动时间 `T`（秒），约束条件：

1. 轨迹起点为 `q_start`，终点为 `q_goal`（每关节容差 0.01 rad）。
2. 任意时刻所有关节角度均在限位范围内。
3. 任意时刻所有关节速度不超过 `MAX_VELOCITIES`。
4. 任意时刻所有关节加速度不超过 `MAX_ACCELERATIONS`。
5. 任意时刻机器人连杆不与障碍物碰撞。

## 4. 轨迹表示

轨迹以**路径点序列**及对应**时间戳**表示，评测器在路径点之间使用三次样条（夹紧边界条件）进行插值。

## 5. 输入 / 输出

### 5.1 输入

无需外部数据文件，所有参数均包含在本文档及 `references/robot_config.json` 中。

### 5.2 输出

生成 `submission.json` 文件，格式如下：

```json
{
  "waypoints": [
    [q1, q2, q3, q4, q5, q6, q7],
    ...
  ],
  "timestamps": [0.0, t1, t2, ..., T]
}
```

**要求**：

- `waypoints[0]` 须在每关节 0.01 rad 精度内等于 `q_start`。
- `waypoints[-1]` 须在每关节 0.01 rad 精度内等于 `q_goal`。
- `timestamps[0]` 须为 `0.0`。
- `timestamps` 须**严格递增**。
- 至少 **2** 个路径点（起点 + 终点）。
- 中间路径点数量不限。

## 6. 评测

评测器（`verification/evaluator.py`）执行以下步骤：

1. 格式与边界验证。
2. 对路径点构建三次样条。
3. 每段以 **30 个采样点**检查关节限位、速度限制和加速度限制。
4. 在每个采样点计算正向运动学，检验胶囊体与 AABB 是否相交。
5. 可行则返回 `T = timestamps[-1]`，否则返回 `+inf`。

**得分**：`T`（秒），**越低越好**。

## 7. 基线方案

`baseline/solution.py` 采用线性关节插值（最大速度的 60%）加单个避障中间点，演示提交格式并给出可行（但非最优）的初始解。

## 8. 参考资料

- 机器人参数：`references/robot_config.json`
- 评测器：`verification/evaluator.py`
- KUKA LBR iiwa 产品页面：https://www.kuka.com/en-us/products/robotics-systems/industrial-robots/lbr-iiwa
