Mod State Development Guide

Mod 状态开发进阶指南

本指南采用渐进式开发路线，从固定姿态状态逐步扩展到自定义输入 Driver。每一阶段都可以作为独立的最终实现。我们始终维护同一个状态：

com.example.wave/wave

第一次开发普通动作：读到第 3 节即可。 后续章节只在需要模型、共享资源、自定义过渡、ROS 实体或新输入协议时阅读。

如何选择阅读终点

需求	阅读到	主要抽象
固定关节目标	第 1 节	PoseState
公式轨迹、插值或周期动作	第 3 节	ProceduralState + dataclass
在线策略推理或动作回放	第 4 节	PolicyState / MotionReplayState
完整控制状态生命周期	第 5 节	RobotControlState
共享模型、数据或硬件对象	第 6 节	Resource
自定义状态切换算法	第 7 节	Transition
状态拥有 ROS 通信实体	第 8 节	on_bind/on_unbind
接入全新输入设备或协议	第 9 节	InputDriver

稳定契约

升级时，id、状态名、按键事件和 routes 都不需要改变；旧配置和遥控器绑定也不需要重写。只有当当前层确实不够用时，才进入下一层。

PoseState
  -> ProceduralState
  -> PolicyState / MotionReplayState
  -> RobotControlState
  -> 自定义 Resource
  -> 自定义 Transition
  -> 自定义 ROS 订阅、service、timer
  -> 自定义输入 Driver

0. 准备与验证闭环

本文命令默认从仓库根目录执行。源码工具不要求先构建：

cd ~/bxi_rl_controller_ros2_example
./tools/bxi-mod --help

用脚手架生成第一个 Mod：

./tools/bxi-mod new com.example.wave \
  --root ~/bxi_mods \
  --state wave \
  --template pose \
  --label 挥手示例 \
  --index 20

它只生成两个文件：

~/bxi_mods/com.example.wave/
  mod.yaml
  state.py

在 src/bxi_example_py_elf3/config/elf3_state_machine.yaml 追加搜索根：

mod_paths:
  - /home/你的用户名/bxi_mods

每改完一级先运行：

./tools/bxi-mod validate ~/bxi_mods/com.example.wave
./tools/bxi-mod inspect ~/bxi_mods/com.example.wave

要启动 ROS 节点时再构建工作区：

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

完成 source 后，安装目录的 bin/ 已加入 PATH，此时可以把 ./tools/bxi-mod 简写为 bxi-mod。新终端没有执行 source install/setup.bash 时，裸命令不可用。

validate 会真正加载 Mod 并构造状态；inspect 还会显示依赖顺序、完整状态名、最终 UI index 和 routes。重复 index 不会退出：先声明的状态保留原值，冲突状态会移动到下一个空闲 index，并打印告警。

脚手架已经生成基础依赖、事件和安全返回 routes。打开 mod.yaml，补充界面信息和安全确认后，可得到下面的完整配置：

id: com.example.wave
version: 1.0.0

requires:
  - {id: com.bxi.basic_actions, version: ">=1,<2"}

events:
  activate: {slot: btn_10, value: 8}

states:
  wave:
    factory: state:WaveState
    label: 挥手示例
    index: 20
    group: Customer
    icon: waving_hand
    confirm: true
    confirm_message: 请确保机器人手臂周围没有障碍物

routes:
  - from: com.bxi.basic_actions/normal
    event: activate
    to: wave
    transition: soft_switch
  - from: wave
    event: com.bxi.basic_actions/normal
    to: com.bxi.basic_actions/normal
    transition: dual_running_blend
  - from: wave
    event: com.bxi.basic_actions/zero_torque
    to: com.bxi.basic_actions/zero_torque

schema 和 api 省略时默认为 1。factory: state:WaveState 表示加载同目录的 state.py 中的 WaveState；这种简单 Mod 不需要 plugin.py。

1. PoseState：只描述目标姿态

新概念：只返回关节目标，框架负责 kp/kd、进入帧、运行帧和电机输出。

只改 state.py：

from bxi_example_py_elf3.utils.state_library import PoseState


class WaveState(PoseState):
    def target_position(self, ctx):
        qpos = ctx.pos_last.copy()
        qpos[4] += 0.35
        return qpos

这里从上一控制帧复制目标，只改变一个关节。首次调试必须先确认 qpos[4] 对应的真实关节、方向和安全范围。PoseState 自动实现 EntryFrameProvider 和 RunningFrameProvider，因此可直接使用 soft_switch、entry_gain_ramp 和 running_blend。

保持不变：mod.yaml、完整状态名、routes、按键绑定。

停在本级：目标是固定姿态、保持姿态或简单零位，不需要时间变化。

2. ProceduralState：加入时间和连续轨迹

新概念：用 elapsed 计算轨迹。框架只在真实更新时推进时间；过渡以 advance=False 采样时不会偷偷推进动作。

仍然只改 state.py：

import math

from bxi_example_py_elf3.utils.state_library import ProceduralState


class WaveState(ProceduralState):
    def compute_frame(self, ctx, elapsed):
        qpos = ctx.pos_last.copy()
        qpos[4] += 0.35
        qpos[5] += 0.25 * math.sin(2.0 * math.pi * 0.7 * elapsed)
        return self.frame(ctx, qpos)

self.frame(ctx, qpos) 默认使用 ctx.joint_kp/joint_kd。需要特殊增益时，可覆盖 gains(ctx)，也可传 self.frame(ctx, qpos, kp=..., kd=...)。

保持不变：mod.yaml 的全部内容。

停在本级：轨迹能由公式、插值或小型有限状态变量表达，不需要模型推理。

3. dataclass 参数：让 YAML 成为有类型的调节面板

新概念：参数拥有名字、类型和默认值；拼错字段或类型错误会在启动时失败，而不是在真机运行中暴露。

替换 state.py：

from dataclasses import dataclass
import math

from bxi_example_py_elf3.utils.state_library import ProceduralState


@dataclass(frozen=True)
class WaveParams:
    shoulder: int = 4
    elbow: int = 5
    shoulder_offset: float = 0.35
    amplitude: float = 0.25
    frequency: float = 0.7
    duration: float | None = None


class WaveState(ProceduralState[WaveParams]):
    Params = WaveParams

    def compute_frame(self, ctx, elapsed):
        qpos = ctx.pos_last.copy()
        qpos[self.params.shoulder] += self.params.shoulder_offset
        qpos[self.params.elbow] += self.params.amplitude * math.sin(
            2.0 * math.pi * self.params.frequency * elapsed
        )
        return self.frame(ctx, qpos)

只在 mod.yaml 的 states.wave 下增加：

    params:
      shoulder: 4
      elbow: 5
      shoulder_offset: 0.35
      amplitude: 0.20
      frequency: 0.8

约定工厂看到类上的 Params 是 dataclass 后，会自动调用 StateBuildContext.dataclass_params()，再构造 WaveState(name, state_id, params)。支持 int、float、bool、str 和这些类型的可选值；bool 不会被误当成整数。dataclass 默认值、default_factory 和未知字段检查都保留。

保持不变：类名、factory、状态名、events 和 routes。

停在本级：客户只需通过 YAML 调动作，不需要替换算法。

4A. PolicyState：先抽象推理生命周期

新概念：策略的创建、预热、重置、进入姿态、推理和增益被分开。先用一个纯 Python 策略理解接口，不急着引入模型文件。

替换 state.py：

from dataclasses import dataclass
import math

from bxi_example_py_elf3.utils.state_library import PolicyState


@dataclass(frozen=True)
class WaveParams:
    joint: int = 5
    amplitude: float = 0.20
    frequency: float = 0.8


class WavePolicy:
    def __init__(self, params):
        self.params = params
        self.elapsed = 0.0

    def reset(self):
        self.elapsed = 0.0

    def infer(self, base, dt, advance):
        qpos = base.copy()
        qpos[self.params.joint] += self.params.amplitude * math.sin(
            2.0 * math.pi * self.params.frequency * self.elapsed
        )
        if advance:
            self.elapsed += dt
        return qpos


class WaveState(PolicyState[WavePolicy]):
    Params = WaveParams

    def create_policy(self, ctx):
        return WavePolicy(self.params)

    def reset_policy(self, ctx, policy):
        policy.reset()

    def policy_entry_position(self, ctx, policy):
        return ctx.pos_last.copy()

    def infer_position(self, ctx, policy, dt, *, advance):
        return policy.infer(ctx.pos_last, dt, advance)

PolicyState 会惰性调用 create_policy()；策略提供框架标准的 inference_step/infer_step 时在 on_prepare() 使用节点预热器，并保证 advance=False 传给策略。特殊模型可覆盖 preheat_policy()。以后把 WavePolicy 换成 ONNX/Torch 推理器时，状态图无需变化。

保持不变：mod.yaml，包括 dataclass 参数。

停在本级：实时模型有自己的 observation/action 逻辑，或存在 recurrent state，但不属于固定动作回放。

4B. MotionReplayState：固定模型动作直接复用

如果模型已经提供 start_frame/end_frame/target_dof_pos/kps/kds/inference_step()，优先复用 MotionReplayState，不要重复写播放、预热、暂停和结束返回逻辑。

状态最小形态：

from bxi_example_py_elf3.utils.state_library import MotionReplayState


class WaveState(MotionReplayState):
    def __init__(self, name, state_id, policy):
        super().__init__(
            name,
            state_id,
            policy,
            finish_trigger="wave_finished",
            end_frame_trim=20,
            end_transition={
                "profile": "dual_running_blend",
                "duration": 0.6,
            },
        )

这时策略通常来自第 6 节的 Resource，因此会增加 plugin.py。完整可运行模型例子见 把模型动作封装成 Mod。

停在本级：离线 motion + policy 的接口与 ReplayPolicy 一致。

5. RobotControlState：打开完整状态生命周期

新概念：直接控制每个生命周期和输出，框架不再替你生成 frame。只有易用基类限制了需求时才下沉。

把 state.py 改成：

from bxi_example_py_elf3.utils.robot_state_base import RobotControlState
from bxi_example_py_elf3.utils.transition_core import (
    EntryFrameProvider,
    RunningFrameProvider,
)


class WaveState(RobotControlState, EntryFrameProvider, RunningFrameProvider):
    def __init__(self, name, state_id):
        super().__init__(name, state_id)
        self.elapsed = 0.0

    def on_enter(self, ctx):
        self.elapsed = 0.0

    def _calculate(self, ctx, elapsed):
        qpos = ctx.pos_last.copy()
        # 在这里可以读取传感器、做 IK、MPC、滤波或任意业务计算。
        return self._motor_frame(qpos, ctx.joint_kp, ctx.joint_kd)

    def get_entry_frame(self, ctx):
        return self._calculate(ctx, 0.0)

    def sample_running_frame(self, ctx, dt, *, advance):
        frame = self._calculate(ctx, self.elapsed)
        if advance:
            self.elapsed += dt
        return frame

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

如果不使用需要进入帧/运行帧的过渡，可以不实现相应 Protocol。on_exit() 默认保存最后电机帧；覆盖时通常应先或后调用 super().on_exit(ctx)。

保持不变：factory: state:WaveState 仍有效；无参数构造仍无需 plugin.py。

停在本级：你已经能实现任意状态内算法，但尚不需要共享、惰性加载和统一释放大型对象。

6. 自定义 Resource：共享、惰性加载并统一释放

新概念：把模型、数据集、硬件句柄等昂贵对象从状态生命周期中分离。此级新增 plugin.py，并把工厂改为显式入口。

目录变成：

com.example.wave/
  mod.yaml
  plugin.py
  state.py
  assets/
    wave.yaml

先建立 assets/wave.yaml：

shoulder: 4
elbow: 5
shoulder_offset: 0.35
amplitude: 0.20
frequency: 0.8

把 state.py 替换为一个完整的 Resource 消费者：

from dataclasses import dataclass
import math

from bxi_example_py_elf3.utils.state_library import PolicyState


@dataclass(frozen=True)
class WaveProfile:
    shoulder: int
    elbow: int
    shoulder_offset: float
    amplitude: float
    frequency: float


class WaveState(PolicyState[WaveProfile]):
    def __init__(self, name, state_id, profile):
        super().__init__(name, state_id, profile)
        self.elapsed = 0.0

    def reset_policy(self, ctx, profile):
        self.elapsed = 0.0

    def policy_entry_position(self, ctx, profile):
        qpos = ctx.pos_last.copy()
        qpos[profile.shoulder] += profile.shoulder_offset
        return qpos

    def infer_position(self, ctx, profile, dt, *, advance):
        qpos = self.policy_entry_position(ctx, profile)
        qpos[profile.elbow] += profile.amplitude * math.sin(
            2.0 * math.pi * profile.frequency * self.elapsed
        )
        if advance:
            self.elapsed += dt
        return qpos

plugin.py 完整内容：

import yaml

from bxi_example_py_elf3.utils.mod_system import (
    ModDefinition,
    ResourceKey,
)

from .state import WaveProfile, WaveState


PROFILE = ResourceKey[WaveProfile]("com.example.wave/profile")


def create_mod(context):
    def load_profile(resource):
        path = resource.asset("assets/wave.yaml")
        with path.open("r", encoding="utf-8") as input_file:
            raw = yaml.safe_load(input_file)
        return WaveProfile(
            shoulder=int(raw["shoulder"]),
            elbow=int(raw["elbow"]),
            shoulder_offset=float(raw["shoulder_offset"]),
            amplitude=float(raw["amplitude"]),
            frequency=float(raw["frequency"]),
        )

    context.register_resource(PROFILE, load_profile)
    profile = context.resource(PROFILE)  # 只是 handle，此处不会读文件
    return ModDefinition(
        state_factories={
            "wave": lambda state: WaveState(
                state.name,
                state.state_id,
                profile,
            )
        }
    )

state.py 中让状态接收 ResourceHandle。若继承 PolicyState，直接把 handle 传给 super().__init__(name, state_id, policy)；第一次 prepare/inference 才会调用 handle.get()。

mod.yaml 改两处：

entrypoint: plugin:create_mod

states:
  wave:
    # 删除 factory: state:WaveState
    label: 挥手示例
    index: 20

其他字段全部不变。这个例子用 YAML 是为了能直接运行；换成 ONNX 时，只需让 loader 返回推理器。资源只能通过 resource.asset("assets/...") 访问本 Mod 的 assets/；实际使用路径会进入热重载追踪。资源对象有 close() 时，运行时关闭会自动调用。

停在本级：多个状态共享模型，或对象需要惰性初始化、缓存和可靠释放。

7. 自定义 Transition：让“怎么切换”也可插拔

新概念：状态只描述行为，Transition 描述两个状态之间如何连续切换。新增 pose_gain_blend.py：

from collections.abc import Mapping

from bxi_example_py_elf3.utils.transition_core import (
    ConfigReader,
    MotorFrame,
    SingleClassTransition,
    require_entry_frame_provider,
)


class PoseGainBlend(SingleClassTransition):
    type_name = "com.example.wave.pose_gain_blend"

    def __init__(self, name, duration):
        super().__init__(name, duration)
        self.start = None
        self.target = None

    @classmethod
    def from_config(cls, name: str, raw: Mapping[str, object]):
        reader = ConfigReader(raw, name)
        duration = reader.float("duration", minimum=0.0)
        reader.finish()
        return cls(name, duration)

    def validate_states(self, from_state, to_state):
        require_entry_frame_provider(to_state)

    def on_start(self, ctx, from_state, to_state):
        self.start = MotorFrame.create(ctx.pos_last, ctx.kp_last, ctx.kd_last)
        self.target = require_entry_frame_provider(to_state).get_entry_frame(ctx)

    def apply(self, ctx, dt, progress):
        alpha = progress * progress * (3.0 - 2.0 * progress)
        ctx.set_motor_target(
            self.start.qpos + (self.target.qpos - self.start.qpos) * alpha,
            self.start.kp + (self.target.kp - self.start.kp) * alpha,
            self.start.kd + (self.target.kd - self.start.kd) * alpha,
        )

在 plugin.py 顶层导入一次以触发注册：

from . import pose_gain_blend  # noqa: F401

在 mod.yaml 增加 profile，并只替换 route 的 transition：

transition_profiles:
  wave_entry:
    type: com.example.wave.pose_gain_blend
    duration: 0.6

routes:
  - from: com.bxi.basic_actions/normal
    event: activate
    to: wave
    transition: wave_entry

Transition 与其动态模块会一起参与热重载和失败回滚。完整字段、能力校验和测试方法见 手把手自定义过渡。

停在本级：内置 instant/hold/gain ramp/running blend/sequence 已不够表达切换过程。

8. 自定义 ROS subscriber、service、timer

新概念：状态可以拥有 ROS 实体，但创建与销毁必须成对。不要在 __init__() 创建 ROS 对象；构造时还没有绑定 node。

在 WaveState 增加：

from std_srvs.srv import SetBool
from std_msgs.msg import Float32


def on_bind(self, ctx):
    self.external_scale = 1.0
    self.enabled = True
    self.subscription = ctx.create_subscription(
        Float32,
        "/wave/amplitude_scale",
        self._on_scale,
        10,
    )
    self.service = ctx.create_service(
        SetBool,
        "/wave/enable",
        self._on_enable,
    )
    self.timer = ctx.create_timer(1.0, self._on_timer)

def _on_scale(self, message):
    # callback 只更新状态私有数据，不直接发电机命令。
    self.external_scale = max(0.0, min(float(message.data), 1.0))

def _on_enable(self, request, response):
    self.enabled = bool(request.data)
    response.success = True
    response.message = "wave enabled" if self.enabled else "wave disabled"
    return response

def _on_timer(self):
    # 低频维护工作；实时电机输出仍只在 on_update 中产生。
    pass

def on_unbind(self, ctx):
    ctx.destroy_timer(self.timer)
    ctx.destroy_service(self.service)
    ctx.destroy_subscription(self.subscription)
    self.timer = None
    self.service = None
    self.subscription = None

之后在 _calculate() 或 infer_position() 中读取 enabled/external_scale。热重载会先对旧状态调用 on_unbind()，所以忘记释放会产生重复订阅、重复 service 或悬空 callback。

保持不变：Mod ID、状态名、routes、Resource 和 Transition。

停在本级：需要外部感知、业务服务或低频任务，但输入协议仍是现有键盘/手柄/CRSF。

9. 自定义输入 Driver：接入一种全新设备或协议

这是状态 Mod 之外的系统扩展点。只有要接入 UDP、SBUS、新串口协议、蓝牙或自定义 HID 时才写 Driver；“换按键”“换手柄映射”“组合键”只改遥控器 YAML。

数据边界必须保持：

自定义 Driver
  -> raw signal（例如 udp.vx、udp.wave）
  -> sources
  -> controls
  -> outputs
  -> MotionCommands.btn_10=8
  -> com.example.wave/activate
  -> 原来的 route 和 wave 状态

因此 Driver 不应知道 com.example.wave/wave。实现步骤：

1. 在 src/remote_controller/src/drivers/ 实现 InputDriverBase。
2. is_available() 必须非阻塞，并依据设备/近期合法帧判断健康。
3. start()/stop() 成对管理线程、fd/socket；is_ready() 只在收到完整安全初始帧后为真。
4. 用 set_signal("udp.wave", 0.0/1.0) 发布 raw signal。
5. 在 driver registry 注册 type: udp，并加入 CMake 源文件。
6. 在遥控器 YAML 的 sources 声明设备，再由 controls/outputs 映射回原来的 btn_10=8。

最小 YAML 连接段：

sources:
  udp_remote:
    type: udp
    priority: 20
    bind: 0.0.0.0
    port: 14550
    ready_timeout_ms: 1000
    loss_timeout_ms: 500
    signals:
      udp.wave: {from: udp.wave, timeout_ms: 500, failsafe: 0.0}

controls:
  udp.wave_event:
    type: bool
    threshold: 0.5
    inputs: [{source: udp.wave}]

outputs:
  edge:
    - output: btn_10=8
      when:
        any:
          - [udp.wave_event]

完整 C++ Driver、注册、断连抢占和发包验证见 手把手 UDP Driver。

10. 最终检查表

每次只证明当前层正确，再升一级：

• ./tools/bxi-mod validate 通过，inspect 中完整名称、index 和 route 正确。
• 仿真中先验证关节顺序、范围、第一帧、退出帧和 advance=False。
• dataclass 不保留无用参数；错误字段能在加载期失败。
• 模型资源没有在 import 或状态 __init__() 阶段加载。
• 自定义 Transition 对所需状态 capability 做加载期验证。
• on_bind() 创建的 ROS 实体全部在 on_unbind() 释放。
• Driver 只产生 raw signal，断连时归零并允许设备管理器安全切换。
• 最后才进行低增益、急停可达、清空障碍物条件下的真机测试。

最重要的升级规则只有一句：保留 com.example.wave/wave 这份稳定契约，按需替换它背后的实现。基础 API 可以直接用于正式功能，高级扩展点则按实际需求逐步引入。