Hands On Dual Running Blend

手把手 5：从零使用双状态运行混合过渡

这一课继续使用前面写过的 SinWaveState。目标是实现这种切换效果：

旧状态继续生成动作，权重 1 -> 0
新状态也提前生成动作，权重 0 -> 1
最终发给电机的是两边电机目标的混合结果

这适合从步态切到动作、从动作切回步态、或两个模型状态之间切换。它比 first_frame_switch 更主动：新状态不是只拿第一帧等着，而是在过渡阶段就开始运行。

1. 先把电机目标计算独立出来

双状态混合依赖状态提供一个纯计算接口：

def get_motor_frame(
    self, ctx: BxiExample, dt: float, on_translation: bool
) -> Optional[MotorFrame]:
    ...
    return self._motor_frame(qpos, kp, kd)

例如 SinWaveState 不要把所有逻辑都塞进 on_update()，而是先这样拆：

def get_motor_frame(
    self, ctx: BxiExample, dt: float, on_translation: bool
) -> Optional[MotorFrame]:
    self.elapsed += dt

    qpos = ctx.joint_nominal_pos.copy()
    qpos[self.joint] += self.amplitude * math.sin(
        2.0 * math.pi * self.frequency * self.elapsed
    )

    return self._motor_frame(qpos, ctx.joint_kp, ctx.joint_kd)


def on_update(self, ctx: BxiExample, dt: float) -> None:
    frame = self.get_motor_frame(ctx, dt, False)
    if frame is not None:
        ctx.set_motor_target(*frame)

正常运行时 on_update() 把帧写给电机；过渡时 dual_running_blend 直接调用 get_transition_frame()，默认再复用 get_motor_frame()。这样状态输出逻辑只有一份，节点也不需要知道怎么采样。

2. 在 YAML 里定义过渡 profile

打开：

src/bxi_example_py_elf3/config/elf3_state_machine.yaml

在 transition_profiles: 下添加：

transition_profiles:
  sin_wave_dual_blend:
    duration: 0.30
    exit_behavior: none
    enter_behavior: dual_running_blend
    data:
      curve: smoothstep
      run_from: true
      run_to: true
      from_fallback: last_motor
      to_fallback: first_frame

关键点：

• enter_behavior: dual_running_blend 表示进入侧状态负责最终混合输出。
• duration: 0.30 表示整个混合持续 0.3 秒。
• run_from: true 表示旧状态在过渡中继续运行并被采样。
• run_to: true 表示新状态在过渡中提前运行并被采样。
• curve: smoothstep 表示权重不是线性突变，开头和结尾更柔和。

3. 把状态切换改成这个 profile

例如从 normal 进入 sin_wave：

states:
  normal:
    transitions:
      on_event:
        sin_wave:
          to: sin_wave
          transition: sin_wave_dual_blend

也可以不定义 profile，直接内联：

states:
  normal:
    transitions:
      on_event:
        sin_wave:
          to: sin_wave
          transition:
            name: normal_to_sin_wave_blend
            duration: 0.30
            exit_behavior: none
            enter_behavior: dual_running_blend
            data:
              curve: smoothstep
              from_fallback: last_motor
              to_fallback: first_frame

常用过渡建议放进 transition_profiles；只服务一次的特殊过渡可以内联。

4. 运行时到底发生了什么

触发 normal -> sin_wave 后，每个控制周期会做：

normal.get_transition_frame(role="from")
sin_wave.get_transition_frame(role="to")
alpha = enter_progress
qpos = normal_qpos * (1 - alpha) + sin_wave_qpos * alpha
kp   = normal_kp   * (1 - alpha) + sin_wave_kp   * alpha
kd   = normal_kd   * (1 - alpha) + sin_wave_kd   * alpha
ctx.set_motor_target(qpos, kp, kd)

RobotControlState.get_transition_frame() 默认调用：

get_motor_frame(ctx, ctx.dt, True)

所以大多数状态只需要实现 get_motor_frame()。

注意：旧状态的 on_exit() 仍然会在过渡开始时调用。内置状态的 on_exit() 只保存上一状态电机信息，不会销毁模型，所以还能继续采样。如果你自己写状态，不要在 on_exit() 里释放 dual_running_blend 还要采样的 policy、动作数据或缓存。

5. 验证状态机信息

启动后 echo 状态机信息：

ros2 topic echo /simulation/state_machine_info

过渡期间应该能看到：

{
  "mode": "transition",
  "transition": {
    "from": {"name": "normal"},
    "to": {"name": "sin_wave"},
    "profile": "sin_wave_dual_blend",
    "progress": 0.5,
    "enter_behavior": "dual_running_blend",
    "data": {
      "curve": "smoothstep",
      "run_from": true,
      "run_to": true
    }
  }
}

progress 到 1 后，状态机会提交到 sin_wave。

6. 为什么不会在结束时回到第一帧

dual_running_blend 会让目标状态在过渡期间提前运行。普通状态切换结束时会调用：

on_enter(ctx)

如果这里再调用一次，目标状态会被重新初始化，动作可能跳回第一帧。

所以当前框架改成：

on_transition_commit(ctx, from_state, transition)

默认情况下它等价于 on_enter()。但 RobotControlState 发现目标状态已经在过渡期间运行过，就会跳过重复的 on_enter()。

7. 什么时候需要手写 get_transition_frame

默认过渡采样会复用 get_motor_frame()。这对多数状态足够，但有些状态不适合：

• 你想过渡采样时不推进动作帧，只看当前帧。
• 你想混合一个专门的预览动作，而不是正式运行逻辑。
• 你想旧状态和新状态使用不同的采样策略。

这时在状态里重写：

def get_transition_frame(
    self,
    ctx: BxiExample,
    role: str,
    transition: TransitionProfile,
) -> Optional[MotorFrame]:
    qpos = ctx.joint_nominal_pos.copy()
    qpos[self.joint] += self.amplitude * math.sin(
        2.0 * math.pi * self.frequency * self.elapsed
    )
    return self._motor_frame(qpos, ctx.joint_kp, ctx.joint_kd)

role 的值：

• from：这个状态是旧状态。
• to：这个状态是新状态。

例如只在作为目标状态时推进时间：

def get_transition_frame(self, ctx, role, transition):
    if role == "to":
        self.elapsed += ctx.dt

    qpos = ctx.joint_nominal_pos.copy()
    qpos[self.joint] += self.amplitude * math.sin(
        2.0 * math.pi * self.frequency * self.elapsed
    )
    return self._motor_frame(qpos, ctx.joint_kp, ctx.joint_kd)

8. fallback 怎么用

如果某一侧采样不到电机帧，框架会使用 fallback。

data:
  from_fallback: last_motor
  to_fallback: first_frame

常见选择：

• last_motor：旧状态采样失败时保持上一帧，适合保守退出。
• first_frame：新状态采样失败时使用第一帧，适合模型还没准备好时兜底。
• none：没有退路。另一侧有帧就直接使用另一侧；两侧都没有帧则保持上一帧。

9. 调试常见问题

看不到混合效果：

• 确认 transition 实际使用的是 dual_running_blend。
• 确认状态实现了 get_motor_frame() 或 get_transition_frame()。
• 确认 duration 不是 0.0。
• 确认状态机信息里 mode 进入了 transition。

过渡结束瞬间动作跳回开头：

• 检查目标状态是否绕过了 RobotControlState。
• 检查是否自己重写了 on_transition_commit() 并重新调用了 on_enter()。

采样不到某一侧输出：

• get_motor_frame() 是否返回了 None。
• 是否需要给状态单独实现 get_transition_frame()。
• fallback 是否把它退化成了 last_motor 或 first_frame。

下一课：手把手 6：从零写一个 UDP 自定义遥控器驱动。