thecountoftuscany / pyteapot-quaternion-euler-cube-rotation

Visualization of IMU orientation from quaternion or Euler angles with a rotating cube

Introduction

Visualization of orientation of any IMU with the help of a rotating cube as per quaternions or Euler angles (strictly speaking, the Tait Bryan Angles received over either the serial port or WiFi using OpenGL in Python. The MPU-9250 (has on-board accelerometer, magnetometer and gyroscope) has been used with Arduino for the demo below:

This is an easy to understand Python implementation of the often-used ‘MPU Teapot’ processing code (an example here) for visualizing an IMU, but not specific to any particular IMU (like the MPU6060 in the case of ‘MPU Teapot’). Note that this project does not do the sensor fusion of each sensor in the IMU to obtain the orientation (for the demo in the above gif, a Madgwick or Mahony filter was used from here to obtain the orientation). It is assumed that the user already has obtained the orientation in the form of either the yaw-pitch-roll angles or quaternions and wishes to visualize this real-time on-screen for debugging etc.

Requirements:

• pygame and PyOpenGL are used for graphics
• PySerial is used to get data from serial port
• simple python built-in UDP sockets are used to get data from WiFi.

Usage

Most of the code is self-explanatory. However some modifications might be required as per different use cases:

• Set useSerial to True if receiving data over serial and False if receiving data over wifi.
• Set serial port by changing the variable ser, if using serial for data transmission.
• Set udp port by changing the variable UDP_PORT, if using wifi for data transmission.
• Set useQuat to True if receiving quaternions over serial or WiFi and False if receiving Euler angles.
• If receiving quaternions over serial or wifi, the declination at the particular location should to be updated in quat_to_ypr(q) function to get correct yaw angles printed on screen. (The cube rotation is not dependent on this and will still work fine otherwise)

String passed over Serial or Wifi

To use this module, the data received over serial or udp port should be in the format specified below:

• First quaternion value should be between two w s
• Second quaternion value should be between two a s
• Third quaternion value should be between two b s
• Fourth quaternion value should be between two c s
• Yaw angle should be betweem two y s
• Pitch angle should be between two p s
• Roll angles should be between two r s

Either quaternion or Euler angles or even both can be received over the serial or udp port. If both are received, the useQuat variable defines which one is used to rotate the cube. So for example, all of the following are valid formats for the data received over the serial or udp port:

# Both quaternions and Euler angles
w0.09wa-0.12ab-0.09bc0.98cy168.8099yp12.7914pr-11.8401r

# Quaternions only
w0.09wa-0.12ab-0.09bc0.98c

# Euler angles only
y168.8099yp12.7914pr-11.8401r

Each of these must be on separate lines (or should have a ‘\n’ at the end of the string). Other data may also be passed over Serial or Wifi, provided that none of the characters w, a, b, c, y, p, r are passed (for example, somethingw0.09wa-0.12ab-0.09bc0.98cy168.8099yp12.7914pr-11.8401rsomethingelse is valid but somedataw0.09wa-0.12ab-0.09bc0.98cy168.8099yp12.7914pr-11.8401ranotherstring is not since it has the characters a and r)

Todos

• [x] Receive data over WiFi instead of serial. - **Done!**
• [ ] Add a nice argparse interface instead of requiring the user to change variables in the script. Include example usage in docstring (a la tldr or bro)
• [ ] Add some keyboard support, eg. pausing / resuming the visualization with spacebar etc
• [ ] Add optional support for x, y, z too. Also, multiple simultaneous viewports (eg. to compare with ground truth from MoCap)
• [ ] Read from text file instead of serial or UDP
• [ ] Other data collection methods: bluetooth?
• [ ] Write tests, docstrings etc

pyteapot-quaternion-euler-cube-rotation's Issues

OpenGL.error.NullFunctionError: Attempt to call an undefined function gluPerspective

I am running on Linux (Ubuntu). The window shows for a brief moment before it closes and this error appears:

pygame 1.9.4
Hello from the pygame community. https://www.pygame.org/contribute.html
Traceback (most recent call last):
File "pyteapot.py", line 188, in
main()
File "pyteapot.py", line 21, in main
resizewin(640, 480)
File "pyteapot.py", line 52, in resizewin
gluPerspective(45, 1.0*width/height, 0.1, 100.0)
File "/usr/local/lib/python2.7/dist-packages/OpenGL/platform/baseplatform.py", line 407, in call
self.name, self.name,
OpenGL.error.NullFunctionError: Attempt to call an undefined function gluPerspective, check for bool(gluPerspective) before calling

Which order to pass quaternions?

Hey!

I have a dataset of IMU recordings with video and I want to verify that the quaternion outputs match the orientation I see on the video itself but I see no correlation between the video and the visualization for some reason.

I am trying to see if the sensor is outputting the wrong quaternion information (could be, using pretty low MAG, GYRO and ACCEL sample rate) but to be sure I need to know that the visualization is not wrong.

I am passing QuatW QuatX QuatY and QuatZ. Is this the correct order?

Thanks

MPU9250 sensor fusion

Where can I get find the code that generates the quaternion for the MPU9250 raw data ? Your sensor performance looks really good.

How to rotate object 180 degrees?

Currently the perspective of the displayed object is flipped 180 degrees and I don't see any support for modifying the direction. Do you know how to implement this? I've tried flipping certain signs withitn glRotatef, but haven't gotten it to change correctly.

Application: Black screen, not-responding

Hi,

I tried to run the application on my Windows, but the window is black and is not responding. I believe it is same issue as mentioned in #5

Below you can find the screenshots describing the issue and my prerequisites installation:

I guess I have some dependency missing, but I don't get any kind of error. If someone has done Windows installation could you please guide what to do regarding other pre-requisites?

Do graceful error handling for socket communication

From issue #11, it seems like the program just hangs if the sockets are not configured properly (issue was reproduced on my Windows computer). The program should raise an error and exit instead of getting stuck. This works fine for misconfigured serial port.

No argparse interface for controlling the program

Raising issue for the TODO
Add a nice [[https://docs.python.org/3/library/argparse.html][ argparse ]] interface instead of requiring the user to change variables in the script. Include example usage in docstring (a la [[https://tldr.sh/][tldr]] or [[http://bropages.org][bro]])

Raising PR with implementation
#7

Euler angles Range

Hi,
Thanks for the nice repo. I would like to ask about the Euler angle ranges obtained from MPU9250. Do you get the orientation values between -180 to +180 degrees on each Euler axes (yaw, roll, pitch)?

[TODO] Read data from file for visualisation

Raising issue as per TODO.

Would like to submit PR for the same

unable to initialize

besides, configuring serial quaternion mode, does anything else need to be done to initialize the application?
I have also installed PyOpenGL and pygame. When i run the app a white window comes up, it never gets to draw the real application

Quaternion calculation

is the calculation correct?
On your code:
yaw = math.atan2(2.0 * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3])
pitch = -math.sin(2.0 * (q[1] * q[3] - q[0] * q[2]))
roll = math.atan2(2.0 * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3])
And on wikipedia for example:
yaw = atan2(2 * (q0 * q3 + q1 * q2), 1 - 2 * (q2 * q2 + q3 * q3));
pitch = asin( 2 * (q0 * q2 - q3 * q1));
roll = atan2(2 * (q0 * q1 + q2 * q3),1 - 2 * (q1 * q1 + q2 * q2));