机器人坐下后拍触摸板站起行动指令无效 — Bug 分析

坐下后拍触摸板站起行动指令无效 --- Bug 分析

一、场景描述

1. 机器人坐下（语音或 App 指令）
2. 拍两下触摸板使其站起
3. 下达行动指令（前进/后退/旋转）
4. 机器人有回复但不执行，原地不动
5. 再次坐下再站起后恢复

---

二、核心概念：MC Action ID

机器人运动控制器（MC）通过 curr_action_id 表示当前姿态。四足相关取值：

ID	常量名	中文含义	能否行走
101	QUADRUPED_STAND_DEFAULT	四足站立	否
102	QUADRUPED_LOCOMOTION_DEFAULT	四足行走态	是
107	QUADRUPED_LOCOMOTION_HANDSHAKE	握手	否
110	QUADRUPED_GET_DOWN_DEFAULT	趴下	否
111	QUADRUPED_SIT_DOWN_DEFAULT	坐下	否

只有 curr_action_id = 102（行走态）时，MC 才会响应移动速度指令。

---

三、完整调用链

步骤 1：用户说/点"坐下"

App 发 PlayAnimation animation_id=3 (kSitDown)
  → T1AnimationTaskDescription::GetSkillParamList(3)
    → GetQuadrupedAnimationParam(kSitDown)
      → action_id = 111 (QUADRUPED_SIT_DOWN_DEFAULT)
  → ActionSkill Exec → 发 RPC 给 MC → MC 执行坐下
  → curr_action_id 变为 111

步骤 2：拍触摸板站起

触摸传感器检测到双击/三连拍 → ROS2 topic 发布 TouchState → Scheduler 回调 → DispatchTouchStatus → CreateTaskTouch

文件 ：t1_interaction.cpp:85-118

std::shared_ptr<SkillParamList>
T1InteractionTaskDescription::GetClickQuadrupedSkillParamList() {

    auto motion_state = StateManager::GetInstance()->GetMotionState();
    int32_t action_id = 0;

    if (motion_state.curr_action_id == QUADRUPED_SIT_DOWN_DEFAULT) {   // 当前坐着(111)
        action_id = QUADRUPED_STAND_DEFAULT;                           // → 目标站起(101)
        audio_params->SetAudioFileNameQuadruped("坐下.wav");
    } else if (motion_state.curr_action_id == QUADRUPED_STAND_DEFAULT) { // 当前站着(101)
        action_id = QUADRUPED_SIT_DOWN_DEFAULT;                          // → 目标坐下(111)
    } else {
        action_id = QUADRUPED_SIT_DOWN_DEFAULT;  // 其他状态默认坐下
    }

    action_params->SetActionId(action_id);  // → 设置目标为 101
    // Push 到 SkillParamList → Worker 执行 → ActionSkill → MC
}

执行结果 ：MC 收到 SetMcAction(101)，机器人站起。curr_action_id = 101。

步骤 3：下达行动指令（PlayMove）

App 下发 PlayMove → Scheduler::PlayMoveService → CheckMove 通过 → DispatchMove → TaskFactory::CreateTaskMove → T1MoveTaskDescription::GetSkillParamList

文件 ：t1_move_task_description.cpp:98-108

std::shared_ptr<SkillParamList>
T1MoveTaskDescription::GetSkillParamList(step, distance, direction) {

    auto curr_action_id = StateManager::GetInstance()->GetMotionState().curr_action_id;
    // curr_action_id = 101 (刚站起，还在 STAND_DEFAULT！)

    // ★ 关键检查：当前状态是否需要先过渡到行走态(102)
    if ((curr_action_id == QUADRUPED_GET_DOWN_DEFAULT         // 110 趴下
         || curr_action_id == QUADRUPED_SIT_DOWN_DEFAULT       // 111 坐下
         || curr_action_id == QUADRUPED_STAND_DEFAULT          // 101 站立 ← 新加的！
         || curr_action_id == QUADRUPED_LOCOMOTION_HANDSHAKE)  // 107 握手
        && direction != Direction::kStopWalk) {

        // 注入过渡 Action: 先切到 QUADRUPED_LOCOMOTION_DEFAULT (102)
        auto action_params = std::make_shared<ActionParams>();
        action_params->SetActionId(QUADRUPED_LOCOMOTION_DEFAULT); // 102
        action_params->SetCheckSet(true);   // 阻塞等待 MC 确认
        skill_param_list->PushSkillParam(action_params);
    }

    // 然后才计算速度、方向，Push MoveParams
    // ...
}

这个函数生成的 Skill 序列（以从站立 101 移动为例）：

Skill 1: ActionParams {action_id=102, check_set=true}
         → ActionSkill 发 SetMcAction(102) 给 MC
         → 阻塞等待 MC 确认 curr_action_id 变为 102
Skill 2: MoveParams {velocity, duration}
         → MoveSkill 循环 pub 速度指令给 MC
         → MC 当前 action=102（行走态）→ 执行移动！

---

四、Bug 根因

修改前的代码（有 Bug）

if ((curr_action_id == QUADRUPED_GET_DOWN_DEFAULT         // 110
     || curr_action_id == QUADRUPED_SIT_DOWN_DEFAULT       // 111
     || curr_action_id == QUADRUPED_LOCOMOTION_HANDSHAKE)  // 107
    // ← 没有 101！
    && direction != Direction::kStopWalk) {

白名单是 {110, 111, 107}。拍触摸板站起后 curr_action_id = 101，不在白名单中。

错误流程

拍触摸板站起 → curr_action_id = 101 (STAND_DEFAULT)
  ↓
App 下发 PlayMove
  ↓
GetSkillParamList 检查：101 ∈ {110, 111, 107}？
  ↓
→ false！不注入 Action(102) 过渡
  ↓
直接生成 MoveParams，发速度指令给 MC
  ↓
MC: curr_action = 101 (站立)，不是 102 (行走态)
  ↓
MC 忽略速度指令 → 机器人不动 ❌

为什么坐下趴下后恢复？

再次让机器人坐下 → curr_action_id = 111，111 在白名单中 → 下次 PlayMove 自动注入 Action(102) → 切到行走态 → 移动恢复。

---

五、修复

文件 ：t1_move_task_description.cpp:103

在白名单中增加 QUADRUPED_STAND_DEFAULT（101）：

if ((curr_action_id == QUADRUPED_GET_DOWN_DEFAULT         // 110 趴下
     || curr_action_id == QUADRUPED_SIT_DOWN_DEFAULT       // 111 坐下
     || curr_action_id == QUADRUPED_STAND_DEFAULT          // 101 站立 ← 新增
     || curr_action_id == QUADRUPED_LOCOMOTION_HANDSHAKE)  // 107 握手
    && direction != Direction::kStopWalk) {
    // 自动注入 Action(102)，过渡到行走态
    action_params->SetActionId(QUADRUPED_LOCOMOTION_DEFAULT); // 102
    action_params->SetCheckSet(true);
    skill_param_list->PushSkillParam(action_params);
}

修复后的流程

拍触摸板站起 → curr_action_id = 101 (STAND_DEFAULT)
  ↓
App 下发 PlayMove
  ↓
GetSkillParamList 检查：101 ∈ {110, 111, 101, 107}？
  ↓
→ true！注入 Action(102) 过渡
  ↓
Skill 1: ActionSkill → SetMcAction(102) → 阻塞等 MC 确认 → MC 切到行走态
Skill 2: MoveSkill → 发速度指令 → MC 执行移动
  ↓
机器人正常行走 ✅

---

六、涉及文件汇总

文件	行号	作用
t1_interaction.cpp	85-118	触摸响应：坐着→站起(101)
t1_move_task_description.cpp	98-108	移动前自动注入行走态过渡(102)
action_skill.cpp	24-84	发送 SetMcAction + check_set 阻塞等待

---

七、一句话总结

拍触摸板站起后机器人处于 STAND_DEFAULT(101)（站立）而非 LOCOMOTION_DEFAULT(102)（行走态），移动代码的白名单漏了 101，导致没有自动先切到 102 再发速度指令。MC 在站立态不响应移动指令。修复是在白名单补上 101。