本页深入解析 roboto_motors 电机驱动库的内部架构，重点阐述抽象接口设计、MIT 阻抗控制的数学原理与帧打包实现，以及SocketCAN 实时通信层的工作机制。理解这些内容，有助于你在扩展新电机类型、调优控制参数或排查通信故障时，建立清晰的因果链路。若你尚未完成环境配置，建议先阅读 环境配置与依赖安装；若需了解电机零点标定操作，请参考 电机零点标定。
模块架构总览
roboto_motors 采用抽象基类 + 工厂模式 + 单例总线的三层架构。顶层 MotorDriver 定义了所有电机通用的控制语义；中层通过 create_motor() 工厂方法实例化特定厂商的驱动（DM、EVO、LRO）；底层由 MotorsCAN / MotorsCANFD 单例管理 SocketCAN 接口的生命周期与实时线程。这种分层使得上层推理节点无需关心总线上的具体电机品牌，只需调用统一的 motor_mit_cmd() 即可。
classDiagram
class MotorDriver {
+enum MotorControlMode_e
+create_motor()$ MotorDriver
+motor_mit_cmd(f_p, f_v, f_kp, f_kd, f_t)* void
+motor_mit_cmd(float*)* void
+lock_motor()* void
+unlock_motor()* void
+init_motor()* uint8_t
+get_motor_pos() float
+get_motor_spd() float
+get_motor_current() float
}
class DmMotorDriver {
+can_rx_cbk(rx_frame) void
+motor_mit_cmd(...) void
}
class EvoMotorDriver {
+bus_registry_ static unordered_map
+motor_mit_cmd(...) void
+motor_mit_cmd(float*) void
}
class LroMotorDriver {
+bus_registry_ static unordered_map
+motor_mit_cmd(...) void
+motor_mit_cmd(float*) void
}
class MotorsCAN {
+transmit(frame)* void
+add_can_callback(cb, id)* void
}
class MotorsSocketCAN {
-sockfd_ int
-tx_queue_ LFQueue
-receiver_thread_ thread
-sender_thread_ thread
+transmit(frame) void
}
class MotorsCANFD {
+transmit(frame)* void
+add_canfd_callback(cb, id)* void
}
class MotorsSocketCANFD {
-sockfd_ int
-tx_queue_ LFQueueFD
-receiver_thread_ thread
-sender_thread_ thread
+transmit(frame) void
}
MotorDriver <|-- DmMotorDriver
MotorDriver <|-- EvoMotorDriver
MotorDriver <|-- LroMotorDriver
MotorsCAN <|-- MotorsSocketCAN
MotorsCANFD <|-- MotorsSocketCANFD
DmMotorDriver --> MotorsCAN
DmMotorDriver --> MotorsCANFD
EvoMotorDriver --> MotorsCAN
EvoMotorDriver --> MotorsCANFD
LroMotorDriver --> MotorsCANFD
Sources: motor_driver.hpp, motor_driver.cpp, socket_can.hpp
抽象接口与工厂模式
MotorDriver 作为抽象基类，将电机控制语义收敛为四种模式：NONE、MIT、POS、SPD。其中 MIT 模式是强化学习策略输出的核心接口，它同时接收目标位置、目标速度、位置刚度、速度阻尼和前馈力矩五个参数，使电机表现出期望的阻抗特性。基类还声明了批量指针重载 motor_mit_cmd(float*, ...)，专为推理引擎的多电机并行控制而设计，避免在热循环中多次进入内核态。
Sources: motor_driver.hpp, motor_driver.hpp
工厂方法 MotorDriver::create_motor() 根据 motor_type 字符串进行分支派发。当前支持 "DM"（达妙）、"EVO" 和 "LRO"（LeadRobot）三种类型，每种类型内部再按 motor_model 枚举选择具体的限幅参数表。例如 "DM" 会进一步区分 DM4340P_48V 与 DM10010L_48V，两者的力矩上限分别为 28 N·m 和 200 N·m，直接决定了后续 limit() 裁剪的边界。
Sources: motor_driver.cpp, dm_motor_driver.hpp
MIT 阻抗控制原理
MIT 控制本质上是一种前馈-反馈混合的阻抗控制律，由 MIT Biomimetic Robotics Lab 在 Mini Cheetah 项目中推广。电机最终输出的力矩由以下公式决定：
$$ \tau = K_p \cdot (p_{\text{target}} - p_{\text{actual}}) + K_d \cdot (v_{\text{target}} - v_{\text{actual}}) + \tau_{\text{ff}} $$
五个参数的分工如下：
| 参数 | 符号 | 物理意义 | 取值范围策略 |
|---|---|---|---|
| 目标位置 | $p_{\text{target}}$ | 期望关节角度 | 受电机型号 PosMax 限制 |
| 目标速度 | $v_{\text{target}}$ | 期望关节角速度 | 受电机型号 SpdMax 限制 |
| 位置刚度 | $K_p$ | 位置误差增益 | 仅非负，受 OKpMax 限制 |
| 速度阻尼 | $K_d$ | 速度误差增益 | 仅非负，受 OKdMax 限制 |
| 前馈力矩 | $\tau_{\text{ff}}$ | 直接叠加的力矩指令 | 受 TauMax 限制，可正可负 |
当 $K_p = 0$ 且 $K_d = 0$ 时，电机表现为纯力矩控制；当 $\tau_{\text{ff}} = 0$ 时，电机表现为 PD 位置伺服。强化学习策略通常输出低刚度、低阻尼并配合前馈力矩，从而获得柔顺且响应迅速的运动。
Sources: dm_motor_driver.cpp, evo_motor_driver.cpp
CAN 帧打包与量化
由于标准 CAN 帧仅有 8 字节有效载荷，MIT 的五个浮点参数必须经过限幅、量化、位打包才能下发。所有驱动实现都遵循同一套预处理流水线：先用 limit() 将浮点值裁剪到电机型号对应的物理极限，再用 range_map() 线性映射到固定位宽的整型，最后按厂商协议填充到 can_frame.data[]。
DM / EVO 的 64 位打包格式
DM 与 EVO 在单电机 MIT 模式下使用完全相同的位布局，总计 64 位：
| 位置 pos | 速度 vel | 刚度 kp | 阻尼 kd | 力矩 t |
| 16 bits | 12 bits | 12 bits | 12 bits | 12 bits |
以 DM 驱动为例，代码首先将 f_p 减去 motor_zero_offset_ 做零位补偿，然后裁剪到 [-PosMax, PosMax]，再映射到 uint16_t(0) 至 0xFFFF。速度、力矩采用 12 位有符号映射，而刚度、阻尼因仅取非负值，从 0 映射到 0x0FFF。最终通过移位与掩码拼接成 8 字节数组：
tx_frame.data[0] = p >> 8;
tx_frame.data[1] = p & 0xFF;
tx_frame.data[2] = v >> 4;
tx_frame.data[3] = (v & 0x0F) << 4 | kp >> 8;
// ... 依此类推
Sources: dm_motor_driver.cpp, evo_motor_driver.cpp
LRO 的大端 64 位打包格式
LRO（LeadRobot）采用的位布局与 DM/EVO 不同，其 64 位大端帧结构为：
| 模式 mode | 刚度 kp | 阻尼 kd | 位置 pos | 速度 vel | 力矩 t |
| 3 bits | 12 bits | 9 bits | 16 bits | 12 bits | 12 bits |
注意 LRO 将 kd 压缩为 9 位（而非 12 位），且最高 3 位用于标识控制模式（LRO_MODE_MIT = 0x00）。代码通过 uint64_t packed 一次性完成大端位或运算，再按字节拆分写入 tx_frame.data。这意味着在调参时，LRO 的阻尼分辨率略低于 DM/EVO，需在整定 $K_d$ 时予以关注。
Sources: lro_motor_driver.cpp
反馈解析的对称性
电机回传的反馈帧同样遵循上述位宽约定。以 DM 为例，RX 回调从 can_frame 中提取 16 位位置、12 位速度和 12 位力矩，再通过 range_map() 的逆映射还原为浮点物理量，并叠加 motor_zero_offset_ 还原到用户坐标系：
pos_int = rx_frame.data[1] << 8 | rx_frame.data[2];
spd_int = rx_frame.data[3] << 4 | (rx_frame.data[4] & 0xF0) >> 4;
t_int = (rx_frame.data[4] & 0x0F) << 8 | rx_frame.data[5];
motor_pos_ = range_map(pos_int, 0, 0xFFFF, -PosMax, PosMax) + motor_zero_offset_;
Sources: dm_motor_driver.cpp
实时通信层：SocketCAN 与线程模型
电机驱动的实时性不仅取决于控制算法，更取决于总线通信的延迟抖动。roboto_motors 在底层做了三项关键优化：单例共享、实时线程隔离与无锁发送队列。
单例与接口复用
MotorsSocketCAN 和 MotorsSocketCANFD 均以接口名（如 "can0"）为键维护静态 instances_ 映射表。同一总线上的所有电机实例共享同一个 SocketCAN 对象，从而避免重复创建内核套接字和竞争线程资源。电机驱动在构造时通过 MotorsCAN::get(interface) 获取该单例，并注册以 motor_id 为键的 RX 回调；析构时自动注销回调。
Sources: socket_can.cpp, socket_canfd.cpp
SCHED_FIFO 与 CPU 亲和性
每个 SocketCAN 实例在 open() 时启动两条线程：can_rx / can_tx（或 canfd_rx / canfd_tx）。两条线程均通过 pthread_setschedparam 设置为 SCHED_FIFO 调度策略，优先级 80，确保它们在任何普通 CFS 线程之前获得 CPU 时间。此外，线程根据接口名末位数字计算亲和性核心：例如 can0 绑定到最后一个可用核心，can1 绑定到倒数第二个核心。这种设计在多总线场景下将不同接口的收发负载分散到独立核心，显著降低中断处理与上下文切换带来的抖动。
Sources: socket_can.cpp, socket_can.cpp
无锁 TX 队列与条件变量
发送端使用 boost::lockfree::queue<can_frame> 作为无锁环形缓冲区，容量固定为 4096 帧。transmit() 将帧推入队列后立即通过 tx_cv_.notify_one() 唤醒发送线程，发送线程则在 std::unique_lock 保护下批量 pop() 并调用 ::write() 写入套接字。若写操作失败，发送线程会以 1 ms 间隔重试最多 3 次。该模型将用户调用线程与内核 I/O 解耦，即使控制循环以 200 Hz~1 kHz 频繁下发命令，也不会因 write() 阻塞而掉节拍。
Sources: socket_can.hpp, socket_can.cpp, socket_can.cpp
CAN-FD 批量控制优化
在标准 CAN 2.0 下，每个电机的 MIT 命令都需要独立的一帧（8 字节）。对于 12 自由度或更高自由度的机器人，总线负载会随着关节数线性上升。为此，EVO 与 LRO 驱动在 CAN-FD 模式下实现了批量 MIT 命令，利用 64 字节有效载荷将最多 8 个电机的控制参数打包到单帧中。
EVO 的 One-to-Many 帧
EVO 驱动的批量指针重载 motor_mit_cmd(float* f_p, ...) 会构造一个 can_id = 0x20、长度为 64 的 CAN-FD 帧。帧内每 8 字节对应一个电机槽位（slot 0~7）。驱动通过静态的 bus_registry_ 查找到当前接口上所有已注册的 EVO 电机实例，按各自的 motor_index_ 将量化的 16+12+12+12+12 位数据填入对应槽位。未使用的槽位填充默认值 0x7FFF...，确保电机不会产生异常运动。
canfd_frame tx_frame;
tx_frame.can_id = EVOFD_MIT_ID; // 0x20
tx_frame.len = 64;
tx_frame.flags = CANFD_BRS;
for (EvoMotorDriver* motor : it->second) {
const uint8_t slot = motor->motor_index_;
// 将 f_p[slot]... 量化后写入 tx_frame.data[slot * 8]
}
canfd_->transmit(tx_frame);
Sources: evo_motor_driver.cpp
LRO 的扩展 ID 批量帧
LRO 的批量模式逻辑与 EVO 类似，但使用 29 位扩展 ID 0x8080，且每个槽位采用 LRO 特有的大端 64 位打包（3bit mode + 12bit kp + 9bit kd + 16bit pos + 12bit vel + 12bit torque）。由于 motor_index_ 同样限制在 0~7，单帧最多覆盖 8 个电机。此模式要求所有目标电机位于同一 canfd 接口，且 ID 连续或至少 motor_index_ 已正确配置。
Sources: lro_motor_driver.cpp
批量模式的使用场景
批量 MIT 命令并非由 Python SDK 直接暴露，而是面向 C++ 侧的高性能推理节点。当策略网络输出 actions[12] 数组时，控制线程可以一次性将位置、速度、Kp、Kd、力矩五个数组传入批量接口，将原本 12 次 write() 系统调用压缩为 2 次（两帧 CAN-FD），大幅降低内核态切换开销。若你需要接入自定义策略并调用此接口，可参考 接入自定义强化学习策略 中的 C++ 集成示例。
Python SDK 使用示例
对于大多数开发者，通过 motors_py 模块即可快速验证电机与 MIT 控制。以下示例展示了创建 DM 电机、初始化、下发 MIT 命令并读取反馈的完整流程：
from motors_py import MotorDriver, MotorControlMode
import time
motor = MotorDriver.create_motor(
motor_id=1,
interface_type="can",
interface="can0",
motor_type="DM",
motor_model=0, # DM4340P_48V
master_id_offset=16
)
motor.init_motor()
motor.set_motor_control_mode(MotorControlMode.MIT)
# MIT 阻抗控制：目标位置 -0.5 rad，零速，Kp=5, Kd=1, 无额外力矩
motor.motor_mit_cmd(-0.5, 0.0, 5.0, 1.0, 0.0)
print(f"pos={motor.get_motor_pos():.4f} rad, "
f"spd={motor.get_motor_spd():.4f} rad/s, "
f"cur={motor.get_motor_current():.4f} A")
motor.deinit_motor()
Sources: motors_py_example.py, pybind_module.cpp
延伸阅读与后续步骤
电机驱动模块并非孤立存在，其输出的位置、速度和力矩数据会经由 RobotInterface 硬件抽象层 汇聚为统一的机器人状态向量，再送入推理节点。若你在调试过程中遇到电机无响应或报错，通常需要结合 通信协议层：CAN/CANFD 与串口 的接口配置说明，检查 ip link set can0 up type can bitrate 1000000 等命令是否已正确执行。对于希望扩展新电机类型的开发者，扩展新电机驱动类型 提供了实现 MotorDriver 子类的完整规范。