RobotInterface 是推理系统与物理硬件之间的单一抽象边界，它将电机驱动、IMU 采集、闭环运动学解耦和总线并行优化封装在一个无状态的配置驱动类中。对于上层 InferenceNode 而言，RobotInterface 提供了完全统一的“读关节状态 → 写动作指令”接口；对于底层硬件而言，它通过 YAML 配置自描述不同机型的电机总线拓扑、IMU 外参和踝关节四连杆参数，从而实现同一套二进制适配多种机器人构型。

Sources: robot_interface.hpp, robot_interface.cpp

架构定位与依赖关系

RobotInterface 不依赖 ROS 2，它是一个纯 C++ 的硬件抽象层，被 InferenceNode 以 std::shared_ptr 持有。其内部通过工厂方法聚合了三类底层能力：电机驱动 (MotorDriver)、IMU 驱动 (IMUDriver) 和闭环解耦器 (Decouple)，并借助 ThreadPool 实现按总线分组的并行指令下发。

classDiagram
    direction TB
    class InferenceNode {
        +shared_ptr~RobotInterface~ robot_
        +control()
        +inference()
    }
    class RobotInterface {
        +apply_action(action)
        +get_joint_q()
        +get_quat()
        +init_motors()
        +deinit_motors()
        -exec_motors_parallel(cmd_func)
        -shared_ptr~IMUDriver~ imu_
        -vector~shared_ptr~MotorDriver~~ motors_
        -unique_ptr~Decouple~ ankle_decouple_
        -unique_ptr~ThreadPool~ thread_pool_
    }
    class MotorDriver {
        +create_motor(...)
        +motor_mit_cmd(pos,vel,kp,kd,tau)
        +get_motor_pos()
    }
    class IMUDriver {
        +create_imu(...)
        +get_quat()
        +get_ang_vel()
    }
    class Decouple {
        +create(type)
        +get_forwardQVT(q,vel,tau,is_left)
        +get_decoupleQVT(q,vel,tau,is_left)
    }
    class ThreadPool {
        +run_parallel(tasks)
    }

    InferenceNode --> RobotInterface : 持有
    RobotInterface --> MotorDriver : 管理 N 个
    RobotInterface --> IMUDriver : 管理 1 个
    RobotInterface --> Decouple : 可选
    RobotInterface --> ThreadPool : 总线并行

从交互图可以看出，InferenceNode 的实时控制线程（实时双线程设计与调度策略）以固定频率调用 apply_action()，而推理线程通过 get_joint_q() / get_quat() 等接口采集观测值。两条路径在 RobotInterface 内部通过独立的互斥锁隔离，避免读写竞争。

Sources: robot_interface.hpp, inference_node.hpp

三段式 YAML 配置模型

RobotInterface 的构造行为完全由配置文件决定，采用 IMU → Motors → Robot 三段式结构。这种分层设计使得硬件变更时只需修改配置，无需重新编译。以下表格以 config/robot.yaml 为例说明各字段的工程含义。

构造阶段会执行三项关键的索引预处理：建立 motor2urdf_ 反向映射表、将 close_chain_motor_id 转换为局部 close_chain_motor_idx_、进一步通过 urdf2motor_ 推导出 close_chain_joint_idx_。这些索引表在运行时避免了每次查询都进行线性查找，保证了控制周期内的确定性延迟。

Sources: robot_interface.cpp, config/robot.yaml

公共 API 与线程安全约定

RobotInterface 对外暴露的接口分为状态读取、动作执行和生命周期管理三类。所有状态读取接口都会在电机未初始化时抛出 std::runtime_error，防止在硬件未就绪时产生无效观测。

特别需要注意的是，get_quat() 和 get_ang_vel() 并非简单返回 IMU 原始缓存，而是在每次调用时执行外参旋转矩阵变换：四元数通过 extrinsic_q_inv_ 将 IMU 坐标系对齐到机体坐标系，角速度通过 extrinsic_R_mat_ 进行同一变换。这种设计确保了即使 IMU 安装姿态不同，上层推理模型看到的始终是标准机体坐标系下的数据。

Sources: robot_interface.hpp, robot_interface.cpp

动作执行流水线：apply_action 详解

apply_action() 是 RobotInterface 中最核心的方法，也是整个控制线程时序的终点。它的执行流程可分为状态采集、闭环解耦与力矩计算、动作映射、指令下发四个阶段。

flowchart TD
    A[调用 apply_action] --> B{is_init_?}
    B -->|否| Z[直接返回]
    B -->|是| C[加 joint_mutex_ 锁]
    C --> D[exec_motors_parallel<br/>读取各电机 pos/spd/current]
    D --> E{response_count ><br/>offline_threshold_?}
    E -->|离线| F[抛 runtime_error]
    E -->|正常| G{存在闭环关节?}
    G -->|是| H[forwardQVT: 电机空间→关节空间]
    H --> I["PD 计算目标力矩<br/>τ = kp*(action−q) + kd*(0−vel)"]
    I --> J[decoupleQVT: 关节空间→电机空间]
    G -->|否| K[直接使用 action 为目标位置]
    J --> L[释放 joint_mutex_]
    K --> L
    L --> M[action 经 motor2urdf_ 写入 motor_target_]
    M --> N{interface_type == canfd?}
    N -->|是| O["按总线打包 pos[8]/kp[8]/kd[8]/tau[8]<br/>thread_pool_ 并行发送"]
    N -->|否| P[exec_motors_parallel<br/>逐个发送 motor_mit_cmd]

阶段一：状态采集与离线检测

apply_action 首先加 joint_mutex_ 锁，然后通过 exec_motors_parallel 并行读取所有电机的位置、速度和电流。与此同时，它检查每个电机的 get_response_count() 是否超过 offline_threshold_（默认 25）。该阈值用于检测 CAN 总线丢包或电机掉线：当连续未响应的周期累积到阈值时，立即抛出异常，由上层 InferenceNode 捕获后执行 rclcpp::shutdown()，避免机器人在传感器失效的情况下继续运行。

Sources: robot_interface.cpp

阶段二：闭环解耦与力矩计算

对于配备四连杆踝关节的机型（如 type: "atom01"），电机编码器读到的角度并非真实关节角度，必须进行正运动学解耦。apply_action 对左右两只脚分别执行：

1. Forward 解耦 (get_forwardQVT)：将电机空间的位置、速度、力矩映射到关节空间，更新 joint_q_ / joint_vel_ / joint_tau_ 供上层观测使用；
2. PD 力矩计算：以关节空间的目标角度 action 为输入，计算期望关节力矩；
3. Inverse 解耦 (get_decoupleQVT)：将关节空间的期望力矩映射回电机空间的力矩指令。

这意味着闭环电机在 apply_action 中并不接收位置控制指令，而是接收经解耦后的力矩前馈指令（tau 通道）。非闭环电机则继续使用位置控制（pos + kp + kd 通道）。两种控制模式在最终的总线打包阶段被区分写入不同的 MIT 命令字段。

Sources: robot_interface.cpp

阶段三：总线并行与 CANFD 打包优化

RobotInterface 支持两种通信模式：普通 CAN 和 CANFD。两者的核心差异在于打包策略。

在普通 CAN 模式下，exec_motors_parallel 将每个电机视为独立任务，通过 ThreadPool 按总线分组并行下发，每个电机单独调用 motor_mit_cmd(pos, vel, kp, kd, tau)。

在 CANFD 模式下，系统会按总线进一步打包：同一条总线上的最多 8 个电机被组织为一个批量命令数组 pos[8], kp[8], kd[8], tau[8]，通过一次 motor_mit_cmd 调用下发。电机 ID 决定了其在数组中的 slot 位置（slot = motor_id - 1）。这种打包显著降低了高频率控制下的总线负载和调度抖动。

ThreadPool 的线程数在构造时初始化为 motor_interface_.size()，即总线数量。这保证了不同总线之间的指令完全并行，而同一总线内部的电机则串行打包，既最大化并行度，又避免了对单条总线的并发竞争。

Sources: robot_interface.cpp, robot_interface.cpp

索引映射与坐标系约定

RobotInterface 内部维护了三张关键映射表，它们共同解决了“URDF 关节顺序”、“电机物理顺序”和“总线拓扑顺序”三者不一致的问题。

apply_action 在读取电机状态时，使用 motor2urdf_[idx] 将电机数据写入关节状态数组；在下发指令时，又使用 motor_target_[idx] = action[motor2urdf_[i]] 将策略输出的关节动作转回电机视角。这种双向映射使得上层强化学习模型可以完全基于标准 URDF 关节顺序开发，而无需关心实际布线中的电机 ID 排列。

Sources: robot_interface.cpp, robot_interface.cpp

生命周期与辅助操作

除了高频控制路径外，RobotInterface 还提供了一系列用于调试和维护的辅助接口。

软启动归位：reset_joints

reset_joints 用于将机器人从任意姿态平滑回到默认站立姿态。它分为两个阶段：先以 kp / 2.5 的较低刚度将关节拉向目标位置，保持 1 秒后切换为全刚度 kp。对于闭环踝关节，该方法还会先执行 get_decoupleQVT，将 URDF 默认角度反向解耦到电机空间，确保四连杆机构不会在内收阶段产生机械干涉。

Sources: robot_interface.cpp

状态刷新：refresh_joints

refresh_joints 与 apply_action 的前半段逻辑几乎一致：强制读取电机状态并执行 forwardQVT，但不执行动作下发。它通常配合 ROS 2 的 refresh_joints 服务调用，用于在推理暂停时校准关节读数。

Sources: robot_interface.cpp

零位标定与错误清除

set_zeros() 和 clear_errors() 对所有电机并行下发标零和清错命令，是硬件维护阶段的标准操作流程。

Sources: robot_interface.cpp

与上下游模块的协作边界

理解 RobotInterface 的职责边界，有助于开发者判断问题归属：

• 观测值组装与归一化（如 obs_scales_dof_pos_、clip_observations_）由 ObsManager 负责，不在 RobotInterface 中；
• 策略推理与帧堆叠由 ONNX Runtime 推理引擎 完成，RobotInterface 只消费其输出的原始动作；
• CAN/CANFD 底层帧封装由 MotorDriver 实现，RobotInterface 仅调用其高层 motor_mit_cmd 接口；
• ROS 2 服务暴露由 InferenceNode 通过 ros_interface.cpp 包装，RobotInterface 本身无 ROS 依赖。

Sources: inference_node.hpp, obs_manager.cpp

阅读路径建议

RobotInterface 作为硬件抽象层，其设计意图需要结合上下文章节才能完整理解。建议按以下顺序深入：

1. 如需了解电机底层的通信协议与帧格式，请阅读 电机驱动与 CAN/CANFD 通信；
2. 如需了解 IMU 外参标定与坐标系对齐细节，请阅读 IMU 接入与坐标变换；
3. 如需了解四连杆踝关节的运动学推导，请阅读 闭环解耦与运动学映射；
4. 如需了解 ThreadPool 的实时调度参数与优先级设置，请阅读 实时双线程设计与调度策略。