James Gregson's Website

Jan 12, 2019

PyQt5 + PyOpenGL Example

It took a little bit for me to find working example code for PyOpenGL+PyQt5. The following exposes a GLUT-like interface where you can provide callback functions for basic window, mouse and keyboard events.

# simple_viewer.py

from PyQt5 import QtWidgets
from PyQt5 import QtCore
from PyQt5 import QtGui
from PyQt5 import QtOpenGL

class SimpleViewer(QtOpenGL.QGLWidget):

    initialize_cb    = QtCore.pyqtSignal()
    resize_cb        = QtCore.pyqtSignal(int,int)
    idle_cb          = QtCore.pyqtSignal()
    render_cb        = QtCore.pyqtSignal()

    mouse_move_cb    = QtCore.pyqtSignal( QtGui.QMouseEvent )
    mouse_press_cb   = QtCore.pyqtSignal( QtGui.QMouseEvent )
    mouse_release_cb = QtCore.pyqtSignal( QtGui.QMouseEvent )
    mouse_wheel_cb   = QtCore.pyqtSignal( QtGui.QWheelEvent )

    key_press_cb     = QtCore.pyqtSignal( QtGui.QKeyEvent )
    key_release_cb   = QtCore.pyqtSignal( QtGui.QKeyEvent )

    def __init__(self, parent=None):
        self.parent = parent
        QtOpenGL.QGLWidget.__init__(self, parent)
        self.setMouseTracking(True)

    def mouseMoveEvent( self, evt ):
        self.mouse_move_cb.emit( evt )

    def mousePressEvent( self, evt ):
        self.mouse_press_cb.emit( evt )

    def mouseReleaseEvent( self, evt ):
        self.mouse_release_cb.emit( evt )

    def keyPressEvent( self, evt ):
        self.key_press_cb.emit(evt)

    def keyReleaseEvent( self, evt ):
        self.key_release_cb.emit(evt)

    def initializeGL(self):
        self.initialize_cb.emit()

    def resizeGL(self, width, height):
        if height == 0: height = 1
        self.resize_cb.emit(width,height)

    def paintGL(self):
        self.render_cb.emit()

And here is a basic example using it:

"""About the simplest PyQt OpenGL example with decent interaction"""
import sys

from OpenGL.GL import *
from OpenGL.GLU import *

from simple_viewer import SimpleViewer

width = 800
height = 600
aspect = width/height

def resize( w, h ):
    width = w
    height = h
    aspect = w/h
    glViewport( 0, 0, width, height )

def initialize():
    glEnable(GL_DEPTH_TEST)
    glClearColor( 0.7, 0.7, 1.0, 0.0 )

def render():
    glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT )

    glMatrixMode( GL_PROJECTION )
    glLoadIdentity()
    gluPerspective( 45.0, aspect, 0.1, 10.0 )

    glMatrixMode( GL_MODELVIEW )
    glLoadIdentity()
    gluLookAt( 0.0, 2.0, 5.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0 )

    glPointSize(5.0)
    glLineWidth(5.0)
    glBegin(GL_LINES)
    glColor3f(  1.0, 0.0, 0.0 )
    glVertex3f( 1.0, 0.0, 0.0 )
    glVertex3f( 0.0, 0.0, 0.0 )
    glColor3f(  0.0, 1.0, 0.0 )
    glVertex3f( 0.0, 1.0, 0.0 )
    glVertex3f( 0.0, 0.0, 0.0 )
    glColor3f(  0.0, 0.0, 1.0 )
    glVertex3f( 0.0, 0.0, 1.0 )
    glVertex3f( 0.0, 0.0, 0.0 )
    glEnd()

def mouse_move( evt ):
    print('Mouse move {}: [{},{}]'.format(evt.button(),evt.x(),evt.y()) )

def mouse_press( evt ):
    print('Mouse press {}: [{},{}]'.format(evt.button(),evt.x(),evt.y()) )

def mouse_release( evt ):
    print('Mouse release {}: [{},{}]'.format(evt.button(),evt.x(),evt.y()) )

def key_press( evt ):
    print('Key press {}'.format(evt.key()) )

def key_release( evt ):
    print('Key release {}'.format(evt.key()) )

# create the QApplication
app = SimpleViewer.application()

# set up the display
viewer = SimpleViewer()
viewer.resize_cb.connect( resize )
viewer.initialize_cb.connect( initialize )
viewer.render_cb.connect( render )

# keyboard & mouse interactions
viewer.key_press_cb.connect( key_press )
viewer.key_release_cb.connect( key_release )
viewer.mouse_press_cb.connect( mouse_press )
viewer.mouse_release_cb.connect( mouse_release )
viewer.mouse_move_cb.connect( mouse_move )

# resize the window
viewer.resize( width, height )
viewer.show()

# main loop
try:
    sys.exit(app.exec_())
except SystemExit:
    pass

Nov 06, 2018

Inverse Kinematics

I've been looking into inverse kinematics a bit lately. Inverse kinematics is the problem of solving for joint parameters given a linkage and end-effector position. There are a number of algorithms for doing this but I'm most interested in cases where there may be cycles and (slightly) incompatible joint configurations.

Two link IK chain

Given two links A and B with initial reference frames \(F_A\) and \(F_B\) connected by a joint J with initial reference frame \(F_J\), the transformations of each link reference frame to the joint reference frame are:

\begin{align*} T_{AJ} = F_J^{-1} F_A \\ T_{BJ} = F_J^{-1} F_B \end{align*}

These transforms are fixed for all time using the reference frames for the links and joint during setup. Using these transforms allows the links to be brought into a common (but unknown) joint frame:

\begin{align*} \hat{T}_A = T_{AJ} T_A \\ \hat{T}_B = T_{BJ} T_B \end{align*}

Once in the joint frame, the joint constraint functions can be defined. In the case of a ball and socket joint this is:

\begin{equation*} \hat{T}_A = R \hat{T}_B \end{equation*}

This allows the joint transformation to be found:

\begin{equation*} R = \hat{T}_A \hat{T}_B^{-1} \end{equation*}

Once the transformation is found, its parameters can be extracted and/or constrained. In the case of the ball and socket joint, this requires setting the translational components to zero, producing a constrained configuration \(R_*\). With \(R_*\) defined, it's then possible to express the constraint in the global frame for points \(P_A\) & \(P_B\) defined in the coordinates of A and B:

\begin{equation*} T_{AJ} T_A p_A = R_* T_{BJ} T_B P_b \end{equation*}

This equation forms the basis of constraints for the joint. The constraints can be enforced by ensuring this equation holds for three points that are not collinear. However the points are expressed in the frames of A and B) which is inconvenient. Instead each point can be expressed in the frame of \(F_J\) initially and transformed by \(T_{AJ}^{-1}\) and \(T_{BJ}^{-1}\) respectively:

\begin{align*} p_A = T_{AJ}^{-1} p_j \\ P_B = T_{BJ}^{-1} p_j \\ T_{AJ} T_A p_A = R_* T_{BJ} T_B p_B \end{align*}

This allows basis vectors in the frame of the joint to be used to specify the correspondences for the constraints.

To actually solve this in practice, the constraint equation can be linearized. If \(\alpha\) and \(\delta \alpha\) are the transformation parameters and incremental change of A, then \(T_A\) can be approximated with a first order Taylor series:

\begin{equation*} T_{\alpha+\delta\alpha} p_A \approx T_\alpha p_A + J_{\alpha, p_A} \delta \alpha \end{equation*}

where \(J_{\alpha,p_A}\) is the Jacobian of the transform of \(p_A\) with respect to link parameters \(\alpha\). This lets the constraint be written as:

\begin{equation*} T_{AJ} \left( T_\alpha p_A + J_{\alpha,p_A} \delta \alpha \right) = R_* T_{BJ} \left( T_\beta p_B + J_{\beta,p_B} \delta \beta \right) \end{equation*}

which can be simplified to:

\begin{equation*} T_{AJ} J_{\alpha,p_A} \delta \alpha - R_* T_{BJ} J_{\beta,p_B} \delta \beta = R_* T_{BJ} T_\beta p_B - T_{AJ} T_\alpha p_A \end{equation*}