Robotics

Unitree Qmini Robot

Development

Thanks for these maintainers: Yanyun Chen, Tiyu Fang, Kaiwen Li, Kunqi Zhang, and Wei zhang

Features

  • High-Performance Inference — Optimized C++ implementation with ONNX Runtime for low-latency, real-time policy execution on robots like Unitree Qmini.
  • Hardware Acceleration — Supports CPU/GPU backends (including CUDA) for maximum inference speed on Linux-based edge devices.
  • Modular Architecture — Easy-to-use API for integrating with custom robot hardware, sensors, and actuators.
  • Safety-Critical Design — Built-in emergency stop mechanisms, sensor validation, and fail-safe protocols for real-world operation.
  • Real-Time Control — Thread-safe implementation supporting hard real-time constraints.
  • Documentation & Examples — Step-by-step guides for robot deployment and custom hardware setups.

Prerequisites

Note

Some params are hard-coded in Motor_thread.hpp, run_interface.cpp, and test_interface.cpp. Be careful about them. This repository is not maintained anymore. If you have any question, please send emails to info@vsislab.com. The project can only be run after successful installation.

Install Dependencies

$ sudo apt install cmake && sudo apt install python3-pip && sudo apt install libjsoncpp-dev
$ cmake --version

Unitree SDK 2

Environment

  • OS (Ubuntu 20.04 LTS)
  • CPU (aarch64 or x86_64)
  • Compiler (gcc version 9.4.0)
$ sudo apt install libeigen3-dev
$ cd ~/
$ git clone https://github.com/unitreerobotics/unitree_sdk2
$ cd unitree_sdk2 && mkdir build && cd build
$ cmake .. -DCMAKE_INSTALL_PREFIX=/opt/unitree
$ sudo make install

You can refer to example/cmake_sample on how to import the unitree_sdk2 into your CMake project.

Note that if you install the library to other places other than /opt/unitree_robotics, you need to make sure the path is added to ${CMAKE_PREFIX_PATH} so that cmake can find it with "find_package()".

If you just want to test it temporarily, run:

$ export CMAKE_PREFIX_PATH=/opt/unitree:$CMAKE_PREFIX_PATH

Alternatively, to make this change permanent, add the export command to your shell configuration file.

$ echo "export CMAKE_PREFIX_PATH=/opt/unitree:$CMAKE_PREFIX_PATH" >> ~/.bashrc

To validate it:

$ echo $CMAKE_PREFIX_PATH

Alternative: Pass Path Directly in CMake, when building your project, you can also explicitly specify the path:

$ cmake .. -DCMAKE_PREFIX_PATH=/opt/unitree

Unitree Actuator SDK

  • Support motor: GO-M8010-6 motor
  • Not support motor: A1 motor、 B1 motor (Check A1B1 branch for support)

Prerequisites

  • gcc >= 5.4.0 (for libUnitreeMotorSDK_M80106_Linux64.so)
  • gcc >= 8.3.0 (for libUnitreeMotorSDK_M80106_Arm64.so)

Run gcc -v command to check your gcc version

Build

$ cd ~/
$ git clone https://github.com/unitreerobotics/unitree_actuator_sdk
$ cd unitree_actuator_sdk
$ git checkout GO-M8010-6
$ mkdir build
$ cd build
$ cmake ..
$ make

Motor Tools

  1. View motor ID

To view and modify a motor ID, the motor must be switched to factory mode. Before switching, ensure that all motors have stopped working and the host is no longer sending motion control commands to the motors.

$ cd ~/unitree_actuator_sdk/motor_tools/Unitree_MotorTools_<version>_<architecture>
$ ./swboot /dev/ttyUSB0

Under normal circumstances, there should be no motors with an ID greater than 15 in the printed list. If this occurs, please power cycle the motor and try again. If there is a motor with ID 15, it indicates that the motor has not yet been assigned an ID. Please follow the instructions below to modify the motor ID.

  1. Change motor ID

Before changing motor ID, ensure that all motors have stopped working and that the host is no longer sending motion control commands to the motors.

For example: Change all motors with ID 0 on the bus to ID 1.

$ cd ~/unitree_actuator_sdk/motor_tools/Unitree_MotorTools_<version>_<architecture>
$ ./changeid /dev/ttyUSB0 0 1
  1. Back to Motor Mode

Viewing and modifying the motor ID switches the motor to factory mode. If you do not manually switch back to motor mode, the motor will remain in factory mode even after power cycling.

Motors in factory mode have a green indicator light on the back that flashes quickly three times per second. To switch back to motor mode, use the following command:

$ cd ~/unitree_actuator_sdk/motor_tools/Unitree_MotorTools_<version>_<architecture>
$ ./swmotor /dev/ttyUSB0

This command will switch all motors on the CAN bus back to motor mode. At this point, the motors can receive motion control commands. Motors without firmware will not start, and the terminal will display their status.

  1. Motor Firmware Upgrade

The GO-M8010-6 motor supports firmware upgrades, which allow for performance improvements and safety fixes in the future. Use the firmware file provided by Unitree and the unisp tool to download the firmware to the motor.

Command format:

$ unisp [serial port] [.bin upgrade file] [target motor ID]

For example:

$ cd ~/unitree_actuator_sdk/motor_tools/Unitree_MotorTools_<version>_<architecture>
$ ./unisp /dev/ttyUSB0 ./GoM80106_v1.0.bin 0

Yaml CPP

$ cd ~
$ git clone https://github.com/jbeder/yaml-cpp
$ cd yaml-cpp
$ mkdir build
$ cd build
$ cmake ..
$ make
$ sudo make install

Eigen Template Library

Eigen is a C++ template library for linear algebra: matrices, vectors, numerical solvers, and related algorithms.

$ cd ~
$ git clone https://gitlab.com/libeigen/eigen
$ cd eigen && mkdir build && cd build
$ cmake .. && make && sudo make install

It will be installed into /usr/local/include/eigen3/

JsonCpp

$ cd ~
$ git clone https://github.com/Microsoft/vcpkg
$ cd vcpkg
$ ./bootstrap-vcpkg.sh
$ ./vcpkg integrate install
$ ./vcpkg install jsoncpp

Joystick Controller

$ sudo apt install python3-pygame

Bluetooth Connectivity

Step 1: Install Required Packages

$ sudo apt update
$ sudo apt install bluetooth

Step 2: Start Bluetooth Service

$ sudo systemctl start bluetooth
$ sudo systemctl enable bluetooth

Step 3: Put PS4/5 Controller in Pairing Mode

  • Hold Share + PS button simultaneously for 3 seconds
  • The light bar should start flashing rapidly (white or blue)

Step 4: Pair using bluetoothctl

$ bluetoothctl

bluetoothctl> scan on
# Pair with the controller (replace MAC_ADDRESS with actual address)
bluetoothctl> pair XX:XX:XX:XX:XX:XX

# Trust the device
bluetoothctl> trust XX:XX:XX:XX:XX:XX

# Connect to the device
bluetoothctl> connect XX:XX:XX:XX:XX:XX

bluetoothctl> exit

PS Controller Pair via Bluetooth Script

#!/bin/bash
# ps4_pair.sh

echo "Put PS controller in pairing mode (Share + PS button for 3 seconds)"
echo "Press Enter when controller is flashing..."
read

# Start scanning and get MAC address
echo "Scanning for PS controller..."
timeout 10s bluetoothctl scan on &
sleep 5

# Get the MAC address of Wireless Controller
MAC=$(bluetoothctl devices | grep "Wireless Controller" | awk '{print $2}')

if [ -z "$MAC" ]; then
    echo "PS controller not found. Make sure it's in pairing mode."
    exit 1
fi

echo "Found PS controller: $MAC"

# Pair, trust, and connect
echo "Pairing..."
echo -e "pair $MAC\ntrust $MAC\nconnect $MAC\nexit" | bluetoothctl

echo "PS controller should now be paired and connected!"

Make it executable and run:

$ chmod +x ps_pair.sh
$ ./ps_pair.sh

USB Connectivity

You typically need to set up permissions in Ubuntu 22.04 for PS4 controllers connected via USB. Here's what you need to do:

  1. Add udev rules
$ sudo nano /etc/udev/rules.d/99-ps4-controller.rules
  1. Add the following content:
# PS4 Controller (DualShock 4)
SUBSYSTEM=="usb", ATTRS{idVendor}=="054c", ATTRS{idProduct}=="05c4", MODE="0666"
SUBSYSTEM=="usb", ATTRS{idVendor}=="054c", ATTRS{idProduct}=="09cc", MODE="0666"
SUBSYSTEM=="hidraw", ATTRS{idVendor}=="054c", ATTRS{idProduct}=="05c4", MODE="0666"
SUBSYSTEM=="hidraw", ATTRS{idVendor}=="054c", ATTRS{idProduct}=="09cc", MODE="0666"
  1. Reload udev rules:
$ sudo udevadm control --reload-rules
$ sudo udevadm trigger
  1. Verify controller detection
$ lsusb | grep Sony
# or check input device
$ ls /dev/input/js*

Install ONNX Runtime GPU (CUDA 12.x)

ONNX Runtime is a cross-platform machine-learning model accelerator, with a flexible interface to integrate hardware-specific libraries. ONNX Runtime can be used with models from PyTorch, Tensorflow/Keras, TFLite, scikit-learn, and other frameworks.

Please visit the Jetson Zoo ONNX Runtime page and locate the appropriate .whl file corresponding to your installed Python and JetPack versions on the Jetson Orin Nano. Copy the link for use in your environment setup.

$ conda create -n qmini python=3.11
$ conda activate qmini
$ pip install "numpy<2" #Downgrade Numpy
$ wget https://nvidia.box.com/shared/static/9yvw05k6u343qfnkhdv2x6xhygze0aq1.whl -O onnxruntime_gpu-1.19.0-cp311-cp311-linux_aarch64.whl
$ pip install onnxruntime_gpu-1.19.0-cp311-cp311-linux_aarch64.whl
$ python3
>>> import onnxruntime as ort
>>> ort.get_available_providers()

Note

Will return this: ['TensorrtExecutionProvider', 'CUDAExecutionProvider', 'CPUExecutionProvider']

Clone the RoboTammerSdk4Qmini

$ cd ~
$ git clone https://github.com/vsislab/RoboTamerSdk4Qmini
$ cd RoboTamerSdk4Qmini/lib/onnx
$ sudo cp -r libonnxruntime.so libonnxruntime.so.1.<x>.1 /usr/local/lib
$ sudo ldconfig

$ cd ~/RoboTamerSdk4Qmini/lib/m8010motor
$ sudo cp libUnitreeMotorSDK_Linux64.so /usr/local/lib/
#or
$ sudo cp libUnitreeMotorSDK_Arm64.so /usr/local/lib/

OAK-D-Pro W IMU

$ pip install depthai
$ echo 'SUBSYSTEM=="usb", ATTRS{idVendor}=="03e7", MODE="0666"' | sudo tee /etc/udev/rules.d/80-movidius.rules
$ sudo udevadm control --reload-rules && sudo udevadm trigger
$ lsusb | grep -i movidius

Note

Bus 001 Device 005: ID 03e7:2485 Intel Movidius MyriadX

$ cd ~/RoboTamerSdk4Qmini/bin
# Basic usage - collect for 60 seconds at 100Hz, save as JSON
$ python3 imu_oak_dump.py
# Quick 10-second test with console output and save data as a txt file
$ python3 imu_oak_dump.py --duration 10 --verbose
# Collect for 30 seconds at 200Hz, save as CSV with verbose output
$ python3 imu_oak_dump.py --duration 30 --frequency 200 --format csv --verbose --output my_imu_data.csv

imu_receiver.py

Replace the /user/IMU/imu_receiver.py with the following code:

import argparse
import sys
import depthai as dai
import json
import math
import time

# Constants
PI = 3.141592653589793
DEG_TO_RAD = 0.017453292519943295
isrun = True

# Global variables for persistent connection
device = None
queue = None
madgwick = None

def parse_opt(known=False):
    parser = argparse.ArgumentParser()
    parser.add_argument('--frequency', type=int, default=100, help='IMU sampling frequency; default: 100Hz')
    parser.add_argument('--timeout', type=int, default=20, help='set the timeout; default: 20')
    
    receive_params = parser.parse_known_args()[0] if known else parser.parse_args()
    return receive_params

class MadgwickFilter:
    """Madgwick AHRS filter for quaternion estimation from accelerometer and gyroscope"""
    def __init__(self, beta=0.1, sample_freq=100):
        self.beta = beta
        self.sample_freq = sample_freq
        self.q = [1.0, 0.0, 0.0, 0.0]  # quaternion [w, x, y, z]
        
    def update(self, gx, gy, gz, ax, ay, az):
        """Update quaternion with gyroscope and accelerometer data"""
        q1, q2, q3, q4 = self.q
        
        # Normalise accelerometer measurement
        norm = math.sqrt(ax * ax + ay * ay + az * az)
        if norm == 0:
            return self.q
        ax /= norm
        ay /= norm
        az /= norm
        
        # Auxiliary variables to avoid repeated arithmetic
        _2q1 = 2 * q1
        _2q2 = 2 * q2
        _2q3 = 2 * q3
        _2q4 = 2 * q4
        _4q1 = 4 * q1
        _4q2 = 4 * q2
        _4q3 = 4 * q3
        _8q2 = 8 * q2
        _8q3 = 8 * q3
        q1q1 = q1 * q1
        q2q2 = q2 * q2
        q3q3 = q3 * q3
        q4q4 = q4 * q4
        
        # Gradient decent algorithm corrective step
        s1 = _4q1 * q3q3 + _2q3 * ax + _4q1 * q2q2 - _2q2 * ay
        s2 = _4q2 * q4q4 - _2q4 * ax + 4 * q1q1 * q2 - _2q1 * ay - _4q2 + _8q2 * q2q2 + _8q2 * q3q3 + _4q2 * az
        s3 = 4 * q1q1 * q3 + _2q1 * ax + _4q3 * q4q4 - _2q4 * ay - _4q3 + _8q3 * q2q2 + _8q3 * q3q3 + _4q3 * az
        s4 = 4 * q2q2 * q4 - _2q2 * ax + 4 * q3q3 * q4 - _2q3 * ay
        
        # Normalise step magnitude
        norm = math.sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4)
        if norm != 0:
            s1 /= norm
            s2 /= norm
            s3 /= norm
            s4 /= norm
        
        # Apply feedback step
        qDot1 = 0.5 * (-q2 * gx - q3 * gy - q4 * gz) - self.beta * s1
        qDot2 = 0.5 * (q1 * gx + q3 * gz - q4 * gy) - self.beta * s2
        qDot3 = 0.5 * (q1 * gy - q2 * gz + q4 * gx) - self.beta * s3
        qDot4 = 0.5 * (q1 * gz + q2 * gy - q3 * gx) - self.beta * s4
        
        # Integrate rate of change of quaternion
        q1 += qDot1 * (1.0 / self.sample_freq)
        q2 += qDot2 * (1.0 / self.sample_freq)
        q3 += qDot3 * (1.0 / self.sample_freq)
        q4 += qDot4 * (1.0 / self.sample_freq)
        
        # Normalise quaternion
        norm = math.sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4)
        if norm != 0:
            self.q = [q1/norm, q2/norm, q3/norm, q4/norm]
        
        return self.q

def quaternion_to_euler(qw, qx, qy, qz):
    """Convert quaternion to Euler angles (roll, pitch, yaw)"""
    # Roll (x-axis rotation)
    sinr_cosp = 2 * (qw * qx + qy * qz)
    cosr_cosp = 1 - 2 * (qx * qx + qy * qy)
    roll = math.atan2(sinr_cosp, cosr_cosp)
    
    # Pitch (y-axis rotation)
    sinp = 2 * (qw * qy - qz * qx)
    if abs(sinp) >= 1:
        pitch = math.copysign(PI / 2, sinp)  # use 90 degrees if out of range
    else:
        pitch = math.asin(sinp)
    
    # Yaw (z-axis rotation)
    siny_cosp = 2 * (qw * qz + qx * qy)
    cosy_cosp = 1 - 2 * (qy * qy + qz * qz)
    yaw = math.atan2(siny_cosp, cosy_cosp)
    
    return roll, pitch, yaw

def initialize_imu_connection(frequency=100):
    """Initialize a persistent connection to the OAK-D-Pro W device"""
    global device, queue, madgwick
    
    try:
        # Create pipeline
        pipeline = dai.Pipeline()
        
        # Create IMU node - BMI270 only supports ACCELEROMETER_RAW and GYROSCOPE_RAW
        imu = pipeline.create(dai.node.IMU)
        imu.enableIMUSensor(dai.IMUSensor.ACCELEROMETER_RAW, frequency)
        imu.enableIMUSensor(dai.IMUSensor.GYROSCOPE_RAW, frequency)
        imu.setBatchReportThreshold(1)
        imu.setMaxBatchReports(10)
        
        # Create output
        imuOut = pipeline.create(dai.node.XLinkOut)
        imuOut.setStreamName("imu")
        imu.out.link(imuOut.input)
        
        # Initialize Madgwick filter for quaternion estimation
        madgwick = MadgwickFilter(beta=0.1, sample_freq=frequency)
        
        # Connect to device and start pipeline
        device = dai.Device(pipeline)
        queue = device.getOutputQueue(name="imu", maxSize=50, blocking=False)
        print(f"Connected to OAK-D-Pro W device with BMI270 IMU sensor")
        return True
    except Exception as e:
        print(f"Error initializing OAK-D-Pro W IMU connection: {e}")
        return False

def read_imu_data(frequency=100, timeout=1):
    """Read IMU data from OAK-D-Pro W device (BMI270 sensor)"""
    global device, queue, madgwick
    
    # Initialize connection if not already done
    if device is None or queue is None or madgwick is None:
        if not initialize_imu_connection(frequency):
            return {
                "Accelerometer_X": 0,
                "Accelerometer_Y": 0,
                "Accelerometer_Z": 0,
                "RollSpeed": 0,
                "PitchSpeed": 0,
                "HeadingSpeed": 0,
                "Roll": 0,
                "Pitch": 0,
                "Heading": 0,
                "qw": 0,
                "qx": 0,
                "qy": 0,
                "qz": 0,
            }
    
    try:
        # Get data from the queue
        inIMU = queue.get()
        if inIMU is None:
            return None
                
        imuData = inIMU.packets[-1]
        
        # Get raw accelerometer and gyroscope data
        ax = imuData.acceleroMeter.x
        ay = imuData.acceleroMeter.y
        az = imuData.acceleroMeter.z
        
        gx = imuData.gyroscope.x
        gy = imuData.gyroscope.y
        gz = imuData.gyroscope.z
        
        # Update Madgwick filter to get quaternion
        quaternion = madgwick.update(gx, gy, gz, ax, ay, az)
        qw, qx, qy, qz = quaternion
        
        # Calculate Euler angles from quaternion
        roll, pitch, yaw = quaternion_to_euler(qw, qx, qy, qz)
        
        # Apply the same coordinate transformations as the original code
        result = {
            "Accelerometer_X": ax,
            "Accelerometer_Y": ay,
            "Accelerometer_Z": az,
            "RollSpeed": gy,  # Swapped as in original
            "PitchSpeed": gx * -1,  # Inverted as in original
            "HeadingSpeed": gz,
            "Roll": pitch,  # Swapped as in original
            "Pitch": roll * -1,  # Inverted as in original
            "Heading": yaw,
            "qw": qw,
            "qx": qx,
            "qy": qy,
            "qz": qz,
        }
        return result
            
    except Exception as e:
        print(f"Error reading OAK-D-Pro W IMU data: {e}")
        # Try to reinitialize the connection
        if initialize_imu_connection(frequency):
            return read_imu_data(frequency, timeout)  # Try again
        return {
            "Accelerometer_X": 0,
            "Accelerometer_Y": 0,
            "Accelerometer_Z": 0,
            "RollSpeed": 0,
            "PitchSpeed": 0,
            "HeadingSpeed": 0,
            "Roll": 0,
            "Pitch": 0,
            "Heading": 0,
            "qw": 0,
            "qx": 0,
            "qy": 0,
            "qz": 0,
        }

def cleanup_imu_connection():
    """Clean up the IMU connection when done"""
    global device
    if device is not None:
        device.close()
        device = None

if __name__ == "__main__":
    args = parse_opt()
    try:
        initialize_imu_connection(frequency=args.frequency)
        while True:
            imu_data = read_imu_data(frequency=args.frequency, timeout=args.timeout)
            print(f"IMU Data: {imu_data}")
            time.sleep(0.01)  # Small delay to prevent overwhelming output
    except KeyboardInterrupt:
        print("Exiting...")
    finally:
        cleanup_imu_connection()

imu_oak_dump.py

import argparse
import sys
import time
import json
import depthai as dai
import math
import numpy as np

# Constants
PI = 3.141592653589793
DEG_TO_RAD = 0.017453292519943295

def parse_opt(known=False):
    parser = argparse.ArgumentParser(description='OAK-D-Pro W IMU Data Dumper')
    parser.add_argument('--output', type=str, default='oak_imu_data.txt', help='output file to dump IMU data')
    parser.add_argument('--duration', type=int, default=60, help='duration to collect data in seconds; default: 60')
    parser.add_argument('--frequency', type=int, default=100, help='IMU sampling frequency; default: 100Hz')
    parser.add_argument('--format', type=str, choices=['json', 'csv'], default='json', help='output format: json or csv')
    parser.add_argument('--verbose', action='store_true', help='print data to console as well')
    
    receive_params = parser.parse_known_args()[0] if known else parser.parse_args()
    return receive_params

class MadgwickFilter:
    """Madgwick AHRS filter for quaternion estimation from accelerometer and gyroscope"""
    def __init__(self, beta=0.1, sample_freq=100):
        self.beta = beta
        self.sample_freq = sample_freq
        self.q = [1.0, 0.0, 0.0, 0.0]  # quaternion [w, x, y, z]
        
    def update(self, gx, gy, gz, ax, ay, az):
        """Update quaternion with gyroscope and accelerometer data"""
        q1, q2, q3, q4 = self.q
        
        # Normalise accelerometer measurement
        norm = math.sqrt(ax * ax + ay * ay + az * az)
        if norm == 0:
            return
        ax /= norm
        ay /= norm
        az /= norm
        
        # Auxiliary variables to avoid repeated arithmetic
        _2q1 = 2 * q1
        _2q2 = 2 * q2
        _2q3 = 2 * q3
        _2q4 = 2 * q4
        _4q1 = 4 * q1
        _4q2 = 4 * q2
        _4q3 = 4 * q3
        _8q2 = 8 * q2
        _8q3 = 8 * q3
        q1q1 = q1 * q1
        q2q2 = q2 * q2
        q3q3 = q3 * q3
        q4q4 = q4 * q4
        
        # Gradient decent algorithm corrective step
        s1 = _4q1 * q3q3 + _2q3 * ax + _4q1 * q2q2 - _2q2 * ay
        s2 = _4q2 * q4q4 - _2q4 * ax + 4 * q1q1 * q2 - _2q1 * ay - _4q2 + _8q2 * q2q2 + _8q2 * q3q3 + _4q2 * az
        s3 = 4 * q1q1 * q3 + _2q1 * ax + _4q3 * q4q4 - _2q4 * ay - _4q3 + _8q3 * q2q2 + _8q3 * q3q3 + _4q3 * az
        s4 = 4 * q2q2 * q4 - _2q2 * ax + 4 * q3q3 * q4 - _2q3 * ay
        
        # Normalise step magnitude
        norm = math.sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4)
        if norm != 0:
            s1 /= norm
            s2 /= norm
            s3 /= norm
            s4 /= norm
        
        # Apply feedback step
        qDot1 = 0.5 * (-q2 * gx - q3 * gy - q4 * gz) - self.beta * s1
        qDot2 = 0.5 * (q1 * gx + q3 * gz - q4 * gy) - self.beta * s2
        qDot3 = 0.5 * (q1 * gy - q2 * gz + q4 * gx) - self.beta * s3
        qDot4 = 0.5 * (q1 * gz + q2 * gy - q3 * gx) - self.beta * s4
        
        # Integrate rate of change of quaternion
        q1 += qDot1 * (1.0 / self.sample_freq)
        q2 += qDot2 * (1.0 / self.sample_freq)
        q3 += qDot3 * (1.0 / self.sample_freq)
        q4 += qDot4 * (1.0 / self.sample_freq)
        
        # Normalise quaternion
        norm = math.sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4)
        if norm != 0:
            self.q = [q1/norm, q2/norm, q3/norm, q4/norm]
        
        return self.q

def quaternion_to_euler(qw, qx, qy, qz):
    """Convert quaternion to Euler angles (roll, pitch, yaw)"""
    # Roll (x-axis rotation)
    sinr_cosp = 2 * (qw * qx + qy * qz)
    cosr_cosp = 1 - 2 * (qx * qx + qy * qy)
    roll = math.atan2(sinr_cosp, cosr_cosp)
    
    # Pitch (y-axis rotation)
    sinp = 2 * (qw * qy - qz * qx)
    if abs(sinp) >= 1:
        pitch = math.copysign(PI / 2, sinp)  # use 90 degrees if out of range
    else:
        pitch = math.asin(sinp)
    
    # Yaw (z-axis rotation)
    siny_cosp = 2 * (qw * qz + qx * qy)
    cosy_cosp = 1 - 2 * (qy * qy + qz * qz)
    yaw = math.atan2(siny_cosp, cosy_cosp)
    
    return roll, pitch, yaw

def read_oak_imu_data(frequency=100):
    """Read IMU data from OAK-D-Pro W device (BMI270 sensor)"""
    try:
        # Create pipeline
        pipeline = dai.Pipeline()
        
        # Create IMU node - BMI270 only supports ACCELEROMETER_RAW and GYROSCOPE_RAW
        imu = pipeline.create(dai.node.IMU)
        imu.enableIMUSensor(dai.IMUSensor.ACCELEROMETER_RAW, frequency)
        imu.enableIMUSensor(dai.IMUSensor.GYROSCOPE_RAW, frequency)
        imu.setBatchReportThreshold(1)
        imu.setMaxBatchReports(10)
        
        # Create output
        imuOut = pipeline.create(dai.node.XLinkOut)
        imuOut.setStreamName("imu")
        imu.out.link(imuOut.input)
        
        # Initialize Madgwick filter for quaternion estimation
        madgwick = MadgwickFilter(beta=0.1, sample_freq=frequency)
        
        # Connect to device and start pipeline
        with dai.Device(pipeline) as device:
            q = device.getOutputQueue(name="imu", maxSize=50, blocking=False)
            
            print(f"Connected to OAK-D-Pro W device with BMI270 IMU sensor")
            
            while True:
                inIMU = q.get()
                if inIMU is None:
                    continue
                    
                imuData = inIMU.packets[-1]
                
                # Get raw accelerometer and gyroscope data
                ax = imuData.acceleroMeter.x
                ay = imuData.acceleroMeter.y
                az = imuData.acceleroMeter.z
                
                gx = imuData.gyroscope.x
                gy = imuData.gyroscope.y
                gz = imuData.gyroscope.z
                
                # Update Madgwick filter to get quaternion
                quaternion = madgwick.update(gx, gy, gz, ax, ay, az)
                qw, qx, qy, qz = quaternion
                
                # Calculate Euler angles from quaternion
                roll, pitch, yaw = quaternion_to_euler(qw, qx, qy, qz)
                
                result = {
                    "timestamp": time.time(),
                    "Accelerometer_X": ax,
                    "Accelerometer_Y": ay,
                    "Accelerometer_Z": az,
                    "RollSpeed": gx,
                    "PitchSpeed": gy,
                    "HeadingSpeed": gz,
                    "Roll": roll,
                    "Pitch": pitch,
                    "Heading": yaw,
                    "qw": qw,
                    "qx": qx,
                    "qy": qy,
                    "qz": qz,
                }
                
                yield result
                
    except Exception as e:
        print(f"Error reading OAK-D-Pro W IMU data: {e}")
        return None

def dump_imu_data():
    """Main function to dump IMU data"""
    args = parse_opt()
    
    print(f"Starting OAK-D-Pro W IMU data collection...")
    print(f"Duration: {args.duration} seconds")
    print(f"Frequency: {args.frequency} Hz")
    print(f"Output file: {args.output}")
    print(f"Format: {args.format}")
    print("Note: Using BMI270 sensor (ACCELEROMETER_RAW + GYROSCOPE_RAW only)")
    print("Quaternions calculated using Madgwick AHRS filter")
    print("Press Ctrl+C to stop early\n")
    
    start_time = time.time()
    data_count = 0
    
    try:
        with open(args.output, 'w') as f:
            # Write CSV header if needed
            if args.format == 'csv':
                header = "timestamp,Accelerometer_X,Accelerometer_Y,Accelerometer_Z,RollSpeed,PitchSpeed,HeadingSpeed,Roll,Pitch,Heading,qw,qx,qy,qz\n"
                f.write(header)
            
            for imu_data in read_oak_imu_data(args.frequency):
                if imu_data is None:
                    break
                    
                # Check if duration exceeded
                if time.time() - start_time > args.duration:
                    break
                
                # Write data to file
                if args.format == 'json':
                    f.write(json.dumps(imu_data) + '\n')
                elif args.format == 'csv':
                    csv_line = f"{imu_data['timestamp']},{imu_data['Accelerometer_X']},{imu_data['Accelerometer_Y']},{imu_data['Accelerometer_Z']},{imu_data['RollSpeed']},{imu_data['PitchSpeed']},{imu_data['HeadingSpeed']},{imu_data['Roll']},{imu_data['Pitch']},{imu_data['Heading']},{imu_data['qw']},{imu_data['qx']},{imu_data['qy']},{imu_data['qz']}\n"
                    f.write(csv_line)
                
                f.flush()  # Ensure data is written immediately
                
                # Print to console if verbose
                if args.verbose:
                    print(f"Sample {data_count + 1}: {json.dumps(imu_data, indent=2)}")
                
                data_count += 1
                
                # Small delay to prevent overwhelming the system
                time.sleep(0.001)
                
    except KeyboardInterrupt:
        print("\nData collection stopped by user.")
    except Exception as e:
        print(f"Error during data collection: {e}")
    
    elapsed_time = time.time() - start_time
    print(f"\nData collection completed!")
    print(f"Total samples collected: {data_count}")
    print(f"Elapsed time: {elapsed_time:.2f} seconds")
    print(f"Average sampling rate: {data_count/elapsed_time:.2f} Hz")
    print(f"Data saved to: {args.output}")

if __name__ == "__main__":
    dump_imu_data()

Full steps of operating RoboTamerSdk4Qmini on the real Qmini robot

Check USB Device Recognition

Plugin the Unitree USB to 485 adapter and run the following commands:

$ lsusb

It will output something like this:

Bus 001 Device 004: ID 0403:6011 Future Technology Devices International, Ltd FT4232H Quad HS USB-UART/FIFO IC

Here, vendor ID is 0403 and product ID is 6011. Remember both ID and use them for the rules later.

Next, add current user to the dialout group:

$ sudo usermod -a -G dialout $USER

Alternative: Use a udev rule for persistent permissions, create a udev rule to automatically set permissions:

$ sudo nano /etc/udev/rules.d/99-usb-serial.rules

Add this line:

SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", MODE="0666", GROUP="dialout", OWNER="ubuntu"

$ sudo udevadm control --reload-rules
$ sudo udevadm trigger

Modification

  1. Modify the USB to 485 adapter serial ports by editing include/user/Motor_thread file.
  2. Modify network interface to be your Jetson Orin Nano or Raspberry Pi with the actual network interface by editing source/run_interface.cpp file.
$ cd ~/RoboTamerSdk4Qmini
$ mkdir -p build && cd build
$ cmake -DPLATFORM=arm64 .. && make && cd ../bin

Step 1: Check the start-up posture of the robot Step 2: Boot up both the joy stick and the robot Step 3: Run the executable file

Manage Process

$ ps aux | grep run_interface
$ kill -9 ****

$ ./run_interface
#or
$ sudo ./run_interface

Step 4: Check the initial state of the robot Step 5: Enter 1 and press enter into ready mode. Step 6: Enter 2 and press enter into position stand mode. Step 7: Enter 3 and press enter into AI stand control mode. Now enjoy your robot!

Reinforcement Training

This repository offers an open-source framework for bipedal robot motion control, leveraging deep reinforcement learning within NVIDIA's Isaac Gym environment. It enables training robots like Unitree Qmini to walk on rough terrain and includes critical domain randomization and random pushes during training for sim-to-real transfer. The repository contains the complete codebase for both training and deploying the biped robots in simulation environments.

Source Code URL: https://github.com/vsislab/RoboTamer4Qmini

Prerequisites

  • Ubuntu 18.04 or 20.04
  • NVIDIA driver version 470+
  • Hardware: NVIDIA Pascal or later GPU with at least 8 gb of VRAM
  • CUDA 11.4+
  • Python 3.8+
  • PyTorch 2.0.0+
  • Isaac Gym 1.0rc3+ (for simulation environments)
  • Additional dependencies (see requirements.txt and Install dependencies)
  • Miniconda

Installation

$ conda create -n isaac python==3.8 && conda activate isaac
$ pip install torch==2.0.0 torchvision==0.15.1 torchaudio==2.0.0
$ tar -zxvf IsaacGym_Preview_3_Package.tar.gz && cd ./isaacgym/python && pip install -e . 
$ pip3 install requirements.txt
$ pip3 install matplotlib pandas tensorboard opencv-python numpy==1.23.5 openpyxl onnxruntime onnx

Usage

To train (default:test):

$ python train.py --config BIRL --name <name>
  • --name # Experiment name (Default: 'test'), overrides settings in the config file
    • --config # Experiment configuration file (Default: 'config.Base'), overrides default configuration
    • --resume # Resume training from checkpoint (Default: test), requires specifying checkpoint 'path'
    • --render # Boolean flag (Default: False), force display off at all times
    • --fix_cam # Boolean flag (Default: False), fix camera view on the robot in environment 0
    • --horovod # Boolean flag (Default: False), enable Horovod multi-GPU training
    • --rl_device # RL device (Default: 'cuda:0'), supports formats like 'cpu'/'cuda:0'
    • --num_envs # Number of environments (Default: None), overrides config file settings
    • --seed # Random seed (Default: None), overrides config file settings
    • --max_iterations # Maximum number of iterations (Default: None), overrides config file settings
Previous
ROS
Next
Bundle