Hands On Custom State

手把手 1：用 Mod API 4.0 开发一个动作

本课创建 com.example.sin_wave，从基础走路状态进入正弦摆动，再按正常模式键返回。

适合第一次接触框架的开发者。 本课只涉及两个文件，不要求先理解 Resource、命令合成或状态机内部实现。示例使用当前的具名关节和长期复用缓冲，不依赖 ELF3 的数字关节下标。

本页是可按顺序操作的入门教程。需要查询完整生命周期和字段时转到 自定义状态参考；需要加入模型、Resource 或 ROS 实体时再进入 Mod 状态开发进阶指南。

完成结果

完成本课后，你将得到：

• 一个可被自动发现的 com.example.sin_wave Mod；
• 一个直接使用核心基类、带强类型参数的 RobotControlState；
• 一个能适应机器人 29、31 或更多关节的具名关节动作；
• 一组能够随节点启动完成加载和校验的状态与 routes。

先理解核心状态基类

RobotControlState 是框架的核心状态基类。状态机要求每个状态工厂最终返回它的实例，并通过它提供的 on_bind/on_prepare/on_enter/on_update/on_exit/on_unbind 生命周期执行控制逻辑。

本课直接继承 RobotControlState，并显式实现时间累计、电机帧输出和稳定进入帧。第一课 只使用进入帧过渡；双状态动态采样留到专门课程，避免同时引入过多概念。

1. 建立目录

先确认终端位于仓库根目录：

cd ~/bxi_rl_controller_ros2_example

手工建立 Mod 目录：

mkdir -p src/bxi_example_py_elf3/mods/com.example.sin_wave

src/bxi_example_py_elf3/mods/com.example.sin_wave/
  mod.yaml
  state.py

2. 写状态

state.py：

from dataclasses import dataclass
import math
import numpy as np

from bxi_example_py_elf3.framework.mod_api import (
    EntryFrameProvider,
    RobotControlState,
)


@dataclass(frozen=True)
class SinWaveParams:
    joint: str = "l_knee_y_joint"
    amplitude: float = 0.25
    frequency: float = 0.5


class SinWaveState(RobotControlState, EntryFrameProvider):
    Params = SinWaveParams

    def __init__(self, name, state_id, params: SinWaveParams | None = None):
        super().__init__(name, state_id)
        self.params = params or SinWaveParams()
        self.elapsed = 0.0
        self._joint_index = None
        self._base_position = None
        self._layout = None

    def on_prepare(self, ctx, from_state):
        layout = ctx.robot_layout
        if self._layout is not layout:
            # 首次进入或 Robot Layout 改变时才重新编译。
            self._joint_index = layout.index(self.params.joint)
            self._base_position = np.empty(
                layout.dof_num,
                dtype=np.float32,
            )
            self._layout = layout
        # 每次进入动作前捕获基准姿态，正弦值不会逐帧累加。
        np.copyto(self._base_position, ctx.last_motor_frame.qpos)

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

    def _calculate_frame(self, ctx, elapsed):
        if self._joint_index is None:
            raise RuntimeError("sin wave state is not prepared")
        last = ctx.last_motor_frame
        frame = self._motor_frame(
            ctx,
            self._base_position,
            last.kp,
            last.kd,
        )
        frame.qpos[self._joint_index] = self._base_position[self._joint_index] + (
            self.params.amplitude * math.sin(
                2.0 * math.pi * self.params.frequency * elapsed
            )
        )
        return frame

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

    def on_update(self, ctx, dt):
        frame = self._calculate_frame(ctx, self.elapsed)
        self._apply_frame(ctx, frame)
        self.elapsed += dt

RobotControlState 提供核心生命周期、长期复用的 _motor_frame() 缓冲和 _apply_frame() 输出入口。示例实现 get_entry_frame()，满足 first_frame_switch 对目标状态的要求；正常控制只经过 on_update()，每周期推进一次 elapsed。

这个状态的自然输出布局是 ctx.robot_layout：目标关节由公式更新，其余机器人关节保持进入动作时的姿态。因此机器人从 29 关节增加到 31 或 N 关节时，不需要修改模型输入或硬编码新的数组长度。关节名和缓冲只在 on_prepare() 首次遇到一个 Robot Layout 时编译；基准数组和 _motor_frame() 的 MotorFrame 都长期复用，控制周期内没有 .copy()，也不会反复创建数组。

不要在 on_bind() 读取 ctx.robot_layout，因为绑定状态时首个机器人状态快照可能尚未到达。on_bind() 主要用于创建 ROS 实体；依赖机器人布局的准备工作放在 on_prepare()。

这里显式选择沿用上一控制帧的 kp/kd，所以本教程只从正常受控状态进入；如果状态可从零力矩模式进入，必须提供自己的非零安全增益。on_enter() 会在 Transition 完成、状态正式接管控制时重置时间，on_update() 则在每个控制周期推进时间并提交电机帧。

3. 写清单

mod.yaml：

schema: 1
id: com.example.sin_wave
name: 正弦摆动
version: 1.0.0
api: ">=4,<5"
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: 4

states:
  sin_wave:
    factory: state:SinWaveState
    label: 正弦摆动
    priority: 100
    group: Customer
    icon: waves
    params:
      joint: l_knee_y_joint
      amplitude: 0.25
      frequency: 0.5

routes:
  - from: com.bxi.basic_actions/normal
    event: activate
    to: sin_wave
    transition: first_frame_switch

  - from: sin_wave
    event: com.bxi.basic_actions/normal
    to: com.bxi.basic_actions/normal
    transition: first_frame_switch

这里的 Transition 描述的是：从状态机收到切换请求到目标状态正式进入之间，每个控制周期 应该怎样生成电机帧。两个状态的 qpos/kp/kd 可能不连续，直接换帧可能让机器人突然 动作。两条 route 都使用 first_frame_switch，它读取目标状态的稳定进入帧并逐步建立 控制力。本例只需实现 EntryFrameProvider；需要混合两个动态状态时，再阅读 双状态运行混合。

entrypoint: null 让约定 factory 根据类上的 Params 自动构造 dataclass，所以不需要 plugin.py；其余描述头字段同样需要显式填写。

4. 构建和验证

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

启动日志应包含：

[com.example.sin_wave]: loaded v1.0.0: .../com.example.sin_wave

状态完整名是 com.example.sin_wave/sin_wave。当前遥控器已有 btn_10=4 的手柄/CRSF 组合，但还没有本课的键盘入口；可先测试该组合，或继续第 3 课补齐键盘绑定。

检查清单

1. factory: state:SinWaveState 的模块和类存在。
2. 跨 Mod 引用有完整名称和 requires。
3. 参数名与 dataclass 字段一致、类型正确。
4. on_update() 每周期只推进一次 elapsed。
5. 代码通过关节名工作，没有把数字下标当成跨组件契约。
6. 控制周期复用 MotorFrame，没有逐帧 .copy() 或创建数组。

如果一个动作的关节命令同时来自模型、ROS 话题、IK 或程序轨迹，不要继续在这个单来源例子里堆逻辑，使用多来源关节命令合成。如果策略输出关节数与机器人不同，阅读关节布局与映射。

下一课：模型动作状态。

要继续把这个状态逐步升级成模型、Resource、自定义 Transition、ROS 和 Driver，进入 Mod 状态开发进阶指南。