BeyondMimic 与 rl_sar：从视频到机器人跳舞的部署实践

2.3 数据转换操作

环境配置完成后，执行以下命令即可将视频转换为 SMPLX 运动轨迹：

python tools/demo/demo.py --video=docs/example_video/tennis.mp4 -s

其中 --video 参数指定输入视频的路径，-s 参数表示保存结果。程序运行完成后会输出一个 .pt 文件，务必记录该文件的完整路径，因为下一步 GMR 需要用到它。

上图展示了 GVHMR 处理视频后的输出信息，可以看到程序会打印出 .pt 文件的保存路径。在实际操作中，建议将视频文件放在项目目录下的统一位置，并为每个视频建立独立的输出目录，方便后续管理多个动作数据。

需要特别注意的是，输入视频的质量直接影响运动提取的精度。理想的输入视频应当满足以下条件：画面中只有一个人、人物全身可见、背景相对简洁、光照均匀。如果视频中存在严重遮挡或多人交叉，GVHMR 的输出质量会明显下降，进而影响整条流程的最终效果。

3. GMR：将人体运动重定向到 G1 机器人

3.1 为什么需要运动重定向

人体有超过 200 块骨骼和数百个自由度，而 Unitree G1 机器人只有 23 或 29 个可控关节。两者在骨骼拓扑结构、关节运动范围、肢体长度比例上都存在本质差异。例如，人类的肩关节是一个球窝关节，可以在三个轴上自由旋转，而机器人的肩关节通常由三个串联的旋转关节来近似实现。直接将 SMPLX 的关节角度赋值给机器人关节，不仅物理上不可行，还可能导致自碰撞或关节超限。

GMR（General Motion Retargeting）通过优化方法解决这个问题。它的核心思路是：选取人体和机器人之间对应的关键身体部位（如手、脚、头、躯干），通过最小化这些对应部位在笛卡尔空间中的位置差异，求解出机器人的关节角度序列。这个过程会自动处理骨骼长度差异和关节约束，确保输出的机器人动作在运动学上是可行的。

3.2 数据转换：pt 到 pkl

由于上一步使用的是 GVHMR 框架，这里选择 GMR 中的 "Retargeting from GVHMR to Robot" 方法进行重定向：

python scripts/gvhmr_to_robot.py \
  --gvhmr_pred_file <path_to_hmr4d_results.pt> \
  --robot unitree_g1 \
  --record_video \
  --save_path motions/G1/G1.pkl

参数说明：

• --gvhmr_pred_file：上一步 GVHMR 生成的 .pt 文件路径
• --robot unitree_g1：目标机器人型号
• --record_video：录制重定向过程的可视化视频
• --save_path：输出 .pkl 文件的保存路径，务必记录

转换成功后，程序会自动播放重定向结果的可视化视频，可以直观地检查机器人动作是否与原始人体动作在视觉上一致：

3.3 数据转换：pkl 到 csv

BeyondMimic 框架要求输入的参考动作格式为 CSV 文件，因此需要将 GMR 输出的 .pkl 文件进一步转换。运行以下命令：

python3 scripts/batch_gmr_pkl_to_csv.py --folder /home/teacher/zzw/GMR/motions/G1/

其中 --folder 参数指定包含 .pkl 文件的目录路径。转换后的 CSV 文件每一行代表一帧数据，格式为：

root_pos_x, root_pos_y, root_pos_z, root_quat_x, root_quat_y, root_quat_z, root_quat_w, joint_0, joint_1, ..., joint_N

前 7 列是根节点（骨盆）的全局位置和四元数朝向，后续列是各关节的角度值。这个 CSV 文件是整条流程中承上启下的关键数据格式，后续的 BeyondMimic 训练和 rl_sar 部署都会用到它。

4. BeyondMimic：强化学习训练全流程

4.1 BeyondMimic 项目概述

BeyondMimic 是 UC 伯克利和斯坦福联合开源的人形机器人全身运动追踪框架。该项目基于 NVIDIA Isaac Lab 仿真平台，使用经典的 PPO（Proximal Policy Optimization）算法训练机器人在物理仿真中高保真地跟踪参考动作。与早期的运动模仿方法相比，BeyondMimic 的核心优势在于：它不仅能处理静态或低动态的动作，还能在真实硬件上实现跳跃旋转、冲刺、侧手翻等高度动态的技能，同时保持业界领先的运动质量。

其对应论文为 BeyondMimic: From Motion Tracking to Versatile Humanoid Control via Guided Diffusion，项目主页为 beyondmimic.github.io。

该项目的训练流程包含四个步骤：数据预处理（csv2npz）、数据上传（WandB Registry）、强化学习训练、以及策略评估与导出。下面逐一展开。

4.2 环境配置

按照 BeyondMimic 官方仓库的安装教程进行配置。核心依赖包括 NVIDIA Isaac Sim 4.5.0、Isaac Lab 2.1.0、RSL-RL 以及 Python 3.10。由于训练需要大量 GPU 并行仿真环境（默认 4096 个），建议使用 RTX 4090 或 A100 级别的显卡。

此外，BeyondMimic 使用 WandB（Weights & Biases）进行运动数据和训练日志的管理。在开始训练之前，需要先注册 WandB 账号，创建一个 Organization，并在本地配置好登录凭证：

pip install wandb && wandb login

4.3 数据预处理：CSV 转 NPZ

BeyondMimic 的训练不直接读取 CSV 文件，而是需要先将其转换为包含完整物理信息的 NPZ 格式。这个转换过程不仅仅是格式变换，还包括帧率插值和正向运动学计算。

python scripts/csv_to_npz.py \
  --input_file {motion_name}.csv \
  --input_fps 30 \
  --output_name {motion_name} \
  --headless

参数说明：

• --input_file：上一步生成的 CSV 文件路径
• --input_fps 30：输入 CSV 的帧率（通常为 30fps）
• --output_name：在 WandB Registry 中的名称
• --headless：不启动可视化窗口（去掉此参数可以看到机器人播放动作）

该脚本内部执行了以下关键步骤：首先将动作从输入帧率（如 30fps）插值到输出帧率（50fps，对应 dt=0.005s、decimation=4 的控制频率）；然后在 Isaac Sim 中通过正向运动学计算每个身体部位的世界坐标系位置、姿态四元数、线速度和角速度；最后将所有数据打包为 NPZ 文件并上传到 WandB Registry。

NPZ 文件包含的数据字段如下：

{
  'fps': 50,
  'joint_pos': [T, N_joints],
  'joint_vel': [T, N_joints],
  'body_pos_w': [T, N_bodies, 3],
  'body_quat_w': [T, N_bodies, 4],
  'body_lin_vel_w': [T, N_bodies, 3],
  'body_ang_vel_w': [T, N_bodies, 3]
}

需要注意的是，csv_to_npz.py 程序是无限循环运行的——它会持续重复播放转换后的数据。在不可视化模式下，程序看起来像是卡住了，实际上数据已经上传成功。当终端出现类似下图的信息时，即可按 Ctrl+C 终止进程：

4.4 数据验证

在启动训练之前，建议先回放一遍上传的数据，确认动作正确无误，避免训练错误的动作浪费算力：

python scripts/replay_npz.py \
  --registry_name={your-organization}-org/wandb-registry-motions/{motion_name}

4.5 强化学习训练

数据准备就绪后，即可启动训练。BeyondMimic 使用 RSL-RL 框架实现 PPO 算法，在 Isaac Lab 中并行运行 4096 个仿真环境进行采样：

python scripts/rsl_rl/train.py \
  --task=Tracking-Flat-G1-v0 \
  --registry_name {your-organization}-org/wandb-registry-motions/{motion_name} \
  --headless \
  --logger wandb \
  --log_project_name {project_name} \
  --run_name {run_name}

如果最终目标是部署到真实机器人上，需要使用不依赖状态估计的任务变体，将 task 参数改为 Tracking-Flat-G1-Wo-State-Estimation-v0：

python scripts/rsl_rl/train.py \
  --task=Tracking-Flat-G1-Wo-State-Estimation-v0 \
  --registry_name {your-organization}-org/wandb-registry-motions/{motion_name} \
  --headless \
  --logger wandb \
  --log_project_name {project_name} \
  --run_name {run_name}

两个 task 的区别在于观测空间的设计。Wo-State-Estimation 变体省略了依赖外部定位系统的线性分量（锚点位置误差的平移部分和根部线速度），因为这些信息在真实机器人上难以精确获取。训练默认迭代 30000 次，在 RTX 4090 上通常需要数小时。

训练过程中的核心超参数配置如下：

# PPO 采样配置
num_steps_per_env = 24  # 每个环境每次采样的步数
num_envs = 4096         # 并行环境数量

# 网络架构
actor_hidden = [512, 256, 128]  # Actor 网络隐藏层
critic_hidden = [512, 256, 128] # Critic 网络隐藏层
activation = "elu"              # 激活函数

# PPO 优化参数
learning_rate = 1e-3    # 初始学习率（自适应调整）
clip_param = 0.2        # PPO 裁剪参数
entropy_coef = 0.005    # 熵正则化系数
num_learning_epochs = 5 # 每次迭代的训练轮数
num_mini_batches = 4    # 每轮的 mini-batch 数
gamma = 0.99            # 折扣因子
lam = 0.95              # GAE lambda
desired_kl = 0.01       # 目标 KL 散度

4.6 策略评估与导出

训练完成后，使用以下命令加载模型并在仿真中评估效果：

python scripts/rsl_rl/play.py \
  --task=Tracking-Flat-G1-v0 \
  --num_envs=2 \
  --wandb_path={wandb-run-path}

评估脚本会自动从 WandB 下载训练好的模型和对应的参考动作，在仿真中运行策略并录制视频。同时，脚本会将策略导出为 ONNX 格式，用于后续在 rl_sar 中部署：

# play.py 内部的导出逻辑
export_motion_policy_as_onnx(env, policy, path="logs/.../exported/", filename="policy.onnx")

导出的 ONNX 模型包含了观测归一化层和策略网络，可以直接被 C++ 的 onnxruntime 加载执行。

5. BeyondMimic 算法深度解析

前面介绍了 BeyondMimic 的操作流程，但要真正理解这个系统为什么能让机器人高保真地复现人类动作，需要深入其算法设计的核心。本节将从观测空间、奖励函数、自适应采样和域随机化四个维度展开分析。

5.1 观测空间设计

强化学习策略的输入——观测空间（observation space）——决定了策略能"看到"什么信息。BeyondMimic 的观测空间设计体现了一个关键的工程权衡：信息量越大，策略理论上能做出越精确的决策，但同时也增加了训练难度和对传感器的依赖。

对于 G1 29DoF 机器人，完整的观测向量维度为 154，由以下六个部分拼接而成：

观测向量 (154 维) = motion_command (58) + motion_anchor_ori_b (6) + ang_vel (3) + dof_pos (29) + dof_vel (29) + actions (29)

各分量的含义如下：

• motion_command（58 维）：参考动作的关节位置和关节速度，维度为 num_of_dofs * 2。这是策略的"目标信号"，告诉策略当前时刻机器人的各关节应该处于什么位置、以什么速度运动。这些数据来自预处理阶段生成的 NPZ 文件，在训练时由仿真环境按时间步索引提供。
• motion_anchor_ori_b（6 维）：锚点（躯干）的朝向误差，用 6D 旋转表示法编码。这个分量描述了当前机器人躯干朝向与参考动作躯干朝向之间的差异。使用 6D 表示而非四元数或欧拉角，是因为 6D 表示在旋转空间中是连续的，避免了四元数的双覆盖问题和欧拉角的万向锁问题，有利于神经网络的学习。
• ang_vel（3 维）：机器人基座的角速度，来自 IMU 陀螺仪。这个信号帮助策略感知自身的旋转状态，对于维持平衡至关重要。
• dof_pos（29 维）：当前各关节的实际位置（弧度），来自关节编码器。
• dof_vel（29 维）：当前各关节的实际速度，来自关节编码器的差分或直接测量。
• actions（29 维）：上一时间步策略输出的动作，即上一步的关节位置目标。将历史动作作为观测的一部分，可以帮助策略学习到动作的时序连贯性，避免输出剧烈抖动的控制信号。

对于 23DoF 的 G1 变体，观测维度相应缩减为 124 维（46 + 6 + 3 + 23 + 23 + 23）。

值得注意的是，BeyondMimic 提供了两种任务变体：Tracking-Flat-G1-v0 和 Tracking-Flat-G1-Wo-State-Estimation-v0。后者省略了锚点位置误差的平移分量和根部线速度，因为这些信息依赖外部定位系统（如动捕或 SLAM），在真实机器人上难以精确获取。如果最终目标是真机部署，应当选择 G1_v0 变体进行训练。

5.2 奖励函数设计

奖励函数是强化学习的灵魂，它定义了"什么是好的行为"。BeyondMimic 的奖励函数由多个子项加权求和构成，每个子项负责约束机器人行为的一个方面。以下是核心奖励项及其物理含义：

# 关节位置跟踪奖励
r_joint_pos = exp(-5.0*||q_actual - q_ref||^2)
# 关节速度跟踪奖励
r_joint_vel = exp(-0.1*||dq_actual - dq_ref||^2)
# 身体部位位置跟踪奖励（关键身体部位在笛卡尔空间的位置误差）
r_body_pos = exp(-40.0*||p_body_actual - p_body_ref||^2)
# 身体部位朝向跟踪奖励
r_body_rot = exp(-10.0*||quat_body_actual - quat_body_ref||^2)
# 能量惩罚（鼓励节能的控制策略）
r_energy = -k_energy * sum(|tau * dq|)
# 动作平滑度惩罚（抑制控制信号的剧烈变化）
r_smooth = -k_smooth * ||a_t - a_{t-1}||^2

这些奖励项的设计遵循了几个重要原则。首先，所有跟踪奖励都使用指数函数 exp(-k * error^2) 的形式，而非简单的线性惩罚。指数形式的优势在于：当误差较小时，奖励接近 1，梯度较小，策略不会过度修正；当误差较大时，奖励迅速衰减到 0，提供强烈的纠正信号。这种"软约束"比硬性的阈值惩罚更有利于策略的平滑收敛。

其次，不同奖励项的系数（如关节位置的 5.0、身体位置的 40.0）反映了各项的相对重要性。身体位置的系数远大于关节位置，意味着系统更关注末端执行器（手、脚）在空间中的绝对位置是否正确，而非每个关节角度是否精确匹配。这符合运动控制的直觉：观众看到的是手脚的轨迹，而非肘关节的角度。

5.3 自适应采样机制

BeyondMimic 引入了一种自适应采样（Adaptive Sampling）机制来提升训练效率。传统的运动模仿训练中，每个 episode 从参考动作的随机时间点开始，所有时间点被均匀采样。但实际上，参考动作中不同片段的难度差异很大——站立和行走相对简单，而跳跃和旋转则困难得多。均匀采样会导致策略在简单片段上浪费大量训练资源，而在困难片段上采样不足。

自适应采样的核心思路是：记录每个时间点的训练失败率，对失败率高的时间点增加采样概率。具体实现如下：

# 自适应采样伪代码
for each training iteration:
    # 1. 根据当前失败率分布采样起始时间点
    start_times = sample_with_bias(failure_rates)
    # 2. 运行仿真，收集轨迹
    trajectories = simulate(policy, start_times)
    # 3. 更新失败率统计
    for t in start_times:
        if episode_terminated_early(t):
            failure_rates[t] += alpha  # 增加失败计数
        else:
            failure_rates[t] *= decay  # 衰减成功计数
    # 4. 归一化为采样概率
    sampling_probs = normalize(failure_rates)

这种机制使得训练资源自动向困难片段倾斜，显著加速了整体收敛速度。在实验中，自适应采样相比均匀采样可以将达到相同跟踪精度所需的训练迭代次数减少 30%-50%。

5.4 域随机化

从仿真到真实（sim-to-real）的迁移是强化学习部署的核心挑战。仿真环境与真实世界之间存在不可避免的差异——物理参数的不精确、传感器噪声、执行器延迟等。BeyondMimic 通过域随机化（Domain Randomization）来弥合这个差距，其核心思想是：在训练时随机扰动仿真环境的物理参数，迫使策略学习到对这些参数变化具有鲁棒性的控制策略。

BeyondMimic 中使用的域随机化参数包括：

# 域随机化配置
randomize_friction = True      # 地面摩擦系数随机化
friction_range = [0.5, 1.25]   # 摩擦系数范围
randomize_base_mass = True     # 基座质量随机化
added_mass_range = [-1.0, 1.0] # 附加质量范围 (kg)
randomize_com_offset = True    # 质心偏移随机化
com_offset_range = [-0.05, 0.05] # 质心偏移范围 (m)
push_robots = True             # 随机推力扰动
push_interval_s = 15           # 推力间隔 (秒)
max_push_vel_xy = 0.5          # 最大推力速度 (m/s)
randomize_joint_default_pos = True # 关节默认位置随机化
joint_default_pos_range = [-0.05, 0.05] # 偏移范围 (rad)

这些随机化参数的选择并非随意的。摩擦系数的随机化范围 [0.5, 1.25] 覆盖了从光滑瓷砖到粗糙地毯的常见地面条件。质心偏移模拟了机器人负载变化或组装误差。随机推力则模拟了外部干扰，如被轻微碰撞。关节默认位置的随机化则补偿了关节编码器的零点漂移。

通过在训练中暴露于这些随机化的环境条件，策略学会了在不确定性下保持稳定的控制能力，这是成功进行 sim-to-real 迁移的关键前提。

6. BeyondMimic 项目代码结构分析

理解 BeyondMimic 的代码组织方式，有助于在需要修改训练配置、添加新机器人或调试问题时快速定位相关文件。本节对项目的核心目录和关键文件进行逐一解读。

6.1 项目目录总览

BeyondMimic 的代码仓库（对应 GitHub 上的 whole_body_tracking）遵循 Isaac Lab 扩展的标准目录结构：

whole_body_tracking/
├── exts/
│   └── robot_lab/
│       └── robot_lab/
│           ├── assets/ # 机器人 URDF/MJCF 模型文件
│           │   └── g1/
│           │       ├── g1.urdf
│           │       └── g1_29dof.xml # MuJoCo 格式模型
│           ├── tasks/ # 任务定义（环境、观测、奖励）
│           │   └── locomotion/
│           │       └── tracking/
│           │           ├── config/
│           │           │   └── g1/
│           │           │       ├── flat_env_cfg.py # 环境配置
│           │           │       └── agents/
│           │           │           └── rsl_rl_ppo_cfg.py # PPO 超参数
│           │           ├── tracking_env.py # 核心环境类
│           │           └── mdp/ # MDP 组件
│           │               ├── observations.py # 观测计算
│           │               ├── rewards.py # 奖励计算
│           │               └── events.py # 域随机化事件
│           └── utils/
│               └── motion_lib.py # 运动数据加载库
├── scripts/
│   ├── csv_to_npz.py # CSV 转 NPZ 预处理
│   ├── replay_npz.py # NPZ 数据回放验证
│   └── rsl_rl/
│       ├── train.py # 训练入口
│       └── play.py # 评估与导出入口
└── motions/ # 运动数据存放目录
    └── g1/
        └── *.csv # 参考动作 CSV 文件

6.2 核心文件解读

tracking_env.py —— 环境核心

这是整个训练系统的中枢。它继承自 Isaac Lab 的 ManagerBasedRLEnv，负责协调仿真步进、观测计算、奖励计算和环境重置。每个训练步骤的执行流程如下：

# tracking_env.py 核心循环（简化）
class TrackingEnv(ManagerBasedRLEnv):
    def step(self, actions):
        # 1. 将策略输出的动作转换为关节目标
        joint_targets = self.default_dof_pos + actions * self.action_scale
        # 2. 应用 PD 控制，计算关节力矩
        torques = self.kp *(joint_targets - self.dof_pos)+ self.kd *(0- self.dof_vel)
        # 3. 在仿真中施加力矩，步进物理引擎
        self.sim.apply_torques(torques)
        for _ in range(self.decimation): # decimation=4, 即每个控制步执行 4 个物理步
            self.sim.step()
        # 4. 读取新的状态
        self.update_state()
        # 5. 计算观测和奖励
        obs = self.compute_observations()
        rewards = self.compute_rewards()
        return obs, rewards, dones, infos

这里有一个关键细节：decimation 参数。物理仿真的时间步长 dt=0.005s（200Hz），但策略的控制频率是 1/(dt * decimation) = 1/(0.005 * 4) = 50Hz。也就是说，策略每输出一个动作，物理引擎会执行 4 步仿真。这种设计既保证了物理仿真的精度，又降低了策略的决策频率，减轻了训练负担。

observations.py —— 观测计算

该文件定义了各个观测分量的计算方法。以 motion_command 为例：

def motion_command(env):
    """从参考动作中提取当前时刻的关节位置和速度目标"""
    motion_time = env.episode_length_buf * env.dt * env.decimation
    ref_joint_pos = env.motion_lib.get_joint_pos(motion_time)
    ref_joint_vel = env.motion_lib.get_joint_vel(motion_time)
    return torch.cat([ref_joint_pos, ref_joint_vel], dim=-1)

rewards.py —— 奖励计算

每个奖励项被实现为一个独立的函数，便于组合和调试：

def joint_pos_tracking(env):
    """关节位置跟踪奖励"""
    error = torch.sum((env.dof_pos - env.ref_joint_pos)**2, dim=-1)
    return torch.exp(-5.0* error)

def body_pos_tracking(env):
    """身体部位笛卡尔位置跟踪奖励"""
    error = torch.sum((env.body_pos - env.ref_body_pos)**2, dim=(-2,-1))
    return torch.exp(-40.0* error)

motion_lib.py —— 运动数据管理

这个工具类负责从 WandB Registry 加载 NPZ 格式的运动数据，并提供按时间索引的查询接口。它在内部维护了一个时间索引表，支持线性插值以获取任意时刻的参考状态。

6.3 PD 控制与动作空间

BeyondMimic 的动作空间是关节位置目标空间。策略输出的动作 a 经过以下变换得到实际的关节位置目标：

q_target = default_dof_pos + a * action_scale

其中 default_dof_pos 是机器人的默认站立姿态，action_scale 控制动作的幅度。然后通过 PD 控制器计算关节力矩：

tau = Kp * (q_target - q_actual) + Kd * (0 - dq_actual)

注意速度目标默认为 0，这意味着 PD 控制器会自然地阻尼关节运动，有助于稳定性。Kp 和 Kd 的值因关节而异——承重的腿部关节使用较大的增益（如髋关节 Kp=99），而手臂关节使用较小的增益（如肩关节 Kp=14）。

action_scale 的计算公式为：

action_scale = 0.25 * effort_limit / stiffness

这个公式确保了策略输出在 [-1, 1] 范围内时，关节目标不会超出物理可行的范围。effort_limit 是关节的最大力矩，stiffness 是 PD 控制器的 Kp 值。0.25 的系数提供了额外的安全裕度。

7. 将 BeyondMimic 集成到 rl_sar：从仿真验证到真机部署

这是本文最核心的实践部分。BeyondMimic 在 Isaac Lab 中完成训练后，需要将策略迁移到 rl_sar 框架中进行独立的仿真验证（sim2sim）和真实机器人部署（sim2real）。这个过程远非简单的"复制粘贴"，涉及大量的参数对齐、数据格式转换和工程细节。本节将完整记录这一过程中的每个步骤和遇到的关键问题。

7.1 rl_sar 框架概述

rl_sar 是一个开源的强化学习 sim-to-real 部署框架，支持在 MuJoCo 和 Gazebo 仿真器中验证策略，并部署到 Unitree 系列机器人的真实硬件上。该框架使用 C++ 编写核心控制循环，通过 ONNX Runtime 或 LibTorch 加载训练好的策略模型，以满足实时控制对延迟的严格要求。

rl_sar 的架构设计围绕有限状态机（FSM）展开。机器人在不同的控制状态之间切换，每个状态对应一种行为模式。以 G1 机器人为例，其 FSM 包含以下状态：

RLFSMStatePassive ← 被动模式（关节阻尼，无主动控制）
↓ [按键 0 / 手柄 A]
RLFSMStateGetUp ← 起立（从当前姿态插值到默认站立姿态）
↓ [按键 1 / 手柄 RB+DPadUp]
RLFSMStateRLRoboMimicLocomotion ← 行走控制（基础运动策略）
↓ [按键 2-5 / 手柄方向键]
RLFSMStateRLRoboMimicCharleston ← 查尔斯顿舞（Num2）
RLFSMStateRLWholeBodyTrackingDance102 ← Dance 102（Num3）
RLFSMStateRLWholeBodyTrackingGangnamStyle ← 江南 Style（Num4）
RLFSMStateRLWholeBodyTrackingMimic ← 自定义动作（Num5）
↓ [按键 9 / 手柄 B]
RLFSMStateGetDown ← 蹲下（插值回初始姿态）
↓ [完成]
RLFSMStatePassive ← 回到被动模式

每个 FSM 状态在进入时加载对应的策略模型和配置文件，在运行时执行策略推理并输出关节控制指令。这种设计使得同一个机器人可以在运行时动态切换不同的技能，而无需重启程序。

7.2 rl_sar 的配置文件体系

rl_sar 使用 YAML 配置文件管理所有与策略相关的参数。配置文件分为两层：基础配置（base.yaml）和策略配置（config.yaml）。

base.yaml —— 机器人硬件参数

以 G1 23DoF 为例，policy/g1_23/base.yaml 定义了与硬件相关的基础参数：

# policy/g1_23/base.yaml（关键字段摘录）
g1_23:
  num_of_dofs: 23
  dt: 0.005 # 物理仿真时间步长
  decimation: 4 # 控制频率 = 1/(dt*decimation) = 50Hz
  # 关节名称（定义了训练时的关节顺序）
  joint_names:
    ["left_hip_pitch_joint","left_hip_roll_joint","left_hip_yaw_joint","left_knee_joint","left_ankle_pitch_joint","left_ankle_roll_joint","right_hip_pitch_joint","right_hip_roll_joint","right_hip_yaw_joint","right_knee_joint","right_ankle_pitch_joint","right_ankle_roll_joint","waist_yaw_joint","left_shoulder_pitch_joint","left_shoulder_roll_joint","left_shoulder_yaw_joint","left_elbow_joint","left_wrist_yaw_joint","right_shoulder_pitch_joint","right_shoulder_roll_joint","right_shoulder_yaw_joint","right_elbow_joint","right_wrist_yaw_joint"]
  # 默认站立姿态（弧度）
  default_dof_pos:
    [-0.10,0.0,0.0,0.32,-0.20,0.0,# 左腿
     -0.10,0.0,0.0,0.32,-0.20,0.0,# 右腿
     0.0,# 腰部偏航
     0.0,0.2,0.0,0.4,0.0,# 左臂
     0.0,-0.2,0.0,0.4,0.0# 右臂]
  # 关节映射：训练关节索引 → MuJoCo/SDK 关节索引
  # 23DoF 跳过了 29DoF 中的 waist_roll(13), waist_pitch(14),
  # left_wrist_pitch(20), left_wrist_yaw(21), right_wrist_pitch(27), right_wrist_yaw(28)
  joint_mapping:
    [0,1,2,3,4,5,6,7,8,9,10,11,12,15,16,17,18,19,22,23,24,25,26]
  # 力矩限制
  torque_limits:
    [88,139,88,139,50,50,88,139,88,139,50,50,88,25,25,25,25,25,25,25,25,25,25,25,25]
  # 编码器偏置（补偿硬件零点偏差）
  use_encoder_bias: true
  encoder_bias:
    [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.87,0,0,0,0,0.87,0]

这里有几个关键点需要特别注意：

第一，joint_mapping 的作用是将训练时的关节索引映射到 MuJoCo 仿真或真实硬件的关节索引。23DoF 的 G1 相比 29DoF 版本去掉了腰部的 roll/pitch 关节和手腕的 pitch/yaw 关节，因此映射数组中出现了跳跃（12 之后直接到 15，19 之后直接到 22）。

第二，encoder_bias 中左右肘关节的偏置值为 0.87 弧度。这是因为 mjlab（BeyondMimic 使用的 MuJoCo 仿真环境）中肘关节的零点定义与 Unitree SDK 不同。如果不加这个偏置，机器人的手臂会出现明显的姿态偏差。

config.yaml —— 策略专属参数

每个策略都有自己的 config.yaml，定义了与该策略相关的特定参数。以 23DoF 全身跟踪策略为例：

# policy/g1_23/whole_body_tracking/config.yaml（关键字段摘录）
g1_23/whole_body_tracking:
  model_name: "policy3.pt" # 观测空间定义（顺序必须与训练时完全一致）
  observations:
    ["whole_body_tracking/motion_command","whole_body_tracking/motion_anchor_ori_b","ang_vel","dof_pos","dof_vel","actions"]
  num_observations: 124 # 46 + 6 + 3 + 23 + 23 + 23
  clip_obs: 100.0 # 动作裁剪：空数组表示不裁剪（关键！）
  clip_actions_lower: []
  clip_actions_upper: []
  # PD 增益（必须与训练时一致）
  rl_kp:
    [40.179,99.098,40.179,99.098,28.501,28.501,40.179,99.098,40.179,99.098,28.501,28.501,40.179,14.251,14.251,14.251,14.251,14.251,14.251,14.251,14.251,14.251,14.251,14.251,14.251]
  rl_kd:
    [2.558,6.309,2.558,6.309,1.814,1.814,2.558,6.309,2.558,6.309,1.814,1.814,2.558,0.907,0.907,0.907,0.907,0.907,0.907,0.907,0.907,0.907,0.907,0.907,0.907]
  # 动作缩放（与训练时一致）
  action_scale:
    [0.548,0.351,0.548,0.351,0.439,0.439,0.548,0.351,0.548,0.351,0.439,0.439,0.548,0.439,0.439,0.439,0.439,0.439,0.439,0.439,0.439,0.439,0.439,0.439,0.439]
  # 参考动作 CSV 文件
  motion_file: "G1_Take_102.bvh_50hz_23dof.csv"
  # 运动数据关节映射（从 29DoF CSV 中选取 23DoF 对应的列）
  motion_joint_mapping:
    [0,1,2,3,4,5,6,7,8,9,10,11,12,15,16,17,18,19,22,23,24,25,26]
  # 腰部关节索引（用于锚点朝向计算）
  waist_joint_indices: [12]

7.3 MotionLoader：运动数据的 C++ 加载器

在 BeyondMimic 的训练环境中，参考动作由 Python 的 motion_lib.py 管理。但在 rl_sar 的 C++ 部署环境中，需要一个等价的 C++ 实现来加载和插值参考动作数据。这就是 MotionLoader 类的作用。