高层运动控制示例
约 1876 字大约 6 分钟
2025-05-29
该示例展示了如何使用 MagicDog SDK 进行初始化、连接机器人、高层运动控制（步态、特技、遥控）等基本操作。
C++
示例文件：high_level_motion_example.cpp
参考文档：C++ API/high_level_motion_reference.md
高层运动控制示例：
#include "magic_robot.h"
#include <termios.h>
#include <unistd.h>
#include <csignal>
#include <iostream>
#include <thread>
using namespace magic::dog;
magic::dog::MagicRobot robot;
std::atomic<bool> is_running(true);
std::atomic<float> left_x_axis(0.0);
std::atomic<float> left_y_axis(0.0);
std::atomic<float> right_x_axis(0.0);
std::atomic<float> right_y_axis(0.0);
void signalHandler(int signum) {
std::cout << "Interrupt signal (" << signum << ") received.\n";
is_running.store(false);
robot.Shutdown();
// Exit process
exit(signum);
}
void print_help(const char* prog_name) {
std::cout << "Key Function Demo Program\n\n";
std::cout << "Usage: " << prog_name << "\n";
std::cout << "Key Function Description:\n";
std::cout << " ESC Exit program\n";
std::cout << " 1 Function 1: Position control standing\n";
std::cout << " 2 Function 2: Force control standing\n";
std::cout << " 3 Function 3: Execute trick - lie down\n";
std::cout << " w Function 4: Move forward\n";
std::cout << " a Function 5: Move left\n";
std::cout << " s Function 6: Move backward\n";
std::cout << " d Function 7: Move right\n";
std::cout << " x Function 7: Stop movement\n";
std::cout << " t Function 8: Turn left\n";
std::cout << " g Function 9: Turn right\n";
}
int getch() {
struct termios oldt, newt;
int ch;
tcgetattr(STDIN_FILENO, &oldt); // Get current terminal settings
newt = oldt;
newt.c_lflag &= ~(ICANON | ECHO); // Disable buffering and echo
tcsetattr(STDIN_FILENO, TCSANOW, &newt);
ch = getchar(); // Read key press
tcsetattr(STDIN_FILENO, TCSANOW, &oldt); // Restore settings
return ch;
}
void RecoveryStand() {
// Get high level motion controller
auto& controller = robot.GetHighLevelMotionController();
// Set gait
auto status = controller.SetGait(GaitMode::GAIT_STAND_R);
if (status.code != ErrorCode::OK) {
std::cerr << "Set robot gait failed"
<< ", code: " << status.code
<< ", message: " << status.message << std::endl;
return;
}
}
void BalanceStand() {
// Get high level motion controller
auto& controller = robot.GetHighLevelMotionController();
// Set posture display gait
auto status = controller.SetGait(GaitMode::GAIT_STAND_B);
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() {
// Get high level motion controller
auto& controller = robot.GetHighLevelMotionController();
// Execute trick
auto status = controller.ExecuteTrick(TrickAction::ACTION_LIE_DOWN);
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) {
::left_x_axis.store(left_x_axis);
::left_y_axis.store(left_y_axis);
::right_x_axis.store(right_x_axis);
::right_y_axis.store(right_y_axis);
}
void JoyThread() {
auto& controller = robot.GetHighLevelMotionController();
while (is_running.load()) {
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 << "Send joystick command failed"
<< ", code: " << status.code
<< ", message: " << status.message << std::endl;
}
usleep(10000);
}
}
int main(int argc, char* argv[]) {
// 绑定 SIGINT（Ctrl+C）
signal(SIGINT, signalHandler);
std::string local_ip = "192.168.54.111";
// Configure local IP address for direct network connection to machine and initialize SDK
if (!robot.Initialize(local_ip)) {
std::cerr << "Robot SDK initialization failed." << std::endl;
robot.Shutdown();
return -1;
}
// Set RPC timeout to 5s
robot.SetTimeout(5000);
// Connect to robot
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;
}
// Switch motion control level to high level controller, default is high level controller
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::thread joy_thread(JoyThread);
print_help(argv[0]);
std::cout << "Press any key to continue (ESC to exit)..."
<< std::endl;
auto change_gait_to_down_climb_stairs = [](auto& robot) -> bool {
GaitMode current_gait = GaitMode::GAIT_PASSIVE;
auto status = robot.GetHighLevelMotionController().GetGait(current_gait);
if (status.code != ErrorCode::OK) {
std::cerr << "Get robot gait failed"
<< ", code: " << status.code
<< ", message: " << status.message << std::endl;
return false;
}
if (current_gait != GaitMode::GAIT_DOWN_CLIMB_STAIRS) {
status = robot.GetHighLevelMotionController().SetGait(GaitMode::GAIT_DOWN_CLIMB_STAIRS);
if (status.code != ErrorCode::OK) {
std::cerr << "Set robot gait failed"
<< ", code: " << status.code
<< ", message: " << status.message << std::endl;
return false;
}
} else {
return true;
}
status = robot.GetHighLevelMotionController().GetGait(current_gait);
if (status.code != ErrorCode::OK) {
std::cerr << "Get robot gait failed"
<< ", code: " << status.code
<< ", message: " << status.message << std::endl;
return false;
}
while (current_gait != GaitMode::GAIT_DOWN_CLIMB_STAIRS) {
usleep(10000);
status = robot.GetHighLevelMotionController().GetGait(current_gait);
if (status.code != ErrorCode::OK) {
std::cerr << "Get robot gait failed"
<< ", code: " << status.code
<< ", message: " << status.message << std::endl;
return false;
}
}
return true;
};
// Wait for user input
while (1) {
int key = getch();
if (key == 27)
break; // ESC
std::cout << "Key ASCII: " << key << ", Character: " << static_cast<char>(key) << std::endl;
switch (key) {
case '1': {
RecoveryStand();
break;
}
case '2': {
BalanceStand();
break;
}
case '3': {
ExecuteTrick();
break;
}
case 'w': {
if (!change_gait_to_down_climb_stairs(robot)) {
std::cerr << "Change robot gait to down climb stairs failed" << std::endl;
break;
}
JoyStickCommand(0.0, 1.0, 0.0, 0.0); // Forward
break;
}
case 'a': {
if (!change_gait_to_down_climb_stairs(robot)) {
std::cerr << "Change robot gait to down climb stairs failed" << std::endl;
break;
}
JoyStickCommand(-1.0, 0.0, 0.0, 0.0); // Left
break;
}
case 's': {
if (!change_gait_to_down_climb_stairs(robot)) {
std::cerr << "Change robot gait to down climb stairs failed" << std::endl;
break;
}
JoyStickCommand(0.0, -1.0, 0.0, 0.0); // Backward
break;
}
case 'd': {
if (!change_gait_to_down_climb_stairs(robot)) {
std::cerr << "Change robot gait to down climb stairs failed" << std::endl;
break;
}
JoyStickCommand(1.0, 0.0, 0.0, 0.0); // Right
break;
}
case 't': {
if (!change_gait_to_down_climb_stairs(robot)) {
std::cerr << "Change robot gait to down climb stairs failed" << std::endl;
break;
}
JoyStickCommand(0.0, 0.0, -1.0, 0.0); // Turn left
break;
}
case 'g': {
if (!change_gait_to_down_climb_stairs(robot)) {
std::cerr << "Change robot gait to down climb stairs failed" << std::endl;
break;
}
JoyStickCommand(0.0, 0.0, 1.0, 0.0); // Turn right
break;
}
case 'x': {
if (!change_gait_to_down_climb_stairs(robot)) {
std::cerr << "Change robot gait to down climb stairs failed" << std::endl;
break;
}
JoyStickCommand(0.0, 0.0, 0.0, 0.0); // Stop
break;
}
default:
std::cout << "Unknown key: " << key << std::endl;
break;
}
usleep(10000);
}
is_running.store(false);
joy_thread.join();
// Disconnect from robot
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
参考文档：Python API/high_level_motion_reference.md
高层运动控制示例：
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
MagicDog SDK Python Usage Example
This file demonstrates how to use MagicDog SDK Python bindings to control the robot.
"""
import sys
import time
import threading
import tty
import termios
import os
import logging
from typing import Optional
logging.basicConfig(
level=logging.INFO, # Minimum log level
format="%(asctime)s [%(levelname)s] %(message)s",
datefmt="%Y-%m-%d %H:%M:%S",
)
try:
import magicdog_python as magicdog
logging.info("Successfully imported MagicDog Python module!")
except ImportError as e:
logging.error(f"Import failed: {e}")
logging.error("Please run build_python.sh to build Python bindings first")
sys.exit(1)
left_x = 0.0
left_y = 0.0
right_x = 0.0
right_y = 0.0
exit_flag = False
robot = None
high_controller = None
# Get single character input (no echo)
def getch():
fd = sys.stdin.fileno()
old_settings = termios.tcgetattr(fd)
try:
tty.setraw(sys.stdin.fileno())
ch = sys.stdin.read(1)
logging.info(f"Received character: {ch}")
finally:
termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
return ch
# Position control standing
def recovery_stand():
global high_controller
try:
# Set gait to position control standing
status = high_controller.set_gait(magicdog.GaitMode.GAIT_STAND_R)
if status.code != magicdog.ErrorCode.OK:
logging.error(f"Failed to set position control standing: {status.message}")
else:
logging.info("Robot set to position control standing")
except Exception as e:
logging.error(f"Error executing position control standing: {e}")
# Force control standing
def balance_stand():
global high_controller
try:
# Set gait to force control standing
status = high_controller.set_gait(magicdog.GaitMode.GAIT_STAND_B)
if status.code != magicdog.ErrorCode.OK:
logging.error(f"Failed to set force control standing: {status.message}")
else:
logging.info("Robot set to force control standing")
except Exception as e:
logging.error(f"Error executing force control standing: {e}")
# Execute trick - lie down
def execute_trick():
global high_controller
try:
# Execute lie down trick
status = high_controller.execute_trick(magicdog.TrickAction.ACTION_LIE_DOWN)
if status.code != magicdog.ErrorCode.OK:
logging.error(f"Failed to execute trick: {status.message}")
else:
logging.info("Trick executed successfully")
except Exception as e:
logging.error(f"Error executing trick: {e}")
# Switch gait to down climb stairs mode
def change_gait_to_down_climb_stairs():
global high_controller
try:
current_gait = high_controller.get_gait()
if current_gait != magicdog.GaitMode.GAIT_DOWN_CLIMB_STAIRS:
status = high_controller.set_gait(magicdog.GaitMode.GAIT_DOWN_CLIMB_STAIRS)
if status.code != magicdog.ErrorCode.OK:
logging.error(f"Failed to set down climb stairs gait: {status.message}")
return False
# Wait for gait switch to complete
while high_controller.get_gait() != magicdog.GaitMode.GAIT_DOWN_CLIMB_STAIRS:
time.sleep(0.01)
return True
except Exception as e:
logging.error(f"Error changing gait: {e}")
return False
# Define a function to simulate joystick input based on key presses
def update_joy_command():
global left_x, left_y, right_x, right_y, exit_flag
logging.info("Key function description:")
logging.info(" 1 Function 1: Position control standing")
logging.info(" 2 Function 2: Force control standing")
logging.info(" 3 Function 3: Execute trick - lie down")
logging.info(" w Move forward")
logging.info(" a Move left")
logging.info(" s Move backward")
logging.info(" d Move right")
logging.info(" t Turn left")
logging.info(" g Turn right")
logging.info(" x Stop movement")
logging.info(" ESC Exit program")
while not exit_flag:
key = getch()
if key == '\x1b': # ESC key
exit_flag = True
break
elif key == '1':
# Position control standing
recovery_stand()
elif key == '2':
# Force control standing
balance_stand()
elif key == '3':
# Execute trick - lie down
execute_trick()
elif key == 'w':
if change_gait_to_down_climb_stairs():
left_y = 1.0
left_x = 0.0
right_x = 0.0
right_y = 0.0
elif key == 's':
if change_gait_to_down_climb_stairs():
left_y = -1.0
left_x = 0.0
right_x = 0.0
right_y = 0.0
elif key == 'a':
if change_gait_to_down_climb_stairs():
left_x = -1.0
left_y = 0.0
right_x = 0.0
right_y = 0.0
elif key == 'd':
if change_gait_to_down_climb_stairs():
left_x = 1.0
left_y = 0.0
right_x = 0.0
right_y = 0.0
elif key == 't':
if change_gait_to_down_climb_stairs():
left_x = 0.0
left_y = 0.0
right_x = -1.0
right_y = 0.0
elif key == 'g':
if change_gait_to_down_climb_stairs():
left_x = 0.0
left_y = 0.0
right_x = 1.0
right_y = 0.0
elif key == 'x':
if change_gait_to_down_climb_stairs():
left_x = 0.0
left_y = 0.0
right_x = 0.0
right_y = 0.0
# Joystick command sending thread
def joy_thread(high_controller):
global left_x, left_y, right_x, right_y, exit_flag
while not exit_flag:
joy_command = magicdog.JoystickCommand()
joy_command.left_x_axis = left_x
joy_command.left_y_axis = left_y
joy_command.right_x_axis = right_x
joy_command.right_y_axis = right_y
status = high_controller.send_joystick_command(joy_command)
if status.code != magicdog.ErrorCode.OK:
logging.error(f"Failed to send joystick command: {status.message}")
break
time.sleep(0.01) # 10ms interval, consistent with C++ code
def main():
"""Main function"""
global robot, high_controller
logging.info("MagicDog SDK Python Example Program")
robot = magicdog.MagicRobot()
if not robot.initialize("192.168.55.10"):
logging.error("Initialization failed")
return
robot.set_timeout(5000)
if not robot.connect():
logging.error("Connection failed")
robot.shutdown()
return
# Set motion control level to high level
status = robot.set_motion_control_level(magicdog.ControllerLevel.HIGH_LEVEL)
if status.code != magicdog.ErrorCode.OK:
logging.error(f"Failed to set motion control level: {status.message}")
robot.shutdown()
return
# Get high level motion controller
high_controller = robot.get_high_level_motion_controller()
# Create a thread for receiving keyboard input
key_thread = threading.Thread(target=update_joy_command)
key_thread.daemon = True # Set as daemon thread
key_thread.start()
# Create joystick command sending thread
joy_send_thread = threading.Thread(target=joy_thread, args=(high_controller,))
joy_send_thread.daemon = True # Set as daemon thread
joy_send_thread.start()
logging.info("Program started, please use keys to control robot...")
# Main thread waits for exit signal
global exit_flag
try:
while not exit_flag:
time.sleep(0.1)
except KeyboardInterrupt:
logging.info("\nReceived interrupt signal, exiting...")
exit_flag = True
logging.info("Closing robot connection...")
robot.shutdown()
robot.disconnect()
logging.info("\nExample program execution completed!")
if __name__ == "__main__":
main()运行说明
- 环境准备：
# 设置环境变量
export PYTHONPATH=/opt/magic_robotics/magicdog_sdk/lib:$PYTHONPATH
export LD_LIBRARY_PATH=/opt/magic_robotics/magicdog_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可以安全停止程序 - 程序会自动清理所有资源