Hands On Custom State

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

本课目标：写一个真正能运行的 SinWaveState。它会让一个关节按 sin() 摆动。

这一课刻意降低门槛：不新建文件，不改 setup.py，不拆 Python 子包。你只需要改两个文件：

src/bxi_example_py_elf3/bxi_example_py_elf3/robot_states.py
src/bxi_example_py_elf3/config/elf3_state_machine.yaml

等你已经能稳定添加状态，再考虑把复杂状态拆到单独文件；入门阶段直接写在 robot_states.py 里最清楚。

0. 你最终会得到什么

新增状态类：

SinWaveState

新增状态名：

sin_wave

新增 YAML 配置：

  sin_wave:
    behavior: SinWaveState
    params:
      joint: 22
      amplitude: 0.4
      frequency: 1.0

1. 先理解状态类放在哪里

打开：

src/bxi_example_py_elf3/bxi_example_py_elf3/robot_states.py

这个文件里已经有很多状态，例如：

class NormalState(RobotControlState):
    ...


class ZeroTorqueState(RobotControlState):
    ...

本课的新状态就直接加在这个文件里。推荐先放在 InitialPosState 后面、DanceState 前面，这样简单状态和模型状态分开一些。

你不需要手动写 id，也不需要手动注册类。utils/robot_state_builder.py 里的 build_robot_states() 会从当前已加载的 RobotControlState 子类里找到 SinWaveState。

2. 添加最小 SinWaveState

在 robot_states.py 里找到：

class InitialPosState(RobotControlState):
    ...

在它后面添加：

class SinWaveState(RobotControlState):
    def __init__(
        self,
        name: str,
        state_id: int,
        joint: int = 22,
        amplitude: float = 0.4,
        frequency: float = 1.0,
    ):
        super().__init__(name, state_id)
        self.joint = joint
        self.amplitude = amplitude
        self.frequency = frequency
        self.elapsed = 0.0
        self.base_qpos: Optional[np.ndarray] = None

    def on_enter(self, ctx: BxiExample) -> None:
        self.reset_loop(ctx)
        self.elapsed = 0.0
        self.base_qpos = ctx.joint_nominal_pos.copy()

    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)

    def get_motor_frame(self, ctx: BxiExample, dt: float) -> Optional[MotorFrame]:
        if self.base_qpos is None:
            self.base_qpos = ctx.joint_nominal_pos.copy()

        self.elapsed += dt
        qpos = self.base_qpos.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)

这段代码不需要新增 import。robot_states.py 顶部已经有本例需要的：

import math
from typing import Optional

import numpy as np

from bxi_example_py_elf3.utils.robot_state_base import MotorFrame, RobotControlState

这个状态的运行逻辑是：

进入状态 on_enter
  -> elapsed 清零
  -> 记录一份基础姿态 base_qpos

每个控制周期 get_motor_frame
  -> elapsed += dt
  -> 在基础姿态上叠加 sin 摆动
  -> 返回 (qpos, kp, kd)

注意：get_motor_frame() 只负责算这一帧电机目标，不要在里面直接发布消息。

3. 在 YAML 里声明状态

打开：

src/bxi_example_py_elf3/config/elf3_state_machine.yaml

找到：

states:

在 states: 下面添加：

  sin_wave:
    behavior: SinWaveState
    params:
      joint: 22
      amplitude: 0.4
      frequency: 1.0
    transitions:
      on_event:
        zero_torque: zero_torque
        normal:
          to: normal
          transition: soft_switch

字段含义：

• sin_wave：状态机里的状态名，遥控器和其他状态切换时使用这个名字。
• behavior: SinWaveState：使用刚刚写的 Python 类。
• params：传给 SinWaveState.__init__() 的参数。
• transitions.on_event.zero_torque：收到 zero_torque 事件时切到 zero_torque 状态。
• transitions.on_event.normal：收到 normal 事件时切回 normal，并使用 soft_switch 过渡。

name 和 state_id 不写进 params，框架会自动传进去。

4. 第一次验证：临时设为初始状态

第一次写状态时，先不要急着接遥控器。最简单的验证方式是临时把它设成初始状态。

在 elf3_state_machine.yaml 顶部找到：

initial_state: zero_torque

临时改成：

initial_state: sin_wave

这样启动后会直接进入 sin_wave。

验证完成后，记得改回：

initial_state: zero_torque

5. 编译并在仿真里启动

编译：

colcon build --symlink-install --packages-select bxi_example_py_elf3

加载环境：

source install/setup.bash

启动仿真 demo：

ros2 launch bxi_example_py_elf3 example_demo.launch.py

查看状态机信息：

ros2 topic echo /simulation/state_machine_info

你应该能看到当前状态类似：

{
  "current": {
    "name": "sin_wave"
  }
}

如果状态没进去，先不要继续加功能。优先检查：

1. Python 类名是不是 SinWaveState。
2. YAML 里的 behavior 是不是 SinWaveState。
3. YAML 缩进是不是还在 states: 下面。
4. 是否重新 colcon build 并 source install/setup.bash。

6. 用 YAML 调动作参数

现在你已经有一个可运行状态了。接下来先不要改代码，只改 YAML：

  sin_wave:
    behavior: SinWaveState
    params:
      joint: 24
      amplitude: 0.25
      frequency: 1.5

这说明一件事：状态代码只定义能力，具体动作参数交给配置。

参数规则：

• params.joint 对应 __init__(..., joint=...)。
• params.amplitude 对应 __init__(..., amplitude=...)。
• params.frequency 对应 __init__(..., frequency=...)。
• 状态运行时私有变量不要写进 YAML，例如 elapsed 和 base_qpos 应该留在状态对象内部。

7. 添加固定时间自动返回

如果只是固定时间后切走，不需要改 Python。直接在 YAML 里加 after：

  sin_wave:
    behavior: SinWaveState
    params:
      joint: 22
      amplitude: 0.4
      frequency: 1.0
    transitions:
      on_event:
        zero_torque: zero_torque
        normal:
          to: normal
          transition: soft_switch
      after:
        - seconds: 3.0
          to: normal
          transition: soft_switch

含义：

进入 sin_wave
  -> 运行 3 秒
  -> 自动切到 normal

固定时间的状态图关系，优先写 YAML，不要硬编码在状态类里。

8. 添加代码内主动退出

有些退出条件不能只靠 YAML，例如：

• 动作文件播放结束。
• 模型输出满足条件。
• 传感器触发。
• 姿态不安全。

这种情况才需要在状态类里写 on_update()。

先给 SinWaveState.__init__() 加一个 duration 参数：

def __init__(
    self,
    name: str,
    state_id: int,
    joint: int = 22,
    amplitude: float = 0.4,
    frequency: float = 1.0,
    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.base_qpos: Optional[np.ndarray] = None

再添加 on_update()：

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

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

YAML 里就可以写：

params:
  joint: 22
  amplitude: 0.4
  frequency: 1.0
  duration: 3.0

什么时候用 YAML after：

• 固定时间自动退出。
• 不依赖传感器、不依赖模型内部状态。

什么时候用代码主动退出：

• 退出条件来自 Python 内部变量、模型、传感器或安全判断。

9. 添加暂停 action

现在给状态加一个不切状态的动作：暂停/继续摆动。

在 __init__() 里加：

self.playing = True

在 on_enter() 里加：

self.playing = True

修改 get_motor_frame() 里的时间更新：

if self.playing:
    self.elapsed += dt

添加：

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

YAML 里可以把某个事件绑定成 action：

  sin_wave:
    behavior: SinWaveState
    transitions:
      on_event:
        toggle_dance_pause:
          action: toggle_sin_pause

这里的关键区别是：

• to：切到另一个状态。
• action：不切状态，只让当前状态执行一段内部逻辑。

10. 添加安全退出

会动的状态都建议加安全判断。把 on_update() 改成：

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

    frame = self.get_motor_frame(ctx, dt)
    if frame is not None:
        ctx.set_motor_target(*frame)

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

推荐顺序：

安全检查
  -> 计算并输出电机目标
  -> 判断是否结束

11. 最终版 SinWaveState

最终可以整理成下面这样，仍然直接放在 robot_states.py 里：

class SinWaveState(RobotControlState):
    def __init__(
        self,
        name: str,
        state_id: int,
        joint: int = 22,
        amplitude: float = 0.4,
        frequency: float = 1.0,
        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
        self.base_qpos: Optional[np.ndarray] = None

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

    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)

    def get_motor_frame(self, ctx: BxiExample, dt: float) -> Optional[MotorFrame]:
        if self.base_qpos is None:
            self.base_qpos = ctx.joint_nominal_pos.copy()

        if self.playing:
            self.elapsed += dt

        qpos = self.base_qpos.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:
        if ctx.is_orientation_unsafe(ctx.current_quat_xyzw):
            ctx.request_state("zero_torque", trigger="safety")
            return

        frame = self.get_motor_frame(ctx, dt)
        if frame is not None:
            ctx.set_motor_target(*frame)

        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

12. 最终 YAML 示例

  sin_wave:
    behavior: SinWaveState
    params:
      joint: 22
      amplitude: 0.4
      frequency: 1.0
      duration: 3.0
    transitions:
      on_event:
        zero_torque: zero_torque
        normal:
          to: normal
          transition: soft_switch
        toggle_dance_pause:
          action: toggle_sin_pause
      after:
        - seconds: 3.0
          to: normal
          transition: soft_switch

注意：这里同时写了 duration 和 after 只是为了展示两种写法。实际项目里二选一即可：

• 想纯配置控制固定时间退出，用 after。
• 想在代码里根据内部条件退出，用 duration + on_update()。

13. 可选：让状态订阅自己的话题

如果某个状态需要额外传感器或外部命令，不要在 __init__() 里调用 ctx.create_subscription()。构造函数只保存参数和私有变量；ROS 资源放到 on_bind(ctx)。

示例：状态订阅一个外部幅值话题，并用它覆盖 sin() 幅值。

先在 __init__() 里加缓存变量：

self.external_amplitude = None
self.amplitude_sub = None

再在类里加：

def on_bind(self, ctx: BxiExample) -> None:
    self.amplitude_sub = ctx.create_subscription(
        Float32,
        "sin_wave_amplitude",
        self._amplitude_callback,
        10,
    )

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

然后在 get_motor_frame() 里使用：

amplitude = self.amplitude
if self.external_amplitude is not None:
    amplitude = self.external_amplitude

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

还需要在 robot_states.py 顶部补一个 import：

from std_msgs.msg import Float32

on_bind(ctx) 会在所有状态对象创建后调用一次，早于状态机进入初始状态。订阅 callback 通常只缓存数据，控制输出仍放在 get_motor_frame() 或 on_update()。

14. 本课检查清单

1. SinWaveState 直接写在 robot_states.py 里。
2. 没有新建 states/ 文件夹。
3. 没有修改 setup.py。
4. elf3_state_machine.yaml 里 states.sin_wave.behavior 是 SinWaveState。
5. initial_state 临时设成 sin_wave 后，仿真能进入该状态。
6. /simulation/state_machine_info 能看到 current.name = sin_wave。
7. 验证完成后，把 initial_state 改回 zero_torque。
8. 如果状态要订阅自己的话题，用 on_bind(ctx)，不要写在 __init__()。

下一课：手把手 2：从零添加模型动作状态。