高层运动控制示例
约 1962 字大约 7 分钟
2025-05-29
该示例展示了如何使用 Magicbot-Gen1 SDK 进行初始化、连接机器人、高层运动控制（步态、特技、遥控）等基本操作。
C++
示例文件： high_level_motion_example.cpp
参考文档： high_level_motion_reference.md
#include "magic_robot.h"
#include <termios.h>
#include <unistd.h>
#include <csignal>
#include <iostream>
using namespace magic::gen1;
magic::gen1::MagicRobot robot;
void signalHandler(int signum) {
std::cout << "Interrupt signal (" << signum << ") received.\n";
robot.Shutdown();
// 退出进程
exit(signum);
}
void print_help(const char* prog_name) {
std::cout << "按键功能演示程序\n\n";
std::cout << "用法: " << prog_name << "\n";
std::cout << "按键功能说明:\n";
std::cout << " ESC 退出程序\n";
std::cout << " 1 功能1:锁定站立\n";
std::cout << " 2 功能2:平衡站立\n";
std::cout << " 3 功能3:执行特技-庆祝动作\n";
std::cout << " w 功能4:向前移动\n";
std::cout << " a 功能5:向左移动\n";
std::cout << " s 功能6:向后移动\n";
std::cout << " d 功能7:向右移动\n";
std::cout << " x 功能8:向右移动\n";
std::cout << " t 功能9:左转向\n";
std::cout << " g 功能10:右转向\n";
}
int getch() {
struct termios oldt, newt;
int ch;
tcgetattr(STDIN_FILENO, &oldt); // 获取当前终端设置
newt = oldt;
newt.c_lflag &= ~(ICANON | ECHO); // 关闭缓冲和回显
tcsetattr(STDIN_FILENO, TCSANOW, &newt);
ch = getchar(); // 读取按键
tcsetattr(STDIN_FILENO, TCSANOW, &oldt); // 恢复设置
return ch;
}
void RecoveryStand() {
// 获取高层运控控制器
auto& controller = robot.GetHighLevelMotionController();
// 设置步态
auto status = controller.SetGait(GaitMode::GAIT_RECOVERY_STAND);
if (status.code != ErrorCode::OK) {
std::cerr << "set robot gait failed"
<< ", code: " << status.code
<< ", message: " << status.message << std::endl;
return;
}
}
void BalanceStand() {
// 获取高层运控控制器
auto& controller = robot.GetHighLevelMotionController();
// 设置姿态展示步态
auto status = controller.SetGait(GaitMode::GAIT_BALANCE_STAND);
if (status.code != ErrorCode::OK) {
std::cerr << "set robot gait failed"
<< ", code: " << status.code
<< ", message: " << status.message << std::endl;
return;
}
std::cout << "robot gait set to GAIT_BALANCE_STAND successfully." << std::endl;
}
void ExecuteTrick() {
// 获取高层运控控制器
auto& controller = robot.GetHighLevelMotionController();
// 执行特技
auto status = controller.ExecuteTrick(TrickAction::ACTION_CELEBRATE);
if (status.code != ErrorCode::OK) {
std::cerr << "execute robot trick failed"
<< ", code: " << status.code
<< ", message: " << status.message << std::endl;
return;
}
std::cout << "robot trick executed successfully." << std::endl;
}
void JoyStickCommand(float left_x_axis,
float left_y_axis,
float right_x_axis,
float right_y_axis) {
// 获取高层运控控制器
auto& controller = robot.GetHighLevelMotionController();
JoystickCommand joy_command;
joy_command.left_x_axis = left_x_axis;
joy_command.left_y_axis = left_y_axis;
joy_command.right_x_axis = right_x_axis;
joy_command.right_y_axis = right_y_axis;
auto status = controller.SendJoyStickCommand(joy_command);
if (status.code != ErrorCode::OK) {
std::cerr << "execute robot trick failed"
<< ", code: " << status.code
<< ", message: " << status.message << std::endl;
usleep(50000);
return;
}
// 等待50ms
usleep(50000);
}
int main(int argc, char* argv[]) {
// 绑定 SIGINT（Ctrl+C）
signal(SIGINT, signalHandler);
std::string local_ip = "192.168.54.111";
// 配置本机网线直连机器的IP地址，并进行SDK初始化
if (!robot.Initialize(local_ip)) {
std::cerr << "robot sdk initialize failed." << std::endl;
robot.Shutdown();
return -1;
}
// 设置rpc超时时间为5s
robot.SetTimeout(5000);
// 连接机器人
auto status = robot.Connect();
if (status.code != ErrorCode::OK) {
std::cerr << "connect robot failed"
<< ", code: " << status.code
<< ", message: " << status.message << std::endl;
robot.Shutdown();
return -1;
}
// 切换运控控制器为底层控制器，默认是高层控制器
status = robot.SetMotionControlLevel(ControllerLevel::HighLevel);
if (status.code != ErrorCode::OK) {
std::cerr << "switch robot motion control level failed"
<< ", code: " << status.code
<< ", message: " << status.message << std::endl;
robot.Shutdown();
return -1;
}
std::cout << "按任意键继续 (ESC退出)..."
<< std::endl;
// 等待用户输入
while (1) {
int key = getch();
if (key == 27)
break; // ESC键ASCII码为27
std::cout << "按键ASCII: " << key << ", 字符: " << static_cast<char>(key) << std::endl;
switch (key) {
case '1': {
RecoveryStand();
break;
}
case '2': {
BalanceStand();
break;
}
case '3': {
ExecuteTrick();
break;
}
case 'w': {
JoyStickCommand(0.0, 1.0, 0.0, 0.0); // 向前
break;
}
case 'a': {
JoyStickCommand(-1.0, 0.0, 0.0, 0.0); // 向左
break;
}
case 's': {
JoyStickCommand(0.0, -1.0, 0.0, 0.0); // 向后
break;
}
case 'd': {
JoyStickCommand(1.0, 0.0, 0.0, 0.0); // 向右
break;
}
case 'x': {
JoyStickCommand(0.0, 0.0, 0.0, 0.0); // 停止
break;
}
case 't': {
JoyStickCommand(0.0, 0.0, -1.0, 0.0); // 左转
break;
}
case 'g': {
JoyStickCommand(0.0, 0.0, 1.0, 0.0); // 右转
break;
}
default:
std::cout << "未知按键: " << key << std::endl;
break;
}
usleep(10000);
}
// 断开与机器人的链接
status = robot.Disconnect();
if (status.code != ErrorCode::OK) {
std::cerr << "disconnect robot failed"
<< ", code: " << status.code
<< ", message: " << status.message << std::endl;
robot.Shutdown();
return -1;
}
robot.Shutdown();
return 0;
}Python
示例文件：high_level_motion_example.py
参考文档：high_level_motion_reference.md
完整代码示例
#!/usr/bin/env python3
import sys
import time
import signal
import logging
from typing import Optional
import magicbot_gen1_python as magicbot
# Configure logging format and level
logging.basicConfig(
level=logging.INFO, # Minimum log level
format="%(asctime)s [%(levelname)s] %(message)s",
datefmt="%Y-%m-%d %H:%M:%S",
)
# Global variables
robot: Optional[magicbot.MagicRobot] = None
running = True
def signal_handler(signum, frame):
"""Signal handler function for graceful exit"""
global running, robot
logging.info("Received interrupt signal (%s), exiting...", signum)
running = False
if robot:
robot.disconnect()
logging.info("Robot disconnected")
robot.shutdown()
logging.info("Robot shutdown")
exit(-1)
def print_help():
"""Print help information"""
logging.info("High-Level Motion Control Function Demo Program")
logging.info("")
logging.info("Key Function Description:")
logging.info(" ESC Exit program")
logging.info(" 1 Function 1: Recovery stand")
logging.info(" 2 Function 2: Balance stand")
logging.info(" 3 Function 3: Execute trick - celebrate action")
logging.info(" w Function 4: Move forward")
logging.info(" a Function 5: Move left")
logging.info(" s Function 6: Move backward")
logging.info(" d Function 7: Move right")
logging.info(" x Function 7: stop move")
logging.info(" t Function 8: Turn left")
logging.info(" g Function 9: Turn right")
def get_user_input():
"""Get user input"""
try:
# Python implementation of getch() on Linux systems
import tty
import termios
fd = sys.stdin.fileno()
old_settings = termios.tcgetattr(fd)
try:
tty.setraw(sys.stdin.fileno())
ch = sys.stdin.read(1)
finally:
termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
return ch
except ImportError:
# If termios is not available, use simple input
return input("Please press a key: ").strip()
def recovery_stand():
"""Recovery stand"""
global robot
try:
logging.info("=== Executing Recovery Stand ===")
# Get high-level motion controller
controller = robot.get_high_level_motion_controller()
# Set gait to recovery stand
status = controller.set_gait(magicbot.GaitMode.GAIT_RECOVERY_STAND)
if status.code != magicbot.ErrorCode.OK:
logging.error(
"Failed to set robot gait, code: %s, message: %s",
status.code,
status.message,
)
return False
logging.info("Robot gait set to recovery stand")
return True
except Exception as e:
logging.error("Exception occurred while executing recovery stand: %s", e)
return False
def balance_stand():
"""Balance stand"""
global robot
try:
logging.info("=== Executing Balance Stand ===")
# Get high-level motion controller
controller = robot.get_high_level_motion_controller()
# Set gait to balance stand
status = controller.set_gait(magicbot.GaitMode.GAIT_BALANCE_STAND)
if status.code != magicbot.ErrorCode.OK:
logging.error(
"Failed to set robot gait, code: %s, message: %s",
status.code,
status.message,
)
return False
logging.info("Robot gait set to balance stand (supports movement)")
return True
except Exception as e:
logging.error("Exception occurred while executing balance stand: %s", e)
return False
def execute_trick_celebrate():
"""Execute trick - celebrate action"""
global robot
try:
logging.info("=== Executing Trick - Celebrate Action ===")
# Get high-level motion controller
controller = robot.get_high_level_motion_controller()
# Execute celebrate trick
status = controller.execute_trick(magicbot.TrickAction.ACTION_CELEBRATE)
if status.code != magicbot.ErrorCode.OK:
logging.error(
"Failed to execute robot trick, code: %s, message: %s",
status.code,
status.message,
)
return False
logging.info("Robot trick executed successfully")
return True
except Exception as e:
logging.error("Exception occurred while executing trick: %s", e)
return False
def joystick_command(left_x_axis, left_y_axis, right_x_axis, right_y_axis):
"""Send joystick control command"""
global robot
try:
# Get high-level motion controller
controller = robot.get_high_level_motion_controller()
# Create joystick command
joy_command = magicbot.JoystickCommand()
joy_command.left_x_axis = left_x_axis
joy_command.left_y_axis = left_y_axis
joy_command.right_x_axis = right_x_axis
joy_command.right_y_axis = right_y_axis
# Send joystick command
status = controller.send_joystick_command(joy_command)
if status.code != magicbot.ErrorCode.OK:
logging.error(
"Failed to send joystick command, code: %s, message: %s",
status.code,
status.message,
)
time.sleep(0.05) # Wait 50ms
return False
# Wait 50ms
time.sleep(0.05)
return True
except Exception as e:
logging.error("Exception occurred while sending joystick command: %s", e)
return False
def move_forward():
"""Move forward"""
logging.info("=== Moving Forward ===")
return joystick_command(0.0, 1.0, 0.0, 0.0)
def move_backward():
"""Move backward"""
logging.info("=== Moving Backward ===")
return joystick_command(0.0, -1.0, 0.0, 0.0)
def move_left():
"""Move left"""
logging.info("=== Moving Left ===")
return joystick_command(-1.0, 0.0, 0.0, 0.0)
def move_right():
"""Move right"""
logging.info("=== Moving Right ===")
return joystick_command(1.0, 0.0, 0.0, 0.0)
def turn_left():
"""Turn left"""
logging.info("=== Turning Left ===")
return joystick_command(0.0, 0.0, -1.0, 0.0)
def turn_right():
"""Turn right"""
logging.info("=== Turning Right ===")
return joystick_command(0.0, 0.0, 1.0, 0.0)
def stop_move():
"""Stop Move"""
logging.info("=== Stop Move ===")
return joystick_command(0.0, 0.0, 0.0, 0.0)
def demo_all_high_level_functions():
"""Demo all high-level motion control functions"""
logging.info("=== Demo All High-Level Motion Control Functions ===")
success_count = 0
total_tests = 6
# Test recovery stand
if recovery_stand():
success_count += 1
time.sleep(2)
# Test balance stand
if balance_stand():
success_count += 1
time.sleep(2)
# Test celebrate trick
if execute_trick_celebrate():
success_count += 1
time.sleep(3)
logging.info(
"High-level motion control function test completed: %s/%s functions tested successfully",
success_count,
total_tests,
)
return success_count == total_tests
def main():
"""Main function"""
global robot, running
# Bind signal handler
signal.signal(signal.SIGINT, signal_handler)
logging.info("Robot model: %s", magicbot.get_robot_model())
# Create robot instance
robot = magicbot.MagicRobot()
try:
# Configure local IP address for direct network connection and initialize SDK
local_ip = "192.168.54.111"
if not robot.initialize(local_ip):
logging.error("Failed to initialize robot SDK")
robot.shutdown()
return -1
# Set RPC timeout to 5 seconds
robot.set_timeout(5000)
# Connect to robot
status = robot.connect()
if status.code != magicbot.ErrorCode.OK:
logging.error(
"Failed to connect to robot, code: %s, message: %s",
status.code,
status.message,
)
robot.shutdown()
return -1
logging.info("Successfully connected to robot")
# Switch motion control controller to high-level controller
status = robot.set_motion_control_level(magicbot.ControllerLevel.HighLevel)
if status.code != magicbot.ErrorCode.OK:
logging.error(
"Failed to switch robot motion control level, code: %s, message: %s",
status.code,
status.message,
)
robot.shutdown()
return -1
logging.info("Switched to high-level motion controller")
# Initialize high-level motion controller
controller = robot.get_high_level_motion_controller()
if not controller.initialize():
logging.error("Failed to initialize high-level motion controller")
robot.disconnect()
robot.shutdown()
return -1
logging.info("Successfully initialized high-level motion controller")
print_help()
logging.info("Press any key to continue (ESC to exit)...")
# Main loop
while running:
try:
key = get_user_input()
if key == "\x1b": # ESC key
break
logging.info("Key pressed: %s", key)
if key == "1":
recovery_stand()
elif key == "2":
balance_stand()
elif key == "3":
execute_trick_celebrate()
elif key == "w":
move_forward()
elif key == "a":
move_left()
elif key == "s":
move_backward()
elif key == "d":
move_right()
elif key == "x":
stop_move()
elif key == "t":
turn_left()
elif key == "g":
turn_right()
else:
logging.info("Unknown key: %s", key)
time.sleep(0.01) # Brief delay
except KeyboardInterrupt:
break
except Exception as e:
logging.error("Exception occurred while processing user input: %s", e)
return 0
except Exception as e:
logging.error("Exception occurred during program execution: %s", e)
return -1
finally:
# Clean up resources
try:
logging.info("Clean up resources")
# Close high-level motion controller
controller = robot.get_high_level_motion_controller()
controller.shutdown()
logging.info("High-level motion controller closed")
# Disconnect
robot.disconnect()
logging.info("Robot connection disconnected")
# Shutdown robot
robot.shutdown()
logging.info("Robot shutdown")
except Exception as e:
logging.error("Exception occurred while cleaning up resources: %s", e)
if __name__ == "__main__":
sys.exit(main())运行说明
- 环境准备：
# 设置环境变量
export PYTHONPATH=/opt/magic_robotics/magicbot_gen1_sdk/lib:$PYTHONPATH
export LD_LIBRARY_PATH=/opt/magic_robotics/magicbot_gen1_sdk/lib:$LD_LIBRARY_PATH- 运行示例：
# C++
./high_level_motion_example
# Python
python3 high_level_motion_example.py- 控制说明：
ESC：退出程序1：恢复站立2：平衡站立3：庆祝动作w/a/s/d/x：前进/左移/后退/右移/停止t/g：左转/右转
- 停止程序：
- 按
Ctrl+C可以安全停止程序 - 程序会自动清理所有资源