Set orientation of object in python using quaternions with pyglet, pywavefront and OpenGL

Question

I want to set the orientation of an object in python using quaternions. I get my quaternions periodically via a serial port (this part works). My goal is to create a program similar to the following javascript project : https://github.com/ZaneL/quaternion_sensor_3d_nodejs (but with this object and in python)

Right now I can rotate the object using the keyboard with the following code (notice the rotation is around a non-zero point):

@window.event
def on_key_press(symbol, modifiers):

    glTranslated(0, 0, 200)   

    if symbol == key.Q:
        glRotated(22,0,1,0)
    if symbol == key.W:
        glRotated(-22,0,1,0)

    glTranslated(0, 0, -200)

But this rotation is relative and I want to set the absolute orientation (with respect to some initial orientation). And I need to use quaternions, since quaternions specify the desired orientation.

So I want to do something like this:

@window.event
def on_key_press(symbol, modifiers):

    if symbol == key.Q:
        q = np.array([1,0,0,0])
    if symbol == key.W:
        q = np.array([0,1,0,0])
    #set orientation based on q

Here is my complete code:

import pyglet
import pywavefront
from pywavefront import visualization
from pyglet.gl import *
from pyglet.window import key
from OpenGL.GL import *
from OpenGL.GLUT import *
from OpenGL.GLU import *

path = '../models/10475_Rocket_Ship_v1_L3.obj'

window = pyglet.window.Window(resizable=True)
window.projection = pyglet.window.Projection3D(zfar=1000)
scene = pywavefront.Wavefront(path)

@window.event
def on_draw():
    # print('draw')
    window.clear()
    visualization.draw(scene)

@window.event
def on_key_press(symbol, modifiers):

    glTranslated(0, 0, 200)   

    if symbol == key.Q:
        glRotated(22,0,1,0)
    if symbol == key.W:
        glRotated(-22,0,1,0)

    glTranslated(0, 0, -200)


if __name__ == "__main__":
    glViewport(0, 0, 500,500)
    glutInitDisplayMode(GLUT_RGBA | GLUT_DOUBLE | GLUT_ALPHA | GLUT_DEPTH)
    glMatrixMode(GL_PROJECTION)
    glLoadIdentity()
    glOrtho(0.0, 500, 0.0, 500, 0.0, 1.0)

    glMatrixMode (GL_MODELVIEW)
    glLoadIdentity()

    glTranslated(0, 0, 100)


    for _ in range(4):
        glRotated(25,0,1,0)
        glTranslated(35, 0, 0)

    glRotated(100,0,1,0)
    glTranslated(0, 0, 200)
    glRotated(-100,1,0,0)
    glTranslated(-100, -275, -250)  

    glScale(0.75, 0.75, 0.75)

    glClearColor(0.85, 0.85, 0.85, 1);


    pyglet.app.run()

I also want to simplify the code for setting the initial orientation and position of the object (as this one was found using trial and error). Preferably 1 or 2 operations. I don't need to use piglet or pywavefront. As long as the object renders properly.

Edit: I currently have the rotation working. But I want to rotate the entire model around the z-axis to compensate for a non-zero initial jaw angle(my monitor is not perfectly magnetic North of my object). My 6-axis sensor is upside down so the model is also upside down.

from squaternion import Quaternion
import numpy as np
...
q = Quaternion(q['quat_w'],q['quat_x'],q['quat_y'],q['quat_z'])

# delete current matrix and replace with copy of initialized matrix:
glPopMatrix()
glPushMatrix()

e = q.to_euler(degrees=True)

# get initial yaw angle:
global init_yaw
if init_yaw == None:
    init_yaw = e[2]
    print(q)
    print(init_yaw)

glTranslated(0, 0, 200)   

# glRotated(init_yaw,0,0,1) #this doesn't work, it rotates using euler angles and it needs to rotate around the z axis

r = np.array(q.to_rot())
r4x4 = np.array([[r[0,0],r[1,0],r[2,0],0],
                [r[0,1],r[1,1],r[2,1],0],
                [r[0,2],r[1,2],r[2,2],0],
                [0,0,0,1]]) 

glMultMatrixd(r4x4)

glTranslated(0, 0, -200)

Answer 1

I was able to solve it.

I use the squaternion library to store the quaternion. It comes with build-in methods for quaternion multiplication and converting to rotation matrix. The rotation matrix is 3x3 and needs to be converted to a 4x4 rotation-translation matrix and converted to Column-major order.

First the quaternion needs to be rotated to compensate for the offset in the Yaw angle. Then the quaternion needs to be rotated around the x axis to compensate for the fact the sensor is mounted upside down. Then the 4x4 matrix is calculated. Then then matrix is applied (after translating and before translating back)

In order to simplify the initial rotation and translating operations (which were found using trial and error) all I had to do was print and inspect the matrix. After rounding relatively small values (<0.01) to zero I was able to find a simpler matrix. I used this code for inspection:

a = (GLdouble * 16)()
mvm = glGetDoublev(GL_MODELVIEW_MATRIX, a)
print(list(a)) 
array = np.array(list(a)).reshape([4,4])
print(array)

Here is my final code:

path = '../models/10475_Rocket_Ship_v1_L3.obj'

import pyglet
import pywavefront
from pywavefront import visualization
from pyglet.gl import *
from pyglet.window import key
from OpenGL.GL import *
from OpenGL.GLUT import *
from OpenGL.GLU import *

import serial
import json

from squaternion import Quaternion
import numpy as np

ser = serial.Serial('COM4',115200,timeout=0)

window = pyglet.window.Window(resizable=True)
window.projection = pyglet.window.Projection3D(zfar=1000)
scene = pywavefront.Wavefront(path)

buffer = ''

init_yaw = None

def timer(self):

    global buffer
    
    len = ser.in_waiting
    if len > 0:
        string = ser.read(len).decode("utf-8")
        buffer_old = str(buffer)
        buffer += string
        last = buffer.rfind('\n')
        if last >= 0:
            second_last = buffer[0:last].rfind('\n')
            if second_last >= 0:
                # extract last full line (starts and ends with newline):
                last_line = buffer[second_last+1:last] 
                
                try:
                    q = json.loads(last_line)
                    q = Quaternion(q['quat_w'],q['quat_x'],q['quat_y'],q['quat_z'])
                    buffer = buffer[last:] # delete everything before last newline
                except:
                    print('invalid input')
                    print('buffer:',buffer)
                    buffer = ''
                    return               
                
                # delete current matrix and replace with initial matrix:
                glPopMatrix()
                glPushMatrix()

                e = q.to_euler(degrees=True)

                global init_yaw
                if init_yaw == None:
                    init_yaw = e[2]
                    print(q)
                    print(init_yaw)

                glTranslated(0, 0, 200)   

                q_yaw = Quaternion.from_euler(0,0,180-init_yaw,degrees=True)

                q = q_yaw*q

                #flip model around x axis, because sensor is upside down:
                q_flip = Quaternion.from_angle_axis(180, [1,0,0],degrees=True)
                q = q*q_flip

                r = np.array(q.to_rot())

                r4x4 = np.array([[r[0,0],r[1,0],r[2,0],0],
                                 [r[0,1],r[1,1],r[2,1],0],
                                 [r[0,2],r[1,2],r[2,2],0],
                                 [0,0,0,1]])
                
                glMultMatrixd(r4x4)

                glTranslated(0, 0, -200)


@window.event
def on_draw():
    window.clear()
    visualization.draw(scene)

if __name__ == "__main__":
    glViewport(0, 0, 500,500)
    glutInitDisplayMode(GLUT_RGBA | GLUT_DOUBLE | GLUT_ALPHA | GLUT_DEPTH)
    glMatrixMode(GL_PROJECTION)
    glLoadIdentity()
    glOrtho(0.0, 500, 0.0, 500, 0.0, 1.0)

    glMatrixMode (GL_MODELVIEW)
    glLoadIdentity()

    glClearColor(0.85, 0.85, 0.85, 1);

    array = np.array([[0,0,1,0],
                      [1,0,0,0],
                      [0,1,0,0],
                      [0,-150,-600,1]])

    glLoadMatrixd(array)

    glPushMatrix();
 
    pyglet.clock.schedule_interval(timer, 1/60);

    pyglet.app.run()