该示例展示了如何使用 MagicDog SDK 进行初始化、连接机器人、控制机器人重复站立、蹲下等基本操作。

C++

示例文件：low_level_motion_example.cpp

参考文档：C++ API/low_level_motion_reference.md

#include "magic_robot.h"
#include "magic_type.h"

#include <unistd.h>
#include <csignal>

#include <iostream>
#include <mutex>

using namespace magic::dog;

magic::dog::MagicRobot robot;

void signalHandler(int signum) {
  std::cout << "Interrupt signal (" << signum << ") received.\n";

  robot.Shutdown();
  // Exit process
  exit(signum);
}

int main() {
  // 绑定 SIGINT（Ctrl+C）
  signal(SIGINT, signalHandler);

  std::string local_ip = "192.168.55.10";
  // 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;
  }

  // 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;
  }

  status = robot.SetMotionControlLevel(ControllerLevel::HighLevel);
  if (status.code != ErrorCode::OK) {
    std::cerr << "Switch robot motion control level to HighLevel failed"
              << ", code: " << status.code
              << ", message: " << status.message << std::endl;
    robot.Shutdown();
    return -1;
  }

  auto& high_controller = robot.GetHighLevelMotionController();
  status = high_controller.SetGait(magic::dog::GaitMode::GAIT_PASSIVE);
  if (status.code != ErrorCode::OK) {
    std::cerr << "Switch robot motion gait failed"
              << ", code: " << status.code
              << ", message: " << status.message << std::endl;
    robot.Shutdown();
    return -1;
  }

  GaitMode current_mode = GaitMode::GAIT_DEFAULT;
  while (current_mode != GaitMode::GAIT_PASSIVE) {
    high_controller.GetGait(current_mode);
    usleep(100000);
  }

  sleep(2);

  // Switch motion control level to low level controller, default is high level controller
  status = robot.SetMotionControlLevel(ControllerLevel::LowLevel);
  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;
  }

  while (current_mode != GaitMode::GAIT_LOWLEVL_SDK) {
    high_controller.GetGait(current_mode);
    usleep(100000);
  }

  sleep(2);

  // Get low level controller
  auto& controller = robot.GetLowLevelMotionController();

  // Set control command sending period to 2ms, 500Hz
  controller.SetPeriodMs(2);

  current_mode = GaitMode::GAIT_NONE;
  while (current_mode != GaitMode::GAIT_LOWLEVL_SDK) {
    high_controller.GetGait(current_mode);
    usleep(100000);
  }

  bool is_had_receive_leg_state = false;
  std::mutex mut;
  LegState receive_state;
  controller.SubscribeLegState([&](const std::shared_ptr<LegState> msg) {
    static unsigned int count = 0;
    if (!is_had_receive_leg_state) {
      std::lock_guard<std::mutex> guard(mut);
      is_had_receive_leg_state = true;
      receive_state = *msg;
    }

    if (count++%1000 == 0)
      std::cout << "Received leg state data." << std::endl;
  });

  while (!is_had_receive_leg_state) {
    usleep(2000);
  }

  sleep(10);

  float j1[] = { 0.0000, 1.0477, -2.0944};   // base height 0.2 
  float j2[] = { 0.0000, 0.7231, -1.4455};   // base height 0.3
  float inital_q[12] = {receive_state.state[0].q, receive_state.state[1].q, receive_state.state[2].q,
                        receive_state.state[3].q, receive_state.state[4].q, receive_state.state[5].q,
                        receive_state.state[6].q, receive_state.state[7].q, receive_state.state[8].q,
                        receive_state.state[9].q, receive_state.state[10].q, receive_state.state[11].q};

  LegJointCommand command;
  unsigned long int cnt = 0;
  while (true) {
    if (cnt < 1000) {
      double t = 1.0*cnt/1000.0;
      t = std::min(std::max(t, 0.0), 1.0);

      for (int i = 0; i < 12; ++i) {
        command.cmd[i].q_des = (1-t)*inital_q[i] + t*j1[i%3];
      }
    } else if (cnt < 1750) {
      double t = 1.0*(cnt-1000)/700.0;
      t = std::min(std::max(t, 0.0), 1.0);

      for (int i = 0; i < 12; ++i) {
        command.cmd[i].q_des = (1-t)*j1[i%3] + t*j2[i%3];
      }
    } else if (cnt < 2500) {
      double t = 1.0*(cnt-1750)/700.0;
      t = std::min(std::max(t, 0.0), 1.0);

      for (int i = 0; i < 12; ++i) {
        command.cmd[i].q_des = (1-t)*j2[i%3] + t*j1[i%3];
      }
    } else {
      cnt = 1000;
    }

    for (int i = 0; i < 12; ++i) {
      command.cmd[i].kp = 100;
      command.cmd[i].kd = 1.2;
    }

    controller.PublishLegCommand(command);
    usleep(2000);
    cnt++;
  }

  usleep(10000000);

  // Disconnect from robot
  status = robot.Disconnect();
  if (status.code != ErrorCode::OK) {
    std::cerr << "Disconnect robot failed"
              << ", code: " << status.code
              << ", message: " << status.message << std::endl;
    return -1;
  }

  robot.Shutdown();

  return 0;
}

Python

示例文件：low_level_motion_example.py

参考文档：Python API/low_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 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)

def main():
    """Main function"""
    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
    
    logging.info("Setting 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
    
    logging.info("Getting high level motion controller")
    high_controller = robot.get_high_level_motion_controller()
    
    logging.info("Setting motion mode to passive")
    status = high_controller.set_gait(magicdog.GaitMode.GAIT_PASSIVE)
    if status.code != magicdog.ErrorCode.OK:
        logging.error(f"Failed to set motion mode: {status.message}")
        robot.shutdown()
        return

    logging.info("Waiting for motion mode to change to passive")
    current_mode = magicdog.GaitMode.GAIT_DEFAULT
    while current_mode != magicdog.GaitMode.GAIT_PASSIVE:
        current_mode = high_controller.get_gait()
        time.sleep(0.1)

    time.sleep(2)

    logging.info("Setting motion control level to low level")
    status = robot.set_motion_control_level(magicdog.ControllerLevel.LOW_LEVEL)
    if status.code != magicdog.ErrorCode.OK:
        logging.error(f"Failed to set motion control level: {status.message}")
        robot.shutdown()
        return

    logging.info("Waiting for motion mode to change to low level")
    while current_mode != magicdog.GaitMode.GAIT_LOWLEVL_SDK:
        current_mode = high_controller.get_gait()
        time.sleep(0.1)

    time.sleep(2)

    logging.info("Getting low level motion controller")
    low_controller = robot.get_low_level_motion_controller()

    logging.info("Setting period to 2ms")
    low_controller.set_period_ms(2)

    is_had_receive_leg_state = False
    mut = threading.Lock()
    receive_state = None
    count = 0
    def leg_state_callback(msg):
        nonlocal is_had_receive_leg_state, receive_state, count
        if not is_had_receive_leg_state:
            with mut:
                is_had_receive_leg_state = True
                receive_state = msg
        if count % 1000 == 0:
            logging.info("Received leg state data.")
        count += 1

    low_controller.subscribe_leg_state(leg_state_callback)
    
    logging.info("Waiting to receive leg state data")
    while not is_had_receive_leg_state:
        time.sleep(0.002)

    time.sleep(10)

    j1 = [0.0000, 1.0477, -2.0944]
    j2 = [0.0000, 0.7231, -1.4455]
    inital_q = [receive_state.state[0].q, receive_state.state[1].q, receive_state.state[2].q,
                receive_state.state[3].q, receive_state.state[4].q, receive_state.state[5].q,
                receive_state.state[6].q, receive_state.state[7].q, receive_state.state[8].q,
                receive_state.state[9].q, receive_state.state[10].q, receive_state.state[11].q]

    command = magicdog.LegJointCommand()
    cnt = 0
    while True:
        if cnt < 1000:
            t = 1.0*cnt/1000.0
            t = min(max(t, 0.0), 1.0)
            for i in range(12):
                command.cmd[i].q_des = (1-t)*inital_q[i] + t*j1[i%3]
        elif cnt < 1750:
            t = 1.0*(cnt-1000)/700.0
            t = min(max(t, 0.0), 1.0)
            for i in range(12):
                command.cmd[i].q_des = (1-t)*j1[i%3] + t*j2[i%3]
        elif cnt < 2500:
            t = 1.0*(cnt-1750)/700.0
            t = min(max(t, 0.0), 1.0)
            for i in range(12):
                command.cmd[i].q_des = (1-t)*j2[i%3] + t*j1[i%3]
        else:
            cnt = 1000

        for i in range(12):
            command.cmd[i].kp = 100
            command.cmd[i].kd = 1.2

        low_controller.publish_leg_command(command)
        time.sleep(0.002)
        cnt += 1

    robot.disconnect()
    robot.shutdown()
    
    logging.info("\nExample program execution completed!")

if __name__ == "__main__":
    main()

运行说明

1. 环境准备：

# 设置环境变量
export PYTHONPATH=/opt/magic_robotics/magicdog_sdk/lib:$PYTHONPATH
export LD_LIBRARY_PATH=/opt/magic_robotics/magicdog_sdk/lib:$LD_LIBRARY_PATH

2. 运行示例：

# C++
./low_level_motion_example
# Python
python3 low_level_motion_example.py

3. 停止程序：

• 按 Ctrl+C 可以安全停止程序
• 程序会自动清理所有资源