Robot Platform Integration

将 BXI 运控框架接入自己的机器人程序

本文面向需要复用 BXI 状态机、Mod、Transition、推理和控制调度，但使用自己机器人消息、 SDK、CAN 总线或已有控制程序的开发者。

当前安装入口 bxi_example_py_elf3_demo 的实现文件是 bxi_example_py_elf3/bxi_example_demo.py。它承担的只是 ELF3 平台适配层角色：订阅 ELF3 状态，把数据整理成框架观测，再把框架输出转换为 ELF3 电机命令。它不是运控框架本体。迁移到其他程序时，通常应新写一个适配层，不应复制和修改状态机内部代码。

先看清边界

自己的机器人程序 / ROS 话题 / SDK / CAN
  -> 读取关节、IMU 和操控指令
  -> 平台适配层（需要新写）
       startup_step()
       snapshot_control_inputs() -> RobotObservation + events
       publish_motor_frame(MotorFrame)
  -> RobotControlRuntime（保留）
       ControlScheduler
       RobotControlFramework
       Mod / State / Transition / Inference
  -> 平台适配层（需要新写）
  -> 自己的硬件命令

迁移时保留与替换的内容如下：

保留	替换或确认
`RobotControlRuntime` 和绝对时间调度	机器人状态订阅或 SDK 读取
Mod 加载、状态机和 Transition	电机消息、CAN 帧或厂商 SDK 调用
推理后端和策略类	关节数量、名称、顺序、方向和零点
状态机 YAML 和适用的 Mod	IMU 坐标系、四元数语义和单位
`MotorFrame(qpos, kp, kd)` 契约	上电、使能、看门狗和安全停机

如果目标机器人与模型具有相同的关节拓扑和策略观测契约，通常只需更换适配层。如果关节数量、机构、默认姿态或传感器语义不同，还需要新的策略模型、Mod 和状态参数；消息转换无法让一个为不同机构训练的策略安全工作。

平台适配器只有三个必需方法

RobotControlRuntime 通过 ControlPlatformAdapter 使用平台，不关心底层是 ROS、共享内存还是厂商 SDK。

class ControlPlatformAdapter(Protocol):
    def startup_step(self, now: float) -> bool: ...

    def snapshot_control_inputs(
        self,
    ) -> tuple[RobotObservation, Sequence[str]]: ...

    def publish_motor_frame(self, frame: MotorFrame) -> None: ...

方法	适配层必须保证
`startup_step(now)`	非阻塞地检查状态是否齐全、硬件是否使能；只有允许控制时返回 `True`
`snapshot_control_inputs()`	返回同一控制周期的一份完整观测和尚未消费的边沿事件
`publish_motor_frame(frame)`	把框架关节顺序的 `qpos/kp/kd` 转成硬件顺序并立即发送

这三个方法运行在框架控制线程上。不能在其中等待 ROS service、等待设备、加载模型、读取磁盘或输出大量日志。耗时会直接计入控制周期。

输入契约：`RobotObservation`

每周期输入包含：

字段	shape	推荐单位/约定
`q`	`(dof_num,)`	框架关节顺序，弧度
`dq`	`(dof_num,)`	与 `q` 相同顺序，弧度/秒
`quat_xyzw`	`(4,)`	同一姿态的 `(x, y, z, w)` 表示
`quat_wxyz`	`(4,)`	同一姿态的 `(w, x, y, z)` 表示
`omega`	`(3,)`	策略约定坐标系下的机身角速度，弧度/秒
`raw_cmd_vel`	`(3,)`	原始 `[vx, vy, yaw_rate]`，由 speed profile 再缩放和限幅

“推荐单位”不是自动转换。最终必须与所用策略训练时的关节顺序、单位、机身坐标系和姿态方向完全一致。尤其要确认 IMU 给出的是 world -> body 还是 body -> world；必要时求共轭或做固定安装旋转。站立静止时，应检查策略计算得到的 projected gravity 是否指向训练时的方向。

quat_xyzw 与 quat_wxyz 必须表示同一个物理姿态，只是元素顺序不同。输入前应检查有限值并归一化，不能把未初始化的全零四元数交给框架。

事件是状态机使用的完整事件名，例如 com.customer.remote/stand。普通回调只把边沿事件放入队列，由 snapshot_control_inputs() 一次性取走；不要在传感器回调里直接执行状态更新。若继续使用现有 MotionCommands，可调用 runtime.extract_remote_events() 复用当前按键映射。

输出契约：`MotorFrame`

框架每周期可能返回一个 MotorFrame：

frame.qpos  # 目标关节角，float32，shape=(dof_num,)
frame.kp    # 位置增益，float32，shape=(dof_num,)
frame.kd    # 速度增益，float32，shape=(dof_num,)

它采用框架关节顺序，不包含硬件消息，也不直接表示力矩。当前 ELF3 适配层把它解释为目标速度和前馈力矩均为零的关节 PD 命令：

q_des = frame.qpos
dq_des = 0
kp = frame.kp
kd = frame.kd
tau_ff = 0

不同执行器接口应按能力处理：

硬件接口	建议
位置 + `kp/kd`	直接转换并发送上述完整 PD 命令
仅目标位置	可以发送 `qpos`，但 gain ramp 等过渡语义会丢失，必须重新验证安全性
目标位置 + 速度	发送 `qpos`，目标速度通常为零，并明确增益由哪一层持有
纯力矩接口	在可靠的底层伺服环实现 PD、饱和和看门狗；不要把 `qpos` 当作力矩

硬件命令发布失败、出现 NaN、越过硬限位或发送超时应视为控制故障，而不是静默丢帧。

关节名称、顺序、方向和零点

适配层必须定义一个稳定的“框架关节顺序”。策略、状态、RobotObservation 和 MotorFrame 全部使用它。硬件顺序只能在适配层边界转换。

假设对框架关节 i 定义：

q_framework[i] = sign[i] * q_hardware[fw_to_hw[i]] + offset[i]
dq_framework[i] = sign[i] * dq_hardware[fw_to_hw[i]]

其中 sign 只能是 +1/-1，offset 使用弧度。输出到硬件时做严格逆变换：

q_hardware = sign * (q_framework - offset)

索引和变换参数应在构造阶段验证并缓存，不能每周期用 list.index() 查找：

hardware_index = {name: i for i, name in enumerate(hardware_joint_names)}
framework_index = {name: i for i, name in enumerate(framework_joint_names)}

fw_to_hw = np.asarray(
    [hardware_index[name] for name in framework_joint_names],
    dtype=np.intp,
)
hw_to_fw = np.asarray(
    [framework_index[name] for name in hardware_joint_names],
    dtype=np.intp,
)

开始控制前必须拒绝重复名称、缺失关节、数量不匹配和未知关节。若机器人还有头部、夹爪等不受本框架控制的关节，应由另一明确的所有者管理，不要把它们伪造进策略向量。

推荐实现：保留框架调度器

推荐让自己的 ROS Node 或平台主类实现三个适配方法，然后把自身作为 platform 传给 RobotControlRuntime。下面是一份完整骨架；my_robot_msgs 和消息字段需要替换成目标机器人的实际接口。

from collections import deque
from pathlib import Path
from threading import Event, Lock
import time

import numpy as np
import rclpy
from ament_index_python.packages import get_package_share_directory
from rclpy.executors import MultiThreadedExecutor
from rclpy.node import Node

from my_robot_msgs.msg import JointCommand, RobotState  # 替换为自己的消息

from bxi_example_py_elf3._runtime.control_runtime import RobotControlRuntime
from bxi_example_py_elf3._runtime.controller import RobotObservation
from bxi_example_py_elf3._runtime.state_machine import load_state_machine_config
from bxi_example_py_elf3.mod_api import MotorFrame


class MyRobotAdapter(Node):
    def __init__(
        self,
        framework_joint_names: tuple[str, ...],
        hardware_joint_names: tuple[str, ...],
        joint_sign: np.ndarray,
        joint_offset: np.ndarray,
    ) -> None:
        super().__init__("my_bxi_robot_adapter")
        self._stopping = Event()
        self._lock = Lock()
        self._events: deque[str] = deque()
        self._control_started = False

        self._validate_joint_contract(
            framework_joint_names,
            hardware_joint_names,
        )
        dof_num = len(framework_joint_names)
        hardware_index = {
            name: i for i, name in enumerate(hardware_joint_names)
        }
        framework_index = {
            name: i for i, name in enumerate(framework_joint_names)
        }
        self._fw_to_hw = np.asarray(
            [hardware_index[name] for name in framework_joint_names],
            dtype=np.intp,
        )
        self._hw_to_fw = np.asarray(
            [framework_index[name] for name in hardware_joint_names],
            dtype=np.intp,
        )
        self._sign = np.asarray(joint_sign, dtype=np.float64).reshape(dof_num)
        self._offset = np.asarray(joint_offset, dtype=np.float64).reshape(dof_num)
        if not np.all((self._sign == 1.0) | (self._sign == -1.0)):
            raise ValueError("joint_sign must contain only +1 or -1")

        # 回调只更新这些持久缓冲，不在回调里运行状态机。
        self._q = np.zeros(dof_num, dtype=np.float64)
        self._dq = np.zeros(dof_num, dtype=np.float64)
        self._quat_xyzw = np.zeros(4, dtype=np.float64)
        self._quat_wxyz = np.zeros(4, dtype=np.float64)
        self._omega = np.zeros(3, dtype=np.float64)
        self._raw_cmd_vel = np.zeros(3, dtype=np.float32)
        self._joint_received = False
        self._imu_received = False
        self._joint_update_at = 0.0
        self._imu_update_at = 0.0

        # 输出重排缓冲同样只申请一次。
        self._cmd_q = np.empty(dof_num, dtype=np.float32)
        self._cmd_kp = np.empty(dof_num, dtype=np.float32)
        self._cmd_kd = np.empty(dof_num, dtype=np.float32)
        self._hw_sign = self._sign[self._hw_to_fw].astype(np.float32)
        self._hw_offset = self._offset[self._hw_to_fw].astype(np.float32)

        self._state_sub = self.create_subscription(
            RobotState,
            "/my_robot/state",
            self._state_callback,
            1,
        )
        self._command_pub = self.create_publisher(
            JointCommand,
            "/my_robot/joint_command",
            1,
        )

        package_share = Path(
            get_package_share_directory("bxi_example_py_elf3")
        )
        config = load_state_machine_config(
            package_share / "config/elf3_state_machine.yaml"
        )
        self.runtime = RobotControlRuntime(
            config,
            built_in_mod_root=package_share / "mods",
            dof_num=dof_num,
            ros_node=self,
            platform=self,
            logger=self.get_logger(),
            fatal_callback=self._on_control_fatal,
        )

    @staticmethod
    def _validate_joint_contract(
        framework_names: tuple[str, ...],
        hardware_names: tuple[str, ...],
    ) -> None:
        if len(set(framework_names)) != len(framework_names):
            raise ValueError("framework joint names contain duplicates")
        if len(set(hardware_names)) != len(hardware_names):
            raise ValueError("hardware joint names contain duplicates")
        if set(framework_names) != set(hardware_names):
            missing = sorted(set(framework_names) - set(hardware_names))
            extra = sorted(set(hardware_names) - set(framework_names))
            raise ValueError(
                f"joint sets differ: missing={missing}, extra={extra}"
            )

    def _state_callback(self, msg: RobotState) -> None:
        # 若消息把 IMU 分开发布，可拆成两个回调，但仍使用同一把锁。
        hw_q = np.asarray(msg.position, dtype=np.float64)
        hw_dq = np.asarray(msg.velocity, dtype=np.float64)
        if hw_q.shape != self._q.shape or hw_dq.shape != self._dq.shape:
            self.get_logger().error("robot state joint shape changed")
            return

        quat_xyzw = np.asarray(
            [msg.imu.x, msg.imu.y, msg.imu.z, msg.imu.w],
            dtype=np.float64,
        )
        norm = float(np.linalg.norm(quat_xyzw))
        if not np.isfinite(norm) or norm < 1.0e-6:
            self.get_logger().error("invalid IMU quaternion")
            return
        quat_xyzw /= norm

        now = time.monotonic()
        with self._lock:
            np.take(hw_q, self._fw_to_hw, out=self._q)
            np.multiply(self._q, self._sign, out=self._q)
            np.add(self._q, self._offset, out=self._q)
            np.take(hw_dq, self._fw_to_hw, out=self._dq)
            np.multiply(self._dq, self._sign, out=self._dq)
            self._quat_xyzw[:] = quat_xyzw
            self._quat_wxyz[:] = (
                quat_xyzw[3],
                quat_xyzw[0],
                quat_xyzw[1],
                quat_xyzw[2],
            )
            self._omega[:] = (
                msg.angular_velocity.x,
                msg.angular_velocity.y,
                msg.angular_velocity.z,
            )
            self._joint_received = True
            self._imu_received = True
            self._joint_update_at = now
            self._imu_update_at = now

    def set_velocity_command(self, vx: float, vy: float, yaw: float) -> None:
        """可由 ROS 回调、SDK 回调或自己的输入线程调用。"""

        with self._lock:
            self._raw_cmd_vel[:] = (vx, vy, yaw)

    def push_event(self, full_event_name: str) -> None:
        """只排队边沿事件，控制线程会在下一周期消费。"""

        with self._lock:
            self._events.append(full_event_name)

    # ---------------- ControlPlatformAdapter ----------------

    def startup_step(self, now: float) -> bool:
        # 这里不能 wait_for_service() 或 sleep()。
        with self._lock:
            ready = self._joint_received and self._imu_received
            joint_age = now - self._joint_update_at
            imu_age = now - self._imu_update_at

        if not ready:
            return False  # 尚未产生任何电机输出，硬件应保持安全模式。
        if joint_age > 0.10 or imu_age > 0.10:
            if not self._control_started:
                return False
            self._send_safe_command()
            raise RuntimeError(
                f"robot state stale: joint={joint_age:.3f}s, "
                f"imu={imu_age:.3f}s"
            )

        # 如果硬件需要异步上电/使能，在此轮询完成状态。
        self._control_started = True
        return True

    def snapshot_control_inputs(
        self,
    ) -> tuple[RobotObservation, tuple[str, ...]]:
        # 简单可靠的基线：在一把锁内复制一个一致快照。
        with self._lock:
            observation = RobotObservation(
                q=self._q.copy(),
                dq=self._dq.copy(),
                quat_xyzw=self._quat_xyzw.copy(),
                quat_wxyz=self._quat_wxyz.copy(),
                omega=self._omega.copy(),
                raw_cmd_vel=self._raw_cmd_vel.copy(),
            )
            events = tuple(self._events)
            self._events.clear()
        return observation, events

    def publish_motor_frame(self, frame: MotorFrame) -> None:
        if not (
            np.all(np.isfinite(frame.qpos))
            and np.all(np.isfinite(frame.kp))
            and np.all(np.isfinite(frame.kd))
        ):
            self._send_safe_command()
            raise RuntimeError("framework produced a non-finite motor frame")

        # frame: framework 顺序 -> hardware 顺序，并做角度逆变换。
        np.take(frame.qpos, self._hw_to_fw, out=self._cmd_q)
        np.subtract(self._cmd_q, self._hw_offset, out=self._cmd_q)
        np.multiply(self._cmd_q, self._hw_sign, out=self._cmd_q)
        np.take(frame.kp, self._hw_to_fw, out=self._cmd_kp)
        np.take(frame.kd, self._hw_to_fw, out=self._cmd_kd)

        # 在这里检查目标机器人自己的软/硬限位，不能依赖模型永远合法。
        msg = JointCommand()
        msg.position = self._cmd_q.tolist()
        msg.velocity = [0.0] * self._cmd_q.size
        msg.kp = self._cmd_kp.tolist()
        msg.kd = self._cmd_kd.tolist()
        msg.feedforward_torque = [0.0] * self._cmd_q.size
        self._command_pub.publish(msg)

    # ---------------- shutdown / fatal ----------------

    def _send_safe_command(self) -> None:
        # 替换为目标硬件明确支持的零力矩、阻尼或急停命令。
        pass

    def _on_control_fatal(self, message: str) -> None:
        self.get_logger().fatal(message)
        self._send_safe_command()
        self._stopping.set()
        rclpy.try_shutdown()

    def destroy_node(self) -> bool:
        if not self._stopping.is_set():
            self._stopping.set()
        runtime = getattr(self, "runtime", None)
        if runtime is not None:
            runtime.close()  # 先停控制线程，防止安全命令后又发普通帧。
        self._send_safe_command()
        return super().destroy_node()


def main() -> None:
    # 必须按实际策略契约列出完整名称，不能直接照抄示例名称。
    framework_joints = ("joint_a", "joint_b", "joint_c")
    hardware_joints = ("joint_c", "joint_a", "joint_b")
    sign = np.asarray([1.0, -1.0, 1.0])
    offset = np.asarray([0.0, 0.10, 0.0])

    rclpy.init()
    node = MyRobotAdapter(
        framework_joints,
        hardware_joints,
        sign,
        offset,
    )
    executor = MultiThreadedExecutor(num_threads=3)
    try:
        executor.add_node(node)
        node.runtime.attach_executor(executor)
        node.runtime.start()
        executor.spin()
    finally:
        node.destroy_node()
        executor.shutdown()
        rclpy.try_shutdown()

示例中的三关节名称只用于展示映射，不能直接配合当前 29 自由度内置 Mod。接入 ELF3 现有策略时，dof_num 和完整关节名称必须与模型及 Mod 契约一致。

接入已有程序而不是 ROS 话题

适配器方法不要求硬件 I/O 使用 ROS。在 snapshot_control_inputs() 中可以读取厂商 SDK 维护的共享状态，在 publish_motor_frame() 中可以调用 SDK 或写 CAN。当前框架仍需要一个 rclpy.node.Node 作为 Mod 节点、日志和高级 ROS 集成的宿主，因此推荐保留一个最小 Node，即使电机数据完全不经过 ROS。

如果要求进程内完全不依赖 ROS，当前版本不能只靠实现这三个方法做到；还需要把 ros_node、Mod 子节点和日志能力抽象为新的 host services 接口。这属于框架级移植，不能用一个假的 Node 冒充完成。没有这项改造前，不应把当前框架宣传为纯 ROS-free。

已有程序已经拥有硬实时循环时，可以直接驱动 RobotControlFramework.update()，但此时调用方必须自行承担绝对时间调度、Framework 锁、低频 maintenance_update()、Mod 子节点 Executor、deadline 统计、异常处理和完整关闭流程。除非现有主循环有明确的实时性要求，优先复用 RobotControlRuntime，避免复制这些已经统一实现的职责。

性能要求

回调中更新预分配数组；控制周期中不要加载文件或创建推理 Session。
关节索引、符号和零偏在启动时预计算，输出使用 np.take(..., out=...) 原地重排。
snapshot_control_inputs() 首先保证一致性。29 个关节的小数组复制通常远小于一次模型推理；只有 benchmark 证明它成为瓶颈后，才换成经过并发验证的多缓冲方案。
ROS Python 消息的 list 字段必然带来转换；需要更低延迟时，优先让硬件 SDK 暴露可复用的 NumPy/共享内存缓冲，而不是在控制周期中反复组装对象。
控制输出只能有一个所有者。禁止旧控制程序和 BXI 适配器同时向执行器发命令。

安全要求

首次连接真实硬件前至少实现：

关节、IMU 未就绪时不允许 startup_step() 返回 True。
对关节状态和 IMU 分别做时间戳超时检测。
检查输入和输出的 shape、NaN、Inf、四元数范数与关节限位。
硬件端设置通信看门狗；连续丢失一至两个控制周期后进入确定的安全模式。
明确零力矩、阻尼制动和位置保持分别使用哪个硬件命令。
退出时先停止 Runtime，确认不会再发布普通帧，再发送安全命令和释放电机。
初次测试使用吊架、低增益、低速度，并保留独立于上位机程序的物理急停。
对状态切换验证 qpos/kp/kd 连续性，不能只验证稳定行走阶段。

不要用“最后一帧保持”代替看门狗。程序崩溃、网线断开或调度线程卡死时，只有硬件或下位机看门狗仍然可靠。

分阶段验收

阶段	电机状态	必须验证
1. 单元测试	不连接	名称映射、正负号、零点和逆变换可往返
2. 数据旁路	断使能	观测 shape、单位、频率、时间戳和四元数方向
3. 命令观察	断使能	`MotorFrame` 转换后的目标角、增益和限位
4. 吊架静止	低增益	初始状态、PD brake、急停、断流看门狗
5. 吊架运动	限速	每个关节方向、状态进入/退出和 Transition
6. 落地低速	保守 profile	指令方向、推理耗时和 deadline miss
7. 长时间运行	逐步放开	温度、丢包、重启、资源释放和故障恢复

建议为映射写一个往返测试：随机生成合法的框架关节角，转换到硬件顺序后再转回来，结果应在浮点误差内完全一致。再用逐关节小幅正方向命令确认物理方向，不能只凭名称判断。

Robot Platform Integration

将 BXI 运控框架接入自己的机器人程序

先看清边界

平台适配器只有三个必需方法

输入契约：RobotObservation

输出契约：MotorFrame

关节名称、顺序、方向和零点

推荐实现：保留框架调度器

接入已有程序而不是 ROS 话题

性能要求

安全要求

分阶段验收

最终检查清单

相关文档

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

快速开始

Mod 与状态开发

模型、推理与真机

输入系统

平台接入与控制时序

配置与 API 参考

框架架构与维护

Clone this wiki locally

输入契约：`RobotObservation`

输出契约：`MotorFrame`