Robot Platform Integration

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

本文面向希望复用 BXI 的状态机、Mod、Transition、推理和控制调度，但使用自己的 ROS 消息、厂商 SDK、共享内存或 CAN 总线的开发者。

当前 bxi_example_py_elf3/bxi_example_demo.py 只是 ELF3 的 ROS 话题适配层：它采集 ELF3 状态，整理成框架观测，再把框架输出转换成 ELF3 电机命令。它不是运控框架本体。 迁移时应新写一个同等级的平台适配器，不要复制和修改 framework/runtime。

需要保留和替换什么

自己的机器人状态 / IMU / 操控输入
  -> 自己的平台适配器
       startup_step(now)
       snapshot_control_inputs() -> RobotObservation + events
       publish_motor_frame(MotorFrame)
  -> RobotControlRuntime
       ControlScheduler
       RobotControlFramework
       Mod / State / Transition / Inference
  -> 自己的平台适配器
  -> ROS 消息 / SDK / CAN / 下位机

保留	替换或确认
framework/ 下的通用运控框架	状态订阅、SDK 读取和电机发送
RobotControlRuntime 和控制调度	机器人关节布局、硬件顺序和标定
适用于新机器人的 Mod 与状态图	IMU 坐标系、四元数方向和单位
可复用的策略与推理后端	模型是否真的适配目标机构
MotorFrame 完整 PD 目标契约	上电、使能、看门狗和故障停机

消息转换不能让为另一种机构训练的模型安全工作。只要关节拓扑、默认姿态、传感器语义或 控制频率改变，就必须重新确认策略契约，必要时重新训练模型和调整 Mod 参数。

三个关节布局不要混为一谈

接入新机器人时至少要区分：

State Layout      机器人实际返回的全部状态关节，例如 31 个
Control Layout    BXI 状态机本次负责控制的关节，例如 29 个
Hardware Layout   无名称硬件接口规定的固定数组顺序，可选

策略还会在类中声明自己的 observation/action layout。它们不要求与上述布局顺序一致： JointPolicy 首次绑定时按名称编译索引，之后热路径只使用缓存映射。如果输入对象与策略 布局是同一个 JointLayout，则直接走零复制路径。

可以在新适配器源码里直接声明目标机器人的布局：

from bxi_example_py_elf3.framework.joints import JointLayout


MY_CONTROL_JOINTS = JointLayout(
    (
        "left_hip_pitch",
        "left_knee_pitch",
        "right_hip_pitch",
        "right_knee_pitch",
    ),
    label="my robot control",
)

MY_STATE_JOINTS = JointLayout(
    MY_CONTROL_JOINTS.names + ("left_gripper", "right_gripper"),
    label="my robot full state",
)

这里状态有 6 个关节，运控框架只控制其中 4 个。Framework 会从状态布局中按名称取出 Control Layout，不要求输入数组先手工裁成 4 维。额外的夹爪必须由另一个明确的控制器 负责；框架不会为它们静默补零或沿用上一帧。

平台适配器的三个必需方法

RobotControlRuntime 只通过 ControlPlatformAdapter 接触平台：

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)	按 frame.layout 的名称语义转换并立即发送完整命令

这三个方法都运行在控制线程。不能在其中等待 service、睡眠、读取磁盘、创建模型 Session 或大量输出日志；这些耗时会直接占用控制周期。

输入契约：RobotObservation

当前接口为：

RobotObservation(
    joints=JointStateView(layout, position, velocity),
    quat_xyzw=quat_xyzw,
    quat_wxyz=quat_wxyz,
    omega=omega,
    raw_cmd_vel=raw_cmd_vel,
)

字段	约定
joints.position	joints.layout 顺序，弧度，浮点一维数组
joints.velocity	同一顺序，弧度/秒
quat_xyzw	(x, y, z, w)，shape (4,)
quat_wxyz	同一物理姿态的 (w, x, y, z)
omega	策略约定机身坐标系的角速度，shape (3,)
raw_cmd_vel	[vx, vy,yaw_rate]，shape (3,)，之后再经 speed profile

单位和坐标系不会被框架猜测或自动修正。必须确认 IMU 给出的是 world -> body 还是 body -> world，并处理固定安装旋转。四元数必须有限、归一化且不能是全零值。

事件使用完整名称，例如 com.customer.remote/stand。订阅回调只负责把边沿事件压入 队列；状态机只在控制线程中推进，不能在传感器回调里直接切状态。

输出契约：MotorFrame

框架可能在一个周期返回：

frame.layout  # 本帧完整关节布局
frame.qpos   # float32, shape=(frame.layout.dof_num,)
frame.kp     # float32, 同一布局
frame.kd     # float32, 同一布局

MotorFrame 是完整的关节 PD 目标，不是硬件消息，也不是力矩。ELF3 将它解释为：

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

只有目标位置的硬件可以只发送 qpos，但这样会丢失 gain ramp 等语义，必须重新验证。 纯力矩执行器应在可靠的底层伺服环中实现 PD、饱和和看门狗，不能把 qpos 当成力矩。

推荐路径：上下游消息都携带关节名

sensor_msgs/JointState 一类消息自带名称，因此消息中的数组顺序可以变化。使用 NamedJointStateSource 后，名称映射只在第一次收到消息或名称顺序变化时重新编译。

下面是一个可以按目标消息字段改造的 ROS 2 骨架。为避免回调和控制线程同时访问同一 数组，示例把“最新回调缓冲”和“本周期快照缓冲”分开；所有对象均长期复用。

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 sensor_msgs.msg import Imu, JointState

from my_robot_msgs.msg import NamedJointCommand  # 替换成自己的消息

from bxi_example_py_elf3.framework.joints import JointLayout, JointStateBuffer
from bxi_example_py_elf3.framework.mod_api import MotorFrame
from bxi_example_py_elf3.framework.platform import (
    NamedJointStateSource,
    RobotControlRuntime,
    RobotObservation,
)
from bxi_example_py_elf3.framework.runtime.state_machine import (
    load_state_machine_config,
)


MY_CONTROL_JOINTS = JointLayout(
    ("joint_a", "joint_b", "joint_c"),
    label="my robot control",
)
MY_STATE_JOINTS = JointLayout(
    ("joint_a", "joint_b", "joint_c", "gripper_joint"),
    label="my robot state",
)


class MyRobotAdapter(Node):
    def __init__(self) -> None:
        super().__init__("my_bxi_robot_adapter")
        self._lock = Lock()
        self._stopping = Event()
        self._events: deque[str] = deque()

        # 回调写入区：消息名称顺序变化时自动重新编译映射。
        self._latest_joints = NamedJointStateSource(MY_STATE_JOINTS)
        self._joint_received = False
        self._imu_received = False
        self._joint_update_at = 0.0
        self._imu_update_at = 0.0
        self._latest_quat_xyzw = np.zeros(4, dtype=np.float64)
        self._latest_quat_wxyz = np.zeros(4, dtype=np.float64)
        self._latest_omega = np.zeros(3, dtype=np.float64)
        self._latest_command = np.zeros(3, dtype=np.float32)

        # 控制周期快照区：对象和数组身份从启动到关闭保持不变。
        self._snapshot_joints = JointStateBuffer(MY_STATE_JOINTS)
        self._snapshot_quat_xyzw = np.zeros(4, dtype=np.float64)
        self._snapshot_quat_wxyz = np.zeros(4, dtype=np.float64)
        self._snapshot_omega = np.zeros(3, dtype=np.float64)
        self._snapshot_command = np.zeros(3, dtype=np.float32)
        self._observation = RobotObservation(
            joints=self._snapshot_joints.view,
            quat_xyzw=self._snapshot_quat_xyzw,
            quat_wxyz=self._snapshot_quat_wxyz,
            omega=self._snapshot_omega,
            raw_cmd_vel=self._snapshot_command,
        )

        self._joint_sub = self.create_subscription(
            JointState, "/my_robot/joint_states", self._joint_callback, 1
        )
        self._imu_sub = self.create_subscription(
            Imu, "/my_robot/imu", self._imu_callback, 1
        )
        self._command_pub = self.create_publisher(
            NamedJointCommand, "/my_robot/joint_command", 1
        )

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

    def _joint_callback(self, msg: JointState) -> None:
        now = time.monotonic()
        try:
            with self._lock:
                self._latest_joints.update(
                    msg.name,
                    msg.position,
                    msg.velocity,
                    timestamp_ns=time.monotonic_ns(),
                )
                self._joint_received = True
                self._joint_update_at = now
        except (TypeError, ValueError) as exc:
            self.get_logger().error(f"invalid joint state: {exc}")

    def _imu_callback(self, msg: Imu) -> None:
        q = msg.orientation
        norm = float(np.sqrt(q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w))
        if not np.isfinite(norm) or norm < 1.0e-6:
            self.get_logger().error("invalid IMU quaternion")
            return
        inv_norm = 1.0 / norm
        w = msg.angular_velocity
        now = time.monotonic()
        with self._lock:
            self._latest_quat_xyzw[:] = (
                q.x * inv_norm, q.y * inv_norm, q.z * inv_norm, q.w * inv_norm
            )
            self._latest_quat_wxyz[:] = (
                q.w * inv_norm, q.x * inv_norm, q.y * inv_norm, q.z * inv_norm
            )
            self._latest_omega[:] = (w.x, w.y, w.z)
            self._imu_received = True
            self._imu_update_at = now

    def set_velocity_command(self, vx: float, vy: float, yaw: float) -> None:
        with self._lock:
            self._latest_command[:] = (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:
        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:
            self._send_safe_command()
            raise RuntimeError(
                f"robot state stale: joint={joint_age:.3f}s, imu={imu_age:.3f}s"
            )
        # 在此轮询异步完成的上电/使能状态，不能阻塞等待。
        return True

    def snapshot_control_inputs(
        self,
    ) -> tuple[RobotObservation, tuple[str, ...]]:
        with self._lock:
            latest = self._latest_joints.view
            self._snapshot_joints.update(
                latest.position,
                latest.velocity,
                timestamp_ns=latest.timestamp_ns,
            )
            np.copyto(self._snapshot_quat_xyzw, self._latest_quat_xyzw)
            np.copyto(self._snapshot_quat_wxyz, self._latest_quat_wxyz)
            np.copyto(self._snapshot_omega, self._latest_omega)
            np.copyto(self._snapshot_command, self._latest_command)
            events = tuple(self._events)
            self._events.clear()
        return self._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")

        msg = NamedJointCommand()
        msg.name = frame.layout.names
        msg.position = frame.qpos.tolist()
        msg.kp = frame.kp.tolist()
        msg.kd = frame.kd.tolist()
        self._command_pub.publish(msg)

    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:
        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:
    rclpy.init()
    node = MyRobotAdapter()
    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()

上例为了展示接线仍加载 ELF3 状态图。真正的 3 关节机器人不能直接运行 ELF3 的 29 关节 策略；必须换成与 MY_CONTROL_JOINTS 匹配的 Mod、参数和模型。

固定数组硬件：显式重排和标定

有些 SDK 或 CAN 协议只有数组，不携带关节名。这时必须声明 Hardware Layout，并在启动 时编译一次映射。不能因为当前数组“看起来顺序一样”就省略契约。

import numpy as np

from bxi_example_py_elf3.framework.joints import (
    CompiledJointMap,
    JointCalibration,
    JointLayout,
)


HARDWARE_JOINTS = JointLayout(
    ("joint_c", "joint_a", "joint_b"),
    label="vendor fixed order",
)

# source=框架输出，target=硬件顺序；只允许完全相同的关节集合。
control_to_hardware = CompiledJointMap.compile(
    MY_CONTROL_JOINTS,
    HARDWARE_JOINTS,
    require_exact=True,
)
calibration = JointCalibration(
    HARDWARE_JOINTS,
    direction=np.asarray([1.0, -1.0, 1.0]),
    zero_offset=np.asarray([0.0, 0.10, 0.0]),
)

semantic_q = np.empty(HARDWARE_JOINTS.dof_num, dtype=np.float32)
hardware_q = np.empty(HARDWARE_JOINTS.dof_num, dtype=np.float32)
hardware_kp = np.empty(HARDWARE_JOINTS.dof_num, dtype=np.float32)
hardware_kd = np.empty(HARDWARE_JOINTS.dof_num, dtype=np.float32)


def publish_motor_frame(frame):
    if frame.layout != MY_CONTROL_JOINTS:
        raise ValueError("unexpected control layout")
    control_to_hardware.map_into(frame.qpos, semantic_q)
    calibration.position_to_hardware_into(semantic_q, hardware_q)
    control_to_hardware.map_into(frame.kp, hardware_kp)
    control_to_hardware.map_into(frame.kd, hardware_kd)
    vendor_sdk.send_pd(hardware_q, hardware_kp, hardware_kd)

输入做严格逆过程：先将硬件固定顺序数组标定到语义坐标，再以 JointStateView(HARDWARE_JOINTS, ...) 交给 Framework。Framework 会继续按名称映射到 Control Layout。direction 只能是 +1/-1；zero_offset 和位置使用同一角度单位。

如果直接处理 PolicyOutput.joints 而不是最终 MotorFrame，可使用：

• NamedJointCommandEncoder：名称消息，默认拒绝不完整目标。
• FixedOrderJointCommandEncoder：固定顺序消息，缓存索引和输出缓冲。
• ExactJointTargetAssembler：策略动作关节集合完整、只有顺序不同。
• PartialJointTargetAssembler：策略只控制子集，同时调用者显式提供完整 fallback。

29 关节策略与更多关节机器人

31 关节状态输入 29 关节策略是允许的，只要 source layout 包含策略 observation layout。 策略自动选择并排列自己需要的关节。

当前 ELF3 适配层会读取 ActuatorStates.name，以首个合法消息的完整名称集合建立 State Layout；后续即使消息顺序变化，也会按名称写入同一稳定缓冲。它不会用消息布局改写策略 契约。ELF3_POLICY_JOINTS 固定描述现有模型的 29 维输入输出， ELF3_CONTROL_JOINTS 则描述状态机当前拥有的控制关节。

29 关节动作不能自动成为 31 关节完整控制命令，因为剩余两个关节由谁控制并不明确：

• 若下游消息支持按名称发送局部目标，可显式允许 partial，并只发送这 29 个名称。
• 若硬件要求 31 维完整数组，必须用 PartialJointTargetAssembler 提供明确的完整 fallback。
• 若状态机的 Control Layout 就是 29 个关节，则夹爪等额外执行器由另一个控制所有者负责。

框架不会默认补零、默认保持上帧或忽略额外关节。这些行为都属于控制策略，而不是数据 格式细节。

接入已有非 ROS 程序

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

如果必须完全移除 ROS，还需要把 ros_node、Mod 子节点和日志抽象成 host services；这 是框架级改造，不能用假的 Node 掩盖。已有程序自己维护硬实时循环时也可以直接调用 RobotControlFramework.update()，但调用方必须承担绝对时间调度、Framework 锁、低频 maintenance_update()、Mod Executor、deadline 统计、异常处理和关闭顺序。一般优先复用 RobotControlRuntime。

性能和线程安全

1. 在构造阶段创建 Layout、名称映射、标定和所有 NumPy 输出缓冲。
2. 回调只原地更新“最新值”；控制周期在同一把锁内复制到独立稳定快照。
3. 不让 Framework 持有随后会被订阅回调并发改写的数组。
4. 不在控制周期调用 list.index()、创建 Session、读取模型或深拷贝整套对象。
5. 固定顺序重排使用 np.take(..., out=...)；数值变换使用 ufunc 的 out=。
6. ROS Python 的变长数组字段通常仍需 .tolist()；需要更低延迟时优先使用共享内存或 厂商 SDK 暴露的稳定缓冲。
7. 电机输出只能有一个所有者，不能让旧控制程序与 BXI 同时发送命令。

29 或 31 个关节的单次数组复制通常远小于一次神经网络推理。先保证并发一致性，再通过 benchmark 判断是否需要双缓冲或无锁快照，不要凭感觉移除锁。

安全检查

• 对输入检查 shape、有限值、时间戳和断流超时。
• 对输出检查有限值、目标速度、位置软限位、增益范围和发送结果。
• 上电后先验证零力矩、阻尼、急停、断流看门狗和进程崩溃保护。
• 看门狗必须位于下位机或硬件侧；“保持最后一帧”不能代替看门狗。
• 真机首次验证使用吊架、低增益、低速度，并保留独立物理急停。
• 逐关节验证正方向和零点，不要只依据关节名称判断。

分阶段验收

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

最终检查清单

• State、Control、Policy 和 Hardware Layout 的责任清晰。
• 所有布局无重复名；缺失关节在启动或首次绑定时立即失败。
• 额外关节和局部动作有明确控制所有者，不依赖静默补值。
• 关节方向、零点、单位和输出逆变换经过往返验证。
• 两种四元数顺序表达同一个姿态，IMU 坐标系与训练一致。
• RobotObservation 和快照数组长期复用，且不会被回调并发修改。
• MotorFrame.layout 在发布边界被检查或随名称消息一起发送。
• 状态断流、控制超时、输出越界和程序退出都会进入硬件安全状态。
• 真机前已经在目标平台运行推理与控制 benchmark。

相关文档

• 架构总览
• 推理框架与具名关节架构
• 框架控制调度
• Sim2Real