Hands On Custom Transition

手把手 4：从零写一个自定义过渡行为

本课目标：从零写一个新的进入过渡行为：

first_frame_blend_pose_gain

它做三件事：

1. 读取目标状态的第一帧。
2. 让 qpos 从当前姿态平滑插值到第一帧。
3. 让 kp/kd 按进度从起始值过渡到目标值。

这比当前 first_frame_ramp_kp 更进一步：不仅 kp/kd 渐变，目标角度也渐变。

0. 你最终会改什么

修改：

src/bxi_example_py_elf3/bxi_example_py_elf3/utils/robot_state_base.py
src/bxi_example_py_elf3/config/elf3_state_machine.yaml

不改：

src/bxi_example_py_elf3/bxi_example_py_elf3/utils/state_machine.py

原因：

• utils/state_machine.py 是通用状态机。
• 机器人电机过渡属于机器人层，应该放 utils/robot_state_base.py。
• 行为参数放 transition.data，不要把专用字段硬编码进 TransitionProfile。

1. 先理解过渡生命周期

切状态时调用顺序：

旧状态.on_exit(ctx)
新状态.on_prepare_enter(ctx, from_state, transition)

过渡期间每个周期：
  旧状态.on_exit_transition(ctx, to_state, exit_progress, transition)
  新状态.on_enter_transition(ctx, from_state, enter_progress, transition)

过渡结束：
  新状态.on_transition_commit(ctx, from_state, transition)

进入侧在退出侧后面调用。如果两边都 ctx.set_motor_target()，进入侧会覆盖退出侧。

所以我们把新行为写在：

RobotControlState.on_enter_transition()

2. 先用现有 first_frame_switch 验证目标状态第一帧

在写新行为前，先确认目标状态实现了：

get_first_frame(ctx)

例如 SinWaveState：

def get_first_frame(self, ctx: BxiExample) -> Optional[MotorFrame]:
    qpos = ctx.joint_nominal_pos.copy()
    return self._motor_frame(qpos, ctx.joint_kp, ctx.joint_kd)

YAML 先用已有 profile：

transition_profiles:
  first_frame_switch:
    exit_duration: 0.02
    enter_duration: 0.1
    exit_behavior: hold_last_motor
    enter_behavior: first_frame_ramp_kp
    data:
      kp_start: zero
      kd_start: target

如果这个都不工作，先修 get_first_frame()，不要急着写新行为。

3. 在 on_enter_transition 中注册新 behavior

打开：

src/bxi_example_py_elf3/bxi_example_py_elf3/utils/robot_state_base.py

找到：

def on_enter_transition(
    self,
    ctx: BxiExample,
    from_state: StateBehavior[BxiExample],
    progress: float,
    transition: TransitionProfile,
) -> None:
    if transition.enter_behavior == "hold_last_motor":
        ctx.hold_last_motor_target()
    elif transition.enter_behavior == "first_frame_ramp_kp":
        self._enter_first_frame_ramp_kp(ctx, progress, transition)

改成：

def on_enter_transition(
    self,
    ctx: BxiExample,
    from_state: StateBehavior[BxiExample],
    progress: float,
    transition: TransitionProfile,
) -> None:
    if transition.enter_behavior == "hold_last_motor":
        ctx.hold_last_motor_target()
    elif transition.enter_behavior == "first_frame_ramp_kp":
        self._enter_first_frame_ramp_kp(ctx, progress, transition)
    elif transition.enter_behavior == "first_frame_blend_pose_gain":
        self._enter_first_frame_blend_pose_gain(ctx, progress, transition)

现在状态基类知道有这个 behavior 名了。

4. 添加曲线函数

在 RobotControlState 类中添加：

def _transition_alpha(self, progress: float, transition: TransitionProfile) -> float:
    alpha = min(max(float(progress), 0.0), 1.0)
    curve = str(transition.data.get("curve", "smoothstep"))
    if curve == "linear":
        return alpha
    if curve == "smoothstep":
        return alpha * alpha * (3.0 - 2.0 * alpha)
    raise ValueError(f"unsupported transition curve: {curve}")

支持：

data:
  curve: linear

或：

data:
  curve: smoothstep

5. 添加 qpos 起点选择

继续在 RobotControlState 中添加：

def _qpos_start(
    self,
    ctx: BxiExample,
    mode: str,
    target: np.ndarray,
) -> np.ndarray:
    if mode == "target":
        return target.copy()
    if mode == "nominal":
        return np.asarray(ctx.joint_nominal_pos, dtype=np.float32).copy()
    if mode == "last_state":
        return np.asarray(ctx.pos_last_state, dtype=np.float32).copy()
    if mode == "current":
        return np.asarray(ctx.current_q, dtype=np.float32).copy()
    raise ValueError(f"unsupported transition qpos start mode: {mode}")

支持模式：

• current：从当前真实关节位置开始。
• last_state：从旧状态退出时缓存的位置开始。
• nominal：从默认站立姿态开始。
• target：不插值位置，直接使用目标第一帧。

推荐默认用 current。

6. 添加核心行为函数

继续在 RobotControlState 中添加：

def _enter_first_frame_blend_pose_gain(
    self,
    ctx: BxiExample,
    progress: float,
    transition: TransitionProfile,
) -> None:
    if self._prepared_first_frame is None:
        first_frame = self.get_first_frame(ctx)
        if first_frame is None:
            ctx.hold_last_motor_target()
            return
        self._prepared_first_frame = self._motor_frame(*first_frame)

    qpos_target, kp_target, kd_target = self._prepared_first_frame
    alpha = self._transition_alpha(progress, transition)

    qpos_start_mode = str(transition.data.get("qpos_start", "current"))
    qpos_start = self._qpos_start(ctx, qpos_start_mode, qpos_target)
    if qpos_start.shape != qpos_target.shape:
        raise ValueError(
            f"qpos start shape {qpos_start.shape} does not match target shape {qpos_target.shape}"
        )

    kp_start_mode = str(transition.data.get("kp_start", "zero"))
    kd_start_mode = str(transition.data.get("kd_start", "target"))
    kp_start = self._gain_start(kp_start_mode, kp_target, ctx.kp_last)
    kd_start = self._gain_start(kd_start_mode, kd_target, ctx.kd_last)

    qpos = qpos_start + (qpos_target - qpos_start) * alpha
    kp = kp_start + (kp_target - kp_start) * alpha
    kd = kd_start + (kd_target - kd_start) * alpha
    ctx.set_motor_target(
        qpos.astype(np.float32),
        kp.astype(np.float32),
        kd.astype(np.float32),
    )

这个函数没有改 utils/state_machine.py。它只使用：

• transition.enter_behavior
• transition.data
• 目标状态的 get_first_frame()
• ctx.set_motor_target()

7. 在 YAML 中定义 profile

打开：

src/bxi_example_py_elf3/config/elf3_state_machine.yaml

在 transition_profiles: 下添加：

  pose_blend_switch:
    exit_duration: 0.02
    enter_duration: 0.4
    exit_behavior: hold_last_motor
    enter_behavior: first_frame_blend_pose_gain
    data:
      qpos_start: current
      kp_start: zero
      kd_start: target
      curve: smoothstep

字段含义：

• enter_behavior：刚刚写的新行为。
• qpos_start：位置插值起点。
• kp_start：kp 起点。
• kd_start：kd 起点。
• curve：插值曲线。

8. 使用新过渡

让 normal -> sin_wave 使用新 profile：

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

现在切换时：

当前姿态
  -> 平滑插值到 SinWaveState.get_first_frame()
  -> kp 从 0 增加到目标 kp
  -> kd 从目标 kd 开始

9. 单次切换覆盖 enter_duration

如果大部分状态使用 0.4 秒，但某一次想 1 秒：

sin_wave:
  to: sin_wave
  transition:
    name: slow_sin_wave_entry
    base: pose_blend_switch
    enter_duration: 1.0

不用新建全局 profile。

10. 单次切换覆盖 data

例如这次不想从 current 插值，而是从 last_state：

transition:
  name: sin_wave_from_last_state
  base: pose_blend_switch
  data:
    qpos_start: last_state
    curve: linear

data 会和 base profile 的 data 合并，覆盖同名字段。

11. 状态专属过渡行为

如果某个过渡只服务 SinWaveState，不要写到 utils/robot_state_base.py，直接写状态类：

from bxi_example_py_elf3.utils.state_machine import StateBehavior, TransitionProfile


def on_enter_transition(
    self,
    ctx: BxiExample,
    from_state: StateBehavior[BxiExample],
    progress: float,
    transition: TransitionProfile,
) -> None:
    if transition.enter_behavior != "sin_wave_private_entry":
        super().on_enter_transition(ctx, from_state, progress, transition)
        return

    alpha = min(max(float(progress), 0.0), 1.0)
    qpos = ctx.joint_nominal_pos.copy()
    qpos[self.joint] += self.amplitude * alpha
    kp = ctx.joint_kp * alpha
    ctx.set_motor_target(qpos, kp, ctx.joint_kd)

YAML：

transition_profiles:
  sin_wave_private_entry:
    enter_duration: 0.3
    enter_behavior: sin_wave_private_entry

判断标准：

• 多个状态复用：写 utils/robot_state_base.py。
• 只有一个状态使用：写状态类。

12. 调试过渡

启动后观察：

ros2 topic echo /simulation/state_machine_info

切换中应该看到：

{
  "mode": "transition",
  "transition": {
    "from": {"name": "normal"},
    "to": {"name": "sin_wave"},
    "profile": "pose_blend_switch",
    "enter_behavior": "first_frame_blend_pose_gain",
    "enter_progress": 0.5,
    "data": {
      "qpos_start": "current",
      "kp_start": "zero",
      "curve": "smoothstep"
    }
  }
}

如果没有看到：

• 检查 transition profile 名是否写对。
• 检查当前状态是否真的走了那条 transition。
• 检查 enter_behavior 拼写是否和代码一致。

13. 本课最终能力上限

读完本课后，你应该能写出三种层级的过渡：

1. 直接使用已有 profile
2. 在 YAML inline 覆盖单次 transition
3. 在 utils/robot_state_base.py 或状态类中新增 enter_behavior / exit_behavior

你也应该理解为什么：

transition.data 是行为私有参数
utils/state_machine.py 不应该硬编码 enter_kp_start 这类字段

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