状态机概述与实现

1. 架构概览

DR02 Pro SDK 内置完整的 RL 状态机系统，由 HumanoidStateMachine 类管理，核心组件：

组件	类名	职责
状态机主体	HumanoidStateMachine	2ms 周期循环、状态调度
机器人接口	RobotInterface / DR2ProInterface	ROS 2 话题订阅与发布
用户命令接口	UserCommandInterface	键盘、手柄、/STEER 输入处理
安全控制器	SafeController	独立线程监控健康状态
运动状态存储	MotionStateStore	线程安全的状态共享
控制参数	ControlParameters	各部位 PD 增益、限位等参数

1.1 运行链路

用户输入（键盘 / 手柄 / /STEER / /REAL_STEER）
    ↓
HumanoidStateMachine（仿人状态机，2ms 周期）
    ↓
DR2ProInterface → RobotInterface（ROS 2 话题通信）
    ↓
ROS 2 Topics: /JOINTS_CMD, /JOINTS_DATA, /IMU_DATA_HEAD, /IMU_DATA_BASE
    ↓
机器人固件（内部控制器执行）

2. 启动流程

main.cpp

auto fsm = std::make_shared<HumanoidStateMachine>(RobotName::DR2PRO);
fsm->Start();   // 初始化接口、创建状态控制器、启动安全监控
fsm->Run();     // 进入 2ms 控制循环
fsm->Stop();    // 停止所有线程

Start() 依次执行：

1. 创建 ROS 2 节点 sdk_deploy
2. 初始化 UserCommandInterface（订阅 /GAMEPAD_KEY、/STEER、/REAL_STEER）
3. 初始化 DR2ProInterface（31 自由度）
4. 创建 ControlParameters 和 SafeController
5. 构建 4 个状态控制器
6. 等待 3 秒安全初始化
7. 启动各接口和安全监控

3. 控制周期

loop_timer_.Start(std::chrono::milliseconds(2));  // 2ms 周期
while (rclcpp::ok()) {
    loop_timer_.Wait();
    ri_ptr_->RefreshRobotData();         // 刷新传感器数据
    current_controller_->Run();          // 运行当前状态逻辑
    next_state_name_ = current_controller_->GetNextStateName();  // 判断下一状态
    if (next_state_name_ != current_state_name_) {
        current_controller_->OnExit();   // 退出当前状态
        current_controller_ = GetStateControllerPtr(next_state_name_);
        current_controller_->OnEnter();  // 进入新状态
        current_state_name_ = next_state_name_;
    }
}

ROS 2 回调在独立线程中通过 rclcpp::spin(node_) 处理。

4. 状态流转

Idle → ZeroPos → RLControl
  ↑                  ↓
  └── JointDamping ←─┘

状态	类	说明
Idle	IdleState	待机，发送零力矩命令，等待站立指令
ZeroPos	ZeroPosState	3 秒三次样条插值，控制全身 31 关节回零位
RLControl	RLControlState	运行 AMP 策略推理，接收速度指令
JointDamping	JointDampingState	全身纯阻尼，3 秒后自动回到 Idle

5. 状态基类

StateBase 定义所有状态的接口：

virtual void OnEnter() = 0;           // 进入状态时执行
virtual void OnExit() = 0;            // 退出状态时执行
virtual void Run() = 0;               // 每个控制周期执行
virtual StateName GetNextStateName() = 0;  // 返回下一状态名

各状态通过 ControllerData 共享以下指针：

struct ControllerData {
    std::shared_ptr<RobotInterface> ri_ptr;          // 机器人接口
    std::shared_ptr<UserCommandInterface> uc_ptr;    // 用户命令接口
    std::shared_ptr<ControlParameters> cp_ptr;       // 控制参数
    std::shared_ptr<MotionStateStore> motion_state_store;  // 运动状态存储
};

6. 各状态详解

6.1 Idle 状态

• 进入时设置运动状态为 Idle
• 每个周期发送零力矩命令（joint_cmd_ = 0）
• 当 target_mode == ZeroPos 时切换到 ZeroPos
• 当 safe_control_mode != 0 时保持 Idle

6.2 ZeroPos 状态

• 进入时记录当前关节位置和速度
• 3 秒内通过三次样条插值平滑过渡到零位
• 各部位使用不同 PD 增益：

腰部 kp: [600, 2800, 2300]    kd: [6, 15, 20]
手臂 kp: [600, 600, 600, 600, 90, 90, 90]  kd: [6, 6, 6, 6, 2, 2, 2]
腿部 kp: [1800, 600, 600, 1800, 600, 90]   kd: [24, 6, 6, 24, 6, 2]
颈部 kp: [0, 0]              kd: [0, 0]

• 当 target_mode == RLControl 时切换到 RLControl
• 当 safe_control_mode != 0 或 target_mode == JointDamping 时切换到 JointDamping

6.3 RLControl 状态

• 进入时启动独立推理线程
• 推理线程以 decimation_（默认 10）分频运行，即 2ms × 10 = 20ms 推理一次
• 每次推理收集 RobotBasicState（基座 RPY、角速度、加速度、关节状态）和用户指令
• 策略输出 21 个关节动作（12 腿 + 1 腰 + 8 臂），颈部保持零力矩
  • 腰部：WaistZ 单自由度
  • 双臂各 4 自由度：ShoulderY, ShoulderX, ShoulderZ, Elbow（不含 WristZ/Y/X）
  • 双腿各 6 自由度：HipY, HipX, HipZ, Knee, AnkleY, AnkleX
• 前 100 步对臂关节做平滑过渡，避免跳变
• 退出时等待推理线程结束

策略推理详情：

• 使用 ONNX Runtime CPU 推理
• 观测维度：730 维（10帧 × 73维/帧）
  • 单次观测 73 维：角速度(3) + 重力投影(3) + 速度指令(3) + 指令标志(1) + 关节位置偏差(21) + 关节速度(21) + 上一步动作(21)
• 动作输出：21 维（腿部12 + 腰部1 + 双臂8）

关节排列映射：

策略和机器人使用不同的关节排列顺序，通过名称映射自动计算置换索引：

策略输出顺序（output_order，重新排列为控制顺序）：

腰部(1, WaistZ) → 左臂(4: ShoulderY/X/Z + Elbow) → 右臂(4: ShoulderY/X/Z + Elbow) → 左腿(6) → 右腿(6)

默认姿态（按 output_order 排列，21 维）：

dof_pos_default_ = [0, 0, 0.15, 0, 1.35, 0, -0.15, 0, 1.35,
                   -0.1, 0, 0, 0.2, -0.1, 0, -0.1, 0, 0, 0.2, -0.1, 0]

策略输出处理：

1. 裁剪到 [-10, 10] 范围
2. 乘以 action_scale_ 得到位置增量
3. 加上 dof_pos_default_ 得到目标位置
4. 前 100 步臂关节平滑过渡（每步最大 0.01 rad 增量），避免从零位附近突然切换导致的跳变

默认参数：

最大指令速度 (max_cmd_vel_): [1.0, 0.5, 1.2]（前进 m/s, 侧移 m/s, 旋转 rad/s）
速度增量限制 (vel_delta_const_): [0.02, 0.02, 0.08]（平滑过渡）
角度缩放 (ang_vel_scale_): 0.2
速度缩放 (lin_vel_scale_): 2.0
关节速度缩放 (dof_vel_scale_): 0.1
观察历史帧数 (obs_history_num_): 10
决策分频 (decimation_): 10（2ms × 10 = 20ms 推理一次）

[!NOTE]

速度指令非线性特性：后退方向的速度响应被缩放到 0.6 倍

if (uc.forward_vel_scale < 0) user_cmd_vel[0] = 0.6f * uc.forward_vel_scale;

这意味着向后移动比向前移动更保守，实际后退速度约为前进的 60%。

6.4 JointDamping 状态

• 对所有 31 个关节施加纯阻尼力矩（仅设置 kd，kp = 0）
• 持续 3 秒后自动回到 Idle
• 退出时重置 target_mode