RobotInterface 是连接高层控制策略与底层物理硬件的统一门户。它以单一 C++ 类形态封装了电机驱动阵列与 IMU 传感器的完整生命周期，向上提供与总线协议无关的关节状态读取和动作下发接口，向下通过工厂方法聚合异构电机与 IMU 的具体实现。对于强化学习推理节点或 Python 脚本而言，开发者无需关心 CAN/CANFD 报文打包、串口波特率或多总线并行调度，只需面向统一配置与标准向量接口进行开发。

Sources: robot_interface.hpp, robot_interface.cpp

架构定位与职责边界

在系统分层中，RobotInterface 处于硬件抽象层的核心位置。其上层直接对接 推理节点设计与线程模型 中的 InferenceNode 或 Python SDK，下层则委托 电机驱动模块与 MIT 控制 的 MotorDriver 子类以及 IMU 驱动与姿态获取 的 IMUDriver 子类完成实际硬件交互。RobotInterface 本身不实现任何通信协议细节，而是通过组合方式持有多个 std::shared_ptr<MotorDriver> 与单个 std::shared_ptr<IMUDriver>，将策略层的关节空间指令翻译为总线层的物理设备命令。此外，它还内聚了 Decouple 解耦器与 ThreadPool 线程池，分别处理闭链踝关节的运动学映射和多总线并行下发。

Sources: robot_interface.hpp

核心组件与依赖关系

以下类图展示了 RobotInterface 的静态结构及其与周边模块的依赖关系。RobotInterface 通过工厂函数 MotorDriver::create_motor 与 IMUDriver::create_imu 完成具体驱动实例的创建，因此天然支持接入不同品牌、不同总线类型的电机与 IMU，而自身代码无需修改。

classDiagram
    class RobotInterface {
        +apply_action(action)
        +get_joint_q()
        +get_joint_vel()
        +get_joint_tau()
        +get_quat()
        +get_ang_vel()
        +init_motors()
        +deinit_motors()
        +reset_joints(joint_default_angle)
        +set_zeros()
        +clear_errors()
        +refresh_joints()
        -setup_motors()
        -setup_imu()
        -exec_motors_parallel(cmd_func)
    }
    class MotorDriver {
        <<abstract>>
        +motor_mit_cmd()*
        +refresh_motor_status()*
        +get_motor_pos()*
        +get_motor_spd()*
        +get_motor_current()*
    }
    class IMUDriver {
        <<abstract>>
        +get_quat()*
        +get_ang_vel()*
    }
    class Decouple {
        <<abstract>>
        +get_forwardQVT()*
        +get_decoupleQVT()*
    }
    class ThreadPool {
        +run_parallel(tasks)
    }
    RobotInterface --> MotorDriver : 持有 N 个
    RobotInterface --> IMUDriver : 持有 1 个
    RobotInterface --> Decouple : 持有 1 个
    RobotInterface --> ThreadPool : 持有 1 个

图中 MotorDriver 与 IMUDriver 均为抽象基类，底层通信协议（如 CAN、CANFD、串口）由各自的实现类封装，相关细节可参阅 通信协议层：CAN/CANFD 与串口。Decouple 负责将踝关节的电机空间状态映射到关节空间，其数学原理在 闭链运动学与踝关节解耦 中有专门论述。

Sources: robot_interface.hpp, motor_driver.hpp, imu_driver.hpp, close_chain_mapping.hpp

配置驱动初始化

RobotInterface 采用构造函数注入配置的方式完成初始化，配置来源为单个 YAML 文件（如 config/robot.yaml）。构造函数内部依次解析 imu、motors、robot 三大配置域，分别填充 IMUCfg、MotorsCfg、RobotCfg 三个结构体，随后调用 setup_imu() 与 setup_motors() 完成驱动实例化。解析完成后，再根据 motors_cfg_->motor_interface_.size() 创建对应数量的线程池工作者线程，确保每个物理总线对应一个独立的发送线程。

Sources: robot_interface.cpp

下表汇总了 robot.yaml 中各配置域的关键字段及其在 RobotInterface 中的用途。

Sources: robot_interface.hpp, robot.yaml

状态读取与坐标变换

RobotInterface 对外暴露五组无阻塞状态接口：get_joint_q()、get_joint_vel()、get_joint_tau()、get_quat() 与 get_ang_vel()。前三者读取电机反馈的位置、速度与等效扭矩，在返回前通过 joint_mutex_ 加锁以保证与 apply_action 内部的写操作互斥；若电机尚未初始化（is_init_ 为假），则抛出 std::runtime_error。后两者读取 IMU 姿态与角速度，并在类内部完成外参坐标变换：get_quat() 将 IMU 原始四元数（格式为 w, x, y, z）左乘由 extrinsic_R 计算得到的逆旋转四元数 extrinsic_q_inv_，从而将 IMU 坐标系下的姿态转换到机体坐标系；get_ang_vel() 则使用 extrinsic_R_mat_ 对标量角速度做同样的刚性旋转。默认配置中 extrinsic_R 为单位矩阵，表示 IMU 与机体坐标系重合。

Sources: robot_interface.hpp

动作执行流水线

apply_action(std::vector<float> action) 是 RobotInterface 最核心的控制周期函数，承载了读状态 → 解耦计算 → 写命令的完整闭环。其内部逻辑可划分为两大阶段：第一阶段在 joint_mutex_ 保护下并行读取所有电机状态，进行方向修正和闭链解耦；第二阶段在 motors_mutex_ 保护下将计算后的目标值写入各电机。下图为该流程的详细分解：

flowchart TD
    A[apply_action 调用] --> B{is_init_?}
    B -->|否| C[直接返回]
    B -->|是| D[加锁 joint_mutex_]
    D --> E[exec_motors_parallel<br/>并行读取 pos / spd / current]
    E --> F{response_count > offline_threshold_?}
    F -->|是| G[抛出 runtime_error<br/>标记电机离线]
    F -->|否| H[应用 motor_sign 方向修正]
    H --> I[通过 motor2urdf_ 做索引映射]
    I --> J{存在闭链关节?}
    J -->|是| K[正运动学解耦<br/>电机空间 → 关节空间]
    K --> L[关节空间 PD 计算]
    L --> M[逆解耦<br/>关节空间 → 电机空间]
    J -->|否| N[action 作为目标位置直接传递]
    N --> O[加锁 motors_mutex_<br/>写入 motor_target_]
    M --> O
    O --> P{接口类型?}
    P -->|CANFD| Q[按总线批量打包<br/>8 电机一组 MIT 数组指令]
    P -->|CAN| R[逐个电机下发 MIT 单条指令]
    Q --> S[ThreadPool 并行发送到各总线]
    R --> S
    S --> T[返回]

上述流程中，若电机接口类型为 canfd，系统会利用数组形式的重载 motor_mit_cmd(float* pos, float* vel, ...) 将同一条总线上的最多 8 个电机参数打包为单条报文，从而显著降低总线负载；若为传统 can，则对每个电机单独调用标量版 motor_mit_cmd。对于闭链踝关节（如 atom01 机型中的 5/6 号与 11/12 号电机），目标动作首先在关节空间计算 PD 力矩，再通过 Decouple::get_decoupleQVT 映射回电机力矩指令，确保物理约束始终被满足。

Sources: robot_interface.cpp

并发模型与线程安全

RobotInterface 的并发设计遵循总线级并行、指令级串行的原则。其内部持有一个 ThreadPool，线程数量等于物理总线数量（如 can0、can1、can2、can3 对应 4 个线程）。exec_motors_parallel 模板函数将所有电机按总线分组，每组生成一个 std::function<void()> 任务提交到线程池，最后通过 future.wait() 同步等待全部任务完成。线程池中的每个工作线程均设置了 SCHED_FIFO 实时调度策略与优先级 70，以满足毫秒级控制周期的确定性要求。

Sources: thread_pool.hpp, robot_interface.cpp

类内部使用两把细粒度互斥锁分离不同临界区：joint_mutex_ 保护关节状态缓存（joint_q_、joint_vel_、joint_tau_），motors_mutex_ 保护电机指令缓存（motor_target_）与底层驱动访问。这种分离使得状态读取与指令组装可以在逻辑上并发执行，仅在必要时串行化。需要注意的是，apply_action 本身并非原子操作，外部调用者（如 InferenceNode 的 control 线程）需保证控制频率与推理频率的解耦，避免在单周期内重复调用导致不可预期的总线竞争。

Sources: robot_interface.hpp, robot_interface.cpp

电机生命周期管理

除高频控制接口外，RobotInterface 还提供了一组电机生命周期管理方法，统一封装了底层驱动的初始化、反初始化、零点标定、错误清除与状态刷新操作。init_motors() 与 deinit_motors() 分别置位和复位原子标志 is_init_，用于控制 apply_action 是否生效。reset_joints() 采用分段 PD 策略：先以半增益将关节牵引至默认角度，停顿 1 秒后切换至全增益锁位，降低启动冲击。set_zeros() 与 clear_errors() 则通过 exec_motors_parallel 并行下发到所有电机，实现一次性全轴零点固化与故障复位。refresh_joints() 用于手动触发一次全量状态读取与闭链解耦更新，常用于标定或诊断场景。

Sources: robot_interface.cpp, robot_interface.cpp

索引映射与闭链解耦

机器人物理电机的编号、URDF 模型中的关节顺序、以及策略网络输出的动作索引往往并不一致。RobotInterface 在构造阶段建立两层映射表：urdf2motor_ 表示第 i 个 URDF 关节对应哪个物理电机，motor2urdf_ 则为其逆映射。所有从电机读取的状态均按 motor2urdf_ 重排后存入关节缓存；所有下发的动作则按 urdf2motor_ 反向寻址后写入 motor_target_。此外，motor_sign_ 数组对每轴施加 ±1 方向修正，补偿电机安装方向与 URDF 正方向不一致的情况。

Sources: robot_interface.cpp, robot.yaml

对于具备闭链踝关节的机型（如 type: atom01），构造函数还会解析 close_chain_motor_id，将涉及电机索引记录到 close_chain_motor_idx_，并将其对应的 URDF 关节索引记录到 close_chain_joint_idx_。在 apply_action 与 reset_joints 中，这些索引被用于触发 Decouple 的正/逆解耦计算。具体而言，get_forwardQVT 将两个耦合电机的编码器读数转换为独立的 roll/pitch 关节空间状态；get_decoupleQVT 则将关节空间计算出的目标力矩重新分配回两个耦合电机。该机制确保了上层策略只需在解耦后的关节空间输出动作，无需感知底层闭链的物理耦合关系。

Sources: robot_interface.cpp, decouple_atom01.hpp

异常处理与安全防护

RobotInterface 在多处设置了运行时安全检查。在 apply_action 的状态读取阶段，若任一电机的连续未响应计数 get_response_count() 超过阈值 offline_threshold_（默认为 25），则立即抛出 std::runtime_error，强制终止当前控制周期。该机制通常意味着电机掉线、总线故障或电源异常，需要人工介入排查。此外，所有状态读取接口在 is_init_ 为假时均会抛出异常，防止在未初始化状态下访问无效内存。析构函数按顺序调用 deinit_motors()、清空电机列表、释放 IMU 实例，确保对象生命周期结束时硬件进入安全态。

Sources: robot_interface.cpp, robot_interface.hpp

上层集成方式

在 ROS2 推理节点中，InferenceNode 在构造阶段以 std::make_shared<RobotInterface>(config_path) 完成硬件抽象层实例化，随后在独立的 control 实时线程中以固定周期调用 robot_->apply_action(last_act_)，而在 inference 线程中通过 robot_->get_joint_q() 等接口采集观测向量。两者之间通过 act_mutex_ 与动作平滑系数 act_alpha_ 实现生产者-消费者解耦。与此同时，pybind_module.cpp 将 RobotInterface 完整暴露给 Python，使得开发者可在不编译 C++ 的情况下直接通过 robot_py.RobotInterface 进行硬件调试、标定与数据采集。

Sources: inference_node.hpp, inference_node.cpp, pybind_module.cpp

延伸阅读

理解 RobotInterface 的工作机制后，建议按以下路径深入相关模块：

• 若需了解电机驱动基类的 MIT 控制接口与不同电机品牌的实现差异，请参阅 电机驱动模块与 MIT 控制。
• 若需了解 IMU 数据获取、四元数滤波与串口通信细节，请参阅 IMU 驱动与姿态获取。
• 若需了解 CAN/CANFD 报文格式、总线配置与 One-to-Many 批量指令的物理层实现，请参阅 通信协议层：CAN/CANFD 与串口。
• 若需了解踝关节闭链机构的几何参数、雅可比矩阵与正逆解耦算法，请参阅 闭链运动学与踝关节解耦。
• 若需了解 InferenceNode 的线程优先级设置、推理周期与控制周期的解耦策略，请参阅 推理节点设计与线程模型。