机器人旋转变形逻辑分析

机器人变形逻辑分析

概述

本文档从代码层面详细描述机器人执行"旋转变形"（kTransformateRotate，animation_id=8）的完整流程，从用户说出"变形"到机器人完成物理变形动作。

涉及的核心模块

关键数据结构

T1Animation 枚举: kTransformateRotate = 8 // 旋转变形 MC Action ID: 101 = QUADRUPED_STAND_DEFAULT // 四足站立 102 = QUADRUPED_LOCOMOTION_DEFAULT // 四足行走态 110 = QUADRUPED_GET_DOWN_DEFAULT // 四足趴下 111 = QUADRUPED_SIT_DOWN_DEFAULT // 四足坐下 300 = BIPED_LOCOMOTION_WBC // 双足WBC控制 21 = BIPED_TO_QUADRUPED_ROTATE // 双足转四足（旋转） RobotForm: kQuadruped = 四足形态 kBiped = 双足形态

第一阶段：RPC 入口与前置校验

步骤 1：ROS2 RPC 回调触发

文件: scheduler.cpp:254-267

aimrt::rpc::StatusScheduler::PlayAnimationService(constaimdk_msgs::srv::PlayAnimation::Request&req,aimdk_msgs::srv::PlayAnimation::Response&res){AIMRTE_INFO("Received PlayAnimation request with animation_id：{}",req.animation_id);// 日志: [20:45:29.299] Received PlayAnimation request with animation_id：8

当 App/语音/遥控器通过 ROS2 调用PlayAnimationRPC 服务时，Scheduler 的PlayAnimationService回调被触发。req.animation_id = 8。

步骤 2：前置校验 — CheckAnimation

文件: scheduler.cpp:257

if(dispatcher_->CheckAnimation(req.animation_id)!=kSuccess){res.success=false;return{};}

文件: t1_checker.cpp:25-58

Checker 按顺序执行 5 道检查：

第 1 关：形态是否支持此动画？

if(notCheckAnimationSupport(animation))returnkFormNotSupportCurrAnimation;

• 四足支持列表：kGreet, kHandshake, kSitDown, kLieDown, kStandUp,kTransformateRotate, kProud, kJoy, kStretch, kDance
• 双足支持列表：kGreet, kHandshake, kBothHandsMakeHeart, kDance,kTransformateRotate, kPhotoPose_1~4, kPhotoThreeShot, kTalking, kProud, kJoy, kRightHandPunch, kLeftHandPunch, kBipedDefaultPos
• kTransformateRotate(8) 在两个列表中都有，无论当前什么形态都能通过

第 2 关：四足变形条件（四足形态 + kTransformateRotate 时才检查）

if(notCheckQuadrupedTransformateCondition(animation))returnkTransformateConditionNotMet;

只在 curr_robot_form == kQuadruped 且 animation == kTransformateRotate 时检查。要求当前 curr_action_id 为以下之一：

• QUADRUPED_STAND_DEFAULT (101)
• QUADRUPED_LOCOMOTION_DEFAULT (102)
• QUADRUPED_LOCOMOTION_TERRAIN (103)
• QUADRUPED_LOCOMOTION_RUN (112)
• QUADRUPED_GET_DOWN_DEFAULT (110)
• QUADRUPED_SIT_DOWN_DEFAULT (111)

这些是"稳定姿态"，不在这些状态下禁止变形（防止运动中变形摔倒）。

第 3 关：双足状态前置 action 检查

if(notCheckBipedAction())returnkBipedActionNotSupportAni;

只在双足形态下检查，要求当前 curr_action_id 为：

• QUADRUPED_TO_BIPED (10)
• QUADRUPED_TO_BIPED_ROTATE (11)
• BIPED_LOCOMOTION_DEFAULT (301)
• BIPED_LOCOMOTION_TERRAIN (302)
• BIPED_LOCOMOTION_WBC (300)

第 4 关：保底动作豁免（双足变形直接放行！）

if(animation==kSitDown||animation==kLieDown||(animation==kTransformateRotate&&curr_robot_form==kBiped)){returnkSuccess;// 直接通过，跳过电量检查！}

双足形态下的 kTransformateRotate 在此处直接返回成功，跳过第 5/6 关的电量检查。这是设计意图：双足变形被视为必须响应的"保底动作"。

第 5 关：电量/充电拦截（全部动画）

boolis_easy_limit=(!IsIgnoreLowBatteryLimit()&&last_battery_level_audio>=4)||is_charging;if(is_easy_limit)returnkFormNotSupportCurrAnimation;

第 6 关：电量/充电拦截（仅复杂动作）

boolis_complex_limit=(同上条件);if(is_complex_limit&&!IsEasyMotion(animation))returnkFormNotSupportCurrAnimation;

只有"简单动作"（kGreet, kHandshake, kBothHandsMakeHeart, 拍照系列, 碰拳）在低电量/充电时允许执行。kTransformateRotate 不是简单动作，但如果走到这里（四足形态），充电时会被拦截。

步骤 3：投递到线程池

文件: scheduler.cpp:262-264

option_.exe_.Post([this,animation_id=req.animation_id,interrupt=req.interrupt](){dispatcher_->DispatchAnimation(animation_id,interrupt);});res.success=true;// 立即返回成功（异步执行）

RPC 立即返回成功给调用方，实际动画执行在线程池中异步处理。

第二阶段：任务创建

步骤 4：Dispatcher 分发

文件: dispatcher.cpp:350-359

voidDispatcher::DispatchAnimation(int32_tanimation_id,boolinterrupt){autotask=TaskFactory::GetInstance()->CreateTaskAnimation(animation_id,interrupt);if(task==nullptr){AIMRTE_WARN("创建Animation任务失败");return;}AIMRTE_INFO("创建Animation task成功， animation_id: {}, interrupt: {}",animation_id,interrupt);worker_manager_->ExecTask(task);}

步骤 5：TaskFactory 创建 Task

文件: task_factory.cpp:110-127

std::shared_ptr<Task>TaskFactory::CreateTaskAnimation(constint32_t&animation_id,boolinterrupt){// 第1步：从 TaskDescriptionManager 获取"说明书"autotask_description=TaskDescriptionManager::GetInstance()->GetTaskDescription(robot_type_,TaskDescriptionType::Animation);// 第2步：查说明书，生成技能清单autoskill_param_list=task_description->GetSkillParamList(animation_id,interrupt);// 第3步：打包成 Task 对象autotask=std::make_shared<Task>();task->SetType(TaskType::AnimationTask);task->SetSkillParamList(*skill_param_list);// 技能清单元塞入 TaskSetDefaultPriorities(task);returntask;}

步骤 6：TaskDescriptionManager 两层查找

文件: task_description_manager.cpp:50-87

数据结构：

TaskDescriptionManager (全局单例) └─ root_task_description_: map<RobotType, RobotTaskDescription> ├─ [T1] → T1TaskDescription │ └─ task_description_map_: map<TaskDescriptionType, TaskDescription> │ ├─ [Animation] → T1AnimationTaskDescription ← 变形用这个 │ ├─ [Move] → T1MoveTaskDescription │ ├─ [Interaction] → T1InteractionTaskDescription │ └─ ... └─ [Q1] → Q1TaskDescription └─ task_description_map_: ├─ [Animation] → Q1AnimationTaskDescription └─ ...

查找过程（第 80-87 行）：

GetTaskDescription(RobotType::T1,TaskDescriptionType::Animation){// 第一层：按机器人类型找autoit=root_task_description_.find(T1);// → T1TaskDescription// 第二层：按任务类型找returnit->second->GetTaskDescription(Animation);// → T1AnimationTaskDescription}

初始化注册（第 50-57 行）：

voidTaskDescriptionManager::Init(){root_task_description_[T1]=newT1TaskDescription();root_task_description_[Q1]=newQ1TaskDescription();root_task_description_[T1]->RegisterTaskDescription(Animation,newT1AnimationTaskDescription());// T1 注册动画说明书// ... 其他类型}

第三阶段：构造技能清单（关键分叉点）

步骤 7：GetSkillParamList — 根据形态分叉

文件: t1_animation_task_description.cpp:35-47

std::shared_ptr<SkillParamList>GetSkillParamList(int32_tanimation_id,boolinterrupt){autoanimation=static_cast<T1Animation>(animation_id);// 8 → kTransformateRotateautoskill_param_list=std::make_shared<SkillParamList>();if(curr_robot_form==kQuadruped){GetQuadrupedAnimationParam(animation,skill_param_list);// 路径 A}elseif(curr_robot_form==kBiped){GetBipedAnimationParam(animation,skill_param_list);// 路径 B}returnskill_param_list;}

这是整个变形流程最关键的分叉点！curr_robot_form的值决定走哪条路径，而curr_robot_form是 ROS2 异步回调更新的（见步骤 12），可能存在延迟。

步骤 8A：路径 A — 四足变双足

文件: t1_animation_task_description.cpp:73-135

casekTransformateRotate:target_action_id=300;// BIPED_LOCOMOTION_WBC（最终目标）

然后执行注入逻辑（第 119-134 行），按顺序 Push 技能：

Skill 序列: ┌─────────────────────────────────────────────────────────┐ │ 1. ActionParams │ │ action_id = 102 (QUADRUPED_LOCOMOTION_DEFAULT) │ │ check_set = true ← 阻塞等待 MC 确认！ │ │ 作用：确保机器人先站起到行走态，再变形 │ │ │ │ 2. AudioParams │ │ quadruped_audio = "transform4to2.wav" (四变双音效) │ │ biped_audio = "transform2to4.wav" (双变四音效) │ │ 实际播哪个由 AudioSkill 根据当前形态决定 │ │ │ │ 3. MotorParams │ │ motor_type = Biped (切双足舵机) │ │ │ │ 4. ActionParams │ │ action_id = 300 (BIPED_LOCOMOTION_WBC) │ │ check_set = true ← 阻塞等待 MC 确认！ │ │ 作用：最终目标，进入双足 WBC 控制 │ └─────────────────────────────────────────────────────────┘

音频延迟的关键原因：Action(102, check_set=true) 排在 Audio 前面。ActionSkill 会用 while+sleep(100ms) 循环轮询等待 MC 确认 action 切换完成。如果 MC 从非行走态（如趴下 110）切换到行走态 102 需要 ~3 秒，音频就被卡了 3 秒。

步骤 8B：路径 B — 双足变四足

文件: t1_animation_task_description.cpp:180-205

casekTransformateRotate:{// 如果当前不是 WBC，先切到 WBCif(curr_action_id!=300){PushActionParams(300,check_set=false);// 不阻塞！}// 旋转变形动作PushActionParams(21,check_set=false);// 不阻塞！(BIPED_TO_QUADRUPED_ROTATE)// 音频PushAudioParams(transform4to2/transform2to4);// 电机切四足PushMotorParams(Quad);return;// 注意：直接 return，不走后面的通用逻辑}

Skill 序列: ┌─────────────────────────────────────────────────────────┐ │ 1. ActionParams (条件注入) │ │ action_id = 300 (BIPED_LOCOMOTION_WBC) │ │ check_set = false ← 不阻塞！发完 RPC 直接返回 │ │ │ │ 2. ActionParams │ │ action_id = 21 (BIPED_TO_QUADRUPED_ROTATE) │ │ check_set = false ← 不阻塞！ │ │ │ │ 3. MotorParams │ │ motor_type = Quad (切四足舵机) │ │ │ │ 4. AudioParams │ │ quadruped_audio = "transform4to2.wav" │ │ biped_audio = "transform2to4.wav" │ │ 实际播哪个由 AudioSkill 根据当前形态决定 │ └─────────────────────────────────────────────────────────┘

双足变形没有延迟的原因：所有 Action 都没设 check_set=true，发完 RPC 直接过。音频虽然排在最后，但前面没有阻塞等待。

两条路径的核心差异

第四阶段：Worker 调度与任务执行

步骤 9：WorkerManager 仲裁与分发

文件: worker_manager.cpp:52-149

boolWorkerManager::ExecTask(conststd::shared_ptr<Task>&task){// 1. 规则检查if(!ApplyRule(task))returnfalse;// 2. 仲裁：与当前运行的 Worker 比较优先级// 如果新任务优先级更高 → 停止旧 Worker// 如果被拒绝 → 返回 false// 3. 创建新 Worker，绑定 Taskautoworker=std::make_shared<Worker>();worker->SetCurrentTask(task);active_workers_[AnimationTask]=worker;// 4. 投递到线程池执行option_.exe_.Post([worker,task](){worker->ExecTask(task);});}

步骤 10：Worker 驱动 Task 执行

文件: worker.cpp:17-36

voidWorker::ExecTask(conststd::shared_ptr<Task>&task){current_task_=task;task->Run();// 同步执行，阻塞到全部技能完成或被打断current_task_=nullptr;}

步骤 11：Task::Run — 按序执行技能清单

文件: task.cpp:37-73

voidTask::Run(){autoskill_params=skill_param_list_.GetSkillsParam();task_state_=TaskState::Running;param_index_=0;while(param_index_<skill_params.size()){if(task_state_==TaskState::Stop){sleep(100ms);// 被中断时等待continue;}autoskill_param=skill_params[param_index_];autoskill=SkillManager::GetInstance()->GetSkill(skill_param->GetType());if(skill){skill->Exec(skill_param.get());// ← 同步调用，逐个执行}++param_index_;}task_state_=TaskState::Ready;}

Worker 在一个单独的线程中同步执行 Task::Run()，按 SkillParamList 的顺序逐个调用 skill->Exec()。这是同步阻塞模型——每个 skill 的 Exec() 必须返回，下一个才能执行。

第五阶段：技能执行

步骤 12A：ActionSkill::Exec — 动作切换

文件: action_skill.cpp:24-84

boolActionSkill::Exec(SkillParam*param){ActionParams*action_param=dynamic_cast<ActionParams*>(param);int32_ttarget_action_id=action_param->GetActionId();// 例如 102auto&motion_state=state_manager_->GetMotionState();// 快速路径：如果 MC 已经是目标 action，直接返回if(motion_state.curr_action_id==target_action_id){AIMRTE_INFO("当前mc已是目标Action, 无需执行");returntrue;// O(1) 返回}// 发 RPC 给 MC：请切换到 target_action_idreq.command.action.value=target_action_id;autores=option_.set_mc_action_proxy.Func.Call(rpc_ctx,req,resp).Sync();// 关键：如果 check_set=true，进入同步轮询等待if(action_param->GetCheckSet()){uint32_tindex=0;sleep(100ms);while(index<50){// 最多 50 次 = 5 秒超时motion_state=state_manager_->GetMotionState();if(motion_state.curr_action_id==target_action_id){break;// MC 确认了，退出}// 没确认？重发 RPC，睡 100ms，再检查++index;sleep(100ms);}// 超时检查if(motion_state.curr_action_id!=target_action_id){AIMRTE_ERROR("超时未能设置成功");returnfalse;}}AIMRTE_INFO("执行ActionSkill成功, action_id: {}, curr_action_id: {}",target_action_id,motion_state.curr_action_id);returntrue;}

执行逻辑：

1. 先检查 curr_action_id（StateManager 缓存）是否已经是目标值，是则直接返回
2. 向 MC 发送 SetMcAction RPC
3. 如果 check_set=true，进入 while+sleep(100ms) 轮询，等待 MC 通过 ROS2 topic 确认切换完成
4. MC 确认后返回，否则最多等 5 秒超时

curr_action_id 缓存的更新路径：

MC 执行完动作切换后，通过 ROS2 topic 发布McCommonState消息 → Scheduler 订阅回调OnMcCommonStatus（scheduler.cpp:105）→ 投递到线程池执行DispatchMcCommonStatus（dispatcher.cpp:100）→ 更新motion_state.curr_action_id。

步骤 12B：AudioSkill::Exec — 音频播放

文件: audio_skill.cpp:24-82

boolAudioSkill::Exec(SkillParam*param){AudioParams*audio_param=dynamic_cast<AudioParams*>(param);// 关键：根据当前形态选择音频文件std::string audio_file;if(motion_state.curr_robot_form==kQuadruped){audio_file=audio_param->GetAudioFileNameQuadruped();// "transform4to2.wav"}else{audio_file=audio_param->GetAudioFileNameBiped();// "transform2to4.wav"}// 构造 RPC 请求，发给 HAL 音频服务播放req.file.file_name=audio_file;req.file.file_path="/robot/data/var/hal_audio/file/";autores=option_.play_audio_file_proxy.Func.Call(rpc_ctx,req,resp).Sync();AIMRTE_INFO("播放音频文件成功, audio_file: {}",audio_file);returntrue;}

音频选择依赖curr_robot_form的当前值——如果该值因 ROS2 异步延迟而滞后，就会选错音频文件。

步骤 12C：MotorSkill::Exec — 舵机切换

文件: motor_skill.cpp:23-60

boolMotorSkill::Exec(SkillParam*param){MotorParams*motor_param=dynamic_cast<MotorParams*>(param);Q1MotorType motor_type=motor_param->GetMotorType();if(motor_type==Quad){req.position=0;// 舵机位置 0° = 四足模式}elseif(motor_type==Biped){req.position=90;// 舵机位置 90° = 双足模式}autores=option_.set_servo_proxy.Func.Call(rpc_ctx,req,resp).Sync();AIMRTE_INFO("执行MotorSkill成功, motor_type: {}",motor_type);returntrue;}

发送舵机位置指令给硬件，90° 对应双足模式，0° 对应四足模式。

第六阶段：状态同步

步骤 13：MC 状态回调

文件: dispatcher.cpp:100-109

voidDispatcher::DispatchMcCommonStatus(int32_taction_id,uint32_tcurrent_form,...){auto&motion_state=state_manager_->GetMotionState();// 去重：与上次一致则忽略if(motion_state.curr_action_id==action_id&&motion_state.curr_robot_form==current_form){return;}// 更新状态缓存motion_state.curr_robot_form=static_cast<RobotForm>(current_form);motion_state.curr_action_id=action_id;}

MC 执行完动作后通过 ROS2 topic 异步推送状态更新。此回调更新 StateManager 中的curr_robot_form和curr_action_id。

完整时序图

用户说"变形" │ ▼ PlayAnimationService [scheduler.cpp:254] ROS2 RPC 回调 │ ├─ CheckAnimation [t1_checker.cpp:25] 6道前置检查 │ ├─ 形态支持？ │ ├─ 变形条件？ │ ├─ 双足action合法？ │ ├─ 双足变形？→ 直接放行（跳过电量检查） │ ├─ 电量/充电拦截 │ └─ 复杂动作限制 │ ├─ Post 到线程池 [scheduler.cpp:262] │ ▼ DispatchAnimation [dispatcher.cpp:350] │ ├─ CreateTaskAnimation [task_factory.cpp:110] │ ├─ GetTaskDescription [task_description_manager.cpp:80] 两层map查找 │ │ → T1AnimationTaskDescription │ │ │ └─ GetSkillParamList [t1_animation_task_description.cpp:35] │ │ │ ├─ curr_robot_form == kQuadruped？ │ │ └─ GetQuadrupedAnimationParam │ │ Skill序列: [Action102阻塞, Audio, MotorBiped, Action300阻塞] │ │ │ └─ curr_robot_form == kBiped？ │ └─ GetBipedAnimationParam │ Skill序列: [Action300不阻塞, Action21, MotorQuad, Audio] │ ├─ ExecTask(task) [worker_manager.cpp:52] │ ├─ 规则检查 │ ├─ 仲裁（优先级比较） │ └─ Post Worker 到线程池 │ ▼ Worker::ExecTask [worker.cpp:17] │ └─ Task::Run() [task.cpp:37] │ └─ while 循环逐 Skill 执行: │ ├─ [Skill1] ActionSkill::Exec() [action_skill.cpp:24] │ ├─ MC已到目标? → 跳过 │ ├─ 发SetMcAction RPC │ └─ check_set=true? → while+sleep(100ms)轮询 ← 阻塞点！ │ ├─ [Skill2] AudioSkill::Exec() [audio_skill.cpp:24] │ ├─ 读 curr_robot_form → 选音频 │ └─ 发PlayAudioFile RPC → HAL播放 │ ├─ [Skill3] MotorSkill::Exec() [motor_skill.cpp:23] │ └─ 发SetServo RPC → 切舵机 │ └─ [Skill4] ActionSkill::Exec() [action_skill.cpp:24] └─ 最终目标action （异步）MC 执行物理动作 │ └─ ROS2 topic → OnMcCommonStatus └─ DispatchMcCommonStatus [dispatcher.cpp:100] └─ 更新 curr_robot_form / curr_action_id

涉及的全部源文件