动作序列加载与运动策略

本文档深入解析推理节点中动作序列（Motion Sequence）的加载机制与多运动策略的实时切换架构。与纯强化学习策略不同，基于动作序列的运动策略（Motion Policy）通过将预录制的关节轨迹以观测参考的形式注入网络，使机器人能够执行挥手、舞蹈、起身等特定行为。理解这一机制对于扩展新动作、调试轨迹对齐以及设计混合控制逻辑至关重要。

动作序列数据格式与加载机制

系统中的动作序列以 .npz（NumPy compressed archive）格式存储，这是强化学习训练管线导出的标准中间格式。MotionLoader 类负责在节点初始化阶段将磁盘文件映射为内存中的帧序列，支持按索引随机访问。

.npz 文件内部包含三个关键数组：

键名	形状	数据类型	说明
`fps`	`(1,)`	`int32`	动作录制时的采样帧率
`joint_pos`	`(num_frames, num_joints)`	`float32`	每一帧的关节位置（相对于默认姿态的偏移或绝对角度）
`joint_vel`	`(num_frames, num_joints)`	`float32`	每一帧的关节速度

MotionLoader 在构造时通过 cnpy::npz_load 完成一次性加载，并将扁平化的 NumPy 数据转置为 std::vector<std::vector<float>> 的二维结构，便于后续按帧索引直接拷贝到观测缓冲区。加载完成后会对关节数量与系统配置的 joint_num 进行严格校验，若出现不匹配则立即抛出运行时异常，防止后续推理出现越位错误。

Sources: motion_loader.hpp, motion_loader.cpp

运动策略运行时架构

推理节点内部采用策略运行时（PolicyRuntime）作为核心抽象。每个 PolicyRuntime 实例维护独立的 ONNX 会话上下文、观测布局、帧堆叠状态以及可选的动作序列加载器。系统支持同时配置多个策略，但同一时刻仅有一个激活策略参与推理循环。

graph TD
    A[配置加载 load_config] --> B[构建 PolicyRuntime 数组]
    B --> C{motion_path 是否为空?}
    C -->|非空| D[实例化 MotionLoader]
    C -->|空| E[标准策略]
    D --> F[注册到 motion_policy_indices_]
    E --> G[初始化 ONNX Session]
    F --> G
    G --> H[推理循环 inference]
    H --> I{激活策略含 motion_loader?}
    I -->|是| J[step_motion_frame 递增帧索引]
    I -->|否| K[纯网络输出]
    J --> L[get_motion_pos_obs / get_motion_vel_obs 注入观测]
    L --> M[ONNX Run 生成动作]
    K --> M
    M --> N[apply_action 下发电机]

关键设计决策在于：动作序列并非直接作为电机指令下发，而是作为额外观测输入馈送给神经网络。这意味着运动策略本质上是一个“参考轨迹跟踪策略”——网络根据当前机体状态与目标动作参考，实时计算跟踪所需的关节力矩或位置指令。该设计兼顾了轨迹的平滑性与环境适应性，避免了开环播放可能导致的失衡。

Sources: inference_node.hpp, inference_node.cpp

多策略配置与观测布局

动作序列策略的启用完全由 YAML 配置驱动。以 inference_beyondmimic.yaml 为例，系统同时注册了 4 个策略：

索引	model_names	motion_names	策略类型
0	`policy.onnx`	`""`	标准 locomotion 策略
1	`policy_wave.onnx`	`wave.npz`	挥手动作策略
2	`policy_dance.onnx`	`dance.npz`	舞蹈动作策略
3	`policy_punch.onnx`	`punch.npz`	击打动作策略

对于含动作序列的策略，其 obs_layout 必须显式包含 motion_pos 与 motion_vel 两个观测源。例如：

obs_layouts:
  - "ang_vel:3, gravity_b:3, cmd_vel:3, dof_pos:23, dof_vel:23, last_action:23"
  - "motion_pos:23, motion_vel:23, ang_vel:3, gravity_b:3, dof_pos:23, dof_vel:23, last_action:23"

观测布局解析器 parse_obs_layout 采用 name:size 的 DSL 格式，并通过 obs_source_definitions() 注册表将名称映射到成员函数指针。motion_pos 与 motion_vel 分别绑定到 get_motion_pos_obs 和 get_motion_vel_obs，在每次推理迭代时将当前帧的关节位置与速度拷贝至对应的观测段。

Sources: ros_interface.cpp, obs_manager.cpp

动作序列步进与观测注入

在推理线程的主循环中，若激活策略关联了 motion_loader，则每次调用 ONNX Run 之前会先执行 step_motion_frame()。该方法以单调递增方式推进帧索引，当到达最后一帧时持续保持在末尾帧，而非循环回绕。这一行为决定了动作序列策略天然适合执行有限长度的技能片段（如起身、挥手），而非周期性动作。

观测注入的具体实现如下：

get_motion_pos_obs 从 policy.motion_loader->get_pos(policy.motion_frame) 读取当前帧位置，拷贝到观测段；
get_motion_vel_obs 同理读取速度；
观测数据经 clip_observations 裁剪后，与机体状态观测拼接为完整输入张量；
网络输出通过 usd2urdf_ 映射表进行关节顺序重排，并叠加默认角度偏移后下发。

需要特别注意的是，动作序列文件的关节顺序默认为训练时使用的 USD 顺序，因此必须与 usd2urdf 映射保持一致。若顺序错误，网络将观测到完全错位的人体参考轨迹，导致动作畸变甚至摔倒。

Sources: obs_manager.cpp, inference_node.cpp

策略切换控制逻辑

系统通过手柄按键实现标准策略与动作策略之间的无停机切换。切换逻辑由 subs_joy_callback 中的 LB（buttons[4]） 与 RB（buttons[5]） 按键共同管理，并遵循“先暂停、后切换、再恢复”的原子流程，以避免在策略状态迁移过程中发生观测缓冲区污染。

sequenceDiagram
    participant U as 用户
    participant I as InferenceNode
    participant P0 as Policy 0 (Locomotion)
    participant Pn as Policy N (Motion)

    U->>I: 按下 RB
    I->>I: current_motion_policy_idx 循环递增
    I->>I: 日志输出待选策略名称
    Note over I: 此时仍处于标准策略

    U->>I: 按下 LB
    I->>I: is_running = false（暂停推理）
    alt supports_interrupt() == true
        I->>I: 翻转 is_interrupt_ 标志
    else has_motion_policy() == true
        I->>I: 翻转 is_motion_policy_
        I->>I: active_policy_idx = motion_policy_indices_[current]
        I->>I: reset_policy_runtime(激活策略)
    end
    I->>I: is_running = true（恢复推理）
    I->>Pn: 以全新状态启动运动策略

reset_policy_runtime 会在切换时清空目标策略的观测缓冲区、ONNX 输入张量以及关键的动作帧计数器 motion_frame。这确保了每次切入动作策略时都从序列的第一帧开始播放，避免从中途残留帧启动造成动作跳变。

Sources: ros_interface.cpp, inference_node.cpp

中断干预机制

除了基于动作序列的运动策略，系统还支持一种更轻量的**中断干预（Interrupt）**模式，用于在标准 locomotion 策略运行时通过外部话题覆盖部分关节动作。该机制在 inference_interrupt.yaml 中启用，其核心是在观测布局中增加 interrupt:1 二元标志。

当用户通过手柄 LB 键启用中断模式后：

is_interrupt_ 置为 true，get_interrupt_obs 向网络注入 1.0；
推理循环在动作后处理阶段检测到 is_interrupt_ 为真时，将 act_ 数组的末尾 10 个关节（通常为手臂）替换为 /joint_ref_states 话题接收到的外部参考值；
其余关节继续由网络输出控制，实现“下半身平衡 + 上半身遥操作”的混合行为。

中断动作通过 subs_joint_state_callback 实时更新，因此可配合手柄或上位机进行即时干预。该模式与动作序列策略互斥：若配置中同时存在 interrupt 观测源与 motion_policy，LB 键优先触发中断切换，RB 键则不可用。

Sources: obs_manager.cpp, inference_node.cpp, ros_interface.cpp

独立动作播放器

对于不依赖神经网络、仅希望直接回放动作序列的场景，项目提供了独立的 motion_player.py 脚本。该工具通过 robot_py Python 绑定直接操作 RobotInterface，以固定周期（默认 200 Hz）将 .npz 中的关节位置作为 apply_action 的目标值下发。

与推理节点中运动策略的本质区别：

维度	MotionPlayer 脚本	推理节点 Motion Policy
控制方式	开环直接下发位置	闭环网络输出跟踪参考
运行依赖	仅需 `robot_py`	需完整 ROS2 + ONNXRuntime
帧率处理	按 `fps` 与 `speed` 倍率抽帧	由推理循环 `dt * decimation` 决定
适用场景	动作验证、电机测试、零点检查	在线平衡自适应、复杂技能执行
关节顺序转换	支持 `--usd2urdf` 参数	依赖配置中 `usd2urdf` 映射

使用该脚本时建议先以较低倍率（如 --speed 0.5）验证轨迹，确认关节顺序与方向无误后再全速运行。

Sources: motion_player.py, motion_player.yaml

配置实践与参数对照

在实际部署中，动作序列策略的配置需关注以下参数耦合关系：

参数	作用域	动作策略注意事项
`motion_names`	全局数组	与 `model_names` 等长，空字符串表示该策略无动作序列
`frame_stacks`	每策略独立	动作策略通常设为 `1`，因为 `motion_pos/vel` 已包含时序信息
`obs_stack_orders`	每策略独立	若使用帧堆叠，`frame_major` 与 `obs_major` 影响输入张量排布
`dt` + `decimation`	全局	决定推理周期，需与动作序列 `fps` 在训练阶段对齐
`usd2urdf`	全局	必须同时匹配训练时 USD 关节顺序与 `.npz` 文件顺序
`act_alpha`	全局	动作滤波系数，动作策略通常设为 `1.0` 以避免额外滞后

若需新增自定义动作，标准工作流为：在仿真环境中录制或生成轨迹 → 导出为含 fps/joint_pos/joint_vel 的 .npz → 放入 src/inference/motions/ → 复制一份配置 YAML，在 model_names 与 motion_names 中追加对应条目 → 重新编译并部署。模型本身需要通过模仿学习或 BeyondMimic 等方法训练，以学会将动作参考观测映射为实际控制指令。

Sources: inference_beyondmimic.yaml, inference_getup.yaml

延伸阅读建议：若希望深入理解观测堆叠的数学细节与多策略切换的底层线程模型，请参阅观测堆叠与多策略切换与推理节点设计与线程模型。如需将新训练的策略接入本系统，可参考接入自定义强化学习策略中的配置规范与模型导出要求。