Unitree GO2 Custom Controller

A high-performance C++ implementation for controlling Unitree GO2 robot with real-time MQTT communication. This project provides a robust, modular architecture for robot control with comprehensive safety features and flexible configuration management.

Features

• 🤖 Robot Control: Full integration with Unitree SDK2 for GO2 robot control
• 📡 MQTT Communication: Real-time command and state communication via MQTT
• 🏗️ Modular Architecture: Clean separation of concerns with well-defined interfaces
• 🛡️ Safety Systems: Emergency stop, timeout handling, and safety limits
• 📝 Comprehensive Logging: Multi-level logging with file and console output
• ⚙️ Configuration Management: JSON-based configuration with runtime presets
• 🧭 Navigation Support: SLAM and navigation capabilities
• 🎭 Special Actions: Support for dances, tricks, and custom motions
• 🔋 Auto Recharge Support: ArUco marker-based automatic recharge capability
• 🚀 High Performance: Optimized for real-time control with configurable frequencies
• 🔧 Development Tools: Built-in scripts and utilities for easy deployment
• 🔍 Service Status Monitoring: Automatic printing of all robot service statuses

Architecture

The system consists of several key components:

• CustomRobot: Main orchestrator class handling MQTT and robot coordination
• Controller: Direct interface to Unitree SDK2 for robot control
• MqttClient: Asynchronous MQTT client with reconnection and message queuing
• Navigation: Navigation and SLAM functionality
• Config: Configuration loading, validation, and management
• Logger: Thread-safe logging system with multiple output targets

Dependencies

Required System Packages

# Ubuntu/Debian
sudo apt update
sudo apt install -y \
    build-essential \
    cmake \
    pkg-config \
    libpaho-mqtt-dev \
    libpaho-mqttpp-dev \
    nlohmann-json3-dev \
    libssl-dev

# Or build from source if packages not available

Unitree SDK2

This project requires the Unitree SDK2 to be available at ../unitree_sdk2 relative to this directory.

Quick Start

# Install dependencies
./scripts/install_deps.sh

# Create build directory
mkdir build && cd build

# Configure with CMake
cmake ..

# Build the project
make -j$(nproc)

# Run the robot
./main

The robot will start immediately with default settings and be ready to receive MQTT commands!

Configuration

The system supports flexible configuration through compile-time defaults. Configuration files are optional - the system can run with compile-time defaults.

Compile-time Configuration

The default configuration is defined in include/config.hpp:

To customize these settings, modify the values in include/config.hpp and rebuild the project.

Usage

Basic Usage

# Simple execution - just run it!
./main

# That's it! No parameters needed.
# The robot will start with default settings and be ready to receive MQTT commands.

Default Configuration

The robot runs with sensible defaults out of the box:

Configuration is done at compile time by modifying values in include/config.hpp. No configuration files or command-line arguments needed!

MQTT API

The robot communicates via MQTT using the following topic structure:

Command Topics

• unitree/go2/cmd: All commands (sport, navigation, robot state, obstacle avoidance, motion switcher)

Commands are sent to the command topic with a JSON payload specifying the command type and parameters.

Sport Commands

Sport commands are sent with a JSON payload containing the command type and optional parameters:

{
  "type": "sport",
  "cmd": "StandUp",
  "param": {
    // Optional parameters depending on the command
  }
}

Supported sport commands:

• StandUp: Stand up from lying position
• StandDown: Lie down from standing position
• Sit: Sit down
• Damp: Enable damping mode
• RecoveryStand: Recovery from emergency stop
• BalanceStand: Balanced standing with pose control
• StopMove: Stop all movement
• Dance1: Dance routine 1
• Dance2: Dance routine 2
• Hello: Greeting gesture
• Move: Move with specified velocities (vx, vy, vyaw)
• Euler: Set body orientation (roll, pitch, yaw)
• RiseSit: Rise and sit motion
• SpeedLevel: Set movement speed level
• Stretch: Stretching motion
• SwitchJoystick: Enable/disable joystick control
• Content: Content expression
• Heart: Heart gesture
• Pose: Pose control
• Scrape: Scrape motion
• FrontFlip: Front flip trick
• FrontJump: Front jump motion
• FrontPounce: Front pounce motion
• LeftFlip: Left flip trick
• BackFlip: Back flip trick
• HandStand: Handstand motion
• FreeWalk: Free walking mode
• FreeBound: Free bounding mode
• FreeJump: Free jumping mode
• FreeAvoid: Free obstacle avoidance
• ClassicWalk: Classic walking mode
• WalkUpright: Upright walking mode
• CrossStep: Cross step motion
• AutoRecoverSet: Set auto recovery mode
• StaticWalk: Static walking mode
• TrotRun: Trot running mode
• EconomicGait: Economic gait mode
• SwitchAvoidMode: Switch obstacle avoidance mode
• BodyHeight: Adjust body height
• SwitchGait: Switch gait type
• TrajectoryFollow: Follow a trajectory path
• ContinuousGait: Continuous gait mode
• MoveToPos: Move to specific position
• FastWalk: Fast walking mode
• FootRaiseHeight: Adjust foot raise height
• And more...

Navigation Commands

Navigation commands are sent with a JSON payload containing the command type and parameters:

{
  "type": "nav",
  "cmd": "startMapping"
}

Supported navigation commands:

• start_mapping: Start SLAM mapping
• end_mapping: End SLAM mapping with map address parameter
• initialize_pose: Set initial pose with position and orientation
• pose_navigation: Navigate to a specific pose
• pause_navigation: Pause navigation
• resume_navigation: Resume navigation
• close_slam: Close SLAM service

Obstacle Avoidance Commands

Obstacle avoidance commands are sent with a JSON payload containing the command type and parameters:

{
  "type": "oac",
  "cmd": "SwitchSet",
  "param": {
    "enable": true
  }
}

Supported obstacle avoidance commands:

• SwitchSet: Enable/disable obstacle avoidance
• SwitchGet: Get obstacle avoidance status
• UseRemoteCommandFromApi: Configure remote command source
• MoveToAbsolutePosition: Move to absolute position
• MoveToIncrementPosition: Move to increment position

Motion Switcher Commands

Motion switcher commands allow switching between different motion modes of the robot:

{
  "type": "msc",
  "cmd": "SelectMode",
  "param": {
    "mode": "ai"
  }
}

Supported motion switcher commands:

• CheckMode: Check the current motion mode and robot form
• SelectMode: Select a specific motion mode (normal, advanced, ai, ai-w, normal-w)
• ReleaseMode: Release the current motion mode

CheckMode

Check the current motion mode and robot form.

Request:

{
  "type": "msc",
  "cmd": "CheckMode"
}

Response:

{
  "code": 0,
  "data": {
    "form": "0",
    "name": "normal"
  }
}

SelectMode

Select a specific motion mode. Available modes:

• normal: Standard walking mode
• advanced: Enhanced walking capabilities
• ai: AI-powered motion mode
• ai-w: AI mode with specific walking pattern
• normal-w: Normal mode with specific walking pattern

Request:

{
  "type": "msc",
  "cmd": "SelectMode",
  "param": {
    "mode": "ai"
  }
}

Response:

{
  "code": 0,
  "data": {
    "form": "0",
    "name": "ai"
  }
}

ReleaseMode

Release the current motion mode and return to default mode.

Request:

{
  "type": "msc",
  "cmd": "ReleaseMode"
}

Response:

{
  "code": 0,
  "data": {
    "form": "0",
    "name": "normal"
  }
}

System Commands

System commands are sent with a JSON payload containing the command type and parameters:

{
  "type": "rsc",
  "cmd": "GetServiceList"
}

Supported system commands:

• list: Get list of available services
• switch: Enable/disable a service

When the service list is retrieved (either through the list command or internally), the system automatically prints the status of each service to the logs. Each service entry includes:

• Service name
• Current status (ACTIVE/INACTIVE)
• Protection status (YES/NO)

Supported Actions

Basic Motions

• StandUp: Stand up from lying position
• StandDown: Lie down from standing position
• Sit: Sit down
• Damp: Enable damping mode
• RecoveryStand: Recovery from emergency stop
• BalanceStand: Balanced standing with pose control
• StopMove: Stop all movement

Special Actions

• Dance1: Dance routine 1
• Dance2: Dance routine 2
• Hello: Greeting gesture

Motion Switcher Actions

• CheckMode: Check the current motion mode and robot form. Returns the current motion mode and robot form information.
• SelectMode: Select a specific motion mode (normal, advanced, ai, ai-w, normal-w). Switches the robot to the specified motion mode.
• ReleaseMode: Release the current motion mode and return to default mode. Releases the currently selected motion mode.

Navigation Actions

• start_mapping: Start SLAM mapping
• end_mapping: End SLAM mapping and save map
• pause_navigation: Pause navigation
• resume_navigation: Resume navigation
• close_slam: Close SLAM service

Safety Features

• Emergency Stop: Immediate motion halt and damping activation
• Velocity Limits: Configurable maximum linear and angular velocities
• Command Timeout: Automatic stop if no commands received within timeout
• Connection Monitoring: Automatic reconnection and error handling

Development

Adding New Actions

1. Add action handler in Controller class (include/controller.hpp, src/controller.cpp)
2. Update MQTT command processing in CustomRobot::processSportCmd() or appropriate processor
3. Add action documentation

Adding New Motion Switcher Actions

1. Add action handler in Controller class (include/controller.hpp, src/controller.cpp) under MotionSwitcher section
2. Update MQTT command processing in CustomRobot::processMscCmd()
3. Add action documentation in the Motion Switcher Actions section

Extending MQTT API

1. Define new message types in the appropriate handler functions in CustomRobot
2. Update JSON parsing and validation
3. Add corresponding response handling

Fixing Namespace Issues

When working with Unitree SDK types, ensure proper namespace qualification:

• Use unitree::robot::ChannelSubscriberPtr instead of just ChannelSubscriberPtr
• Check all SDK type references for correct namespace usage

Custom Configurations

The robot's configuration is defined at compile time in include/config.hpp. To customize the configuration:

1. Edit the constants in include/config.hpp to change default values
2. Rebuild the project:

   cd build
   make -j$(nproc)
   

Common configuration options include:

• Network interface
• MQTT broker settings
• Topic prefixes
• Control frequencies
• Safety limits

Quick Start Scripts

The project includes utility scripts for easy deployment:

# Install system dependencies
./scripts/install_deps.sh

# Run robot with recommended settings
./scripts/run_robot.sh

Project Structure

unitree-go2/
├── CMakeLists.txt          # Build configuration
├── README.md               # This file
├── LICENSE                 # License information
├── .gitignore              # Git ignore file
├── config/                 # Configuration files (optional)
├── include/                # Header files
│   ├── config.hpp          # Configuration management
│   ├── controller.hpp      # Robot controller
│   ├── custom_robot.hpp    # Main orchestrator
│   ├── logger.hpp          # Logging system
│   ├── mqtt.hpp            # MQTT client
│   ├── navigation.hpp      # Navigation and SLAM (inherits from unitree::robot::Client)
│   └── nlohmann/           # JSON library
├── src/                    # Source files
│   ├── config.cpp          # Configuration implementation
│   ├── controller.cpp      # Robot controller implementation
│   ├── custom_robot.cpp    # Main orchestrator implementation
│   ├── logger.cpp          # Logging system implementation
│   ├── main.cpp            # Entry point
│   ├── mqtt.cpp            # MQTT client implementation
│   └── navigation.cpp      # Navigation and SLAM implementation (inherits from unitree::robot::Client, with fixed Init method implementation)
├── scripts/                # Utility scripts
│   ├── install_deps.sh     # Install dependencies
│   └── run_robot.sh        # Run with optimal settings
└── build/                  # Build directory (created during build)

Troubleshooting

Common Issues

1. SDK Not Found: Ensure unitree_sdk2 is in the correct relative path (../unitree_sdk2)
2. MQTT Connection Failed: Check broker address, port, and credentials
3. Robot Not Responding: Verify network interface and robot connection
4. Permission Denied: Run with appropriate privileges for network access
5. Build Errors: Run ./scripts/install_deps.sh to install required packages

Network Issues

Test network connectivity to the robot:

# Check interface
ip addr show eth0

# Test robot connectivity (replace with robot IP)
ping 192.168.123.15

# Check if MQTT broker is accessible
telnet 192.168.2.236 1883

Performance Tuning

For optimal performance:

# Modify control frequency in include/config.hpp
# Rebuild the project
cd build
make -j$(nproc)

License

This project is licensed under the MIT License - see the LICENSE file for details.

Performance Notes

• Control Frequency: Default 200Hz, configurable up to 400Hz for high-performance applications
• Memory Usage: Optimized for minimal heap allocations during runtime
• CPU Usage: Multi-threaded design with separate threads for control and MQTT

Contributing

1. Fork the repository
2. Create a feature branch
3. Commit your changes
4. Push to the branch
5. Create a Pull Request

Version History

• v1.0.0: Initial release with full MQTT API and safety systems
• Modular architecture with clean separation of concerns
• Support for configuration through compile-time constants
• Comprehensive logging and error handling
• Navigation and SLAM capabilities

Acknowledgments

• Inspired by Python custom_unitree implementations
• Uses Unitree SDK2 for robot communication
• Built with Eclipse Paho MQTT C++ library
• JSON configuration powered by nlohmann/json