Qmini SDK Custom文件浅析

custom.hpp + custom.cpp是 RoboTamerSdk4Qmini 项目的核心控制逻辑，负责把“强化学习策略、模式切换、人机交互、电机通信”整合在一起，实现 Qmini 机器人的实机控制。

下面我从整体架构、核心模块、关键线程三个维度详细剖析：

---

一、整体架构：多线程 + DDS 通信

这是一个典型的多线程实时控制架构，核心思路是：

1. 不同功能用不同线程，按不同频率运行（电机控制高频，策略控制低频）；
2. 线程间通过“数据缓冲区”安全传递数据，避免竞争；
3. 通过宇树的 DDS 通信接口（LowCmd/LowState）与电机/硬件交互。

graph TD A[游戏手柄线程<br>RunJoystick] -->|输入指令| B[模式切换线程<br>ModeProcess] B -->|切换模式| C[控制线程<br>Control] C -->|生成电机命令| D[数据缓冲区<br>motor_command_buffer_] D -->|读取命令| E[电机读写线程<br>JointStateReadWriter] E -->|DDS LowCmd| F[电机硬件] F -->|DDS LowState| E E -->|记录状态| G[数据缓冲区<br>motor_state_buffer_] H[IMU线程<br>IMUStateReader] -->|记录姿态| I[数据缓冲区<br>base_state_buffer_] G -->|读取状态| C I -->|读取姿态| C C -->|上报数据| J[数据上报线程<br>ReportData]

---

二、核心模块详解

1. 通信模块：DDS 话题与数据缓冲区

（1）DDS 通信接口（宇树机器人标准）

// custom.hpp
static const std::string HG_CMD_TOPIC = "rt/lowcmd";   // 电机命令话题
static const std::string HG_STATE_TOPIC = "rt/lowstate"; // 电机状态话题

ChannelPublisherPtr<unitree_hg::msg::dds_::LowCmd_> lowcmd_publisher_;  // 发布命令
ChannelSubscriberPtr<unitree_hg::msg::dds_::LowState_> lowstate_subscriber_; // 订阅状态

• 作用：通过宇树的 DDS（Data Distribution Service）中间件，与机器人的电机/硬件进行实时通信；
• LowCmd：发给电机的命令（目标位置、速度、力矩、刚度 kp、阻尼 kd）；
• LowState：电机返回的状态（实际位置、速度、温度）。

（2）线程安全的数据缓冲区

// custom.hpp
DataBuffer<MotorState> motor_state_buffer_;    // 电机状态缓冲区
DataBuffer<MotorCommand> motor_command_buffer_; // 电机命令缓冲区
DataBuffer<BaseState> base_state_buffer_;       // 机器人基础状态（IMU）缓冲区

• 作用：不同线程运行频率不同（比如电机控制 500Hz，策略控制 100Hz），用缓冲区安全传递数据，避免直接读写冲突；
• DataBuffer：项目封装的线程安全队列，支持 SetData()（写入）和 GetData()（读取）。

---

2. 控制核心：G1 类与多线程

G1 类是整个控制程序的“入口类”，在构造函数中启动了6个关键线程，每个线程负责一个功能：

线程名	运行频率	对应函数	核心功能
control	100Hz（0.01s）	Control()	核心控制逻辑：根据模式调用站立/RL/测试控制
command_writer	500Hz（0.002s）	JointStateReadWriter()	高频读写电机：发命令 + 收状态
imu	~333Hz（0.003s）	IMUStateReader()	读取 IMU 数据（机器人姿态）
joystick	250Hz（0.004s）	RunJoystick()	读取游戏手柄输入
report_rpy	100Hz（0.01s）	ReportData()	上报数据（用于调试/可视化）
mode_process	50Hz（0.02s）	ModeProcess()	处理模式切换

---

3. 关键函数详解

（1）模式切换：ModeProcess()

// custom.cpp
void G1::ModeProcess() {
    // 1. 获取选择的模式（本地测试用键盘，实机用手柄）
    if (_is_test_local) {
        selected_mode = modeSwitcher.get_selected_key(current_mode);
    } else {
        selected_mode = modeSwitcher.get_selected_jskey(current_mode);
    }

    // 2. 如果模式切换，重置控制器
    if (selected_mode != current_mode) {
        relative_time = 0.;
        current_mode = selected_mode;
        rlController->reset(_is_test_local); // 重置 RL 控制器
    }
    rlController->task_mode = modeSwitcher.rl_task_mode;
}

• 模式定义（结合 Control() 函数）：
  • '1'：初始模式（站立）；
  • '2'：站立模式；
  • '3'：RL 控制模式（运行强化学习策略）；
  • '5'：测试模式（正弦波测试或仿真步态回放）；
  • 'q'：退出程序。

（2）核心控制：Control()

这是整个程序的“大脑”，根据当前模式执行不同控制：

// custom.cpp
void G1::Control() {
    relative_time += control_dt_;
    float ratio = fmin(relative_time / MOVE_DURATION, 1.f); // 动作平滑过渡比例

    // 1. 把 DDS 电机状态转换成 RL 算法需要的状态
    rlController->convert_dds_state2rl_state();

    // 2. 根据模式执行控制
    switch (current_mode) {
        case 'q': // 退出
            rlController->_kp.setZero();
            rlController->_kd.setZero();
            exit(1);
        case '2': // 站立控制
            rlController->stand_control(ratio);
            break;
        case '3': // RL 控制
            rlController->rl_control();
            if (rlController->counter_rl < 2) // 前2步先保持站立
                rlController->stand_control(ratio);
            break;
        case '5': // 测试模式
            if (rlController->configParams.use_sim_gait)
                rlController->sim_gait_control(); // 回放仿真步态
            else
                rlController->sin_control(0.2, 2., relative_time); // 正弦波测试电机
            break;
        default: // 默认站立
            rlController->stand_control(ratio);
            break;
    }

    // 3. 把 RL 算法输出的关节动作转换成 DDS 电机命令
    rlController->set_rl_joint_act2dds_motor_command(current_mode);
}

• 关键逻辑：
  • convert_dds_state2rl_state()：把电机/IMU 的原始数据，转换成强化学习策略需要的输入（比如关节角度归一化、身体姿态计算）；
  • stand_control(ratio)：站立控制，通过 ratio 实现从“初始位置”到“站立位置”的平滑过渡；
  • rl_control()：运行强化学习策略（加载 ONNX 模型），输出关节动作；
  • set_rl_joint_act2dds_motor_command()：把策略输出的关节动作，填充到 motor_command_buffer_ 缓冲区。

（3）电机读写：JointStateReadWriter()

这是与硬件交互的“桥梁”，高频运行（500Hz）：

// custom.cpp
void G1::JointStateReadWriter() {
    // 1. 从缓冲区获取电机命令，通过 DDS 发给电机
    Motor_control.Run(SetMotorCmd());

    // 2. 从电机获取状态，存入缓冲区
    RecordMotorState(Motor_control.GetData());
}

• SetMotorCmd()：从 motor_command_buffer_ 读取命令，填充宇树的 LowCmd 消息：

  unitree_hg::msg::dds_::LowCmd_ G1::SetMotorCmd() {
      unitree_hg::msg::dds_::LowCmd_ dds_low_command;
      const std::shared_ptr<const MotorCommand> mc = motor_command_buffer_.GetData();
  
      // 填充每个电机的命令：前馈力矩、目标位置、目标速度、kp、kd
      for (size_t i = 0; i < rlController->NUM_JOINTS; i++) {
          dds_low_command.motor_cmd().at(i).tau() = mc->tau_ff.at(i);
          dds_low_command.motor_cmd().at(i).q() = mc->q_target.at(i);
          dds_low_command.motor_cmd().at(i).dq() = mc->dq_target.at(i);
          dds_low_command.motor_cmd().at(i).kp() = mc->kp.at(i);
          dds_low_command.motor_cmd().at(i).kd() = mc->kd.at(i);
      }
      return dds_low_command;
  }
  

• RecordMotorState()：把电机返回的状态（实际位置 q、速度 dq）存入 motor_state_buffer_。

（4）IMU 读取：IMUStateReader()

读取机器人的姿态数据，用于控制策略（比如判断机器人是否倾斜）：

// custom.cpp
void G1::IMUStateReader() {
    if (imuReader.fetchIMUData()) {
        RecordBaseState(); // 把 IMU 数据存入 base_state_buffer_
    }
}

void G1::RecordBaseState() {
    BaseState bs_tmp;
    bs_tmp.omega = {imuReader.RollSpeed, imuReader.PitchSpeed, imuReader.HeadingSpeed}; // 角速度
    bs_tmp.rpy = {imuReader.Roll, imuReader.Pitch, imuReader.Heading}; // 滚转、俯仰、偏航
    bs_tmp.acc = {imuReader.Accelerometer_X, imuReader.Accelerometer_Y, imuReader.Accelerometer_Z}; // 加速度
    bs_tmp.quat = {imuReader.qw, imuReader.qx, imuReader.qy, imuReader.qz}; // 四元数
    base_state_buffer_.SetData(bs_tmp);
}

---

三、总结：代码的核心价值

这两个文件把“复杂的机器人控制”拆分成了清晰的模块化逻辑：

1. 多线程解耦：高频电机控制和低频策略控制分离，保证实时性；
2. 数据缓冲区安全：避免线程竞争，保证数据传递可靠；
3. 模式切换灵活：支持站立、RL、测试等多种模式，方便调试；
4. DDS 通信标准化：直接用宇树的官方接口，与硬件无缝对接。

如果你想修改控制逻辑（比如调整站立速度、更换 RL 策略），重点关注 Control() 函数和 RLController 类即可。