Mod State Development Guide

Mod 状态开发进阶指南

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

com.example.wave/wave

先理解核心，再选择便捷类。 RobotControlState 是状态系统的主要抽象；PoseState、ProceduralState、PolicyState 和 MotionReplayState 都是它的子类，只是替常见场景实现了重复代码。

核心类与便捷类的关系

状态机构建和运行的始终是 RobotControlState：

RobotControlState                         核心生命周期与完整控制能力
├─ PoseState                              固定目标姿态封装
├─ ProceduralState                        带 elapsed 的轨迹封装
├─ PolicyState                            策略创建、预热和推理封装
└─ MotionReplayState                      固定动作回放封装

核心类定义 on_bind/on_prepare/on_enter/on_update/on_exit/on_unbind 等生命周期，并提供速度输入、MotorFrame 和电机输出辅助方法。便捷类通过继承核心类，进一步实现 on_update()、进入帧或运行帧等通用逻辑，让初学者只填写动作本身。

使用便捷类不会产生另一套运行时，也不会降低框架上限。当默认行为不适用时，可以改为直接继承 RobotControlState；只要保留构造契约和状态能力，Mod ID、状态名及 routes 都不需要变化。

便捷类的实现原理

• PoseState 的 on_update() 调用 target_position()，使用 gains() 提供的 kp/kd 构造完整 MotorFrame，再把 frame 写给控制上下文。它用同一计算结果实现进入帧和运行帧。
• ProceduralState 在 on_enter() 把 elapsed 清零。每次真实更新先调用 compute_frame(ctx, elapsed)，随后推进时间；Transition 使用 advance=False 采样时只观察当前输出，不改变时间。
• PolicyState 在 prepare/enter 阶段解析并重置策略，可调用模型预热；运行阶段由子类提供进入位置和推理位置，基类统一处理策略增益、ResourceHandle 和 MotorFrame。
• MotionReplayState 面向符合 ReplayPolicy 接口的固定动作，统一处理 timestep、预热、暂停、结束帧和返回基础状态。

这些封装的核心价值是复用正确的生命周期和 Transition 采样语义，而不是隐藏或替代 RobotControlState。开发者仍可覆盖继承方法，或者在需求复杂后直接实现核心类。

第一次开发普通动作：读到第 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 都不需要改变；旧配置和遥控器绑定也不需要重写。只有当当前层确实不够用时，才进入下一层。

需要更多能力时，在同一个核心模型上继续引入 Resource、Transition、ROS 实体和 InputDriver。

0. 手工建立 Mod

建立目录：

mkdir -p ~/bxi_mods/com.example.wave

本指南将创建两个文件：

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

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

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

构建工作区并加载环境：

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

手工创建 ~/bxi_mods/com.example.wave/mod.yaml：

schema: 1
id: com.example.wave
name: 挥手示例
version: 1.0.0
api: ">=2,<3"
enable: true
entrypoint: null
visibility: public
requires:
  - {id: com.bxi.basic_actions, version: ">=1,<2"}
conflicts: []
python_exports: []
runtime_requirements:
  python: []
  ros: []
  system: []

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

states:
  wave:
    factory: state:WaveState
    label: 挥手示例
    priority: 100
    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

factory: state:WaveState 表示加载同目录 state.py 中的 WaveState；这种简单 Mod 使用显式的 entrypoint: null，不需要 plugin.py。描述 Mod 的头部字段即使为空也不能省略。

1. PoseState：只描述目标姿态

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

只改 state.py：

from bxi_example_py_elf3.framework.mod_api import PoseState


class WaveState(PoseState):
    def gains(self, ctx):
        return ctx.last_motor_frame.kp, ctx.last_motor_frame.kd

    def target_position(self, ctx):
        qpos = ctx.last_motor_frame.qpos.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.framework.mod_api import ProceduralState


class WaveState(ProceduralState):
    def gains(self, ctx):
        return ctx.last_motor_frame.kp, ctx.last_motor_frame.kd

    def compute_frame(self, ctx, elapsed):
        last = ctx.last_motor_frame
        frame = self.frame(ctx, last.qpos)
        frame.qpos[4] += 0.35
        frame.qpos[5] += 0.25 * math.sin(2.0 * math.pi * 0.7 * elapsed)
        return frame

self.frame(ctx, qpos) 会调用当前状态的 gains(ctx)；框架不再提供与具体机器人耦合的全局增益。这里明确沿用上一控制帧的 kp/kd。需要从零力矩状态进入或使用特殊增益时，应让状态返回自身经过验证的 kp/kd，也可传 self.frame(ctx, qpos, kp=..., kd=...)。

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

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

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

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

替换 state.py：

from dataclasses import dataclass
import math

from bxi_example_py_elf3.framework.mod_api 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 gains(self, ctx):
        return ctx.last_motor_frame.kp, ctx.last_motor_frame.kd

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

只在 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.framework.mod_api 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 gains(self, ctx):
        return ctx.last_motor_frame.kp, ctx.last_motor_frame.kd

    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.last_motor_frame.qpos.copy()

    def infer_position(self, ctx, policy, dt, *, advance):
        return policy.infer(ctx.last_motor_frame.qpos, 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.framework.mod_api 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

RobotControlState 从一开始就是所有状态的核心基类。本节不再使用便捷子类，而是直接实现核心生命周期、Transition capability 和电机输出。

把 state.py 改成：

from bxi_example_py_elf3.framework.mod_api import RobotControlState
from bxi_example_py_elf3.framework.mod_api.transition 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):
        last = ctx.last_motor_frame
        qpos = last.qpos.copy()
        # 在这里可以读取传感器、做 IK、MPC、滤波或任意业务计算。
        return self._motor_frame(ctx, qpos, last.kp, last.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.framework.mod_api 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 gains(self, ctx):
        return ctx.last_motor_frame.kp, ctx.last_motor_frame.kd

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

    def policy_entry_position(self, ctx, profile):
        qpos = ctx.last_motor_frame.qpos.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.framework.mod_api 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, loading="lazy")
    profile = context.resource(PROFILE)  # 只是 handle；lazy 时此处不会读文件
    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: 挥手示例
    priority: 100

其他字段全部不变。这个例子用 YAML 是为了能直接运行；换成 ONNX 时，只需让 loader 返回推理器。资源只能通过 resource.asset("assets/...") 访问本 Mod 的 assets/。资源对象有 close() 时，节点关闭会自动调用。加载较慢且切换时必须立即可用的模型应把注册参数改为 loading="eager"；框架会在控制循环启动前完成加载。

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

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

新概念：状态描述自己运行时的行为，Transition 描述从状态机收到切换请求到目标状态正式进入之间如何生成电机帧。需要单独设计这段过程，是因为两边的 qpos/kp/kd 可能不连续，直接切换可能让机器人突然动作；内置实现可以保持、渐增增益或混合两边的运行帧。

只有内置策略不能表达业务需要时才自定义 Transition。下面新增 pose_gain_blend.py：

from collections.abc import Mapping
import numpy as np

from bxi_example_py_elf3.framework.mod_api.transition 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
        self.output = 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.robot_layout,
            ctx.last_motor_frame.qpos,
            ctx.last_motor_frame.kp,
            ctx.last_motor_frame.kd,
        )
        natural_target = require_entry_frame_provider(to_state).get_entry_frame(ctx)
        self.target = MotorFrame.empty(ctx.robot_layout)
        ctx.resolve_motor_frame(natural_target, self.target)
        self.output = MotorFrame.empty(ctx.robot_layout)

    def apply(self, ctx, dt, progress):
        if self.start is None or self.target is None or self.output is None:
            raise RuntimeError("pose gain blend has not started")
        alpha = progress * progress * (3.0 - 2.0 * progress)
        for start, target, output in (
            (self.start.qpos, self.target.qpos, self.output.qpos),
            (self.start.kp, self.target.kp, self.output.kp),
            (self.start.kd, self.target.kd, self.output.kd),
        ):
            np.subtract(target, start, out=output)
            output *= alpha
            output += start
        ctx.set_motor_target(self.output)

在 plugin.py 中显式加入 Mod 定义：

from .pose_gain_blend import PoseGainBlend


return ModDefinition(
    state_factories={...},
    transition_plugins={PoseGainBlend.type_name: PoseGainBlend},
)

在 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。

如果 ROS 实体本身是可独立运行的后台能力，例如相机发布器、感知预处理或 Mod 专属服务，应优先声明为 mod.yaml 顶层 nodes。它可以选择与控制器同进程，也可以隔离在 Python 子进程；还可以随整个 Mod 常驻，或在一组状态准备前启动。只有明确属于单个状态对象的轻量订阅、service 或 timer 才放在 on_bind/on_unbind。

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. 最终检查表

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

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

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