高层运动控制服务
约 4404 字大约 15 分钟
2025-05-29
提供机器人系统高层运动控制服务,通过 HighLevelMotionController 可以通过 RPC 通信方式实现对机器人的步态、特技、遥控的控制。
⚠️
接口定义
HighLevelMotionController
是面向语义控制的高层运动控制器,支持如行走、特技等控制操作,封装底层细节以供上层系统调用。
HighLevelMotionController
项目 | 内容 |
---|---|
函数名 | HighLevelMotionController |
函数声明 | HighLevelMotionController(); |
功能概述 | 构造函数,初始化高层控制器状态。 |
备注 | 构造内部控制资源。 |
~HighLevelMotionController
项目 | 内容 |
---|---|
函数名 | ~HighLevelMotionController |
函数声明 | virtual ~HighLevelMotionController(); |
功能概述 | 析构函数,释放控制器资源。 |
备注 | 配合 构造函数 使用。 |
Initialize
项目 | 内容 |
---|---|
函数名 | Initialize |
函数声明 | virtual bool Initialize() override; |
功能概述 | 初始化控制器,准备高层控制功能。 |
返回值 | true 表示成功,false 表示失败。 |
备注 | 首次使用前必须调用。 |
Shutdown
项目 | 内容 |
---|---|
函数名 | Shutdown |
函数声明 | virtual void Shutdown() override; |
功能概述 | 关闭控制器并释放资源。 |
备注 | 配合 Initialize 使用。 |
SetGait
项目 | 内容 |
---|---|
函数名 | SetGait |
函数声明 | Status SetGait(const GaitMode gait_mode); |
功能概述 | 设置机器人步态模式(如位控站立、力控站立、慢跑等)。 |
参数说明 | gait_mode :步态控制枚举。 |
返回值 | Status::OK 表示成功,其他为失败。 |
备注 | 阻塞接口,支持多种步态模式切换。 |
GetGait
项目 | 内容 |
---|---|
函数名 | GetGait |
函数声明 | Status GetGait(GaitMode& gait_mode); |
功能概述 | 获取机器人步态模式(如位控站立、力控站立、慢跑等)。 |
参数说明 | gait_mode :步态控制枚举。 |
返回值 | Status::OK 表示成功,其他为失败。 |
备注 | 阻塞接口,获取当前步态模式。 |
ExecuteTrick
项目 | 内容 |
---|---|
函数名 | ExecuteTrick |
函数声明 | Status ExecuteTrick(const TrickAction trick_action); |
功能概述 | 执行特技动作(如扭屁股、趴下等)。 |
参数说明 | trick_action :特技动作标识。 |
返回值 | Status::OK 表示成功,其他为失败。 |
备注 | 阻塞接口,需确保机器人当前处于可执行特技的状态。 |
- 注意事项:特技动作必须在 GaitMode::GAIT_STAND_R(2) 位控站立步态下才能执行
SendJoyStickCommand
项目 | 内容 |
---|---|
函数名 | SendJoyStickCommand |
函数声明 | Status SendJoyStickCommand(const JoystickCommand& joy_command); |
功能概述 | 发送实时摇杆控制指令。 |
参数说明 | joy_command :包含摇杆坐标的控制数据。 |
返回值 | Status::OK 表示成功,其他为失败。 |
备注 | 非阻塞接口,建议发送频率为 20Hz。 |
类型定义
JoystickCommand
— 高层运动控制摇杆指令结构体
字段名 | 类型 | 描述 |
---|---|---|
left_x_axis | float | 左侧摇杆的 X 轴方向值(-1.0:左,1.0:右) |
left_y_axis | float | 左侧摇杆的 Y 轴方向值(-1.0:下,1.0:上) |
right_x_axis | float | 右侧摇杆的 X 轴方向值(旋转 -1.0:左,1.0:右) |
right_y_axis | float | 右侧摇杆的 Y 轴方向值(暂未定义用途) |
枚举类型定义
GaitMode
— 机器人步态模式枚举
枚举值 | 数值 | 描述 |
---|---|---|
GAIT_PASSIVE | 0 | 掉落(关闭电机使能) |
GAIT_TRANS | -1 | 过渡状态 |
GAIT_STAND_R | 2 | 位控站立、RecoveryStand |
GAIT_STAND_B | 3 | 力控站立、姿态展示、BalanceStand |
GAIT_RUN_FAST | 8 | 快跑 |
GAIT_TROT | 10 | 小跑 |
GAIT_PRONK | 11 | 跳跃 |
GAIT_BOUND | 12 | 前后跳 |
GAIT_AMBLE | 14 | 交叉步 |
GAIT_CRAWL | 29 | 爬行 |
GAIT_LOWLEVL_SDK | 30 | 低层 SDK 步态 |
GAIT_WALK | 39 | 缓走 |
GAIT_UP_CLIMB_STAIRS | 56 | 上爬楼梯(全地形) |
GAIT_DOWN_CLIMB_STAIRS | 9 | 下爬楼梯=>盲走=>慢跑 |
GAIT_TEST_1 | 101 | 预留1 |
GAIT_TEST_2 | 102 | 预留2 |
GAIT_RL_TERRAIN | 110 | 全地形 |
GAIT_RL_FALL_RECOVERY | 111 | 跌倒爬起 |
GAIT_RL_HAND_STAND | 112 | 倒立 |
GAIT_RL_FOOT_STAND | 113 | 正立 |
GAIT_ENTER_RL | 1001 | 进入 RL |
GAIT_DEFAULT | 99 | 默认 |
GAIT_NONE | 9999 | 无步态 |
TrickAction
— 特技动作指令枚举
枚举值 | 数值 | 描述 |
---|---|---|
ACTION_NONE | 0 | 无动作 |
ACTION_WIGGLE_HIP | 26 | 扭屁股 |
ACTION_SWING_BODY | 27 | 甩身 |
ACTION_STRETCH | 28 | 伸懒腰 |
ACTION_STOMP | 29 | 跺脚 |
ACTION_JUMP_JACK | 30 | 开合跳 |
ACTION_SPACE_WALK | 31 | 太空步 |
ACTION_IMITATE | 32 | 模仿 |
ACTION_SHAKE_HEAD | 33 | 摇头 |
ACTION_PUSH_UP | 34 | 俯卧撑 |
ACTION_CHEER_UP | 35 | 加油 |
ACTION_HIGH_FIVES | 36 | 击掌 |
ACTION_SCRATCH | 37 | 挠痒 |
ACTION_HIGH_JUMP | 38 | 跳高 |
ACTION_SWING_DANCE | 39 | 摇摆舞 |
ACTION_LEAP_FROG | 40 | 蛙跳 |
ACTION_BACK_FLIP | 41 | 后空翻 |
ACTION_FRONT_FLIP | 42 | 前空翻 |
ACTION_SPIN_JUMP_LEFT | 43 | 旋转左跳70度 |
ACTION_SPIN_JUMP_RIGHT | 44 | 旋转右跳70度 |
ACTION_JUMP_FRONT | 45 | 前跳0.5米 |
ACTION_ACT_CUTE | 46 | 撒娇 |
ACTION_BOXING | 47 | 打拳 |
ACTION_SIDE_SOMERSAULT | 48 | 侧空翻 |
ACTION_RANDOM_DANCE | 49 | 随机跳舞 |
ACTION_LEFT_SIDE_SOMERSAULT | 84 | 左侧侧空翻 |
ACTION_RIGHT_SIDE_SOMERSAULT | 85 | 右侧侧空翻 |
ACTION_DANCE2 | 91 | 跳舞2 |
ACTION_DEBUG_TEST_2 | 92 | 预留2 |
ACTION_DEBUG_TEST_3 | 93 | 预留3 |
ACTION_EMERGENCY_STOP | 101 | 急停 |
ACTION_LIE_DOWN | 102 | 趴下 |
ACTION_RECOVERY_STAND | 103 | 起立 |
ACTION_HAPPY_NEW_YEAR | 105 | 拜年=作揖 |
ACTION_SLOW_GO_FRONT | 108 | 过来 |
ACTION_SLOW_GO_BACK | 109 | 后退 |
ACTION_BACK_HOME | 110 | 回窝 |
ACTION_LEAVE_HOME | 111 | 离窝 |
ACTION_TURN_AROUND | 112 | 转圈 |
ACTION_DANCE | 115 | 跳舞 |
ACTION_ROLL_ABOUT | 116 | 打滚 |
ACTION_SHAKE_RIGHT_HAND | 117 | 握右手 |
ACTION_SHAKE_LEFT_HAND | 118 | 握左手 |
ACTION_SIT_DOWN | 119 | 坐下 |
遥控器示意图
- 左右摇杆 x 轴和 y 轴的取值范围为 [-1.0, 1.0];
- 左右摇杆 x 轴和 y 轴的方向上/右为正,如示意图所示;
高层运动控制机器人状态介绍
机器人的运动包含位控站立、力控站立、基础运动、特技动作状态,机器人在运行过程中,通过状态机在不同状态之间进行切换,以实现不同的控制任务。各个状态的解释说明如下:
- 位控站立:在位控站立状态下,可调用 SDK 的各部分接口实现机器人的特技动作和基础运动控制。
- 力控站立:在力控站立状态下,可以用于姿态展示。
- 基础运动:在运动执行过程中,可调用 SDK 接口,让机器人进入不同的步态。
- 特技动作:当进入特殊动作执行状态后,其他运动控制服务会先被挂起,等待当前动作执行完毕并进入平衡站立状态后再生效。
机器人状态切换机制;
高层运动控制接口
机器人的高层运动控制服务可分为基础运动控制和特技动作控制。
- 基础运动控制服务中,可调用相应的接口,根据不同的地形场景和任务需求切换机器人的行走步态。
- 特技动作控制服务中,可调用相应的接口,实现机器人内置的特殊特技,比如扭屁股、趴下等。
步态切换
机器人当前步态 | 步态切换流程 | 示意图 |
---|---|---|
力控站立 | ![]() | ![]() |
慢跑模式 | ![]() | ![]() |
小跑模式 | ![]() | ![]() |
特技执行
机器人特技 | 特技执行流程 | 示意图 |
---|---|---|
扭屁股 | ![]() | ![]() |
伸懒腰 | ![]() | ![]() |
跺脚 | ![]() | ![]() |
开合跳 | ![]() | ![]() |
太空步 | ![]() | ![]() |
摇头 | ![]() | ![]() |
俯卧撑 | ![]() | ![]() |
加油 | ![]() | ![]() |
击掌 | ![]() | ![]() |
挠痒 | ![]() | ![]() |
后空翻 | ![]() | ![]() |
旋转左跳70度 | ![]() | ![]() |
旋转右跳70度 | ![]() | ![]() |
前跳0.5米 | ![]() | ![]() |
打拳 | ![]() | ![]() |
转圈 | ![]() | ![]() |
跳舞 | ![]() | ![]() |
握右手 | ![]() | ![]() |
握左手 | ![]() | ![]() |
坐下 | ![]() | ![]() |