361 lines
9.7 KiB
Markdown
361 lines
9.7 KiB
Markdown
# 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
|
|
- **📊 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
|
|
- **Config**: Configuration loading, validation, and management
|
|
- **Logger**: Thread-safe logging system with multiple output targets
|
|
|
|
## Dependencies
|
|
|
|
### Required System Packages
|
|
|
|
```bash
|
|
# 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
|
|
|
|
```bash
|
|
# Create build directory
|
|
mkdir build && cd build
|
|
|
|
# Configure with CMake
|
|
cmake ..
|
|
|
|
# Build the project
|
|
make -j$(nproc)
|
|
|
|
# Run the robot (that's it!)
|
|
./main
|
|
```
|
|
|
|
The robot will start immediately with default settings and be ready to receive MQTT commands!
|
|
|
|
## Configuration
|
|
|
|
The system supports flexible configuration through JSON files and built-in presets. Configuration files are optional - the system can run with compile-time defaults.
|
|
|
|
### Configuration File Structure
|
|
|
|
Create `config/robot_config.json` for custom settings:
|
|
|
|
```json
|
|
{
|
|
"network": {
|
|
"interface": "eth0"
|
|
},
|
|
"mqtt": {
|
|
"broker": "localhost",
|
|
"port": 1883,
|
|
"username": "",
|
|
"password": "",
|
|
"client_id": "unitree_go2_client",
|
|
"keep_alive": 60,
|
|
"qos": 1
|
|
},
|
|
"control": {
|
|
"frequency": 200,
|
|
"state_publish_frequency": 50
|
|
},
|
|
"safety": {
|
|
"max_linear_velocity": 1.5,
|
|
"max_angular_velocity": 2.0,
|
|
"emergency_stop_timeout": 5.0,
|
|
"command_timeout": 2.0
|
|
},
|
|
"logging": {
|
|
"level": "INFO",
|
|
"file_output": false,
|
|
"console_output": true
|
|
}
|
|
}
|
|
```
|
|
|
|
### Using Configuration Presets
|
|
|
|
Instead of creating JSON files, you can use built-in presets:
|
|
|
|
```bash
|
|
# High performance mode
|
|
./build/unitree_go2_custom --preset high-perf
|
|
|
|
# Development mode
|
|
./build/unitree_go2_custom --preset dev
|
|
|
|
# Safety-first mode
|
|
./build/unitree_go2_custom --preset safety
|
|
```
|
|
|
|
## Usage
|
|
|
|
### Basic Usage
|
|
|
|
```bash
|
|
# Simple execution - just run it!
|
|
./build/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` (auto-detected)
|
|
- **MQTT Broker**: `localhost:1883`
|
|
- **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/motion`: Motion commands
|
|
- `unitree/go2/cmd/control`: System control commands
|
|
- `unitree/go2/cmd/config`: Configuration updates
|
|
|
|
### 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
|
|
|
|
### Motion Commands
|
|
|
|
```json
|
|
{
|
|
"request_id": "unique_id",
|
|
"type": "motion",
|
|
"motion_type": "velocity",
|
|
"linear_velocity": [1.0, 0.0, 0.0],
|
|
"angular_velocity": [0.0, 0.0, 0.5],
|
|
"duration": 2.0
|
|
}
|
|
```
|
|
|
|
### Special Actions
|
|
|
|
```json
|
|
{
|
|
"request_id": "unique_id",
|
|
"type": "special_action",
|
|
"action": "dance1",
|
|
"params": {}
|
|
}
|
|
```
|
|
|
|
### System Commands
|
|
|
|
```json
|
|
{
|
|
"request_id": "unique_id",
|
|
"type": "system",
|
|
"command": "emergency_stop"
|
|
}
|
|
```
|
|
|
|
## Supported Actions
|
|
|
|
### Basic Motions
|
|
- `stand_up`: Stand up from lying position
|
|
- `stand_down`: Lie down from standing position
|
|
- `sit`: Sit down
|
|
- `damp`: Enable damping mode
|
|
- `balance_stand`: Balanced standing with pose control
|
|
- `recovery_stand`: Recovery from emergency stop
|
|
|
|
### Special Actions
|
|
- `dance1`, `dance2`: Dance routines
|
|
- `hello`: Greeting gesture
|
|
- `stretch`: Stretching motion
|
|
- `front_flip`, `back_flip`: Acrobatic flips (use with caution)
|
|
|
|
### Movement
|
|
- Velocity control: Linear and angular velocity commands
|
|
- Position control: Move to absolute positions
|
|
- Body pose control: Roll, pitch, yaw, and height adjustment
|
|
|
|
## 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
|
|
- **Battery Monitoring**: Low battery detection and safe shutdown
|
|
- **Connection Monitoring**: Automatic reconnection and error handling
|
|
|
|
## Development
|
|
|
|
### Adding New Actions
|
|
|
|
1. Add action handler in `Controller::performAction()`
|
|
2. Update MQTT command processing in `CustomRobot::processSpecialAction()`
|
|
3. Add action documentation
|
|
|
|
### Extending MQTT API
|
|
|
|
1. Define new message types in the appropriate handler functions
|
|
2. Update JSON parsing and validation
|
|
3. Add corresponding response handling
|
|
|
|
### Custom Configurations
|
|
|
|
Create specialized configuration files for different environments:
|
|
|
|
```bash
|
|
# Development configuration
|
|
cp config/robot_config.json config/dev_config.json
|
|
|
|
# Production configuration
|
|
cp config/robot_config.json config/prod_config.json
|
|
```
|
|
|
|
## Quick Start Scripts
|
|
|
|
The project includes utility scripts for easy deployment:
|
|
|
|
```bash
|
|
# 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
|
|
├── src/ # Source files
|
|
│ ├── config.cpp
|
|
│ ├── controller.cpp
|
|
│ ├── custom_robot.cpp
|
|
│ ├── logger.cpp
|
|
│ ├── main.cpp
|
|
│ └── mqtt.cpp
|
|
└── scripts/ # Utility scripts
|
|
├── install_deps.sh # Install dependencies
|
|
└── run_robot.sh # Run with optimal settings
|
|
```
|
|
|
|
## 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:
|
|
|
|
```bash
|
|
# 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 localhost 1883
|
|
```
|
|
|
|
### Performance Tuning
|
|
|
|
For optimal performance:
|
|
|
|
```bash
|
|
# Use high-performance preset
|
|
./build/unitree_go2_custom --preset high-perf
|
|
|
|
# Or customize control frequency
|
|
./build/unitree_go2_custom -c config/high_freq_config.json
|
|
```
|
|
|
|
## License
|
|
|
|
This project is licensed under the same terms as the original custom_unitree Python implementation.
|
|
|
|
## Performance Notes
|
|
|
|
- **Control Frequency**: Default 200Hz, configurable up to 500Hz 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. Follow the existing code style and patterns
|
|
2. Add appropriate error handling and logging
|
|
3. Update documentation for new features
|
|
4. Test thoroughly with the actual robot hardware
|
|
5. Use the provided scripts for consistent development environment
|
|
|
|
## Version History
|
|
|
|
- **v1.0.0**: Initial release with full MQTT API and safety systems
|
|
- Modular architecture with clean separation of concerns
|
|
- Support for configuration presets and runtime parameter adjustment
|
|
- Comprehensive logging and error handling
|
|
|
|
## 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 |