Hands On Custom State

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

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

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

完成结果

完成本课后，你将得到：

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

先理解状态基类

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

本课使用的 ProceduralState 继承自 RobotControlState。它不是另一套状态系统，只是替初学者实现了时间累计、进入帧、运行帧和 on_update()。当这些默认行为不满足需求时，可以直接改为继承 RobotControlState，Mod ID、状态名和 routes 都不需要变化。

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 ProceduralState


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


class SinWaveState(ProceduralState[SinWaveParams]):
    Params = SinWaveParams

    def __init__(self, name, state_id, params=None):
        super().__init__(name, state_id, params)
        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 gains(self, ctx):
        return ctx.last_motor_frame.kp, ctx.last_motor_frame.kd

    def compute_frame(self, ctx, elapsed):
        if self._joint_index is None:
            raise RuntimeError("sin wave state is not prepared")
        frame = self.frame(ctx, self._base_position)
        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

ProceduralState 已经实现进入帧、运行帧、时间重置和正常电机输出。advance=False 由基类保证不推进 elapsed。

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

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

这里显式选择沿用上一控制帧的 kp/kd，所以本教程只从正常受控状态进入；如果状态可从零力矩模式进入，必须提供自己的非零安全增益。

3. 写清单

mod.yaml：

schema: 1
id: com.example.sin_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:
  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: dual_running_blend

这里的 Transition 描述的是：从状态机收到切换请求到目标状态正式进入之间，每个控制周期应该怎样生成电机帧。之所以不能一律直接切换，是因为两个状态的 qpos/kp/kd 可能不连续，直接换成目标输出可能让机器人突然动作。first_frame_switch 先用目标进入帧逐步建立控制力，dual_running_blend 则混合两边的运行帧；ProceduralState 已经提供了这两种过渡需要的帧能力。

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

4. 构建和验证

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

启动日志应包含：

Mod com.example.sin_wave@1.0.0

状态完整名是 com.example.sin_wave/sin_wave。此时还没有遥控器输出 btn_10=8，可先用测试发布或继续第 3 课完成绑定。

检查清单

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

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

下一课：模型动作状态。

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