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 参数。

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

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

Robot Layout      机器人实际返回并最终接收命令的全部关节，例如 31 个
Policy Layout     每个模型在类中声明的 observation/action 关节，例如 29 个
Hardware Layout   无名称硬件接口规定的固定数组顺序，可选

六种布局、29↔31/N 双向映射、Defaults、输出裁剪和 Transition 规则集中说明在 关节布局与映射。

具名状态平台不需要在代码里重复声明 Robot Layout。NamedJointStateSource() 以首个合法 消息的完整名称集合建立它；Framework 首次收到快照时锁定该布局。策略布局不要求与 Robot Layout 等长或同序：JointPolicy 首次绑定时按名称编译索引，之后只使用缓存映射。

平台只需要声明旧策略没有输出的关节应收到什么安全目标：

from bxi_example_py_elf3.framework.joints import (
    JointCommandDefaults,
    JointDefault,
)


MY_COMMAND_DEFAULTS = JointCommandDefaults(
    {
        "left_gripper": JointDefault(position=0.0, kp=20.0, kd=0.5),
        "right_gripper": JointDefault(position=0.0, kp=20.0, kd=0.5),
    }
)

例如 Robot Layout 有 31 个关节而旧模型只输出 29 个，解析器会按名称覆盖 29 个模型目标， 再用这里的两个默认目标形成完整 31 维命令。若默认项缺失，首次绑定会直接失败。反方向 部署时，如果模型输出的关节多于机器人，解析器会在首次绑定该模型布局时发出一次 warning， 按名称只保留当前机器人实际存在的关节；后续控制周期使用缓存映射，不再重复报警或查名称。 新 31 关节模型输出到完整 31 关节机器人时不会读取默认项，也不会发生裁剪。

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

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 或大量输出日志；这些耗时会直接占用控制周期。

平台适配器也不应给整个主进程添加 taskset。control_runtime.cpu_affinity: control 会按本机 topology 只绑定控制线程；Mod 独立进程默认使用 shared，需要重计算时在节点 清单声明 scheduling.cpu_affinity: compute。所有平台共用这些用途名称，具体 CPU 编号由框架在 cgroup 允许集合内解析，详见框架控制调度。

输入契约：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 (
    JointCommandDefaults,
    JointDefault,
    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_COMMAND_DEFAULTS = JointCommandDefaults(
    {
        "gripper_joint": JointDefault(position=0.0, kp=15.0, kd=0.4),
    }
)


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()
        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 | None = None
        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 | None = None

        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",
            command_defaults=MY_COMMAND_DEFAULTS,
            ros_node=self,
            platform=self,
            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
            if self._snapshot_joints is None:
                self._snapshot_joints = JointStateBuffer(latest.layout)
                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._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()
        assert self._observation is not None
        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 状态图。关节名称或机构不兼容的机器人不能直接运行 ELF3 策略；必须换成与自身 Policy Layout 匹配的 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",
)
ROBOT_JOINTS = JointLayout(
    ("joint_a", "joint_b", "joint_c"),
    label="robot semantic order",
)

# source=框架输出，target=硬件顺序；只允许完全相同的关节集合。
robot_to_hardware = CompiledJointMap.compile(
    ROBOT_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.names != ROBOT_JOINTS.names:
        raise ValueError("unexpected robot layout")
    robot_to_hardware.map_into(frame.qpos, semantic_q)
    calibration.position_to_hardware_into(semantic_q, hardware_q)
    robot_to_hardware.map_into(frame.kp, hardware_kp)
    robot_to_hardware.map_into(frame.kd, hardware_kd)
    vendor_sdk.send_pd(hardware_q, hardware_kp, hardware_kd)

输入做严格逆过程：先将硬件固定顺序数组标定并重排到 ROBOT_JOINTS，再以 JointStateView(ROBOT_JOINTS, ...) 交给 Framework。Framework 会继续按名称映射到各策略 的 observation 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；后续即使消息顺序变化，也会按名称写入同一稳定缓冲。它不会用消息布局改写策略 契约。policies/joints.py 中的 ELF3_POLICY_JOINTS 固定描述现有模型的 29 维输入 输出；Robot Layout 则保持消息中的完整 N 个关节。

每个状态产生自己的自然 MotorFrame.layout。旧 29 关节状态通过 JointCommandResolver 与平台的 JointCommandDefaults 合成完整 31 维目标；新 31 关节 状态直接覆盖完整布局；任意子集模型也使用同一机制。默认目标按关节名声明，缺一项就失败， 不会默认补零或保持上帧。

Transition 在插值前分别把两端解析到完整 Robot Layout，所以不同关节数的状态可以安全 互相切换。最终传给 publish_motor_frame() 的始终是完整 Robot Layout；如果下游名称消息 本身支持局部目标，也不要在平台边界擅自丢弃框架已补齐的关节。

接入已有非 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=...)；命令补齐使用预编译索引和复用缓冲。
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. 长时间运行	逐步放开	温度、丢包、重启、资源释放和故障恢复

最终检查清单

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

相关文档

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