PyODE Tutorial 3

By Matthias Baas. Thanks to Pierre Gay for removing the dependencies on pygame and cgkit.

In this tutorial we'll finally add collision detection which makes a simulation much more interesting. In general, this involves two parts: collision detection and collision response. First, you have to detect that two objects collide and then you have to take care that they won't interpenetrate. In ODE these two parts are decoupled. There are separate objects and functions to detect collisions, and the actual simulation code deals with the collision response. The interface between these two parts is a special type of joint called contact joint. These joints are created by the collision detection code and will be used by the simulation code to prevent interpenetration. With this scheme you can easily switch to another collision detection library as long as it provides the necessary information to create contact joints.

To run the example program you'll need PyOpenGL installed.

You can also view the result of the example program as an animation (2.4MB, DivX 5.05).

Program Listing

Download.

# pyODE example 3: Collision detection

# Originally by Matthias Baas.
# Updated by Pierre Gay to work without pygame or cgkit.

import sys, os, random, time
from math import *
from OpenGL.GL import *
from OpenGL.GLU import *
from OpenGL.GLUT import *

import ode

# geometric utility functions
def scalp (vec, scal):
    vec[0] *= scal
    vec[1] *= scal
    vec[2] *= scal

def length (vec):
    return sqrt (vec[0]**2 + vec[1]**2 + vec[2]**2)

# prepare_GL
def prepare_GL():
    """Prepare drawing.
    """

    # Viewport
    glViewport(0,0,640,480)

    # Initialize
    glClearColor(0.8,0.8,0.9,0)
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
    glEnable(GL_DEPTH_TEST)
    glDisable(GL_LIGHTING)
    glEnable(GL_LIGHTING)
    glEnable(GL_NORMALIZE)
    glShadeModel(GL_FLAT)

    # Projection
    glMatrixMode(GL_PROJECTION)
    glLoadIdentity()
    gluPerspective (45,1.3333,0.2,20)

    # Initialize ModelView matrix
    glMatrixMode(GL_MODELVIEW)
    glLoadIdentity()

    # Light source
    glLightfv(GL_LIGHT0,GL_POSITION,[0,0,1,0])
    glLightfv(GL_LIGHT0,GL_DIFFUSE,[1,1,1,1])
    glLightfv(GL_LIGHT0,GL_SPECULAR,[1,1,1,1])
    glEnable(GL_LIGHT0)

    # View transformation
    gluLookAt (2.4, 3.6, 4.8, 0.5, 0.5, 0, 0, 1, 0)

# draw_body
def draw_body(body):
    """Draw an ODE body.
    """

    x,y,z = body.getPosition()
    R = body.getRotation()
    rot = [R[0], R[3], R[6], 0.,
           R[1], R[4], R[7], 0.,
           R[2], R[5], R[8], 0.,
           x, y, z, 1.0]
    glPushMatrix()
    glMultMatrixd(rot)
    if body.shape=="box":
        sx,sy,sz = body.boxsize
        glScalef(sx, sy, sz)
        glutSolidCube(1)
    glPopMatrix()


# create_box
def create_box(world, space, density, lx, ly, lz):
    """Create a box body and its corresponding geom."""

    # Create body
    body = ode.Body(world)
    M = ode.Mass()
    M.setBox(density, lx, ly, lz)
    body.setMass(M)

    # Set parameters for drawing the body
    body.shape = "box"
    body.boxsize = (lx, ly, lz)

    # Create a box geom for collision detection
    geom = ode.GeomBox(space, lengths=body.boxsize)
    geom.setBody(body)

    return body, geom

# drop_object
def drop_object():
    """Drop an object into the scene."""

    global bodies, geom, counter, objcount

    body, geom = create_box(world, space, 1000, 1.0,0.2,0.2)
    body.setPosition( (random.gauss(0,0.1),3.0,random.gauss(0,0.1)) )
    theta = random.uniform(0,2*pi)
    ct = cos (theta)
    st = sin (theta)
    body.setRotation([ct, 0., -st, 0., 1., 0., st, 0., ct])
    bodies.append(body)
    geoms.append(geom)
    counter=0
    objcount+=1

# explosion
def explosion():
    """Simulate an explosion.

    Every object is pushed away from the origin.
    The force is dependent on the objects distance from the origin.
    """
    global bodies

    for b in bodies:
        l=b.getPosition ()
        d = length (l)
        a = max(0, 40000*(1.0-0.2*d*d))
        l = [l[0] / 4, l[1], l[2] /4]
        scalp (l, a / length (l))
        b.addForce(l)

# pull
def pull():
    """Pull the objects back to the origin.

    Every object will be pulled back to the origin.
    Every couple of frames there'll be a thrust upwards so that
    the objects won't stick to the ground all the time.
    """
    global bodies, counter

    for b in bodies:
        l=list (b.getPosition ())
        scalp (l, -1000 / length (l))
        b.addForce(l)
        if counter%60==0:
            b.addForce((0,10000,0))

# Collision callback
def near_callback(args, geom1, geom2):
    """Callback function for the collide() method.

    This function checks if the given geoms do collide and
    creates contact joints if they do.
    """

    # Check if the objects do collide
    contacts = ode.collide(geom1, geom2)

    # Create contact joints
    world,contactgroup = args
    for c in contacts:
        c.setBounce(0.2)
        c.setMu(5000)
        j = ode.ContactJoint(world, contactgroup, c)
        j.attach(geom1.getBody(), geom2.getBody())



######################################################################

# Initialize Glut
glutInit ([])

# Open a window
glutInitDisplayMode (GLUT_RGB | GLUT_DOUBLE)

x = 0
y = 0
width = 640
height = 480
glutInitWindowPosition (x, y);
glutInitWindowSize (width, height);
glutCreateWindow ("testode")

# Create a world object
world = ode.World()
world.setGravity( (0,-9.81,0) )
world.setERP(0.8)
world.setCFM(1E-5)

# Create a space object
space = ode.Space()

# Create a plane geom which prevent the objects from falling forever
floor = ode.GeomPlane(space, (0,1,0), 0)

# A list with ODE bodies
bodies = []

# The geoms for each of the bodies
geoms = []

# A joint group for the contact joints that are generated whenever
# two bodies collide
contactgroup = ode.JointGroup()

# Some variables used inside the simulation loop
fps = 50
dt = 1.0/fps
running = True
state = 0
counter = 0
objcount = 0
lasttime = time.time()


# keyboard callback
def _keyfunc (c, x, y):
    sys.exit (0)

glutKeyboardFunc (_keyfunc)

# draw callback
def _drawfunc ():
    # Draw the scene
    prepare_GL()
    for b in bodies:
        draw_body(b)

    glutSwapBuffers ()

glutDisplayFunc (_drawfunc)

# idle callback
def _idlefunc ():
    global counter, state, lasttime

    t = dt - (time.time() - lasttime)
    if (t > 0):
        time.sleep(t)

    counter += 1

    if state==0:
        if counter==20:
            drop_object()
        if objcount==30:
            state=1
            counter=0
    # State 1: Explosion and pulling back the objects
    elif state==1:
        if counter==100:
            explosion()
        if counter>300:
            pull()
        if counter==500:
            counter=20

    glutPostRedisplay ()

    # Simulate
    n = 2

    for i in range(n):
        # Detect collisions and create contact joints
        space.collide((world,contactgroup), near_callback)

        # Simulation step
        world.step(dt/n)

        # Remove all contact joints
        contactgroup.empty()

    lasttime = time.time()

glutIdleFunc (_idlefunc)

glutMainLoop ()

Explanation

If you run the program you'll first see a couple of boxes falling from above. Then there'll be kind of an "explosion" that blows the boxes away, followed by a pulling back phase, another explosion, and so on.

First, a short overview of the functions:

prepare_GL()

Initializes some OpenGL stuff necessary for drawing the scene.

draw_body(body)

Draws a box object.

create_box(world, space, density, lx, ly, lz)

Creates a new box object.

drop_object()

Creates a box and places it above the scene, using a little randomness.

explosion()

Adds forces to the objects that simulate something like an explosion.

pull()

Adds forces that pull the object back to the origin.

near_callback(args, geom1, geom2)

This is the callback function for the collision detection.

And now to the parts that are relevant for the collision detection:

# Create a space object
space = ode.Space()

In the main code (at the bottom) we create a Space object. Spaces are containers for geom objects that are the actual objects tested for collision. For the collision detection a Space is the same as the World for the dynamics simulation, and a geom object corresponds to a body object. For the pure dynamics simulation the actual shape of an object doesn't matter, you only have to know its mass properties. However, to do collision detection you need to know what an object actually looks like, and this is what's the difference between a body and a geom.

# Create a plane geom which prevent the objects from falling forever
floor = ode.GeomPlane(space, (0,1,0), 0)

Here we create our first geom object. It's a plane that serves as a floor so that our boxes won't fall forever. The floor won't participate in the actual dynamics simulation (this means, it's a fixed object that's unmovable), so we don't create a corresponding Body object.

# create_box
def create_box(world, space, density, lx, ly, lz):
    """Create a box body and its corresponding geom."""

    # Create body
    body = ode.Body(world)
    M = ode.Mass()
    M.setBox(density, lx, ly, lz)
    body.setMass(M)

    # Set parameters for drawing the body
    body.shape = "box"
    body.boxsize = (lx, ly, lz)

    # Create a box geom for collision detection
    geom = ode.GeomBox(space, lengths=body.boxsize)
    geom.setBody(body)

    return body

In this function we first create a body we want to simulate and later on a corresponding geom object, in this case a GeomBox. By calling geom.setBody() we connect the body and the geom, so that the geom always has the same position and orientation than the body (otherwise it'd be our own responsibility to keep them both in sync).

By the way, in the middle of the function we set two attributes shape and boxsize in the body. These are no predefined attributes but are just initialized here to be used in the drawing code later on. So as you can see, you can just store arbitrary attributes in a body object. You can regard this as a generalization of the ODE functions dBodySetData() and dBodyGetData().

# Simulate
n = 2
for i in range(n):
    # Detect collisions and create contact joints
    space.collide((world,contactgroup), near_callback)

    # Simulation step
    world.step(dt/n)

    # Remove all contact joints
    contactgroup.empty()

This is the actual simulation (we do 2 simulation steps per frame to prevent jittering due to “small” collisions). Since the collision detection is decoupled from the dynamics simulation we have to call it manually. We do this by just calling the collide() method of the space object. This method takes a callback function and an arbitrary argument that gets passed to the callback. The callback is called whenever two objects may potentially collide. The callback function then has to do a proper collision test and has to create contact joints whenever a collision occured. Once all necessary contact joints are created we can proceed by calling world.step(). The last line removes all contact joints (since there'll be new ones in the next step). We took advantage of a joint group to easily remove all contact joints (and keep all regular joints (well, you're right, we don't have any regular joints in this example, but there could be some...)).

Finally, we have to take a look at the collision callback function:

# Collision callback
def near_callback(args, geom1, geom2):
    """Callback function for the collide() method.

    This function checks if the given geoms do collide and
    creates contact joints if they do.
    """

    # Check if the objects do collide
    contacts = ode.collide(geom1, geom2)

    # Create contact joints
    world,contactgroup = args
    for c in contacts:
        c.setBounce(0.2)
        c.setMu(5000)
        j = ode.ContactJoint(world, contactgroup, c)
        j.attach(geom1.getBody(), geom2.getBody())

This function has three arguments. The first one is just the one we provided as input to the collide() method of the space object. The other two are the geoms that potentially intersect. In the function we first call the collide() function (which is different from the space.collide() method) which checks if two geoms intersect. This function returns a list of Contact objects (or an empty list if the geoms don't intersect). These contact objects carry all the necessary information that's used inside the dynamics part (such as the contact position and normal, but also the surface properties like friction and bounciness). Once you have those contact objects you can create contact joints which are added to a joint group so we can easily remove them after the simulation step.

That's all to get collision detection and response working. The remaining functions are just used to make the example a bit more interesting.

As a summary, here are the steps to do collision detection:

1. Create a Space object.
2. Create an appropriate geom object for each body that should take part in the collision detection and set the body in the geom (if your body should have a more complex shape than just a sphere, box or cylinder it's also possible to approximate the actual shape by using several geoms).
3. Create additional geoms without bodies for any fixed geometry.
4. Before every call to world.step() call space.collide() and provide a callback function that does the actual collision test and creates contact joints.
5. After the call to world.step() remove all previously created contact joints (this is best done by placing all contact joints into a joint group and call the group's empty() method).