Hands On Custom State

手把手 1：从零添加一个自定义状态

本课添加一个 SinWaveState，让单个关节围绕标称位置做正弦运动，并接入 YAML 状态图。

1. 添加状态类

编辑：

src/bxi_example_py_elf3/bxi_example_py_elf3/robot_states.py

class SinWaveState(RobotControlState, EntryFrameProvider, RunningFrameProvider):
    def __init__(
        self,
        name: str,
        state_id: int,
        joint: int = 22,
        amplitude: float = 0.3,
        frequency: float = 0.5,
        duration: float = 0.0,
    ):
        super().__init__(name, state_id)
        self.joint = joint
        self.amplitude = amplitude
        self.frequency = frequency
        self.duration = duration
        self.elapsed = 0.0
        self.playing = True

    def on_enter(self, ctx: BxiExample) -> None:
        self.elapsed = 0.0
        self.playing = True

    def get_entry_frame(self, ctx: BxiExample) -> MotorFrame:
        return self._motor_frame(
            ctx.joint_nominal_pos,
            ctx.joint_kp,
            ctx.joint_kd,
        )

    def sample_running_frame(
        self,
        ctx: BxiExample,
        dt: float,
        *,
        advance: bool,
    ) -> MotorFrame | None:
        next_elapsed = self.elapsed
        if self.playing and advance:
            next_elapsed += dt

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

        if advance:
            self.elapsed = next_elapsed
        return self._motor_frame(qpos, ctx.joint_kp, ctx.joint_kd)

    def on_update(self, ctx: BxiExample, dt: float) -> None:
        if ctx.is_orientation_unsafe(ctx.current_quat_xyzw):
            ctx.request_state("zero_torque", trigger="safety")
            return

        self._apply_frame(
            ctx,
            self.sample_running_frame(ctx, dt, advance=True),
        )

        if self.duration > 0.0 and self.elapsed >= self.duration:
            ctx.request_state(
                "normal",
                trigger="sin_wave_finished",
                transition="soft_switch",
            )

    def on_action(self, ctx: BxiExample, action_name: str) -> bool:
        if action_name != "toggle_sin_pause":
            return False
        self.playing = not self.playing
        return True

补充 import：

from bxi_example_py_elf3.utils.transition_core import (
    EntryFrameProvider,
    MotorFrame,
    RunningFrameProvider,
)

RobotControlState 只强制要求 on_update()。这里额外实现两个 Protocol，是为了让该状态能够参与进入增益渐变和双状态运行混合。

2. 理解 advance

sample_running_frame(..., advance=False) 只观察当前输出，不推进内部时间；advance=True 才提交时间变化。

正常 on_update
  -> advance=True
  -> elapsed 推进

过渡只采样
  -> advance=False
  -> elapsed 保持

这能避免一个控制周期内因为状态正常运行和过渡采样而重复推进动作。

3. 添加 YAML 状态

编辑：

src/bxi_example_py_elf3/config/elf3_state_machine.yaml

states:
  sin_wave:
    behavior: SinWaveState
    params:
      joint: 22
      amplitude: 0.3
      frequency: 0.5
      duration: 5.0
    manifest:
      label: 正弦测试
      index: 20
      group: Debug
      icon: waves
    transitions:
      on_event:
        normal_event:
          to: normal
          transition: soft_switch
        zero_torque_event: zero_torque
        toggle_dance_pause_event:
          action: toggle_sin_pause

无需给状态手动维护 id，也无需修改状态注册表。

4. 添加入口边

在 normal 的 event 边中增加：

sin_wave_event:
  to: sin_wave
  transition: first_frame_switch

并在 remote_events 中把一个 MotionCommands slot 映射成 sin_wave_event。

5. 第一次验证

colcon build --packages-select bxi_example_py_elf3 --symlink-install
source install/setup.bash

启动后检查：

• 图检查没有 unknown behavior。
• sin_wave 从初始状态可达。
• first_frame_switch 没有报告缺少 EntryFrameProvider。
• 进入后目标关节平滑摆动。
• 到达 duration 后返回 normal。
• 安全事件可以在进入过渡期间中断并进入零力矩状态。

6. 调整为只支持普通切换

如果状态永远不参与进入帧或运行混合，可以移除两个 Protocol 和对应方法，只保留 on_update()。这正是可选能力设计的目的。

7. 添加外部参数订阅

订阅应在 on_bind() 中创建：

def on_bind(self, ctx: BxiExample) -> None:
    self._subscription = ctx.create_subscription(
        Float32,
        "/sin_wave/amplitude",
        self._amplitude_callback,
        10,
    )

def _amplitude_callback(self, msg: Float32) -> None:
    self.amplitude = float(msg.data)

callback 只更新缓存；电机输出仍然集中在 on_update()。

8. 本课检查清单

1. 类直接添加到 robot_states.py。
2. on_update() 有明确输出或切换请求。
3. YAML 的 behavior 与类名一致。
4. 只实现实际需要的过渡能力。
5. advance=False 不改变运行状态。
6. 保留安全退出路径。

下一课：手把手 3：从零绑定按键并接入状态机。