SimToReal 概述

AKA-Sim2Real 是一个基于前视视角的自动驾驶模拟到真实（Sim2Real）系统，旨在通过模拟器采集人类驾驶数据，训练 ACT（Action Chunking Transformer）模型，并将训练好的策略迁移到真实小车上运行。

系统架构

┌─────────────────────────────────────────────────────────┐
│                     AKA-Sim2Real                        │
│                                                         │
│  ┌─────────────┐    ┌──────────────┐    ┌───────────┐  │
│  │  模拟器/真车  │───▶│  数据采集模块  │───▶│  数据集   │  │
│  │ (Sim/Real)  │    │  Episode API │    │ output/  │  │
│  └─────────────┘    └──────────────┘    │ dataset/ │  │
│         │                               └────┬─────┘  │
│         │                                    │        │
│         │           ┌──────────────┐    ┌────▼─────┐  │
│         │           │  ACT 模型推理  │◀───│ 模型训练  │  │
│         └──────────▶│  Inference   │    │Training  │  │
│                     │   Runtime    │    └──────────┘  │
│                     └──────────────┘                  │
└─────────────────────────────────────────────────────────┘

系统由三个核心部分组成：

模块	说明
模拟器 / 真车接口	提供前视视角仿真环境，支持键盘手动控制与自动推理两种模式
数据采集	记录图像帧 + 车辆状态 + 动作，导出为结构化数据集
ACT 训练与推理	基于 Action Chunking Transformer 进行模仿学习，支持 CVAE 与时序集成

技术栈

层级	技术
后端框架	FastAPI + Socket.IO (Python)
深度学习	PyTorch + ResNet18 + Transformer
前端	React + TypeScript + Socket.IO Client

文档目录

快速开始 — 安装依赖、启动服务
数据采集 — 在模拟器中录制驾驶演示
模型训练 — 训练 ACT 模型
模型推理 — 加载模型并运行自动驾驶
ACT 模型架构 — 技术原理与模型细节

快速开始

本节介绍如何在本地搭建并运行 AKA-Sim2Real 系统。

环境要求

依赖	版本建议
Python	3.10+
Node.js	18+
PyTorch	2.0+（支持 CUDA/MPS/CPU）

1. 克隆仓库

git clone <仓库地址>
cd AKA-Sim2Real

2. 安装后端依赖

cd backend
pip install -r requirements.txt

提示：推荐使用 conda 或 venv 创建独立 Python 环境，避免依赖冲突。

3. 安装前端依赖

cd ui
npm install

4. 启动后端服务

cd backend
python main.py

后端将运行在 http://localhost:8000

启动成功后，终端会输出类似以下信息：

INFO:     Uvicorn running on http://0.0.0.0:8000 (Press CTRL+C to quit)

注意：后端启动时会尝试自动加载 output/train/model.pt，如果模型文件不存在，则以无模型状态运行（推理功能不可用，但数据采集和训练功能正常）。

6. 访问界面

在浏览器中打开 http://localhost:5173，可以看到：

Sim 页面：模拟器视角，用于数据采集与模拟推理
Real 页面：真实小车接口视图

目录结构说明

AKA-Sim2Real/
├── backend/          # Python 后端（FastAPI + Socket.IO）
│   ├── main.py       # 服务入口
│   ├── api/          # REST API 路由
│   ├── sio_handlers/ # Socket.IO 事件处理
│   └── services/     # 核心业务逻辑（ACT、训练、采集）
├── ui/               # React 前端
├── policies/         # ACT 模型定义与训练脚本
├── output/           # 运行时输出目录
│   ├── dataset/      # 采集的数据集
│   └── train/        # 训练输出（model.pt）
└── tests/act/        # ACT 最小回归测试

下一步

数据采集 — 开始录制驾驶演示数据
模型训练 — 训练 ACT 策略模型
模型推理 — 运行自动驾驶推理

数据采集

数据采集是 Sim2Real 流程的第一步。通过在模拟器中手动驾驶小车，系统会同步记录摄像头图像、车辆状态和控制动作，最终导出为结构化数据集供 ACT 模型训练使用。

字段	类型	说明
`image`	RGB 图像帧	模拟器前视摄像头画面
`state`	`[vel_left, vel_right]`	左右轮速度（当前车辆状态）
`action`	`[vel_left, vel_right]`	左右轮控制指令

数据采集步骤

2. 控制小车

使用键盘控制小车行驶：

按键	动作
`W` / `↑`	前进
`S` / `↓`	后退
`A` / `←`	左转
`D` / `→`	右转

3. 开始录制

点击界面上的 "开始采集" 按钮，系统进入录制模式，此时每个控制帧的图像、状态和动作均会被记录。

4. 停止录制

完成一段演示后，点击 "停止采集" 按钮。数据会自动导出到：

output/dataset/

数据集结构

导出后的数据集组织结构如下：

output/dataset/
├── episode_0/
│   ├── images/        # 每帧图像（PNG 格式）
│   ├── states.npy     # 状态序列 [N, 2]
│   └── actions.npy    # 动作序列 [N, 2]
├── episode_1/
│   └── ...
└── stats.json         # 数据集统计信息（均值、标准差）

stats.json 保存了状态和动作的归一化统计量，推理时也需要用到：

{
  "state_mean": [0.0, 0.0],
  "state_std":  [1.0, 1.0],
  "action_mean": [0.0, 0.0],
  "action_std":  [1.0, 1.0]
}

采集建议

数量：建议至少采集 100 个样本（帧）才能开始训练，样本越多模型越稳定
多样性：涵盖直行、左转、右转等多种驾驶场景，提升模型泛化能力
一致性：每段演示应保持合理的驾驶行为，避免急停急转等极端操作

REST API（高级用法）

数据采集功能也通过 REST API 暴露，可用于自动化脚本：

方法	路径	说明
`POST`	`/api/episode/start`	开始采集
`POST`	`/api/episode/stop`	停止采集并导出
`GET`	`/api/episode/list`	列出所有 episode
`DELETE`	`/api/episode/{id}`	删除指定 episode

模型训练

采集到足够的演示数据后，即可训练 ACT（Action Chunking Transformer）模型。训练过程由后端的训练编排器（Training Orchestrator）自动完成，无需手动干预。

训练前提

已完成数据采集，数据保存在 output/dataset/
数据集中至少有 100 个样本

启动训练

在前端界面点击 "开始训练" 按钮，或通过 REST API 触发：

curl -X POST http://localhost:8000/api/training/start

训练流程详解

训练器（backend/services/training/orchestrator.py）执行以下步骤：

数据集加载
    │
    ▼
构建 ACT 配置
    │
    ▼
初始化模型（ACTModel）
    │
    ▼
训练循环（L1 Loss + KL Loss）
    │
    ▼
保存模型检查点
output/train/model.pt（含 CVAE 潜变量统计）

损失函数

ACT 使用两个损失项联合训练：

损失	公式	说明
重建损失	L1(predicted_actions, gt_actions)	动作预测的主要监督信号
KL 散度	KL(q(z\|obs,action) ∥ p(z\|obs))	CVAE 正则项，权重为 `kl_weight=0.1`

总损失：Loss = L1_loss + kl_weight × KL_loss

默认超参数

参数	默认值	说明
`state_dim`	2	状态维度（左右轮速）
`action_dim`	2	动作维度（左右轮指令）
`action_chunk_size`	8	每次预测的动作步数
`hidden_dim`	512	Transformer 隐层维度
`num_attention_heads`	8	多头注意力头数
`num_encoder_layers`	4	Transformer 编码器层数
`num_decoder_layers`	4	Transformer 解码器层数
`dim_feedforward`	3200	前馈网络中间维度
`kl_weight`	0.1	KL 散度损失权重
`latent_dim`	32	CVAE 潜变量维度
`use_cvae`	True	是否启用 CVAE

训练输出

训练完成后，模型保存在：

output/train/
├── model.pt           # 完整模型检查点（含 CVAE 潜变量统计）
└── final_model.pt     # 最终模型（如启用了 CVAE 则包含 latent stats）

检查点内容：

{
    "model_state_dict": ...,   # 模型权重
    "config": { ... },         # ACT 配置字典
    "latent_mean": ...,        # CVAE 潜变量均值（用于推理时采样）
    "latent_std":  ...,        # CVAE 潜变量标准差
}

查看训练状态

通过 REST API 查询训练进度：

# 获取训练状态
curl http://localhost:8000/api/training/status

# 停止训练
curl -X POST http://localhost:8000/api/training/stop

单独运行训练脚本（高级）

如需在命令行直接运行训练（不通过 Web 界面）：

python policies/models/act/train_act.py \
    --dataset_dir output/dataset \
    --output_dir output/train

模型推理

模型推理阶段，系统使用训练好的 ACT 模型替代人工操控，实现小车自主驾驶。系统同时支持**模拟器（Sim）和真实小车（Real）**两种模式。

推理前提

已完成模型训练，output/train/model.pt 存在
后端和前端均已启动

加载模型

方式一：自动加载（推荐）

后端启动时会自动尝试加载 output/train/model.pt，无需手动操作。可通过以下接口确认模型状态：

curl http://localhost:8000/health

返回示例：

{
  "model_loaded": true,
  "device": "mps"
}

方式三：REST API

curl -X POST http://localhost:8000/api/inference/load

启动自动推理

前端操作

点击 "自动推理" 按钮，系统进入自动驾驶模式：

摄像头每帧捕获当前图像
图像与车辆状态输入 ACT 模型
模型输出动作块（action chunk，8 个时间步）
取第一个动作执行，循环往复

Socket.IO 事件

推理通过 Socket.IO 实时传输控制指令：

事件	方向	说明
`start_inference`	客户端 → 服务端	启动自动推理
`stop_inference`	客户端 → 服务端	停止自动推理
`inference_action`	服务端 → 客户端	下发控制动作

推理流程详解

摄像头图像 ──┐
             ├──▶ ACTInferenceRuntime.infer()
车辆状态   ──┘         │
                       ├── 图像预处理（ResNet18 归一化）
                       ├── 状态归一化
                       ├── Transformer 编码解码
                       ├── 输出 action chunk [8, 2]
                       ├── （可选）时序集成（Temporal Ensembling）
                       └── 动作反归一化 → 发送给小车

图像预处理：图像统一缩放并按 ImageNet 均值/方差归一化后送入 ResNet18 视觉编码器。

状态归一化：使用数据集中保存的 state_mean 和 state_std 进行 Z-score 归一化。

动作反归一化：输出的动作使用 action_mean 和 action_std 还原为真实控制量。

时序集成（Temporal Ensembling）

时序集成是 ACT 论文中提出的平滑技术，通过对多个历史 action chunk 的加权融合来减少控制抖动。

启用方式（设置环境变量）：

export ACT_TEMPORAL_ENSEMBLING=1
python backend/main.py

衰减权重由 temporal_ensembling_weight 配置参数控制（默认 0.5），值越小融合越平滑。

注意：当前默认关闭时序集成，适合大多数场景。如控制输出存在抖动，可尝试开启。

Sim 与 Real 模式

系统同时支持两个 Socket.IO 命名空间：

命名空间	用途
`/sim`	模拟器推理，控制虚拟小车
`/real`	真实小车推理，通过硬件接口控制实体小车

两个命名空间使用相同的推理逻辑，仅底层执行器（模拟器控制器 vs 真实硬件驱动）不同。

推理 REST API

方法	路径	说明
`GET`	`/health`	查询模型加载状态
`POST`	`/api/inference/load`	加载模型
`POST`	`/api/inference/reset`	重置推理上下文（清空时序集成缓存）

推理调试

查看后端日志中的推理输出，确认模型正常运行：

INFO: 加载 ACT 模型...
INFO: 状态归一化: mean=[0.0, 0.0], std=[1.0, 1.0]
INFO: 动作归一化: mean=[0.0, 0.0], std=[1.0, 1.0]
INFO: ACT 模型加载完成，使用设备: mps

若模型未加载时触发推理，系统会返回零动作 [0.0, 0.0] 并打印警告日志，而非抛出异常，保证系统鲁棒性。

ACT 算法精讲

本课程将 ACT（Action Chunking Transformer）的所有实现代码逐行拆解讲解，以算法课的方式带你深入理解每一行代码背后的数学原理和设计决策。

课程目录

章节	标题	核心内容
第 0 讲	课程概述与数学基础	ACT 核心思想、代码地图、符号约定
第 1 讲	配置系统	ACTConfig 25 个超参数详解、参数校验、默认配置
第 2 讲	数据集与归一化	ACTDataset 滑窗采样、QUANTILES 百分位数归一化
第 3 讲	模型实现（上）	Temporal Ensembling、2D 正弦位置编码、ResNet18 视觉编码器、状态编码器
第 4 讲	模型实现（下）	Transformer Encoder/Decoder（Pre-Norm）、CVAE 重参数化、ACTModel 完整前向传播
第 5 讲	训练流程	Parquet 数据加载、训练循环、CVAE latent 统计收集、检查点保存
第 6 讲	推理运行时系统	检查点加载、预处理管线、Temporal Ensembling 策略、Singletone 运行时

如何使用本课程

代码对照阅读

每讲的标题下方标注了对应的源文件。建议打开源文件和本课程并排阅读——先看课程讲解，再看代码实现。

第 0 讲：课程概述与数学基础

本课程的目标

本课程将逐行讲解 AKA-Sim2Real 项目中的 ACT（Action Chunking Transformer）实现代码。学完本课程后，你将能够：

理解 ACT 论文中每个公式在代码中的具体实现位置
掌握 Transformer 在机器人控制中的端到端应用方式
能够独立修改、调试、优化 ACT 模型

前置知识

熟悉 Python 和 PyTorch 基本操作
了解 Transformer 的 Encoder-Decoder 结构（Self-Attention, Cross-Attention）
了解变分自编码器（VAE）的重参数化技巧
了解模仿学习（Imitation Learning / Behavior Cloning）的基本概念

ACT 论文核心思想回顾

ACT（Action Chunking Transformer）来自论文 Learning Fine-Grained Bimanual Manipulation with Low-Cost Hardware (Zhao et al., 2023)。其核心创新点：

1. 动作分块（Action Chunking）

传统行为克隆每一步预测一个动作 $a_t$，这会导致复合误差（compounding error）——每一步的小误差会随时间累积，最终偏离训练分布。

ACT 的解决方案：一次预测未来 $k$ 步的动作序列（称为一个 "chunk"）：

$$ \hat{a}_{t:t+k} = \pi\sb{\theta}(o_t) $$

推理时，每次只执行 chunk 的第一个动作，然后重新预测。这相当于每次预测都"向前看"了 $k$ 步，减少了短视（myopic）行为。

2. CVAE（条件变分自编码器）

模仿学习中，同一个观测下可能有多种合理的动作（多模态性）。标准的回归模型会学习这些动作的"平均值"，导致模型输出模糊不清。

CVAE 引入一个潜变量 $z$ 来捕获这种多模态性：

训练时：$z$ 从编码器 $q(z | o, a)$ 中采样（利用了"未来动作"的信息）
推理时：$z$ 从先验分布 $p(z) = \mathcal{N}(0, I)$ 中采样

3. 时间集成（Temporal Ensembling）

由于每个时间步都会预测一个完整的 chunk，相邻时间步的预测会有重叠。Temporal Ensembling 利用指数加权平均来融合这些重叠的预测，进一步平滑输出：

$$ a_t^{(i)} = \sum_{j=0}^{i} w_j \cdot \hat{a}_t^{(i,j)} $$

其中 $w_j = \exp(-m \cdot j)$，$m$ 是一个小的衰减系数。

代码文件地图

本课程按以下顺序讲解代码文件：

章节	文件	核心内容
第 1 讲	`configuration_act.py` + `defaults.py`	模型超参数配置
第 2 讲	`ACTDataset.py`	数据加载与 QUANTILES 归一化
第 3 讲	`modeling_act.py` (上)	位置编码、视觉编码器、状态编码器
第 4 讲	`modeling_act.py` (下)	Transformer Encoder/Decoder、CVAE、ACTModel
第 5 讲	`train_act.py`	训练循环与检查点管理
第 6 讲	`runtime.py` + `checkpoint.py` + `preprocess.py` + `execution.py`	推理运行时系统

符号	含义	代码中对应
$B$	batch 大小	`batch_size`
$C$	图像通道数	`in_channels`（默认 3）
$H, W$	图像高宽	`image_size`（默认 224×224）
$d_s$	状态维度	`state_dim`（默认 2: 左右轮速度）
$d_a$	动作维度	`action_dim`（默认 2: 左右轮速度指令）
$k$	action chunk 大小	`action_chunk_size`（默认 8）
$d_h$	Transformer 隐藏维度	`hidden_dim`（默认 512）
$d_z$	CVAE 潜变量维度	`latent_dim`（默认 32）

第 1 讲：配置系统

对应源文件：policies/models/act/configuration_act.py + policies/models/act/defaults.py

学习目标

理解 ACT 模型所有超参数的含义和数学对应关系
掌握参数校验逻辑的设计原因
理解默认配置的"为什么"

1.1 ACTConfig — 模型配置数据类

configuration_act.py 定义了 ACTConfig 类，它是整个模型唯一的配置入口。

1.1.1 类型导入

from typing import Tuple

解释：导入 Tuple 用于 image_size 参数的类型注解 Tuple[int, int]，明确表示图像尺寸是 "(高度, 宽度)"的二元组。

1.1.2 类声明与文档

class ACTConfig:
    """
    ACT 模型配置 - 对齐 LeRobot
    """

解释：@dataclass 是 Python 的标准数据类装饰器，它会自动为类生成 __init__、__repr__、__eq__ 等方法。使用它而不是 dataclasses.dataclass 装饰器的原因是：我们需要在 __init__ 中添加自定义的参数校验逻辑（调用 _validate()），如果使用 @dataclass 装饰器，自定义 __init__ 会比较复杂。

1.1.3 `init` 参数：观察空间

def __init__(
    self,
    # 观察空间
    image_size: Tuple[int, int] = (224, 224),
    in_channels: int = 3,
    state_dim: int = 7,  # 关节状态维度

逐行解释：

image_size = (224, 224)：输入图像的像素尺寸。ResNet18 的标准输入是 224×224（ImageNet 预训练权重对应此尺寸）。这里"(高, 宽)"的约定与 PyTorch 的 [C, H, W] 格式一致。
in_channels = 3：输入图像的通道数。RGB 图像 = 3 个通道。ResNet18 的第一层卷积期望 3 通道。
state_dim = 7：机器人关节状态的维度。在 AKA-Sim2Real 中实际只用 2 维（左右轮速度），但默认值 7 对齐了 LeRobot（ALOHA 机械臂有 7 个自由度）。

1.1.4 `init` 参数：动作空间

    # 动作空间
    action_dim: int = 7,  # 动作维度

解释：action_dim = 7 是动作向量的维度。ALOHA 机械臂中每个手臂控制 7 个关节。在 AKA-Sim2Real 中实际只用 2 维（左右轮速度指令），通过 defaults.py 的覆盖实现。

1.1.5 `init` 参数：模型架构

    # 模型参数
    hidden_dim: int = 512,
    num_attention_heads: int = 8,
    num_encoder_layers: int = 6,
    num_decoder_layers: int = 6,
    dropout: float = 0.1,
    dim_feedforward: int = 3200,  # 根据 LeRobot: 3200

逐行解释（也是 Transformer 标准参数）：

hidden_dim = 512：Transformer 中所有 token 的向量维度。注意这个数字必须能被 num_attention_heads 整除，因为多头注意力需要将 hidden_dim 均分给每个头：$d_{head} = d_h / n_{heads} = 512 / 8 = 64$。
num_attention_heads = 8：多头自注意力的头数。每个头独立计算 Attention，然后拼接。8 个头是 Transformer 原论文的默认值。
num_encoder_layers = 6：Encoder 堆叠层数。每层包含一层 Self-Attention + 一层 FFN。6 层是 Transformer 原论文的设置。
num_decoder_layers = 6：Decoder 堆叠层数。每层包含一层 Masked Self-Attention + 一层 Cross-Attention + 一层 FFN。
dropout = 0.1：Dropout 丢弃概率。在每个子层（Attention 和 FFN）之后应用，用于正则化。
dim_feedforward = 3200：FFN 中间层的维度。注意这里不是 Transformer 原论文的 2048，而是 LeRobot 特有的 3200。FFN 的结构是：Linear(512→3200) → GELU → Dropout → Linear(3200→512)。

1.1.6 `init` 参数：动作分块

    # 动作分块
    action_chunk_size: int = 16,
    n_action_steps: int = 16,  # 每次实际执行多少步（≤ chunk_size）

逐行解释：

action_chunk_size = 16：模型一次预测多少步动作。这是 ACT 论文的核心参数。ALOHA 原论文使用 100（50Hz 控制），但这个值应该根据具体任务的频率和延迟需求调整。
n_action_steps = 16：每次推理后实际执行的步数。必须 ≤ action_chunk_size。例如：
- n_action_steps = 1（AKA-Sim2Real 默认）：每次只执行 chunk 的第一步，下一帧重新预测。这是最保守的策略，也是 Temporal Ensembling 模式下的唯一合法值。
- n_action_steps > 1：每次执行 chunk 的前 N 步，然后等待 N 步后再推理。这适用于推理延迟较高的场景。

1.1.7 `init` 参数：相机与 CVAE

    # 相机数量
    num_cameras: int = 1,
    # CVAE 参数
    latent_dim: int = 32,  # 隐变量 z 的维度
    use_cvae: bool = True,  # 是否使用 CVAE
    kl_weight: float = 0.1,  # KL 散度损失权重

逐行解释：

num_cameras = 1：相机数量（AKA-Sim2Real 只有前视视角）。多相机场景下，每个相机的图像独立通过 ResNet 编码后拼接。支持多相机是 LeRobot 的设计，因为 ALOHA 有 4 个相机。
latent_dim = 32：潜变量 $z$ 的维度。这个维度的选择是一个权衡：
- 太小（如 8）：无法捕获足够的多模态信息
- 太大（如 128）：KL 散度难以优化，且推理时采样的随机性更大
- 32 是 CVAE 文献中常用的中间值
use_cvae = True：是否启用 CVAE。设为 False 则退化为纯确定性模型（类似原始 Transformer 行为克隆）。
kl_weight = 0.1：KL 散度在总损失中的权重。这是一个关键的超参数：
- 值太小：潜变量退化为无信息先验，CVAE 失去多模态能力
- 值太大：模型过于随机，重建精度下降
- 0.1 是 LeRobot 调优后的值

1.1.8 `init` 参数：Temporal Ensembling

    # Temporal Ensembling 参数
    use_temporal_ensembling: bool = True,  # 是否使用时间集成
    temporal_ensembling_coeff: float = 0.01,  # 时间集成衰减系数

逐行解释：

use_temporal_ensembling = True：是否启用时间集成。
temporal_ensembling_coeff = 0.01：指数衰减系数 $m$。计算公式为 $w_i = e^{-m \cdot i}$。这是 LeRobot ACT 原版的值（建议不要改）：
- 当 $m = 0.01$，$w_0 = 1.0$，$w_7 \approx 0.93$——接近均匀加权，略微偏向旧动作
- 当 $m = 0$：所有动作完全均匀加权
- 当 $m < 0$（如 -0.01）：更重视新动作——这会让输出更快响应变化
为什么 LeRobot 选择 "略微偏向旧动作"（$m > 0$）？因为机器人动作通常需要平滑性，稍加偏向历史预测可以减少高频抖动。

1.1.9 `init` 参数：Spatial Softmax

    # Spatial Softmax 参数
    use_spatial_softmax: bool = True,  # 是否使用 Spatial Softmax
    spatial_softmax_temperature: float = 1.0,  # Spatial Softmax 温度参数

逐行解释：

use_spatial_softmax = True：是否对视觉特征使用 Spatial Softmax（而非简单的全局平均池化）。Spatial Softmax 从特征图的每个空间位置计算 2D 关键点坐标，保留空间信息。
spatial_softmax_temperature = 1.0：Softmax 的温度参数。温度越高 → 关键点分布越"软"（扩散），温度越低 → 关键点分布越"硬"（集中）。1.0 表示标准 Softmax。

注意：在当前的 modeling_act.py 实现中，Spatial Softmax 相关代码被注释掉了（_compute_spatial_softmax 方法未在 forward 中调用），改用直接 Flatten + 2D 位置编码的方案。这是因为 ResNet18 输出的特征图本身已经很丰富，Spatial Softmax 带来的提升有限，但增加了计算量。

1.1.10 `init` 方法体

    self.image_size = image_size
    self.in_channels = in_channels
    self.state_dim = state_dim
    # ... (所有参数都赋值给 self)
    self._validate()

逐行解释：

每个 self.xxx = xxx：将构造函数参数保存为实例属性。这是标准的配置模式——参数通过 __init__ 传入，通过 self.xxx 访问。
self._validate()：构造配置后立即执行参数校验。这体现了 Fail Fast 原则——配置错误应该在模型构造时就抛出异常，而不是在训练到一半时才崩溃。

1.1.11 `_validate()` — 参数校验

def _validate(self):
    """参数校验"""
    if self.use_temporal_ensembling and self.n_action_steps > 1:
        raise ValueError(
            f"Temporal Ensembling 模式下 n_action_steps 必须为 1，"
            f"当前 n_action_steps={self.n_action_steps}"
        )

逐行解释：

第一个校验：Temporal Ensembling 需要 n_action_steps = 1。为什么？
- Temporal Ensembling 的工作原理是：每次推理产生一个 chunk，执行第一步，剩下的 7 步用于和下一次推理的预测做加权融合。
- 如果你一次性执行多步（n_action_steps > 1），这些步之间就没有"重叠"可以融合——Temporal Ensembling 失去了意义。
- 这个校验防止用户同时启用两个冲突的功能。

    if self.n_action_steps > self.action_chunk_size:
        raise ValueError(
            f"n_action_steps ({self.n_action_steps}) 不能大于 "
            f"action_chunk_size ({self.action_chunk_size})"
        )

解释：

第二个校验：逻辑约束——你不可能执行比预测更多的步数。n_action_steps 是实际执行的步数，它必须是 chunk 的一个子集。

1.1.12 `temporal_ensemble_coeff` — 兼容属性

@property
def temporal_ensemble_coeff(self) -> float:
    """兼容性别名"""
    return self._temporal_ensembling_coeff if hasattr(self, '_temporal_ensembling_coeff') \
        else self.__dict__.get('temporal_ensembling_coeff', 0.01)

逐行解释：

@property：这是一个属性（property），访问时不需要括号——config.temporal_ensemble_coeff 而非 config.temporal_ensemble_coeff()。
这是一个向后兼容的设计：
1. hasattr(self, '_temporal_ensembling_coeff')：检查是否存在旧命名风格（下划线前缀）的属性。
2. self.__dict__.get('temporal_ensembling_coeff', 0.01)：回退到检查新命名风格。
3. 如果都不存在，返回默认值 0.01。
为什么需要这个？因为不同版本的代码可能用不同的命名约定。这个属性确保无论模型文件是用旧名字还是新名字保存的，都能正确读取系数。

1.2 defaults.py — 默认配置与工厂函数

defaults.py 定义了 AKA-Sim2Real 的实际默认配置（覆盖 ACTConfig.__init__ 的通用默认值），并提供配置构建工具。

1.2.1 导入

from __future__ import annotations
from typing import Any
from .configuration_act import ACTConfig

逐行解释：

from __future__ import annotations：启用 PEP 563 的延迟注解求值。这使得类型注解中的类名可以在定义之前引用——在 TYPE_CHECKING 场景下特别有用（虽然本文件没用到，但这是项目的统一风格）。
from typing import Any：Any 类型表示"任意类型"。用于 dict[str, Any]，因为配置字典的值可能是 int、float、bool、Tuple 等。
from .configuration_act import ACTConfig：相对导入。. 表示当前包目录（policies/models/act/）。

1.2.2 DEFAULT_ACT_CONFIG

DEFAULT_ACT_CONFIG: dict[str, Any] = {
    "state_dim": 2,            # [vel_left, vel_right]
    "action_dim": 3,           # [left_vel, right_vel, gripper_target]
    "action_chunk_size": 8,    # 预测8步未来动作
    "n_action_steps": 1,       # Temporal Ensembling 模式下必须为 1
    "hidden_dim": 512,
    "num_attention_heads": 8,
    "num_encoder_layers": 4,
    "num_decoder_layers": 4,
    "dim_feedforward": 3200,
    "use_cvae": True,
    "kl_weight": 0.1,
    "use_temporal_ensembling": True,
    "temporal_ensembling_coeff": 0.01,
    "use_spatial_softmax": True,
    "latent_dim": 32,
    "num_cameras": 1,
}

逐行解释（聚焦与 ACTConfig 默认值的差异）：

state_dim = 2：AKA-Sim2Real 的车辆状态是 2 维——[左轮速度, 右轮速度]。这与 ALOHA 的 7 维关节状态不同。
action_dim = 3：动作空间是 3 维——[左轮速度指令, 右轮速度指令, 夹爪目标]。注意在 AKA-Sim2Real 中实际使用的训练参数是 action_dim = 2（不含夹爪），这个 3 是预留的。
action_chunk_size = 8：预测 8 步动作。如果控制频率是 10Hz，这对应 0.8 秒的预测视野。
n_action_steps = 1：每次只执行第一步（与 Temporal Ensembling 兼容）。
num_encoder_layers = 4、num_decoder_layers = 4：相比 ACTConfig.__init__ 的默认值 6 层，AKA-Sim2Real 使用更浅的网络（4 层）。原因是：车辆控制任务比 ALOHA 双臂操作简单，更少的层数可以加快推理速度且不易过拟合。

设计模式说明：这里使用一个大字典而非在 ACTConfig.__init__ 中直接设置默认值，遵循了 关注点分离 原则：

ACTConfig.__init__：定义通用默认值（对齐 LeRobot/ALOHA 的场景）
defaults.py：定义本项目特有的默认值（面向车辆控制）

1.2.3 build_act_config() — 配置构建器

def build_act_config(**overrides: Any) -> ACTConfig:
    config_dict = DEFAULT_ACT_CONFIG.copy()
    config_dict.update(overrides)
    return ACTConfig(**config_dict)

逐行解释：

**overrides: Any：关键字可变参数。允许调用者覆盖默认配置中的任意项。例如 build_act_config(state_dim=3, hidden_dim=256)。
DEFAULT_ACT_CONFIG.copy()：必须 copy！如果你直接修改 DEFAULT_ACT_CONFIG，由于它是模块级变量（全局单例），后续的 build_act_config() 调用会受影响。.copy() 创建浅拷贝，对于这个全是基本类型的字典来说足够安全。
config_dict.update(overrides)：将调用者提供的覆盖值合并到默认配置中。如果 key 已存在则覆盖，否则添加。
ACTConfig(**config_dict)：使用字典展开的方式构造 ACTConfig 实例。Python 的 ** 操作符将字典的 key-value 对展开为关键字参数。

1.2.4 act_config_to_dict() — 配置序列化

def act_config_to_dict(config: ACTConfig) -> dict[str, Any]:
    return {
        "state_dim": config.state_dim,
        "action_dim": config.action_dim,
        # ... 所有字段
    }

逐行解释：

这个函数是 build_act_config 的反向操作：ACTConfig → dict。
为什么需要手动列出所有字段，而不是用 config.__dict__？因为：
1. 显式优于隐式：明确控制哪些字段需要序列化（避免把 _inference_latent_mu 等内部状态也序列化进去）。
2. 稳定性：如果 ACTConfig 新增了内部属性，不会意外泄漏到保存的配置中。
3. 排序保证：手动列出保证了 JSON 输出中字段的顺序（dict 在 Python 3.7+ 中保持插入顺序）。

1.3 关键设计决策

为什么 encoder/decoder 层数（4 层）比原论文（6 层）少？

任务复杂度：车辆控制（2 维动作）远低于 ALOHA 双臂操作（14 维动作）
数据量：AKA-Sim2Real 的数据集通常比 ALOHA 小，更浅的网络不容易过拟合
推理延迟：在浏览器端的模拟器中，需要实时推理（~30Hz），更少的层数意味着更低的延迟

为什么 `dim_feedforward = 3200` 而非 2048？

这是 LeRobot 调优后的值。FFN 维度越大，模型容量越大。3200 相比 2048 大约增加了 56% 的参数量。LeRobot 团队发现这个值在高精度操作任务中表现更好。

课后思考

如果控制频率从 10Hz 提升到 30Hz，action_chunk_size 应该如何调整？为什么？
什么情况下应该将 use_cvae 设为 False？
Temporal Ensembling 的 n_action_steps 校验为什么是必要的？如果强行设为 2 会发生什么？

第 2 讲：数据集与归一化

对应源文件：policies/models/act/ACTDataset.py

学习目标

理解 ACT 数据集的滑窗构造逻辑
掌握 QUANTILES 百分位数归一化的数学原理
理解两种数据格式（单步 vs chunked）的兼容设计

2.1 导入

from typing import Dict, Tuple, Optional
import torch

解释：Dict, Tuple, Optional 用于类型注解，提高代码可读性。torch 是 PyTorch 的核心库。

2.2 ACTDataset 类

2.2.1 类声明

class ACTDataset(torch.utils.data.Dataset):

解释：继承 torch.utils.data.Dataset——这是 PyTorch 数据加载的标准基类。继承它后，只需要实现 __len__ 和 __getitem__ 两个方法，就可以使用 PyTorch 的 DataLoader 进行批量加载、shuffle、多进程加载等。

2.2.2 `init` 参数

def __init__(
    self,
    data: Dict[str, torch.Tensor],
    action_chunk_size: int = 16,
    normalize_images: bool = True,
    image_mean: Tuple[float, float, float] = (0.485, 0.456, 0.406),
    image_std: Tuple[float, float, float] = (0.229, 0.224, 0.225),
    state_q01: Optional[torch.Tensor] = None,
    state_q99: Optional[torch.Tensor] = None,
    action_q01: Optional[torch.Tensor] = None,
    action_q99: Optional[torch.Tensor] = None,
):

逐行解释：

data: Dict[str, torch.Tensor]：原始数据字典。有两种可能的键名格式：
1. 扁平格式：'observation.image'、'observation.state'、'action'（来自 load_dataset()）
2. 嵌套格式：'observation' 是字典，包含 'image' 和 'state'（来自自定义加载器）
action_chunk_size = 16：动作分块大小。注意这里的默认值是 16（匹配 LeRobot/ALOHA），而 AKA-Sim2Real 默认配置中是 8。
normalize_images = True：是否对图像做 ImageNet 标准化。不开启时图像像素值保持 [0, 1] 范围。
image_mean = (0.485, 0.456, 0.406)：ImageNet 训练集的 RGB 三通道均值。这些值是 ImageNet 数据集上统计得到的，用于均值减法，使每个通道的均值为 0。
image_std = (0.229, 0.224, 0.225)：ImageNet 训练集的 RGB 三通道标准差。用于标准差除法，使每个通道的标准差为 1。

为什么要用 ImageNet 的统计量？因为 ResNet18 的预训练权重是在 ImageNet 上用这些归一化参数训练的。如果不用相同的归一化，ResNet18 提取的特征分布会偏离预训练时的分布，破坏迁移学习的效果。
state_q01/q99：状态的 1% 和 99% 百分位数（详见 2.3 节）。
action_q01/q99：动作的 1% 和 99% 百分位数。

2.2.3 图像归一化参数的张量化

    self.data = data
    self.action_chunk_size = action_chunk_size

    # 图像归一化参数
    self.normalize_images = normalize_images
    self.image_mean = torch.tensor(image_mean).view(1, 3, 1, 1)
    self.image_std = torch.tensor(image_std).view(1, 3, 1, 1)

逐行解释：

torch.tensor(image_mean)：将 Python tuple 转换为 PyTorch 张量。不指定 dtype 时默认为 float32。
.view(1, 3, 1, 1)：重塑为 [1, 3, 1, 1] 形状。这是为了和图像张量 [C, H, W] 做广播（broadcasting）运算：
- 图像张量形状：[3, H, W]
- image_mean 形状：[1, 3, 1, 1] —— 广播后等价于 [3, H, W]，每个通道的 H×W 像素共享同一个均值
为什么不用 .view(3, 1, 1)？因为图像可能是 4 维（[B, C, H, W]）或 3 维（[C, H, W]），在第一个维度加一个 1 可以兼容两种格式，PyTorch 的广播机制会自动处理。实际上，在 __getitem__ 中使用 self.image_mean.to(images.device) 时，广播机制确保了无论批量维度是否存在都能正确相减。

2.2.4 百分位数归一化参数的存储

    # QUANTILES 归一化参数
    self.state_q01 = state_q01
    self.state_q99 = state_q99
    self.action_q01 = action_q01
    self.action_q99 = action_q99

解释：直接保存传入的百分位数张量。如果传入 None（表示不使用归一化），在 __getitem__ 中会通过条件判断跳过归一化步骤。

2.2.5 动作数据格式的自适应

    action_tensor = data["action"]
    if action_tensor.ndim not in (2, 3):
        raise ValueError(
            f"Unsupported action tensor shape {tuple(action_tensor.shape)}; "
            "expected [T, action_dim] or [N, chunk_size, action_dim]."
        )

逐行解释：

.ndim：张量的维度数（number of dimensions）。
- ndim=2：形状 [T, action_dim]，单步动作格式（来自 LeRobot 数据集）
- ndim=3：形状 [N, chunk_size, action_dim]，已分块格式（来自自定义数据）
- 其他维度数：不支持，抛出 ValueError
tuple(action_tensor.shape)：将 torch.Size 转换为 Python tuple，方便在错误信息中打印。

2.2.6 已分块格式的处理

    self.actions_are_chunked = action_tensor.ndim == 3
    if self.actions_are_chunked:
        self.num_samples = action_tensor.shape[0]
        if action_tensor.shape[1] != action_chunk_size:
            raise ValueError(
                f"Configured action_chunk_size={action_chunk_size}, "
                f"but action data has chunk size {action_tensor.shape[1]}."
            )

逐行解释：

self.actions_are_chunked：布尔标志位，在 __getitem__ 中用于选择不同的索引逻辑。
self.num_samples = action_tensor.shape[0]：已分块格式下，样本数 = 数据的第一维大小。
if action_tensor.shape[1] != action_chunk_size：校验 chunk 大小一致性。如果你用训练脚本 train_act.py --action_chunk_size 8 但数据集是 chunk_size=16 的格式，这里会报错。

2.2.7 单步格式的处理（滑窗采样）

    else:
        self.num_samples = action_tensor.shape[0] - action_chunk_size + 1
        if self.num_samples <= 0:
            raise ValueError(
                f"Not enough timesteps ({action_tensor.shape[0]}) for "
                f"action_chunk_size={action_chunk_size}."
            )

逐行解释：

action_tensor.shape[0] - action_chunk_size + 1：滑窗采样的关键公式。假设有 $T$ 个时间步的动作数据，chunk_size = 8：
- 第 0 个样本：时间步 [0, 7]
- 第 1 个样本：时间步 [1, 8]
- ...
- 最后一个样本：时间步 [T-8, T-1]
- 总共 T - 8 + 1 个样本
如果 T <= 8（数据量不足一个 chunk），样本数为 0 或负数，直接报错。

2.2.8 `len` — 返回数据集大小

def __len__(self) -> int:
    return self.num_samples

解释：PyTorch DataLoader 需要的标准接口。返回可采样的样本总数。

2.2.9 `getitem` — 取一个样本

这是数据集中最重要的方法。DataLoader 通过调用它来获取每个 batch 中的样本。

观测数据的获取

def __getitem__(self, idx: int) -> Dict[str, torch.Tensor]:
    # 获取当前时间步的观察（不是未来时刻！）
    current_idx = idx

    # 支持两种数据格式
    if "observation.image" in self.data:
        images = self.data["observation.image"][current_idx]
        state = self.data["observation.state"][current_idx]
    else:
        images = self.data["observation"]["image"][current_idx]
        state = self.data["observation"]["state"][current_idx]

逐行解释：

current_idx = idx：关键概念——ACT 根据"当前观测"预测"未来动作"。这意味着：
- 观测取的是时刻 t（idx）
- 动作取的是时刻 [t, t + chunk_size)（下面会讲）
有一类常见的错误实现是：用时刻 t + chunk_size 的观测来预测时刻 t 到 t + chunk_size 的动作——这导致模型"看到了未来"，是一个信息泄漏的 bug。本实现正确地对齐了观测和动作的时间。
if "observation.image" in self.data：扁平格式 vs 嵌套格式的兼容分支。使用 in 操作符而不是 try/except，因为这是已知的两种格式，不需要异常处理的开销。

动作数据的获取

    if self.actions_are_chunked:
        action = self.data["action"][idx]
    else:
        # 动作是从当前时刻开始的未来 chunk_size 步
        action = self.data["action"][idx:idx + self.action_chunk_size]

逐行解释：

已分块格式：每个样本独立存储，idx 直接索引到对应的 chunk。
单步格式：使用切片 [idx : idx + chunk_size] 取连续的 chunk_size 步动作。这就是滑窗——相邻的样本有 chunk_size - 1 步的重叠。

图像归一化

    # 归一化图像
    if self.normalize_images:
        images = (images - self.image_mean.to(images.device)) / self.image_std.to(images.device)

逐行解释：

self.image_mean.to(images.device)：将归一化参数移动到图像所在的设备（CPU 或 CUDA）。to() 方法对于 CPU 上的张量是空操作（no-op），所以没有性能损失。
(images - mean) / std：标准归一化公式。对于每个通道 $(r, g, b)$： $$ r' = \frac{r - 0.485}{0.229}, \quad g' = \frac{g - 0.456}{0.224}, \quad b' = \frac{b - 0.406}{0.225} $$

广播机制：images 形状为 [H, W, 3] 或 [3, H, W]，mean/std 形状为 [1, 3, 1, 1]，PyTorch 自动将后者广播到前者的形状。

QUANTILES 状态归一化

    # 使用 QUANTILES 归一化状态: 2 * (x - q01) / (q99 - q01) - 1
    if self.state_q01 is not None and self.state_q99 is not None:
        q01 = self.state_q01.to(state.device)
        q99 = self.state_q99.to(state.device)
        denom = q99 - q01
        denom = torch.where(denom == 0, torch.tensor(1e-8, device=state.device), denom)
        state = 2 * (state - q01) / denom - 1

逐行解释（这是本讲最核心的数学部分）：

公式：$x' = 2 \cdot \frac{x - q_{01}}{q_{99} - q_{01}} - 1$
步骤分解：
1. $x - q_{01}$：减去 1% 百分位数，使得 1% 分位数处映射到 0
2. $\frac{x - q_{01}}{q_{99} - q_{01}}$：除以极差（99% 分位 - 1% 分位），使得 99% 分位数处映射到 1
3. $2 \cdot (\dots) - 1$：将 [0, 1] 区间线性映射到 [-1, 1]
为什么用百分位数而不是 min/max？
- Min/Max 对异常值极度敏感——一个离谱的数据点就能拉偏整个归一化
- 百分位数（如 1%/99%）剔除了两端的极端异常值，归一化更鲁棒
- 这是 LeRobot 的标准做法
torch.where(denom == 0, torch.tensor(1e-8, device=state.device), denom)：防止除零。
- 如果 denom == 0（q01 == q99，即数据没有变化），分母替换为 1e-8
- 使用 torch.where 而非 if 语句，因为 denom 是张量，需要支持向量化条件

QUANTILES 动作归一化

    # 使用 QUANTILES 归一化动作: 2 * (x - q01) / (q99 - q01) - 1
    if self.action_q01 is not None and self.action_q99 is not None:
        q01 = self.action_q01.to(action.device)
        q99 = self.action_q99.to(action.device)
        denom = q99 - q01
        denom = torch.where(denom == 0, torch.tensor(1e-8, device=action.device), denom)
        action = 2 * (action - q01) / denom - 1

解释：与状态归一化完全相同的逻辑。唯一需要注意的是：动作经过归一化后输出在 [-1, 1] 范围内，这意味着模型的预测输出也是 [-1, 1] 范围，推理时需要反归一化才能用于实际控制。

返回样本

    return {
        "observation": {
            "image": images,
            "state": state,
        },
        "action": action,
    }

解释：返回一个嵌套字典。这个结构使得训练代码中可以通过 batch["observation"]["image"] 优雅地访问数据。

2.3 QUANTILES 归一化详解

什么是 QUANTILES 归一化？

标准归一化使用全局最小值/最大值： $$ x' = \frac{x - x_{min}}{x_{max} - x_{min}} $$

QUANTILES 归一化使用百分位数： $$ x' = 2 \cdot \frac{x - q_{01}}{q_{99} - q_{01}} - 1 $$

为什么是 [-1, 1]？

对称性：正值和负值对称，梯度更新更均衡
与 tanh 激活兼容：[-1, 1] 是 tanh 的自然输出范围
神经网络友好：输入分布在 0 附近时训练更稳定

百分位数的计算

在数据采集端（backend/services/episode/），统计量是这样计算的：

# 伪代码
q01 = numpy.percentile(all_states, 1, axis=0)
q99 = numpy.percentile(all_states, 99, axis=0)

注意 99% 的分位值 < 实际的最大值——这意味着有少量数据点在归一化后会超出 [-1, 1] 范围。这是有意为之——允许模型"见过"超出常规范围的值，但又不让极端值主导归一化。

课后思考

如果 action_chunk_size=8 且数据有 100 个时间步，num_samples 是多少？如果 action_chunk_size=16 呢？
为什么需要在归一化时做"防止除零"检查？在什么场景下 q01 == q99 会发生？
为什么不对图像使用 QUANTILES 归一化，而是用 ImageNet 的均值和标准差？

第 3 讲：模型实现（上）— 位置编码与编码器

对应源文件：policies/models/act/modeling_act.py 第 1–187 行

学习目标

理解 2D sinusoidal 位置编码的数学推导
掌握 ResNet18 作为视觉 backbone 的改造方式
理解为什么视觉特征需要位置编码

3.0 导入与日志

import math
import logging
import torch
import torch.nn as nn
import torch.nn.functional as F
from typing import Optional, Dict, Tuple, List
from .configuration_act import ACTConfig

logger = logging.getLogger(__name__)

逐行解释：

math：用于位置编码中的三角函数和指数计算（sin, cos, pow）
logging：Python 标准日志库。logging.getLogger(__name__) 创建一个以当前模块名为名的 logger，便于日志分级和过滤
torch.nn as nn：PyTorch 的神经网络模块。所有自定义层都继承 nn.Module
torch.nn.functional as F：PyTorch 的函数式接口。提供 F.gelu, F.l1_loss 等无状态的激活/损失函数
Optional, Dict, Tuple, List：类型注解
from .configuration_act import ACTConfig：相对导入配置类

3.1 ACTTemporalEnsembler — 时间集成器

这是 ACT 论文 Algorithm 2 的精确实现。

3.1.1 类文档

class ACTTemporalEnsembler:
    """Temporal Ensembling - LeRobot ACT 官方实现

    根据 Algorithm 2 of https://huggingface.co/papers/2304.13705

    权重计算: w_i = exp(-temporal_ensemble_coeff * i)，其中 w0 是最旧的动作
    权重归一化: 除以 Σw_i

    系数工作原理:
    - 设为 0: 所有动作均匀加权
    - 设为正数: 更重视旧动作
    - 设为负数: 更重视新动作

    默认值 0.01 (LeRobot ACT 原版) 会更重视旧动作。
    """

逐行解释：

关键公式：w_i = exp(-c * i)

其中 $c$ = temporal_ensemble_coeff，$i$ 表示该预测是第几个时间步之前产生的（$i=0$ 是最早的，$i=k-1$ 是最新的）。

当 $c > 0$（如 $c=0.01$）：$e^{-0.01 \cdot 0} = 1.0$（旧动作权重大），$e^{-0.01 \cdot 7} \approx 0.93$（新动作权重略小）。更重视旧动作 → 输出更平滑稳定
当 $c = 0$：所有权重为 1 → 均匀加权
当 $c < 0$（如 $c=-0.01$）：$e^{-(-0.01) \cdot 0} = 1.0$（旧动作权重小），$e^{-(-0.01) \cdot 7} \approx 1.07$（新动作权重大）。更重视新动作 → 响应更快但抖动更大

3.1.2 `init` — 初始化权重

def __init__(self, temporal_ensemble_coeff: float, chunk_size: int) -> None:
    self.chunk_size = chunk_size
    self.ensemble_weights = torch.exp(-temporal_ensemble_coeff * torch.arange(chunk_size))
    self.ensemble_weights_cumsum = torch.cumsum(self.ensemble_weights, dim=0)
    self.reset()

逐行解释：

torch.arange(chunk_size)：创建 [0, 1, 2, ..., chunk_size-1] 的索引序列
-temporal_ensemble_coeff * torch.arange(chunk_size)：计算每个位置上的指数参数。假设 coeff=0.01, chunk_size=8： $$[-0.00, -0.01, -0.02, -0.03, -0.04, -0.05, -0.06, -0.07]$$
torch.exp(...)：逐元素指数运算，得到权重向量： $$w = [1.000, 0.990, 0.980, 0.970, 0.961, 0.951, 0.942, 0.932]$$
torch.cumsum(self.ensemble_weights, dim=0)：计算累积和，用于在线更新时的归一化。第 $i$ 个元素 = $\sum_{j=0}^{i} w_j$。这是在线运行指数加权平均时的归一化分母。
self.reset()：初始化在线计算状态（详见 3.1.3）。

3.1.3 `reset()` — 重置状态

def reset(self):
    """重置在线计算变量"""
    self.ensembled_actions = None
    self.ensembled_actions_count = None

逐行解释：

self.ensembled_actions = None：累积的动作张量。形状为 [B, remaining_chunk, action_dim]。设为 None 表示"尚未开始累积"。
self.ensembled_actions_count = None：每个位置已经被累积了多少次。形状为 [remaining_chunk, 1]。当 count 达到 chunk_size 时，该位置不再更新（因为它已经累积了一轮完整的 chunk）。

3.1.4 `update()` — 在线更新（核心算法）

def update(self, actions: torch.Tensor) -> torch.Tensor:
    """
    输入: (batch, chunk_size, action_dim) 的动作序列
    输出: (batch, action_dim) - 序列中的下一个动作

    更新所有时间步的 temporal ensemble，并返回下一个动作。
    """
    self.ensemble_weights = self.ensemble_weights.to(device=actions.device)
    self.ensemble_weights_cumsum = self.ensemble_weights_cumsum.to(device=actions.device)

逐行解释：

to(device=actions.device)：将预计算的权重移动到与输入相同的设备（CPU/GPU）。这是必要的，因为模型可能在不同的设备上运行，权重需要跟随。

第一次调用：初始化

    if self.ensembled_actions is None:
        # 第一次调用：用第一个时间步的动作序列初始化
        self.ensembled_actions = actions.clone()
        self.ensembled_actions_count = torch.ones(
            (self.chunk_size, 1), dtype=torch.long, device=self.ensembled_actions.device
        )

逐行解释：

actions.clone()：深拷贝动作张量。clone() 创建新的内存，断开与原始计算图的连接。在推理时（torch.no_grad() 下），clone() 和普通的赋值没有梯度问题，但保持一致性。
torch.ones((self.chunk_size, 1), dtype=torch.long)：创建一个 [chunk_size, 1] 的全 1 计数张量。每个位置都被认为是"累积了 1 次"。

后续调用：在线更新

    else:
        # 在线更新: 对 (batch_size, chunk_size - 1, action_dim) 部分进行更新
        self.ensembled_actions *= self.ensemble_weights_cumsum[self.ensembled_actions_count - 1]
        self.ensembled_actions += actions[:, :-1] * self.ensemble_weights[self.ensembled_actions_count]
        self.ensembled_actions /= self.ensemble_weights_cumsum[self.ensembled_actions_count]
        self.ensembled_actions_count = torch.clamp(self.ensembled_actions_count + 1, max=self.chunk_size)

逐行解释（这是整个 temporal ensembling 最核心的数学）：

这一段实现了在线指数加权移动平均（EWMA）。设我们有：

$A_{n-1}$：之前累积的动作（ensembled_actions）
$A_n$：新预测的动作（actions[:, :-1]——去掉最后一个，因为它没有历史对应）
$c_{n-1}$：之前的累积次数（ensembled_actions_count）
$w_i = e^{-m \cdot i}$：权重

在线更新公式为： $$ A_n = \frac{A_{n-1} \cdot C_{c-1} + a_n \cdot w_{c}}{C_c} $$

其中 $C_c = \sum_{j=0}^{c} w_j$（累积权重和）。

第一步：

self.ensembled_actions *= self.ensemble_weights_cumsum[self.ensembled_actions_count - 1]

将旧累积值乘以旧累积权重和——相当于恢复未归一化的加权和。

第二步：

self.ensembled_actions += actions[:, :-1] * self.ensemble_weights[self.ensembled_actions_count]

加上新预测乘以新权重——现在 ensembled_actions 是未归一化的加权和。

第三步：

self.ensembled_actions /= self.ensemble_weights_cumsum[self.ensembled_actions_count]

除以新的累积权重和——完成归一化。

第四步：

self.ensembled_actions_count = torch.clamp(self.ensembled_actions_count + 1, max=self.chunk_size)

计数 + 1，但不超过 chunk_size（因为累积 chunk_size 次后，第一个位置的信息已经被完全替代，不再需要增加计数）。

拼接最后一个动作

        # 最后一个动作（没有先前的在线平均）需要拼接到末尾
        self.ensembled_actions = torch.cat([self.ensembled_actions, actions[:, -1:]], dim=1)
        self.ensembled_actions_count = torch.cat(
            [self.ensembled_actions_count, torch.ones_like(self.ensembled_actions_count[-1:])]
        )

逐行解释：

actions[:, -1:]：取最后一个动作（索引 -1），保留维度（: 切片不降维）。形状 [B, 1, action_dim]。
为什么最后一个动作特殊对待？因为在线更新只更新了前 chunk_size-1 个位置（它们有历史对应），最新产生的第 chunk_size 个位置是全新的、没有历史的。所以它直接拼接到累积序列末尾，计数从 1 开始。
torch.ones_like(self.ensembled_actions_count[-1:])：为新加入的位置创建计数 = 1。

消费第一个动作

    # "消费"第一个动作
    action, self.ensembled_actions, self.ensembled_actions_count = (
        self.ensembled_actions[:, 0],
        self.ensembled_actions[:, 1:],
        self.ensembled_actions_count[1:],
    )
    return action

逐行解释：

self.ensembled_actions[:, 0]：取出第一个（最"成熟"、累积最久的）动作，形状 [B, action_dim]。这就是本次推理实际输出的动作。
self.ensembled_actions[:, 1:]：剩下的动作（索引 1 到末尾）保留在状态中，等下次推理时继续累积。
self.ensembled_actions_count[1:]：对应的计数也同步移出第一个。
这个"消费"模式是 temporal ensembling 的精髓——每次推理产出一个完整的 chunk，但只输出第一个位置（已经过多次累积），其余位置保留用于未来融合。

3.2 位置编码（Positional Encoding）

3.2.1 `create_sinusoidal_pos_embedding()` — 1D 正弦位置编码

def create_sinusoidal_pos_embedding(num_positions: int, dimension: int) -> torch.Tensor:
    """1D sinusoidal positional embeddings"""
    def get_position_angle_vec(position):
        return [position / math.pow(10000, 2 * (hid_j // 2) / dimension) for hid_j in range(dimension)]

    sinusoid_table = torch.tensor([
        get_position_angle_vec(pos_i) for pos_i in range(num_positions)
    ], dtype=torch.float32)
    sinusoid_table[:, 0::2] = sinusoid_table[:, 0::2].sin()
    sinusoid_table[:, 1::2] = sinusoid_table[:, 1::2].cos()
    return sinusoid_table

逐行解释（这是 Transformer 原论文 "Attention Is All You Need" 中位置编码的实现）：

get_position_angle_vec(position)：对给定的位置，计算 dimension 个角频率。公式： $$\text{PE}(pos, 2i) = \sin\left(\frac{pos}{10000^{2i/d}}\right)$$ $$\text{PE}(pos, 2i+1) = \cos\left(\frac{pos}{10000^{2i/d}}\right)$$

其中 $pos$ 是位置索引，$i$ 是维度对索引（hid_j // 2），$d$ 是总维度。
math.pow(10000, 2 * (hid_j // 2) / dimension)：计算频率的基数。注意 hid_j // 2（整除）使得偶数/奇数维度对共享同一个频率。
[:, 0::2]（偶数索引）应用 sin()，[:, 1::2]（奇数索引）应用 cos()。这是标准做法——sin/cos 交替使得位置编码可以通过线性变换表达相对位置关系。

3.2.2 `ACTSinusoidalPositionEmbedding2d` — 2D 正弦位置编码

用于图像特征图的空间位置编码——这是视觉 Transformer 的标准做法。

`init`

class ACTSinusoidalPositionEmbedding2d(nn.Module):
    """2D sinusoidal positional embeddings"""
    def __init__(self, dimension: int):
        super().__init__()
        self.dimension = dimension
        self._two_pi = 2 * math.pi
        self._eps = 1e-6
        self._temperature = 10000

逐行解释：

self._two_pi = 2 * math.pi：$2\pi$，用于将空间坐标映射到完整的周期范围
self._eps = 1e-6：一个小常数，防止除零
self._temperature = 10000：与 1D 位置编码相同的温度基数

`forward()` — 2D 位置编码的前向传播

def forward(self, x: torch.Tensor) -> torch.Tensor:
    not_mask = torch.ones_like(x[0, :1])
    y_range = not_mask.cumsum(1, dtype=torch.float32)
    x_range = not_mask.cumsum(2, dtype=torch.float32)

逐行解释：

torch.ones_like(x[0, :1])：创建一个与输入第一个通道同形状的全 1 张量。x[0, :1] 取第一个 batch 的第一个通道，形状 [1, H', W']。
not_mask.cumsum(1, dtype=torch.float32)：沿高度维度（dim=1）计算累积和。由于输入是全 1，结果就是 [1, 2, 3, ..., H'] 的 y 坐标网格。dtype=torch.float32 确保后续的浮点运算精度。
not_mask.cumsum(2, dtype=torch.float32)：沿宽度维度（dim=2）计算累积和。结果就是 [1, 2, 3, ..., W'] 的 x 坐标网格。

思考：这里通过 cumsum 而非直接 torch.arange 来生成坐标，是一种巧妙的方式——它利用了张量运算，避免额外的 reshape 操作，并且代码更简洁。

    y_range = y_range / (y_range[:, -1:, :] + self._eps) * self._two_pi
    x_range = x_range / (x_range[:, :, -1:] + self._eps) * self._two_pi

逐行解释：

y_range[:, -1:, :]：取每列的最大 y 值（H'）。用 -1: 而非 -1 保留维度，确保广播正确。
y_range / (y_range[:, -1:, :] + self._eps)：将 y 坐标归一化到 $[0, 1]$ 范围。加上 _eps 防止除零。
* self._two_pi：映射到 $[0, 2\pi]$ 范围。这使得 sin/cos 能够在完整的周期上采样，产生丰富的频率成分。

    inverse_frequency = self._temperature ** (
        2 * (torch.arange(self.dimension, dtype=torch.float32, device=x.device) // 2) / self.dimension
    )

逐行解释：

torch.arange(self.dimension)：创建 [0, 1, ..., dimension-1] 的维度索引。
// 2（整除）：将维度索引分组为对——[0,0,1,1,2,2,...]，使得偶数/奇数对共享同一频率。
self._temperature ** (2 * i / dimension)：与 1D 位置编码相同的频率计算：$10000^{2i/d}$

    x_range = x_range.unsqueeze(-1) / inverse_frequency
    y_range = y_range.unsqueeze(-1) / inverse_frequency

解释：将空间坐标的每个位置除以对应的频率——这是"位置 × 频率"的标准傅里叶特征构造。.unsqueeze(-1) 添加一个维度用于广播，结果形状为 [1, H', W', dimension]。

    pos_embed_x = torch.stack((x_range[..., 0::2].sin(), x_range[..., 1::2].cos()), dim=-1).flatten(3)
    pos_embed_y = torch.stack((y_range[..., 0::2].sin(), y_range[..., 1::2].cos()), dim=-1).flatten(3)
    pos_embed = torch.cat((pos_embed_y, pos_embed_x), dim=3).permute(0, 3, 1, 2)

逐行解释：

x_range[..., 0::2].sin()：偶数索引维度应用 sin
x_range[..., 1::2].cos()：奇数索引维度应用 cos
torch.stack(..., dim=-1)：将 sin 和 cos 结果堆叠在最后一维，使得每个频率对在同一个维度上
.flatten(3)：将最后一维（dim=3）的 sin/cos 对展平
torch.cat((pos_embed_y, pos_embed_x), dim=3)：拼接 y 和 x 方向的位置编码
.permute(0, 3, 1, 2)：将维度顺序从 [B, H', W', D] 转为标准的 [B, D, H', W'] 特征图格式

    return pos_embed

解释：返回形状 [B, dimension, H', W'] 的 2D 位置编码，可以直接与特征图相加。

3.3 RGBEncoder — 视觉编码器

3.3.1 `init` — 加载 ResNet18

class RGBEncoder(nn.Module):
    """视觉编码器 - LeRobot 风格"""
    def __init__(self, in_channels: int = 3, hidden_dim: int = 512):
        super().__init__()
        self.hidden_dim = hidden_dim
        self.feature_dim = 512

逐行解释：

self.feature_dim = 512：ResNet18 最后一个卷积层输出的通道数。ResNet 架构中，各阶段输出通道为 64→128→256→512（最后一个阶段）。这里保留这个中间特征维度。

        from torchvision.models import resnet18, ResNet18_Weights
        try:
            weights = ResNet18_Weights.DEFAULT
            resnet = resnet18(weights=weights)
        except Exception as exc:
            logger.warning("Failed to load pretrained ResNet18 weights, falling back to random init: %s", exc)
            resnet = resnet18(weights=None)

逐行解释：

from torchvision.models import resnet18, ResNet18_Weights：懒导入（lazy import）——只在实例化 RGBEncoder 时才导入 torchvision。这避免了在不需要视觉编码器的场景下（如纯测试）导入重量级的 torchvision。
ResNet18_Weights.DEFAULT：torchvision 0.14+ 的新权重 API。它会自动下载 ImageNet 预训练权重。与旧 API (pretrained=True) 相比，新 API 提供了更好的权重版本管理。
Try/Except 回退策略：如果下载权重失败（如网络问题），使用随机初始化（weights=None）。这保证了代码在任何环境下都能运行，但会显著影响训练效果。使用 logger 发出警告让用户知道发生了什么。

        # 保留卷积特征图，避免全局池化后丢失空间信息。
        self.backbone = nn.Sequential(*list(resnet.children())[:-2])

逐行解释（这是改造 ResNet 为特征提取器的关键步骤）：

list(resnet.children())：将 ResNet 的各个子模块转换为列表。ResNet18 的结构：

[0] Conv2d(3→64, 7×7, stride=2)
[1] BatchNorm2d(64)
[2] ReLU
[3] MaxPool2d(3×3, stride=2)
[4] BasicBlock(64→64)  ×2  -- layer1
[5] BasicBlock(64→128) ×2  -- layer2
[6] BasicBlock(128→256) ×2 -- layer3
[7] BasicBlock(256→512) ×2 -- layer4
[8] AdaptiveAvgPool2d    -- 去掉
[9] Linear(512→1000)     -- 去掉

[:-2]：移除最后两个子模块——AdaptiveAvgPool2d 和 Linear。这两个模块会将特征图压缩为 1D 分类向量，而我们需要的恰恰是空间特征图。
nn.Sequential(*...)：将修改后的子模块重新包装为 Sequential，便于前向传播。
最终 backbone 的输出形状：[B, 512, H/32, W/32]（224×224 输入 → 7×7 特征图）。

        self.encoder_img_feat_input_proj = nn.Conv2d(self.feature_dim, hidden_dim, kernel_size=1)
        self.encoder_cam_feat_pos_embed = ACTSinusoidalPositionEmbedding2d(hidden_dim // 2)

逐行解释：

nn.Conv2d(512, 512, kernel_size=1)：1×1 卷积，也称为"逐点卷积"（pointwise convolution）。它的作用是：
1. 将 ResNet 特征维度（512）投影到 Transformer 的隐藏维度（也是 512）
2. 即使用了相同的维度数，1×1 卷积仍然提供了可学习的跨通道混合
3. 不改变空间分辨率（kernel_size=1，无 padding 损失）
ACTSinusoidalPositionEmbedding2d(hidden_dim // 2)：2D 位置编码，输出维度 = 256（= 512/2）。为什么是 half？因为 pos_embed_y 和 pos_embed_x 各占一半，拼接后总维度 = 512。

3.3.2 `forward()` — 视觉编码

def forward(self, images: torch.Tensor) -> torch.Tensor:
    batch_size = images.shape[0]

解释：记录原始 batch size，用于后续的 view/reshape 操作中恢复批量维度。

    if images.ndim == 5:
        num_cameras = images.shape[1]
        images = images.view(-1, *images.shape[2:])
    else:
        num_cameras = 1

逐行解释：

images.ndim == 5：多相机输入，形状 [B, num_cameras, C, H, W]
images.ndim == 4：单相机输入，形状 [B, C, H, W]
images.view(-1, *images.shape[2:])：将 [B, num_cameras, C, H, W] 展开为 [B*num_cameras, C, H, W]。-1 表示自动推断该维度大小，*images.shape[2:] 保持 [C, H, W] 形状不变。

    features = self.backbone(images)  # [B*num_cameras, 512, H', W']

解释：通过改造后的 ResNet18 提取特征。输出形状 [B*num_cameras, 512, H', W']，其中 H' = H/32, W' = W/32。

    cam_pos_embed = self.encoder_cam_feat_pos_embed(features)
    features = self.encoder_img_feat_input_proj(features) + cam_pos_embed

逐行解释：

self.encoder_cam_feat_pos_embed(features)：计算 2D 正弦位置编码，形状 [B*num_cam, 256, H', W']。但投影卷积期望 512 通道——这里有个细节：在 ACTSinusoidalPositionEmbedding2d 中，y 和 x 方向各 256 维，拼接后是 512 维。但我重新检查代码，ACTSinusoidalPositionEmbedding2d(dimension) 的输出维度 = 2 * dimension（因为拼接了 y 和 x），所以 dimension = hidden_dim // 2 = 256，输出 = 512 维。正确！
self.encoder_img_feat_input_proj(features)：1×1 卷积投影，输出也是 512 通道。
+ cam_pos_embed：残差式的位置编码加法。这不同于 NLP 中的"加到 token embedding 上"，这里是直接加到特征图的每个空间位置上，让模型知道"这个特征在图像的哪个位置"。

    features = features.flatten(2).transpose(1, 2)  # [B*num, H*W, hidden_dim]

逐行解释：

flatten(2)：将 [B*num_cam, 512, H', W'] 展平为 [B*num_cam, 512, H'*W']
transpose(1, 2)：转置为 [B*num_cam, H'*W', 512]——这是 Transformer 期望的序列格式：[batch, seq_len, hidden_dim]

    features = features.view(batch_size, num_cameras, -1, self.hidden_dim)
    features = features.reshape(batch_size, -1, self.hidden_dim)

逐行解释：

view(batch_size, num_cameras, -1, self.hidden_dim)：恢复批量维度，中间形状为 [B, num_cam, H'*W', 512]
reshape(batch_size, -1, self.hidden_dim)：将多相机的 tokens 拼接为一个长序列 [B, num_cam * H'*W', 512]

    return features

解释：返回视觉特征 tokens，形状 [B, num_cam * H'*W', 512]。对于单相机 224×224 输入，H'=W'=7，所以视觉 tokens 数 = 49。

3.4 StateEncoder — 状态编码器

class StateEncoder(nn.Module):
    """状态编码器"""
    def __init__(self, state_dim: int, hidden_dim: int):
        super().__init__()
        self.encoder = nn.Sequential(
            nn.Linear(state_dim, hidden_dim),
            nn.GELU(),
            nn.Linear(hidden_dim, hidden_dim),
        )

逐行解释：

这是一个两层 MLP（多层感知机）。
Linear(state_dim, hidden_dim)：将低维状态向量投影到高维隐藏空间。例如 2 维 → 512 维。
GELU()：Gaussian Error Linear Unit 激活函数。与 ReLU 相比，GELU 在 0 附近是平滑的，提供更好的梯度流。这是 Transformer 标准使用的激活函数。
Linear(hidden_dim, hidden_dim)：保持维度的变换层，提供额外的非线性表达能力。

def forward(self, state: torch.Tensor) -> torch.Tensor:
    if state.ndim == 2:
        state = state.unsqueeze(1)
    return self.encoder(state)

逐行解释：

if state.ndim == 2：状态输入可能是 [B, state_dim]。.unsqueeze(1) 在 dim=1 位置插入一个维度，变为 [B, 1, state_dim]。
self.encoder(state)：线性层作用于最后一维（state_dim），所以 [B, 1, state_dim] → [B, 1, hidden_dim] 是正确的。
最终输出形状：[B, 1, hidden_dim]——单一的状态 token，将与其他 tokens（latent、视觉）拼接后送入 Transformer。

设计意图：为什么状态编码这么简单（2 层 MLP），而视觉编码那么复杂（ResNet18 + 位置编码）？因为状态的维度很低（2 维，左右轮速度），信息量小，不需要深层网络。而图像包含丰富的空间信息，需要预训练 backbone 来提取有意义的特征。

课后思考

为什么 ACTSinusoidalPositionEmbedding2d 中 y 和 x 各占一半的维度（hidden_dim // 2），而不是各占完整维度？
为什么去掉 ResNet 的最后两层（AvgPool + Linear）而不是只去掉最后一层？
Temporal Ensembling 中 torch.clamp(count + 1, max=chunk_size) 为什么需要 max=chunk_size？如果去掉会发生什么？

第 4 讲：模型实现（下）— Transformer 与 CVAE

对应源文件：policies/models/act/modeling_act.py 第 189–591 行

学习目标

逐行理解 Transformer Encoder/Decoder 的 Pre-Norm 实现
掌握 CVAE 的编码、重参数化、KL 散度计算的完整推导
理解 ACTModel.forward() 中 9 个步骤的数据流动

4.1 ACTEncoderLayer — 编码器层

4.1.1 `init` — 构建 Self-Attention + FFN

class ACTEncoderLayer(nn.Module):
    """Transformer Encoder Layer - 与 LeRobot 一致"""
    def __init__(self, config: ACTConfig):
        super().__init__()
        self.self_attn = nn.MultiheadAttention(
            config.hidden_dim, config.num_attention_heads, dropout=config.dropout, batch_first=True
        )

逐行解释：

nn.MultiheadAttention(embed_dim=512, num_heads=8, dropout=0.1, batch_first=True)：PyTorch 内置的多头注意力。
- embed_dim = config.hidden_dim = 512：输入/输出特征维度
- num_heads = config.num_attention_heads = 8：每个头的维度 = 512/8 = 64
- dropout = 0.1：在注意力权重矩阵上应用 dropout（正则化）
- batch_first = True：输入/输出格式为 [batch, seq_len, hidden_dim]（而非 PyTorch 旧默认的 [seq_len, batch, hidden_dim]）

        self.linear1 = nn.Linear(config.hidden_dim, config.dim_feedforward)    # 512 → 3200
        self.dropout = nn.Dropout(config.dropout)                               # p=0.1
        self.linear2 = nn.Linear(config.dim_feedforward, config.hidden_dim)    # 3200 → 512

逐行解释：

FFN（Feed-Forward Network）的结构：LinearUp → Activation → Dropout → LinearDown。这是 Transformer 标准的两层全连接子层。
linear1：将隐藏维度从 512 扩展到 3200（约 6.25 倍），提供更大的表示容量。
dropout：在激活函数之后、线性投影之前应用丢弃正则化。
linear2：将 FFN 中间维度 3200 压缩回 512，恢复为 Transformer 的残差连接兼容的维度。

        self.norm1 = nn.LayerNorm(config.hidden_dim)
        self.norm2 = nn.LayerNorm(config.hidden_dim)
        self.dropout1 = nn.Dropout(config.dropout)
        self.dropout2 = nn.Dropout(config.dropout)

逐行解释：

LayerNorm：层归一化，对每个样本的每个 token 独立归一化（对所有特征维度）。与 BatchNorm 在整个 batch 上归一化不同，LayerNorm 不依赖 batch 大小，在 batch 大小变化时更稳定。
norm1：Attention 子层之前的归一化
norm2：FFN 子层之前的归一化
dropout1：Attention 输出之后的 dropout
dropout2：FFN 输出之后的 dropout

        self.activation = F.gelu
        self.pre_norm = True  # LeRobot 使用 pre-norm

逐行解释：

F.gelu：GELU 激活函数。与 nn.GELU() 不同，F.gelu 是函数式的——不包含可学习参数，不需要存储在 state_dict 中。这是一个微小的优化：当模块只使用 F.xxx 而非 nn.XXX()，模型的参数量统计更精确。
self.pre_norm = True：标志位，指示使用的是 Pre-Norm（归一化在子层之前）而非 Post-Norm（归一化在子层之后）。
- Pre-Norm：$x + \text{SubLayer}(\text{LayerNorm}(x))$
- Post-Norm：$\text{LayerNorm}(x + \text{SubLayer}(x))$
Pre-Norm 在现代 Transformer 中更流行，因为它在训练初期梯度更稳定，不需要 warmup 学习率调度。

4.1.2 `forward()` — 编码器层的前向传播

def forward(self, x: torch.Tensor, pos_embed: Optional[torch.Tensor] = None,
            key_padding_mask: Optional[torch.Tensor] = None) -> torch.Tensor:
    skip = x
    x = self.norm1(x)
    q = k = x if pos_embed is None else x + pos_embed
    x, _ = self.self_attn(q, k, value=x, key_padding_mask=key_padding_mask)
    x = skip + self.dropout1(x)

逐行解释（Self-Attention 子层 + 残差连接）：

skip = x：保存输入作为残差连接的 skip connection。这是 ResNet 的核心创新——直接传递梯度回到更早的层。
x = self.norm1(x)：Pre-Norm。在 Attention 前先归一化。LayerNorm 的计算： $$\text{LayerNorm}(x) = \gamma \cdot \frac{x - \mu}{\sqrt{\sigma^2 + \epsilon}} + \beta$$ 其中 $\mu, \sigma^2$ 在最后一维（hidden_dim=512）上计算。
q = k = x if pos_embed is None else x + pos_embed：构建 Query 和 Key，如果提供了位置编码则加到 Query/Key 上。注意 Value 不使用位置编码（value=x，不带 pos_embed）。这是因为：
- Query 和 Key 用于计算注意力权重（"哪里需要注意"），位置信息帮助模型知道"token A 和 token B 的空间关系"
- Value 是实际被聚合的语义信息，不需要位置编码叠加
x, _ = self.self_attn(q, k, value=x, ...)：多头自注意力。返回两个值：注意力输出和注意力权重矩阵（后者用 _ 丢弃）。Self-Attention 的公式： $$\text{Attention}(Q, K, V) = \text{softmax}\left(\frac{QK^T}{\sqrt{d_k}}\right) V$$ 其中 $d_k = 512/8 = 64$（每个头的维度）。
x = skip + self.dropout1(x)：残差连接 + Dropout 正则化。$x_{out} = x_{in} + \text{Dropout}(\text{Attention}(\text{Norm}(x_{in})))$

    skip = x
    x = self.norm2(x)
    x = self.linear2(self.dropout(self.activation(self.linear1(x))))
    x = skip + self.dropout2(x)
    return x

逐行解释（FFN 子层 + 残差连接）：

skip = x：再次保存残差
x = self.norm2(x)：Pre-Norm（第二次）
self.linear1(x)：Linear(512→3200)，扩展维度
self.activation(...)：GELU 激活，引入非线性。GELU 公式： $$\text{GELU}(x) = x \cdot \Phi(x) = x \cdot \frac{1}{2}\left[1 + \text{erf}\left(\frac{x}{\sqrt{2}}\right)\right]$$
self.dropout(...)：在激活后丢弃（防止过拟合）
self.linear2(...)：Linear(3200→512)，恢复到原始维度
x = skip + self.dropout2(x)：残差连接 + Dropout

4.2 ACTEncoder — 编码器堆栈

class ACTEncoder(nn.Module):
    """Transformer Encoder"""
    def __init__(self, config: ACTConfig, is_vae_encoder: bool = False):
        super().__init__()
        self.is_vae_encoder = is_vae_encoder
        num_layers = config.num_encoder_layers
        self.layers = nn.ModuleList([ACTEncoderLayer(config) for _ in range(num_layers)])
        self.norm = nn.LayerNorm(config.hidden_dim)

逐行解释：

is_vae_encoder: bool = False：标志位（尽管当前代码未使用，但保留了扩展性）。未来可能会用不同的层数或配置构建 VAE 专用的 encoder。
nn.ModuleList([ACTEncoderLayer(config) for _ in range(num_layers)])：创建 num_layers 个相同的 encoder 层。注意是 ModuleList 而非 Python list——ModuleList 会正确注册子模块，使它们的参数出现在 model.parameters() 中。
self.norm = nn.LayerNorm(config.hidden_dim)：编码器输出后的最终 LayerNorm。在 Pre-Norm 架构中，最后一层只有残差没有后归一化，需要显式添加这个最终 norm。

def forward(self, x, pos_embed=None, key_padding_mask=None):
    for layer in self.layers:
        x = layer(x, pos_embed=pos_embed, key_padding_mask=key_padding_mask)
    x = self.norm(x)
    return x

逐行解释：

for layer in self.layers：顺序通过每一层。Transformer 的 encoder 是自回归无关的（所有 token 同时处理），所以不需要像 RNN 那样的循环状态传递。
x = self.norm(x)：最后的归一化。在 Pre-Norm 架构中，子层内部做了 norm，但最终输出可能没有 norm。这里显式添加确保输出分布稳定。

4.3 ACTDecoderLayer — 解码器层

4.3.1 `init` — 构建 Self-Attention + Cross-Attention + FFN

class ACTDecoderLayer(nn.Module):
    def __init__(self, config: ACTConfig):
        super().__init__()
        self.self_attn = nn.MultiheadAttention(
            config.hidden_dim, config.num_attention_heads, dropout=config.dropout, batch_first=True
        )
        self.multihead_attn = nn.MultiheadAttention(
            config.hidden_dim, config.num_attention_heads, dropout=config.dropout, batch_first=True
        )

逐行解释：

self_attn：解码器的自注意力（Self-Attention）。用于处理 decoder 内部的 action query tokens。注意这里没有使用 causal mask（attn_mask），因为 action queries 之间是全连接的——所有 8 个 action query 可以同时看到彼此。
multihead_attn：交叉注意力（Cross-Attention）。将 decoder 的 action queries 与 encoder 的视觉/状态特征进行匹配。Query 来自 decoder，Key/Value 来自 encoder。

这是 Transformer 的核心机制——Encoder 产生"理解"，Decoder 根据这个理解"生成"。

        self.linear1 = nn.Linear(config.hidden_dim, config.dim_feedforward)
        self.dropout = nn.Dropout(config.dropout)
        self.linear2 = nn.Linear(config.dim_feedforward, config.hidden_dim)

        self.norm1 = nn.LayerNorm(config.hidden_dim)
        self.norm2 = nn.LayerNorm(config.hidden_dim)
        self.norm3 = nn.LayerNorm(config.hidden_dim)
        self.dropout1 = nn.Dropout(config.dropout)
        self.dropout2 = nn.Dropout(config.dropout)
        self.dropout3 = nn.Dropout(config.dropout)

逐行解释：

相比 EncoderLayer（2 个 norm + 2 个 dropout），DecoderLayer 有 3 个 norm/dropout 对：
- norm1/dropout1：Self-Attention 子层
- norm2/dropout2：Cross-Attention 子层
- norm3/dropout3：FFN 子层

4.3.2 `maybe_add_pos_embed` — 位置编码辅助方法

def maybe_add_pos_embed(self, x: torch.Tensor, pos_embed: Optional[torch.Tensor]) -> torch.Tensor:
    return x if pos_embed is None else x + pos_embed

逐行解释：

这是一个工具方法，提取了"如果位置编码存在就加上"的逻辑。避免在 forward 中重复写条件判断。
x + pos_embed：利用 PyTorch 的广播机制——即使 pos_embed 的 batch/dim 维度不同，只要兼容就能相加。

4.3.3 `forward()` — 解码器层的前向传播

def forward(self, x, encoder_out, decoder_pos_embed=None, encoder_pos_embed=None):
    # 1. Self-Attention
    skip = x
    x = self.norm1(x)
    q = k = self.maybe_add_pos_embed(x, decoder_pos_embed)
    x, _ = self.self_attn(q, k, value=x)
    x = skip + self.dropout1(x)

逐行解释（Self-Attention 子层）：

q = k = self.maybe_add_pos_embed(x, decoder_pos_embed)：Query 和 Key 加上 decoder 的位置编码。Decoder 的 action queries 需要知道"我是第几个动作"——例如第一个 query 对应 chunk 中第 1 步动作。

    # 2. Cross-Attention
    skip = x
    x = self.norm2(x)
    x, _ = self.multihead_attn(
        query=self.maybe_add_pos_embed(x, decoder_pos_embed),
        key=self.maybe_add_pos_embed(encoder_out, encoder_pos_embed),
        value=encoder_out,
    )
    x = skip + self.dropout2(x)

逐行解释（Cross-Attention 子层）：

query：来自 decoder（action queries），带上 decoder 位置编码。
key：来自 encoder 输出（视觉+状态特征），带上 encoder 位置编码。
value：来自 encoder 输出（不带位置编码的原始内容信息）。

Cross-Attention 的直觉：
- "Query（decoder）：'我应该预测什么样的动作？'"
- "Key（encoder）：'我看到了什么视觉特征和状态信息？'"
- "Attention 权重表示：'这个动作位置应该关注图像的哪个区域/状态的哪个维度'"
- "Value：实际的图像和状态内容"

    # 3. FFN
    skip = x
    x = self.norm3(x)
    x = self.linear2(self.dropout(self.activation(self.linear1(x))))
    x = skip + self.dropout3(x)
    return x

解释：与 EncoderLayer 相同的 FFN 子层，不再赘述。

4.4 ACTDecoder — 解码器堆栈

class ACTDecoder(nn.Module):
    """Transformer Decoder"""
    def __init__(self, config: ACTConfig):
        super().__init__()
        self.layers = nn.ModuleList([ACTDecoderLayer(config) for _ in range(config.num_decoder_layers)])
        self.norm = nn.LayerNorm(config.hidden_dim)

    def forward(self, x, encoder_out, decoder_pos_embed=None, encoder_pos_embed=None):
        for layer in self.layers:
            x = layer(x, encoder_out, decoder_pos_embed=decoder_pos_embed, encoder_pos_embed=encoder_pos_embed)
        if self.norm is not None:
            x = self.norm(x)
        return x

逐行解释：

与 ACTEncoder 结构相似，但 forward 需要额外接收 encoder_out 参数（用于 Cross-Attention）。
if self.norm is not None：防御性检查。虽然 __init__ 总是创建了 self.norm，但这个检查使得子类可以安全地重写 __init__ 不创建 norm。

4.5 ACTModel — 完整模型

这是整个 ACT 系统的核心，将所有组件组装在一起。

4.5.1 `init` — 组装所有子模块

class ACTModel(nn.Module):
    def __init__(self, config: ACTConfig):
        super().__init__()
        self.config = config

        # 视觉编码器
        self.vision_encoder = RGBEncoder(
            in_channels=config.in_channels,
            hidden_dim=config.hidden_dim,
        )

        # 状态编码器
        self.state_encoder = StateEncoder(
            state_dim=config.state_dim,
            hidden_dim=config.hidden_dim,
        )

解释：初始化视觉和状态编码器。视觉使用 ResNet18，状态使用 MLP。

        # CVAE 编码器 (仅当 use_cvae=True 时)
        if config.use_cvae:
            self.action_encoder = nn.Linear(
                config.action_chunk_size * config.action_dim,
                config.hidden_dim
            )
            self.vae_output_proj = nn.Linear(config.hidden_dim, config.latent_dim * 2)
            self.latent_query = nn.Embedding(1, config.hidden_dim)

逐行解释（CVAE 的三个核心组件）：

action_encoder = Linear(chunk_size * action_dim, hidden_dim)：将完整动作 chunk 展平后投影到隐藏维度。例如 8×2=16 → 512。这个编码器只在训练时使用——它将"正确答案"编码为潜变量的条件信息。
vae_output_proj = Linear(hidden_dim, latent_dim * 2)：将隐藏表示投影为潜变量分布的参数——均值 $\mu$（前 latent_dim 维）和 log 方差（后 latent_dim 维）。* 2 是因为要同时输出 $\mu$ 和 $\log\sigma^2$。 $$h = \text{GELU}(\text{action_encoder}(a_{flattened}))$$ $$\mu = h[:, 0:d_z], \quad \log\sigma^2 = h[:, d_z:2d_z]$$
latent_query = Embedding(1, hidden_dim)：一个可学习的 latent token。概念上等同于 ViT 的 [CLS] token 或 BERT 的 [SEP] token——它是一组可学习参数，作为 Transformer encoder 的第一输入，通过 SA 聚合整个序列的全局信息。

        # Transformer Encoder
        self.encoder = ACTEncoder(config)

        # Transformer Decoder
        self.decoder = ACTDecoder(config)

        # 动作预测头
        self.action_head = nn.Linear(config.hidden_dim, config.action_dim)

逐行解释：

self.action_head：将 decoder 输出的 hidden_dim 维向量投影到 action_dim 维。对于每个 action query，输出一个动作向量。
- 输入：[B, chunk_size, 512]
- 输出：[B, chunk_size, action_dim]

        # 图像 2D 位置编码
        self.encoder_cam_feat_pos_embed = ACTSinusoidalPositionEmbedding2d(config.hidden_dim // 2)

        # Encoder 1D 位置编码 (使用可学习的 embedding)
        self.encoder_pos_embed = nn.Embedding(128, config.hidden_dim)  # 最大 128 个位置

        # Decoder 位置编码
        self.decoder_pos_embed = nn.Embedding(config.action_chunk_size, config.hidden_dim)

逐行解释（位置编码的三种形式）：

encoder_cam_feat_pos_embed：2D 正弦位置编码（用于图像特征图内部）。固定不可学习。
encoder_pos_embed = Embedding(128, 512)：可学习的 1D 位置编码。128 是预设的最大序列长度（latent(1) + state(1) + visual(H'×W')，对于 7×7 特征图 = 51 tokens，远小于 128）。
decoder_pos_embed = Embedding(chunk_size, hidden_dim)：可学习的解码器位置编码，每个 action query 一个。

        # Latent 投影
        self.latent_proj = nn.Linear(config.latent_dim, config.hidden_dim)

逐行解释：

latent_proj = Linear(32, 512)：将 CVAE 潜变量 $z$（维度 32）投影到 Transformer 的隐藏维度（512）。这使得 latent token 可以和其他 token（状态、视觉）在同一个向量空间内交互。

        # CVAE 推理时使用的 latent 统计（训练后设置）
        self.register_buffer('_inference_latent_mu', torch.zeros(1, config.latent_dim))
        self.register_buffer('_inference_latent_log_sigma', torch.zeros(1, config.latent_dim))
        self._has_inference_latent = False

逐行解释：

register_buffer('_inference_latent_mu', ...)：注册为 PyTorch buffer。buffer 是模型状态的一部分（会被 state_dict() 保存），但不是可训练参数（不会被 optimizer 更新）。
torch.zeros(1, latent_dim)：初始化为零向量。在训练完成后，set_inference_latent() 会用训练数据中收集的统计量更新这些值。
self._has_inference_latent = False：标志位——模型加载后需要调用 set_inference_latent() 才会设置为 True。未经设置的模型在推理时会使用噪声先验（randn * 0.1）。

4.5.2 `_reset_parameters()` — 参数初始化

def _reset_parameters(self):
    """只初始化新增层，保留视觉 backbone 的预训练权重。"""
    modules = [
        self.state_encoder,
        self.encoder,
        self.decoder,
        self.action_head,
        self.encoder_pos_embed,
        self.decoder_pos_embed,
        self.latent_proj,
        self.vision_encoder.encoder_img_feat_input_proj,
    ]
    if self.config.use_cvae:
        modules.extend([
            self.action_encoder,
            self.vae_output_proj,
            self.latent_query,
        ])

逐行解释：

关键设计决策：_reset_parameters() 不重置 vision_encoder.backbone（ResNet18）。这是因为 ResNet18 已经用 ImageNet 预训练权重初始化了，重新随机初始化会丢失迁移学习的效果。
被初始化的模块列表包含了所有新增的可学习层。

    for module in modules:
        for name, p in module.named_parameters():
            if p.dim() > 1:
                nn.init.xavier_uniform_(p)
            elif "bias" in name:
                nn.init.zeros_(p)
            elif "weight" in name:
                nn.init.ones_(p)

逐行解释（三种初始化策略）：

if p.dim() > 1：矩阵参数（如 Linear.weight, Embedding.weight）——使用 Xavier 均匀初始化： $$W \sim U\left[-\frac{\sqrt{6}}{\sqrt{n_{in} + n_{out}}}, \frac{\sqrt{6}}{\sqrt{n_{in} + n_{out}}}\right]$$ Xavier 初始化保持了前向和反向传播中的方差，使得深层网络训练更稳定。
elif "bias" in name：偏置参数——初始化为 0。这是标准做法，偏差在训练初期对输出没有贡献。
elif "weight" in name：1D 权重（如 LayerNorm 的 weight）——初始化为 1。LayerNorm 初始化为恒等映射。

4.5.3 CVAE 推理潜变量管理

def set_inference_latent(self, mu: torch.Tensor, log_sigma: torch.Tensor):
    if mu.ndim == 1:
        mu = mu.unsqueeze(0)
    if log_sigma.ndim == 1:
        log_sigma = log_sigma.unsqueeze(0)
    self.register_buffer('_inference_latent_mu', mu)
    self.register_buffer('_inference_latent_log_sigma', log_sigma)
    self._has_inference_latent = True

逐行解释：

if mu.ndim == 1: mu = mu.unsqueeze(0)：将 [latent_dim] 提升为 [1, latent_dim]。统一为 batch 格式。
register_buffer：保存推理统计量。这些值在训练后通过统计训练集中的 $\mu$ 和 $\log\sigma^2$ 获得（见 train_act.py 中的 latent_collection_epochs 逻辑）。

def clear_inference_latent(self):
    self._has_inference_latent = False
    self.register_buffer('_inference_latent_mu', torch.zeros(1, self.config.latent_dim))
    self.register_buffer('_inference_latent_log_sigma', torch.zeros(1, self.config.latent_dim))

解释：清除推理统计量，恢复为零向量。在加载新模型或想要使用纯噪声先验时使用。

4.5.4 CVAE 编码/采样/KL 方法

def _encode_action(self, action_target: torch.Tensor) -> Tuple[torch.Tensor, torch.Tensor]:
    batch_size = action_target.shape[0]
    action_flat = action_target.reshape(batch_size, -1)
    h = F.gelu(self.action_encoder(action_flat))
    latent_params = self.vae_output_proj(h)
    mu = latent_params[:, :self.config.latent_dim]
    log_sigma_x2 = latent_params[:, self.config.latent_dim:]
    return mu, log_sigma_x2

逐行解释：

action_flat = action_target.reshape(batch_size, -1)：将 [B, k, d_a] 展平为 [B, k*d_a]。例如 [8, 8, 2] → [8, 16]。
F.gelu(self.action_encoder(action_flat))：[B, k*d_a] → [B, hidden_dim]。编码动作序列的特征表示。
self.vae_output_proj(h)：[B, hidden_dim] → [B, latent_dim*2]。投影为分布参数。
mu = latent_params[:, :latent_dim]：前 latent_dim 维是均值 $\mu$。
log_sigma_x2 = latent_params[:, latent_dim:]：后 latent_dim 维是 log 方差 $\log\sigma^2$。注意变量名中的 x2 表示"$\sigma^2$ 的 log"。
- 为什么用 $\log\sigma^2$ 而非 $\sigma$？因为 $\log\sigma^2 \in (-\infty, +\infty)$ 是无约束的，适合神经网络直接输出。而 $\sigma$ 必须是正数，需要通过 softplus 等操作保证（增加了复杂度）。

def _sample_latent(self, mu: torch.Tensor, log_sigma_x2: torch.Tensor) -> torch.Tensor:
    sigma = (log_sigma_x2 / 2).exp()
    eps = torch.randn_like(mu)
    z = mu + sigma * eps
    return z

逐行解释（重参数化技巧 Reparameterization Trick）：

sigma = (log_sigma_x2 / 2).exp()：从 $\log\sigma^2$ 计算 $\sigma$： $$\sigma = \exp\left(\frac{\log\sigma^2}{2}\right) = \sqrt{\exp(\log\sigma^2)} = \sqrt{\sigma^2}$$
eps = torch.randn_like(mu)：从 $\mathcal{N}(0, I)$ 采样噪声。
z = mu + sigma * eps：重参数化采样。$z = \mu + \sigma \cdot \epsilon$，其中 $\epsilon \sim \mathcal{N}(0, I)$。

重参数化的意义：将随机性（$\epsilon$）与可学习参数（$\mu, \sigma$）分离。梯度可以通过 $\mu$ 和 $\sigma$ 回传，但 $\epsilon$ 不接收梯度（因为它来自固定的随机过程）。这使得 VAE 可以使用标准反向传播训练。

def _compute_kl_loss(self, mu: torch.Tensor, log_sigma_x2: torch.Tensor) -> torch.Tensor:
    kl = -0.5 * (1 + log_sigma_x2 - mu.pow(2) - log_sigma_x2.exp())
    return kl.sum(-1).mean()

逐行解释（KL 散度解析解）：

两个高斯分布之间的 KL 散度有解析形式。$KL(\mathcal{N}(\mu, \sigma^2) | \mathcal{N}(0, 1))$：

$$KL = -\frac{1}{2}\left(1 + \log\sigma^2 - \mu^2 - \sigma^2\right)$$

其中 $\log\sigma^2$ 就是我们的 log_sigma_x2。

mu.pow(2)：$\mu^2$
log_sigma_x2.exp()：$\exp(\log\sigma^2) = \sigma^2$
对于每个潜变量维度 $j$：$KL_j = -\frac{1}{2}(1 + \log\sigma^2_j - \mu^2_j - \sigma^2_j)$
.sum(-1)：对所有潜变量维度求和：$KL = \sum_{j=1}^{d_z} KL_j$
.mean()：在 batch 维度上取平均

最终返回值是一个标量（scalar），表示这个 batch 的平均 KL 散度。

4.5.5 `forward()` — 完整前向传播（9 步）

这是整个模型最核心的方法。我们逐步拆解。

def forward(self, images, state, action_target=None, infer_cvae=True):
    batch_size = images.shape[0]

解释：infer_cvae=True 表示推理时会采样潜变量 z（随机性推理）。训练时传入 infer_cvae=False，因为 z 的采样在 _encode_action 中已经处理。

步骤 1：处理 latent (CVAE)

    mu = None
    log_sigma_x2 = None

    if self.config.use_cvae and action_target is not None and self.training:
        # 训练模式：从 action target 编码获取 latent
        mu, log_sigma_x2 = self._encode_action(action_target)
        latent = self._sample_latent(mu, log_sigma_x2)

逐行解释：

条件 use_cvae and action_target is not None and self.training：三个条件同时满足时才进入 CVAE 编码分支：
1. CVAE 开关开启
2. 提供了目标动作（训练时）
3. 模型处于训练模式
训练时：编码器 $q(z|o, a)$ 将观测和动作映射为潜变量分布，然后重参数化采样。

    elif self.config.use_cvae and infer_cvae and self._has_inference_latent:
        # 推理模式（已设置 latent 分布）：从存储的分布采样
        inf_mu = self._inference_latent_mu.to(images.device)
        inf_log_sig = self._inference_latent_log_sigma.to(images.device)
        if batch_size > 1:
            inf_mu = inf_mu.expand(batch_size, -1)
            inf_log_sig = inf_log_sig.expand(batch_size, -1)
        latent = self._sample_latent(inf_mu, inf_log_sig)

逐行解释：

.to(images.device)：确保分布参数在正确的设备上。
if batch_size > 1: .expand(batch_size, -1)：当 batch 推理时（如并行评估多个场景），将 [1, latent_dim] 扩展到 [batch_size, latent_dim]。.expand 不复制内存（只是修改 stride），比 .repeat 更高效。

    elif self.config.use_cvae and infer_cvae:
        # 推理模式（未设置 latent 分布）：使用零向量并添加噪声
        latent = torch.randn(batch_size, self.config.latent_dim, device=images.device) * 0.1
    else:
        latent = torch.zeros(batch_size, self.config.latent_dim, device=images.device)

逐行解释：

torch.randn(...) * 0.1：当没有训练好的 latent 统计量时，使用缩小到 0.1 倍的标准正态噪声。乘以 0.1 是为了限制噪声范围——未经训练的 latent 可能产生极端的动作输出。
torch.zeros(...)：当 CVAE 禁用时，latent 为全 0 向量（退化为确定性模型）。

步骤 2：视觉编码

    vision_features = self.vision_encoder(images)  # [B, H*W, hidden_dim]

步骤 3：状态编码

    state_features = self.state_encoder(state)  # [B, 1, hidden_dim]

步骤 4：Latent 投影

    latent_features = self.latent_proj(latent).unsqueeze(1)  # [B, 1, hidden_dim]

解释：Linear(32→512) 投影后加 .unsqueeze(1) 使其成为 [B, 1, 512] 格式，与状态 token 维度统一。

步骤 5：构建 Encoder 输入序列

    encoder_in = torch.cat([latent_features, state_features, vision_features], dim=1)
    # [B, 1 + 1 + H'*W', hidden_dim] = [B, 2+49, 512] = [B, 51, 512]

逐行解释：

拼接顺序：[latent(1), state(1), vision(49)]。这个顺序不是任意的——它反映了信息流的层次：
- latent token 在最前面：作为"全局上下文查询"，通过 Self-Attention 聚合后续所有 token 的信息
- state token 紧随其后：状态与动作直接相关
- vision tokens 在最后：空间信息是动作预测的基础
实际上，由于 Self-Attention 是全连接的，token 的顺序（在没有位置编码的情况下）并不影响信息传播。但加上位置编码后，嵌入的位置信息会赋予不同位置的 token 不同的"角色"。

步骤 6：构建位置编码

    seq_len = encoder_in.shape[1]
    if seq_len <= self.encoder_pos_embed.num_embeddings:
        pos_embed = self.encoder_pos_embed.weight[:seq_len].unsqueeze(0)
    else:
        pos_embed = self.encoder_pos_embed.weight.repeat(
            (seq_len // self.encoder_pos_embed.num_embeddings) + 1, 1
        )[:, :seq_len]

逐行解释：

self.encoder_pos_embed.weight[:seq_len]：从 Embedding 权重矩阵中取前 seq_len 行。Embedding 的权重形状是 [128, 512]，取 [:51] 表示只用前 51 个位置编码。
.unsqueeze(0)：添加 batch 维度：[51, 512] → [1, 51, 512]。这利用广播使得后续可以直接与 [B, 51, 512] 相加。
else 分支：如果序列长度超过 128（不太可能），使用循环重复位置编码。

步骤 7：Transformer Encoder

    encoder_out = self.encoder(encoder_in, pos_embed=pos_embed)

解释：编码器处理整个序列，输出 [B, 51, 512]。每个 token 现在都包含了全局上下文信息（通过 Self-Attention）。

步骤 8：Transformer Decoder

    decoder_pos_embed = self.decoder_pos_embed.weight.unsqueeze(0).expand(batch_size, -1, -1)

    decoder_in = torch.zeros(
        batch_size, self.config.action_chunk_size, self.config.hidden_dim,
        device=images.device
    ) + decoder_pos_embed

    decoder_out = self.decoder(
        decoder_in, encoder_out,
        decoder_pos_embed=decoder_pos_embed,
        encoder_pos_embed=pos_embed,
    )

逐行解释：

decoder_pos_embed = Embedding.weight.unsqueeze(0).expand(B, -1, -1)：[k, 512] → [1, k, 512] → [B, k, 512]
decoder_in = torch.zeros(...) + decoder_pos_embed：Decoder 的初始输入是纯位置编码（全零内容 + 位置编码）。这与 NLP Transformer 不同——NLP 中 decoder 输入是"已生成的部分序列"。在 ACT 中，所有 k 个动作是一次并行输出的（非自回归），所以 8 个 action queries 同时以全零的内容开始，仅靠位置编码区分彼此。
self.decoder(...)：Cross-Attention 使得 action queries 从 encoder 输出中提取相关信息，经过 FFN 加工后输出 [B, k, 512]。

步骤 9：预测动作与计算 KL 损失

    action_pred = self.action_head(decoder_out)  # [B, k, 512] → [B, k, action_dim]

    kl_loss = None
    if self.config.use_cvae and mu is not None and log_sigma_x2 is not None:
        kl_loss = self._compute_kl_loss(mu, log_sigma_x2)

    return {
        "action": action_pred,
        "mu": mu,
        "log_sigma_x2": log_sigma_x2,
        "kl_loss": kl_loss,
    }

逐行解释：

action_head = Linear(512, action_dim)：每个 action query 独立投影为动作向量。
返回字典包含所有可能需要的中间输出。训练时使用 action 和 kl_loss；分析时可能用到 mu 和 log_sigma_x2。

4.5.6 `get_action()` — 推理接口

def get_action(self, images, state, use_temporal_ensembling=False,
               temporal_ensembler=None, noise=0.0):
    self.eval()

    with torch.no_grad():
        output = self.forward(images, state, action_target=None, infer_cvae=True)
        actions = output["action"]  # [batch, chunk_size, action_dim]

        if noise > 0:
            actions = actions + torch.randn_like(actions) * noise

        if use_temporal_ensembling and temporal_ensembler is not None:
            action = temporal_ensembler.update(actions)
        else:
            action = actions[:, 0]

    return action

逐行解释：

self.eval()：将模型设为评估模式。这会：
1. 关闭 Dropout（所有神经元都参与计算）
2. 确保 BatchNorm 使用运行统计量而非 batch 统计量
3. 不影响 CVAE，因为 forward 中的路径由 self.training 和 infer_cvae 参数控制
torch.no_grad()：禁用梯度计算，节省内存和计算量。
noise > 0：可选的动作噪声。在探索性场景（如在线学习）中注入噪声帮助探索。
action = actions[:, 0]：无 Temporal Ensembling 时，只取 chunk 的第一个动作。
return action：返回形状 [B, action_dim] 的单步动作。

4.5.7 `reset_temporal_ensembler()` — 重置集成器

def reset_temporal_ensembler(self, temporal_ensembler):
    temporal_ensembler.reset()

解释：在每次 episode 开始时调用，清除历史累积。否则上一个 episode 的动作预测会污染新 episode。

课后思考

为什么 decoder 使用全零的 content + 位置编码作为初始输入，而不是像 NLP Transformer 那样使用已生成的部分序列？
如果去掉 _reset_parameters() 中"跳过 ResNet backbone"的逻辑，模型性能会如何变化？
CVAE 的 KL 散度公式中，为什么 $\log\sigma^2$ 放在前面是 + 号，$\sigma^2$ 放在后面是 - 号？推导一下这个公式。

第 5 讲：训练流程

对应源文件：policies/models/act/train_act.py

学习目标

掌握 ACT 训练循环的完整流程
理解 CVAE latent 统计量收集的原理
掌握检查点保存策略

5.1 导入与路径设置

import sys
import json
import argparse
import logging
from pathlib import Path
from typing import Dict, List, Any

import torch
import torch.nn as nn
import torch.nn.functional as F
import torch.optim as optim
from torch.utils.data import DataLoader
import numpy as np
from PIL import Image
import io
import base64

逐行解释：

argparse：命令行参数解析。train_act.py 作为独立脚本运行，通过命令行传递超参数。
Path（pathlib）：跨平台的文件路径操作。比 os.path 更现代、更可读。
PIL.Image：Pillow 库的图像加载。用于从文件中读取 PNG/JPEG 图像。
base64：Base64 编解码。虽然训练脚本中不直接使用，但导入是为了兼容数据可能来源于 base64 编码的场景。

project_root = Path(__file__).parent.parent.parent
sys.path.insert(0, str(project_root))

逐行解释：

Path(__file__)：当前文件的绝对路径。__file__ 是 Python 的模块级变量，指向当前 .py 文件的路径。
.parent.parent.parent：向上三级目录——train_act.py → act/ → models/ → policies/，即项目根目录。
sys.path.insert(0, str(project_root))：将项目根目录插入 Python 导入路径的最前面（索引 0）。这确保 from policies.models.act.modeling_act import ... 能找到正确的模块，而不会与系统中已安装的同名包冲突。

logger = logging.getLogger("backend.services.training.orchestrator")
if not logger.handlers:
    logging.basicConfig(level=logging.INFO)

逐行解释：

logging.getLogger("backend.services.training.orchestrator")：使用与训练编排器相同的 logger 名称。这意味着训练脚本的日志会与后端服务的训练日志合并，统一管理。
if not logger.handlers：防御性检查。如果 logger 已经被其他模块配置过了（有 handlers），就不重复配置，避免日志重复输出。
logging.basicConfig(level=logging.INFO)：设置日志级别为 INFO。这会输出 epoch 进度、损失值等信息。

本地导入

from policies.models.act.modeling_act import ACTModel, ACTConfig
from policies.models.act.ACTDataset import ACTDataset
from policies.models.act.defaults import build_act_config, act_config_to_dict

逐行解释：

这三个导入是项目内部的，放在 sys.path 修改之后，确保路径正确。
ACTModel 和 ACTConfig 从 modeling_act 导入，而不是从 configuration_act——因为 ACTModel 是训练的入口类。
build_act_config 用于构建配置，act_config_to_dict 用于序列化到检查点。

5.2 `load_dataset()` — 数据加载

5.2.1 函数签名

def load_dataset(data_dir: str = "dataset") -> Dict[str, torch.Tensor]:

解释：返回一个字典，包含三个键："observation.image", "observation.state", "action"。这是 LeRobot 数据集的标准格式。

5.2.2 加载统计信息

    data_path = Path(data_dir)

    with open(data_path / "meta" / "stats.json", "r") as f:
        stats = json.load(f)

逐行解释：

data_path / "meta" / "stats.json"：/ 操作符是 pathlib.Path 的特有语法——路径拼接。等价于 os.path.join(data_dir, "meta", "stats.json")。
json.load(f)：从文件中读取 JSON。stats.json 包含训练数据集的 QUANTILES 百分位数统计量（1%/99%分位）。

5.2.3 加载 Parquet 数据文件

    import pandas as pd

    parquet_files = sorted(data_path.glob("data/chunk-*/file-*.parquet"))
    print(f"找到 {len(parquet_files)} 个数据文件")

逐行解释：

import pandas as pd：延迟导入（Lazy Import）。只有在实际调用 load_dataset() 时才导入 pandas。这避免了在不使用该函数时加载这个重量级库。
data_path.glob("data/chunk-*/file-*.parquet")：通配符匹配。LeRobot 数据集存储为 data/chunk-000/file-000.parquet, data/chunk-000/file-001.parquet, ...
sorted(...)：按文件名排序，确保数据文件按正确的时间顺序加载。

5.2.4 逐文件加载

    images = []
    states = []
    actions = []

    for parquet_file in parquet_files:
        df = pd.read_parquet(parquet_file)

逐行解释：

pd.read_parquet(parquet_file)：读取 Apache Parquet 格式文件。Parquet 是列式存储格式，比 CSV/JSON 更高效。
使用列表 [] 逐个追加，最后用 torch.stack 合并——这是处理大量数据时的内存友好做法。

        for img_path in df["observation.image"]:
            full_path = data_path / img_path
            if full_path.exists():
                img = Image.open(full_path).convert("RGB")
                img = img.resize((224, 224))
                img_tensor = torch.from_numpy(np.array(img)).permute(2, 0, 1).float() / 255.0
                images.append(img_tensor)
            else:
                images.append(torch.zeros(3, 224, 224))

逐行解释：

Image.open(full_path).convert("RGB")：Pillow 打开图像并确保是 RGB 格式（处理 RGBA/灰度等异常格式）。
img.resize((224, 224))：缩放到 ResNet18 的标准输入尺寸。
np.array(img)：Pillow Image → numpy 数组，形状 [H, W, C]。
torch.from_numpy(...)：numpy → PyTorch tensor（零拷贝）。注意 from_numpy 创建的 tensor 与 numpy 数组共享内存！
.permute(2, 0, 1)：通道顺序从 [H, W, C] 转为 PyTorch 标准 [C, H, W]。
.float() / 255.0：uint8 [0, 255] → float32 [0.0, 1.0]。
else 分支：图像文件缺失时的容错处理。使用零张量（全黑图像）。这种做法至少能继续训练而不崩溃，但训练质量会受影响。

        for state_list in df["observation.state"]:
            state = torch.tensor(state_list, dtype=torch.float32)
            states.append(state)

逐行解释：

torch.tensor(state_list, dtype=torch.float32)：从 Python list 创建 float32 张量。Parquet 中可能存储为 list 或 numpy array，torch.tensor 能处理多种输入。

        for action_data in df["action"]:
            action = torch.tensor(action_data, dtype=torch.float32)
            actions.append(action)

5.2.5 合并与返回

    images = torch.stack(images)  # [N, 3, 224, 224]
    states = torch.stack(states)  # [N, state_dim]
    actions = torch.stack(actions)  # [N, action_dim]

    print(f"加载完成: {len(images)} 个样本")
    print(f"  图像形状: {images.shape}")
    print(f"  状态形状: {states.shape}")
    print(f"  动作形状: {actions.shape}")

逐行解释：

torch.stack(images)：将图像列表堆叠为一个大的张量。stack 创建新维度（dim=0），要求列表中所有张量形状一致。如果 List 中某个元素是形状不一致的张量，stack 会报错。
打印形状信息用于调试——让用户确认数据加载正确。

    return {
        "observation.image": images,
        "observation.state": states,
        "action": actions,
    }

解释：返回 LeRobot 标准格式的字典。注意是扁平键名 "observation.image" 而非嵌套字典，这与 ACTDataset.__getitem__ 中的解包逻辑对应。

5.3 `train()` — 训练主函数

5.3.1 函数签名

def train(
    data_dir: str = "dataset",
    output_dir: str = "checkpoints",
    epochs: int = 50,
    batch_size: int = 8,
    lr: float = 1e-4,
    state_dim: int = 2,
    action_dim: int = 2,
    action_chunk_size: int = 8,
    hidden_dim: int = 512,
) -> ACTModel:

逐行解释：

epochs = 50：完整遍历数据集 50 次。
batch_size = 8：每次更新使用 8 个样本。注意这个值较小——机器人数据集通常不如 NLP/视觉数据集那么大，小 batch 也是合理的。
lr = 1e-4：学习率 $10^{-4}$。这是 Adam 优化器的常用学习率。
state_dim = 2, action_dim = 2：AKA-Sim2Real 的实际维度（左右轮速度）。
action_chunk_size = 8：预测 8 步动作。

5.3.2 加载数据与归一化统计

    data = load_dataset(data_dir)

    stats_path = Path(data_dir) / "meta" / "stats.json"
    state_q01, state_q99, action_q01, action_q99 = None, None, None, None
    if stats_path.exists():
        with open(stats_path, "r") as f:
            stats = json.load(f)
        state_q01 = torch.tensor(stats["observation.state"]["q01"], dtype=torch.float32)
        state_q99 = torch.tensor(stats["observation.state"]["q99"], dtype=torch.float32)
        action_q01 = torch.tensor(stats["action"]["q01"], dtype=torch.float32)
        action_q99 = torch.tensor(stats["action"]["q99"], dtype=torch.float32)

逐行解释：

stats_path.exists()：防御性检查——stats.json 可能不存在（数据采集阶段可能没有生成统计量）。

stats["observation.state"]["q01"]：JSON 中嵌套访问。stats.json 的结构：

{
  "observation.state": {"q01": [...], "q99": [...]},
  "action": {"q01": [...], "q99": [...]}
}

如果 stats.json 不存在，所有归一化参数为 None，ACTDataset 会跳过归一化步骤。

5.3.3 构建配置与模型

    config = build_act_config(
        state_dim=state_dim,
        action_dim=action_dim,
        action_chunk_size=action_chunk_size,
        hidden_dim=hidden_dim,
    )

    model = ACTModel(config)
    print(f"\n模型参数量: {sum(p.numel() for p in model.parameters()):,}")

逐行解释：

build_act_config(...)：使用 defaults.py 的工厂函数，只覆盖与默认值不同的参数。
sum(p.numel() for p in model.parameters())：计算模型总参数量。numel() 返回张量中元素的数量。对于典型配置（hidden_dim=512, 4 encoder + 4 decoder layers），参数量大约在 20-30M。

5.3.4 创建 DataLoader

    dataset = ACTDataset(
        data, action_chunk_size=action_chunk_size, normalize_images=True,
        state_q01=state_q01, state_q99=state_q99,
        action_q01=action_q01, action_q99=action_q99,
    )

    dataloader = DataLoader(
        dataset, batch_size=batch_size, shuffle=True, num_workers=0,
    )

逐行解释：

shuffle=True：每个 epoch 随机打乱数据顺序。这是训练中的标准做法——防止模型记忆数据的顺序。
num_workers=0：使用主进程加载数据（不启用多进程）。这是因为：
1. 机器人数据集通常较小，多进程加载的开销可能大于收益
2. 多进程与某些文件系统/OS 有兼容性问题
3. 调试更简单（单进程的错误信息更清晰）

5.3.5 创建优化器与损失函数

    optimizer = optim.Adam(model.parameters(), lr=lr)
    criterion = nn.MSELoss()

逐行解释：

optim.Adam：Adam 优化器。自适应学习率方法，结合了 Momentum 和 RMSProp： $$m_t = \beta_1 m_{t-1} + (1-\beta_1) g_t$$ $$v_t = \beta_2 v_{t-1} + (1-\beta_2) g_t^2$$ $$\theta_t = \theta_{t-1} - \alpha \cdot \frac{m_t}{\sqrt{v_t} + \epsilon}$$
nn.MSELoss()：均方误差损失。注意这个 criterion 实际上在训练循环中没有被使用——取而代之的是 F.l1_loss（见下文）。这是一个开发过程中的遗留（可能最初计划用 MSE，后来改为 L1）。

5.3.6 设备与模式设置

    model.train()
    device = torch.device("cuda" if torch.cuda.is_available() else "cpu")
    model = model.to(device)
    print(f"使用设备: {device}")

逐行解释：

model.train()：设置训练模式。启用 Dropout 等正则化技术。
torch.device("cuda" if torch.cuda.is_available() else "cpu")：优先使用 GPU（CUDA），回退到 CPU。
model = model.to(device)：将模型和所有参数/缓冲区移动到目标设备。注意必须重新赋值——to() 返回新模型（虽然原地操作时是同一个对象，但保险起见使用返回值）。

5.3.7 CVAE Latent 统计收集设置

    all_mu = []
    all_log_sigma = []
    latent_collection_epochs = min(5, epochs // 2)

逐行解释：

all_mu, all_log_sigma：收集最后几个 epoch 中产生的潜变量分布参数。存为 Python 列表，最后合并。
latent_collection_epochs = min(5, epochs // 2)：用训练的最后几个 epoch（最多 5 个）来收集 latent 统计量。为什么用最后几个 epoch？
- 训练初期模型不稳定，潜变量分布还在剧烈变化
- 训练后期模型收敛，潜变量分布趋于稳定
- 用最后几个 epoch 的统计量更能代表推理时的分布

5.3.8 训练循环

    for epoch in range(epochs):
        total_loss = 0
        total_l1_loss = 0
        total_kl_loss = 0
        num_batches = 0

逐行解释：

每个 epoch 开始前重置累积统计量。这些变量用于计算每个 epoch 的平均损失（用于日志输出和监控训练进度）。

        for batch in dataloader:
            images = batch["observation"]["image"].to(device)
            states = batch["observation"]["state"].to(device)
            actions = batch["action"].to(device)

逐行解释：

.to(device)：将每个 batch 移动到 GPU/CPU。如果不做这一步，数据和模型不在同一设备上，PyTorch 会报 Expected all tensors to be on the same device 错误。

前向传播

            optimizer.zero_grad()

            output = model(
                images, states,
                action_target=actions,
                infer_cvae=False,
            )

逐行解释：

optimizer.zero_grad()：清零梯度。PyTorch 的梯度默认是累加的（.backward() 把新梯度加到 .grad 属性上）。如果不清零，多次 backward 的梯度会累加，导致错误。
infer_cvae=False：训练模式下，即使不在 self.training 条件下，也不走 CVAE 推理路径。infer_cvae=False 告诉 forward()："不要尝试从先验分布采样 z，我们已经在训练时用 action_target 编码了 z"。

            predicted_actions = output["action"]
            kl_loss = output.get("kl_loss")
            mu = output.get("mu")
            log_sigma_x2 = output.get("log_sigma_x2")

逐行解释：

output.get("kl_loss")：使用 .get() 而非 [] 索引。如果 use_cvae=False，kl_loss 为 None，.get() 返回 None 而非抛出 KeyError。

收集 latent 统计

            if config.use_cvae and mu is not None and log_sigma_x2 is not None:
                if epoch >= epochs - latent_collection_epochs:
                    all_mu.append(mu.detach().cpu())
                    all_log_sigma.append(log_sigma_x2.detach().cpu())

逐行解释：

epoch >= epochs - latent_collection_epochs：只在最后几个 epoch 收集。例如 epochs=50, latent_collection_epochs=5 → 从 epoch 45 开始收集。
.detach()：断开计算图。.detach() 阻止梯度从 latents 统计量回传到模型。不 detach 的后果：这些保存的 tensor 持有对计算图的引用，可能导致内存泄漏。
.cpu()：将 tensor 移动到 CPU。因为这些统计量需要在 GPU 训练循环之外使用（保存到文件时），移到 CPU 确保兼容性。

损失计算

            l1_loss = F.l1_loss(predicted_actions, actions)

            if kl_loss is not None:
                loss = l1_loss + kl_loss * config.kl_weight
                total_kl_loss += kl_loss.item()
            else:
                loss = l1_loss

            total_l1_loss += l1_loss.item()

逐行解释：

F.l1_loss(predicted_actions, actions)：L1 损失（平均绝对误差）： $$\text{L1} = \frac{1}{N} \sum_{i=1}^{N} |\hat{y}_i - y_i|$$

为什么用 L1 而非 MSE（L2）？
- L1 对异常值（outliers）不那么敏感——单个大误差不会主导损失
- 在机器人控制中，偶尔的大误差比持续的小误差更危险，L1 更鲁棒
- L1 在高维动作空间中产生更清晰的梯度信号
loss = l1_loss + kl_loss * config.kl_weight：总损失 = 重建损失 + 0.1 × KL 散度。KL 权重 0.1 使得模型优先学习好的重建，同时温和地约束潜变量分布。
.item()：将标量 tensor 转换为 Python float。这既节约 GPU 内存（不再持有这些 tensor），又方便日志输出。

反向传播与优化

            loss.backward()
            optimizer.step()

            total_loss += loss.item()
            num_batches += 1

逐行解释：

loss.backward()：反向传播。计算损失对每个参数的梯度 $\frac{\partial L}{\partial \theta}$。梯度存储在 param.grad 中。
optimizer.step()：参数更新。使用 Adam 算法根据梯度更新所有参数。
total_loss += loss.item()：累积损失用于 epoch 统计。

5.3.9 Epoch 日志与检查点

        avg_loss = total_loss / num_batches
        avg_l1 = total_l1_loss / num_batches
        if config.use_cvae:
            avg_kl = total_kl_loss / num_batches
            logger.info(f"Epoch {epoch + 1}/{epochs}, Loss: {avg_loss:.6f} (L1: {avg_l1:.6f}, KL: {avg_kl:.6f})")

逐行解释：

avg_loss = total_loss / num_batches：计算 epoch 平均损失。
日志格式 {avg_loss:.6f}：保留 6 位小数。有助于观察训练的微小变化。

        if (epoch + 1) % 10 == 0:
            checkpoint_path = Path(output_dir) / f"checkpoint_epoch_{epoch + 1}.pt"
            checkpoint_path.parent.mkdir(parents=True, exist_ok=True)
            torch.save(model.state_dict(), checkpoint_path)

逐行解释：

(epoch + 1) % 10 == 0：每 10 个 epoch 保存一个检查点。使用 epoch + 1 因为 epoch 从 0 开始计数。
checkpoint_path.parent.mkdir(parents=True, exist_ok=True)：确保输出目录存在。parents=True 递归创建父目录，exist_ok=True 如果目录已存在不报错。
torch.save(model.state_dict(), checkpoint_path)：保存模型权重。只保存 state_dict() 而非整个模型对象——避免 pickle 依赖和版本不兼容。

5.3.10 保存最终模型

    if config.use_cvae and len(all_mu) > 0:
        all_mu_tensor = torch.cat(all_mu, dim=0)
        all_log_sigma_tensor = torch.cat(all_log_sigma, dim=0)

        latent_mu_mean = all_mu_tensor.mean(dim=0)
        latent_log_sigma_mean = all_log_sigma_tensor.mean(dim=0)

        final_path = Path(output_dir) / "final_model.pt"
        checkpoint = {
            'model_state_dict': model.state_dict(),
            'inference_latent_mu': latent_mu_mean,
            'inference_latent_log_sigma': latent_log_sigma_mean,
            'config': act_config_to_dict(config),
        }
        torch.save(checkpoint, final_path)

逐行解释：

torch.cat(all_mu, dim=0)：将收集到的所有 $\mu$ 向量沿 batch 维度拼接。如果收集了 5 个 epoch × 10 batches/epoch × 8 samples/batch = 400 个样本：[400, 32]。
all_mu_tensor.mean(dim=0)：沿 batch 维度取平均，得到 [32] 的均值向量。这就是推理时使用的潜变量分布均值。
检查点字典的结构：model_state_dict（权重）+ inference_latent_mu（推理均值）+ inference_latent_log_sigma（推理方差）+ config（配置）。加载时通过 load_checkpoint_bundle 解析。

    else:
        final_path = Path(output_dir) / "final_model.pt"
        torch.save(model.state_dict(), final_path)

解释：无 CVAE 时只保存权重。

5.4 `export_to_huggingface()` — 导出

def export_to_huggingface(model, config, output_dir="huggingface_model", model_name="aka-sim-act"):
    output_path = Path(output_dir)
    output_path.mkdir(parents=True, exist_ok=True)

    torch.save(model.state_dict(), output_path / "pytorch_model.bin")

    # 保存 config.json
    config_dict = {
        "model_type": "act",
        "state_dim": config.state_dim,
        ...
    }
    with open(output_path / "config.json", "w") as f:
        json.dump(config_dict, f, indent=2)

逐行解释：

HuggingFace 格式约定：pytorch_model.bin（权重）+ config.json（配置）是 HF 的标准文件命名。
json.dump(..., indent=2)：格式化输出，便于人类阅读和调试。

生成的推理代码、README

    inference_code = '''...'''   # 模板推理代码
    with open(output_path / "inference.py", "w") as f:
        f.write(inference_code)

    readme = f'''...'''          # 模板 README（使用 f-string 插入实际配置值）
    with open(output_path / "README.md", "w") as f:
        f.write(readme)

解释：导出的推理代码是一个模板/存根（stub），需要用户根据实际情况修改。它的作用是指明推理 API 的使用方式。f'''...''' 是 f-string 三重引号——在运行时将配置值插入模板文本。

5.5 `main()` — 命令行入口

def main():
    parser = argparse.ArgumentParser(description="训练ACT模型")
    parser.add_argument("--data_dir", type=str, default="dataset")
    parser.add_argument("--output_dir", type=str, default="checkpoints")
    parser.add_argument("--epochs", type=int, default=50)
    parser.add_argument("--batch_size", type=int, default=8)
    parser.add_argument("--lr", type=float, default=1e-4)
    parser.add_argument("--export_hf", action="store_true")

    args = parser.parse_args()

    model = train(
        data_dir=args.data_dir, output_dir=args.output_dir,
        epochs=args.epochs, batch_size=args.batch_size, lr=args.lr,
    )

    if args.export_hf:
        config = build_act_config(state_dim=7, action_dim=5, action_chunk_size=16, hidden_dim=512)
        export_to_huggingface(model, config)

if __name__ == "__main__":
    main()

逐行解释：

argparse.ArgumentParser：Python 标准库的命令行解析器。
action="store_true"：--export_hf 是一个布尔标志——出现则为 True，否则为 False。
if __name__ == "__main__"：Python 的标准入口保护。当文件被直接运行时（python train_act.py）执行 main()；被 import 时不会执行。
export_to_huggingface 中硬编码了 state_dim=7, action_dim=5 而非使用训练时的配置——这是一个已知的限制，导出 HF 格式时需要手动调整参数来匹配训练配置。

课后思考

为什么 latent 统计量在最后几个 epoch 收集，而不是在整个训练过程中收集？
如果 batch_size 太小（如 1）或太大（如 256）会导致什么问题？LayerNorm 和 Adam 对此分别有何影响？
torch.save(model.state_dict()) vs torch.save(model) 各有什么优缺点？什么场景下选择哪种？

第 6 讲：推理运行时系统

对应源文件：

backend/services/inference/runtime.py — 推理运行时

backend/services/inference/checkpoint.py — 检查点加载

backend/services/inference/preprocess.py — 预处理

backend/services/inference/execution.py — 时序集成策略

学习目标

掌握模型从磁盘加载到推理输出的完整数据流
理解 Singleton 运行时模式的设计意图
掌握观测归一化→模型推理→动作反归一化的完整 pipeline

6.1 checkpoint.py — 检查点加载

6.1.1 ACTNormalizationStats — 归一化统计量数据类

@dataclass
class ACTNormalizationStats:
    state_q01: torch.Tensor = field(default_factory=lambda: torch.zeros(2))
    state_q99: torch.Tensor = field(default_factory=lambda: torch.ones(2))
    action_q01: torch.Tensor = field(default_factory=lambda: torch.zeros(2))
    action_q99: torch.Tensor = field(default_factory=lambda: torch.ones(2))

逐行解释：

@dataclass：Python 的 dataclasses.dataclass 装饰器。自动生成 __init__, __repr__, __eq__。比普通类少写大量样板代码。
field(default_factory=lambda: torch.zeros(2))：默认值用 default_factory 而非直接 torch.zeros(2)。这是因为：
- Python 的 dataclass 默认值在类定义时只求值一次
- 如果直接用 torch.zeros(2)，所有实例会共享同一个 tensor 对象
- default_factory 在每次实例化时调用 lambda，每个实例拥有独立的 tensor
默认值 zeros(2) 和 ones(2)：当没有加载 stats.json 时，q01=0, q99=1 使得归一化 2*(x-0)/(1-0)-1 = 2x-1——相当于简单的恒等映射到 [-1, 1]。

6.1.2 ACTCheckpointBundle — 检查点数据包

@dataclass
class ACTCheckpointBundle:
    model_config: "ACTConfig"
    state_dict: dict
    inference_latent_mu: Optional[torch.Tensor] = None
    inference_latent_log_sigma: Optional[torch.Tensor] = None

逐行解释：

model_config: "ACTConfig"：字符串形式的类型注解（前向引用）。因为文件顶部使用了 from __future__ import annotations，所有注解都是字符串，在 TYPE_CHECKING 时才求值。这避免了循环导入。
这个 dataclass 的作用是将"加载检查点"这个操作的结果打包为一个不可变的值对象（Value Object），后续的 instantiate_model 从这个包中读取所有需要的信息。

6.1.3 `get_default_device()` — 设备选择

def get_default_device() -> str:
    if torch.cuda.is_available():
        return "cuda"
    if torch.backends.mps.is_available():
        return "mps"
    return "cpu"

逐行解释：

设备选择优先级：CUDA (NVIDIA GPU) > MPS (Apple Silicon GPU) > CPU。
torch.cuda.is_available()：检查是否有 NVIDIA GPU（需要安装 CUDA 版本的 PyTorch）。
torch.backends.mps.is_available()：检查是否有 Apple M 系列芯片的 GPU（macOS 12.3+ 且 PyTorch 1.12+）。
返回字符串而非 torch.device 对象——字符串在后续代码中可以直接传给 .to() 方法。

6.1.4 `resolve_default_model_path()` — 默认路径解析

def resolve_default_model_path() -> Path:
    project_root = Path(__file__).resolve().parents[3]
    model_path = project_root / "output" / "train" / "final_model.pt"
    if not model_path.exists():
        model_path = project_root / "output" / "train" / "model.pt"
    return model_path

逐行解释：

Path(__file__).resolve().parents[3]：从当前文件向上 4 级目录：
- execution.py → inference/ → services/ → backend/ → 项目根目录
- .resolve() 解析符号链接，.parents[3] 取第 4 层父目录。
回退策略：先尝试 final_model.pt（训练完保存的最终模型），不存在则尝试 model.pt（兼容旧命名）。

6.1.5 `load_checkpoint_bundle()` — 加载检查点

def load_checkpoint_bundle(model_path: Optional[str], device: str) -> ACTCheckpointBundle:
    project_root = Path(__file__).resolve().parents[3]
    if model_path is not None:
        path = Path(model_path) if Path(model_path).is_absolute() else project_root / model_path
    else:
        path = resolve_default_model_path()
    if not path.exists():
        raise FileNotFoundError(f"模型文件不存在: {path}")

逐行解释：

Path(model_path).is_absolute()：判断用户提供的路径是否为绝对路径。
- 绝对路径：直接使用
- 相对路径：相对于项目根目录拼接
raise FileNotFoundError(...)：Fail Fast——文件不存在立即报错，比后续 KeyError 或 RuntimeError 更容易定位问题。

    checkpoint = torch.load(path, map_location=device, weights_only=False)

逐行解释：

torch.load(path, map_location=device)：加载 PyTorch 序列化文件。map_location 指定将 tensor 加载到哪个设备。这避免了"GPU 模型加载到 CPU 机器"的问题。
weights_only=False：允许加载包含非张量数据（如配置字典、Python 对象）。weights_only=True（PyTorch 2.0+）更安全但限制更多。这里因为检查点包含 dict、str 等非张量数据，必须设为 False。

    if isinstance(checkpoint, dict) and "model_state_dict" in checkpoint:
        state_dict = checkpoint["model_state_dict"]
        checkpoint_config = checkpoint.get("config", {})
        inference_latent_mu = checkpoint.get("inference_latent_mu")
        inference_latent_log_sigma = checkpoint.get("inference_latent_log_sigma")
    else:
        state_dict = checkpoint
        checkpoint_config = {}
        inference_latent_mu = None
        inference_latent_log_sigma = None

逐行解释：

兼容两种检查点格式：
- 新格式（train_act.py 输出）：{"model_state_dict": ..., "config": ..., "inference_latent_mu": ..., ...}
- 旧格式（仅权重）：直接是 state_dict
checkpoint.get("config", {})：如果旧格式没有 config，返回空字典——build_act_config(**{}) 使用全部默认值。

    return ACTCheckpointBundle(
        model_config=build_act_config(**checkpoint_config),
        state_dict=state_dict,
        inference_latent_mu=inference_latent_mu,
        inference_latent_log_sigma=inference_latent_log_sigma,
    )

6.1.6 `load_stats()` — 加载归一化统计

def load_stats(stats_dir: Optional[str]) -> ACTNormalizationStats:
    project_root = Path(__file__).resolve().parents[3]
    if stats_dir is not None:
        data_dir = Path(stats_dir) if Path(stats_dir).is_absolute() else project_root / stats_dir
    else:
        data_dir = project_root / "output" / "dataset"
    stats_path = data_dir / "meta" / "stats.json"
    if not stats_path.exists():
        return ACTNormalizationStats()

解释：如果 stats.json 不存在，返回默认统计量（q01=0, q99=1），相当于不做 QUANTILES 归一化。

    with open(stats_path, "r") as f:
        stats = json.load(f)

    state_entry = stats.get("observation.state", {})
    action_entry = stats.get("action", {})

    state_q01 = torch.tensor(state_entry.get("q01", [0.0, 0.0]), dtype=torch.float32)
    state_q99 = torch.tensor(state_entry.get("q99", [1.0, 1.0]), dtype=torch.float32)
    action_q01 = torch.tensor(action_entry.get("q01", [0.0, 0.0]), dtype=torch.float32)
    action_q99 = torch.tensor(action_entry.get("q99", [1.0, 1.0]), dtype=torch.float32)

    return ACTNormalizationStats(
        state_q01=state_q01, state_q99=state_q99,
        action_q01=action_q01, action_q99=action_q99,
    )

逐行解释：

stats.get("observation.state", {})：使用 .get() 提供默认空字典。如果 stats.json 的结构不完整，不会抛出 KeyError。
state_entry.get("q01", [0.0, 0.0])：每级都提供合理默认值。
这种多层防御性提取确保了系统在任何不完整的 stats.json 下都能正常运行。

6.1.7 `instantiate_model()` — 从检查点实例化模型

def instantiate_model(bundle: ACTCheckpointBundle, device: str) -> "ACTModel":
    from policies.models.act.modeling_act import ACTModel as PyACTModel

    model = PyACTModel(bundle.model_config)
    model.load_state_dict(bundle.state_dict)
    if bundle.inference_latent_mu is not None and bundle.inference_latent_log_sigma is not None:
        model.set_inference_latent(bundle.inference_latent_mu, bundle.inference_latent_log_sigma)
    model = model.to(device)
    model.eval()
    return model

逐行解释：

from policies.models.act.modeling_act import ACTModel as PyACTModel：延迟导入 + 重命名。延迟导入避免了循环依赖（checkpoint.py 和 modeling_act.py 之间）。重命名为 PyACTModel 是为了与可能的其他 ACTModel 引用区分。
model.load_state_dict(bundle.state_dict)：将权重加载到模型中。PyTorch 的 load_state_dict 使用严格的键名匹配（strict=True 默认）——如果保存的权重中有一个键在模型中不存在，会抛出错误。这是一种安全特性：避免"静默加载了不完整的权重"。
model.set_inference_latent(mu, log_sigma)：设置 CVAE 推理分布。这是在加载后的关键步骤——如果没有这一步，即使训练了 CVAE，推理时也用噪声采样。
model.eval()：设为评估模式（关闭 Dropout 等）。

6.2 preprocess.py — 预处理

6.2.1 ACTPreprocessor 类的初始化

class ACTPreprocessor:
    def __init__(self):
        self.image_transform = transforms.Compose([
            transforms.Resize((224, 224)),
            transforms.ToTensor(),
            transforms.Normalize(mean=[0.485, 0.456, 0.406], std=[0.229, 0.224, 0.225]),
        ])

逐行解释：

transforms.Compose([...])：torchvision 的图像预处理管道。Compose 按顺序应用列表中的每个变换。
transforms.Resize((224, 224))：缩放图像到 ResNet18 的标准输入尺寸。默认使用双线性插值（InterpolationMode.BILINEAR）。
transforms.ToTensor()：将 PIL Image (uint8, [0, 255]) 转换为 float tensor ([0.0, 1.0])，同时自动将通道顺序从 [H, W, C] 转为 [C, H, W]。
transforms.Normalize(mean=[0.485, 0.456, 0.406], std=[0.229, 0.224, 0.225])：ImageNet 均值/标准差归一化。注意这与 ACTDataset 中的手动归一化逻辑等价，但使用 torchvision 的标准化实现更简洁。

6.2.2 `process_image()` — 图像处理

def process_image(self, image_input: Union[str, Image.Image, None], device: str) -> torch.Tensor:
    if image_input is None:
        return torch.randn(1, 1, 3, 224, 224).to(device)

逐行解释：

输入为 None（无图像）时：创建随机噪声张量 [1, 1, 3, 224, 224]。形状解释：
- dim 0 = batch = 1
- dim 1 = num_cameras = 1
- dim 2-4 = 3×224×224 (RGB 图像) 使用随机噪声是故障恢复策略——至少能产生一些输出（可能是随机的），比整个系统崩溃好。

    try:
        if isinstance(image_input, str):
            if "," in image_input:
                image_input = image_input.split(",")[1]
            image_data = base64.b64decode(image_input)
            image = Image.open(io.BytesIO(image_data)).convert("RGB")
        elif isinstance(image_input, Image.Image):
            image = image_input.convert("RGB")
        else:
            return torch.randn(1, 1, 3, 224, 224).to(device)
        return self.image_transform(image).unsqueeze(0).unsqueeze(0).to(device)
    except Exception:
        return torch.randn(1, 1, 3, 224, 224).to(device)

逐行解释：

if "," in image_input: image_input = image_input.split(",")[1]：处理 Data URL 格式 data:image/png;base64,iVBORw0...。, 之后的部分才是 base64 编码图像数据。
base64.b64decode(image_input)：Base64 解码。将 ASCII 字符串转换回二进制数据。
io.BytesIO(image_data)：将 bytes 包装为内存中的文件对象，Image.open() 可以从这个虚拟文件读取。
image.convert("RGB")：统一转换为 RGB 格式（处理 RGBA、灰度等）。
.unsqueeze(0).unsqueeze(0)：两次 unsqueeze。第一次添加 batch 维度，第二次添加 camera 维度。最终形状：[1, 1, 3, 224, 224]。
try/except Exception：捕获所有可能的图像处理错误（损坏的数据、不支持的格式等），回退到随机噪声。这在生产环境中很重要——一个坏图像不应该拖垮整个推理 pipeline。

6.2.3 `normalize_state()` — 状态归一化

def normalize_state(self, state: list, stats: ACTNormalizationStats, device: str) -> torch.Tensor:
    """使用 QUANTILES 归一化状态：2 * (x - q01) / (q99 - q01) - 1"""
    state_tensor = torch.tensor(state[:2], dtype=torch.float32).unsqueeze(0).to(device)
    q01 = stats.state_q01.unsqueeze(0).to(device)
    q99 = stats.state_q99.unsqueeze(0).to(device)
    denom = q99 - q01
    denom = torch.where(denom == 0, torch.tensor(1e-8, device=device), denom)
    return 2 * (state_tensor - q01) / denom - 1

逐行解释：

state[:2]：只取前 2 个维度。这与 AKA-Sim2Real 的实际状态空间 [vel_left, vel_right] 对应。如果传入的状态列表更长（如包含夹爪状态），多余的维度被截断。
.unsqueeze(0)：添加 batch 维度 [2] → [1, 2]。
归一化公式与 ACTDataset 中的完全一致（见第 2 讲）。

6.2.4 `denormalize_action()` — 动作反归一化

def denormalize_action(self, action: torch.Tensor, stats: ACTNormalizationStats, device: str) -> torch.Tensor:
    """使用 QUANTILES 反归一化动作：(x + 1) / 2 * (q99 - q01) + q01"""
    q01 = stats.action_q01.unsqueeze(0).to(device)
    q99 = stats.action_q99.unsqueeze(0).to(device)
    denom = q99 - q01
    denom = torch.where(denom == 0, torch.tensor(1e-8, device=device), denom)
    return (action + 1) / 2 * denom + q01

逐行解释：

反归一化公式是归一化公式的逆运算：归一化：$x' = 2 \cdot \frac{x - q_{01}}{q_{99} - q_{01}} - 1$ 反归一化：$x = \frac{x' + 1}{2} \cdot (q_{99} - q_{01}) + q_{01}$
推导验证：
- 当 $x' = -1$：$x = 0/2 * \Delta + q_{01} = q_{01}$ ✓（最小值映射回 1% 分位）
- 当 $x' = 1$：$x = 2/2 * \Delta + q_{01} = q_{99}$ ✓（最大值映射回 99% 分位）

6.3 execution.py — 时序集成策略

@dataclass
class TemporalEnsemblingPolicy:
    """Online action-chunk fusion policy for ACT inference."""
    decay: float = 0.5
    step: int = 0
    predictions: Deque[dict] = field(default_factory=deque)

逐行解释：

decay = 0.5：一种简化版 Temporal Ensembling 的衰减系数（与 ACTTemporalEnsembler 不同，这里用幂律衰减 $0.5^{rel_idx}$）。
step = 0：当前推理步数（自 episode 开始以来的计数）。
predictions: Deque[dict]：双端队列，存储历史预测。每个预测是一个 dict：{"start_step": int, "actions": Tensor[k, d_a]}。

注意：这个类TemporalEnsemblingPolicy与modeling_act.py中的ACTTemporalEnsembler是两个独立的实现。execution.py中的版本更简单（幂律衰减权重），适用于快速原型；modeling_act.py中的版本更精确（指数加权），对齐了 LeRobot。当前 runtime 使用的是 ACTTemporalEnsembler，这个类作为备选方案保留。

def reset(self):
    self.step = 0
    self.predictions.clear()

解释：在 episode 开始时调用，清除所有历史预测。不清除的话，上一个 episode 的动作会污染新 episode 的起始动作。

def blend(self, action_chunk: torch.Tensor) -> torch.Tensor:
    if action_chunk.ndim != 3 or action_chunk.shape[0] != 1:
        return action_chunk

逐行解释：

防御性检查：只处理 [1, k, d_a] 格式的 chunk。如果格式不对，直接返回原值（不做融合）。

    current_step = self.step
    chunk_size = action_chunk.shape[1]
    prediction = action_chunk[0].detach().clone().cpu()
    self.predictions.append({
        "start_step": current_step,
        "actions": prediction,
    })

逐行解释：

.detach().clone().cpu()：断开计算图 + 深拷贝 + 移回 CPU。存储在 predictions 中的张量需要在 GPU 推理循环之外持久化，移到 CPU 避免 GPU 内存泄漏。

    min_valid_start = current_step - chunk_size + 1
    while self.predictions and self.predictions[0]["start_step"] < min_valid_start:
        self.predictions.popleft()

逐行解释：

min_valid_start = current_step - chunk_size + 1：有效窗口的边界。只有 start_step >= current_step - k + 1 的预测才可能覆盖当前步。例如 k=8，当前步=20：只有 start_step ≥ 13 的预测能覆盖步 20。
while ... popleft()：从左侧移除过期的预测。双端队列使得 O(1) 的左侧弹出效率。

    candidates = []
    weights = []
    for pred in self.predictions:
        rel_idx = current_step - pred["start_step"]
        if 0 <= rel_idx < pred["actions"].shape[0]:
            weight = self.decay ** rel_idx
            candidates.append(pred["actions"][rel_idx])
            weights.append(weight)

逐行解释：

rel_idx = current_step - pred["start_step"]：相对索引——"这个预测产生于多少步之前"。rel_idx=0 表示"刚刚产生的预测"。
0 <= rel_idx < chunk_size：确保当前步骤在预测的有效范围内。
weight = self.decay ** rel_idx：幂律衰减权重。例如 decay=0.5：
- rel_idx=0（最新）：$w = 0.5^0 = 1.0$
- rel_idx=1：$w = 0.5^1 = 0.5$
- rel_idx=7（最旧）：$w = 0.5^7 \approx 0.008$
这比标准 Temporal Ensembling 更激进地偏向最新预测（衰减更快）。

    if candidates:
        weight_tensor = torch.tensor(weights, dtype=prediction.dtype).unsqueeze(1)
        candidate_tensor = torch.stack(candidates, dim=0)
        blended = (candidate_tensor * weight_tensor).sum(dim=0) / weight_tensor.sum()
        action_chunk = action_chunk.clone()
        action_chunk[0, 0] = blended.to(action_chunk.device)

逐行解释：

加权平均公式： $$a_{blended} = \frac{\sum_{j} w_j \cdot a_j}{\sum_{j} w_j}$$
action_chunk[0, 0] = blended.to(action_chunk.device)：只替换 chunk 的第一个动作（当前步），其他位置保持不变。

    self.step += 1
    return action_chunk

解释：步数递增，返回融合后的动作 chunk。调用方（get_action）会取 [:, 0] 获取当前动作。

6.4 runtime.py — 推理运行时

6.4.1 ACTInferenceRuntime 类

class ACTInferenceRuntime:
    def __init__(self):
        self.model: Optional["ACTModel"] = None
        self.device = get_default_device()
        self.stats = ACTNormalizationStats()
        self.preprocessor = ACTPreprocessor()
        self.temporal_ensembler: Optional[ACTTemporalEnsembler] = None
        self.action_chunk_size = 8

逐行解释：

运行时是一个有状态对象，持有模型、设备、预处理器的引用。
model: Optional["ACTModel"] = None：初始时模型未加载。类型注解 Optional 表示可以为 None。
self.stats = ACTNormalizationStats()：默认统计量（q01=0, q99=1），在 _load_stats 中被覆盖。
self.action_chunk_size = 8：默认值，在 load_model 时从配置中更新。

6.4.2 `load_model()` — 模型加载与组装

def load_model(self, model_path=None, stats_dir=None):
    logger.info("加载 ACT 模型...")
    self.device = get_default_device()
    bundle = load_checkpoint_bundle(model_path, self.device)
    self.model = instantiate_model(bundle, self.device)
    self.reset_inference_context()
    self._load_stats(stats_dir)

逐行解释：

顺序：设备选择 → 加载检查点 → 实例化模型 → 重置推理上下文 → 加载归一化统计。每个步骤依赖上一步的结果。

    if self.should_use_temporal_ensembling():
        self.action_chunk_size = getattr(self.model.config, "action_chunk_size", 8)
        coeff = float(getattr(self.model.config, "temporal_ensembling_coeff", 0.01))
        self.temporal_ensembler = ACTTemporalEnsembler(
            temporal_ensemble_coeff=coeff, chunk_size=self.action_chunk_size,
        )

逐行解释：

getattr(self.model.config, "action_chunk_size", 8)：安全获取属性，不存在时默认为 8。
float(...)：显式类型转换——确保系数是 float 类型（配置中可能是 int 0）。
初始化 ACTTemporalEnsembler（而非 TemporalEnsemblingPolicy）——使用 LeRobot 对齐的实现。

6.4.3 `infer()` — 单步推理

def infer(self, state: list, image=None, user_id=None) -> list:
    if self.model is None:
        logger.warning(f"ACT 模型未加载，返回随机动作")
        return [[0.0, 0.0]]

逐行解释：

模型未加载时返回 [[0.0, 0.0]]（零动作）。这比抛异常更好——UI 层可以继续运行并提示用户加载模型。

    with torch.no_grad():
        state_tensor = self.preprocessor.normalize_state(state, self.stats, self.device)
        image_tensor = self.process_image(image)

        use_temporal = self.should_use_temporal_ensembling()
        action = self.model.get_action(
            image_tensor, state_tensor,
            use_temporal_ensembling=use_temporal,
            temporal_ensembler=self.temporal_ensembler,
        )

逐行解释：

torch.no_grad()：推理不需要梯度——节省内存和计算。
normalize_state(...)：原始状态 → [-1, 1] 归一化状态。
process_image(...)：原始图像 → 归一化 tensor。
model.get_action(...)：参见第 4 讲 4.5.6 节。

        action = self.preprocessor.denormalize_action(action, self.stats, self.device)
        return action[0].cpu().tolist()

逐行解释：

denormalize_action(action, ...)：将 [-1, 1] 模型输出映射回实际动作范围。
.cpu()：将 tensor 从 GPU 移回 CPU。
.tolist()：PyTorch tensor → Python list。例如 tensor([0.5, -0.3]) → [0.5, -0.3]。这是 API 响应所需的格式。

6.4.4 单例模式

_runtime = ACTInferenceRuntime()

def get_act_runtime() -> ACTInferenceRuntime:
    return _runtime

逐行解释：

_runtime：模块级变量（模块单例）。Python 的模块只在第一次 import 时初始化——所有后续 import 都共享同一个 _runtime 实例。
这种模式确保整个应用中只有一个模型加载在内存中——避免多次加载模型的巨大内存浪费（每个模型实例 ~100MB+）。
为什么不直接用全局变量而包装在函数中？函数提供了依赖注入（DI）的可能性——未来可以替换为不同的运行时实例而不影响调用方。

def load_act_model(model_path=None, stats_dir=None):
    return _runtime.load_model(model_path=model_path, stats_dir=stats_dir)

def act_inference(state, image=None, user_id=None):
    return _runtime.infer(state, image, user_id)

逐行解释：

这些是便利函数（convenience functions）——让调用方可以直接 import load_act_model 而不需要先 get_act_runtime()。
这种模式在 FastAPI 架构中很常见——API 路由通过简单的函数调用就能触发推理，而不需要管理运行时对象的生命周期。

6.5 完整推理数据流

回顾一次完整推理的数据流动：

用户输入（HTTP/WebSocket）
    │
    ▼
┌─────────────────────────────────────────────────────┐
│ ACTPreprocessor                                     │
│                                                     │
│  state: [0.5, -0.3]  ──► normalize_state()  ──► [-0.2, 0.1] │
│  image: "base64..."   ──► process_image()   ──► [1,1,3,224,224] │
└─────────────────────────────────────────────────────┘
    │
    ▼
┌─────────────────────────────────────────────────────┐
│ ACTModel.get_action()                               │
│                                                     │
│  forward() → { action: [B, k, d_a], ... }          │
│                                                     │
│  if temporal_ensemble:                              │
│    ACTTemporalEnsembler.update() → [B, d_a]        │
│  else:                                              │
│    action[:, 0] → [B, d_a]                         │
└─────────────────────────────────────────────────────┘
    │
    ▼
┌─────────────────────────────────────────────────────┐
│ ACTPreprocessor                                     │
│                                                     │
│  denormalize_action() → [0.3, -0.5]                │
└─────────────────────────────────────────────────────┘
    │
    ▼
返回给控制器/模拟器

课后思考

为什么 process_image() 在异常时返回随机噪声而不是 None 或抛出异常？这种容错策略的利弊是什么？
模块级单例 _runtime 在多线程环境（FastAPI 的 async workers）中是否安全？需要考虑什么并发问题？
normalize_state() 中 state[:2] 的硬编码有什么风险？如何改进？

ACT 模型架构

ACT（Action Chunking Transformer）是一种基于 Transformer 的模仿学习策略，通过预测**动作块（action chunk）**而非单步动作来实现平滑、稳定的控制输出。

整体架构图

                     ┌──────────────────────────────────────┐
                     │           ACTModel                   │
                     │                                      │
  图像输入            │  ┌──────────────┐                   │
  (H×W×3) ─────────▶│  │  RGBEncoder   │──▶ 视觉特征 (D)   │
                     │  │  (ResNet18)   │                   │
                     │  └──────────────┘         │          │
                     │                           │          │
  状态输入            │  ┌──────────────┐         │          │
  [vel_l, vel_r] ──▶│  │ StateEncoder  │──▶ 状态特征 (D)   │
                     │  │    (MLP)      │         │          │
                     │  └──────────────┘         │          │
                     │                           ▼          │
  ┌──────────┐       │               ┌────────────────┐     │
  │   CVAE   │──────▶│               │ Transformer    │     │
  │ (训练时)  │  z   │               │ Encoder-Decoder│     │
  └──────────┘       │               └────────┬───────┘     │
                     │                        │             │
                     │                        ▼             │
                     │               ┌────────────────┐     │
                     │               │  动作预测头     │     │
                     │               │  (Linear)      │     │
                     │               └────────────────┘     │
                     │                        │             │
                     └────────────────────────┼─────────────┘
                                              │
                                              ▼
                              action_chunk [chunk_size, action_dim]
                              （默认 8 步 × 2 维）

核心组件

1. RGBEncoder（视觉编码器）

骨干网络：ResNet18（ImageNet 预训练）
输入：归一化后的 RGB 图像
输出：展平的视觉特征向量，投影到 hidden_dim（默认 512）
图像预处理：resize → ImageNet 均值/方差归一化

2. StateEncoder（状态编码器）

结构：简单 MLP（多层感知机）
输入：归一化后的车辆状态 [vel_left, vel_right]（维度 2）
输出：状态特征向量（维度 hidden_dim）

3. CVAE（条件变分自编码器）

CVAE 在训练阶段引入潜变量 z，为动作预测提供多模态表达能力。

训练时：
  encoder: (观测, 动作序列) → (μ, σ) → z ~ N(μ, σ²)

推理时：
  z ~ N(latent_mean, latent_std)  # 从训练分布中采样
  或 z = 0                        # 使用零向量（确定性推理）

KL 散度损失约束潜变量分布接近标准正态分布：

KL Loss = KL(N(μ, σ²) ∥ N(0, I))

4. Transformer Encoder-Decoder

参数	默认值
`hidden_dim`	512
`num_attention_heads`	8
`num_encoder_layers`	4
`num_decoder_layers`	4
`dim_feedforward`	3200

编码器：将视觉特征 + 状态特征 + CVAE 潜变量拼接，经多层自注意力编码为上下文表示
解码器：以可学习的动作查询（action queries）为输入，通过交叉注意力从编码器上下文中提取信息，输出 action_chunk_size 个动作向量

5. 动作预测头

线性层将 Transformer 解码器的输出投影到动作空间（维度 action_dim=2），输出归一化后的动作块。

动作块（Action Chunking）

ACT 的核心创新之一是不预测单步动作，而是一次性预测 action_chunk_size 步的动作序列（默认 8 步）。

优势：

缓解复合误差（compound error）
提升长序列动作的时间一致性
减少控制器与环境之间的高频交互需求

执行策略：推理时每次只执行 action chunk 的第一步动作（或使用时序集成），下一帧重新预测。

时序集成（Temporal Ensembling）

时序集成通过加权融合多个历史 action chunk 的预测，进一步平滑控制输出。

当前动作 = α × 当前 chunk[0] + (1-α) × 历史融合动作

权重衰减系数 α（temporal_ensembling_weight，默认 0.5）控制历史信息的保留程度：

值越大：跟随最新预测，响应速度快
值越小：保留更多历史，控制更平滑

代码层次结构

policies/models/act/
├── modeling_act.py         # ACTModel、RGBEncoder、StateEncoder、CVAE、Transformer 层
├── configuration_act.py    # ACTConfig 数据类
├── defaults.py             # DEFAULT_ACT_CONFIG 与 build_act_config()
└── train_act.py            # 独立训练脚本

backend/services/inference/
├── checkpoint.py           # 检查点加载、模型实例化、统计量读取
├── preprocess.py           # 图像预处理、状态/动作归一化与反归一化
├── execution.py            # TemporalEnsemblingPolicy（时序集成）
└── runtime.py              # ACTInferenceRuntime（推理运行时装配）

训练损失汇总

损失项	权重	说明
L1 重建损失	1.0	预测动作块与真实动作的 L1 误差
KL 散度	0.1（`kl_weight`）	CVAE 潜变量正则化

总损失 = L1_loss + 0.1 × KL_loss

参考资料

UI 文档

本节介绍 AKA-Sim2Real 前端的架构、页面和组件。

页面概览

页面	路由	用途
SimPage	`/`	模拟器视角，用于数据采集与模拟推理
RealPage	`/real`	真实小车控制接口

技术栈

框架: React 19 + TypeScript
路由: React Router DOM 7
状态管理: Zustand
样式: Tailwind CSS 4
HTTP 客户端: Ky
实时通信: Socket.IO Client
构建工具: Vite 7

目录结构

ui/src/
├── main.tsx              # 应用入口
├── App.tsx               # 根组件，路由配置
├── index.css             # Tailwind 入口
├── api/
│   ├── api.ts            # REST API 客户端
│   ├── socket.ts         # Socket.IO 客户端工厂
│   └── realCar.ts        # 真实小车 HTTP API
├── models/
│   └── types.ts          # 共享 TypeScript 类型
├── stores/
│   └── simCarStore.ts    # Zustand 状态管理
└── pages/
    ├── SimPage/          # 模拟器页面
    ├── RealPage/         # 真实小车页面
    └── NotFound.tsx      # 404 页面

核心概念

页面详解

SimPage

路由: /

模拟器主页面，用于数据采集与模型推理测试。

布局结构

┌─────────────────────────────────────────────────────────────┐
│                      SimPage                                │
├───────────────────────────────┬─────────────────────────────┤
│                               │       RightPanel             │
│        TopDownView            │  ┌─────────────────────┐    │
│       (俯视画布)              │  │   FirstPersonView   │    │
│                               │  │   (第一视角)          │    │
│   [小车站 + 障碍物 + 地图]    │  └─────────────────────┘    │
│                               │  ┌─────────────────────┐    │
│                               │  │    LogConsole       │    │
│                               │  │   (日志控制台)       │    │
├───────────────────────────────┴─────────────────────────────┤
│                   TrainingControl + InferenceControl         │
└─────────────────────────────────────────────────────────────┘

核心功能

键盘控制: WASD/方向键控制小车运动
数据采集: 录制驾驶演示数据
训练控制: 启动/停止模型训练
推理测试: 加载模型进行模拟推理
实时日志: 显示后端运行日志

Socket.IO 事件

事件名	方向	说明
`action`	emit	发送动作指令
`car_state_update`	listen	接收小车状态更新
`collect_data`	emit	发送采集数据
`training_progress`	listen	接收训练进度
`act_infer_result`	listen	接收推理结果

RealPage

路由: /real

真实小车控制页面，用于连接和控制物理机器人。

布局结构

┌─────────────────────────────────────────────────────────────┐
│                      RealPage                                │
├─────────────────────────────────┬─────────────────────────────┤
│                                 │        RealRightPanel       │
│       RealCameraView            │  ┌─────────────────────┐    │
│      (浏览器摄像头)              │  │     Car IP 配置      │    │
│                                 │  │   连接状态显示        │    │
│                                 │  │   电机状态显示        │    │
├─────────────────────────────────┴─────────────────────────────┤
│                      控制面板                                  │
│     [连接] [前进] [后退] [左转] [右转] [停止]                   │
└─────────────────────────────────────────────────────────────┘

核心功能

摄像头访问: 使用浏览器 API 获取小车摄像头画面
IP 配置: 设置小车 IP 地址
电机控制: HTTP 请求控制小车电机
状态监控: 显示心跳和电机状态

API 通信

真实小车通过 HTTP API 直接通信：

// 发送心跳
carHeartbeat(carIP)

// 获取电机状态
motorStatusAt(carIP)

// 直接控制电机
motorDirect(carIP, leftVel, rightVel)

// 小车整体控制
carControl(carIP, action)

组件详解

SimPage 组件

TopDownView

俯视画布组件，展示小车、障碍物和地图。

文件: pages/SimPage/TopDownView.tsx

功能:

Canvas 绑定的 2D 地图渲染
键盘事件监听（WASD/方向键）
小车位置和角度显示
障碍物绘制

状态依赖: 监听 simCarStore 获取小车状态

RightPanel

右侧面板容器，包含第一视角视图和日志控制台。

文件: pages/SimPage/RightPanel.tsx

子组件:

FirstPersonView（第一视角渲染）
LogConsole（日志显示）

InferenceControl

推理控制面板。

文件: pages/SimPage/InferenceControl.tsx

功能:

加载训练好的模型
单步推理测试
自动推理模式

TrainingControl

训练和数据采集控制面板。

文件: pages/SimPage/TrainingControl.tsx

功能:

Episode 管理（开始/结束/保存）
数据采集开关
FPS 配置
训练启动/停止

LogConsole

实时日志显示组件。

文件: pages/SimPage/LogConsole.tsx

功能:

Socket.IO log_message 事件监听
自动滚动到底部
日志级别过滤（可选）

RealPage 组件

RealCameraView

浏览器摄像头访问组件。

文件: pages/RealPage/RealCameraView.tsx

功能:

navigator.mediaDevices.getUserMedia 获取视频流
设备选择下拉框
视频预览显示

RealRightPanel

真实小车右侧状态面板。

文件: pages/RealPage/RealRightPanel.tsx

功能:

小车 IP 输入
连接状态指示
电机状态显示

共享组件

actionMapping

键盘按键到动作的映射工具。

文件: pages/SimPage/actionMapping.ts

映射关系:

按键	动作
W / ↑	forward
S / ↓	backward
A / ←	turn_left
D / →	turn_right
Q	forward_left
E	forward_right
Z	backward_left
C	backward_right
Space	stop

状态管理

Zustand Store

使用 Zustand 进行轻量级状态管理。

simCarStore

文件: stores/simCarStore.ts

管理模拟器小车状态。

interface CarState {
  x: number;        // 小车 X 坐标
  y: number;        // 小车 Y 坐标
  angle: number;   // 小车角度 (弧度)
  vel_left: number;  // 左轮速度
  vel_right: number; // 右轮速度
}

const initialState: CarState = {
  x: 400,
  y: 300,
  angle: -Math.PI / 2,
  vel_left: 0,
  vel_right: 0,
};

Actions

Action	说明
`setCarState(state)`	更新小车状态
`resetCarState()`	重置为初始状态

页面级 State

除全局 Store 外，各页面使用 useState 管理本地状态。

SimPage

State	类型	说明
`isRecording`	boolean	是否正在录制
`episodeCount`	number	Episode 数量
`trainingStatus`	string	训练状态
`inferenceResult`	object	推理结果

RealPage

State	类型	说明
`carIP`	string	小车 IP 地址
`isConnected`	boolean	连接状态
`cameraDevices`	MediaDeviceInfo[]	摄像头设备列表
`motorStatus`	object	电机状态

API 通信

REST API

使用 Ky HTTP 客户端与后端通信。

基础配置: api/api.ts

const api = ky.create({
  prefixUrl: '/api',
  headers: { 'Content-Type': 'application/json' },
});

端点列表

方法	路径	说明
POST	`dataset/collect`	采集训练图片数据
POST	`train`	启动模型训练
POST	`train/stop`	停止训练
POST	`act/load_trained`	加载训练好的模型

Socket.IO

实时双向通信，用于模拟器状态同步和日志推送。

Socket 工厂

文件: api/socket.ts

createSocket(namespace: string): Socket

创建命名空间的 Socket 实例：

实例	命名空间	用途
`simSocket`	`/sim`	模拟器状态同步
`realSocket`	`/real`	真实小车控制

事件列表

Emit 事件 (前端 → 后端)

事件	数据格式	说明
`action`	`{ action: string }`	发送动作指令
`reset_car_state`	-	重置小车状态
`get_car_state`	-	请求当前状态
`collect_data`	`{ image: string, state: object }`	采集数据
`start_episode`	`{ episodeId: number }`	开始 Episode
`end_episode`	`{ episodeId: number }`	结束 Episode
`finalize_episode`	`{ episodeId: number }`	保存 Episode
`act_infer`	`{ image: string, state: object }`	推理请求

Listen 事件 (后端 → 前端)

事件	数据格式	说明
`connected`	-	连接成功
`car_state_update`	`CarState`	小车状态更新
`collection_count`	`{ count: number }`	采集数量
`episode_info`	`EpisodeInfo`	Episode 信息
`training_progress`	`{ epoch: number, loss: number }`	训练进度
`act_infer_result`	`{ action: string }`	推理结果
`log_message`	`{ level: string, message: string }`	日志消息

真实小车 HTTP API

文件: api/realCar.ts

直接向小车 IP 发送 HTTP 请求。

接口列表

函数	HTTP 方法	路径	说明
`carHeartbeat`	GET	`/api/heartbeat`	发送心跳
`motorStatusAt`	GET	`/api/motor/status`	获取电机状态
`motorDirect`	POST	`/api/motor/direct`	直接控制电机
`carControl`	POST	`/api/car/control`	小车整体控制
`carTimeSync`	GET	`/api/time/sync`	时间同步

使用示例

import { carControl, motorDirect } from '@/api/realCar';

// 控制小车前进
await carControl(carIP, 'forward');

// 直接控制电机速度
await motorDirect(carIP, 100, 100);

MuJoCo

MuJoCo（Multi-Joint dynamics with Contact）是一个高效的物理仿真引擎，广泛用于机器人学和强化学习研究。

学习资源

MuJoCo 官方教程 — 推荐的学习指南

文档

安装指南

安装指南

本指南旨在帮助不同系统的开发者快速搭建 MuJoCo 虚拟仿真环境，重点针对 Linux (Debian/WSL) 进行了优化。

如果使用强化学习,推荐安装如下pip包

# 提供强化学习(RL)的标准化环境接口
pip install gymnasium

# 录制视频或制作 GIF 动图
pip install imageio

# 处理庞大的矩阵和数组数学运算
pip install numpy

# 深度学习框架
pip install torch

Linux 安装 (Debian/WSL)

安装miniconda

打开 Linux 系统终端，依次运行以下命令进行静默安装：

# 1. 创建 miniconda 安装文件夹
mkdir -p ~/miniconda3

# 2. 从官方源下载最新的 Linux 安装包 (大概 140MB，稍微等进度条跑完)
wget https://repo.anaconda.com/miniconda/Miniconda3-latest-Linux-x86_64.sh -O ~/miniconda3/miniconda.sh

# 3. 执行静默安装 (-b 代表后台静默，-u 代表更新，-p 指定绝对路径)
bash ~/miniconda3/miniconda.sh -b -u -p ~/miniconda3

# 4. 安装完后，把刚才下载的安装包删掉，节约空间！(可选)
rm ~/miniconda3/miniconda.sh

# 5. 初始化 conda，让你的终端认识 conda 命令
~/miniconda3/bin/conda init bash

激活并初始化 Conda

为了让刚才的配置立刻生效，需要刷新当前终端

source ~/.bashrc

验证成功标志：终端命令行最左边出现 (base) 前缀！

接受协议与创建Python环境

重要提示：由于新版 Conda 的合规要求，第一次下载包前需要接受服务条款，否则会触发报错。

miniconda_bug

请执行以下命令接受 Anaconda 官方服务条款

conda tos accept

miniconda_bug

创建一个独立的 Python 3.10 环境：

# 创建一个名叫 mujoco_env 的环境，并指定 python 版本为 3.10
conda create -n mujoco_env python=3.10 -y

激活环境

# 激活进入专属环境
conda activate mujoco_env

安装 MuJoCo

# 安装 MuJoCo 引擎
pip install mujoco

代码测试

创建 test_mujoco.py 文件

import mujoco
import mujoco.viewer

model = mujoco.MjModel.from_xml_string("""
<mujoco>
  <worldbody>
    <body>
      <geom type="box" size=".1 .1 .1" rgba="1 0 0 1"/>
    </body>
  </worldbody>
</mujoco>
""")

data = mujoco.MjData(model)

with mujoco.viewer.launch_passive(model, data) as viewer:
    while viewer.is_running():
        mujoco.mj_step(model, data)
        viewer.sync()

通过python运行

python test_mujoco.py

若弹出包含红色方块的 3D 软件窗口，则表示安装成功

linux_init

温馨提示：建议第4步下载速度慢，可配置清华镜像源：

pip config set global.index-url https://pypi.tuna.tsinghua.edu.cn/simple

Macos 安装

安装miniconda

要安装miniconda，请按照官方安装指南

创建干净的 Python 环境

conda create -n mujoco python=3.10
conda activate mujoco

安装 MuJoCo

pip install mujoco

用代码测试

创建 test_mujoco.py 文件

import mujoco
import mujoco.viewer

model = mujoco.MjModel.from_xml_string("""
<mujoco>
  <worldbody>
    <body>
      <geom type="sphere" size="0.1"/>
    </body>
  </worldbody>
</mujoco>
""")

data = mujoco.MjData(model)

with mujoco.viewer.launch_passive(model, data) as viewer:
    while viewer.is_running():
        mujoco.mj_step(model, data)
        viewer.sync()

通过mjpython运行

mjpython test_mujoco.py

弹出软件窗口为安装成功

mac_init

Windows 安装

Windows 用户有两种选择：快速查看或 Python 开发环境（推荐两者都做）

A. 快速查看 (免配置)

下载并解压 MuJoCo点击下载MuJoCo 3.5.0 Windows 压缩包

sha256校验：mujoco-3.5.0-windows-x86_64.zip.sha256 877d0dfbceac3de90a874c41e0f20c568d104e8ca19de955c0482e3b63832519

解压后双击 bin/simulate.exe

windows_init

双击上图的simulate.exe后，会同时打开两个窗口
注意：这里的shell窗口不要关闭（最小化即可）

windows_init1

鼠标拖拽，mujoco-3.5.0-windows-x86_64/model/humanoid/humanoid.xml ，下的文件到MuJoCo窗口，即可查看效果

windows_init2

B. 开发环境 (推荐使用PyCharm + Miniconda使用)

安装环境

安装 Miniconda：前往官方下载

安装 PyCharm：前往官方下载

创建环境：

在Anaconda Powershell Prompt 或 Anaconda Prompt 中执行以下命令：

conda create -n mujoco_env python=3.10 -y
conda activate mujoco_env

安装库：

pip install mujoco
pip install gymnasium
pip install imageio

在PyCharm环境下运行代码测试

创建 test_mujoco.py 文件

import mujoco
import mujoco.viewer

model = mujoco.MjModel.from_xml_string("""
<mujoco>
  <worldbody>
    <body>
      <geom type="box" size=".1 .1 .1" rgba="1 1 1 1"/>
    </body>
  </worldbody>
</mujoco>
""")

data = mujoco.MjData(model)

with mujoco.viewer.launch_passive(model, data) as viewer:
    while viewer.is_running():
        mujoco.mj_step(model, data)
        viewer.sync()

在PyCharm环境下运行 python test_mujoco.py，弹出软件窗口为安装成功

windows_init3

No.1 第一个 MuJoCo 仿真

本节通过一个最小示例，帮助你快速上手 MuJoCo XML 建模与 Python 仿真脚本。

文件说明

本节的示例文件位于 mujoco/No_1/ 目录下：

mujoco/No_1/
├── hello.xml   # MuJoCo XML 场景描述文件
└── no_1.py     # Python 仿真脚本

hello.xml 详解

完整内容

<mujoco>
    <!-- 全局仿真选项：重力加速度 -->
    <option gravity="0 0 -9.81"/>

    <!-- 编译器设置：角度单位使用弧度 -->
    <compiler angle="radian"/>

    <!-- 视觉渲染设置 -->
    <visual>
        <headlight ambient="0.1 0.1 0.1"/>
    </visual>

    <!-- 资产定义：可复用的材质 -->
    <asset>
        <material name="white" rgba="1 1 1 1"/>
    </asset>

    <!-- worldbody: 物理世界中的所有物体 -->
    <worldbody>
        <!-- 场景光源 -->
        <light diffuse=".5 .5 .5" pos="0 0 3" dir="0 0 -1"/>

        <!-- 地面（红色平面） -->
        <geom type="plane" size="1 1 0.5" rgba=".9 0 0 1"/>

        <!-- 物体 1：白色盒子，z=1 -->
        <body pos="0 0 1" euler="0 0 0">
            <joint type="free"/>
            <inertial pos="0 0 0" mass="1" diaginertia="0.01 0.01 0.01"/>
            <geom type="box" size=".1 .2 .3" material="white"/>
        </body>

        <!-- 物体 2：青色盒子，z=2，pitch=90°（侧立） -->
        <body pos="0 0 2" euler="0 90 0">
            <joint type="free"/>
            <inertial pos="0 0 0" mass="1" diaginertia="0.01 0.01 0.01"/>
            <geom type="box" size=".1 .2 .3" rgba="0 .9 .9 1"/>
        </body>

        <!-- 物体 3：灰色球体，z=3 -->
        <body pos="0 0 3" euler="0 0 0">
            <joint type="free"/>
            <inertial pos="0 0 0" mass="1" diaginertia="0.01 0.01 0.01"/>
            <geom type="sphere" size=".1" rgba=".5 .5 .5 1"/>
        </body>
    </worldbody>
</mujoco>

元素说明

`<option>`

全局仿真选项。

gravity="0 0 -9.81"：标准重力加速度 9.81 m/s²（向下）

`<compiler>`

编译器设置。

angle="radian"：角度单位使用弧度（默认是角度），这样 euler 值可以直接写数值如 90 表示 90°

`<visual>`

渲染视觉设置。

headlight：场景主光源的亮度

`<asset>`

可复用的资产定义。

material：定义可复用的材质（name="white"，rgba 白色），在 geom 中通过 material="white" 引用

`<worldbody>`

物理世界的根容器，包含所有物体。

`<light>`

场景光源。

diffuse：漫反射颜色（灰白色）
pos：光源位置（上方 3 米）
dir：光照方向（沿 -z，即向下）

`<geom>` 地面

type="plane"：平面
size="1 1 0.5"：半尺寸（x=1, y=1, z=0.5 → 全尺寸 2×2×1 米）
rgba=".9 0 0 1"：红色

`<body>`

刚体，可内嵌关节、惯性、几何体。

属性	说明
`pos`	初始位置（世界坐标系）
`euler`	初始欧拉角（roll pitch yaw，弧度）

三个刚体的配置：

物体	pos	euler	geom 类型	颜色
物体 1	z=1	0 0 0（水平）	box	白色（material）
物体 2	z=2	0 90 0（侧立）	box	青色
物体 3	z=3	0 0 0	sphere	灰色

`<joint>`

关节，连接 body 与父级（或世界）。

type="free"：自由关节，物体不受任何约束，可在 6 个自由度上自由运动（平移 + 旋转）

`<inertial>`

刚体的质量分布特性。

属性	说明
`pos`	质心在 body 本地坐标系中的位置
`mass`	质量（kg）
`diaginertia`	惯性张量的三个主对角元素（Ixx, Iyy, Izz）

提示：如果不手动指定 inertial，MuJoCo 会根据 geom 的形状和默认密度自动计算。

`<geom>` 物体几何体

type：形状类型，支持 box、sphere、cylinder、capsule、plane、ellipsoid 等
size：半尺寸向量
- box：size="0.1 0.2 0.3" → 全尺寸 0.2×0.4×0.6 米
- sphere：size="0.1" → 半径 0.1 米
rgba：颜色 + 透明度（RGBA，各通道 0~1）
material：引用 <asset> 中定义的材质（优先于 rgba）

no_1.py 详解

import mujoco
import mujoco.viewer

# 1. 从 XML 文件加载模型
model = mujoco.MjModel.from_xml_path('hello.xml')

# 2. 创建仿真数据容器
data = mujoco.MjData(model)

# 3. 启动交互式查看器
with mujoco.viewer.launch_passive(model, data) as viewer:
    # 4. 主循环：每帧推进一次仿真
    while viewer.is_running():
        mujoco.mj_step(model, data)   # 执行一步仿真（默认 dt=0.002s）
        viewer.sync()                  # 同步查看器显示

步骤	说明
`MjModel.from_xml_path()`	解析 XML 构建物理模型
`MjData`	存储仿真运行时数据（位置、速度、力等）
`mj_step()`	推进一个仿真时间步
`viewer.sync()`	将仿真状态同步到可视化窗口

运行方法

在 mujoco/No_1/ 目录下执行：

mjpython no_1.py

运行效果：三个物体同时从不同高度自由下落，落到红色地面上后弹起/静止。

常见错误

1. `ValueError: XML Error: Schema violation: unrecognized element`

原因：XML 中有拼写错误的标签名，如 <gemo>、<intertial>、<muhoco>。

解决：检查并修正标签拼写，确认是 <geom>、<inertial>、<mujoco>。

2. 物体没有出现或直接穿透地面

原因：inertial 未指定且 geom 没有定义时，MuJoCo 可能使用了零质量。

解决：确保每个 body 下都有 <inertial mass="..."/> 或让 geom 的密度足够大。

3. `mjpython: command not found`

原因：mujoco 包未正确安装，或 mjpython 不在 PATH 中。

解决：

pip install mujoco
# 或直接用 python 运行
python no_1.py

No.2 交互式仿真与鼠标控制

本节介绍如何使用 GLFW 窗口创建交互式 3D 仿真，包括鼠标视角控制和控制器回调。

文件说明

本节的示例文件位于 mujoco/No_2/temp_mjcpy/ 目录下：

mujoco/No_2/temp_mjcpy/
├── ball.xml              # MuJoCo XML 场景描述文件
├── projectile.py         # 基础交互式仿真脚本
└── template_mujoco.py    # 带详细注释的模板脚本

ball.xml 详解

一个简单的弹球场景：

<mujoco>
  <worldbody>
    <!-- 场景光源 -->
    <light diffuse=".5 .5 .5" pos="0 0 3" dir="0 0 -1"/>

    <!-- 红色平面地面 -->
    <geom type="plane" size="10 1 0.1" rgba=".9 0 0 1"/>

    <!-- 绿色小球，初始位置 z=1 -->
    <body pos="0 0 1">
      <joint type="free"/>
      <geom type="sphere" size=".1" rgba="0 .9 0 1"/>
    </body>
  </worldbody>
</mujoco>

元素	说明
`plane`	平面地面，半尺寸 10×1×0.1
`sphere`	绿色小球，半径 0.1

template_mujoco.py 详解

核心结构

import mujoco as mj
from mujoco.glfw import glfw
import numpy as np
import os

# 1. 模型加载
model = mj.MjModel.from_xml_path(xml_path)
data = mj.MjData(model)
cam = mj.MjvCamera()      # 视角相机
opt = mj.MjvOption()      # 可视化选项

# 2. GLFW 窗口初始化
glfw.init()
window = glfw.create_window(1200, 900, "Demo", None, None)
glfw.make_context_current(window)
glfw.swap_interval(1)

# 3. 可视化数据结构
scene = mj.MjvScene(model, maxgeom=10000)
context = mj.MjrContext(model, mj.mjtFontScale.mjFONTSCALE_150.value)

# 4. 注册回调函数
glfw.set_key_callback(window, keyboard)
glfw.set_cursor_pos_callback(window, mouse_move)
glfw.set_mouse_button_callback(window, mouse_button)
glfw.set_scroll_callback(window, scroll)
mj.set_mjcb_control(controller)

# 5. 主循环
while not glfw.window_should_close(window):
    mj.mj_step(model, data)
    # 渲染...
    glfw.poll_events()

glfw.terminate()

回调函数

controller

每步仿真前调用的控制回调，可写入外力：

def controller(model, data):
    """控制回调，每 mj_step 前自动调用"""
    pass

在 projectile.py 中的示例实现了空气阻力：

def controller(model, data):
    vx, vy, vz = data.qvel[0], data.qvel[1], data.qvel[2]
    v = np.sqrt(vx**2 + vy**2 + vz**2)
    c = 1.0  # 阻力系数
    data.qfrc_applied[0] = -c * v * vx
    data.qfrc_applied[1] = -c * v * vy
    data.qfrc_applied[2] = -c * v * vz

keyboard

键盘事件处理：

def keyboard(window, key, scancode, act, mods):
    if act == glfw.PRESS and key == glfw.KEY_BACKSPACE:
        mj.mj_resetData(model, data)
        mj.mj_forward(model, data)

按键	动作
Backspace	重置仿真到初始状态

mouse_move

鼠标拖动改变视角：

组合	动作
左键拖动	旋转视角
右键拖动	移动视角
中键拖动	缩放
Shift + 拖动	切换交互模式

scroll

滚轮缩放视角。

交互操作

视角控制

操作	功能
左键拖动	旋转视角（水平/垂直）
右键拖动	移动视角
滚轮	缩放
Shift + 拖动	切换模式

键盘

按键	功能
Backspace	重置仿真

初始条件设置

# 设置小球初始位置
data.qpos[2] = 0.1

# 设置小球初速度 (vx=2, vy=0, vz=5)
data.qvel[0] = 2.0
data.qvel[2] = 5.0

# 设置相机视角
cam.azimuth = 90.0
cam.distance = 8.0
cam.elevation = -45.0

运行方法

在 mujoco/No_2/temp_mjcpy/ 目录下执行：

python projectile.py
# 或带注释模板
python template_mujoco.py

运行效果：绿色弹球以初速度 (2, 0, 5) 抛出，受重力下落并受空气阻力影响。

与 No.1 的区别

特性	No.1 (Passive)	No.2 (GLFW)
窗口	`mujoco.viewer` 被动窗口	GLFW 主动创建窗口
视角控制	受限	鼠标自由控制
控制回调	无	`mj.set_mjcb_control`
渲染控制	自动同步	手动调用 `mjr_render`
帧率控制	自动	手动控制循环频率

No.3 单摆控制仿真

本节介绍倒立摆（Inverted Pendulum）的 MuJoCo 建模与闭环控制，包括关节电机、传感器配置，以及两种控制模式（力矩模式 vs 伺服模式）。

文件说明

本节的示例文件位于 mujoco/No_3/ 目录下：

mujoco/No_3/
├── no_3.py              # 最小主脚本（使用 viewer.launch_passive）
├── control_pendulum.py   # 完整交互脚本（使用 GLFW）
└── pendulum.xml         # MuJoCo XML 模型文件

一、pendulum.xml 详解（对比 No.2 的 ball.xml）

No.3 pendulum.xml 完整代码

<mujoco>
    <!-- 全局仿真选项：重力加速度 -->
    <option gravity="0 0 -9.81">
    </option>

    <worldbody>
        <!-- 场景光源 -->
        <light diffuse=".5 .5 .5" pos="0 0 3" dir="0 0 -1"/>

        <!-- 红色平面地面 -->
        <geom type="plane" size="1 1 0.1" rgba=".9 0 0 1"/>

        <!-- 摆杆 body：固定在 (0, 0, 2)，绕 y 轴旋转 -->
        <body pos="0 0 2" euler="0 180 0">
            <!--
                hinge 关节：约束为只能绕一个轴旋转
                对比 No.2：No.2 使用 free 关节（6 自由度）
                          No.3 使用 hinge 关节（1 自由度）
            -->
            <joint name="pin" type="hinge" axis="0 -1 0" pos="0 0 0.5"/>
            <!-- 绿色圆柱几何体，半径 0.05，长度 0.5，质量 1 -->
            <geom type="cylinder" size=".05 .5" rgba="0 .9 0 1" mass="1"/>
        </body>
    </worldbody>

    <!--
        actuator 驱动器：【No.3 新增】
        对比 No.2：No.2 没有 actuator，只有纯物理仿真
                  No.3 新增了 3 种驱动器用于闭环控制
    -->
    <actuator>
        <!-- 力矩电机：直接控制关节力矩 -->
        <motor joint="pin" name="torque" gear="1" ctrllimited="true" ctrlrange="-100 100"/>
        <!-- 位置伺服：使用 PD 控制跟踪目标位置 -->
        <position name="position_servo" joint="pin" kp="10"/>
        <!-- 速度伺服：使用 PD 控制跟踪目标速度 -->
        <velocity name="velocity_servo" joint="pin" kv="0"/>
    </actuator>

    <!--
        sensor 传感器：【No.3 新增】
        对比 No.2：No.2 没有 sensor
                  No.3 新增了位置和速度传感器用于闭环反馈
    -->
    <sensor>
        <!-- 关节位置传感器，带噪声 -->
        <jointpos joint="pin" noise="0.2"/>
        <!-- 关节速度传感器，带噪声 -->
        <jointvel joint="pin" noise="1"/>
    </sensor>
</mujoco>

No.2 ball.xml 完整代码（对比参考）

<mujoco>
    <worldbody>
        <!-- 场景光源 -->
        <light diffuse=".5 .5 .5" pos="0 0 3" dir="0 0 -1"/>

        <!-- 红色平面地面 -->
        <geom type="plane" size="10 1 0.1" rgba=".9 0 0 1"/>

        <!-- 绿色小球，初始位置 z=1 -->
        <body pos="0 0 1">
            <!-- free 关节：6 自由度，可自由平移和旋转 -->
            <joint type="free"/>
            <geom type="sphere" size=".1" rgba="0 .9 0 1"/>
        </body>
    </worldbody>
    <!-- No.2 没有 actuator 和 sensor -->
</mujoco>

XML 配置对比表

配置项	No.2 (ball.xml)	No.3 (pendulum.xml)
关节类型	`free`（6 自由度）	`hinge`（1 自由度）
关节名称	无	`name="pin"`
几何体	`sphere`（球）	`cylinder`（圆柱）
actuator	无	3 个（motor、position、velocity）
sensor	无	2 个（jointpos、jointvel）
mass 定义	无（自动计算）	`mass="1"` 显式指定

关节类型详解

关节类型	自由度	适用场景	No.2	No.3
`free`	6	自由运动物体（球、无人机）	✅	❌
`hinge`	1	旋转关节（摆、门）	❌	✅
`slide`	1	滑动关节（活塞）	-	-
`ball`	3	球形关节（机械臂）	-	-

二、no_3.py 详解（最小脚本）

完整代码

import mujoco
import mujoco.viewer
import time

# 加载 XML 模型
model = mujoco.MjModel.from_xml_path('pendulum.xml')
data = mujoco.MjData(model)

# 使用 viewer.launch_passive 启动被动查看器
# 对比 No.2：No.2 的 template_mujoco.py 使用完整 GLFW 窗口（170 行）
#           No.3 的 no_3.py 使用简化 viewer（仅 12 行核心代码）
with mujoco.viewer.launch_passive(model, data) as viewer:
    while viewer.is_running():
        mujoco.mj_step(model, data)
        viewer.sync()
        time.sleep(1/500)  # ~60 Hz real-time，约 500ms 休眠实现实时仿真

核心 API 对比

API	No.1 (no_1.py)	No.3 (no_3.py)
模型加载	`MjModel.from_xml_path()`	`MjModel.from_xml_path()`
可视化	`viewer.launch_passive()`	`viewer.launch_passive()`
仿真步进	`mj_step()`	`mj_step()`
同步方式	`viewer.sync()`	`viewer.sync()`
主循环条件	`viewer.is_running()`	`viewer.is_running()`
帧率控制	无	`time.sleep(1/500)`

三、control_pendulum.py 详解（完整脚本）

完整代码

import mujoco as mj
from mujoco.glfw import glfw
import numpy as np
import os

# XML 模型文件路径
xml_path = 'pendulum.xml'
# 仿真结束时间（秒），simend=5 表示仿真运行 5 秒后自动停止
simend = 5

# ============================================================
# 鼠标状态变量（用于鼠标拖动交互）
# 【与 No.2 完全相同】
# ============================================================
button_left = False   # 鼠标左键是否按下
button_middle = False # 鼠标中键是否按下
button_right = False  # 鼠标右键是否按下
lastx = 0             # 上一次鼠标 x 位置
lasty = 0             # 上一次鼠标 y 位置

# ============================================================
# controller 回调：每一步仿真前调用，可在此写入控制指令
# 【核心差异】No.2 的 controller 为空 pass，No.3 实现 PD 控制
# ============================================================
def controller(model, data):
    """
    PD 控制器实现

    对比 No.2：
    - No.2 的 controller 为空 pass，无任何控制逻辑
    - No.3 的 controller 根据 actuator_type 实现两种控制模式
    """
    if actuator_type == "torque":
        # 力矩模式：直接写入控制量
        # 对比 No.2：No.2 没有 actuator_gainprm 配置
        model.actuator_gainprm[0, 0] = 1
        # PD 控制：ctrl = -kp * pos_error - kv * vel_error
        data.ctrl[0] = -10 * \
            (data.sensordata[0] - 0.0) - \
            1 * (data.sensordata[1] - 0.0)
    elif actuator_type == "servo":
        # 伺服模式：配置增益参数
        kp = 10.0
        model.actuator_gainprm[1, 0] = kp
        model.actuator_biasprm[1, 1] = -kp
        data.ctrl[1] = -0.5

        kv = 1.0
        model.actuator_gainprm[2, 0] = kv
        model.actuator_biasprm[2, 2] = -kv
        data.ctrl[2] = 0.0

# ============================================================
# keyboard 回调：键盘事件处理
# 【与 No.2 完全相同】
# ============================================================
def keyboard(window, key, scancode, act, mods):
    if act == glfw.PRESS and key == glfw.KEY_BACKSPACE:
        mj.mj_resetData(model, data)
        mj.mj_forward(model, data)

# ============================================================
# mouse_button 回调：记录鼠标按键状态
# 【与 No.2 完全相同】
# ============================================================
def mouse_button(window, button, act, mods):
    global button_left, button_middle, button_right
    button_left = (glfw.get_mouse_button(window, glfw.MOUSE_BUTTON_LEFT) == glfw.PRESS)
    button_middle = (glfw.get_mouse_button(window, glfw.MOUSE_BUTTON_MIDDLE) == glfw.PRESS)
    button_right = (glfw.get_mouse_button(window, glfw.MOUSE_BUTTON_RIGHT) == glfw.PRESS)

# ============================================================
# mouse_move 回调：鼠标拖动改变视角
# 【与 No.2 完全相同】
# ============================================================
def mouse_move(window, xpos, ypos):
    global lastx, lasty, button_left, button_middle, button_right
    dx = xpos - lastx
    dy = ypos - lasty
    lastx = xpos
    lasty = ypos

    if not button_left and not button_middle and not button_right:
        return

    width, height = glfw.get_window_size(window)
    mod_shift = (glfw.get_key(window, glfw.KEY_LEFT_SHIFT) == glfw.PRESS or
                 glfw.get_key(window, glfw.KEY_RIGHT_SHIFT) == glfw.PRESS)

    if button_right:
        action = mj.mjtMouse.mjMOUSE_MOVE_H if mod_shift else mj.mjtMouse.mjMOUSE_MOVE_V
    elif button_left:
        action = mj.mjtMouse.mjMOUSE_ROTATE_H if mod_shift else mj.mjtMouse.mjMOUSE_ROTATE_V
    else:
        action = mj.mjtMouse.mjMOUSE_ZOOM

    mj.mjv_moveCamera(model, action, dx/height, dy/height, scene, cam)

# ============================================================
# scroll 回调：滚轮缩放视角
# 【与 No.2 完全相同】
# ============================================================
def scroll(window, xoffset, yoffset):
    mj.mjv_moveCamera(model, mj.mjtMouse.mjMOUSE_ZOOM, 0.0, -0.05*yoffset, scene, cam)

# ============================================================
# 模型加载
# 【与 No.2 完全相同】
# ============================================================
dirname = os.path.dirname(os.path.abspath(__file__))
abspath = os.path.join(dirname, xml_path)

model = mj.MjModel.from_xml_path(abspath)
data = mj.MjData(model)
cam = mj.MjvCamera()
opt = mj.MjvOption()

# ============================================================
# GLFW 初始化和窗口创建
# 【与 No.2 完全相同】
# ============================================================
glfw.init()
window = glfw.create_window(1200, 900, "Demo", None, None)
glfw.make_context_current(window)
glfw.swap_interval(1)

# ============================================================
# MuJoCo 可视化数据结构初始化
# 【与 No.2 完全相同】
# ============================================================
mj.mjv_defaultCamera(cam)
mj.mjv_defaultOption(opt)
scene = mj.MjvScene(model, maxgeom=10000)
context = mj.MjrContext(model, mj.mjtFontScale.mjFONTSCALE_150.value)

# ============================================================
# 注册 GLFW 回调和 MuJoCo 控制回调
# 【与 No.2 完全相同】
# ============================================================
glfw.set_key_callback(window, keyboard)
glfw.set_cursor_pos_callback(window, mouse_move)
glfw.set_mouse_button_callback(window, mouse_button)
glfw.set_scroll_callback(window, scroll)
mj.set_mjcb_control(controller)

# ============================================================
# 初始条件设置
# 【核心差异】
# No.2：data.qvel[0] = 2.0（设置初速度）
# No.3：data.qpos[0] = np.pi/2（设置初始角度）
# ============================================================
data.qpos[0] = np.pi/2  # 摆杆初始角度 90°（朝上位置）

# 设置相机视角
cam.azimuth = 90.0
cam.distance = 5.0
cam.elevation = -5
cam.lookat = np.array([0.012768, -0.000000, 1.254336])

# ============================================================
# 主仿真循环
# 【与 No.2 完全相同】
# ============================================================
while not glfw.window_should_close(window):
    simstart = data.time

    while (data.time - simstart < 1.0/60.0):
        mj.mj_step(model, data)

    if (data.time >= simend):
        break

    # 获取窗口帧缓冲区大小
    viewport_width, viewport_height = glfw.get_framebuffer_size(window)
    viewport = mj.MjrRect(0, 0, viewport_width, viewport_height)

    # 更新场景并渲染
    mj.mjv_updateScene(model, data, opt, None, cam,
                       mj.mjtCatBit.mjCAT_ALL.value, scene)
    mj.mjr_render(viewport, scene, context)

    # 交换 OpenGL 缓冲区
    glfw.swap_buffers(window)
    # 处理 GUI 事件
    glfw.poll_events()

glfw.terminate()

控制模式详解

模式 1：力矩模式（torque）

model.actuator_gainprm[0, 0] = 1
data.ctrl[0] = -10 * (data.sensordata[0] - 0.0) - 1 * (data.sensordata[1] - 0.0)

参数	说明	来源
`sensordata[0]`	关节位置（rad）	`<sensor><jointpos>`
`sensordata[1]`	关节速度（rad/s）	`<sensor><jointvel>`
`kp=10`	位置增益	手动设置
`kv=1`	速度增益	手动设置
目标位置	0.0	手动设置

模式 2：伺服模式（servo）

kp = 10.0
model.actuator_gainprm[1, 0] = kp
model.actuator_biasprm[1, 1] = -kp
data.ctrl[1] = -0.5  # 目标位置

使用 XML 中定义的 position_servo，通过配置增益参数实现 PD 控制。

四、No.2 与 No.3 完整代码对比

代码结构对比

模块	No.2 (template_mujoco.py)	No.3 (control_pendulum.py)
import	✅	✅
全局变量（鼠标状态）	✅	✅
controller 回调	`pass`（空）	PD 控制器
keyboard 回调	✅	✅
mouse_button 回调	✅	✅
mouse_move 回调	✅	✅
scroll 回调	✅	✅
模型加载	✅	✅
GLFW 初始化	✅	✅
可视化结构	✅	✅
回调注册	✅	✅
初始条件	`qvel` 设置速度	`qpos` 设置角度
主循环	✅	✅
glfw.terminate	✅	✅

关键差异代码片段

1. 初始条件设置

No.2（template_mujoco.py）：

# No initial velocity in template, but in projectile.py:
data.qvel[0] = 2.0  # vx
data.qvel[2] = 5.0  # vz

No.3（control_pendulum.py）：

data.qpos[0] = np.pi/2  # 摆杆初始角度 90°（朝上）

2. 控制回调

No.2（template_mujoco.py）：

def controller(model, data):
    """控制回调，每 mj_step 前自动调用"""
    pass  # 无任何控制逻辑

No.3（control_pendulum.py）：

actuator_type = "torque"  # 或 "servo"

def controller(model, data):
    if actuator_type == "torque":
        model.actuator_gainprm[0, 0] = 1
        data.ctrl[0] = -10 * (data.sensordata[0] - 0.0) - 1 * (data.sensordata[1] - 0.0)
    elif actuator_type == "servo":
        kp = 10.0
        model.actuator_gainprm[1, 0] = kp
        model.actuator_biasprm[1, 1] = -kp
        data.ctrl[1] = -0.5

五、运行方法

在 mujoco/No_3/ 目录下执行：

# 最小脚本（仅可视化，无控制）
python no_3.py

# 完整脚本（带控制）
python control_pendulum.py

运行效果：

no_3.py：摆杆从 90° 位置自由摆动（无控制），仅做可视化
control_pendulum.py：摆杆在 PD 控制下稳定在目标位置（0 rad）

六、与 No.2 的整体对比总结

功能特性对比

特性	No.2 (弹球)	No.3 (倒立摆)
模型类型	弹球 + 地面	单摆 + 地面
关节类型	`free`（6 自由度）	`hinge`（1 自由度）
驱动器	无	3 个（motor、position、velocity）
传感器	无	2 个（jointpos、jointvel）
控制方式	无（被动仿真）	力矩控制 + 伺服控制
初始状态设置	`data.qvel`（速度）	`data.qpos`（角度）
主脚本行数	170 行（GLFW）	12 行（viewer）/ 171 行（GLFW）
控制器复杂度	无	PD 控制实现

学习路径

No.1: 基础建模 + viewer 可视化（被动窗口）
  ↓
No.2: GLFW 窗口 + 鼠标交互 + 回调机制 + 弹球物理
  ↓
No.3: 关节控制 + 传感器读取 + 闭环控制 + 倒立摆

代码复用情况

代码模块	No.2 → No.3
鼠标状态变量	完全相同
keyboard 回调	完全相同
mouse_button 回调	完全相同
mouse_move 回调	完全相同
scroll 回调	完全相同
GLFW 初始化	完全相同
可视化结构	完全相同
回调注册	完全相同
主循环	完全相同
controller 回调	完全不同（空 → PD）
初始条件	完全不同（速度 → 角度）
XML 模型	完全不同（弹球 → 倒立摆）

七、常见问题

1. 摆杆直接穿透地面

原因：初始位置或关节配置错误。

解决：检查 euler="0 180 0" 确保摆杆朝下，检查 joint axis 方向。

2. 控制不稳定

原因：

比例增益（kp）过大
缺少重力补偿
传感器噪声过大

解决：调整 kp、kv 参数，或启用重力补偿。

3. `AttributeError: 'NoneType' object has no attribute '...'`

原因：GLFW 窗口未正确创建。

解决：glfw.create_window() 返回 None 时，检查显示器配置或降低分辨率。

No.4 双摆控制仿真

本节介绍双摆（Double Pendulum）的 MuJoCo 建模与反馈线性化控制，包括多体系统层级结构、RK4 积分器、以及基于动力学模型的控制器设计。

文件说明

本节的示例文件位于 mujoco/No_4/ 目录下：

mujoco/No_4/
├── no_4.py              # 最小主脚本（使用 viewer.launch_passive）
├── double_pendulum.py   # 完整交互脚本（使用 GLFW）
├── doublependulum.xml   # MuJoCo XML 模型文件
└── template_mujoco.py   # GLFW 模板（参考）

一、doublependulum.xml 详解（对比 No.3 的 pendulum.xml）

No.4 doublependulum.xml 完整代码

<mujoco>
    <!--
        全局仿真选项：【No.4 新增】
        - timestep：仿真步长 0.0001s（比默认 0.002 更精细）
        - integrator：使用 RK4（4阶龙格-库塔）积分器，精度更高
        对比 No.3：No.3 使用默认积分器（Euler）， timestep 0.002
    -->
    <option timestep="0.0001" integrator="RK4">
    </option>

    <worldbody>
        <!-- 场景光源 -->
        <light diffuse=".5 .5 .5" pos="0 0 3" dir="0 0 -1"/>

        <!-- 红色平面地面 -->
        <geom type="plane" size="1 1 0.1" rgba=".9 0 0 1"/>

        <!--
            第一连杆 body：固定在 (0, 0, 2.5)，绕 y 轴旋转
            对比 No.3：No.3 只有单个摆杆，No.4 有两个级联的摆杆
        -->
        <body pos="0 0 2.5" euler="0 0 0">
            <!--
                hinge 关节：约束为只能绕一个轴旋转
                注意：pos="0 0 -0.5" 表示关节在连杆的下端
            -->
            <joint name="pin" type="hinge" axis="0 -1 0" pos="0 0 -0.5"/>
            <!-- 绿色圆柱几何体，半径 0.05，长度 0.5，质量 1 -->
            <geom type="cylinder" size="0.05 0.5" rgba="0 .9 0 1" mass="1"/>

            <!--
                第二连杆 body：【No.4 新增】
                嵌套在第一连杆内部，形成层级结构
                位置 pos="0 0.1 1" 相对于父 body
            -->
            <body pos="0 0.1 1" euler="0 0 0">
                <!-- 第二关节 -->
                <joint name="pin2" type="hinge" axis="0 -1 0" pos="0 0 -0.5"/>
                <!-- 蓝色圆柱几何体 -->
                <geom type="cylinder" size="0.05 0.5" rgba="0 0 0.9 1" mass="1"/>
            </body>
        </body>
    </worldbody>
    <!-- No.4 没有 actuator 和 sensor，使用 qfrc_applied 直接施加力 -->
</mujoco>

No.3 pendulum.xml 完整代码（对比参考）

<mujoco>
    <option gravity="0 0 -9.81">
    </option>

    <worldbody>
        <light diffuse=".5 .5 .5" pos="0 0 3" dir="0 0 -1"/>
        <geom type="plane" size="1 1 0.1" rgba=".9 0 0 1"/>

        <!-- 摆杆 body -->
        <body pos="0 0 2" euler="0 180 0">
            <joint name="pin" type="hinge" axis="0 -1 0" pos="0 0 0.5"/>
            <geom type="cylinder" size=".05 .5" rgba="0 .9 0 1" mass="1"/>
        </body>
    </worldbody>

    <!-- actuator 驱动器 -->
    <actuator>
        <motor joint="pin" name="torque" gear="1" ctrllimited="true" ctrlrange="-100 100"/>
        <position name="position_servo" joint="pin" kp="10"/>
        <velocity name="velocity_servo" joint="pin" kv="0"/>
    </actuator>

    <!-- sensor 传感器 -->
    <sensor>
        <jointpos joint="pin" noise="0.2"/>
        <jointvel joint="pin" noise="1"/>
    </sensor>
</mujoco>

XML 配置对比表

配置项	No.3 (pendulum.xml)	No.4 (doublependulum.xml)
关节类型	`hinge`（1 个）	`hinge`（2 个，层级嵌套）
关节名称	`name="pin"`	`name="pin"`, `name="pin2"`
几何体	`cylinder`（单色）	`cylinder`（双色）
body 层级	单层	双层嵌套
actuator	3 个（motor、position、velocity）	无（使用 qfrc_applied）
sensor	2 个（jointpos、jointvel）	无
积分器	默认（Euler）	`RK4`（4阶龙格-库塔）
timestep	0.002	0.0001

积分器类型详解

积分器	精度	计算成本	适用场景	No.3	No.4
Euler	低	最低	快速预览	✅	❌
RK4	高	4 倍 Euler	高精度仿真	❌	✅
implicit	中	中	刚体系统	-	-

二、no_4.py 详解（最小脚本）

完整代码

import mujoco
import mujoco.viewer
import time

model = mujoco.MjModel.from_xml_path('ball.xml')  # 注：实际加载 ball.xml
data = mujoco.MjData(model)

with mujoco.viewer.launch_passive(model, data) as viewer:
    while viewer.is_running():
        mujoco.mj_step(model, data)
        viewer.sync()
        time.sleep(1/500)  # ~60 Hz real-time

核心 API 对比

API	No.3 (no_3.py)	No.4 (no_4.py)
模型加载	`MjModel.from_xml_path()`	`MjModel.from_xml_path()`
可视化	`viewer.launch_passive()`	`viewer.launch_passive()`
仿真步进	`mj_step()`	`mj_step()`
同步方式	`viewer.sync()`	`viewer.sync()`
主循环条件	`viewer.is_running()`	`viewer.is_running()`
帧率控制	`time.sleep(1/500)`	`time.sleep(1/500)`

注意：no_4.py 实际加载的是 ball.xml（单球模型），而非 doublependulum.xml。如需查看双摆，请使用 double_pendulum.py。

三、double_pendulum.py 详解（完整脚本）

完整代码

import mujoco as mj
from mujoco.glfw import glfw
import numpy as np
import os

xml_path = 'doublependulum.xml'
simend = 50  # 仿真时间 50 秒

# 鼠标状态变量
button_left = False
button_middle = False
button_right = False
lastx = 0
lasty = 0

def controller(model, data):
    """
    反馈线性化控制器（Feedback Linearization）

    对比 No.3：
    - No.3 使用简单的 PD 控制：ctrl = -kp * pos_error - kv * vel_error
    - No.4 使用反馈线性化，基于系统动力学模型
    """
    # 计算位置相关能量（势能）
    mj.mj_energyPos(model, data)

    # 计算速度相关能量（动能）
    mj.mj_energyVel(model, data)

    # 将稀疏惯性矩阵 M 转换为满矩阵
    M = np.zeros((2, 2))
    mj.mj_fullM(model, M, data.qM)

    # PD 增益和参考角度
    Kp = 100 * np.eye(2)  # 比例增益
    Kd = 10 * np.eye(2)   # 微分增益
    qref = np.array([[-0.5], [-1.6]])  # 目标角度（弧度）

    # f 补偿科里奥利力和重力
    f = data.qfrc_bias[:, np.newaxis]

    # τ = M * ddqref（反馈线性化公式）
    ddqref = Kp @ (qref - data.qpos[:2][:, np.newaxis]) + \
        Kd @ (0 - data.qvel[:2][:, np.newaxis])
    tau = M @ ddqref

    # 直接施加力到系统
    data.qfrc_applied = (tau + f)[:, 0]

反馈线性化控制器详解

动力学模型

双摆系统的动力学方程：

M(q) * qdd + C(q, qd) + g(q) = τ

其中：

M(q)：惯性矩阵（2x2）
C(q, qd)：科里奥利力和离心力
g(q)：重力
τ：控制力矩

控制律

ddqref = Kp @ (qref - q) + Kd @ (0 - qd)  # 伪加速度
tau = M @ ddqref                           # 力矩 = 惯性矩阵 × 加速度
data.qfrc_applied = tau + f                # 加上重力/科里奥利补偿

参数	说明	来源
`qref`	目标角度 `[-0.5, -1.6]` rad	手动设置
`Kp`	位置增益 `100 * I`	手动设置
`Kd`	速度增益 `10 * I`	手动设置
`M`	惯性矩阵	`mj_fullM()` 从 `data.qM` 计算
`f`	偏置力（重力+科里奥利）	`data.qfrc_bias`

与 No.3 PD 控制的对比

方面	No.3 PD 控制	No.4 反馈线性化
控制量计算	`ctrl = -kp * e - kv * ed`	`τ = M @ (Kpe + Kded)`
模型知识	无需	需要惯性矩阵 M
重力补偿	无	通过 `qfrc_bias` 补偿
科里奥利补偿	无	通过 `qfrc_bias` 补偿
控制精度	中等	高（理论上精确跟踪）

四、no_4.py 与 double_pendulum.py 对比

代码结构对比

模块	no_4.py	double_pendulum.py
import	✅	✅
全局变量（鼠标状态）	❌	✅
controller 回调	❌	✅（反馈线性化）
keyboard 回调	❌	✅
mouse_button 回调	❌	✅
mouse_move 回调	❌	✅
scroll 回调	❌	✅
模型加载	`ball.xml`	`doublependulum.xml`
GLFW 窗口	❌	✅
主循环	viewer 被动模式	GLFW 主循环

五、运行方法

在 mujoco/No_4/ 目录下执行：

# 最小脚本（仅可视化，实际加载的是 ball.xml）
python no_4.py

# 完整脚本（带反馈线性化控制）
python double_pendulum.py

运行效果：

double_pendulum.py：双摆在反馈线性化控制下稳定到目标角度 [-0.5, -1.6] rad

六、与 No.3 的整体对比总结

功能特性对比

特性	No.3 (单摆)	No.4 (双摆)
模型类型	单摆 + 地面	双摆 + 地面
关节数量	1	2
body 层级	单层	双层嵌套
积分器	Euler	RK4
timestep	0.002	0.0001
驱动器	actuator（motor、servo）	qfrc_applied
传感器	jointpos、jointvel	无
控制方式	PD 控制	反馈线性化
初始条件	`qpos[0] = np.pi/2`	`qpos[0] = 0.1`

学习路径

No.1: 基础建模 + viewer 可视化（被动窗口）
  ↓
No.2: GLFW 窗口 + 鼠标交互 + 回调机制 + 弹球物理
  ↓
No.3: 单摆关节控制 + 传感器读取 + PD 闭环控制
  ↓
No.4: 双摆层级结构 + RK4 积分器 + 反馈线性化控制

代码复用情况

代码模块	No.3 → No.4
鼠标状态变量	完全相同
keyboard 回调	完全相同
mouse_button 回调	完全相同
mouse_move 回调	完全相同
scroll 回调	完全相同
GLFW 初始化	完全相同
可视化结构	完全相同
回调注册	完全相同
主循环	完全相同
controller 回调	完全不同（PD → 反馈线性化）
初始条件	完全不同（单自由度 → 双自由度）
XML 模型	完全不同（单摆 → 双摆）

七、常见问题

1. 双摆运动不稳定

原因：

timestep 过大（RK4 需要更小的步长）
增益参数不合适

解决：No.4 已使用 timestep="0.0001" 和 RK4 积分器，如仍不稳定可进一步减小 timestep。

2. 初始位置不正确

原因：qpos[0] 只设置了第一个关节的角度。

解决：如需设置两个关节的初始角度：

data.qpos[0] = 0.1      # 第一关节
data.qpos[1] = 0.2      # 第二关节

3. 反馈线性化控制器不工作

原因：qfrc_bias 包含重力但控制器没有正确补偿。

解决：

data.qfrc_applied = (tau + f)[:, 0]  # 确保加上偏置力

4. `no_4.py` 显示的不是双摆

原因：no_4.py 加载的是 ball.xml 而非 doublependulum.xml。

解决：使用 double_pendulum.py 查看双摆模型。

No.5 双摆有限状态机（FSM）轨迹跟踪

本节介绍在 No.4 双摆模型的基础上，引入有限状态机（Finite State Machine, FSM） 实现多段轨迹跟踪。系统按时间在 HOLD → SWING1 → SWING2 → STOP 四个状态间切换，每个阶段跟踪一条三次多项式轨迹。

文件说明

本节的示例文件位于 mujoco/No_5/ 目录下：

mujoco/No_5/
├── no_5.py                    # 最小主脚本（使用 viewer.launch_passive）
├── doublependulum_fsm.py      # 完整交互脚本（使用 GLFW + FSM 控制器）
└── doublependulum_fsm.xml     # MuJoCo XML 模型文件

一、doublependulum_fsm.xml 详解（对比 No.4 的 doublependulum.xml）

No.5 doublependulum_fsm.xml 完整代码

<mujoco>
    <!--
        全局仿真选项：
        - timestep：0.0001s
        - integrator：RK4
        - flag：【No.5 新增】开启 energy/contact 监测
    -->
    <option timestep="0.0001" integrator="RK4" >
        <flag energy="enable" contact="enable" />
    </option>

    <worldbody>
        <light diffuse=".5 .5 .5" pos="0 0 3" dir="0 0 -1"/>
        <geom type="plane" size="1 1 0.1" rgba=".9 0 0 1"/>

        <!--
            第一连杆 body：
            - 位置 pos="0 0 1.25"（比 No.4 的 2.5 更低，留出向上摆动空间）
            - euler="0 180 0"：【No.5 新增】绕 X 轴翻转 180°，使摆杆初始朝下悬挂
        -->
        <body pos="0 0 1.25" euler="0 180 0">
            <joint name="pin" type="hinge" axis = "0 -1 0" pos="0 0 -0.5"/>
            <geom type="cylinder" size="0.05 0.5" rgba="0 .9 0 1" mass="1"/>

            <!--
                第二连杆 body：层级嵌套，结构与 No.4 相同
            -->
            <body pos="0 0.1 1" euler="0 0 0">
                <joint name="pin2" type="hinge" axis = "0 -1 0" pos="0 0 -0.5"/>
                <geom type="cylinder" size="0.05 0.5" rgba="0 0 .9 1" mass="1"/>
            </body>
        </body>
    </worldbody>

    <!--
        actuator 驱动器：【No.5 新增】
        - 两个 motor：通过 ctrl 直接施加力矩（替代 No.4 的 qfrc_applied）
        - position/velocity servo：预留接口（kp/kv=0，未启用）
    -->
    <actuator>
        <motor joint="pin"  name="torque"  gear="1" ctrllimited="true" ctrlrange="-100 100" />
        <position name="pservo1" joint="pin"  kp="0" />
        <velocity name="vservo1" joint="pin"  kv="0" />
        <motor joint="pin2" name="torque2" gear="1" ctrllimited="true" ctrlrange="-100 100" />
        <position name="pservo2" joint="pin2" kp="0" />
        <velocity name="vservo2" joint="pin2" kv="0" />
    </actuator>
</mujoco>

XML 配置对比表

配置项	No.4 (doublependulum.xml)	No.5 (doublependulum_fsm.xml)
关节	`hinge` × 2	`hinge` × 2（相同）
body 层级	双层嵌套	双层嵌套（相同）
初始 body 位置	`(0, 0, 2.5)`	`(0, 0, 1.25)`（更低）
初始 body 朝向	`euler="0 0 0"`	`euler="0 180 0"`（新增，翻转朝下）
积分器	RK4	RK4（相同）
timestep	0.0001	0.0001（相同）
`<flag>` 元素	无	新增 `energy` / `contact`
actuator	无	新增 2× motor + 4× servo
驱动方式	`qfrc_applied`	`data.ctrl`（通过 actuator）

关键变更说明

1. 高度降低 + 朝向翻转

No.5 把第一连杆固定点从 z=2.5 降到 z=1.25，并加上 euler="0 180 0"，让摆杆初始朝下悬挂。这是因为 FSM 任务是要让双摆向上甩到顶部（类似杂技「手倒立」），需要低悬挂点 + 朝下初始位形。

2. 引入 actuator

No.4 通过 data.qfrc_applied 直接施加广义力，不经过 actuator。No.5 改用 actuator 中的 motor + data.ctrl 通道，这是更标准的工业控制接口。

3. actuator 通道映射

actuator 名	关节	ctrl 索引	用途
`torque`	pin	`ctrl[0]`	第一个关节的力矩输入
`torque2`	pin2	`ctrl[3]`	第二个关节的力矩输入
`pservo1` / `vservo1`	pin	ctrl[1] / ctrl[2]	预留（kp=0, kv=0，未启用）
`pservo2` / `vservo2`	pin2	ctrl[4] / ctrl[5]	预留（kp=0, kv=0，未启用）

注意：motor 和 servo 共享同一关节的 ctrl 索引空间，所以 pin 的 motor 占 ctrl[0]，servo 占 ctrl[1]/[2]；pin2 的 motor 占 ctrl[3]，servo 占 ctrl[4]/[5]。控制器中需要先清空所有 6 个 ctrl 通道再赋值。

二、no_5.py 详解（最小脚本）

完整代码

import time

import mujoco
import mujoco.viewer

model = mujoco.MjModel.from_xml_path('doublependulum_fsm.xml')
data = mujoco.MjData(model)

with mujoco.viewer.launch_passive(model, data) as viewer:
    while viewer.is_running():
        mujoco.mj_step(model, data)
        viewer.sync()
        time.sleep(1e-3)

与 no_4.py 的核心差异

项	no_4.py	no_5.py
`import time` 位置	放在 `import mujoco` 之后	放在最顶部（PEP 8 标准）
加载的 XML	`ball.xml`（bug，本意是双摆）	`doublependulum_fsm.xml`（修正）
帧率控制	`time.sleep(1/500)` ≈ 2ms	`time.sleep(1e-3)` = 1ms
controller	❌	❌（仅观察模型，无控制）
初始条件	未设置	未设置（XML 默认下垂）

关键改进：no_4.py 加载的是 ball.xml（一个误用），而 no_5.py 正确加载了双摆模型。最小脚本只验证「模型能加载、能跑」，不展示 FSM 控制效果——要看 FSM 必须运行 doublependulum_fsm.py。

三、doublependulum_fsm.py 详解（完整脚本）

完整代码

import mujoco as mj
from mujoco.glfw import glfw
import numpy as np
import os

xml_path = 'doublependulum_fsm.xml'
simend = 5

# 时序参数（单位：秒）
t_hold   = 0.5   # 起始保持阶段
t_swing1 = 1.0   # 第一段上摆
t_swing2 = 1.0   # 第二段上摆

# FSM 状态枚举
FSM_HOLD   = 0
FSM_SWING1 = 1
FSM_SWING2 = 2
FSM_STOP   = 3

# 三段轨迹的目标点
q_init = np.array([[-1.0], [0.0]])    # 初始下垂
q_mid  = np.array([[ 0.5], [-2.0]])   # 中间姿态
q_end  = np.array([[ 1.0], [0.0]])    # 末端顶部倒立

t_init = t_hold
t_mid  = t_hold + t_swing1
t_end  = t_hold + t_swing1 + t_swing2

def init_controller(model, data):
    """初始化 FSM 状态，并预生成两段三次多项式轨迹。"""
    global fsm_state, a_swing1, a_swing2
    fsm_state = FSM_HOLD
    a_swing1 = generate_trajectory(t_init, t_mid, q_init, q_mid)
    a_swing2 = generate_trajectory(t_mid,  t_end, q_mid,  q_end)

def controller(model, data):
    """FSM + PD 轨迹跟踪控制器。"""
    global fsm_state, a_swing1, a_swing2
    time = data.time

    # 状态机转移（仅基于时间）
    if fsm_state == FSM_HOLD and time >= t_hold:
        fsm_state = FSM_SWING1
    elif fsm_state == FSM_SWING1 and time >= t_mid:
        fsm_state = FSM_SWING2
    elif fsm_state == FSM_SWING2 and time >= t_end:
        fsm_state = FSM_STOP

    # 各状态下的参考轨迹
    if fsm_state == FSM_HOLD:
        q_ref  = q_init
        dq_ref = np.zeros((2, 1))
    elif fsm_state == FSM_SWING1:
        a = a_swing1
        q_ref  = a[0] + a[1]*time + a[2]*time**2 + a[3]*time**3
        dq_ref = a[1] + 2*a[2]*time + 3*a[3]*time**2
    elif fsm_state == FSM_SWING2:
        a = a_swing2
        q_ref  = a[0] + a[1]*time + a[2]*time**2 + a[3]*time**3
        dq_ref = a[1] + 2*a[2]*time + 3*a[3]*time**2
    elif fsm_state == FSM_STOP:
        q_ref  = q_end
        dq_ref = np.zeros((2, 1))

    # PD 控制（增益比 No.4 大 5 倍）
    kp, kv = 500, 50
    torque = kp * (q_ref[:, 0] - data.qpos) + kv * (dq_ref[:, 0] - data.qvel)

    # 清空所有 6 个 ctrl 通道（motor+servo 共享索引空间）
    for i in range(6):
        data.ctrl[i] = 0
    # 写入两个 motor 通道
    data.ctrl[0] = torque[0]   # pin  → torque
    data.ctrl[3] = torque[1]   # pin2 → torque2

def generate_trajectory(t0, tf, q0, qf):
    """
    三次多项式轨迹：q(t) = a0 + a1*t + a2*t^2 + a3*t^3
    满足边界条件：q(t0)=q0, q(tf)=qf, dq(t0)=0, dq(tf)=0
    """
    tf_t0_3 = (tf - t0)**3
    a0 = (qf*(t0**2)*(3*tf - t0) + q0*(tf**2)*(tf - 3*t0)) / tf_t0_3
    a1 = (6*t0*tf*(q0 - qf)) / tf_t0_3
    a2 = (3*(t0 + tf)*(qf - q0)) / tf_t0_3
    a3 = (2*(q0 - qf)) / tf_t0_3
    return a0, a1, a2, a3

3.1 有限状态机（FSM）架构

时间 ─────────────────────────────────────────────────▶
  │                                                      
  ├─ t ∈ [0, 0.5)        ├─ [0.5, 1.5)  ├─ [1.5, 2.5)  ├─ [2.5, ∞)
  │   HOLD               │   SWING1     │   SWING2     │   STOP
  │   q_ref = q_init     │   三次曲线   │   三次曲线   │   q_ref = q_end
  │   dq_ref = 0         │   init→mid   │   mid→end    │   dq_ref = 0

状态转移条件：纯粹基于仿真时间 data.time，是一种时间驱动的确定性 FSM。

状态	时长	目标位姿	控制目标
`HOLD`	0.5s	`q_init = [-1, 0]` rad	保持下垂，等待起摆
`SWING1`	1.0s	沿三次曲线扫过 `q_init → q_mid`	第一段上摆
`SWING2`	1.0s	沿三次曲线扫过 `q_mid → q_end`	第二段上摆，到达顶部
`STOP`	永久	`q_end = [1, 0]` rad	保持顶部倒立姿态

3.2 三次多项式轨迹生成

目标：在 t0 时刻处于 q0、在 tf 时刻处于 qf，且起止速度均为 0。这是最小jerk风格的边界条件。

求解后得到 4 个系数 a0, a1, a2, a3（形状 (2, 1)，对应 2 个关节）：

q(t)   = a0 + a1·t + a2·t² + a3·t³
dq(t)  = a1 + 2·a2·t + 3·a3·t²

generate_trajectory() 在 init_controller 中预先计算好两段轨迹的系数，运行时只做多项式求值，避免每步反解线性方程组。

3.3 PD 控制器（与 No.4 对比）

控制器特性	No.4 (反馈线性化)	No.5 (PD + FSM)
控制律	`τ = M·ddqref + f`	`τ = kp·e + kv·ed`
增益 `kp`	`100·I`	500（5 倍）
增益 `kv`	`10·I`	50（5 倍）
需惯性矩阵 M	✅	❌
需补偿重力/科氏	✅（用 `qfrc_bias`）	❌（靠大 kp/kv 隐式抑制）
驱动接口	`data.qfrc_applied`	`data.ctrl`（actuator）
跟踪目标	固定位姿 `qref`	时变轨迹 `q(t), dq(t)`

设计权衡：No.4 用模型做精确补偿 + 较小增益；No.5 用高增益 PD 暴力跟踪，不依赖模型知识，更鲁棒于参数误差但控制信号更"硬"。

3.4 ctrl 通道的写入

for i in range(6):
    data.ctrl[i] = 0          # 清空（含预留的 servo 通道）
data.ctrl[0] = torque[0]      # pin 的 motor
data.ctrl[3] = torque[1]      # pin2 的 motor

由于 pin 的 motor 占 ctrl[0]，而 pin 的 position/velocity servo 占 ctrl[1]/[2]，所以必须全部清零再写 motor，否则 servo 通道的 kp=0 不会注入任何力，但保险起见仍清零。

四、no_5.py 与 doublependulum_fsm.py 对比

模块	no_5.py	doublependulum_fsm.py
import 风格	`import time` 在最顶	同
模型加载	`doublependulum_fsm.xml` ✅	同
`init_controller`	❌	✅（预生成轨迹）
FSM 状态变量	❌	✅
controller 回调	❌	✅（FSM + PD）
轨迹生成器	❌	✅（三次多项式）
keyboard 回调	❌	✅（Backspace 重置）
mouse_button 回调	❌	✅
mouse_move 回调	❌	✅
scroll 回调	❌	✅
GLFW 窗口	❌	✅（1200×900）
仿真时长	无限（手动关闭）	5 秒（`simend=5`）
帧率控制	`time.sleep(1e-3)`	内层 `1.0/60.0` 步进 + `glfw.swap_interval(1)`

注意：doublependulum_fsm.py 没有 no_5.py 中的 time.sleep(1e-3)，因为它用 GLFW 的 swap_interval(1)（v-sync）来限速在 60Hz。

五、运行方法

在 mujoco/No_5/ 目录下执行：

# 最小脚本（仅可视化双摆自由下落，没有 FSM 控制）
mjpython no_5.py

# 完整脚本（FSM + 轨迹跟踪，把双摆甩到顶部）
mjpython doublependulum_fsm.py

macOS 上必须用 mjpython 启动（含 MJPEG 编码器）；Linux/Windows 可用普通 python。

预期效果：

no_5.py：双摆从下垂自然摆动，没有控制输入。
doublependulum_fsm.py：
- t ∈ [0, 0.5)：保持下垂。
- t ∈ [0.5, 1.5)：第一关节大幅正向加速，把第二关节甩起。
- t ∈ [1.5, 2.5)：第二关节继续被驱动到顶部。
- t > 2.5：保持在 q_end = [1, 0] rad（顶部倒立）。

六、与 No.4 的整体对比总结

功能特性对比

特性	No.4 (双摆 + 反馈线性化)	No.5 (双摆 + FSM)
模型	双摆，下垂初始	双摆，下垂 + 翻转 180°
驱动接口	`qfrc_applied`	`data.ctrl`（actuator）
actuator	无	2× motor + 4× servo
控制目标	单点镇定	多段轨迹跟踪
控制器	反馈线性化	PD
状态机	❌	✅（4 状态）
轨迹生成	❌	三次多项式
模型知识需求	需 M(q)、qfrc_bias	无（高增益 PD）
增益	kp=100, kv=10	kp=500, kv=50
仿真时长	50s	5s
最小脚本	加载 `ball.xml`（bug）	加载正确 XML（修复）

控制思想对比

No.4 的「反馈线性化」思维：
   已知 M(q), g(q) → 算出补偿项 → 用 M⁻¹ 精确跟踪参考加速度
   优点：理论精度高、增益需求小
   缺点：依赖模型精度，参数不准时性能下降

No.5 的「FSM + 高增益 PD」思维：
   不知道精确模型 → 用大 kp/kv 暴力跟踪
   优点：鲁棒于参数误差、结构清晰、易扩展多阶段
   缺点：控制信号"硬"、可能激发未建模动力学

学习路径

No.1: 基础建模 + viewer 可视化（被动窗口）
  ↓
No.2: GLFW 窗口 + 鼠标交互 + 回调机制
  ↓
No.3: 单摆关节控制 + 传感器读取 + PD 闭环控制
  ↓
No.4: 双摆层级结构 + RK4 + 反馈线性化（单点镇定）
  ↓
No.5: 双摆 + actuator + FSM + 三次多项式轨迹（多段跟踪）  ← 当前
  ↓
（未来）No.6: 接触 / 抓取 / 强化学习策略

代码复用情况

代码模块	No.4 → No.5
鼠标状态变量	完全相同
keyboard / mouse / scroll 回调	完全相同
GLFW 初始化 / 可视化结构	完全相同
关节定义（pin、pin2）	完全相同
几何体	完全相同
积分器 / timestep	完全相同
驱动接口	完全不同（`qfrc_applied` → `data.ctrl`）
控制器	完全不同（反馈线性化 → FSM + PD）
轨迹生成	No.5 新增
状态机	No.5 新增
XML 初始位姿	完全不同（无翻转 → 翻转 180°）
actuator 配置	No.5 新增

七、常见问题

1. XML 报错 `unrecognized attribute: 'sensornoise'`

原因：<flag sensornoise="enable" ...> 是老版本 MuJoCo 的写法，新版已移除该属性。

解决：删掉 sensornoise="enable"，噪声在 <sensor> 元素中单独配置。

2. `from_xml_path` 报 `ValueError: XML Error`

原因：常见两种——

传了 .py 文件（误用）
XML 里有 schema 不识别的属性

解决：确认路径是 .xml，并检查 XML 中所有属性是否在 MuJoCo XML 文档中。

3. FSM 不切换状态

原因：init_controller 未在 set_mjcb_control 之前调用，导致 fsm_state 未初始化。

解决：

init_controller(model, data)  # 必须在 set_mjcb_control 之前
mj.set_mjcb_control(controller)

4. 摆杆抖动剧烈

原因：

timestep 不够小
kp/kv 过大激发数值不稳定
actuator 的 ctrlrange 不够大

解决：

减小 timestep（当前 0.0001 已较小）
适度降低 kp/kv（先各降 50% 试试）
确认 ctrlrange 足够大（当前 ±100）

5. `data.ctrl[i]` 写入但摆杆没反应

原因：写入了非 motor 通道（如 ctrl[1] 是 position servo），而 servo 的 kp=0 不产生力。

解决：只写 ctrl[0]（pin motor）和 ctrl[3]（pin2 motor），其余清零。

6. `no_5.py` 看不出 FSM 效果

原因：no_5.py 是最小脚本，没有注册 controller，所以双摆自由下落。

解决：要观察 FSM 控制效果，必须运行 mjpython doublependulum_fsm.py。

No.6 双摆逆运动学（IK）

本节介绍双摆的逆运动学（Inverse Kinematics, IK） 控制 —— 从「指挥关节」升级到「指挥末端执行器」。核心思想是用 Jacobian 把「末端想去哪」翻译成「关节该怎么动」。

文件说明

mujoco/No_6/
├── doublependulum.xml     # MuJoCo XML 模型文件
└── doublependulum_ik.py   # 完整脚本：含 IK 控制器

No.6 没有最小脚本（no_6.py）。要看效果必须跑 doublependulum_ik.py。

一、doublependulum.xml 详解（对比 No.5）

<mujoco>
    <option timestep="0.0001" integrator="RK4" gravity="0 0 0" >  <!-- 重力关掉 -->
        <flag energy="enable" contact="disable" />
    </option>

    <worldbody>
        <light diffuse=".5 .5 0.5" pos="0 0 3" dir="0 0 -1"/>
        <geom type="plane" size="1 1 0.1" rgba=".9 0 0 1"/>

        <body pos="0 0 1.25" euler="0 90 0">  <!-- euler="0 90 0" 让关节在 (x,z) 平面内运动 -->
            <joint name="pin" type="hinge" axis="0 -1 0" pos="0 0 -0.5"/>
            <geom type="cylinder" size="0.05 0.5" rgba="0 .9 0 1" mass="1"/>
            <body pos="0 0.1 1" euler="0 0 0">
                <joint name="pin2" type="hinge" axis="0 -1 0" pos="0 0 -0.5"/>
                <geom type="cylinder" size="0.05 0.5" rgba="0 0 .9 1" mass="1"/>
                <!-- 【No.6 新增】在第二根杆末端定义「末端点」 -->
                <site name="endeff" pos="0 0 0.5" size="0.1"/>
            </body>
        </body>
    </worldbody>

    <!-- 【No.6 新增】位置伺服（代替 No.5 的 motor） -->
    <actuator>
        <position name="pservo1" joint="pin"  kp="100" />
        <velocity name="vservo1" joint="pin"  kv="10"  />
        <position name="pservo2" joint="pin2" kp="100" />
        <velocity name="vservo2" joint="pin2" kv="10"  />
    </actuator>

    <!-- 【No.6 新增】暴露末端点位置给 Python -->
    <sensor>
        <framepos    objtype="site" objname="endeff"/>
        <framelinvel objtype="site" objname="endeff"/>
    </sensor>
</mujoco>

XML 配置对比表

配置项	No.5 (FSM)	No.6 (IK)
重力	默认开启	`gravity="0 0 0"`（关）
朝向	`euler="0 180 0"`	`euler="0 90 0"`（关节在 x-z 平面）
末端标记	❌	`<site name="endeff">` 新增
actuator	2 motor + 4 servo	2 position servo + 2 velocity servo
增益	无（motor）	kp=100, kv=10（PD 内部由 MuJoCo 算）
sensor	❌	`framepos` + `framelinvel` 新增

关键变更说明

1. `gravity="0 0 0"`：关掉重力

为什么？IK 任务是「末端画圆」，不需要与重力对抗。纯运动学演示让控制更干净。

2. `euler="0 90 0"`：让运动平面是 (x, z)

axis="0 -1 0" 配合 euler="0 90 0" 之后，关节旋转轴变成水平方向 → 两个关节的运动全在 (x, z) 平面内。这让 IK 任务降维到 2D（x、z 两个分量），Jacobian 变成 2×2 方阵可逆。

3. `<site name="endeff">`：定义「末端执行器」

<site name="endeff" pos="0 0 0.5" size="0.1"/>

site 是 MuJoCo 里的虚拟标记点，附在 body 上，不参与碰撞。pos="0 0 0.5" 表示在第二根 body 的末端（局部 z=0.5）。size="0.1" 是可视化半径（橙色小球）。

关键概念：site 是「逻辑上的末端」，不是物理实体。它让控制器能问「末端在哪」「末端的 Jacobian 是什么」。

4. 位置伺服代替 motor

No.5 的 motor 是力矩输入（要自己写 PD）。No.6 的 position servo 是目标关节角（MuJoCo 内部帮你做 PD）：

motor  + 自己写 PD      =  τ           (低层)
position servo          =  q_target    (高层)

控制层级上移：你只关心目标位置，不关心怎么到位。

5. 传感器：暴露末端信息

<framepos    objtype="site" objname="endeff"/>  → sensordata[0:3] = (x, y, z)
<framelinvel objtype="site" objname="endeff"/>  → sensordata[3:6] = (vx, vy, vz)

framepos 把末端世界坐标暴露给 Python。这是「仿真器作弊」——它直接把答案告诉你（见下文）。

二、controller 详解：逆运动学核心

2.1 任务定义

让末端在 (x, z) 平面画一个圆：

# 在主循环初始化时算：
mj.mj_forward(model, data)         # 算当前末端位置

x_0 = data.sensordata[0] - r       # 圆心 x = 当前末端 x - 0.5
z_0 = data.sensordata[2]           # 圆心 z = 当前末端 z

# 在 controller 里：
x_target = x_0 + r * cos(t)        # 圆参数化（周期 2π）
z_target = z_0 + r * sin(t)

2.2 完整 controller 代码

def controller(model, data):
    # 1. 读末端当前位置（来自 <sensor><framepos>）
    end_eff_pos = data.sensordata[:3]

    # 2. 算末端 Jacobian（3D 位置 × 2 关节）
    jacp = np.zeros((3, 2))
    mj.mj_jac(model, data, jacp, None, end_eff_pos, 2)  # 2 = body id

    # 3. 切到 (x, z) 子空间（忽略 y），变 2×2 方阵
    J = jacp[[0, 2], :]

    # 4. 末端位置误差
    dx = np.array([
        [x_0 + r * np.cos(data.time) - data.sensordata[0]],
        [z_0 + r * np.sin(data.time) - data.sensordata[2]]
    ])

    # 5. 逆运动学：Δq = J⁻¹ · Δx
    dq = inv(J) @ dx

    # 6. 命令位置伺服：让关节去 q + Δq
    data.ctrl[0] = data.qpos[0] + dq[0, 0]   # pin
    data.ctrl[2] = data.qpos[1] + dq[1, 0]   # pin2

2.3 核心公式：Δq = J⁻¹ · Δx

Jacobian J 是「关节速度 → 末端速度」的线性映射：

ẋ_末端 = J(q) · q̇_关节
   (3D)    (3×2)  (2D)

反解：给定末端期望速度 → 求关节速度

q̇ = J⁻¹ · ẋ

No.6 取 J 的 x、z 两行 → 2×2 方阵可逆。

2.4 公式链

目标末端位置: x*(t) = (x_0 + r·cos t, z_0 + r·sin t)
末端误差:     Δx = x*(t) - x_current
关节修正:     Δq = J⁻¹ · Δx
目标关节角:   q_target = q_current + Δq
ctrl[0]      = q_target_pin
ctrl[2]      = q_target_pin2
内部 kp=100   → MuJoCo 帮你做 PD 到位

三、`mj_jac` 的两个非显然参数

mj.mj_jac(model, data, jacp, None, end_eff_pos, 2)

参数位置	含义	这个值
`jacp`	位置 Jacobian（输出）	3×2 零数组（被填）
第 4 参数	旋转 Jacobian	`None`（不要）
`end_eff_pos`	算 Jacobian 的世界坐标点	末端当前世界坐标
`2`	这个点附在哪个 body	body id 2（第二根杆）

易错点：end_eff_pos 必须是世界坐标（不是 body 局部坐标）；2 是 body 在 MuJoCo 内部的 id（按 XML 声明顺序：world=0, 第一根杆=1, 第二根杆=2）。

四、初始化的精妙之处

data.qpos[0] = -0.5
data.qpos[1] = 1.0
mj.mj_forward(model, data)            # ← 关键

x_0 = data.sensordata[0] - r          # 圆心 = 当前末端 x - 0.5
z_0 = data.sensordata[2]              # 圆心 z = 当前末端 z

为什么 `mj_forward`？

data.sensordata 不是赋值 qpos 后立即更新的。要算「当前末端在哪」必须先 mj_forward（不消耗时间的前向计算）。

圆心为什么这么算？

圆方程: x(t) = x_0 + r·cos(t),  z(t) = z_0 + r·sin(t)
t=0 时:  x(0) = x_0 + r,            z(0) = z_0
        = 当前末端位置    = 当前末端位置

所以圆心 = 末端当前位置向左移 r。这样画出来的圆从末端当前位置出发，自然过渡。

五、仿真器「作弊」的两处

5.1 第一处：直接读 sensordata

end_eff_pos = data.sensordata[:3]   # ← 偷懒：直接拿答案

真实代码应该自己算正运动学 (FK)：

# 自己写 FK
L1, L2 = 0.5, 0.5  # 杆长
x_ee = L1 * cos(q[0]) + L2 * cos(q[0] + q[1])
z_ee = L1 * sin(q[0]) + L2 * sin(q[0] + q[1])
end_eff_pos = np.array([x_ee, 0, z_ee])

效果完全一样（因为 sensor 内部就是这个 FK），但不依赖 MuJoCo 内部状态。

5.2 第二处：mj_jac 算 Jacobian

mj.mj_jac(model, data, jacp, None, end_eff_pos, 2)  # ← MuJoCo 自动求导

真实代码应该手写 Jacobian 解析式（FK 的导数）：

# 自己写 Jacobian
J = np.array([
    [-L1·sin(q1) - L2·sin(q1+q2),  -L2·sin(q1+q2)],
    [ L1·cos(q1) + L2·cos(q1+q2),   L2·cos(q1+q2)]
])
J = J[[0, 1], :]  # 提取 x, z 行

关键洞察：「不知道末端位置」是假问题。知道 q 就能算出 x（FK 是确定性函数）。代码里用 sensordata 和 mj_jac 只是「仿真器帮你算好」的便利，真实部署时把这两步换成自己的 FK 和 J 即可。

六、跟 No.4/5 的本质区别

维度	No.4 反馈线性化	No.5 PD+FSM	No.6 IK
控制空间	关节空间 q	关节空间 q	任务空间 x（末端位置）
目标	固定点 qref	时变轨迹 q(t)	空间几何曲线 x(t)（圆）
核心数学	`τ = M·v + f`	FSM + PD	`Δq = J⁻¹·Δx`
驱动	力矩（qfrc）	力矩（motor）	位置（servo）
需要模型吗	需要 M	不需要	需要 J（比 M 简单）

控制思想的演进

No.4: 已知模型 → 用模型「抵消」非线性
No.5: 不知道模型 → 用大增益「硬追」
No.6: 不知道关节该怎么动 → 问 Jacobian（局部线性映射）

No.6 的核心价值：你不用关心关节怎么动，只告诉系统「末端应该到哪」。

七、运行方法

cd mujoco/No_6/
mjpython doublependulum_ik.py

预期效果：末端绕圆心逆时针画圆（周期 2π ≈ 6.28 秒），双臂在 (x, z) 平面内优雅地摆动。

八、常见问题

1. 末端画的不是圆

原因：Jacobian 奇异点（det(J) = 0）。两个杆完全伸直或完全折叠时，末端速度被「锁死」，J⁻¹ 数值爆炸。

解决：

减小圆半径 r（让运动范围远离奇异点）
用 DLS（阻尼最小二乘）：J⁺ = Jᵀ(JJᵀ + λ²I)⁻¹，牺牲精度换稳定

2. `mj_jac` 报 body id 错误

原因：body id 不是 2。

检查：

print("body 0 =", model.body(0).name)  # world
print("body 1 =", model.body(1).name)  # 第一根杆
print("body 2 =", model.body(2).name)  # 第二根杆（含 site）

3. 末端从一开始就不在预期位置

原因：初始化时没 mj_forward，sensordata 还是上一步的旧值。

解决：在 data.qpos = ... 之后立即 mj.mj_forward(model, data)。

4. `dx` 永远不收敛到 0

原因：

IK 公式是「一步修正」（不是积分），理论上单步就能闭合误差
但因为 data.qpos 在 mj_step 过程中变化了，单帧内 dx 不会完全为 0
控制器每物理步跑 10000 次（因为 timestep=0.0001），所以视觉上会很快收敛

5. 没有重力，物理意义是什么？

No.6 是纯运动学演示：测试 IK 数学是否正确，不关心物理真实性。

如果要恢复物理真实性：移除 gravity="0 0 0"，并把 position servo 换成 motor + 自己写 PD。

6. 跟 No.7 LQR 怎么选？

场景	用 IK	用 LQR
末端走几何路径	✅ 直观	❌ 麻烦
关节镇定到平衡点	❌ 不自然	✅ 经典用法
避障	✅ 显式规划	❌ 难
抗扰	❌（单步 IK 不抗扰）	✅ 闭环稳定

简单规则：控制末端用 IK，镇定关节用 LQR。

九、整体公式对应

───────── 任务（任务空间）─────────
x*(t) = (x_0 + r·cos t, z_0 + r·sin t)    ← 圆参数化

───────── 误差（任务空间）─────────
Δx = x* - x_current                       ← sensordata[:3]

───────── 逆运动学（任务空间→关节空间）──
J = ∂fk/∂q                                ← 3×2
J = J[[0, 2], :]                          ← 切到 2×2
Δq = J⁻¹ · Δx                            ← IK 核心

───────── 执行（关节空间）─────────
ctrl[0] = qpos[0] + Δq[0]                ← pin 的目标角
ctrl[2] = qpos[1] + Δq[1]                ← pin2 的目标角

───────── 物理（mujoco）─────────
position servo (kp=100, kv=10)
  → 让关节实际到达 q_target
  → 末端到达 x*

十、一句话总结

No.6 = 「指挥末端而不是关节」。核心是 Jacobian 逆 Δq = J⁻¹·Δx —— 把几何意图翻译成关节命令。site + framepos 让控制器能问「末端在哪」，mj_jac 让它能问「关节动一点末端会怎么动」。这是运动规划的基础。

No.7 双摆 LQR 最优控制（含系统线性化）

本节介绍在 No.4–No.6 已有控制器的基础上，引入最优控制与在线性化技术。核心思路：

线性化：用有限差分把非线性双摆模型在平衡点附近线性化为 ẋ = Ax + Bu
LQR 设计：解连续代数 Riccati 方程 (CARE)，得到最优反馈增益 K
状态反馈：控制律 u = -Kx（这里 K 已含负号，代码里直接 +K@state）
鲁棒性测试：往第一个关节注入高斯噪声，看 LQR 能否镇定

文件说明

mujoco/No_7/
├── doublependulum.xml      # MuJoCo XML 模型文件
└── doublependulum_lqr.py   # 完整脚本：含线性化、LQR 设计、噪声注入

No.7 没有最小脚本（no_7.py）。要看效果必须跑 doublependulum_lqr.py。

一、doublependulum.xml 详解（对比 No.6）

No.7 doublependulum.xml 完整代码

<mujoco>
    <!--
        timestep=0.001（比 No.4/5/6 的 0.0001 粗 10 倍）
        integrator=RK4
    -->
    <option timestep="0.001" integrator="RK4">
        <flag sensornoise="enable" contact="disable" energy="enable"/>
    </option>

    <worldbody>
        <light diffuse=".5 .5 .5" pos="0 0 3" dir="0 0 -1"/>
        <geom type="plane" size="1 1 0.1" rgba=".9 0 0 1"/>

        <!--
            第一连杆 body：高度 2.5（No.4 同款），euler="0 180 0"（朝下）
            【No.7 新增】<inertial> 显式指定惯量
        -->
        <body pos="0 0 2.5" euler="0 180 0">
            <joint name="pin" type="hinge" pos="0 0 0.5" axis="0 -1 0" />
            <geom type="cylinder" size=".05 .5" rgba="0 .9 0 1" />
            <inertial mass="1" pos="0 0 0" diaginertia="0.1 0.1 0.1"/>

            <body pos="0 0 -1" euler="0 0 0">
                <joint name="pin2" type="hinge" pos="0 0 0.5" axis="0 -1 0" />
                <geom type="cylinder" size=".05 .5" rgba="0 0 .9 1" />
                <inertial mass="1" pos="0 0 0" diaginertia="0.1 0.1 0.1"/>
            </body>
        </body>
    </worldbody>

    <actuator>
        <!-- pin 的 motor 被注释掉！只驱动 pin2 -->
        <motor name="torque2" joint="pin2" gear="1" ctrlrange="-1000 1000" ctrllimited="true"/>
    </actuator>
</mujoco>

XML 配置对比表

配置项	No.6 (IK)	No.7 (LQR)
关节定义	`pin`, `pin2`（关节在杆底）	`pin`, `pin2`（关节在杆顶）
初始 body 位置	`(0, 0, 1.25)`	`(0, 0, 2.5)`
朝向	`euler="0 90 0"`	`euler="0 180 0"`（朝下）
`<inertial>` 块	❌（用默认）	✅（显式指定 `mass=1`，`diaginertia=0.1 0.1 0.1`）
timestep	0.0001	0.001（粗 10 倍）
actuator	4 个（2 motor + 2 servo）	1 个（只驱动 pin2）
sensor	`framepos` + `framelinvel`	❌（没有）
`<flag sensornoise>`	❌	有（但该属性已废弃，见 FAQ）
重力	`gravity="0 0 0"`（关）	默认开启

关键变更说明

1. 关节位置 `pos="0 0 0.5"`（No.6 是 `-0.5`）

No.6 中关节在 body 的底部（局部 z=-0.5），第二根 body 接在第一根底部。
No.7 改成关节在 body 顶部（局部 z=0.5），第二根 body 接在第一根顶部 pos="0 0 -1"。

为什么改？ 因为 LQR 是基于线性化模型的，关节位置的不同会让平衡点附近的 A、B 矩阵完全不同。No.7 的设计选择是「杆朝下、关节在顶、像倒挂的钟摆」。

2. 显式 `<inertial>` 块

<inertial mass="1" pos="0 0 0" diaginertia="0.1 0.1 0.1"/>

diaginertia="0.1 0.1 0.1" 是对角惯量张量。No.4/5/6 用默认值（MuJoCo 根据 cylinder 几何自动算），No.7 显式指定让惯量与几何无关——便于 LQR 线性化时数值稳定。

3. 只驱动 pin2

<!-- <motor name="torque1" joint="pin" ... /> -->  <!-- 注释掉 -->
<motor name="torque2" joint="pin2" ... />

只控第二个关节。第一个关节是被动的（受重力、噪声扰动，但无控制输入）。

设计哲学：让 LQR 「只用一个执行器稳定整个 2 自由度系统」——这是控制理论的经典挑战。系统是欠驱动（underactuated）的。

4. timestep 变粗（0.0001 → 0.001）

为什么？LQR 在仿真里不需要那么小的步长——它设计时就考虑了「在平衡点附近有界」。粗 10 倍 = 仿真快 10 倍。

二、核心：状态空间与线性化（最重要的新概念）

2.1 状态向量

No.7 第一次明确把系统表示成状态空间形式：

state = [q1, dq1, q2, dq2]ᵀ       # 4 维列向量
                              # 两个关节角 + 两个关节角速度

控制输入：

u = data.ctrl[0]                # 1 维标量（pin2 的力矩）

2.2 状态导数函数 `get_dx`

def get_dx(inputs):
    """
    inputs = [q1, dq1, q2, dq2, u]   (5 维)
    outputs = [dq1, ddq1, dq2, ddq2] (4 维)
    """
    data.qpos[0] = inputs[0]
    data.qvel[0] = inputs[1]
    data.qpos[1] = inputs[2]
    data.qvel[1] = inputs[3]
    data.ctrl[0] = inputs[4]

    mj.mj_forward(model, data)

    dq1 = data.qvel[0]
    dq2 = data.qvel[1]

    M = np.zeros((2, 2))
    mj.mj_fullM(model, M, data.qM)

    f = np.array([
        [0 - data.qfrc_bias[0]],
        [data.ctrl[0] - data.qfrc_bias[1]]
    ])

    ddq = inv(M) @ f

    return np.array([dq1, ddq[0, 0], dq2, ddq[1, 0]])

这就是 ẋ = f(x, u) 的「真值」——给定当前 x 和 u，算出状态导数 ẋ。

输入分量	物理含义	MuJoCo 字段
`inputs[0]`	关节 1 角	`data.qpos[0]`
`inputs[1]`	关节 1 角速度	`data.qvel[0]`
`inputs[2]`	关节 2 角	`data.qpos[1]`
`inputs[3]`	关节 2 角速度	`data.qvel[1]`
`inputs[4]`	pin2 力矩	`data.ctrl[0]`

输出分量	物理含义
`dq1`	关节 1 角速度（`= inputs[1]`，恒等）
`ddq1`	关节 1 角加速度（从 `M @ d̈q = f` 解出）
`dq2`	关节 2 角速度
`ddq2`	关节 2 角加速度

关键洞察：dq/dt = 角速度本身，d(dq)/dt = 角加速度。这俩是不同的物理量，但 get_dx 把它们一起算出来构成完整的 ẋ。

2.3 为什么第一关节的 `f[0]` 是 `0 - qfrc_bias[0]`？

f = [[0 - qfrc_bias[0]],               # pin 没有力矩输入（motor 被注释）
     [data.ctrl[0] - qfrc_bias[1]]]   # pin2 收到 ctrl[0] - 偏置

MuJoCo 的动力学方程是：

M(q) q̈ = τ_applied + τ_bias_constraint - τ_bias

整理成 M q̈ = f 形式：

f = τ_applied - qfrc_bias

关节	τ_applied	f = τ_applied - qfrc_bias
pin	0（无 motor）	`0 - qfrc_bias[0]`
pin2	`data.ctrl[0]`	`data.ctrl[0] - qfrc_bias[1]`

然后 q̈ = M⁻¹ f 解出加速度。

注意：这里的 qfrc_bias 含重力，所以 f 自动「扣除」了重力——这跟 No.4 的反馈线性化一个思路。

2.4 数值线性化 `linearization`

def linearization(pert=0.001):
    f0 = get_dx(np.zeros(5))       # 在 x=0, u=0 处求 ẋ

    Jacobians = []
    for i in range(5):
        inputs_i = np.zeros(5)
        inputs_i[i] = pert         # 扰动第 i 个分量
        jac = (get_dx(inputs_i) - f0) / pert   # 有限差分
        Jacobians.append(jac[:, np.newaxis])

    A = np.concatenate(Jacobians[:4], axis=1)  # 4×4（对 x 导数）
    B = Jacobians[-1]                          # 4×1（对 u 导数）

    return A, B

这是有限差分法求 Jacobian——把 get_dx 当黑箱，数值地求 ∂ẋ/∂x 和 ∂ẋ/∂u：

A[i, j] = (get_dx(x + pert·eⱼ, u) - get_dx(x, u))[i] / pert
B[i, 0] = (get_dx(x, u + pert) - get_dx(x, u))[i] / pert

为什么在 x=0, u=0 处线性化？ 因为双摆自然下垂（q=0）是稳定平衡点。ẋ₀ = 0（不动），这就是线性化的基准点。

2.5 线性化结果：状态空间方程

ẋ ≈ A·x + B·u            (4 维)
   = [A]·[q1, dq1, q2, dq2]ᵀ + [B]·u

矩阵	形状	含义
A	4×4	状态转移：x 变化 → ẋ 变化
B	4×1	控制响应：u 变化 → ẋ 变化

这是 LQR 设计的前提：把非线性系统「假装」成线性系统，再设计线性控制器。

三、LQR 设计（核心算法）

3.1 LQR 想干什么？

LQR 找最优增益 K，使下面这个二次型代价函数最小：

J = ∫₀^∞ ( xᵀ Q x + uᵀ R u ) dt
       \_______________/  \_______/
            状态误差代价     控制代价

Q 大 → 重视状态误差（快速收敛，但控制信号大）
R 大 → 重视控制代价（节能，但响应慢）
Q、R 选得不同 → 不同 K，没有"正确"答案

3.2 代码实现

Q = np.diag([10, 10, 10, 10])    # 状态权重（q1, dq1, q2, dq2 各 10）
R = np.diag([0.1])                # 控制权重（u 一个分量 0.1）

P = solve_continuous_are(A, B, Q, R)  # ① 解 Riccati 方程
K = -inv(B.T @ P @ B + R) @ B.T @ P @ A   # ② 计算最优增益

步骤 ①：解连续代数 Riccati 方程 (CARE)

Aᵀ P + P A - P B R⁻¹ Bᵀ P + Q = 0

这是关于矩阵 P 的非线性矩阵方程。scipy.linalg.solve_continuous_are 用迭代法（schur decomposition）解。

步骤 ②：LQR 增益公式

K = -R⁻¹ Bᵀ P          （连续 LQR 公式）
  = -(Bᵀ P B + R)⁻¹ Bᵀ P A     （代码里的等价形式，避免求 R⁻¹）

注意负号：K 已经是「带负号」的形式了，所以控制律是 u = K @ x（不是 u = -K @ x）。

3.3 控制律

def controller(model, data):
    state = np.array([
        [data.qpos[0]],
        [data.qvel[0]],
        [data.qpos[1]],
        [data.qvel[1]],
    ])
    data.ctrl[0] = (K @ state)[0, 0]   # u = K @ x

    # 噪声注入（鲁棒性测试）
    noise = mj.mju_standardNormal(0.0)
    data.qfrc_applied[0] = noise

线性状态反馈：把 x 直接乘以 K 矩阵就得到控制量。

3.4 噪声注入：鲁棒性测试

noise = mj.mju_standardNormal(0.0)   # 标准正态 N(0, 1)
data.qfrc_applied[0] = noise          # 注入到 pin 关节

mj.mju_standardNormal 是 MuJoCo 自带的伪随机数生成器（基于 Box-Muller）。注入到第一个关节（pin，是被动的）——模拟外部扰动。

为什么这样测？ 因为 LQR 在「无扰动、模型精确」下一定稳。有扰动还稳才证明控制器鲁棒。这是控制理论的标配实验。

特别说明：噪声注入 qfrc_applied[0] 是叠加在系统上的，跟 LQR 控制无关。LQR 不需要知道这个噪声，靠反馈把它「压」回去。

四、与 No.4–No.6 的本质区别

维度	No.4 反馈线性化	No.5 PD+FSM	No.6 IK	No.7 LQR
控制律	`τ = M·v + f`	FSM 切换的 PD	`Δq = J⁻¹·Δx`	`u = K·x`
设计方法	手算 + 调参	手算 + 调参	手算 + IK 公式	算法求解（CARE）
需要模型吗	✅ M, qfrc_bias	❌	✅ J	✅ A, B（要线性化）
需要全状态吗	部分（q, q̇）	部分	是	是（x = [q, q̇, q, q̇]）
稳定性保证	局部（精确模型时）	无	局部（远离奇异点）	全局（线性化点附近，理论上）
最优性	❌	❌	❌	✅（最小化二次代价）
鲁棒性	差（依赖 M）	中（大增益）	中	可调（Q/R 调节）
计算复杂度	O(n²)	O(1)	O(n³)	O(n³)（一次性离线）

控制思想的演进

No.4: 已知模型 → 用模型「抵消」非线性
No.5: 不知道模型 → 用大增益「硬追」
No.6: 不知道关节该怎么动 → 问 Jacobian（局部线性映射）
No.7: 知道模型 → 把模型「线性化」→ 用最优控制理论设计 K

五、controller / get_dx / linearization 三函数协作图

线性化（仅在启动时跑一次）                LQR 控制（每物理步）
─────────────────────                ─────────────────
linearization()                       controller(model, data)
  │                                    │
  ├─ get_dx(0,0,0,0,0)                 ├─ 读 qpos, qvel → state (4,1)
  │    └─ 设 qpos/qvel/ctrl            │
  │    └─ mj_forward                   ├─ u = K @ state  ← 一次矩阵乘
  │    └─ 算 M, qfrc_bias              │
  │    └─ q̈ = M⁻¹ f                    └─ data.ctrl[0] = u
  │    └─ return ẋ
  │                                    └─ 注入噪声（鲁棒性测试）
  ├─ 扰动每个输入 → 重复 5 次                qfrc_applied[0] = N(0,1)
  ├─ 有限差分 → A (4×4), B (4×1)
  │
  └─ Q, R 选权重
     P = solve CARE
     K = -R⁻¹ Bᵀ P

六、关键设计参数与调参指南

参数	当前值	含义	怎么调
`Q = diag(10, 10, 10, 10)`	全 10	4 个状态分量同等重视	增大 → 收敛更快但 u 更大
`R = 0.1`	0.1	控制代价	增大 → u 更小但跟踪变慢
`Q/R` 比	100	关键比例	越大越激进，越小越保守
`timestep`	0.001	仿真步长	线性化假设连续时间，dt 不影响 K，但太大会让数字不稳
`pert`	0.001	有限差分步长	太小 → 数值误差；太大 → 截断误差；~1e-3 ~ 1e-5 都行

调参实验建议

想看什么	改什么
收敛更快	`Q` 整体乘 2
控制更平缓	`R` 乘 10
位置精度更高	角分量 `Q[0,0]`、`Q[2,2]` 调大
速度更小	角速度分量 `Q[1,1]`、`Q[3,3]` 调大
测试鲁棒性边界	噪声系数放大 10 倍

七、运行方法

cd mujoco/No_7/
mjpython doublependulum_lqr.py

预期效果：

无噪声情况下：双摆稳定在自然下垂（q=0），不摆动。
有噪声注入：双摆仍然稳定在 q=0 附近，但会有小幅抖动（被 LQR 抑制）。

⚠️ 启动时控制台会打印 solve_continuous_are 的解算过程（如果用 verbose 模式）。这是正常的——Riccati 求解是离线一次性计算，不影响仿真速度。

八、跟 No.4–No.6 的学习路径

No.1: 基础建模 + viewer 可视化
  ↓
No.2: GLFW + 鼠标交互
  ↓
No.3: 单摆 PD 闭环
  ↓
No.4: 双摆 + 反馈线性化（需 M）
  ↓
No.5: 双摆 + actuator + FSM + 三次多项式轨迹
  ↓
No.6: 双摆 + 任务空间 IK（需 J）
  ↓
No.7: 双摆 + 线性化 + LQR（需 A, B）  ← 当前
  ↓
（未来）No.8: MPC / 强化学习 / 接触 / 抓取

控制器「知识需求」演进

节	需要预先知道
No.4	动力学 M、qfrc_bias
No.5	轨迹生成、FSM 切换
No.6	雅可比 J 的几何意义
No.7	状态空间、线性化、Riccati 方程

九、常见问题

1. XML 报错 `unrecognized attribute: 'sensornoise'`

原因：sensornoise="enable" 是老版本 MuJoCo 的写法，新版已移除该属性。

解决：删除 <flag sensornoise="enable" contact="disable" energy="enable"/> 中的 sensornoise="enable"，或整个 flag 元素简化为 <flag energy="enable"/>。

2. `solve_continuous_are` 报错 / 数值不稳定

原因：

(A, B) 不可控（controllable）→ Riccati 无解
Q、R 选得不合理（如 R=0）

检查：

import numpy as np
controllability = np.concatenate([B, A @ B, A @ A @ B, A @ A @ A @ B], axis=1)
print("rank:", np.linalg.matrix_rank(controllability))  # 应为 4（满秩）

3. 双摆剧烈震荡不收敛

可能原因：

K 算错了（行/列顺序错）
状态向量顺序跟 K 不匹配
噪声太大盖过控制

调试：

print("K =", K)
print("state =", state.flatten())
print("u =", (K @ state)[0, 0])

4. 第一关节（pin）完全不动

原因：这是预期行为。pin 没有 motor，只有 qfrc_applied 注入噪声。

意图：让 LQR 「只用 1 个执行器稳定 2 自由度欠驱动系统」——经典控制难题。

5. 噪声注入在哪一行生效？

data.qfrc_applied[0] = noise   # 注入到 pin
data.ctrl[0] = (K @ state)[0]  # LQR 控 pin2

qfrc_applied 是叠加到系统上的力，不经过 actuator。LQR 不需要知道这个力，靠反馈自然抑制。

6. 为什么 `get_dx` 里第一行写 `0 - qfrc_bias[0]`？

因为 pin 没有 motor（τ_applied = 0）。代入 M q̈ = τ_applied - qfrc_bias：

f[0] = 0 - qfrc_bias[0]

7. 跟 No.4 反馈线性化的本质区别

	No.4 反馈线性化	No.7 LQR
取消非线性的方式	实时算 `M(q)` 并用它做补偿	离线线性化成 A, B，不再算 M
控制律	`τ = M·v + f`（实时）	`u = K·x`（一次矩阵乘）
计算成本	每步 O(n³)	每步 O(n²)
全局最优？	❌	✅（线性化点附近）

8. LQR 适用范围

LQR 只在线性化点附近有效。远离平衡点性能会急剧下降（因为 A·x + B·u 不再近似 f(x, u)）。

解决办法：

在多个平衡点设计 LQR，切换（类似 No.5 的 FSM）
用 LTV-LQR（时变 LQR，每步重新线性化）
用 MPC（模型预测控制，No.8 可能涉及）

十、整体公式对应

────────────── 系统（物理）────────────────
M(q) q̈ + C(q, q̇) + g(q) = τ
                                       ← 真实非线性动力学
────────────── 线性化（get_dx + 数值 Jacobian）────
ẋ = A x + B u                          ← 在 x=0, u=0 处的线性近似
       x = [q1, q̇1, q2, q̇2]ᵀ (4×1)
       u = pin2 力矩 (1×1)
       A: 4×4, B: 4×1
────────────── LQR 设计（离线）─────────────
min ∫(xᵀQx + uᵀRu)dt
  s.t. ẋ = Ax + Bu
  → 解 CARE: AᵀP + PA − PBR⁻¹BᵀP + Q = 0
  → 增益 K = -R⁻¹BᵀP
────────────── 实时控制（每步）────────────
u = K @ x
data.ctrl[0] = u[0]
data.qfrc_applied[0] = noise   # 鲁棒性测试
────────────── 物理执行（mj_step）─────────
更新 qpos, qvel, x → 回到第一步

十一、一句话总结

No.7 = 「离线线性化 + LQR 最优控制 + 噪声鲁棒性测试」。跟前几节比，最大跃迁是把控制器设计从「手算调参」升级到「算法求解」——Q、R 选好，自动算出最优 K。理论上的保证更强了（全局最优、闭环稳定），代价是只在线性化点附近有效。

No.8 双摆约束力「移交」仿真

本节介绍一个混合仿真（hybrid simulation）技巧：用约束反作用力作为「虚拟连杆」，在两个没有真实机械连接的 body 之间传递力，并在适当时机「释放」。这是把 connect equality 约束当作传感器用的典型例子。

文件说明

mujoco/No_8/
├── pendulum.xml        # MuJoCo XML 模型文件
└── hybrid_pendulum.py  # 完整脚本：含约束力读取、FSM 状态机

No.8 没有最小脚本（no_8.py）。要看效果必须跑 hybrid_pendulum.py。

一、pendulum.xml 详解（对比 No.7）

<mujoco>
    <option timestep="0.001" integrator="RK4" gravity="0 0 -9.81">
        <flag contact="enable" energy="enable"/>
    </option>

    <worldbody>
        <light diffuse=".5 .5 .5" pos="0 0 3" dir="0 0 -1"/>
        <geom type="plane" size="5 5 0.1" rgba=".9 0 0 1"/>

        <!--
            第一个 body：x 平移 + z 平移 + 旋转（3 个 slide + 1 个 hinge）
            【No.8 关键】所有 3 个 DOF 都参与约束求解
        -->
        <body name="pole" pos="0 0 2" euler="0 0 0">
            <joint name="x"   type="slide" pos="0 0 0.5" axis="1 0 0" />
            <joint name="z"   type="slide" pos="0 0 0.5" axis="0 0 1" />
            <joint name="pin" type="hinge" pos="0 0 0.5" axis="0 -1 0" />
            <geom type="cylinder" size=".05 .5" rgba="0 .9 0 0.1" mass="1"/>
        </body>

        <!--
            第二个 body：完全自由（不参与任何约束）
        -->
        <body name="pole2" pos="0 -1 2" euler="0 0 0">
            <joint name="x2"   type="slide" pos="0 0 0.5" axis="1 0 0" />
            <joint name="z2"   type="slide" pos="0 0 0.5" axis="0 0 1" />
            <joint name="pin2" type="hinge" pos="0 0 0.5" axis="0 -1 0" />
            <geom type="cylinder" size=".05 .5" rgba=".9 .9 .9 1" mass="1"/>
        </body>
    </worldbody>

    <!--
        【No.8 关键】connect equality：把 pole 的 anchor 点钉在世界的 (0,0,0.5)
        这是 3D 约束（x, y, z 三个分量都约束）
    -->
    <equality>
        <connect body1="pole" body2="world" anchor="0 0 0.5" />
    </equality>

    <actuator>
        <motor name="torque" joint="pin" gear="1" ctrlrange="-100 100" ctrllimited="true"/>
        <position name="position_servo" joint="pin" kp="0"/>
        <velocity name="velocity_servo" joint="pin" kv="0"/>
    </actuator>
</mujoco>

XML 配置对比表

配置项	No.7 (LQR)	No.8 (Hybrid)
body 数量	2（嵌套双摆）	2（独立两个 body）
body 关系	父子嵌套	互不连接
关节类型	2 个 hinge	6 个（3 slide + 3 hinge）
DOF 总数	2	6
自由度	`nv=2`	`nv=6`
actuator	1 motor	1 motor + 2 servo（servo kp/kv=0）
equality	❌	`<connect>` 新增（3D 约束）
重力	默认	默认（开启）
约束求解	无	有（connect 是硬约束）
timestep	0.001	0.001

关键变更说明

1. 两个 body 没有机械连接

No.7 的两个 body 是父子嵌套（自动连成一根杆）。No.8 的 pole 和 pole2 是两个独立的 body —— 它们没有真实的关节相连。

2. `<connect>` 等式约束

<connect body1="pole" body2="world" anchor="0 0 0.5" />

含义：把 pole 上局部坐标 (0, 0, 0.5) 这个点，钉死在世界的 (0, 0, 0.5) 上。MuJoCo 内部用拉格朗日乘子强制这个约束成立。

约束维度：3（x, y, z 三个分量）。所以 nefc = 3（活跃约束数 = 3）。

3. 6 个 DOF

DOF	关节	物理含义	是否被 connect 约束
0	`x`	pole 沿 x 平移	✅
1	`z`	pole 沿 z 平移	✅
2	`pin`	pole 绕 y 旋转	✅
3	`x2`	pole2 沿 x 平移	❌ 自由
4	`z2`	pole2 沿 z 平移	❌ 自由
5	`pin2`	pole2 绕 y 旋转	❌ 自由

关键事实：pole 实际只剩 0 个自由 DOF（被 connect 全部钉死）。pole2 才有 3 个真正自由的 DOF。

二、核心概念：connect 等式约束与约束 Jacobian

2.1 约束的本质

<connect body1="pole" body2="world" anchor="0 0 0.5" />

这是一个硬约束：在仿真每一步，MuJoCo 都要求：

position_of_anchor_on_pole_in_world == (0, 0, 0.5)    (3D 等式)

不满足就施加拉格朗日乘子力让它满足。

2.2 约束 Jacobian（efc_J）

efc_J 是约束对各 DOF 的偏导数矩阵：

[约束违反量] = J · qvel
[3×1]       [3×6][6×1]

每一行对应一个约束维度（x, y, z 的位置误差），每一列对应一个 DOF。

索引	形状	含义
`data.efc_J`	`(nefc*nv,)` 1D 平铺	约束 Jacobian（MuJoCo 3.x 是 1D）
`data.efc_force`	`(nefc,)` 1D	约束反作用力（拉格朗日乘子）
`data.nefc`	int	当前活跃约束数（connect 给 3）
`model.nv`	int	系统自由度数（这里 = 6）

2.3 约束反作用力（efc_force）

efc_force 是保持约束所需的力（3D 笛卡尔力）：

F0 = efc_force[:3]     # (3,)  ← x, y, z 三个方向的约束反力

物理含义：把 pole 的 anchor 钉在 (0, 0, 0.5) 需要的力有多大。重力 + 摆动 + 一切外力产生的「偏离趋势」都由这个力抵消。

三、controller 详解：FSM + 力移交

3.1 完整代码

def controller(model, data):
    global fsm

    # 1. 读约束 Jacobian（reshape 后切片）
    J = data.efc_J[:data.nefc * model.nv].reshape((data.nefc, model.nv))[:3, :3]

    # 2. 读约束反作用力
    F0 = data.efc_force[:3][:, np.newaxis]   # (3, 1)

    # 3. 把约束力「投影」到 pole 的 3 个 DOF
    JT_F = J.T @ F0                            # (3, 1)

    # 4. 状态转移：pin2 旋转 > 1.0 rad → 释放
    if fsm == FSM_SWING and data.qpos[5] > 1.0:
        fsm = FSM_FREE

    # 5. SWING 阶段：驱动 pin + 移交约束力到 pole2
    if fsm == FSM_SWING:
        data.qfrc_applied[2] = -1 * (data.qvel[2] - 5.0)        # pin 速度控制
        data.qfrc_applied[3] = JT_F[0, 0]                       # x2 ← 约束力 x
        data.qfrc_applied[4] = JT_F[1, 0]                       # z2 ← 约束力 y
        data.qfrc_applied[5] = JT_F[2, 0] + data.qfrc_applied[2]  # pin2 ← 约束力 z + pin 力矩

    # 6. FREE 阶段：pole2 自由飞
    elif fsm == FSM_FREE:
        data.qfrc_applied[3] = 0.0
        data.qfrc_applied[4] = 0.0
        data.qfrc_applied[5] = 0.0

3.2 状态机

                          pin2 旋转 > 1.0 rad
       ┌───────────────────────────────────────────┐
       │                                           │
       ▼                                           │
  ┌─────────┐                                   ┌─────────┐
  │ FSM_SWING│                                  │ FSM_FREE│
  │  摆动 +   │ ──────────────────────────────▶ │   释放   │
  │  力移交   │                                  │ 自由飞行 │
  └─────────┘                                   └─────────┘
       │                                           │
       └───────────────────────────────────────────┘
                  持续仿真直到 simend

状态	条件	行为
`FSM_SWING`	初始 / pin2 角度 ≤ 1.0	驱动 pin 加速到 5 rad/s + 把约束力移交到 pole2
`FSM_FREE`	pin2 角度 > 1.0 rad	pole2 自由，不再受约束力

3.3 关键公式链

约束力 (3D 笛卡尔):   F0 = efc_force[:3]                       (3, 1)
约束 Jacobian:        J = efc_J reshape → (3, 6)               (3, 6)
pole 的子块:           J_pole = J[:, :3]                         (3, 3)
pole DOF 上的力:      f_pole = J_pole.T @ F0                     (3, 1)
                       = (3, 3).T @ (3, 1) = (3, 1)

核心思想：f_pole 是本来作用在 pole 上的力，现在直接写到 pole2 的 DOF（qfrc_applied[3, 4, 5]）。

这就是「力移交」—— pole 被约束钉住，约束反力被「偷」过来，转手给 pole2。

四、「力移交」机制详解

4.1 为什么要做力移交？

因为 pole 和 pole2 没有真实关节连接，但你想让它们互动。三种方案：

方案	实现	优缺点
真实关节	在 XML 加 `<joint>`	永久连接，不能分离
距离约束	`<distance>` 等式	软连接，可调刚度
力移交（No.8 方案）	读约束力 + 写到另一 body	可释放，FSM 控制

4.2 为什么 `pin2` 旋转会让 pole2 受力？

直觉：

pole 被 connect 钉死在 (0, 0, 0.5)。
你强行给 pin 加速（qvel[2] → 5），但 pin 旋转会试图让 anchor 偏离 (0, 0, 0.5)。
MuJoCo 算出「保持约束」需要的 3D 力 F0。
F0 不是真的在 anchor 点上——它在每个 DOF 上的投影才是 pole 实际感受到的。
把这个投影写到 pole2 上，pole2 就「好像被连着」了。

4.3 释放时会发生什么？

if fsm == FSM_SWING and data.qpos[5] > 1.0:
    fsm = FSM_FREE

当 pin2 旋转超过 1.0 rad（约 57°），FSM 切换到 FSM_FREE：

elif fsm == FSM_FREE:
    data.qfrc_applied[3] = 0.0
    data.qfrc_applied[4] = 0.0
    data.qfrc_applied[5] = 0.0

不再写力给 pole2。此时 pole2 已经在旋转+平移中获得动能，带着这个动量飞出去。pole 仍然被 connect 钉住（如果 MuJoCo 的约束没有被释放）。

注意：代码里只移除了 qfrc_applied 的力，没有移除 <connect> 约束本身（XML 没改）。所以 pole 仍然被钉，pole2 自由飞。

五、MuJoCo 版本陷阱：efc_J 形状

5.1 历史变化

版本	`data.efc_J` 形状	索引方式
< 3.0	`(nefc, nv)` 2D 密集	`J[i, j]`
≥ 3.0（当前 3.8.0）	`(njmax * nv,)` 1D 平铺	`J[i*nv + j]`

老代码 data.efc_J[:3, :3] 直接报错（IndexError: too many indices）。

5.2 正确写法（已修复）

J = data.efc_J[:data.nefc * model.nv].reshape((data.nefc, model.nv))[:3, :3]

解释：

data.nefc = 3（connect 给 3 个约束）
model.nv = 6（总 DOF）
data.efc_J[:18] 取前 18 个元素（3×6）
.reshape((3, 6)) 变回 2D
[:3, :3] 保留原作者的语义（取 pole 的 3×3 子块）

5.3 如果想知道完整的 Jacobian

J_full = data.efc_J[:data.nefc * model.nv].reshape((data.nefc, model.nv))
# 形状 (3, 6): 行=约束维度(x,y,z), 列=DOF
# [:, :3] = pole 的 3 个 DOF
# [:, 3:] = pole2 的 3 个 DOF

六、跟 No.4–No.7 的本质区别

维度	No.4 反馈线性化	No.5 PD+FSM	No.6 IK	No.7 LQR	No.8 力移交
物理模型	串联双摆	串联双摆	串联双摆	串联双摆	两个独立 body
连接方式	真实关节	真实关节	真实关节	真实关节	connect 等式约束
控制律	`τ = M·v + f`	FSM + PD	`Δq = J⁻¹·Δx`	`u = K·x`	读约束力 + 写到另一 body
需要模型吗	需要 M	否	需要 J	需要 A, B	需要 efc_J, efc_force
可释放？	❌	❌（FSM 切状态）	❌	❌	✅（FSM_FREE 切走）
核心数学	矩阵运算	时序逻辑	Jacobian 逆	Riccati 方程	约束 Jacobian 转置

控制思想的演进

No.4: 已知模型 → 用模型「抵消」非线性
No.5: 不知道模型 → 用大增益「硬追」
No.6: 不知道关节怎么动 → 问 Jacobian（末端映射）
No.7: 已知模型 → 线性化 + 最优控制
No.8: 已知约束 → 读约束反力 → 「借」力给另一 body

No.8 的核心价值：不修改 XML 就能在两个 body 之间建立可释放的「虚拟连接」。

七、运行方法

cd mujoco/No_8/
mjpython hybrid_pendulum.py

预期效果：

前段（FSM_SWING）：pole 在原地摆动（被钉住），pole2 跟着动（接收约束反力）。
pin2 转过 1.0 rad（约 1-2 秒后）：FSM 切到 FREE。
后段（FSM_FREE）：pole2 自由飞行，pole 仍被钉。

八、跟 No.5 FSM 的对比

	No.5 FSM	No.8 FSM
状态数	4（HOLD/SWING1/SWING2/STOP）	2（SWING/FREE）
切换条件	时间触发	状态触发（`qpos[5] > 1.0`）
切换逻辑	时间表	物理量阈值
状态行为	切 PD 参考	切是否施加约束反力

No.5 是「按时间表跑任务」，No.8 是「按物理事件触发」。

九、常见问题

1. `IndexError: too many indices for array: array is 1-dimensional`

原因：MuJoCo 3.x 把 efc_J 改成 1D 平铺了。

解决：

J = data.efc_J[:data.nefc * model.nv].reshape((data.nefc, model.nv))

2. 约束力算出来是 0

原因：<connect> 的 body2="world" 让 anchor 钉在世界。如果 pole 不动（重力被某物平衡），约束力为 0。

检查：

print("efc_force =", data.efc_force)
print("nefc =", data.nefc)

3. pole2 飞得不对（方向/角度错）

可能原因：力移交的几何不严格。代码把约束力直接当 pole2 的关节力矩用，忽略了力臂。

严格做法：

在 pole2 上定义一个等效 anchor（用 <site>）
算这个 anchor 的 Jacobian J_anchor2
qfrc_applied_pole2 = J_anchor2.T @ F0

4. 怎么验证「力移交」真的发生了？

# 在 controller 里加：
print("F0 =", F0.flatten())
print("JT_F =", JT_F.flatten())
print("qfrc_applied[3:6] =", data.qfrc_applied[3:6])

F0 非零 + JT_F 非零 → 力确实在传。

5. 怎么「真的」释放 pole 本身？

代码只移除了 qfrc_applied 的力，没移除 <connect> 约束（XML 里的 body1="pole" 还在）。

要真释放，得在仿真中改 XML（用 mj_deleteConnection 或等价的 Python API）—— 这是 MuJoCo 的另一个大话题（动态模型修改）。

6. 状态变量 `qpos[5]` 是哪个？

是 pole2 的 pin2 角度（DOF 5）。qpos 顺序按 XML 声明：

qpos[0] = pole.x
qpos[1] = pole.z
qpos[2] = pole.pin
qpos[3] = pole2.x2
qpos[4] = pole2.z2
qpos[5] = pole2.pin2   ← FSM 用这个

7. 不用约束力，直接加 `<joint>` 连两个 body 行不行？

可以 —— 但那就不是「可释放」的连接了。No.8 的精妙之处就是用约束当传感器，不用真实关节，保留「释放」的能力。

8. 跟强化学习里的「虚拟力」有什么关系？

No.8 是把约束反力当作 RL 里的奖励塑形信号或辅助控制。MuJoCo 里这种「借约束力」的技巧也常用于接触感知（efc_force 反映接触力大小）。

十、整体公式对应

──────── 系统定义（XML）──────
两个独立 body (pole, pole2) + connect 等式约束
nefc = 3 (x, y, z), nv = 6 (3+3 个 DOF)

──────── 约束求解（mj_step）──────
约束 Jacobian: J (3×6) = ∂constraint/∂q
约束反力:      F0 (3×1) = 拉格朗日乘子
保持 anchor 钉在 (0, 0, 0.5) 所需要的力

──────── 力移交（controller）──────
J_pole = J[:, :3]              (3×3)
f_pole = J_pole.T @ F0         (3×1) ← pole 上要施加的力
data.qfrc_applied[3:6] = f_pole  ← 改写到 pole2 的 DOF

──────── FSM 状态机 ──────
FSM_SWING: 驱动 pin + 移交力 + 检查 pin2 > 1.0
FSM_FREE:  pole2 自由，不再移交

──────── 物理（mj_step）──────
重力 + 约束反力 + 用户的 qfrc_applied → 更新 qpos, qvel

十一、一句话总结

No.8 = 「读约束反力 + 写给另一 body + FSM 释放」。这是把 MuJoCo 的等式约束当传感器用的经典技巧 —— 不修改 XML 也能在两个 body 之间建立可释放的虚拟连接。核心数学是 f_pole = J_pole.T @ F0，核心代码是 FSM 切换 qfrc_applied 的写入。

No.9 单腿跳跃机器人（Hopper）4 状态 FSM

本节介绍一个单腿跳跃机器人控制：4 状态 FSM 模拟完整的「空中 → 落地 → 压缩 → 蹬地 → 再次腾空」跳跃循环，运行时切换 PD 增益实现不同的刚度/阻尼需求。这是 Raibert hopper 风格的经典控制范式，也是强化学习 locomotion 任务的 baseline controller。

文件说明

mujoco/No_9/
├── hopper.xml     # MuJoCo XML 模型文件
└── hopper.py      # 完整脚本：4 状态 FSM + 动态增益切换

No.9 没有最小脚本（no_9.py）。要看效果必须跑 hopper.py。

一、hopper.xml 详解

<mujoco>
    <!-- 【No.9 新增】视觉配置：启用头灯 -->
    <visual>
        <headlight ambient="0.5 0.5 0.5"/>
    </visual>

    <option timestep="0.001" integrator="RK4" gravity="0 0 -9.81">
        <flag contact="enable" energy="enable"/>
    </option>

    <worldbody>
        <!-- 地面（很长 100） -->
        <geom type="plane" size="100 1 0.1" rgba=".9 0 0 1"/>

        <!--
            躯干 torso：sphere 形状（mass=1）
            2 个 slide 关节：x（前进）、z（上下）
        -->
        <body name="torso" pos="0 0 2">
            <joint name="x" type="slide" pos="0 0 0" axis="1 0 0" />
            <joint name="z" type="slide" pos="0 0 0" axis="0 0 1" />
            <geom type="sphere" size="0.1" rgba=".9 .9 .9 1" mass="1"/>

            <!--
                大腿 leg：cylinder（mass=1）
                1 个 hinge 关节：hip（髋关节，绕 y 旋转）
            -->
            <body name="leg" pos="0 0 -0.5" euler="0 0 0">
                <joint name="hip" type="hinge" pos="0 0 0.5" axis="0 -1 0" />
                <geom type="cylinder" size=".05 .5" rgba="0 .9 0 1" mass="1"/>

                <!--
                    脚 foot：cylinder（mass=0，视觉）+ sphere（mass=0.1，碰撞）
                    1 个 slide 关节：knee（膝关节，垂直伸缩）
                -->
                <body name="foot" pos="0 0 -0.75">
                    <joint name="knee" type="slide" pos="0 0 0.25" axis="0 0 -1" />
                    <geom type="cylinder" pos="0 0 0.125" size=".01 .125" rgba="0 0 .9 1" mass="0"/>
                    <geom type="sphere"  size="0.05"            rgba=".9 .9 0 1"  mass="0.1"/>
                </body>
            </body>
        </body>
    </worldbody>

    <!--
        【No.9 关键】4 个 actuator 通道：每个关节都有 position + velocity servo
        初始 kp/kv=0，由 init_controller 和 controller 在运行时设置
    -->
    <actuator>
        <position name="pservo-hip"  joint="hip"  kp="0"/>
        <velocity name="vservo-hip"  joint="hip"  kv="0"/>
        <position name="pservo-knee" joint="knee" kp="0"/>
        <velocity name="vservo-knee" joint="knee" kv="0"/>
    </actuator>
</mujoco>

关键设计说明

1. 机器人形态（4 DOF）

DOF 索引	关节名	类型	物理含义	范围
0	`x`	slide	躯干前后平移	无界
1	`z`	slide	躯干上下平移	无界
2	`hip`	hinge	髋关节旋转	±π
3	`knee`	slide	膝关节伸缩	无界

特点：没有「前进/后退」的主动驱动 —— 跳跃产生的反作用力是唯一的水平推力来源。

2. 4 个 actuator 通道：每个关节 position + velocity servo

<position name="pservo-hip"  joint="hip"  kp="0"/>   <!-- ctrl[0] -->
<velocity name="vservo-hip"  joint="hip"  kv="0"/>   <!-- ctrl[1] -->
<position name="pservo-knee" joint="knee" kp="0"/>   <!-- ctrl[2] -->
<velocity name="vservo-knee" joint="knee" kv="0"/>   <!-- ctrl[3] -->

每个关节有两个 actuator（一个 P、一个 D），它们叠加出力：

F_total = F_position_servo + F_velocity_servo
        = kp · (ctrl[0] - q) + kv · (ctrl[1] - qd)

技巧：通过运行时修改 kp/kv，可以同一个 actuator 在不同状态下表现得像弹簧、阻尼器、刚性关节。

3. `<visual><headlight>`

<visual>
    <headlight ambient="0.5 0.5 0.5"/>
</visual>

启用头灯（跟随相机移动的虚拟光源），让画面有立体感。ambient 是环境光强度。

4. foot 的双 geom 设计

<geom type="cylinder" ... mass="0"/>     <!-- 视觉圆柱 -->
<geom type="sphere"  ... mass="0.1"/>    <!-- 物理碰撞球 -->

一个轻量球（mass=0.1）做接触碰撞，一个零质量圆柱做视觉装饰。这是机器人模型常用的「质量集中」技巧。

二、核心：4 状态 FSM（跳跃循环）

2.1 状态定义

FSM_AIR1   = 0   # 空中，下落中
FSM_STANCE1= 1   # 刚落地，压缩中
FSM_STANCE2= 2   # 蹬地，向上加速
FSM_AIR2   = 3   # 离地后再腾空

2.2 状态转移图

                            脚高度 > 0.05
   ┌────────────────────────────────────────┐
   │                                        │
   ▼                                        │
┌────────┐  脚高<0.05    ┌────────┐  vz>0   ┌────────┐
│ AIR1   │ ──────────────▶│ STANCE1│ ───────▶│ STANCE2│
│  下落   │                │  压缩   │         │  蹬地   │
└────────┘                └────────┘         └────────┘
   ▲                                            │
   │              vz<0                          │
   │  ┌──────────────────────────┐              │
   │  │                                          │
   │  │  脚高>0.05  ▼                             │
   │ ┌────────┐                                  │
   │ │ AIR2   │                                  │
   │ │  腾空   │ ─────────────────────────────────┘
   │ └────────┘

2.3 状态转移条件详解

body_no = 3
z_foot = data.xpos[body_no, 2]     # 脚的世界坐标 z
vz_torso = data.qvel[1]            # 躯干 z 方向速度

# AIR1 → STANCE1：脚触地
if fsm == FSM_AIR1 and z_foot < 0.05:
    fsm = FSM_STANCE1

# STANCE1 → STANCE2：躯干开始上升（压缩反弹）
if fsm == FSM_STANCE1 and vz_torso > 0.0:
    fsm = FSM_STANCE2

# STANCE2 → AIR2：脚离开地面
if fsm == FSM_STANCE2 and z_foot > 0.05:
    fsm = FSM_AIR2

# AIR2 → AIR1：躯干开始下落（完成一个跳跃周期）
if fsm == FSM_AIR2 and vz_torso < 0.0:
    fsm = FSM_AIR1
    step_no += 1

转移	触发条件	物理含义
AIR1 → STANCE1	`z_foot < 0.05`	脚接触地面（脚 z 坐标小于 5cm）
STANCE1 → STANCE2	`vz_torso > 0.0`	躯干开始反弹上升
STANCE2 → AIR2	`z_foot > 0.05`	脚蹬离地面
AIR2 → AIR1	`vz_torso < 0.0`	到达最高点，开始下落

这是事件驱动 FSM（跟 No.5 的时间驱动、No.8 的单事件触发都不同）—— 转移条件是物理量而不是时间或单一标志位。

2.4 body_no = 3 是怎么定的？

MuJoCo 按 XML 声明顺序给 body 编号：

索引	body	来源
0	worldbody	隐含
1	torso	XML 第一个 `<body>`
2	leg	torso 内嵌的 `<body>`
3	foot	leg 内嵌的 `<body>`

所以 body_no = 3 是 foot。这是个脆弱的硬编码，XML 一改就错。

三、动态增益切换：每个状态不同刚度/阻尼

3.1 状态-增益对照表

状态	pservo-hip kp	vservo-hip kv	pservo-knee kp	vservo-knee kv	ctrl[0]
AIR1	100	10	100	10	0
STANCE1	1000	0	1000	0	0
STANCE2	1000	0	1000	0	-0.2
AIR2	100	10	100	10	0

规律：

空中（AIR1, AIR2）：kp=100, kv=10 —— 软弹簧 + 阻尼，落地不冲击
着地（STANCE1, STANCE2）：kp=1000, kv=0 —— 硬弹簧、无阻尼，存储弹性势能
蹬地（STANCE2）：髋关节目标设为 -0.2 rad —— 腿向后摆，把身体「弹」出去

3.2 增益函数

def set_position_servo(actuator_no, kp):
    model.actuator_gainprm[actuator_no, 0] = kp
    model.actuator_biasprm[actuator_no, 1] = -kp

def set_velocity_servo(actuator_no, kv):
    model.actuator_gainprm[actuator_no, 0] = kv
    model.actuator_biasprm[actuator_no, 2] = -kv

3.3 运行时改增益的机制

model.actuator_gainprm 和 model.actuator_biasprm 是模型参数，但 MuJoCo 允许运行时修改。

MuJoCo 通用 actuator 模型：

actuator_force = gainprm[0] · ctrl + biasprm[0]
               + biasprm[1] · (actuated_quantity)         ← 位置反馈
               + biasprm[2] · (actuated_velocity)         ← 速度反馈
               + biasprm[3] · ctrl²                       ← 二阶项（一般不用）

对于位置伺服（actuator_gainprm[0] = kp, biasprm[1] = -kp）：

F = kp · ctrl + (-kp) · q
  = kp · (ctrl - q)

对于速度伺服（actuator_gainprm[0] = kv, biasprm[2] = -kv）：

F = kv · ctrl + (-kv) · qd
  = kv · (ctrl - qd)

关键设计：gainprm[0] 既可能是 P 也可能是 D 的系数，靠 biasprm 的索引区分作用位置。这是个非常紧凑的通用模型。

3.4 为什么 STANCE 阶段 kv=0？

蹬地时不要阻尼 —— 你想让弹簧完全弹性地释放能量。如果有阻尼，弹性势能会被消耗而不是全部转化为动能。

阶段	kp	kv	行为
空中	100	10	软着地、摆动平稳
落地	1000	0	储能（弹簧压缩）
蹬地	1000	0	释能（弹簧反弹）

没有阻尼意味着「完全弹性碰撞」—— 落到地面的动能全部存进弹簧，弹起时全部释放。这就是 Raibert hopper 的精髓。

四、视觉：headlight + 跟随相机

4.1 headlight

<visual>
    <headlight ambient="0.5 0.5 0.5"/>
</visual>

注释掉了两个 <light> 元素。headlight 是相机跟随的虚拟光源，自然地随相机移动，照亮场景。

4.2 跟随相机

cam.lookat[0] = data.qpos[0]   # 相机 lookat 的 x 跟随躯干 x

# 在主循环里（注意：原始代码里是在 mj_step 之后）
cam.lookat[0] = data.qpos[0]
mj.mjv_updateScene(model, data, opt, None, cam, ...)

效果：相机永远盯着躯干，hopper 跳到哪，相机就看到哪。

小 bug：cam.lookat 是 numpy 数组，但 cam.lookat[0] = data.qpos[0] 这种属性赋值在某些 MuJoCo 版本里不会真正更新到 MjrRect。可能需要 cam.lookat = np.array([data.qpos[0], 0, 1.5]) 重新赋值。

五、init_controller / controller 分工

5.1 init_controller（启动时调一次）

def init_controller(model, data):
    set_position_servo(0, 100)   # pservo-hip
    set_velocity_servo(1, 10)    # vservo-hip
    set_position_servo(2, 1000)  # pservo-knee
    set_velocity_servo(3, 0)     # vservo-knee

默认值：髋软、膝硬。然后 controller 会根据状态重新设。

5.2 controller（每物理步调一次）

if fsm == FSM_AIR1:
    set_position_servo(2, 100)   # 膝软
    set_velocity_servo(3, 10)    # 膝阻尼

if fsm == FSM_STANCE1:
    set_position_servo(2, 1000)  # 膝硬
    set_velocity_servo(3, 0)     # 膝无阻尼

if fsm == FSM_STANCE2:
    set_position_servo(2, 1000)  # 膝硬
    set_velocity_servo(3, 0)     # 膝无阻尼
    data.ctrl[0] = -0.2          # 髋目标 -0.2 rad（腿后摆）

if fsm == FSM_AIR2:
    set_position_servo(2, 100)   # 膝软
    set_velocity_servo(3, 10)    # 膝阻尼
    data.ctrl[0] = 0.0           # 髋目标归零

只有 pservo-hip（ctrl[0]）在 STANCE2 期间被显式设目标 —— 用来在蹬地瞬间把腿向后甩。其他时候 ctrl[0]=0，意思是「腿保持竖直」。

六、跟 No.5/No.8 FSM 的对比

维度	No.5 FSM	No.8 FSM	No.9 FSM
状态数	4	2	4
触发条件	时间	单一物理量（`qpos[5] > 1.0`）	多个物理量（z_foot, vz_torso）
触发类型	时间驱动	事件驱动	多事件驱动
切换内容	切 PD 参考	切 `qfrc_applied`	切 kp/kv + 切 ctrl[0]
是否运行时改模型	❌	❌	✅ 改 `actuator_gainprm`
应用域	关节空间	约束力	完整运动周期

控制思想对比

No.5: 时间表 → 切任务（HOLD / SWING1 / SWING2 / STOP）
No.8: 单事件 → 切物理交互（SWING / FREE）
No.9: 多事件 + 动态增益 → 切运动阶段（AIR / STANCE / 蹬地）

No.9 是第一个把「运行时调整模型参数」作为控制手段的例。

七、整体控制流程图

启动:  init_controller(model, data)
  ├─ 设默认增益: pservo-hip kp=100, vservo-hip kv=10
  ├─ 设默认增益: pservo-knee kp=1000, vservo-knee kv=0
  └─ mj.set_mjcb_control(controller)

主循环 (60Hz) ─────────────────────────────────────
  内层 1000Hz: mj_step → 每步自动调 controller():
  │
  │   读 z_foot = data.xpos[3, 2]
  │   读 vz_torso = data.qvel[1]
  │
  │   ┌─ 状态转移（多条件检查）─┐
  │   │  AIR1  + z_foot<0.05  → STANCE1
  │   │  STANCE1 + vz>0       → STANCE2
  │   │  STANCE2 + z_foot>0.05 → AIR2
  │   │  AIR2  + vz<0         → AIR1
  │   └────────────────────────┘
  │
  │   ┌─ 状态-增益映射 ─┐
  │   │  AIR*:   kp=100,  kv=10    (软着地)
  │   │  STANCE: kp=1000, kv=0     (储能/释能)
  │   │  STANCE2: ctrl[0] = -0.2   (腿后摆)
  │   └────────────────┘
  │
  │   set_position_servo(2, kp)   ← 改 model 参数
  │   set_velocity_servo(3, kv)   ← 改 model 参数
  │   data.ctrl[0] = target       ← 改控制目标
  │
  └─ 物理: mj_step 应用所有力，更新 qpos, qvel
  外层: 渲染 + cam.lookat[0] = data.qpos[0]  (跟随)

八、运行方法

cd mujoco/No_9/
mjpython hopper.py

预期效果：

Hopper 原地（或缓慢前进）跳跃
大约 0.5-1 秒一跳，step_no 累计
simend = 20 秒应该看到 15-25 跳
相机自动跟随（lookat[0] = qpos[0]）

九、调试 / 验证方法

1. 打印状态和步数

def controller(model, data):
    global fsm, step_no
    body_no = 3
    z_foot = data.xpos[body_no, 2]
    vz_torso = data.qvel[1]
    print(f"t={data.time:.2f}  fsm={fsm}  z_foot={z_foot:.3f}  vz={vz_torso:.2f}  step={step_no}")
    # ... 原有代码

2. 验证增益确实被改了

def controller(model, data):
    print(f"hip kp={model.actuator_gainprm[0, 0]:.0f}  "
          f"hip kv={model.actuator_gainprm[1, 0]:.0f}  "
          f"knee kp={model.actuator_gainprm[2, 0]:.0f}  "
          f"knee kv={model.actuator_gainprm[3, 0]:.0f}")

3. 调参方向

想改	改什么
跳得更高	增大 `pservo-knee` 的 kp，或延长 STANCE 阶段
跳得更稳	增大 `vservo-knee` 在 AIR 阶段的 kv
跳得更远	STANCE2 时设 `data.ctrl[0] = -0.5`（腿更向后摆）
跳得更快	把状态转移阈值 `0.05` 改小（更快检测着地/离地）

十、常见问题

1. Hopper 跳不起来

原因：STANCE 阶段 kp 太小，没有储能。

解决：

增大 pservo-knee 在 STANCE 的 kp（从 1000 试到 2000）
检查 vz_torso 是不是真的能 > 0
试着增加躯干质量（gravity 改小也行）

2. Hopper 触地后「粘在地上」

原因：蹬地力度不够，弹不起来。

解决：

检查 STANCE2 状态有没有真的进入（看 z_foot > 0.05）
data.ctrl[0] = -0.2 改大（比如 -0.5）
vservo 在 STANCE 阶段必须 kv=0，不然会消耗弹性势能

3. `body_no = 3` 报错 / 不对

原因：XML 改了，body 顺序变了。

解决：用名字查 id 而不是硬编码：

foot_id = mj.mj_name2id(model, mj.mjtObj.mjOBJ_BODY, "foot")
z_foot = data.xpos[foot_id, 2]

4. `data.xpos[body_no, 2]` 是脚的哪点？

是 foot body 质心的 z 坐标（不是脚底）。脚底可能比质心低 0.05m 左右，所以阈值 0.05 实际上对应脚底刚刚触地。

5. `cam.lookat[0] = data.qpos[0]` 没生效

原因：某些 MuJoCo 版本对 MjvCamera.lookat 的元素赋值不更新底层 C 结构。

解决：

cam.lookat = np.array([data.qpos[0], 0.0, 1.5])  # 整体赋值

6. 运行时改 `actuator_gainprm` 会有性能影响吗？

没有。这是直接修改内存里的 float 值，下一步 mj_step 就用新值。对仿真速度零影响。

但有限制：必须每步改才能持续生效（虽然 model 是持久的，但如果你重置 data 会保留 model 的修改）。

7. 为什么没有 LQR、IK、反馈线性化这些高级控制？

因为跳跃是高度非线性 + 不连续的（着地瞬间）。这些方法都基于「线性化点附近」假设，跳跃时严重偏离任何平衡点。

FSM + 动态增益是事件驱动的简单方案，鲁棒且可解释，是经典 Raibert hopper 的做法。

8. 跟强化学习里的 locomotion 任务什么关系？

这是经典控制（model-based FSM），RL 的 PPO/SAC 是学习控制（model-free）。RL baseline 通常也是 4 状态 FSM + 简单 PD，但学习状态转移的精确时机和增益。

十一、整体公式对应

─────── 机器人形态 ───────
torso (sphere, mass=1) + leg (cylinder, mass=1) + foot (sphere, mass=0.1)
nv = 4 (x, z, hip, knee)
4 个 actuator: 2 关节 × (position + velocity) servo

─────── 状态机 ───────
fsm ∈ {AIR1, STANCE1, STANCE2, AIR2}
转移条件: 脚高度 / 躯干速度 阈值

─────── 控制律 ───────
每个状态一对增益 (kp, kv):
  F_joint = kp · (ctrl_target - q) + kv · (ctrl_vel_target - qd)
运行时改 model.actuator_gainprm / biasprm

─────── 周期 ───────
AIR1  (0.4s)  → STANCE1  (0.05s)
     → STANCE2  (0.05s)  [data.ctrl[0]=-0.2 腿后摆]
     → AIR2    (0.4s)   [step_no++]
     → AIR1

十二、一句话总结

No.9 = 「4 状态事件驱动 FSM + 运行时动态增益切换」。这是 Raibert hopper 风格的经典控制范式 —— 跳跃周期用物理事件（脚高度、躯干速度）分段，每段用不同的 PD 增益实现软着地 / 硬储能 / 完全弹性反弹。核心创新是 set_position_servo / set_velocity_servo 运行时改 actuator_gainprm，让同一个 actuator 在不同时刻扮演弹簧/阻尼器/刚性关节。

No.11 抛射体轨迹优化（NLopt 非线性规划）

本节介绍一个完全不同的控制范式 —— 离线轨迹优化（trajectory optimization）。前 9 节都是「在线」控制器：每步读状态、算控制量。No.11 是「离线」求解：先用 NLopt 找最优初始速度（v, θ），然后开环播放让小球飞向目标。

核心思想：把仿真器当作「约束求值器」，扔给优化器，让算法自己找参数。

文件说明

mujoco/No_11/
├── ball.xml             # MuJoCo XML 模型文件（地面 + 圆柱 + 球 + 目标盒）
└── projectile_opt.py    # 完整脚本：含 NLopt 优化、simulator、开环播放

No.11 没有最小脚本（no_11.py）。要看效果必须跑 projectile_opt.py。

一、ball.xml 详解

<mujoco>
    <visual>
        <headlight ambient="0.5 0.5 0.5"/>
    </visual>

    <worldbody>
        <geom type="plane" size="100 1 0.1" rgba=".9 0 0 1"/>

        <!-- 【No.11 关键】视觉参考柱 -->
        <geom type="cylinder" pos="5 0 1" size="0.2 1" rgba="0.9 0.9 0.9 1" />

        <!--
            抛射体（球）：
            pos="0 0 0.1"（地面附近）
            <joint type="free"/>：6 DOF 自由关节
        -->
        <body pos="0 0 .1">
            <joint type="free"/>
            <geom type="sphere" size=".1" rgba="0 .9 0 1" mass="1"/>
        </body>

        <!--
            目标（盒）：
            pos="5 0 2.1"（圆柱顶上）
            同样 free 关节
        -->
        <body pos="5 0 2.1">
            <joint type="free"/>
            <geom type="box" size="0.1 0.1 0.1" rgba="0.9 0.9 0 1" mass="0.1"/>
        </body>
    </worldbody>
</mujoco>

场景布局

                  ┌──┐  ← 目标盒 (5, 0, 2.1)
                  │  │
                  │  │
       ┌──────────┴──┴──────────┐
       │  cylinder (5, 0, 1)    │   ← 视觉参考柱
       │  size="0.2 1"          │
       └──────────┬──────────────┘
                  │
══════════════════╪═══════════════  ← 地面
       ●                              ← 抛射体 (0, 0, 0.1)
       ball (mass=1)

物体	位置	作用
地面	z=0	球碰到就停
圆柱	(5, 0, 1)，半径 0.2，高 2	视觉参考柱（无质量）
球	(0, 0, 0.1)，mass=1	抛射体，free 关节
盒	(5, 0, 2.1)，mass=0.1	目标，需要被击中

关键设计：圆柱只是视觉（无质量），目标是盒（不是柱顶）。这个反直觉设计让优化目标精确（不用算柱顶坐标）。

二、核心：弹道优化（NLopt）

2.1 优化问题定义

        min_X      0
   subject to 0.1 ≤ v   ≤ 10000
              0.1 ≤ θ   ≤ π/2 - 0.1
              0.1 ≤ T   ≤ 10000
              x(T) = 5.0      ← 落点 x
              z(T) = 2.1      ← 落点 z

3 个决策变量：X = [v, θ, T]（初速度大小、发射角、飞行时间）

目标函数：恒为 0（可行性问题，找满足约束的参数即可）

2 个等式约束：飞行 T 秒后，位置 (x, z) 必须等于目标 (5.0, 2.1)

2.2 为什么需要优化？

经典抛物线公式的解析解存在（v = sqrt(g·R² / (2·cos²θ·(tanθ - H/R))），但：

仿真器有空气阻力、接触、积分误差等复杂因素
想在「真实仿真」里命中目标，解析解不准确
用仿真器本身作为约束求值器，可以自动处理这些细节

2.3 优化算法：COBYLA

opt = nlopt.opt(nlopt.LN_COBYLA, 3)

属性	值	含义
算法	`LN_COBYLA`	无导数、约束优化算法
维度	3	决策变量数 = 3

为什么 COBYLA：

不需要梯度（仿真器的约束没法解析求导）
支持等式/不等式约束
对小规模、低维问题效率够用

2.4 完整代码

def optimize_ic(x):
    opt = nlopt.opt(nlopt.LN_COBYLA, 3)
    opt.set_lower_bounds([0.1, 0.1, 0.1])
    opt.set_upper_bounds([10000.0, np.pi/2 - 0.1, 10000.0])
    opt.set_min_objective(cost_func)             # cost = 0
    opt.add_equality_mconstraint(equality_constraints, tol=[1e-4, 1e-4])
    opt.set_xtol_rel(1e-4)
    sol = opt.optimize(x)
    return sol

def cost_func(x, grad):
    return 0.0                                   # 可行性问题，cost 恒为 0

def equality_constraints(result, x, grad):
    pos = simulator(x)                          # 仿真 → 终态位置
    result[0] = pos[0] - 5.0                    # x 误差
    result[1] = pos[1] - 2.1                    # z 误差

simulator(x) 是核心：把决策变量 (v, θ, T) 转成「仿真 T 秒后」的位置。这个函数是优化器的「黑箱」。

三、simulator 函数详解：仿真器当作约束求值器

def simulator(x):
    v, theta, time_of_flight = x[0], x[1], x[2]

    # 1. 设置初速度
    data.qvel[0] = v * np.cos(theta)    # x 方向
    data.qvel[2] = v * np.sin(theta)    # z 方向

    # 2. 仿真 T 秒
    while data.time < time_of_flight:
        mj.mj_step(model, data)

    # 3. 读终态位置
    pos = np.array([data.qpos[0], data.qpos[2]])

    # 4. 重置 data（为下一次调用准备）
    mj.mj_resetData(model, data)

    return pos

3.1 输入 → 输出

输入	含义
`x[0]` = v	速度大小
`x[1]` = θ	发射角（与水平面夹角）
`x[2]` = T	飞行时间

输出	含义
`pos[0]`	T 秒后的 x 坐标
`pos[1]`	T 秒后的 z 坐标

3.2 为什么用 `data.time` 作为循环条件？

while data.time < time_of_flight:
    mj.mj_step(model, data)

data.time 由 MuJoCo 内部维护，每一步 mj_step 增加 model.opt.timestep。当 data.time 达到 time_of_flight 时停止。

3.3 为什么 `mj_resetData` 放在最后？

每次 simulator 调用都会修改全局 data（设初速度、跑仿真）。如果不重置，下一次调用会从「上一次终态」开始 → 结果完全错。

mj_resetData 把 data.qpos 和 data.qvel 复位到 XML 里的初始值。

潜在 bug：simulator 假设初始 qpos 和 qvel 就是 XML 默认值。如果你在调用 simulator 之前先动了 data，状态会污染。

3.4 `qvel[0]`、`qvel[2]` 的索引

<joint type="free"/> 给 body 6 个 DOF（3 平移 + 3 旋转）：

`qvel` 索引	含义	No.11 是否用到
0	x 方向线速度	✅
1	y 方向线速度	❌
2	z 方向线速度	✅
3	绕 x 角速度	❌
4	绕 y 角速度	❌
5	绕 z 角速度	❌

所以 qvel[0] = v·cos(θ)、qvel[2] = v·sin(θ)。

四、init_controller：找最优 v, θ, T

def init_controller(model, data):
    # 初始猜测
    v = 10.0
    theta = np.pi / 4
    time_of_flight = 2.0

    if NLOPT_IMPORTED:
        sol = optimize_ic(np.array([v, theta, time_of_flight]))
    else:
        sol = np.array([9.398687489285555, 1.2184054599970882, 1.5654456340479144])

    v_sol, theta_sol = sol[0], sol[1]
    simend = sol[2] + 2  # 仿真时间稍长于飞行时间

    data.qvel[0] = v_sol * np.cos(theta_sol)
    data.qvel[2] = v_sol * np.sin(theta_sol)

4.1 Fallback 机制

try:
    import nlopt
except ImportError:
    print("nlopt not imported, switching to pre-computed solution")
    NLOPT_IMPORTED = False

鲁棒性设计：如果没装 nlopt，用预计算解 sol = [9.40, 1.22, 1.57]。

预计算解 = 优化器对初始猜测 [10, π/4, 2] 跑出来的解。

4.2 初始猜测的物理意义

变量	初始值	物理含义
v	10.0 m/s	较快速度（够得着 5m 远）
θ	π/4 ≈ 45°	经典最优抛射角（无空气阻力时）
T	2.0 s	估计飞行时间

4.3 simend 的设计

simend = sol[2] + 2    # 仿真时间比飞行时间长 2 秒

飞行 T 秒后球已经落地。多给 2 秒让球在地面滚/停，方便看效果。

五、controller 详解：空函数

def controller(model, data):
    pass

这是 No.11 最大的特点：

之前 No.4-9	No.11
controller 每步算 `u`	controller 什么都不做

为什么？因为：

优化器已经算好了 v 和 θ
init_controller 已经把 qvel[0] 和 qvel[2] 设成最优值
之后开环播放，让物理引擎自己把球送过去
不需要任何反馈

这就是「开环控制」的极致形式 —— 控制器不存在。

六、主循环：开环播放

init_controller(model, data)        # 离线找最优 v, θ
mj.set_mjcb_control(controller)     # 注册空 controller

while not glfw.window_should_close(window):
    simstart = data.time

    while (data.time - simstart < 1.0/60.0):
        mj.mj_step(model, data)      # 物理步进，**没有控制输入**

    if (data.time >= simend):
        break

    # 相机跟随球
    cam.lookat[0] = data.qpos[0]
    mj.mjv_updateScene(...)
    mj.mjr_render(...)
    glfw.swap_buffers(window)
    glfw.poll_events()

整个主循环没有读状态、算控制量、写 actuator —— 纯粹的物理仿真 + 渲染。

七、跟 No.4-9 的本质区别

维度	No.4-9 在线控制	No.11 离线优化
决策时机	每步	一次性（init_controller）
反馈	必须有	不需要（开环）
控制律	`u = K(x)`, `Δq = J⁻¹·Δx` 等	没有控制律
目标	跟踪/镇定/轨迹	一次性命中目标
方法	PD、IK、LQR、FSM	非线性规划（NLopt）
仿真器角色	物理引擎	约束求值器
失败恢复	可以反馈纠错	不行（开环）
计算成本	每步 O(n²) ~ O(n³)	离线 O(N) 次仿真
鲁棒性	中（取决于控制器）	差（参数不准就 miss）

控制思想对比

No.4-9:  「**在线**」
  每步: 读 x → 算 u → 写 actuator
  优点: 鲁棒于扰动和参数误差
  缺点: 需要设计控制器、调参

No.11:  「**离线**」
  一次性: 找参数 (v, θ, T) → 设初速度 → 开环播放
  优点: 不需要控制器，理论最优
  缺点: 完全开环，扰动即失败

No.11 是「模型预测 + 开环执行」的最简形式。

八、整体流程图

启动 ───────────────────────────────────────────
  │
  ├─ 加载 ball.xml
  ├─ 创建 model, data
  │
  ├─ init_controller:
  │    ├─ 设初始猜测 (v=10, θ=π/4, T=2)
  │    │
  │    └─ 调 optimize_ic:
  │         │
  │         └─ 循环（COBYLA 内部）:
  │              │
  │              ├─ 提议新参数 (v', θ', T')
  │              │
  │              ├─ 调 simulator(v', θ', T'):
  │              │     ├─ 设 qvel
  │              │     ├─ while data.time < T': mj_step
  │              │     ├─ 读 (qpos[0], qpos[2])
  │              │     └─ mj_resetData
  │              │
  │              └─ 算约束违反: (pos - target)
  │
  ├─ 解: v_sol, θ_sol, T_sol
  ├─ data.qvel[0] = v_sol * cos(θ_sol)
  ├─ data.qvel[2] = v_sol * sin(θ_sol)
  └─ simend = T_sol + 2

主循环 ───────────────────────────────────────────
  每帧 (60Hz):
    内层 1000Hz: mj_step（**无控制**）
    外层: 渲染 + 相机跟随

  最终: 球**精准**落在目标盒 (5, 0, 2.1) 附近

九、运行方法

# 1. 安装依赖
pip install nlopt numpy

# 2. 运行
cd mujoco/No_11/
mjpython projectile_opt.py

预期效果：

球从 (0, 0, 0.1) 出发
按优化器算的 (v, θ) 抛射
精准落在 (5, 0, 2.1) 的目标盒上
相机自动跟随

⚠️ 第一次启动会卡 1-3 秒 —— NLopt 在跑优化（大约 50-200 次 simulator 调用）。这是离线代价。

十、调参 / 玩转

1. 改目标位置

# 改 XML
<body pos="5 0 2.1">  →  <body pos="7 0 3.0">

# 改约束
result[0] = pos[0] - 5.0  →  result[0] = pos[0] - 7.0
result[1] = pos[1] - 2.1  →  result[1] = pos[1] - 3.0

2. 加快优化速度

opt.set_xtol_rel(1e-4)  →  opt.set_xtol_rel(1e-2)   # 粗糙一点
tol = [1e-4, 1e-4]      →  tol = [1e-2, 1e-2]        # 约束放宽

3. 改用更快的算法

opt = nlopt.opt(nlopt.LN_COBYLA, 3)         # 慢但稳
opt = nlopt.opt(nlopt.LN_NELDERMEAD, 3)    # 单纯形，无约束
opt = nlopt.opt(nlopt.GN_AGS, 3)           # 全局优化（慢但能找到全局最优）

4. 加成本函数

def cost_func(x, grad):
    # 最小化发射能量
    return 0.5 * x[0]**2

# 注意：这样 min 不再是 0，是 0.5·v²
# 优化器会找「**最省力**」的命中方式

十一、常见问题

1. `ImportError: No module named nlopt`

解决：

# macOS
brew install nlopt
pip install nlopt

# 或 conda
conda install -c conda-forge nlopt

装不上就用预计算解（代码里已经 fallback）。

2. 球飞出去没命中目标

可能原因：

数值精度不够（tol=[1e-4, 1e-4] 太松）
初始猜测离真实解太远，COBYLA 陷入局部
仿真器本身有 bug（mj_resetData 顺序错）

调试：

def simulator(x):
    print(f"  try v={x[0]:.2f} θ={x[1]:.2f} T={x[2]:.2f}")
    v, theta, time_of_flight = x[0], x[1], x[2]
    data.qvel[0] = v * np.cos(theta)
    data.qvel[2] = v * np.sin(theta)
    while data.time < time_of_flight:
        mj.mj_step(model, data)
    pos = np.array([data.qpos[0], data.qpos[2]])
    mj.mj_resetData(model, data)
    print(f"    → pos=({pos[0]:.2f}, {pos[1]:.2f})")
    return pos

3. 优化要好几秒

原因：COBYLA 是无梯度算法，需要很多次 simulator 调用（每次 ~1000 步仿真）。

加速：

用有限差分给 grad 参数填值，配合 LD_MMA（需要梯度）
用更小的 tol
减少决策变量数

4. 球飞完砸穿了地面

原因：积分器在大 v 下不稳定。

解决：

减小 timestep（XML 里的）
增大阻尼（无中生有……这个例子没有）

5. `simulator` 重置 data 之后，初始条件变了？

原因：mj_resetData 把 qpos 恢复到 XML 里的值。如果之后再调 simulator，初始 v 又是基于这个新 qpos。

验证：

print(data.qpos)  # 调用 simulator 后
# 应该是 [0, 0, 0.1, 1, 0, 0, 0] (位置+四元数)

6. 为什么目标盒是 free 关节？

因为目标盒也要被重力影响（mass=0.1）。如果用固定关节，撞上去会刚性弹开。free 关节让它能跟着被撞飞，物理上更真实。

7. 跟 MPC 什么关系？

	No.11 离线优化	MPC（在线）
求解时机	一次性	每个控制步
计算预算	多	少
反应扰动	❌	✅
实现复杂度	简单	复杂

No.11 是 MPC 的「最简离线版」。

8. `data.qvel[0]` 是线速度还是广义速度？

对 free joint，是线速度（单位 m/s）。对 hinge joint，是角速度（rad/s）。这是约定。

9. 怎么改成「加空气阻力」？

XML 里加：

<option density="1.225"/>  <!-- 空气密度 -->

MuJoCo 会自动算空气阻力。约束函数会相应改变（解析公式不准确了，必须靠优化器）。

10. 优化器解不稳定（每次结果不一样）

原因：COBYLA 是确定性的，但浮点累积误差可能让结果差 1e-3。

解决：

np.random.seed(0) （如果有随机性）
用 set_xtol_rel(1e-6) 更紧的容差
多目标：用全局算法（GN_AGS）

十二、整体公式对应

──────── 优化问题（数学）───────
min_{v, θ, T}      0
subject to:
    0.1 ≤ v ≤ 10000
    0.1 ≤ θ ≤ π/2 - 0.1
    0.1 ≤ T ≤ 10000
    x_simulator(v, θ, T) = 5.0
    z_simulator(v, θ, T) = 2.1

──────── 仿真器（约束求值）───────
simulator(v, θ, T):
    qvel[0] = v·cos(θ)
    qvel[2] = v·sin(θ)
    while time < T: mj_step
    return (qpos[0], qpos[2])

──────── 优化器（COBYLA）───────
sol = nlopt.LN_COBYLA.optimize([v, θ, T])
~ 50-200 次 simulator 调用

──────── 开环播放（主循环）───────
mj_step(model, data)  # 无控制输入
球从初速度自然飞到目标

十三、一句话总结

No.11 = 「离线轨迹优化 + 开环播放」。把仿真器当作约束求值器扔给 NLopt，让 COBYLA 算法自动找最优 (v, θ, T) 让球命中目标。没有控制律、没有反馈、controller 是空函数 —— 这是「模型预测 + 开环执行」的最简形式，也是 No.4-9 在线控制范式的对照面。

No.12 双摆 Lemniscate 数值逆运动学

本节把 No.6 的解析 Jacobian IK 升级为 NLopt 数值 IK，并把目标轨迹从圆换成 Lemniscate（∞ 字形曲线）。核心思想是：把仿真器当作「黑箱 FK」，扔给 COBYLA 优化器，让算法自己解 IK，不用手算 Jacobian。

核心对比：No.6 = 解析 IK（手算 J⁻¹）；No.12 = 数值 IK（NLopt 自动解）。各有取舍。

文件说明

mujoco/No_12/
├── manipulator.xml      # MuJoCo XML 模型文件
└── manipulator_ik.py    # 完整脚本：含 NLopt IK、Lemniscate 轨迹、matplotlib 可视化

No.12 没有最小脚本（no_12.py）。要看效果必须跑 manipulator_ik.py。

⚠️ 需要额外依赖：pip install nlopt matplotlib

一、manipulator.xml 详解（对比 No.6）

<mujoco>
    <option timestep="0.001" integrator="RK4" gravity="0 0 0">  <!-- 重力关 -->
        <flag energy="enable" contact="disable" />
    </option>

    <worldbody>
        <light diffuse=".5 .5 .5" pos="0 0 3" dir="0 0 -1"/>
        <geom type="plane" size="1 1 0.1" rgba=".9 0 0 1"/>

        <body pos="0.5 0 1.25" euler="0 90 0">
            <joint name="pin" type="hinge" axis="0 -1 0" pos="0 0 -0.5"/>
            <geom type="cylinder" size="0.05 0.5" rgba="0 .9 0 1" mass="1"/>
            <body pos="0 0.1 1" euler="0 0 0">
                <joint name="pin2" type="hinge" axis="0 -1 0" pos="0 0 -0.5"/>
                <geom type="cylinder" size="0.05 0.5" rgba=".9 .9 .9 1" mass="1"/>
                <site name="tip" pos="0 0 0.5" size="0.1" />
            </body>
        </body>
    </worldbody>

    <actuator>
        <position name="pservo1" joint="pin"  kp="100" />
        <velocity name="vservo1" joint="pin"  kv="10"  />
        <position name="pservo2" joint="pin2" kp="100" />
        <velocity name="vservo2" joint="pin2" kv="10"  />
    </actuator>

    <sensor>
        <framepos    objtype='site' objname='tip' />
        <framelinvel objtype='site' objname='tip' />
    </sensor>
</mujoco>

XML 跟 No.6 几乎一样

配置项	No.6	No.12
关节	`pin`, `pin2`	同
`euler`	`0 90 0`	同
末端 site	`<site name="endeff">`	`<site name="tip">`（改名）
重力	`gravity="0 0 0"`	同
actuator	2 position + 2 velocity servo	同
sensor	framepos + framelinvel	同
timestep	0.0001	0.001（粗 10 倍）

唯一明显差异：site 名字从 endeff 改成 tip，timestep 粗 10 倍。其他完全一致。

二、Lemniscate（∞ 字形）轨迹

2.1 参数方程

def get_lemniscate_ref(t):
    wt = omega * t
    denominator = 1 + np.sin(wt)**2

    x = center_x + (a * np.cos(wt)) / denominator
    z = center_z + (a * np.sin(wt) * np.cos(wt)) / denominator
    return np.array([x, z])

Lemniscate of Bernoulli 的标准参数化（极坐标版转笛卡尔）：

x(t) = a·cos(ωt) / (1 + sin²(ωt))
z(t) = a·sin(ωt)·cos(ωt) / (1 + sin²(ωt))

2.2 形状示意

       z
       ↑
   ╭───╮ ╭───╮
   │   │ │   │       ← ∞ 字
   ╰───┼─┼───╯
       └─┘───→ x
   ╭───╮ ╭───╮
   │   │ │   │
   ╰───╯ ╰───╯

2.3 参数含义

参数	值	含义
`a`	0.25	半轴长（控制 ∞ 字大小）
`omega`	0.4	角频率（rad/s）
周期	`2π/ω ≈ 15.7s`	画完一个 ∞ 需 15.7 秒

2.4 simend 设计

simend = 0.25 + 2 * np.pi / omega    # ≈ 15.95 秒

0.25 秒 启动 + 一个完整周期（2π/ω）。

2.5 圆心锚定

def init_controller(model, data):
    end_eff_pos = forward_kinematics([-0.5, 1.0])
    center_x = end_eff_pos[0] - 0.25       # 圆心 x = 当前末端 x - a
    center_z = end_eff_pos[1]              # 圆心 z = 当前末端 z

为什么要锚定？让 Lemniscate 从末端当前位置附近开始，自然过渡，而不是凭空出现。

三、核心：数值 IK（NLopt）

3.1 优化问题

        min_{q1, q2}      0
   subject to -π ≤ q1 ≤ π
              -π ≤ q2 ≤ π
              FK(q) = X_target

维度	No.6 解析 IK	No.12 数值 IK
决策变量	Δq（关节修正）	q1, q2（绝对关节角）
求解方法	`J⁻¹ · Δx`	NLopt COBYLA
目标	误差最小	可行性（cost=0）
约束	无	2 个等式约束（末端 = 目标）
求解时间	O(1) 微秒级	O(N) ms 级（N=迭代次数）

3.2 完整 IK 函数

def inverse_kinematics(x):
    opt = nlopt.opt(nlopt.LN_COBYLA, 2)
    opt.set_lower_bounds([-np.pi, -np.pi])
    opt.set_upper_bounds([np.pi, np.pi])
    opt.set_min_objective(cost_func)
    tol = [1e-4, 1e-4]
    opt.add_equality_mconstraint(equality_constraints, tol)
    opt.set_xtol_rel(1e-4)
    sol = opt.optimize(x)
    return sol

完全没用到任何关于这个机械臂的 Jacobian 或 DH 参数 —— 纯靠仿真器的 FK 当黑箱。

3.3 约束函数 = 仿真器当 FK

def equality_constraints(result, x, grad):
    end_eff_pos = forward_kinematics(x)        # 调仿真器算 FK
    result[0] = end_eff_pos[0] - X_target[0]  # x 误差
    result[1] = end_eff_pos[1] - X_target[1]  # z 误差

跟 No.11 的 simulator 思路一致：仿真器 = 约束求值器。

四、forward_kinematics：单独 `data_sim` 的妙用

4.1 关键设计

# 主代码里（第 259 行）
data_sim = mj.MjData(model)     # 独立的 FK 仿真数据

为什么需要独立的 data_sim？

用途	用什么 data
主仿真（动画显示）	`data`
IK 计算（每步调一次）	`data_sim`

好处：

IK 计算不污染主仿真的状态
IK 调 mj_forward 不会反过来影响主仿真
主仿真跑 1000 步，IK 跑 N 次（不同 q 试探），互不干扰

4.2 实现

def forward_kinematics(q):
    data_sim.qpos[0] = q[0]
    data_sim.qpos[1] = q[1]
    data_sim.ctrl[0] = data_sim.qpos[0]     # ← 多余，见 FAQ
    data_sim.ctrl[2] = data_sim.qpos[1]

    mj.mj_forward(model, data_sim)

    end_eff_pos = np.array([
        data_sim.sensordata[0],   # tip x
        data_sim.sensordata[2]    # tip z
    ])
    return end_eff_pos

关键：mj_forward 不消耗时间步，只算一次前向运动学。

五、controller 详解

def controller(model, data):
    global X_target

    # 1. 算当前时刻的 Lemniscate 目标
    X_target = get_lemniscate_ref(data.time)

    # 2. 当前关节角作为初始猜测
    qpos = np.array([data.qpos[0], data.qpos[1]])

    # 3. 调 NLopt 解 IK
    sol = inverse_kinematics(qpos)

    # 4. 写 position servo 目标
    data.ctrl[0] = sol[0]
    data.ctrl[2] = sol[1]

5.1 每步都重新解 IK

是的，每步都调 NLopt。这是 No.12 的最大开销。

5.2 `X_target` 全局变量

X_target 在 controller 里被赋值，但在 equality_constraints 里被读。这俩函数不直接传参，靠 global 共享。

def equality_constraints(result, x, grad):
    global X_target    # 读全局
    ...

设计选择：用全局变量避免在 opt.add_equality_mconstraint 的 callback 里传参。能用但丑陋。

5.3 `set_mjcb_control` 没注册

# mj.set_mjcb_control(controller)    ← 注释掉

为什么注释掉？因为主循环里手动调 controller：

while (data.time - simstart < 0.1):
    controller(model, data)         # 手动调
    mj.mj_step(model, data)

好处：

每 0.1 秒才调一次 controller（不是每 mj_step）
避免每步 1000Hz 跑 NLopt（太慢）

坏处：

不是标准用法
跟 mj_step 解耦，可能漏调（如果 data.time - simstart >= 0.1 的判断出问题）

六、init_controller 详解

def init_controller(model, data):
    global center_x, center_z, X_target

    # 1. 用某个 q 算末端位置，确定 Lemniscate 中心
    end_eff_pos = forward_kinematics([-0.5, 1.0])
    center_x = end_eff_pos[0] - 0.25
    center_z = end_eff_pos[1]

    # 2. 解 t=0 时的 IK，得到初始关节角
    q_guess = np.array([-0.5, 1.0])
    X_target = get_lemniscate_ref(0.0)
    q_pos = inverse_kinematics(q_guess)

    # 3. 写初始关节角
    data.qpos[0] = q_pos[0]
    data.qpos[1] = q_pos[1]

为什么这里也解一次 IK？因为 get_lemniscate_ref(0.0) 给的初始目标位置可能不是当前 FK 位置，先解一次让仿真正确起步。

七、graph() 可视化（运行结束后）

def graph():
    # 收集的 end_eff_pos
    end_eff_pos_arr = np.concatenate(end_eff_pos, axis=1)

    # 参考 Lemniscate
    wt = omega * np.linspace(0.0, simend, 500)
    denominator = 1 + np.sin(wt)**2
    leminiscate_x = center_x + (a * np.cos(wt)) / denominator
    leminiscate_z = center_z + (a * np.sin(wt) * np.cos(wt)) / denominator

    # 画图
    fig, ax = plt.subplots(1, 1, figsize=(8, 5))
    ax.plot(end_eff_pos_arr[0, :], end_eff_pos_arr[1, :], color="cornflowerblue", ...)
    ax.plot(leminiscate_x, leminiscate_z, color="darkorange", ...)
    ax.set_aspect("equal")
    plt.show(block=False)
    plt.pause(5)
    plt.close()

在主循环结束后画 matplotlib 对比图：实测末端轨迹（蓝）vs 参考 Lemniscate（橙）。

依赖：需要 matplotlib + LaTeX（mpl.rcParams['text.usetex'] = True）。没装 LaTeX 会报错。

八、跟 No.6 解析 IK 的对比

维度	No.6 解析 IK	No.12 数值 IK
核心公式	`Δq = J⁻¹ · Δx`	NLopt COBYLA
需要 Jacobian 吗	✅ 必须手算	❌ 完全不用
需要 FK 闭式解吗	❌	❌（用仿真器当 FK）
需要系统参数吗	✅（杆长 L1, L2）	❌
每步求解时间	O(1) ≈ 10 μs	O(N) ≈ 1-10 ms
精度	精确（一阶近似）	近似（靠 tol 控制）
奇异点	需手动处理	自动避开（靠约束）
适用性	仅简单机械臂	任何系统（黑箱）
可扩展到 N 关节吗	难（手写 J）	✅ 直接通用

数值 IK 的核心优势

你完全不需要知道系统的 Jacobian、DH 参数、连杆长度。只要有仿真器，任何机器人都能用这套方法。

这就是 model-free IK 的吸引力 —— 系统变了就换仿真器，算法代码完全不用改。

数值 IK 的核心劣势

慢：每步要解 50-200 次 NLopt
不稳定：可能卡在局部最优（COBYLA 是局部算法）
依赖初值：初始猜测 x0 选得不好可能不收敛

九、整体控制流程图

启动 ───────────────────────────────────────────
  │
  ├─ 创建 data (主仿真) + data_sim (FK 用)
  ├─ init_controller:
  │    ├─ 锚定 Lemniscate 中心
  │    ├─ 解 t=0 的 IK
  │    └─ 设 data.qpos 为解出来的关节角
  └─ 注册 controller（实际**手动**调）

主循环 (60Hz) ────────────────────────────────────
  内层 (0.1s 跑 100 步):
    │
    ├─ 手动调 controller:
    │    ├─ 读 data.time
    │    ├─ X_target = get_lemniscate_ref(t)  ← 算参考目标
    │    ├─ 用当前 qpos 当初始猜测
    │    ├─ inverse_kinematics(qpos):
    │    │    └─ COBYLA 内部:
    │    │         └─ equality_constraints:
    │    │              └─ forward_kinematics(q'):
    │    │                   ├─ data_sim.qpos = q'
    │    │                   ├─ mj_forward
    │    │                   └─ return (data_sim.sensordata[0], [2])
    │    └─ data.ctrl[0] = sol[0]  (pin)
    │    └─ data.ctrl[2] = sol[1]  (pin2)
    │
    └─ mj_step（用 ctrl 推进物理）
  
  外层: 渲染 + 收集 end_eff_pos
  结束: graph() 画 matplotlib 对比图

十、运行方法

pip install nlopt matplotlib
cd mujoco/No_12/
mjpython manipulator_ik.py

预期效果：

双摆末端在 (x, z) 平面画 ∞ 字
持续约 15.7 秒（一个完整周期）
结束后弹 matplotlib 窗口，对比实测 vs 参考轨迹
两条曲线应该重合（数值 IK 精度足够）

⚠️ 没装 LaTeX 会报错（text.usetex = True 那行）。可以注释掉 mpl.rcParams['text.usetex'] = True。

十一、常见问题 / Bugs

1. ⚠️ 重复的 `forward_kinematics` 函数（dead code）

# 第 45-58 行：错误的版本
def forward_kinematics(self, q):    # ← 多了个 self
    ...

# 第 78-91 行：正确的版本
def forward_kinematics(q):
    ...

第一个永远不会被调用（Python 用的是第二个）。可以删掉。

2. ⚠️ `forward_kinematics` 里的 `ctrl` 设置是死代码

data_sim.ctrl[0] = data_sim.qpos[0]
data_sim.ctrl[2] = data_sim.qpos[1]

mj_forward 不会应用 actuator dynamics。这两行对 FK 结果没影响。可以删。

3. ⚠️ `set_mjcb_control` 注释掉

# mj.set_mjcb_control(controller)

靠手动调 controller：

while (data.time - simstart < 0.1):
    controller(model, data)
    mj.mj_step(model, data)

能用但不规范。如果想标准用法，恢复 set_mjcb_control 并删除主循环里的手动调用。

4. matplotlib LaTeX 报错

! LaTeX Error: File `amsmath.sty' not found.

解决：

# 注释掉
# mpl.rcParams['text.usetex'] = True
# mpl.rcParams['text.latex.preamble'] = r'\usepackage{amsmath}'

或装 MacTeX（macOS）/ texlive-full（Linux）。

5. NLopt 每步都跑，太慢

观察：每个 0.1s 的控制步要解 1 次 NLopt ≈ 1-10ms。加上 mj_step 100 步 ≈ 0.1s，单帧总耗时 0.1-0.2s → 实际跑 5-10x 实时速度。

加速：

降低 NLopt 容差（tol=[1e-3, 1e-3]）
减少迭代次数（xtol_rel=1e-3）
用解析 IK（No.6）代替

6. 末端轨迹不完全画 ∞ 字

可能原因：

NLopt 容差太大
奇异点附近 IK 解不稳
仿真器自身数值误差

调试：在 controller 里加：

print(f"X_target={X_target}, sol={sol}, FK(sol)={forward_kinematics(sol)}")

7. `X_target` 全局变量的隐患

def controller(model, data):
    global X_target
    X_target = get_lemniscate_ref(data.time)   # 写

def equality_constraints(result, x, grad):
    global X_target                          # 读
    ...

隐患：如果 equality_constraints 在 controller 之前被调（理论上不会），会读到上一步的 X_target。

更好的做法：inverse_kinematics(x_target) 显式传参，不用全局变量。

8. 跟 No.6 比速度差异

任务	No.6 解析	No.12 数值
单步 IK	≈ 10 μs	≈ 1-10 ms
1000 步仿真	≈ 10 ms	≈ 1-10 s
实时性	1000+ Hz	1-10 Hz

No.12 不能用于实时高频控制。适合离线规划 + 重放（MPC 风格）。

9. 跟 No.11 抛射体优化的区别

	No.11 抛射体	No.12 Lemniscate IK
优化变量	v, θ, T（3 个）	q1, q2（2 个）
目标	命中目标点	跟踪连续轨迹
调用频率	1 次（启动时）	每 0.1s
控制器	空	写 `data.ctrl`

No.12 是 No.11 的「在线版」 —— 每步都做一次小优化。

10. 用更高效的优化器？

算法	速度	适用
`LN_COBYLA`	慢	当前（无梯度、鲁棒）
`LD_MMA`	快	需要梯度（手动有限差分）
`GN_AGS`	很慢	全局最优

十二、整体公式对应

─────── 参考轨迹（任务空间）──────
X*(t) = (x_lemniscate(t), z_lemniscate(t))   ← Lemniscate 参数化

─────── IK 优化（每 0.1s 一次）──────
min_{q1, q2}  0
s.t.  FK(q) = X*(t)
      -π ≤ q ≤ π

─────── 仿真器作 FK（黑箱）──────
data_sim.qpos = q
mj_forward
return (data_sim.sensordata[0], data_sim.sensordata[2])

─────── 实时控制（每 0.1s 一次）──────
data.ctrl[0] = sol[0]
data.ctrl[2] = sol[1]
position servo (kp=100) 内部做 PD 到位

─────── 物理仿真（每 0.0001s 一次）──────
mj_step 用 ctrl 推进
末端到达 sol 对应的位置

十三、一句话总结

No.12 = 「Lemniscate 目标 + NLopt 数值 IK + 独立 data_sim FK + matplotlib 对比可视化」。跟 No.6 的解析 IK 相比，牺牲了速度换来了通用性 —— 不需要推导 Jacobian 也能让任意系统跟踪任意轨迹。核心代价是每步 1-10ms 的 NLopt 求解开销，所以只能 10Hz 控制频率。

No.13 双足步行机器人（Biped）—— 3 状态机并行控制

本节把 No.9 的单腿 hopper 扩展为双足步行机器人。核心升级：

3 个并行 FSM（髋 + 膝1 + 膝2），每个负责自己的状态切换
四元数状态估计 —— 用 quat2euler 算腿的倾斜角
斜坡重力 —— 通过旋转重力模拟斜面行走
腿角色互换 —— leg1/leg2 在 STANCE/SWING 间切换

文件说明

mujoco/No_13/
├── biped.xml    # MuJoCo XML 模型文件
└── biped.py     # 完整脚本：3 FSM + 状态估计 + 斜坡重力

No.13 没有最小脚本（no_13.py）。要看效果必须跑 biped.py。

⚠️ 需要 scipy：pip install scipy

一、biped.xml 详解

<mujoco>
    <visual>
        <headlight ambient="0.25 0.25 0.25"/>
    </visual>

    <option timestep="0.001" integrator="RK4" gravity="0 0 0">
        <!-- 注：gravity 在 Python 里被覆盖为「斜坡重力」 -->
        <flag contact="enable" energy="enable"/>
    </option>

    <worldbody>
        <geom type="plane" size="1000 5 0.1" rgba=".9 0 0 1"/>

        <!--
            主体（双足共享）—— leg1
            3 个关节：x 平移 + z 平移 + pin 旋转
        -->
        <body name="leg1" pos="0 0 0.75" euler="0 0 0">
            <joint name="x"   type="slide" pos="0 0 0.5" axis="1 0 0" />
            <joint name="z"   type="slide" pos="0 0 0.5" axis="0 0 1" />
            <joint name="pin" type="hinge" pos="0 0 0.5" axis="0 -1 0" />
            <geom type="cylinder" size=".05 .5" rgba="0 .9 0 1" mass="1"/>

            <!--
                脚1（knee1 控制伸缩）
            -->
            <body name="foot1" pos="0 0 -0.75">
                <joint name="knee1" type="slide" pos="0 0 0.25" axis="0 0 -1" />
                <geom type="sphere" size=".05" rgba=".9 .9 0 1" mass="0.1"/>
            </body>

            <!--
                leg2（另一条腿，y 方向偏移 0.25）
                1 个 hip 关节（控制 leg2 的旋转）
            -->
            <body name="leg2" pos="0 0.25 0" euler="0 0 0">
                <joint name="hip" type="hinge" pos="0 0 0.5" axis="0 -1 0" />
                <geom type="cylinder" size=".05 .5" rgba=".9 .9 .9 1" mass="1"/>

                <!-- 脚2（knee2 控制伸缩） -->
                <body name="foot2" pos="0 0 -0.75">
                    <joint name="knee2" type="slide" pos="0 0 0.25" axis="0 0 -1" />
                    <geom type="sphere" size=".05" rgba=".9 .9 0 1" mass="0.1"/>
                </body>
            </body>
        </body>
    </worldbody>

    <actuator>
        <position name="pservo_hip"   joint="hip"   kp="5"/>
        <velocity name="vservo_hip"   joint="hip"   kv="1"/>
        <position name="pservo_knee1" joint="knee1" kp="1000"/>
        <velocity name="vservo_knee1" joint="knee1" kv="100"/>
        <position name="pservo_knee2" joint="knee2" kp="1000"/>
        <velocity name="vservo_knee2" joint="knee2" kv="100"/>
    </actuator>
</mujoco>

body 拓扑与索引

worldbody (0)
└── leg1 (1)         ← "主" body，3 个关节 (x, z, pin)
    ├── foot1 (2)    ← 1 个关节 (knee1)
    └── leg2 (3)     ← "副" body，1 个关节 (hip)
        └── foot2 (4) ← 1 个关节 (knee2)

注意：leg1 是个奇怪的"复合体" —— 它同时是身体、第一条腿、还承载着第二条腿（leg2 通过 hip 关节接在 leg1 上）。这是个简化的「双足一体的躯干」模型。

关节总览

关节	所属 body	类型	物理含义
`x`	leg1	slide	整体水平平移
`z`	leg1	slide	整体垂直平移
`pin`	leg1	hinge	（未使用 —— 见 FAQ）
`knee1`	foot1	slide	腿 1 伸缩（脚上下）
`hip`	leg2	hinge	腿 2 摆动（绕 y 轴）
`knee2`	foot2	slide	腿 2 伸缩

⚠️ pin 关节在 XML 里定义但未使用（代码里没设 data.ctrl[X] 给它），是个遗留设计。

跟 No.9 hopper 的对比

维度	No.9 Hopper	No.13 Biped
腿数	1	2
关节数	4	5（knee1, knee2, hip, x, z）
躯干	单 sphere	leg1（cylinder + 嵌套 leg2）
FSM 数	1	3 并行（hip + knee1 + knee2）
状态估计	直接读 qvel	四元数 → Euler
斜坡支持	❌	✅ 旋转重力
关节可视化	❌	✅ `mjVIS_JOINT`
leg1 的 `pin` 关节	有且用	有但不用（遗留）

二、核心：3 个并行 FSM

2.1 FSM 拓扑

fsm_hip   = FSM_LEG2_SWING     # 髋：哪条腿是摆动腿
fsm_knee1 = FSM_KNEE1_STANCE   # 膝1 状态
fsm_knee2 = FSM_KNEE2_STANCE   # 膝2 状态

3 个 FSM 独立运行，不互相嵌套：

状态空间 = fsm_hip × fsm_knee1 × fsm_knee2 = 2 × 2 × 2 = 8 种组合

2.2 状态定义

# 髋 FSM
FSM_LEG1_SWING = 0    # leg1 正在摆动
FSM_LEG2_SWING = 1    # leg2 正在摆动

# 膝1 FSM
FSM_KNEE1_STANCE   = 0   # 脚1 触地
FSM_KNEE1_RETRACT  = 1   # 脚1 抬起

# 膝2 FSM
FSM_KNEE2_STANCE   = 0   # 脚2 触地
FSM_KNEE2_RETRACT  = 1   # 脚2 抬起

2.3 状态转移图

髋 FSM（决定哪条腿摆动）

              foot2 着地 + leg1 越过竖直
   ┌──────────────────────────────────────┐
   │                                      ▼
┌─────────┐                          ┌─────────┐
│ LEG1    │                          │ LEG2    │
│ _SWING  │ ◀─────────────────────── │ _SWING  │
│ (leg1 摆)│   foot1 着地 + leg2 越过竖直  │ (leg2 摆)│
└─────────┘                          └─────────┘

关键转移条件：

pos_foot2[2] < 0.05（脚 2 触地）
abs_leg1 < 0.0（leg1 倾斜过竖直线）

物理含义：脚 2 落地的瞬间，判断身体有没有倾斜到对侧（abs_leg1 < 0）—— 是的话切到 leg1 摆动。

膝 1 FSM

              leg1 越过竖直 (abs_leg1 > 0.1)
   ┌──────────────────────────────────────┐
   │                                      ▼
┌──────────────┐                       ┌──────────────┐
│ KNEE1        │ ◀────────────────────│ KNEE1        │
│ _STANCE      │                       │ _RETRACT     │
│ (脚1 触地)    │                       │ (脚1 抬起)    │
└──────────────┘                       └──────────────┘
        ▲                                      │
        │       foot2 着地 + leg1 越过竖直      │
        └──────────────────────────────────────┘

膝 2 FSM

对称于膝 1，把上面的 leg1 替换为 leg2，foot1 替换为 foot2。

2.4 状态转移代码

# 髋转移
if fsm_hip == FSM_LEG2_SWING and pos_foot2[2] < 0.05 and abs_leg1 < 0.0:
    fsm_hip = FSM_LEG1_SWING
if fsm_hip == FSM_LEG1_SWING and pos_foot1[2] < 0.05 and abs_leg2 < 0.0:
    fsm_hip = FSM_LEG2_SWING

# 膝 1 转移
if fsm_knee1 == FSM_KNEE1_STANCE and pos_foot2[2] < 0.05 and abs_leg1 < 0.0:
    fsm_knee1 = FSM_KNEE1_RETRACT
if fsm_knee1 == FSM_KNEE1_RETRACT and abs_leg1 > 0.1:
    fsm_knee1 = FSM_KNEE1_STANCE

# 膝 2 转移（对称）
if fsm_knee2 == FSM_KNEE2_STANCE and pos_foot1[2] < 0.05 and abs_leg2 < 0.0:
    fsm_knee2 = FSM_KNEE2_RETRACT
if fsm_knee2 == FSM_KNEE2_RETRACT and abs_leg2 > 0.1:
    fsm_knee2 = FSM_KNEE2_STANCE

2.5 状态-控制映射

# 髋控制
if fsm_hip == FSM_LEG1_SWING: data.ctrl[0] = -0.5   # 髋目标 -0.5 rad
if fsm_hip == FSM_LEG2_SWING: data.ctrl[0] = +0.5   # 髋目标 +0.5 rad

# 膝 1 控制
if fsm_knee1 == FSM_KNEE1_STANCE:  data.ctrl[2] =  0.0
if fsm_knee1 == FSM_KNEE1_RETRACT: data.ctrl[2] = -0.25

# 膝 2 控制
if fsm_knee2 == FSM_KNEE2_STANCE:  data.ctrl[4] =  0.0
if fsm_knee2 == FSM_KNEE2_RETRACT: data.ctrl[4] = -0.25

状态	关节目标	物理含义
`LEG1_SWING`	hip = -0.5	髋前摆
`LEG2_SWING`	hip = +0.5	髋后摆
`KNEE_STANCE`	knee = 0.0	脚伸出（不缩回）
`KNEE_RETRACT`	knee = -0.25	脚缩回（抬起）

三、状态估计：四元数 → 欧拉角

3.1 为什么需要状态估计？

代码里没有 data.qpos[hip] 这种直接的关节角可用 —— 因为 hip 关节是 leg2 的，**但 leg1 的「倾斜角」**需要从 xquat[1]（leg1 的四元数）推算。

3.2 关键代码

quat_leg1 = data.xquat[1, :]         # leg1 的世界四元数 (w, x, y, z)
euler_leg1 = quat2euler(quat_leg1)  # 转成欧拉角 (roll, pitch, yaw)
abs_leg1 = -euler_leg1[1]           # 取 -pitch 作为「腿倾角」

pos_foot1 = data.xpos[2, :]          # 脚1 世界坐标

3.3 quat2euler 详解

def quat2euler(quat):
    # SciPy 用 [x, y, z, w]，MuJoCo 用 [w, x, y, z]
    _quat = np.concatenate([quat[1:], quat[:1]])   # 转换顺序
    r = R.from_quat(_quat)
    euler = r.as_euler('xyz', degrees=False)        # roll, pitch, yaw
    return euler

两个坑：

来源	四元数顺序	例子
MuJoCo	`[w, x, y, z]`（w 在前）	`data.xquat`
SciPy	`[x, y, z, w]`（w 在后）	`R.from_quat()`

代码用 np.concatenate([quat[1:], quat[:1]]) 转换顺序。

3.4 为什么 `abs_leg1 = -euler_leg1[1]`（取负）？

euler_leg1[1] 是 pitch（绕 y 轴的旋转）。axis="0 -1 0" 意味着关节绕 -y 旋转，所以四元数给出的 pitch 跟「腿的真实倾斜」符号相反。

abs_leg1 = -pitch 是个手调的符号修正。没有通用公式 —— 取决于：

旋转轴方向
局部坐标系
欧拉角顺序（'xyz' vs 'zyx'）

替代方案：用 mj.rotateQuaternion 直接算相对角度，避开欧拉角的歧义。

四、斜坡重力（ramp 模拟）

# 倾斜重力 0.1 rad ≈ 5.7°
model.opt.gravity[0] =  9.81 * np.sin(0.1)   # x 分量（向右）
model.opt.gravity[2] = -9.81 * np.cos(0.1)   # z 分量（向下）

4.1 物理含义

原来:  gravity = (0, 0, -9.81)        ← 纯向下
现在:  gravity = (0.98, 0, -9.76)     ← 略带向右的水平分量

等效于机器人走5.7° 斜坡。水平分量 9.81·sin(0.1) ≈ 0.98 会让机器人向右滑。

4.2 为什么这么模拟而不是加斜面几何？

方案	实现	优点
改重力（No.13 用）	`model.opt.gravity = ...`	一行代码，几何不变
改 `<geom>` 朝向	旋转地面	视觉更真实
加 `quat` 旋转	给 body 旋转	可控但复杂

No.13 用第一种 —— 最小改动测试 FSM 能不能应对扰动。

五、joint frame 可视化

opt.flags[mj.mjtVisFlag.mjVIS_JOINT] = 1

开启关节轴可视化，每个关节会显示红/绿/蓝三轴（对应 x/y/z）。调试用，不影响物理。

       ┌─ 红轴 (x)
       │
   ────┼─── 绿轴 (y)   ← 关节
       │
       └─ 蓝轴 (z)

六、init_controller 详解

def init_controller(model, data):
    data.qpos[4] = 0.5           # 设置 knee1 初始位置
    data.ctrl[0] = data.qpos[4]  # 髋目标 = 0.5

只设了 2 个值：

qpos[4] = 0.5：knee1 初始位置 0.5 m（让脚 1 初始抬高）
ctrl[0] = qpos[4]：髋目标也设 0.5（可能是个遗留错误，应该是 hip 的目标值而非 knee1 的 qpos）

⚠️ 可疑的 init 逻辑：data.ctrl[0] = data.qpos[4] 把 knee1 的位置赋值给 hip 的目标，不是同一个量。可能是 bug。

七、set_mjcb_control 又被注释掉了

# mj.set_mjcb_control(controller)

跟 No.12 一样，靠手动调 controller：

while (data.time - simstart < 1.0/60.0):
    mj.mj_step(model, data)
    controller(model, data)    # 手动调

后果：每 1/60s 调一次（≈60Hz），每步 0.001s 物理里调 16-17 次 controller。会重复判断 FSM 转移条件（但状态没变就不会切换）。

八、整体控制流程图

启动 ───────────────────────────────────────────
  │
  ├─ 创建 model, data
  ├─ 设斜坡重力: gravity = (0.98, 0, -9.76)
  ├─ 设初始 qpos[4] = 0.5 (knee1 抬高)
  └─ init_controller

主循环 (60Hz) ────────────────────────────────────
  内层 (~16 次 mj_step / controller):
    │
    ├─ mj_step (物理推进)
    │
    └─ controller(model, data):
         │
         ├─ 状态估计:
         │    ├─ quat_leg1 = data.xquat[1, :]
         │    ├─ euler_leg1 = quat2euler(quat_leg1)
         │    ├─ abs_leg1 = -euler_leg1[1]
         │    ├─ quat_leg2 = data.xquat[3, :]
         │    ├─ euler_leg2 = quat2euler(quat_leg2)
         │    ├─ abs_leg2 = -euler_leg2[1]
         │    ├─ pos_foot1 = data.xpos[2, :]
         │    └─ pos_foot2 = data.xpos[4, :]
         │
         ├─ FSM 转移:
         │    ├─ fsm_hip: 哪条腿摆动
         │    ├─ fsm_knee1: 脚1 状态
         │    └─ fsm_knee2: 脚2 状态
         │
         └─ 控制:
              ├─ data.ctrl[0] = ±0.5 (hip)
              ├─ data.ctrl[2] = 0.0 / -0.25 (knee1)
              └─ data.ctrl[4] = 0.0 / -0.25 (knee2)
  
  外层: 渲染 + 相机跟随 + 关节可视化

九、运行方法

pip install scipy
cd mujoco/No_13/
mjpython biped.py

预期效果：

机器人向右走（受斜坡重力推）
两条腿交替摆动
关节上有红绿蓝三轴指示
相机跟随（cam.lookat[0] = data.qpos[0]）

十、跟 No.9 单腿 hopper 的对比

维度	No.9 Hopper	No.13 Biped
腿数	1	2
FSM 数	1（4 状态）	3（各 2 状态）
FSM 关系	单 FSM 串行	3 FSM 并行
状态数	4	8 = 2³
状态估计	读 `qvel[1]`	四元数 → 欧拉
斜坡	❌	✅ 旋转重力
joint 可视化	❌	✅ `mjVIS_JOINT`
复用 XML 关节	6 个全用	5 个用（`pin` 遗留）

控制思想对比

No.9: 「单 FSM 串行」
  FSM_AIR1 → STANCE1 → STANCE2 → AIR2 → AIR1 → ...
  (时间/事件驱动，单线流程)

No.13: 「3 FSM 并行」
  髋 FSM: 决定哪条腿摆
  膝1 FSM: 决定脚 1 抬起/放下
  膝2 FSM: 决定脚 2 抬起/放下
  (3 个独立状态机，组合出 8 种可能)

No.13 更接近真实双足控制 —— 双足行走本质上是多状态机的协调。

十一、调参指南

想改	改什么	效果
走更快	`FSM_LEG*_SWING` 时 `ctrl[0]` ±0.5 改 ±0.8	步幅变大
走更稳	减小斜坡角度 `0.1 → 0.05`	扰动变小
跳着走	让 KNEE_RETRACT 更早触发（改 `abs_leg1 > 0.1` → `> 0.05`）	抬脚时机更早
走更慢	把 `knee` 的 kp 减小（1000 → 100）	腿反应迟钝
让 leg1 的 `pin` 用上	在 controller 里加 `data.ctrl[X] = ...`	（需要先确定 ctrl 索引）

十二、常见问题 / Bugs

1. `pin` 关节在 XML 定义了但未使用

原因：pin 关节的 ctrl 通道在 controller 里从来没被设过。这是个遗留设计。

解决：要么删掉 XML 里的 pin 关节，要么在 controller 里驱动它。

2. `init_controller` 里的可疑赋值

data.ctrl[0] = data.qpos[4]    # ← 髋目标 = 膝1 位置？

ctrl[0] 是 hip 位置伺服的目标，赋值 qpos[4]（knee1 的位置）意义不明。可能是 bug。

可能正解：

data.ctrl[0] = 0.5   # hip 目标设个固定值

3. `set_mjcb_control` 注释掉

跟 No.12 同款问题，靠手动调用。能用但不规范。

4. `abs_leg1 = -euler_leg1[1]` 为什么取负？

手调的符号修正，跟 axis="0 -1 0"（负 y 轴）有关。没有通用公式。如果改了轴方向，这个负号也得改。

5. 机器人不走路 / 摔倒

可能原因：

FSM 没切换（状态估计不对）
斜坡角度太大
关节增益太小（pservo_hip kp=5 太小！）

调试：

def controller(model, data):
    global fsm_hip, fsm_knee1, fsm_knee2, step_no
    print(f"fsm_hip={fsm_hip}, fsm_knee1={fsm_knee1}, "
          f"abs_leg1={abs_leg1:.2f}, abs_leg2={abs_leg2:.2f}, "
          f"pos_foot1={pos_foot1[2]:.2f}, pos_foot2={pos_foot2[2]:.2f}")

6. `pservo_hip` 的 kp=5 是不是太小？

是。knee 是 1000，hip 是 5 —— 差 200 倍。但这是刻意的：hip 应该是「软」关节，让腿自然摆动；knee 是「硬」关节，保证支撑稳定。

如果走路时髋反应太慢，可以试 kp=20-50。

7. 没装 scipy 报错

ImportError: No module named scipy

解决：pip install scipy（用于四元数 → 欧拉的转换）。

8. `data.xpos[2, :]` 跟 `data.xpos[4, :]` 是什么意思？

MuJoCo 按 XML 声明顺序给 body 编号：

索引	body
0	worldbody
1	leg1
2	foot1
3	leg2
4	foot2

所以 [2, :] 是 foot1，[4, :] 是 foot2。这是个脆弱的硬编码，XML 一改就错。

更鲁棒：

foot1_id = mj.mj_name2id(model, mj.mjtObj.mjOBJ_BODY, "foot1")
pos_foot1 = data.xpos[foot1_id, :]

9. 怎么从「走路」变成「跑」？

跑 = 有飞行相（两脚同时离地）。No.13 当前控制是两脚交替，没飞行相。

改成跑：

在 FSM 里加 FSM_AIR 状态
当两脚都离地时进入 AIR
AIR 阶段不设 knee 目标（让脚自然下落）

10. 跟 No.7 LQR、No.6 IK 怎么选？

任务	推荐
单点镇定（双足站直）	No.7 LQR
末端抓取	No.6 IK / No.12 数值 IK
跳跃/跑步	No.13 风格多 FSM
离线轨迹规划	No.11 NLopt

十三、整体公式对应

────── 机器人形态（XML）──────
leg1 (3 关节) + leg2 (1 关节) + foot1/foot2 (各 1 关节)
共 5 个有效关节: x, z, knee1, hip, knee2

────── 状态估计（controller）──────
quat_leg1 = data.xquat[1, :]               # leg1 四元数
euler_leg1 = quat2euler(quat_leg1)        # → 欧拉
abs_leg1 = -euler_leg1[1]                 # 取负的 pitch

────── 3 FSM 状态机 ──────
fsm_hip:   LEG1_SWING ↔ LEG2_SWING
fsm_knee1: STANCE ↔ RETRACT
fsm_knee2: STANCE ↔ RETRACT
总组合: 2 × 2 × 2 = 8

────── 状态-控制映射 ──────
LEG1_SWING → hip = -0.5
LEG2_SWING → hip = +0.5
STANCE     → knee = 0.0
RETRACT    → knee = -0.25

────── 斜坡重力 ──────
gravity = (9.81·sin(0.1), 0, -9.81·cos(0.1))
       ≈ (0.98, 0, -9.76)    ← 5.7° 斜面

十四、一句话总结

No.13 = 「双足机器人 + 3 个并行 FSM + 四元数状态估计 + 斜坡重力」。把 No.9 的单腿 hopper 扩成双足，引入多 FSM 协调的范式（比单 FSM 更接近真实机器人控制），用 quat2euler 从世界姿态反推腿倾斜角，配合旋转重力模拟斜面扰动。核心复杂度是 8 种 FSM 组合的协调，核心新工具是四元数 → 欧拉的转换。

统一 Demo：移动操作机器人 + 数据采集（No.1–No.13 融合）

本节把 No.1–No.13 全部概念融合到一个文件 demo_collect.py 中：一辆带 4-DOF 机械臂的小车，通过 9 状态 FSM 自动完成"捡地上的盒子 → 运回来 → 放下"，并同步采集训练数据。

文件说明

mujoco/Chenlong_Robot/
├── car.xml           # MuJoCo 模型（小车 + 2× 臂 + 双指夹爪 + 28 路传感器）
├── demo_collect.py   # 统一 Demo + 数据采集（~800 行）
└── episodes/         # 采集数据：ep_*.npz + summary_*.png

运行：

python3 demo_collect.py                     # GUI，默认 5 轮
python3 demo_collect.py --headless --episodes 10  # 无头，10 轮

一、car.xml 模型详解

<mujoco>
    <option timestep="0.001" integrator="RK4" gravity="0 0 -15"/>
    <!--
        timestep=0.001 → 1000Hz 仿真频率
        integrator=RK4 → 四阶龙格库塔，比默认 Euler 更精确
        gravity=0 0 -15 → 重力稍强于地球（9.81），加快掉落
    -->

    <default>
        <geom friction="1 0.1 0.1"/>
        <!-- 所有 geom 的默认摩擦：滑动1、扭转0.1、滚动0.1 -->
    </default>

    <worldbody>
        <!-- 5 点均匀光照：无阴影，各方向可见 -->
        <light diffuse="0.6 0.6 0.6" pos="0 0 6"  dir="0 0 -1"/>

        <geom type="plane" size="5 5 0.1" rgba="0.8 0.8 0.8 1"
              friction="2.5 1.0 0.001"/>
        <!--
            地面：5×5m 灰色平面
            friction: 滑动2.5 / 扭转1.0 / 滚动0.001
        -->

        <!-- ===== 目标盒子：自由落体，初始放在地上 ===== -->
        <body name="target_box" pos="1.5 0 0.04">
            <freejoint/>
            <!-- freejoint → 6-DOF 自由运动 -->
            <geom type="box" size="0.04 0.04 0.04"
                  rgba="1.0 0.85 0.2 1" mass="0.05"/>
            <!-- 8cm 黄色立方体，质量 50g -->
        </body>

        <!-- ===== 小车主体 ===== -->
        <body name="car" pos="0 0 0.22">
            <freejoint/>
            <!-- 小车也是 freejoint → 可以自由驾驶 -->

            <geom type="box" size="0.5 0.3 0.15"
                  rgba="0.2 0.6 0.8 1" mass="10"
                  contype="0" conaffinity="0"/>
            <!--
                车身：1m×0.6m×0.3m 蓝色盒子
                contype=0 conaffinity=0 → 车身不参与碰撞，只有轮子接触地面
            -->

            <!-- ===== 4-DOF 机械臂 ===== -->
            <body name="arm_base" pos="0 0 0.22">
                <!--
                    臂基座：在车顶上方 22cm
                    shoulder_pan：绕 Z 轴旋转（水平转台），±360°
                -->
                <joint name="shoulder_pan" type="hinge" axis="0 0 1"
                       range="-360 360" armature="0.10" damping="0.5"/>
                <geom type="cylinder" size="0.08 0.07"
                      rgba="0.9 0.5 0.1 1" mass="0.35"/>

                <body name="upper_arm" pos="0 0 0.12">
                    <!--
                        upper_arm：上臂，长 0.50m，粗 0.065m
                        shoulder_lift：绕 Y 轴俯仰，±360°
                    -->
                    <joint name="shoulder_lift" type="hinge" axis="0 1 0"
                           range="-360 360" armature="0.10" damping="0.5"/>
                    <geom type="capsule" size="0.065"
                          fromto="0 0 0 0 0 0.50"
                          rgba="0.9 0.5 0.1 1" mass="0.40"/>

                    <body name="forearm" pos="0 0 0.50">
                        <!--
                            forearm：前臂，长 0.40m，粗 0.055m
                            elbow：肘关节，±360°
                        -->
                        <joint name="elbow" type="hinge" axis="0 1 0"
                               range="-360 360" armature="0.10" damping="0.5"/>
                        <geom type="capsule" size="0.055"
                              fromto="0 0 0 0 0 0.40"
                              rgba="0.9 0.5 0.1 1" mass="0.30"/>

                        <body name="wrist" pos="0 0 0.40">
                            <!--
                                wrist：手腕，长 0.16m，粗 0.045m
                                wrist_pitch：腕部俯仰，±360°
                            -->
                            <joint name="wrist_pitch" type="hinge" axis="0 1 0"
                                   range="-360 360" armature="0.06" damping="0.5"/>
                            <geom type="capsule" size="0.045"
                                  fromto="0 0 0 0 0 0.16"
                                  rgba="0.9 0.5 0.1 1" mass="0.15"/>

                            <!-- 双指平行夹爪 -->
                            <body name="gripper_palm" pos="0 0 0.16">
                                <!-- 手掌：固定不动 -->
                                <geom type="box" pos="0 0 -0.03"
                                      size="0.04 0.06 0.03"
                                      rgba="0.3 0.3 0.3 1" mass="0.10"/>

                                <body name="finger_l" pos="0 0.03 0.04">
                                    <!--
                                        左指：slide 沿 +Y，范围 0→0.04m
                                        position=0 → 手指内收（夹紧）
                                        position=0.04 → 手指张开
                                    -->
                                    <joint name="finger_l_j" type="slide"
                                           axis="0 1 0" range="0 0.04"
                                           armature="0.01" damping="1.0"/>
                                    <geom type="box" size="0.015 0.008 0.06"
                                          rgba="0.5 0.5 0.5 1" mass="0.03"/>
                                    <site name="touch_l" pos="0 -0.01 0"
                                          size="0.004" rgba="0 1 0 0.5"/>
                                    <!-- touch_l：触觉 site，位于指尖内侧 -->
                                </body>

                                <body name="finger_r" pos="0 -0.03 0.04">
                                    <!-- 右指：slide 沿 -Y，镜像左指 -->
                                    <joint name="finger_r_j" type="slide"
                                           axis="0 -1 0" range="0 0.04"
                                           armature="0.01" damping="1.0"/>
                                    <geom type="box" size="0.015 0.008 0.06"
                                          rgba="0.5 0.5 0.5 1" mass="0.03"/>
                                    <site name="touch_r" pos="0 0.01 0"
                                          size="0.004" rgba="0 1 0 0.5"/>
                                </body>

                                <site name="end_effector" pos="0 0 0.05"
                                      size="0.02" rgba="1 0 0 1"/>
                                <!-- EE site：红色标记点，用于 IK 目标 -->
                            </body>
                        </body>
                    </body>
                </body>
            </body>

            <!-- ===== 四个轮子 ===== -->
            <body name="wheel_fl" pos="0.4 0.3 -0.1">
                <joint name="wheel_fl_joint" type="hinge"
                       axis="0 1 0" damping="0.5"/>
                <geom type="cylinder" size="0.12 0.05" euler="90 0 0"
                      rgba="0.1 0.1 0.1 1" mass="1"/>
                <!--
                    euler="90 0 0" → 把圆柱从 Z 轴翻转到 Y 轴（变成竖直轮子）
                    size: 半径0.12m, 半宽0.05m
                -->
            </body>
            <!-- wheel_fr, wheel_rl, wheel_rr 结构相同，省略 -->
        </body>
    </worldbody>

    <actuator>
        <!-- 4 个轮子 motor -->
        <motor joint="wheel_fl_joint" name="motor_wheel_fl"
               gear="3" ctrllimited="true" ctrlrange="-5 5"/>
        <!-- gear=3 → 力矩放大3倍  ctrlrange=±5 → 速度限制 -->

        <!-- 4 个臂关节 position servo（PD 控制器） -->
        <position name="pservo_shoulder_pan"  joint="shoulder_pan"  kp="200" kv="10"/>
        <position name="pservo_shoulder_lift" joint="shoulder_lift" kp="200" kv="10"/>
        <position name="pservo_elbow"          joint="elbow"         kp="200" kv="10"/>
        <position name="pservo_wrist_pitch"    joint="wrist_pitch"   kp="120" kv="6"/>
        <!--
            position servo：ctrl = kp*(target - q) - kv*qvel
            kp=200, kv=10 → 快速跟踪，有一定阻尼
            wrist_pitch 降低增益（防止末端抖动）
        -->

        <!-- 2 个手指 servo -->
        <position name="pservo_finger_l" joint="finger_l_j" kp="200" kv="10"/>
        <position name="pservo_finger_r" joint="finger_r_j" kp="200" kv="10"/>
    </actuator>

    <equality>
        <!-- 抓取 weld：激活时把盒子焊在手掌上 -->
        <weld name="grasp_weld" body1="gripper_palm" body2="target_box"
              active="false" solref="0.02 1" solimp="0.9 0.95 0.001"/>
        <!--
            active="false" → 初始不激活，GRASP 状态时激活
            solref="0.02 1" → 约束刚度：时间常数20ms，阻尼比1（临界）
        -->
    </equality>

    <sensor>
        <!-- 6 个臂关节 × 2（位置+速度）= 12 -->
        <jointpos joint="shoulder_pan"  name="arm_pan_pos"/>
        <jointvel joint="shoulder_pan"  name="arm_pan_vel"/>
        <!-- ... lift/elbow/wrist/finger_l/finger_r 各两个 ... -->

        <!-- 4 个轮子 × 2 = 8 -->
        <jointpos joint="wheel_fl_joint" name="wheel_fl_pos"/>
        <jointvel joint="wheel_fl_joint" name="wheel_fl_vel"/>
        <!-- ... -->

        <!-- 末端位姿+速度 = 6 -->
        <framepos    objtype="site" objname="end_effector" name="ee_pos"/>
        <framelinvel objtype="site" objname="end_effector" name="ee_vel"/>

        <!-- 手指触觉 = 2 -->
        <touch site="touch_l" name="finger_l_touch"/>
        <touch site="touch_r" name="finger_r_touch"/>
        <!-- touch sensor：接触时输出 >0，不接触 =0 -->
    </sensor>
</mujoco>

臂几何参数

段	长度	半径	关节	范围
上臂	0.50m	0.065m	shoulder_lift	±360°
前臂	0.40m	0.055m	elbow	±360°
手腕	0.16m	0.045m	wrist_pitch	±360°
夹爪	—	—	finger_l/r slide	0→0.04m
总臂展	1.06m

二、demo_collect.py 逐段详解

2.1 常量与几何参数（第 59–92 行）

# 盒子放在地上 (z=0.04 是盒子半高，底部刚好贴地)
TARGET_POS = np.array([1.5, 0.0, 0.04])
PLACE_POS  = np.array([0.25, 0.0, 0.55])  # 放到原点前方

# 臂长 —— 必须与 car.xml 一致
L_UPPER   = 0.50
L_FOREARM = 0.40
L_WRIST   = 0.16
L_GRIPPER = 0.08
L2_EFF    = L_FOREARM + L_WRIST + L_GRIPPER  # = 0.64，用于 2 连杆 IK

# 控制参数
DRIVE_SPEED    = 0.5   # 轮子速度
REACH_TOL      = 0.18  # 末端到达容差（18cm）
TRAJ_DURATION  = 0.6   # 三次轨迹时长（秒）
NUM_EPISODES   = 5     # 默认运行 5 轮

# 夹爪位置
STOWED = [0.0, 0.5, -1.0, 0.0, 0.04, 0.04]  # [pan, lift, elbow, wrist, f_l, f_r]
#                                                手指=0.04 → 张开

# 关节限位（必须与 XML 中 range 一致）
Q_MIN = np.array([-6.28, -6.28, -6.28, -6.28])  # ±360°
Q_MAX = np.array([ 6.28,  6.28,  6.28,  6.28])

2.2 模型 ID 缓存（第 109–124 行）

def _cache_ids(m):
    """一次性查表，存下所有 body/site/joint 的 ID，后续用整数寻址比字符串快。"""
    for name in ("car", "arm_base", "target_box"):
        _ids[name] = mj.mj_name2id(m, mj.mjtObj.mjOBJ_BODY, name)
    _ids["ee"]       = mj.mj_name2id(m, mj.mjtObj.mjOBJ_SITE, "end_effector")
    _ids["gripper"]  = mj.mj_name2id(m, mj.mjtObj.mjOBJ_BODY, "gripper_palm")

    arm_names = ["shoulder_pan", "shoulder_lift", "elbow", "wrist_pitch",
                 "finger_l_j", "finger_r_j"]
    _arm_qpos_adr = [m.jnt_qposadr[m.joint(n).id] for n in arm_names]
    _arm_dof_adr  = [m.jnt_dofadr[m.joint(n).id]  for n in arm_names]
    # jnt_qposadr → 关节在 qpos 数组中的起始下标
    # jnt_dofadr  → 关节在 qvel 数组中的起始下标

为什么需要 _arm_qpos_adr？ 因为模型中还有 target_box freejoint、car freejoint 在 qpos 前面，不能直接假设下标。通过 jnt_qposadr 查表才是正确的。

2.3 状态读取器（第 129–134 行）

def _car_pos(d):      return d.xpos[_ids["car"]]        # (3,) 世界坐标
def _arm_base_pos(d): return d.xpos[_ids["arm_base"]]    # (3,)
def _target_pos(d):   return d.xpos[_ids["target_box"]]  # (3,)
def _ee_pos(d):       return d.site_xpos[_ids["ee"]]     # (3,) site 的世界坐标
def _arm_qpos(d):     return np.array([d.qpos[a] for a in _arm_qpos_adr])  # (6,)
def _car_quat(d):     return d.xquat[_ids["car"]].copy() # (4,) w,x,y,z

xpos vs qpos：xpos 是笛卡尔世界坐标（已通过正运动学计算），qpos 是关节空间原始值。用 xpos 读位置不需要手工做坐标系变换。

2.4 解析 2D IK（第 140–155 行）—— No.6 核心

def _analytical_ik_2d(x, z, L1=L_UPPER, L2=L2_EFF):
    """2 连杆平面 IK：已知目标 (x,z)，求 (lift, elbow, wrist)。"""
    D = np.hypot(x, z)           # 目标距臂基座的距离
    max_r = L1 + L2              # 最大臂展
    if D > max_r * 0.95:         # 太远 → 缩放到 95% 最大范围
        s = max_r * 0.95 / D
        x, z = x * s, z * s

    # 余弦定理求肘关节角
    D2 = x*x + z*z
    cos_q2 = np.clip((D2 - L1*L1 - L2*L2) / (2*L1*L2), -1.0, 1.0)
    q2_int = np.arccos(cos_q2)   # 肘部内角（0=伸直, π=完全折叠）

    alpha = np.arctan2(z, x)     # 目标方向角
    beta  = np.arctan2(L2 * np.sin(q2_int), L1 + L2 * np.cos(q2_int))
    q1 = -(alpha - beta)         # shoulder_lift（负号来自 MuJoCo 轴方向）
    q2 = -(np.pi - q2_int)       # elbow
    q3 = -(q1 + q2)              # wrist_pitch（保持末端水平）
    return q1, q2, q3

几何意义：

        L1        L2
   ●─────────●─────────●  (目标)
  arm_base   elbow      EE

已知 arm_base → EE 的距离 D，用余弦定理算出 elbow 折叠角 q2_int，
再通过 α（目标方向）和 β（上臂相对目标线的偏角）合成 q1。

2.5 数值 IK 精调（第 164–211 行）—— No.11 核心

def numerical_ik(target_world, arm_base_pos, q_guess=None,
                 max_iter=40, tol=0.015, alpha=0.5, gripper_cmd=0.0):
    m, d = model, _fk_data

    # Step 1: 解析 IK 给出初值
    local = target_world - arm_base_pos
    r_xy = np.hypot(local[0], local[1])
    pan0 = float(np.clip(np.arctan2(local[1], local[0]), -1.57, 1.57))
    lift0, elbow0, wrist0 = _analytical_ik_2d(r_xy, local[2])
    q = np.clip([pan0, lift0, elbow0, wrist0], Q_MIN, Q_MAX)

    # Step 2: 把主仿真状态拷贝到独立的 FK data 中（不污染主数据）
    d.qpos[:] = data.qpos[:]
    d.qvel[:] = data.qvel[:]
    mj.mj_fwdPosition(m, d)

    # Step 3: Jacobian 伪逆迭代
    for _ in range(max_iter):
        # 把当前关节角写入 FK data 并计算正运动学
        for adr, val in zip(_arm_qpos_adr,
                            [q[0], q[1], q[2], q[3], gripper_cmd, gripper_cmd]):
            d.qpos[adr] = val
        mj.mj_fwdPosition(m, d)
        ee = d.site_xpos[_ids["ee"]]
        err = target_world - ee[:3]      # 位置误差（世界坐标）

        if np.linalg.norm(err) < tol:
            break                         # 收敛

        # 计算末端位置雅可比（3×nv），提取 4 个臂关节列
        jacp = np.zeros((3, m.nv))
        mj.mj_jac(m, d, jacp, None, ee[:3], _ids["gripper"])
        J = np.zeros((3, 4))
        for i, adr in enumerate(_arm_dof_adr[:4]):
            J[:, i] = jacp[:, adr]        # 只取 pan/lift/elbow/wrist 四列

        # 阻尼伪逆：Δq = (JᵀJ + λI)⁻¹ Jᵀ · err · α
        lam = 0.05
        dq = np.linalg.solve(J.T @ J + lam * np.eye(4), J.T @ err) * alpha
        q = np.clip(q + dq, Q_MIN, Q_MAX)
        q[3] = float(np.clip(-(q[1] + q[2]), -1.57, 1.57))
        # ↑ 强制 wrist = -(lift+elbow)，保持末端水平

    return np.array([q[0], q[1], q[2], q[3], gripper_cmd, gripper_cmd])

为什么用独立 FK data（No.12 模式）：迭代过程中的 mj_fwdPosition 是纯数学计算，不能影响主仿真状态。独立的 MjData 保证隔离。

阻尼 λ 的作用：当 J 接近奇异（臂伸直）时，JᵀJ 不可逆。λ=0.05 的阻尼项保证始终有解。

2.6 三次轨迹插值（第 217–225 行）—— No.5 核心

def _cubic_coeffs(q0, qf, T):
    """q(t) = a₀ + a₂t² + a₃t³，边界速度=0"""
    return np.array([q0, 0.0, 3*(qf - q0)/(T*T), -2*(qf - q0)/(T*T*T)])

def _eval_cubic(c, t):
    """求值：返回 (pos, vel)"""
    pos = c[0] + c[1]*t + c[2]*t*t + c[3]*t*t*t
    vel = c[1] + 2*c[2]*t + 3*c[3]*t*t
    return pos, vel

为什么用三次多项式？ 给定起点 q₀、终点 q_f、起止速度=0，三次多项式是唯一满足这 4 个约束的最低次多项式。比直接跳变平滑，不会激发机械臂振动。

2.7 动力学提取（第 231–238 行）—— No.4 核心

def _log_mass_matrix_diag():
    """每次 FSM 切换时，提取并打印质量矩阵对角线。"""
    M = np.zeros((model.nv, model.nv))
    mj.mj_fullM(model, M, data.qM)  # 从稀疏 qM 构建稠密 M
    print(f"[No.4 Dynamics] mass-matrix diagonal (nv={model.nv}): ...")

质量矩阵对角线反映了各关节的等效惯量。数值大 → 该关节"重"，加速慢。

2.8 抓取管理（第 243–250 行）—— No.8 核心

def _grasp(activate):
    """激活/停用 grasp_weld 约束。"""
    eq = model.eq("grasp_weld")
    eq.active0[0] = 1 if activate else 0

weld 是 MuJoCo 的 equality constraint，把两个 body 刚性连接。处理方式比纯摩擦力可靠。

2.9 DataCollector 数据采集器（第 255–330 行）

class DataCollector:
    def maybe_record(self, t, fsm, ee, target, last_action):
        """10Hz 采样：状态、动作、传感器数据"""
        if t - self._last_coll_t < 0.1:   # 10Hz = 0.1s 间隔
            return
        self.joint_states.append(np.concatenate([
            _car_pos(data), _car_quat(data), _arm_qpos(data)]))  # 7+6=13
        self.sensordata.append(data.sensordata.copy())  # 28 维全部传感器
        self.actions.append(last_action.copy())          # 4轮+6臂=10

    def save(self):
        """保存为 .npz"""
        np.savez_compressed(path,
            joint_states=..., ee_position=..., target_position=...,
            actions=..., sensordata=..., fsm_state=..., timestamps=...)

2.10 FSM 状态机（第 334–458 行）—— No.5 + No.9 核心

9 个状态：

DRIVE(0) → REACH(1) → LOWER(2) → GRASP(3) → LIFT(4)
  → DRIVE_BACK(5) → PLACE(6) → RELEASE(7) → DONE(8)

状态转换函数：

def _check_transitions(st):
    car_x = _car_pos(data)[0]
    ee    = _ee_pos(data)
    elap  = data.time - st.fsm_enter

    # 超时保护：防止某状态永远达不到条件
    if elap > FSM_TIMEOUT.get(st.fsm, float("inf")):
        return 下一个状态

    if fsm == FSM_DRIVE:
        if car_x > DRIVE_TARGET_X:          # 车开到 x>1.05 就停
            return FSM_REACH

    elif fsm == FSM_REACH:
        goal = TARGET_POS.copy(); goal[2] += 0.15
        if np.linalg.norm(ee - goal) < REACH_TOL:  # EE 到目标上方 18cm 内
            return FSM_LOWER

    elif fsm == FSM_LOWER:
        goal[2] = max(TARGET_POS[2], box_z + 0.10)
        if np.linalg.norm(ee - goal) < REACH_TOL:  # EE 到盒子附近
            return FSM_GRASP

    elif fsm == FSM_GRASP:
        if elap > 1.0:                      # 等 1 秒让手指夹紧
            return FSM_LIFT

    elif fsm == FSM_LIFT:
        goal = TARGET_POS.copy(); goal[2] += 0.15
        if np.linalg.norm(ee - goal) < REACH_TOL:  # 抬起到安全高度
            return FSM_DRIVE_BACK

    elif fsm == FSM_DRIVE_BACK:
        if car_x < 0.30:                    # 开回原点附近
            return FSM_PLACE

    elif fsm == FSM_PLACE:
        if np.linalg.norm(ee - PLACE_POS) < REACH_TOL:  # EE 到放置点
            return FSM_RELEASE

    elif fsm == FSM_RELEASE:
        if elap > 0.5:                      # 等 0.5 秒手指张开
            return FSM_DONE

FSM 切换时做的事（在 controller 中）：

if nxt != fsm:
    if nxt == FSM_GRASP:
        _grasp(True)                  # 激活 weld，锁住盒子
    if nxt == FSM_RELEASE:
        _grasp(False)                 # 释放 weld
    state.fsm = nxt
    state.fsm_enter = data.time
    _compute_arm_target(nxt, state)   # 为新状态算一次 IK（只算一次！）
    # 启动三次轨迹：从当前 q → 新目标，TRAJ_DURATION 秒内平滑过渡
    state._traj_coeffs = np.array([_cubic_coeffs(cur_q[j],
        state._cached_target[j], TRAJ_DURATION) for j in range(6)])
    _log_mass_matrix_diag()           # No.4 动力学快照

2.11 controller 控制回调（第 465–557 行）

每步仿真（1000Hz）调用一次，是整个系统的"大脑"：

def controller(m, d):
    fsm = state.fsm

    # ── ① 车控制 ──
    if fsm == FSM_DRIVE:
        # 比例速度：离目标越近越慢
        dist = max(0, DRIVE_TARGET_X - car_x)
        speed = min(DRIVE_SPEED, 0.8 * dist + 0.15)
        d.ctrl[0:4] = max(0.1, speed)          # 最低 0.1 克服静摩擦
    elif fsm == FSM_DRIVE_BACK:
        dist = max(0, car_x - 0.3)
        speed = min(DRIVE_SPEED, 0.8 * dist + 0.15)
        d.ctrl[0:4] = -max(0.1, speed)         # 反向
    else:
        # 刹车：用速度反馈抵抗剩余速度
        for i in range(4):
            wv = d.qvel[wheel_dof_start + i]
            d.ctrl[i] = -np.sign(wv) * BRAKE_TORQUE if abs(wv) > 0.01 else 0.0

    # ── ② 臂控制 ──
    # 每 0.5 秒刷新一次 IK（车刹停后可能还在漂移）
    if fsm in (FSM_REACH, FSM_LOWER, FSM_LIFT, FSM_PLACE):
        if data.time - state._last_ik_time > 0.5:
            _compute_arm_target(fsm, state)
            # 如果新目标和旧目标差距 > 0.03 rad，重新启动轨迹
            if np.linalg.norm(new - old) > 0.03:
                # 生成新三次轨迹
                state._traj_coeffs = ...

    # 轨迹插值：在轨迹期间用三次多项式，结束后直接 hold
    if state._traj_coeffs active:
        arm_cmd = eval_cubic(traj, data.time)   # 平滑过渡
    else:
        arm_cmd = state._cached_target           # 保持在目标位置

    for i in range(6):
        d.ctrl[4 + i] = arm_cmd[i]              # 写入 position servo

    # ── ③ 数据记录 ──
    collector.maybe_record(data.time, fsm, ee, target, last_action)

    # ── ④ FSM 转换 ──
    nxt = _check_transitions(state)
    if nxt != fsm:
        # 抓取/释放管理 + 重新算IK + 启动新轨迹

ctrl 数组布局：

下标	内容	驱动方式
0–3	4 轮 motor	速度指令
4	shoulder_pan	position servo (PD)
5	shoulder_lift	position servo
6	elbow	position servo
7	wrist_pitch	position servo
8	finger_l	position servo
9	finger_r	position servo

2.12 GLFW 鼠标/键盘（第 562–609 行）—— No.2 模式

# Backspace → 重置仿真
mj.mj_resetData(model, data)
mj.mj_forward(model, data)
_grasp(False)
state = SimState()          # 重新创建 FSM 状态

# S → 手动保存数据
collector.save()

# Q/Esc → 退出
glfw.set_window_should_close(window, True)

# 鼠标拖拽 → 旋转/平移/缩放
mj.mjv_moveCamera(model, action, dx, dy, scene, cam)

2.13 main 主循环（第 671–793 行）—— No.1 + No.2

def main(headless=False):
    # 加载模型
    model = mj.MjModel.from_xml_path(XML_PATH)
    data  = mj.MjData(model)
    _cache_ids(model)
    _fk_data = mj.MjData(model)   # 独立的 FK 数据（No.12）

    if headless:
        # 无头模式：纯 mj_step 循环，N 轮自动重置
        for ep in range(NUM_EPISODES):
            while data.time < simend and state.fsm != FSM_DONE:
                mj.mj_step(model, data)
            mj.mj_resetData(model, data)  # 轮间重置
        collector.save()
        return

    # GUI 模式：
    # ① 创建 GLFW 窗口
    # ② 设置鼠标/键盘回调
    # ③ 渲染循环:
    while not glfw.window_should_close(window):
        # 每 1/60 秒渲染一帧，期间跑多个 mj_step
        while data.time - t_start < 1/60:
            mj.mj_step(model, data)

        # FSM 完成后自动重置进入下一轮
        if state.fsm == FSM_DONE:
            ep_count += 1
            mj.mj_resetData(model, data)
            state = SimState()

        # 相机跟踪小车
        cam.lookat = 0.5*(car + TARGET)  # 中点

        # 渲染
        mj.mjv_updateScene(...)
        mj.mjr_render(...)
        glfw.swap_buffers(window)

    # 结束：保存数据 + matplotlib 总结图
    collector.save()
    _plot_summary()

渲染 vs 仿真频率：仿真跑 1000Hz（timestep=0.001），渲染跑 60Hz。每帧之间跑 ~16 个 mj_step。

三、数据流总览

mj_step (1000Hz)
  └── controller (每步)
        ├── 车: 比例速度 / 刹车
        ├── 臂: 读缓存目标 → 三次轨迹插值 → 写 ctrl[4:10]
        ├── 记录: collector.maybe_record (10Hz)
        └── FSM: 检查转换条件 → (切换?) → 算 IK → 启动新轨迹
              └── GRASP 时激活 weld, RELEASE 时释放

渲染循环 (60Hz)
  └── mj_step × ~16 → updateScene → render → swapBuffers

结束后
  └── collector.save() → ep_*.npz
  └── matplotlib → summary_*.png

四、No.1–No.13 概念对照

No.	概念	在 demo_collect.py 中的位置
No.1	基础仿真循环	`main()` — `mj_step` + render loop
No.2	GLFW 渲染	`main()` — GLFW window + camera + scene + 鼠标回调
No.3	位置伺服 PD	`controller` — `data.ctrl[4:10]` 写 position servo 目标
No.4	动力学提取	`_log_mass_matrix_diag` — `mj_fullM` 提取 M 矩阵对角线
No.5	FSM + 三次轨迹	`_check_transitions` 9状态机 + `_cubic_coeffs` 轨迹插值
No.6	Jacobian IK	`numerical_ik` — `mj_jac` 算雅可比 + 伪逆
No.7	状态反馈控制	`controller` — 比例速度驱动 + 速度反馈刹车
No.8	约束管理	`_grasp` — 激活/停用 weld equality
No.9	位置触发	`_check_transitions` — car_x/EE距离触发状态切换
No.11	数值优化	`numerical_ik` — 阻尼伪逆迭代
No.12	独立 FK + 绘图	`_fk_data` 独立 MjData + `_plot_summary` matplotlib
No.13	姿态估计	`_car_quat` — `xquat` 读车身四元数 + 相机跟踪