Custom State

手把手添加自定义状态

本页从一个最简单的 sin() 摆动动作开始，完整演示如何新增一个状态类、注册到状态机、绑定按键、添加参数、私有变量、第一帧、自动退出和 action。

示例状态名：

sin_wave

示例 Python 类：

SinWaveState

1. 新状态应该放在哪里

当前已有状态在：

src/bxi_example_py_elf3/bxi_example_py_elf3/robot_states.py

入门和普通扩展推荐直接把新状态类写在 robot_states.py 里。这样不用新建 Python 子包，也不用改 setup.py，门槛最低。

状态很多以后可以再拆文件，但拆文件时必须确保 robot_states.py 导入了对应类，否则 YAML 里的 behavior 会找不到。

2. 写最小状态类

打开：

src/bxi_example_py_elf3/bxi_example_py_elf3/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,
    ):
        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)

如果当前 robot_states.py 顶部缺少这些名字，再补对应 import：

import math
from typing import Optional

import numpy as np

重点：

• ctx: BxiExample 明确写出来，方便代码提示和类型检查。
• utils/state_machine.py 仍然保持泛型，不依赖 BxiExample。
• elapsed、base_qpos、joint、amplitude、frequency 是状态私有变量，放在 self 上。
• get_first_frame() 给进入过渡使用。
• get_motor_frame() 是推荐的电机目标计算接口，返回 (qpos, kp, kd)。
• 没有特殊安全判断时，可以只实现 get_motor_frame()，让 RobotControlState.on_update() 自动调用 ctx.set_motor_target()。
• get_transition_frame() 给 dual_running_blend 这种双状态运行混合过渡使用；默认会复用 get_motor_frame()。

3. 自动发现和 on_bind

utils/robot_state_builder.py 里的 build_robot_states() 会从当前已加载的 RobotControlState 子类里找行为类。直接写在 robot_states.py 里时，不需要手动注册，也不需要手动写 id。

如果状态需要创建自己的 ROS 订阅、client 或 timer，实现：

def on_bind(self, ctx: BxiExample) -> None:
    self.my_sub = ctx.create_subscription(
        Float32,
        "some_topic",
        self._my_callback,
        10,
    )

def _my_callback(self, msg: Float32) -> None:
    self.latest_value = float(msg.data)

on_bind(ctx) 只会在状态对象创建后调用一次，并且早于状态机进入初始状态。建议：

• __init__() 只保存 YAML 参数和状态私有变量。
• on_bind(ctx) 创建 ROS 资源。
• callback 里只缓存数据，真正控制逻辑仍放在 on_update() 或 get_motor_frame()。
• 如果 callback 和控制周期共享可变数据，自己加锁或只做原子替换。

上面的例子需要在 robot_states.py 顶部添加：

from std_msgs.msg import Float32

4. 在状态机 YAML 注册状态

打开：

src/bxi_example_py_elf3/config/elf3_state_machine.yaml

在 states: 下添加：

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

字段含义：

• sin_wave：状态名。
• behavior: SinWaveState：Python 类名。
• params：传给 SinWaveState.__init__() 的参数。
• normal event：切回 normal。
• zero_torque: zero_torque：简写，收到 zero_torque event 后切到 zero_torque 状态。

不要手写 id，除非外部系统必须依赖固定状态编号。当前框架会自动分配状态 id。

5. 给入口状态添加转移

假设从 normal 切到 sin_wave。

在 states.normal.transitions.on_event 里添加：

        sin_wave:
          to: sin_wave
          transition: soft_switch

状态图现在是：

normal --sin_wave--> sin_wave
sin_wave --normal--> normal

6. 声明 remote event

状态机只认识 event，不直接认识遥控器按键。

在 remote_events: 下添加：

  sin_wave:
    slot: btn_10
    value: 5

含义：

MotionCommands.btn_10 从其他值跳变到 5
  -> 产生 sin_wave event

7. 绑定键盘按键

打开：

src/remote_controller/config/xbox_default.yaml

在 sources.keyboard.signals 添加：

      keyboard.sin_wave: {from: keyboard.key, key: "0"}

在 controls 添加：

  keyboard.sin_wave: {type: bool, source: keyboard.sin_wave}

在 outputs.edge 添加：

    - output: btn_10=5
      when: [keyboard.sin_wave]

为什么用 edge：

• 状态切换是一次性事件。
• edge 条件满足时输出一帧，下一帧自动回 0。
• RemoteEventAdapter 看到 btn_10 跳变到 5，就触发一次 sin_wave event。

8. 同时绑定手柄组合键

如果希望键盘 0 或手柄组合键都能触发：

outputs:
  edge:
    - output: btn_10=5
      when:
        any:
          - [trigger.right, button.west]
          - [keyboard.sin_wave]

如果这个组合键会在多处复用，可以先定义派生 control：

controls:
  command.sin_wave:
    type: bool
    expr:
      any:
        - [trigger.right, button.west]
        - [keyboard.sin_wave]

然后：

outputs:
  edge:
    - output: btn_10=5
      when: [command.sin_wave]

9. 用 YAML 调状态参数

SinWaveState.__init__() 支持：

joint: int = 22
amplitude: float = 0.4
frequency: float = 1.0

YAML 里可以直接改：

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

规则：

• name 和 state_id 由框架传入，不写进 params。
• params 字段必须和构造函数关键字参数匹配。
• 状态运行时私有变量放在 self，不要放到 ctx。

10. 添加自动返回

如果希望状态运行 3 秒后自动回到 normal，推荐写 YAML：

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

这样状态类只负责动作本身，生命周期策略交给配置。

11. 在代码里主动切状态

如果结束条件依赖运行时，例如动作文件播放完、姿态不安全、模型状态达到某个阈值，可以在状态类里主动请求切换：

ctx.request_state(
    "normal",
    trigger="sin_wave_finished",
    transition="soft_switch",
)

也可以使用 inline transition：

ctx.request_state(
    "normal",
    trigger="sin_wave_finished",
    transition={"base": "first_frame_switch", "enter_duration": 0.1},
)

原则：

• 固定状态图写 YAML。
• 运行时判断写状态代码。

12. 定义第一帧

get_first_frame() 用于进入过渡。它返回：

(qpos, kp, kd)

示例：

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

配合：

transition: first_frame_switch

或：

transition:
  base: first_frame_switch
  enter_duration: 0.3

进入状态前会先以新状态第一帧作为目标，并逐步增加 kp/kd。

13. 给状态添加 action

action 适合“不切状态，只改变当前状态内部行为”。

例如给 SinWaveState 加暂停：

class SinWaveState(RobotControlState):
    def __init__(...):
        super().__init__(name, state_id)
        self.playing = True

    def on_update(self, ctx: BxiExample, dt: float) -> None:
        if self.playing:
            self.elapsed += dt
        # 后面继续用 self.elapsed 计算 qpos

    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：

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

返回值：

• True：action 已处理。
• False：状态机继续找全局 action handler。
• 都找不到时会报错，避免静默失效。

14. 添加安全退出

很多动作需要姿态保护：

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

    # 正常动作逻辑

高危动作更应该在状态内部保留安全退出，而不是只依赖外部按键。

15. 使用速度输入和 get_cmd_vel

如果状态需要遥控速度，不要直接读 MotionCommands，也不要直接读 ctx.raw_cmd_vel。在状态类里调用：

cmd_vel = self.get_cmd_vel(ctx)

默认 RobotControlState.get_cmd_vel(ctx) 会做这些事：

读取 ctx.current_raw_cmd_vel
  -> 查询当前状态 YAML 里的 speed_profile
  -> 从 speed_profiles 取缩放和限幅
  -> 调用 process_cmd_vel(ctx, cmd_vel) 做状态私有处理
  -> 返回处理后的 [vx, vy, yaw]
  -> 写入 ctx.current_cmd_vel

状态 YAML 示例：

states:
  normal:
    behavior: NormalState
    speed_profile: normal

代码示例：

def get_motor_frame(self, ctx: BxiExample, dt: float) -> Optional[MotorFrame]:
    cmd_vel = self.get_cmd_vel(ctx)
    qpos, _ = ctx.normal.inference_step(
        ctx.current_q,
        ctx.current_dq,
        ctx.current_quat_wxyz,
        ctx.current_omega,
        cmd_vel,
    )
    return self._motor_frame(qpos, ctx.normal.kps, ctx.normal.kds)

规则：

• 状态没有 speed_profile 时，get_cmd_vel(ctx) 返回零速度。
• speed_profile 写错时会 warning 一次，并返回零速度。
• 状态不调用 get_cmd_vel(ctx) 时，速度配置不会影响这个状态。
• 需要特殊处理时重写 process_cmd_vel(ctx, cmd_vel)，不要直接写 ctx.current_cmd_vel。

重写示例：只允许前后速度，禁止横移：

def process_cmd_vel(
    self,
    ctx: BxiExample,
    cmd_vel: np.ndarray,
) -> Optional[np.ndarray]:
    cmd_vel[1] = 0.0
    return cmd_vel

get_cmd_vel(ctx) 会把 process_cmd_vel() 的返回值统一写入 ctx.current_cmd_vel。如果你在 process_cmd_vel() 里直接原地修改 cmd_vel，也可以返回 None。

16. 使用模型状态作为动作

如果状态要调用模型，建议模式参考已有状态：

• 在 on_prepare_enter() 里预热模型。
• 在 on_enter() 里重置 timestep、私有变量。
• 在 get_first_frame() 里返回模型第一帧目标。
• 在 get_motor_frame() 里调用模型推理并返回 (qpos, kp, kd)。

结构：

def on_prepare_enter(self, ctx: BxiExample, from_state, transition) -> None:
    super().on_prepare_enter(ctx, from_state, transition)
    ctx.preheat_model(
        ctx.my_policy,
        with_cmd_vel=True,
        cmd_vel=self.get_cmd_vel(ctx),
    )

def on_enter(self, ctx: BxiExample) -> None:
    self.reset_loop(ctx)
    ctx.my_policy.timestep = ctx.my_policy.start_frame

def get_first_frame(self, ctx: BxiExample) -> Optional[MotorFrame]:
    return self._motor_frame(
        ctx.my_policy.target_dof_pos,
        ctx.my_policy.kps,
        ctx.my_policy.kds,
    )

def get_motor_frame(self, ctx: BxiExample, dt: float) -> Optional[MotorFrame]:
    cmd_vel = self.get_cmd_vel(ctx)
    qpos = ctx.my_policy.inference_step(..., cmd_vel)
    return self._motor_frame(qpos, ctx.my_policy.kps, ctx.my_policy.kds)

如果需要新增模型对象，修改 bxi_example_demo.py 的模型加载，并确认 launch 里有对应模型 key。

17. 编译和运行

编译：

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

加载环境：

source install/setup.bash

启动键盘遥控器：

ros2 launch remote_controller remote_controller_keyboard.launch.py

启动 example：

ros2 launch bxi_example_py_elf3 example_demo_hw.launch.py

观察状态机：

ros2 topic echo /simulation/state_machine_info

观察遥控器输出：

ros2 topic echo /motion_commands

18. 新状态 checklist

1. 新建状态类，继承 RobotControlState。
2. 状态私有变量放 self。
3. ctx 类型直接写 BxiExample。
4. 实现 on_enter / on_update，或实现 get_motor_frame 让基类默认输出。
5. 如果需要过渡，提供 get_first_frame。
6. 如果状态自己订阅话题，实现 on_bind(ctx)。
7. 如果状态需要速度输入，YAML 写 speed_profile，代码调用 self.get_cmd_vel(ctx)。
8. 在 elf3_state_machine.yaml 的 states 注册状态。
9. 给入口状态添加 transitions.on_event。
10. 在 remote_events 声明 event -> btn_N/value。
11. 在 xbox_default.yaml 绑定 source/control/output。
12. colcon build。
13. 观察 /motion_commands 和 /state_machine_info。