Build Qmini Robot with OAK-D-Pro W IMU

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

1import argparse
2import sys
3import depthai as dai
4import json
5import math
6import time
7
8# Constants
9PI = 3.141592653589793
10DEG_TO_RAD = 0.017453292519943295
11isrun = True
12
13# Global variables for persistent connection
14device = None
15queue = None
16madgwick = None
17
18def parse_opt(known=False):
19    parser = argparse.ArgumentParser()
20    parser.add_argument('--frequency', type=int, default=100, help='IMU sampling frequency; default: 100Hz')
21    parser.add_argument('--timeout', type=int, default=20, help='set the timeout; default: 20')
22    
23    receive_params = parser.parse_known_args()[0] if known else parser.parse_args()
24    return receive_params
25
26class MadgwickFilter:
27    """Madgwick AHRS filter for quaternion estimation from accelerometer and gyroscope"""
28    def __init__(self, beta=0.1, sample_freq=100):
29        self.beta = beta
30        self.sample_freq = sample_freq
31        self.q = [1.0, 0.0, 0.0, 0.0]  # quaternion [w, x, y, z]
32        
33    def update(self, gx, gy, gz, ax, ay, az):
34        """Update quaternion with gyroscope and accelerometer data"""
35        q1, q2, q3, q4 = self.q
36        
37        # Normalise accelerometer measurement
38        norm = math.sqrt(ax * ax + ay * ay + az * az)
39        if norm == 0:
40            return self.q
41        ax /= norm
42        ay /= norm
43        az /= norm
44        
45        # Auxiliary variables to avoid repeated arithmetic
46        _2q1 = 2 * q1
47        _2q2 = 2 * q2
48        _2q3 = 2 * q3
49        _2q4 = 2 * q4
50        _4q1 = 4 * q1
51        _4q2 = 4 * q2
52        _4q3 = 4 * q3
53        _8q2 = 8 * q2
54        _8q3 = 8 * q3
55        q1q1 = q1 * q1
56        q2q2 = q2 * q2
57        q3q3 = q3 * q3
58        q4q4 = q4 * q4
59        
60        # Gradient decent algorithm corrective step
61        s1 = _4q1 * q3q3 + _2q3 * ax + _4q1 * q2q2 - _2q2 * ay
62        s2 = _4q2 * q4q4 - _2q4 * ax + 4 * q1q1 * q2 - _2q1 * ay - _4q2 + _8q2 * q2q2 + _8q2 * q3q3 + _4q2 * az
63        s3 = 4 * q1q1 * q3 + _2q1 * ax + _4q3 * q4q4 - _2q4 * ay - _4q3 + _8q3 * q2q2 + _8q3 * q3q3 + _4q3 * az
64        s4 = 4 * q2q2 * q4 - _2q2 * ax + 4 * q3q3 * q4 - _2q3 * ay
65        
66        # Normalise step magnitude
67        norm = math.sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4)
68        if norm != 0:
69            s1 /= norm
70            s2 /= norm
71            s3 /= norm
72            s4 /= norm
73        
74        # Apply feedback step
75        qDot1 = 0.5 * (-q2 * gx - q3 * gy - q4 * gz) - self.beta * s1
76        qDot2 = 0.5 * (q1 * gx + q3 * gz - q4 * gy) - self.beta * s2
77        qDot3 = 0.5 * (q1 * gy - q2 * gz + q4 * gx) - self.beta * s3
78        qDot4 = 0.5 * (q1 * gz + q2 * gy - q3 * gx) - self.beta * s4
79        
80        # Integrate rate of change of quaternion
81        q1 += qDot1 * (1.0 / self.sample_freq)
82        q2 += qDot2 * (1.0 / self.sample_freq)
83        q3 += qDot3 * (1.0 / self.sample_freq)
84        q4 += qDot4 * (1.0 / self.sample_freq)
85        
86        # Normalise quaternion
87        norm = math.sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4)
88        if norm != 0:
89            self.q = [q1/norm, q2/norm, q3/norm, q4/norm]
90        
91        return self.q
92
93def quaternion_to_euler(qw, qx, qy, qz):
94    """Convert quaternion to Euler angles (roll, pitch, yaw)"""
95    # Roll (x-axis rotation)
96    sinr_cosp = 2 * (qw * qx + qy * qz)
97    cosr_cosp = 1 - 2 * (qx * qx + qy * qy)
98    roll = math.atan2(sinr_cosp, cosr_cosp)
99    
100    # Pitch (y-axis rotation)
101    sinp = 2 * (qw * qy - qz * qx)
102    if abs(sinp) >= 1:
103        pitch = math.copysign(PI / 2, sinp)  # use 90 degrees if out of range
104    else:
105        pitch = math.asin(sinp)
106    
107    # Yaw (z-axis rotation)
108    siny_cosp = 2 * (qw * qz + qx * qy)
109    cosy_cosp = 1 - 2 * (qy * qy + qz * qz)
110    yaw = math.atan2(siny_cosp, cosy_cosp)
111    
112    return roll, pitch, yaw
113
114def initialize_imu_connection(frequency=100):
115    """Initialize a persistent connection to the OAK-D-Pro W device"""
116    global device, queue, madgwick
117    
118    try:
119        # Create pipeline
120        pipeline = dai.Pipeline()
121        
122        # Create IMU node - BMI270 only supports ACCELEROMETER_RAW and GYROSCOPE_RAW
123        imu = pipeline.create(dai.node.IMU)
124        imu.enableIMUSensor(dai.IMUSensor.ACCELEROMETER_RAW, frequency)
125        imu.enableIMUSensor(dai.IMUSensor.GYROSCOPE_RAW, frequency)
126        imu.setBatchReportThreshold(1)
127        imu.setMaxBatchReports(10)
128        
129        # Create output
130        imuOut = pipeline.create(dai.node.XLinkOut)
131        imuOut.setStreamName("imu")
132        imu.out.link(imuOut.input)
133        
134        # Initialize Madgwick filter for quaternion estimation
135        madgwick = MadgwickFilter(beta=0.1, sample_freq=frequency)
136        
137        # Connect to device and start pipeline
138        device = dai.Device(pipeline)
139        queue = device.getOutputQueue(name="imu", maxSize=50, blocking=False)
140        print(f"Connected to OAK-D-Pro W device with BMI270 IMU sensor")
141        return True
142    except Exception as e:
143        print(f"Error initializing OAK-D-Pro W IMU connection: {e}")
144        return False
145
146def read_imu_data(frequency=100, timeout=1):
147    """Read IMU data from OAK-D-Pro W device (BMI270 sensor)"""
148    global device, queue, madgwick
149    
150    # Initialize connection if not already done
151    if device is None or queue is None or madgwick is None:
152        if not initialize_imu_connection(frequency):
153            return {
154                "Accelerometer_X": 0,
155                "Accelerometer_Y": 0,
156                "Accelerometer_Z": 0,
157                "RollSpeed": 0,
158                "PitchSpeed": 0,
159                "HeadingSpeed": 0,
160                "Roll": 0,
161                "Pitch": 0,
162                "Heading": 0,
163                "qw": 0,
164                "qx": 0,
165                "qy": 0,
166                "qz": 0,
167            }
168    
169    try:
170        # Get data from the queue
171        inIMU = queue.get()
172        if inIMU is None:
173            return None
174                
175        imuData = inIMU.packets[-1]
176        
177        # Get raw accelerometer and gyroscope data
178        ax = imuData.acceleroMeter.x
179        ay = imuData.acceleroMeter.y
180        az = imuData.acceleroMeter.z
181        
182        gx = imuData.gyroscope.x
183        gy = imuData.gyroscope.y
184        gz = imuData.gyroscope.z
185        
186        # Update Madgwick filter to get quaternion
187        quaternion = madgwick.update(gx, gy, gz, ax, ay, az)
188        qw, qx, qy, qz = quaternion
189        
190        # Calculate Euler angles from quaternion
191        roll, pitch, yaw = quaternion_to_euler(qw, qx, qy, qz)
192        
193        # Apply the same coordinate transformations as the original code
194        result = {
195            "Accelerometer_X": ax,
196            "Accelerometer_Y": ay,
197            "Accelerometer_Z": az,
198            "RollSpeed": gy,  # Swapped as in original
199            "PitchSpeed": gx * -1,  # Inverted as in original
200            "HeadingSpeed": gz,
201            "Roll": pitch,  # Swapped as in original
202            "Pitch": roll * -1,  # Inverted as in original
203            "Heading": yaw,
204            "qw": qw,
205            "qx": qx,
206            "qy": qy,
207            "qz": qz,
208        }
209        return result
210            
211    except Exception as e:
212        print(f"Error reading OAK-D-Pro W IMU data: {e}")
213        # Try to reinitialize the connection
214        if initialize_imu_connection(frequency):
215            return read_imu_data(frequency, timeout)  # Try again
216        return {
217            "Accelerometer_X": 0,
218            "Accelerometer_Y": 0,
219            "Accelerometer_Z": 0,
220            "RollSpeed": 0,
221            "PitchSpeed": 0,
222            "HeadingSpeed": 0,
223            "Roll": 0,
224            "Pitch": 0,
225            "Heading": 0,
226            "qw": 0,
227            "qx": 0,
228            "qy": 0,
229            "qz": 0,
230        }
231
232def cleanup_imu_connection():
233    """Clean up the IMU connection when done"""
234    global device
235    if device is not None:
236        device.close()
237        device = None
238
239if __name__ == "__main__":
240    args = parse_opt()
241    try:
242        initialize_imu_connection(frequency=args.frequency)
243        while True:
244            imu_data = read_imu_data(frequency=args.frequency, timeout=args.timeout)
245            print(f"IMU Data: {imu_data}")
246            time.sleep(0.01)  # Small delay to prevent overwhelming output
247    except KeyboardInterrupt:
248        print("Exiting...")
249    finally:
250        cleanup_imu_connection()

1import argparse
2import sys
3import time
4import json
5import depthai as dai
6import math
7import numpy as np
8
9# Constants
10PI = 3.141592653589793
11DEG_TO_RAD = 0.017453292519943295
12
13def parse_opt(known=False):
14    parser = argparse.ArgumentParser(description='OAK-D-Pro W IMU Data Dumper')
15    parser.add_argument('--output', type=str, default='oak_imu_data.txt', help='output file to dump IMU data')
16    parser.add_argument('--duration', type=int, default=60, help='duration to collect data in seconds; default: 60')
17    parser.add_argument('--frequency', type=int, default=100, help='IMU sampling frequency; default: 100Hz')
18    parser.add_argument('--format', type=str, choices=['json', 'csv'], default='json', help='output format: json or csv')
19    parser.add_argument('--verbose', action='store_true', help='print data to console as well')
20    
21    receive_params = parser.parse_known_args()[0] if known else parser.parse_args()
22    return receive_params
23
24class MadgwickFilter:
25    """Madgwick AHRS filter for quaternion estimation from accelerometer and gyroscope"""
26    def __init__(self, beta=0.1, sample_freq=100):
27        self.beta = beta
28        self.sample_freq = sample_freq
29        self.q = [1.0, 0.0, 0.0, 0.0]  # quaternion [w, x, y, z]
30        
31    def update(self, gx, gy, gz, ax, ay, az):
32        """Update quaternion with gyroscope and accelerometer data"""
33        q1, q2, q3, q4 = self.q
34        
35        # Normalise accelerometer measurement
36        norm = math.sqrt(ax * ax + ay * ay + az * az)
37        if norm == 0:
38            return
39        ax /= norm
40        ay /= norm
41        az /= norm
42        
43        # Auxiliary variables to avoid repeated arithmetic
44        _2q1 = 2 * q1
45        _2q2 = 2 * q2
46        _2q3 = 2 * q3
47        _2q4 = 2 * q4
48        _4q1 = 4 * q1
49        _4q2 = 4 * q2
50        _4q3 = 4 * q3
51        _8q2 = 8 * q2
52        _8q3 = 8 * q3
53        q1q1 = q1 * q1
54        q2q2 = q2 * q2
55        q3q3 = q3 * q3
56        q4q4 = q4 * q4
57        
58        # Gradient decent algorithm corrective step
59        s1 = _4q1 * q3q3 + _2q3 * ax + _4q1 * q2q2 - _2q2 * ay
60        s2 = _4q2 * q4q4 - _2q4 * ax + 4 * q1q1 * q2 - _2q1 * ay - _4q2 + _8q2 * q2q2 + _8q2 * q3q3 + _4q2 * az
61        s3 = 4 * q1q1 * q3 + _2q1 * ax + _4q3 * q4q4 - _2q4 * ay - _4q3 + _8q3 * q2q2 + _8q3 * q3q3 + _4q3 * az
62        s4 = 4 * q2q2 * q4 - _2q2 * ax + 4 * q3q3 * q4 - _2q3 * ay
63        
64        # Normalise step magnitude
65        norm = math.sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4)
66        if norm != 0:
67            s1 /= norm
68            s2 /= norm
69            s3 /= norm
70            s4 /= norm
71        
72        # Apply feedback step
73        qDot1 = 0.5 * (-q2 * gx - q3 * gy - q4 * gz) - self.beta * s1
74        qDot2 = 0.5 * (q1 * gx + q3 * gz - q4 * gy) - self.beta * s2
75        qDot3 = 0.5 * (q1 * gy - q2 * gz + q4 * gx) - self.beta * s3
76        qDot4 = 0.5 * (q1 * gz + q2 * gy - q3 * gx) - self.beta * s4
77        
78        # Integrate rate of change of quaternion
79        q1 += qDot1 * (1.0 / self.sample_freq)
80        q2 += qDot2 * (1.0 / self.sample_freq)
81        q3 += qDot3 * (1.0 / self.sample_freq)
82        q4 += qDot4 * (1.0 / self.sample_freq)
83        
84        # Normalise quaternion
85        norm = math.sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4)
86        if norm != 0:
87            self.q = [q1/norm, q2/norm, q3/norm, q4/norm]
88        
89        return self.q
90
91def quaternion_to_euler(qw, qx, qy, qz):
92    """Convert quaternion to Euler angles (roll, pitch, yaw)"""
93    # Roll (x-axis rotation)
94    sinr_cosp = 2 * (qw * qx + qy * qz)
95    cosr_cosp = 1 - 2 * (qx * qx + qy * qy)
96    roll = math.atan2(sinr_cosp, cosr_cosp)
97    
98    # Pitch (y-axis rotation)
99    sinp = 2 * (qw * qy - qz * qx)
100    if abs(sinp) >= 1:
101        pitch = math.copysign(PI / 2, sinp)  # use 90 degrees if out of range
102    else:
103        pitch = math.asin(sinp)
104    
105    # Yaw (z-axis rotation)
106    siny_cosp = 2 * (qw * qz + qx * qy)
107    cosy_cosp = 1 - 2 * (qy * qy + qz * qz)
108    yaw = math.atan2(siny_cosp, cosy_cosp)
109    
110    return roll, pitch, yaw
111
112def read_oak_imu_data(frequency=100):
113    """Read IMU data from OAK-D-Pro W device (BMI270 sensor)"""
114    try:
115        # Create pipeline
116        pipeline = dai.Pipeline()
117        
118        # Create IMU node - BMI270 only supports ACCELEROMETER_RAW and GYROSCOPE_RAW
119        imu = pipeline.create(dai.node.IMU)
120        imu.enableIMUSensor(dai.IMUSensor.ACCELEROMETER_RAW, frequency)
121        imu.enableIMUSensor(dai.IMUSensor.GYROSCOPE_RAW, frequency)
122        imu.setBatchReportThreshold(1)
123        imu.setMaxBatchReports(10)
124        
125        # Create output
126        imuOut = pipeline.create(dai.node.XLinkOut)
127        imuOut.setStreamName("imu")
128        imu.out.link(imuOut.input)
129        
130        # Initialize Madgwick filter for quaternion estimation
131        madgwick = MadgwickFilter(beta=0.1, sample_freq=frequency)
132        
133        # Connect to device and start pipeline
134        with dai.Device(pipeline) as device:
135            q = device.getOutputQueue(name="imu", maxSize=50, blocking=False)
136            
137            print(f"Connected to OAK-D-Pro W device with BMI270 IMU sensor")
138            
139            while True:
140                inIMU = q.get()
141                if inIMU is None:
142                    continue
143                    
144                imuData = inIMU.packets[-1]
145                
146                # Get raw accelerometer and gyroscope data
147                ax = imuData.acceleroMeter.x
148                ay = imuData.acceleroMeter.y
149                az = imuData.acceleroMeter.z
150                
151                gx = imuData.gyroscope.x
152                gy = imuData.gyroscope.y
153                gz = imuData.gyroscope.z
154                
155                # Update Madgwick filter to get quaternion
156                quaternion = madgwick.update(gx, gy, gz, ax, ay, az)
157                qw, qx, qy, qz = quaternion
158                
159                # Calculate Euler angles from quaternion
160                roll, pitch, yaw = quaternion_to_euler(qw, qx, qy, qz)
161                
162                result = {
163                    "timestamp": time.time(),
164                    "Accelerometer_X": ax,
165                    "Accelerometer_Y": ay,
166                    "Accelerometer_Z": az,
167                    "RollSpeed": gx,
168                    "PitchSpeed": gy,
169                    "HeadingSpeed": gz,
170                    "Roll": roll,
171                    "Pitch": pitch,
172                    "Heading": yaw,
173                    "qw": qw,
174                    "qx": qx,
175                    "qy": qy,
176                    "qz": qz,
177                }
178                
179                yield result
180                
181    except Exception as e:
182        print(f"Error reading OAK-D-Pro W IMU data: {e}")
183        return None
184
185def dump_imu_data():
186    """Main function to dump IMU data"""
187    args = parse_opt()
188    
189    print(f"Starting OAK-D-Pro W IMU data collection...")
190    print(f"Duration: {args.duration} seconds")
191    print(f"Frequency: {args.frequency} Hz")
192    print(f"Output file: {args.output}")
193    print(f"Format: {args.format}")
194    print("Note: Using BMI270 sensor (ACCELEROMETER_RAW + GYROSCOPE_RAW only)")
195    print("Quaternions calculated using Madgwick AHRS filter")
196    print("Press Ctrl+C to stop early\n")
197    
198    start_time = time.time()
199    data_count = 0
200    
201    try:
202        with open(args.output, 'w') as f:
203            # Write CSV header if needed
204            if args.format == 'csv':
205                header = "timestamp,Accelerometer_X,Accelerometer_Y,Accelerometer_Z,RollSpeed,PitchSpeed,HeadingSpeed,Roll,Pitch,Heading,qw,qx,qy,qz\n"
206                f.write(header)
207            
208            for imu_data in read_oak_imu_data(args.frequency):
209                if imu_data is None:
210                    break
211                    
212                # Check if duration exceeded
213                if time.time() - start_time > args.duration:
214                    break
215                
216                # Write data to file
217                if args.format == 'json':
218                    f.write(json.dumps(imu_data) + '\n')
219                elif args.format == 'csv':
220                    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"
221                    f.write(csv_line)
222                
223                f.flush()  # Ensure data is written immediately
224                
225                # Print to console if verbose
226                if args.verbose:
227                    print(f"Sample {data_count + 1}: {json.dumps(imu_data, indent=2)}")
228                
229                data_count += 1
230                
231                # Small delay to prevent overwhelming the system
232                time.sleep(0.001)
233                
234    except KeyboardInterrupt:
235        print("\nData collection stopped by user.")
236    except Exception as e:
237        print(f"Error during data collection: {e}")
238    
239    elapsed_time = time.time() - start_time
240    print(f"\nData collection completed!")
241    print(f"Total samples collected: {data_count}")
242    print(f"Elapsed time: {elapsed_time:.2f} seconds")
243    print(f"Average sampling rate: {data_count/elapsed_time:.2f} Hz")
244    print(f"Data saved to: {args.output}")
245
246if __name__ == "__main__":
247    dump_imu_data()

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