In this tutorial, we’ll pick up where we left off on incrementally writing a simple robot simulator in python using Qt and OpenGL. Last time, we discussed how to install these libraries and walked through a simple interactive cube GUI; check out the previous tutorial or find the script here.

In the previous tutorial of this series, we created a simple GUI containing an OpenGL rendering of a multicolored cube and some sliders for rotating the cube.

This was a good first foray into the basics of writing GUI and graphics code in python; now that we’ve got the basic structure of such an application down, let’s work from there to start writing a simple simulator. We’ll accomplish a few things in this tutorial:

- Define a ground plane for our simulator and render it
- Implement a simple adjustable-view camera
- Create a graphics object defining a vector
- Combine three vectors into an axes graphics object

The multicolored cube was a good place to start in understanding the OpenGL rendering process, but the most basic object we need in a robot simulator is the ground plane. Let’s encapsulate this functionality in a new `GroundGraphics`

class for portability and define the geometry in its constructor:

```
class GroundGraphics(object):
"""
This class defines a grid (triangular mesh) representing the ground plane. The render
function must be called from the main paintGL rendering function.
"""
def __init__(self, length, width):
""" Initialize the ground graphics object.
Initialize the ground graphics object.
Args:
length (float): Length of the ground grid, in meters.
width (float): Width of the ground grid, in meters.
Returns:
(None)
"""
# Store the grid dimensions and compute number of squares and vertices
self.len = length
self.w = width
self.res = 10
self.n_sq = self.res**2
self.n_vert = 6 * self.n_sq
# Define the vertex (x,y,z) values as a grid:
self.vx = np.linspace(-0.5*self.len, 0.5*self.len, self.res + 1)
self.vy = np.linspace(-0.5*self.w, 0.5*self.w, self.res + 1)
self.vz = np.zeros((self.res + 1, self.res + 1))
self.vert = np.zeros((self.n_vert, 3))
# Organize the vertices into triangles for storing in a VBO:
sq_ind = 0
for i in range(self.res):
for j in range(self.res):
# Upper triangle in square:
self.vert[6 * sq_ind,:] = np.array([self.vx[i], self.vy[j], self.vz[i, j]])
self.vert[6 * sq_ind + 1,:] = np.array([self.vx[i+1], self.vy[j+1], self.vz[i+1, j+1]])
self.vert[6 * sq_ind + 2,:] = np.array([self.vx[i], self.vy[j+1], self.vz[i, j+1]])
# Lower triangle in square:
self.vert[6 * sq_ind + 3,:] = np.array([self.vx[i], self.vy[j], self.vz[i, j]])
self.vert[6 * sq_ind + 4,:] = np.array([self.vx[i+1], self.vy[j], self.vz[i+1, j]])
self.vert[6 * sq_ind + 5,:] = np.array([self.vx[i+1], self.vy[j+1], self.vz[i+1, j+1]])
sq_ind += 1
# Pack the triangle vertices into a dedicated VBO:
self.vert_stride = 12 # number of bytes between successive triangles
self.vert_vbo = vbo.VBO(np.reshape(self.vert, (1,-1), order='C').astype(np.float32))
```

Let’s break down the geometry initialization a bit. First, we use the input `length`

and `width`

dimensions to define the number of squares `n_sq`

in the grid, the number of triangle vertices `n_vert`

in the grid (two triangles per square times three vertices per triangle) and arrays of all possible vertex positions in the `x`

, `y`

and `z`

dimensions:

```
# Store the grid dimensions and compute number of squares and vertices
self.len = length
self.w = width
self.res = 10
self.n_sq = self.res**2
self.n_vert = 6 * self.n_sq
# Define the vertex (x,y,z) values as a grid:
self.vx = np.linspace(-0.5*self.len, 0.5*self.len, self.res + 1)
self.vy = np.linspace(-0.5*self.w, 0.5*self.w, self.res + 1)
self.vz = np.zeros((self.res + 1, self.res + 1))
```

Then, we iterate over each square in the grid and store the positions of the six vertices composing the two triangles which define that square:

```
self.vert = np.zeros((self.n_vert, 3))
# Organize the vertices into triangles for storing in a VBO:
sq_ind = 0
for i in range(self.res):
for j in range(self.res):
# Upper triangle in square:
self.vert[6 * sq_ind,:] = np.array([self.vx[i], self.vy[j], self.vz[i, j]])
self.vert[6 * sq_ind + 1,:] = np.array([self.vx[i+1], self.vy[j+1], self.vz[i+1, j+1]])
self.vert[6 * sq_ind + 2,:] = np.array([self.vx[i], self.vy[j+1], self.vz[i, j+1]])
# Lower triangle in square:
self.vert[6 * sq_ind + 3,:] = np.array([self.vx[i], self.vy[j], self.vz[i, j]])
self.vert[6 * sq_ind + 4,:] = np.array([self.vx[i+1], self.vy[j], self.vz[i+1, j]])
self.vert[6 * sq_ind + 5,:] = np.array([self.vx[i+1], self.vy[j+1], self.vz[i+1, j+1]])
sq_ind += 1
```

This last bit is a little obtuse, but the idea is that we pack the triangle vertices into a VBO, which will later be rendered by interpreting the packed data as a collection of triangles with a specified *stride* or byte offset between triangles (12, since each of three verticles is stored by default as a 32bit float in numpy):

```
# Pack the triangle vertices into a dedicated VBO:
self.vert_stride = 12 # number of bytes between successive triangles
self.vert_vbo = vbo.VBO(np.reshape(self.vert, (1,-1), order='C').astype(np.float32))
```

Now that the geometry has been defined in the `GroundObject`

constructor, we need to define how its geometry is rendered in another class member called `render`

. This member funcion should be called from the `paintGL`

function we overrode last time to render the cube:

```
def render(self):
""" Renders the ground plane graphics object.
Render the ground plane graphics using the geometry defined in the constructor.
This function must be called from the main paintGL rendering function.
Args:
(None)
Returns:
(None)
"""
gl.glPushMatrix()
try:
# Bind the vertex data buffer to the VBO all future rendering
# (or until unbound with 'unbind'):
self.vert_vbo.bind()
# Set the vertex pointer for rendering:
gl.glEnableClientState(gl.GL_VERTEX_ARRAY)
gl.glVertexPointer(3, gl.GL_FLOAT, self.vert_stride, self.vert_vbo)
# Set the polygons to have front and back faces and to not be filled:
gl.glColor3f(1.0, 1.0, 1.0)
gl.glPolygonMode(gl.GL_FRONT_AND_BACK, gl.GL_LINE)
# Render triangle edges using the loaded vertex pointer data:
gl.glDrawArrays(gl.GL_TRIANGLES, 0, self.n_vert)
# Set the polygons to have front and back faces and to not be filled:
gl.glColor3f(0.5, 0.5, 0.5)
gl.glPolygonMode(gl.GL_FRONT_AND_BACK, gl.GL_FILL)
# Render triangle faces using the loaded vertex pointer data:
gl.glDrawArrays(gl.GL_TRIANGLES, 0, self.n_vert)
except Exception as e:
print(e)
finally:
self.vert_vbo.unbind()
gl.glDisableClientState(gl.GL_VERTEX_ARRAY)
gl.glPopMatrix()
```

The general rendering process should look similar to rendering the cube - it’s sandwiched by calls to `glPushMatrix`

and `glPopMatrix`

and has calls to `glVertexPointer(size, type, stride, pointer)`

for loading in the VBO, but instead of `glDrawElements`

we now use `glDrawArrays`

. This lets us easily render geometric primitives - here, triangles as specified by passing in `GL_TRIANGLES`

- from the VBO.

We actually render the triangles twice here - once for the black edges, and then once for the grey faces. The `glPolygonMode`

function allows us to specify whether to render the triangle with only edges (`GL_LINE`

) or faces (`GL_FILL`

). After rendering, we make sure to unbind the VBO since we’re done with it.

To use the ground plane we defined, let’s add an instance as a class member in our trusty `GLWidget`

class’ `initializeGL`

member function, which should now look as follows:

```
def initializeGL(self):
""" Initializes OpenGL functionality and geometry.
Virtual function provided by QGLWidget, called once at the beginning of application.
OpenGL and geometry initialization is performed here.
Args:
(None)
Returns:
(None)
"""
# Convenience function, calls glClearColor under the hood.
# QColor is specified as RGB ints (0-255). Specify this clear
# color once and call glClear(GL_COLOR_BUFFER_BIT) before each
# round of rendering (in paintGL):
self.qglClearColor(QtGui.QColor(100, 100, 100)) # a grey background
# Enable the depth buffer:
gl.glEnable(gl.GL_DEPTH_TEST)
# Initialize the user-specified geometry
self.initGeometry()
# Initialize the ground plane graphics geometry
self.ground_graphics = GroundGraphics(length=10.0, width=10.0)
```

Defining the ground plane is as simple as creating the object with desired dimensions! Also note that we can safely remove all code which previously defined the cube from `initGeometry()`

, along with the sliders for rotating it. I won’t go through every change, but you can always check the `git diff`

to see what I mean.

To ensure the ground plane is rendered, make sure your `paintGL`

function looks as follows:

```
def paintGL(self):
""" Defines behavior of OpenGL window when resized.
Virtual function provided by QGLWidget, called from QGLWidget method updateGL.
All user rendering code should be defined here.
Args:
(None)
Returns:
(None)
"""
# Start from a blank slate each render by clearing buffers
gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT)
self.ground_graphics.render()
```

That’s it! The ground plane will now be defined at the start and rendered every cycle.

Before we can visualize our ground plane, it will help tremendously to make our camera (view) adjustable via user input.

The main function we need to add to our class is one which updates the camera view based on the current camera state, which we’ll make adjustable in a bit. The `update_view`

function accomplishes this as follows:

```
def update_view(self):
""" Updates the camera view using current camera state.
Function to be called after updating any camera state variable in order to update
the camera view. Converts spherical camera coordinates to a Cartesian position for
the eye of the camera, with the center position (focal point) fixed at the origin.
Args:
(None)
Returns:
(None)
"""
self.eye_pos = np.array([self.eye_r*np.sin(self.eye_phi)*np.cos(self.eye_th),
self.eye_r*np.sin(self.eye_phi)*np.sin(self.eye_th),
self.eye_r*np.cos(self.eye_phi)])
up_vec = np.array([0.0, 0.0, 1.0])
gl.glLoadIdentity()
GLU.gluLookAt(*np.concatenate((self.eye_pos, self.center_pos, up_vec)))
```

In OpenGL, the camera view is set with `gluLookAt(eyeX,eyeY,eyeZ,centerX,centerY,centerZ,upX,upY,upZ)`

where the eye is the camera position, the center is the focal point and the up vector fixes the camera orientation.

We choose to adjust the camera in a *spherical* coordinate frame with the origin as the fixed center (focal) point because it’s easy and feels natural, though there are certainly other ways to define an adjustable camera in 3D space. The spherical camera coordinates are radius (distance from origin/center) , azimuth angle and elevation angle as shown below.

We thus convert the camera’s current spherical coordinates to Cartesian coordinates using the formula

which is implemented as

```
self.eye_pos = np.array([self.eye_r*np.sin(self.eye_phi)*np.cos(self.eye_th),
self.eye_r*np.sin(self.eye_phi)*np.sin(self.eye_th),
self.eye_r*np.cos(self.eye_phi)])
```

We then simply pass the eye position, fixed center position and “up” vector `up_vec`

(which just points in +z) to `gluLookAt`

with a concatenation to join the numpy arrays and an asterisk to unpack them to the required individual arguments:

```
up_vec = np.array([0.0, 0.0, 1.0])
gl.glLoadIdentity()
GLU.gluLookAt(*np.concatenate((self.eye_pos, self.center_pos, up_vec)))
```

Now, we need to tie user input to the camera state (spherical coordinates). This is as simple as overriding the `keyPressEvent(event)`

function provided by the Qt base class in our main class!

```
def keyPressEvent(self, event):
""" Defines callbacks for keypress events.
Implement override for virtual function provided by Qt base class for defining
keypress event callbacks, for example manipulating the primary view camera.
Args:
event (QKeyEvent): Screen width in pixels.
height (int): Screen height in pixels.
Returns:
(None)
"""
if type(event) == QtGui.QKeyEvent:
if event.key() == QtCore.Qt.Key_W:
# Hold W to decrease range (zoom in)
self.eye_r -= 0.5
self.update_view()
elif event.key() == QtCore.Qt.Key_S:
# Hold S to increase range (zoom out)
self.eye_r += 0.5
self.update_view()
elif event.key() == QtCore.Qt.Key_Down:
# Hold DOWNARROW to increase elevation angle
self.eye_phi += 0.05
self.update_view()
elif event.key() == QtCore.Qt.Key_Up:
# Hold UPARROW to decrease elevation angle
self.eye_phi -= 0.05
self.update_view()
elif event.key() == QtCore.Qt.Key_Right:
# Hold RIGHTARROW to increase azimuth angle
self.eye_th += 0.05
self.update_view()
elif event.key() == QtCore.Qt.Key_Left:
# Hold LEFTARROW to decrease azimuth angle
self.eye_th -= 0.05
self.update_view()
```

The above is fairly self-explanatory - on a keypress, the overriden `keyPressEvent`

class captures a `QKeyEvent`

variable `event`

which contains information about the press. We check which key was pressed and handle it accordingly, using W/S to zoom in/out, left/right arrows to rotate in azimuth, and up/down arrows to rotate in elevation. Each time the camera state is changed, we call `update_view`

to move the camera accordingly.

One final required addition is to set the initial camera state, since we adjust it incrementally in the keypress callbacks. This should be added to `initializeGL`

:

```
# Initialize the camera state and set the initial view
self.eye_r = 20.0 # camera range, in meters
self.eye_th = 1.0 # camera azimuth angle, in radians
self.eye_phi = 1.0 # camera elevation angle, in radians
self.center_pos = np.array([0.0, 0.0, 0.0])
self.update_view()
# Set focus to the window
self.setFocusPolicy(QtCore.Qt.StrongFocus)
```

We also set the focus to the window to ensure our keypresses take effect. That’s it! Run the script and use the keys to manipulate the camera and view the ground plane as below.

Next, let’s add a simple graphics object for an oriented vector (3D arrow) which we’ll build on to create an axes (coordinate frame) graphics object.

As we did for the ground plane, let’s encapsulate our vector in a class for portability. The full class is show below and broken up step-by-step:

```
class VectorGraphics(object):
"""
Class for rendering a three-dimensional vector.
"""
def __init__(self):
self.quadric = GLU.gluNewQuadric()
GLU.gluQuadricNormals(self.quadric, gl.GLU.GLU_SMOOTH) #Create Smooth Normals
GLU.gluQuadricTexture(self.quadric, gl.GL_TRUE) #Create Texture Coords
```

The vector object geometry is much simpler than the ground object because it’s just two primitives - a cylindrical shaft and a conical head. In fact, both are implemented as a cylinder in OpenGL - and, in turn, a cylinder is one member of the class of *quadric* shapes (those that can be defined by a quadratic function).

We thus create a quadric with `gluNewQuadric()`

in the arrow object’s constructor and pass it to the rendering functions below.

```
def render(self, start, dir, length, width, color):
if length > 0.0:
# Compute the angle-axis rotation require to orient the vector along dir:
up_vec = np.array([0.0, 0.0, 1.0])
axis = np.cross(up_vec, dir)
trip_prod = np.linalg.det(np.dstack((up_vec, dir, axis)))
if trip_prod > 0:
angle = np.arccos(np.dot(up_vec, dir))
else:
angle = 2*np.pi - np.arccos(np.dot(up_vec, dir))
```

We specify the vector by passing in a start position `start`

and unit direction vector `dir`

, as well as length, width (diameter) and color values. In rendering the vector, we actually need to rotate it from its default orientation so it lies along the specified direction - we thus compute the required axis-angle rotation for this step.

```
# Draw the shaft using a cylinder:
gl.glPushMatrix()
gl.glColor3f(*color)
gl.glTranslatef(*start)
gl.glRotate((180.0/np.pi)*angle, *axis)
GLU.gluCylinder(self.quadric, width, width, length, 100, 10)
gl.glPopMatrix()
# Draw the head using a cylinder having zero width on top:
gl.glPushMatrix()
gl.glColor3f(*color)
gl.glTranslatef(*start)
gl.glRotate((180.0/np.pi)*angle, *axis)
gl.glTranslatef(0.0, 0.0, length)
GLU.gluCylinder(self.quadric, 2.0*width, 0.0, 0.1*length, 100, 10)
gl.glPopMatrix()
```

After rotating the cylinders accordingly, we use `gluCylinder(quadric, base, top, height, slices, stacks)`

in the `render`

function to render it; here, `slices`

and `stacks`

refer to the rendering discretization along the lengthwise and diametric axes, respectively.

Note that we create the head by simply passing in a value for `base`

wider than the shaft, zero for `top`

so that it comes to a point, and some fraction of the shaft `length`

for the head length.

Now that we have an arrow object, we can easily render three arrows in orthogonal directions to create an axes object:

```
class AxesGraphics(object):
"""
Class for rendering an axes (frame vectors) object.
"""
def __init__(self):
self.x_axis = VectorGraphics()
self.y_axis = VectorGraphics()
self.z_axis = VectorGraphics()
def render(self, R, t):
self.x_axis.render(t, R[:,0],
0.3, 0.01, np.array([1.0, 0.0, 0.0]))
self.y_axis.render(t, R[:,1],
0.3, 0.01, np.array([0.0, 1.0, 0.0]))
self.z_axis.render(t, R[:,2],
0.3, 0.01, np.array([0.0, 0.0, 1.0]))
```

We create three vector objects in the constructor and render them using the specified frame pose, which is define by an orientation matrix `R`

and a translation vector `t`

. For now, we leave the arrow dimensions set to hardcoded values.

Let’s render a frame! It’s always useful to visualize the global coordinate frame at its origin, so let’s add to `initializeGL`

:

```
# Initialize the origin axes graphics geometry
self.origin_axes_graphics = AxesGraphics()
```

and then render it in `paintGL`

as follows:

```
self.origin_axes_graphics.render(np.identity(3), np.zeros(3))
```

Run the simulator script should now produce the ground plane with axes graphics visualizing the global coordinate frame.

In this tutorial, we took the simple “hello world” OpenGL application from last time and built it out into a still simple but solid base for a robot simulator. With a functional camera and basic graphics objects in place, we’ll next do some code refactoring before adding robot kinemtics, state and visualiztion.

Written on January 17th, 2020 by Nick RotellaFeel free to share!