# 线性二次调节器

LQR（Linear Quadratic Regulator，线性二次调节器）是一个典型的连续控制与稳定化任务。本仓库当前提供两个变体：

-   `dm-lqr-2-1`：两个小球通过绳索相连，只有最后一个小球受控。
-   `dm-lqr-6-2`：六个小球串联成链，只有最后两个小球受控。

智能体需要在尽量小的控制代价下，将整条系统拉回中心附近并稳定停下。

```{video} /_static/videos/dm_lqr_2_1.mp4
:poster: _static/images/poster/dm_lqr_2_1.jpg
:nocontrols:
:autoplay:
:playsinline:
:muted:
:loop:
:width: 100%
```

```{video} /_static/videos/dm_lqr_6_2.mp4
:poster: _static/images/poster/dm_lqr_6_2.jpg
:nocontrols:
:autoplay:
:playsinline:
:muted:
:loop:
:width: 100%
```

---

## 任务描述

这两个任务都可以看作一维弹簧-阻尼链的平衡控制问题。每个小球只有一个沿 x 轴的滑动自由度，相邻小球之间通过“绳”形成耦合，系统同时受到：

-   球体自身阻尼
-   相邻球体之间的弹簧力和相对速度阻尼
-   指向原点的中心回复力
-   控制输入作用在末端受控自由度上

其中：

-   `dm-lqr-2-1` 更简单，适合先验证是否能学会稳定驻点。
-   `dm-lqr-6-2` 需要把控制作用沿链条传递到更远的非受控球，难度更高。

---

## 动作空间（Action Space）

### dm-lqr-2-1

| 项目     | 详细信息                        |
| -------- | ------------------------------- |
| **类型** | `Box(-1.0, 1.0, (1,), float32)` |
| **维度** | 1                               |

| 序号 | 动作含义                 | 最小值 | 最大值 | 对应 XML 关节 |
| ---: | ------------------------ | :----: | :----: | :-----------: |
|    0 | 作用在末端球上的控制输入 |  -1.0  |  1.0   |     `q1`      |

### dm-lqr-6-2

| 项目     | 详细信息                        |
| -------- | ------------------------------- |
| **类型** | `Box(-1.0, 1.0, (2,), float32)` |
| **维度** | 2                               |

| 序号 | 动作含义               | 最小值 | 最大值 | 对应 XML 关节 |
| ---: | ---------------------- | :----: | :----: | :-----------: |
|    0 | 倒数第二个球的控制输入 |  -1.0  |  1.0   |     `q4`      |
|    1 | 最后一个球的控制输入   |  -1.0  |  1.0   |     `q5`      |

---

## 观察空间

观察由所有位置 `qpos` 和速度 `qvel` 直接拼接而成。

### dm-lqr-2-1

| 项目     | 详细信息                        |
| -------- | ------------------------------- |
| **类型** | `Box(-inf, inf, (4,), float32)` |
| **维度** | 4                               |

| 序号 | 观察量 | 含义            |
| ---: | ------ | --------------- |
|    0 | `q0`   | 第 1 个球的位置 |
|    1 | `q1`   | 第 2 个球的位置 |
|    2 | `dq0`  | 第 1 个球的速度 |
|    3 | `dq1`  | 第 2 个球的速度 |

### dm-lqr-6-2

| 项目     | 详细信息                         |
| -------- | -------------------------------- |
| **类型** | `Box(-inf, inf, (12,), float32)` |
| **维度** | 12                               |

前 6 维为 `q0 ~ q5`，后 6 维为 `dq0 ~ dq5`。

---

## 奖励函数设计

当前实现的奖励由状态代价、速度代价、控制代价、成功奖励和越界惩罚组成：

```python
state_cost = 0.5 * sum(qpos ** 2)
velocity_cost = 0.5 * velocity_cost_coef * sum(qvel ** 2)
control_cost = 0.5 * control_cost_coef * sum(action ** 2)

reward = 1.0 - (state_cost + velocity_cost + control_cost)
reward += success_bonus
reward -= out_of_bounds_penalty
```

直观上：

-   离中心越远，奖励越低
-   速度越大，奖励越低
-   控制越激进，奖励越低
-   当系统进入“接近原点且速度足够小”的稳定区域时，会得到成功奖励
-   越界时会受到额外惩罚

---

## 初始状态

每次重置时：

-   位置向量从随机方向采样，再归一化到固定范数
-   速度初始化为 0

当前配置中：

-   `dm-lqr-2-1` 的初始位置范数约为 `0.8`
-   `dm-lqr-6-2` 的初始位置范数约为 `1.0`

---

## Episode 终止条件

满足以下任一条件时，当前 episode 结束并重置：

-   达到成功条件：
    位置范数小于成功距离阈值，且速度范数小于成功速度阈值
-   超出边界：
    任意位置超过边界范围，或任意速度超过速度边界
-   状态非常接近零点平衡态
-   观察或动作中出现 `NaN`

---

## 使用指南

### 1. 环境预览

```bash
uv run scripts/view.py --env dm-lqr-2-1
uv run scripts/view.py --env dm-lqr-6-2
```

### 2. 开始训练

```bash
uv run scripts/train.py --env dm-lqr-2-1
uv run scripts/train.py --env dm-lqr-6-2
```

### 3. 查看训练曲线

```bash
uv run tensorboard --logdir runs/dm-lqr-2-1
uv run tensorboard --logdir runs/dm-lqr-6-2
```

### 4. 测试训练结果

```bash
uv run scripts/play.py --env dm-lqr-2-1
uv run scripts/play.py --env dm-lqr-6-2
```

---

## 预期训练结果

### dm-lqr-2-1

1. 受控球能够带动未受控球一起回到中心附近。
2. 两个球的位置和速度最终都收敛到较小范围内。
3. 策略不会长期停在偏离中心的静止点。

### dm-lqr-6-2

1. 最后两个受控球能够把整条链逐步拉回中心。
2. 链条传播过程中不会出现明显发散或持续振荡。
3. 成功率随训练推进逐步上升，越界率逐步下降。