URDF 与电机映射关系

在强化学习训练环境（如 Isaac Gym/Sim）中生成的 ONNX 策略模型，其输入输出的关节顺序通常由训练时加载的 USD/URDF 解析器决定；而实际机器人硬件的电机 ID 分配则受限于 CAN 总线拓扑和驱动板布局。为了让策略模型能够无缝驱动真实硬件，系统在推理节点与硬件抽象层之间引入了两组显式映射配置，将策略空间、URDF 逻辑空间与电机物理空间三者解耦。本文档将从索引空间定义、映射表语义、闭链关节解耦以及控制循环中的数据流四个维度，完整阐述这一映射体系的设计与实现。

三层索引空间

整个系统围绕 23 个主动自由度运转，但在不同模块中，这 23 个维度的排列顺序并不相同。代码中实际存在三个独立的索引空间，任何跨模块的数据流转都必须经过显式映射。

URDF 空间（逻辑关节空间） 是推理节点内部的状态统一表示。InferenceNode 中的 act_（目标动作）、RobotInterface 中的 joint_q_ / joint_vel_ / joint_tau_（关节反馈），以及 ROS Topic /joint_states、/action 均使用该顺序。此顺序由 inference.yaml 中的 joint_default_angle 和 joint_limits 隐式定义，索引范围为 0–22。

电机物理空间（硬件空间） 对应 RobotInterface 构造函数中 motors_cfg_->motor_id_ 数组的索引。robot.yaml 中 motor_id: [1, 2, ..., 23] 定义了每个电机在物理总线上的真实 ID，同时 motor_interface 与 motor_num 将其分组到 can0（6 个）、can1（7 个）、can2（5 个）、can3（5 个）四条总线。驱动层通信时，以此空间的索引定位具体电机对象。

策略/USD 空间（模型空间） 是 ONNX 模型输入输出所采用的关节顺序。由于训练环境对 URDF 的解析顺序可能与机器人本体的逻辑关节顺序不同，策略输出的第 i 维未必对应 URDF 的第 i 个关节。inference.yaml 中的 usd2urdf 数组专门负责将模型空间映射到 URDF 空间。

Sources: robot_interface.cpp, inference_node.hpp, robot.yaml, inference.yaml

核心映射配置与语义

urdf2motor：URDF 到电机数组索引

robot.yaml 中的 urdf2motor 数组定义了从 URDF 空间到 motor_id_ 数组索引的映射。当前配置为恒等映射 [0, 1, 2, ..., 22]，表示 URDF 索引 i 直接取用 motor_id_ 数组的第 i 个元素（即物理电机 ID 为 i+1）。在 RobotInterface 构造函数中，系统遍历 urdf2motor 并反推其逆映射 motor2urdf_：motor2urdf_[urdf2motor[i]] = i。该逆映射在后续电机状态读取和指令下发环节被高频使用，用于将物理电机数组索引转换为 URDF 逻辑索引。

Sources: robot_interface.cpp, robot.yaml

usd2urdf：策略空间到 URDF 空间

inference.yaml 中的 usd2urdf 数组定义了 ONNX 模型输出维度到 URDF 关节索引的映射，其值为 [0, 6, 12, 1, 7, 13, 18, 2, 8, 14, 19, 3, 9, 15, 20, 4, 10, 16, 21, 5, 11, 17, 22]。这意味着策略输出的第 0 维对应 URDF 关节 0，第 1 维对应 URDF 关节 6，第 2 维对应 URDF 关节 12，依此类推。该映射在观测构造（get_dof_pos_obs、get_dof_vel_obs）和动作解析（inference()）两个方向均被使用，确保训练环境的关节顺序与机器人本体的逻辑顺序正确对齐。

Sources: inference.yaml, inference_node.cpp, obs_manager.cpp

motor_sign：方向极性修正

由于电机在机械结构上的安装方向可能与 URDF 中关节正方向定义相反，系统在读取电机反馈和下发控制指令时统一应用 motor_sign 进行符号修正。在 apply_action() 中，joint_q_[motor2urdf_[idx]] = motor->get_motor_pos() * robot_cfg_->motor_sign_[idx] 保证上层拿到的关节角度与 URDF 定义一致；在指令下发阶段，motor_target_[idx] * robot_cfg_->motor_sign_[idx] 则将 URDF 空间的目标值转回电机所需的物理方向。

Sources: robot_interface.cpp, robot_interface.cpp, robot.yaml

闭链踝关节的额外映射层

Atom01 机器人采用并联闭链结构实现踝关节，左右脚踝各由 2 个电机共同驱动 pitch/roll 两个自由度。这 4 个电机（物理 ID 为 5、6、11、12）无法直接视为独立 1-DOF 关节，必须在电机空间与关节空间之间插入运动学解耦层。

robot.yaml 中的 close_chain_motor_id: [5, 6, 11, 12] 标识了闭链电机的物理 ID。RobotInterface 构造函数通过查找将其转换为两组内部索引：close_chain_motor_idx_（电机数组索引，值为 [4, 5, 10, 11]）和 close_chain_joint_idx_（URDF 索引，同样为 [4, 5, 10, 11]，因当前 urdf2motor 为恒等映射）。DecoupleAtom01 类基于机构的连杆几何参数，通过数值迭代正向运动学（forward_kinematics）和解析逆运动学（inverse_kinematics）实现双向映射：

读取方向：电机原始角度经 motor_sign 修正后，通过 get_forwardQVT 转换为 URDF 空间中的 ankle pitch/roll 关节角度、速度和等效力矩。
下发方向：推理节点计算出的关节目标力矩经 PD 控制后，通过 get_decoupleQVT 映射回两个电机的力矩指令，再以纯力矩模式（tau 通道）下发。

Sources: robot_interface.cpp, robot_interface.cpp, close_chain_mapping.hpp, decouple_atom01.hpp, decouple_atom01.cpp, robot.yaml

控制循环中的映射数据流

在一次完整的控制周期内，数据需要穿越所有三层索引空间以及闭链解耦层。以下 Mermaid 序列图展示了从电机反馈到策略推理，再到电机指令下发的完整映射链路：

sequenceDiagram
    participant M as 电机物理层<br/>(Motor ID 1~23)
    participant RI as RobotInterface
    participant DC as DecoupleAtom01<br/>(闭链解耦)
    participant IN as InferenceNode
    participant ONNX as ONNX 策略模型

    Note over M,RI: 反馈读取阶段
    RI->>M: 批量读取 pos/spd/current<br/>(motor_id_ 数组顺序)
    RI->>RI: 应用 motor_sign 极性修正
    RI->>RI: 通过 motor2urdf_ 写入<br/>joint_q_[], joint_vel[], joint_tau[]<br/>(URDF 顺序)
    RI->>DC: 对 close_chain_joint_idx_<br/>调用 get_forwardQVT()
    DC-->>RI: 更新 ankle pitch/roll<br/>的 q, vel, tau

    Note over RI,IN: 观测构造阶段
    IN->>RI: get_joint_q() / get_joint_vel()
    IN->>IN: 通过 usd2urdf_ 重排为<br/>策略空间顺序的 dof_pos / dof_vel<br/>(输入 ONNX)

    Note over IN,ONNX: 推理阶段
    IN->>ONNX: 运行模型推理
    ONNX-->>IN: output_buffer[]<br/>(策略空间顺序)

    Note over IN,RI: 动作解析与下发阶段
    IN->>IN: output_buffer[i] → act_[usd2urdf_[i]]<br/>应用 action_scale + joint_default_angle
    IN->>RI: apply_action(act_)
    RI->>DC: 对 ankle 调用 get_decoupleQVT()<br/>将关节力矩映射为电机力矩
    DC-->>RI: 更新 action[] 中的电机目标
    RI->>RI: action[motor2urdf_[i]] → motor_target_[i]
    RI->>M: 应用 motor_sign 后下发 MIT 指令<br/>非闭链电机用 pos/kp/kd 模式<br/>闭链电机用 tau 模式

该流程中有三个关键的映射转换点：一是 motor2urdf_ 在 RobotInterface 内部隔离了电机物理顺序与 URDF 逻辑顺序；二是 usd2urdf_ 在 InferenceNode 内部隔离了策略模型顺序与 URDF 逻辑顺序；三是 DecoupleAtom01 在 RobotInterface 内部对特定关节进行了电机空间与关节空间的双向运动学转换。

Sources: robot_interface.cpp, robot_interface.cpp, inference_node.cpp, obs_manager.cpp

映射配置速查表

下表汇总了涉及映射关系的核心配置项、所在文件及语义说明：

配置项	配置文件	数据类型	语义说明
`urdf2motor`	`robot.yaml`	`int[]`	URDF 索引 → `motor_id_` 数组索引的映射。当前为恒等映射。
`motor_sign`	`robot.yaml`	`int[]`	电机方向修正系数，取值 ±1。在读写时统一乘入。
`close_chain_motor_id`	`robot.yaml`	`int[]`	闭链机构电机的物理 ID 列表（如踝关节驱动电机）。
`usd2urdf`	`inference.yaml`	`int[]`	策略/USD 空间索引 → URDF 空间索引的映射。
`joint_default_angle`	`inference.yaml`	`double[]`	URDF 空间中各关节的默认/初始角度，用于观测归一化和动作偏置。
`joint_limits`	`inference.yaml`	`double[]`	URDF 空间中各关节的 [min, max] 极限，按 URDF 顺序成对排列。

需要特别注意的是，urdf2motor 和 usd2urdf 虽然同为索引映射数组，但作用域完全不同：前者只在 RobotInterface 内部生效，用于桥接硬件层与逻辑层；后者只在 InferenceNode 内部生效，用于桥接模型层与逻辑层。两者以 URDF 空间为公共中间表示，彼此不直接耦合。

Sources: robot.yaml, inference.yaml

映射与相邻模块的边界

URDF 与电机映射关系并不是孤立存在的配置，它与系统其他模块存在清晰的职责边界。闭链踝关节的运动学解耦数学推导属于闭链运动学与踝关节解耦的范畴；电机驱动命令的实际封装与 CAN/CANFD 总线发送属于通信协议层：CAN/CANFD 与串口的范畴；而 RobotInterface 中映射表的初始化与线程安全封装则在 RobotInterface 硬件抽象层中有更完整的说明。如果需要在新的机器人平台或新的训练环境中复用本推理框架，通常只需修改 robot.yaml 和 inference.yaml 中的上述四组映射数组，以及对应的 joint_default_angle 与 joint_limits，即可实现零代码迁移。