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
• 📊 State Publishing: Real-time robot state broadcasting
• 🎭 Special Actions: Support for dances, tricks, and custom motions
• 🚀 High Performance: Optimized for real-time control with configurable frequencies
• 🔧 Development Tools: Built-in scripts and utilities for easy deployment

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:

// Network settings
constexpr std::string_view NETWORK_INTERFACE = "eth0";

// MQTT settings
constexpr std::string_view MQTT_BROKER = "192.168.2.236";
constexpr int MQTT_PORT = 1883;
constexpr std::string_view MQTT_CLIENT_ID = "unitree_go2_client";
constexpr std::string_view MQTT_USERNAME = "lzwc";
constexpr std::string_view MQTT_PASSWORD = "Lzwc@4187.";

// Topic settings
constexpr std::string_view TOPIC_PREFIX = "unitree/go2";
constexpr std::string_view TOPIC_CMD = "cmd";
constexpr std::string_view TOPIC_STATE = "state";

// Robot control settings
constexpr double CONTROL_FREQUENCY = 200.0;       // Hz
constexpr double STATE_PUBLISH_FREQUENCY = 50.0;  // Hz

// Safety settings
constexpr double MAX_LINEAR_VELOCITY = 1.5;       // m/s
constexpr double MAX_ANGULAR_VELOCITY = 2.0;      // rad/s
constexpr double EMERGENCY_STOP_TIMEOUT = 5.0;    // seconds

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:

• Network Interface: eth0
• MQTT Broker: 192.168.2.236:1883
• MQTT Credentials: Username lzwc, Password Lzwc@4187.
• Control Frequency: 200Hz
• Safety Limits: Conservative settings for safe operation
• Logging: INFO level to console

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/oac: Obstacle avoidance commands
• unitree/go2/cmd/sport: Sport mode commands
• unitree/go2/cmd/rsc: Robot state commands
• unitree/go2/cmd/nav: Navigation commands

State Topics

• unitree/go2/state/robot: Robot state (position, IMU, motors, etc.)
• unitree/go2/state/heartbeat: System status and statistics
• unitree/go2/state/response: Command execution responses
• unitree/go2/state/error: Error messages

Sport Commands

{
  "request_id": "unique_id",
  "cmd": "StandUp"
}

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

Navigation Commands

{
  "request_id": "unique_id",
  "cmd": "startMapping"
}

Supported navigation commands:

• startMapping: Start SLAM mapping
• endMapping: End SLAM mapping
• pauseNavigation: Pause navigation
• resumeNavigation: Resume navigation
• closeSlam: Close SLAM service

System Commands

{
  "request_id": "unique_id",
  "cmd": "GetServiceList"
}

Supported system commands:

• GetServiceList: Get list of available services
• SwitchService: Enable/disable a service
• SetReportFreq: Set state reporting frequency

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

Navigation Actions

• startMapping: Start SLAM mapping
• endMapping: End SLAM mapping and save map
• pauseNavigation: Pause navigation
• resumeNavigation: Resume navigation
• closeSlam: 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

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

Custom Configurations

To customize the configuration, modify the values in include/config.hpp:

// Example: Change MQTT broker address
constexpr std::string_view MQTT_BROKER = "192.168.1.100";

// Example: Change control frequency
constexpr double CONTROL_FREQUENCY = 400.0;  // Hz

After making changes, rebuild the project:

cd build
make -j$(nproc)

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
├── 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
│   └── nlohmann/           # JSON library
├── src/                    # Source files
│   ├── config.cpp
│   ├── controller.cpp
│   ├── custom_robot.cpp
│   ├── logger.cpp
│   ├── main.cpp
│   ├── mqtt.cpp
│   └── navigation.cpp
├── 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
• State Publishing: Default 50Hz, can be adjusted based on network bandwidth
• Memory Usage: Optimized for minimal heap allocations during runtime
• CPU Usage: Multi-threaded design with separate threads for control, MQTT, and state publishing

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