Custom State

自定义状态

机器人状态继承 RobotControlState。基类唯一强制实现的运行函数是 on_update()；过渡需要的额外函数通过可选 Protocol 表达，不会污染所有状态的基类接口。

状态通常写在：

src/bxi_example_py_elf3/bxi_example_py_elf3/robot_states.py

新增状态类不需要修改状态构建器注册表。

1. 最小状态

from __future__ import annotations

import math

import numpy as np

from bxi_example_py_elf3.utils.robot_state_base import RobotControlState


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

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

    def on_update(self, ctx: BxiExample, dt: float) -> None:
        self.elapsed += dt
        qpos = ctx.joint_nominal_pos.copy()
        qpos[self.joint] += self.amplitude * math.sin(
            2.0 * math.pi * self.frequency * self.elapsed
        )
        ctx.set_motor_target(qpos, ctx.joint_kp, ctx.joint_kd)

on_update() 必须明确输出电机目标，或者明确请求切换到其他状态。

2. 注册到 YAML

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

• behavior 必须与 Python 类名一致。
• params 作为关键字参数传入构造函数。
• id 不写时自动分配。
• manifest 只用于外部展示。

状态类由 build_robot_states() 从 RobotControlState 子类中自动发现。

3. 生命周期

on_bind

def on_bind(self, ctx: BxiExample) -> None:
    ...

状态对象创建后调用一次，适合创建订阅、client 或其他依赖 ROS node 的资源。不要在构造函数中访问 ctx。

on_prepare

def on_prepare(
    self,
    ctx: BxiExample,
    from_state: StateBehavior[BxiExample],
) -> None:
    ctx.preheat_model(ctx.my_policy)

过渡 Session 创建前调用。适合预热模型和准备目标状态资源，但不要在这里写电机目标。

on_prepare_cancel

def on_prepare_cancel(
    self,
    ctx: BxiExample,
    from_state: StateBehavior[BxiExample],
) -> None:
    ...

目标状态准备后，如果过渡被 event、延迟请求或异常中断，就会调用它释放本次准备资源。

on_enter

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

过渡完成、状态正式成为 current 后调用。

on_update

def on_update(self, ctx: BxiExample, dt: float) -> None:
    ...

唯一必需实现的状态接口。只有不在活动过渡中时，状态机才调用 current 状态的 on_update()。

on_exit

def on_exit(self, ctx: BxiExample) -> None:
    super().on_exit(ctx)

过渡 Session 完成后、current 切换前调用。默认实现保存上一状态的电机信息。

4. 支持进入帧过渡

只有会被 entry_gain_ramp 或需要目标进入帧的过渡使用的状态，才实现 EntryFrameProvider：

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


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

    def on_update(self, ctx: BxiExample, dt: float) -> None:
        ...

不使用这种过渡的状态不需要实现该方法。

状态图加载时会提前验证目标状态能力；错误会明确指出哪个状态缺少能力，而不是运行到一半才失败。

5. 支持运行帧混合

需要被 running_blend 动态采样的状态实现 RunningFrameProvider：

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


class SinWaveState(RobotControlState, RunningFrameProvider):
    def sample_running_frame(
        self,
        ctx: BxiExample,
        dt: float,
        *,
        advance: bool,
    ) -> MotorFrame | None:
        next_elapsed = self.elapsed + dt if advance else self.elapsed
        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:
        self._apply_frame(
            ctx,
            self.sample_running_frame(ctx, dt, advance=True),
        )

约束：

• advance=False 时不得推进播放帧、时间或 policy history。
• 只计算并返回 MotorFrame，不要在采样函数内调用 set_motor_target()。
• 暂时无法提供帧时可以返回 None。
• 正常运行和过渡采样应尽量复用同一份帧计算逻辑。

一个状态可以同时实现两个能力：

class SinWaveState(
    RobotControlState,
    EntryFrameProvider,
    RunningFrameProvider,
):
    ...

6. MotorFrame

frame = MotorFrame.create(qpos, kp, kd)

MotorFrame 会：

• 转换成独立拥有的 float32 NumPy 数组。
• 检查 qpos/kp/kd shape 一致。
• 暴露强类型的 qpos、kp、kd。

状态基类提供两个便利方法：

frame = self._motor_frame(qpos, kp, kd)
self._apply_frame(ctx, frame)

7. 速度 profile

YAML：

states:
  normal:
    behavior: NormalState
    speed_profile: normal

状态代码：

cmd_vel = self.get_cmd_vel(ctx)

可以重写 profile 之后的处理：

def process_cmd_vel(
    self,
    ctx: BxiExample,
    cmd_vel: NDArray[np.float32],
) -> NDArray[np.float32] | None:
    result = cmd_vel.copy()
    result[1] = 0.0
    return result

8. 状态主动切换

安全条件：

if ctx.is_orientation_unsafe(ctx.current_quat_xyzw):
    ctx.request_state("zero_torque", trigger="safety")
    return

动作完成并使用 profile：

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

使用内联插件配置：

ctx.request_state(
    "normal",
    trigger="motion_finished",
    transition={
        "profile": "dual_running_blend",
        "duration": 0.5,
        "sample_from": False,
    },
)

9. action

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

• 返回 True 表示已处理。
• 返回 False 时状态机会尝试全局 action handler。
• 两者都不存在时抛出配置错误。

10. 类型提示原则

• ctx 使用 BxiExample。
• 电机帧使用 MotorFrame，不要返回裸 tuple。
• 可选能力显式继承对应 Protocol。
• 配置边界使用 Mapping[str, object]，读取后立即验证和收窄。
• 不要为了某个过渡的特殊函数修改所有状态的基类。

11. 检查清单

1. 添加一个 RobotControlState 子类。
2. 实现 on_update()。
3. 在 YAML 中添加 behavior 和必要参数。
4. 需要进入帧时才实现 EntryFrameProvider。
5. 需要运行混合采样时才实现 RunningFrameProvider。
6. 在入口状态添加 event 边。
7. 保留到 zero_torque 等安全状态的路径。
8. 启动时检查状态图能力错误和不可达 warning。