[ODE] Intro Experiment

[Chris Spencer]

Hi,

I'm new to ODE and I've been using PyODE
(http://pyode.sourceforge.net/) to run some experiments to get a feel
for how the library works. However, I've been getting some very
strange results that don't make any sense to me.

The code below is a simple script based on the 2nd and 3rd PyODE
tutorials, and makes use of PyGame to visualize the results in 2D.
Basically, it creates 3 spheres, 1 box, and 1 plane, and then attempts
to simulate collision detection.

With gravity enabled, I'd expect the spheres and box to fall down onto
the plane, bounce, and eventually stop. However, they appear to fall
indefinitely, as if no collision detection is happening.

Also, even with gravity disabled and no explicit forces being applied,
the bodies move slightly. Does ODE simulate gravitational attraction
of masses, and would this be causing this slight movement?

If anyone could help point out what I'm doing wrong, I'd be very grateful.

Thanks,
Chris

import sys, os, random, time
import pygame
from pygame.locals import *
import ode

bodies = []

def create_box(world, space, position, density, lengths, color, geom=1):
    """Create a box body and its corresponding geom."""

    # Create body
    body = ode.Body(world)
    M = ode.Mass()
    M.setBox(density, *lengths)
    body.setMass(M)

    # Set parameters for drawing the body
    body.shape = "box"
    body.boxsize = lengths
    body.color = color

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

    body.setPosition(position)
    return body

def create_sphere(world, space, position, density, radius, color, geom=1):
    """Create a sphere body and its corresponding geom."""

    # Create body
    body = ode.Body(world)
    M = ode.Mass()
    M.setSphere(density, radius)
    body.setMass(M)

    # Set parameters for drawing the body
    body.shape = "sphere"
    body.radius = radius
    body.color = color

    # Create a box geom for collision detection
    if geom:
        geom = ode.GeomSphere(space, radius)
        geom.setBody(body)

    body.setPosition(position)
    return body

def add_body(body):
    """Drop an object into the scene."""

    global bodies
    bodies.append(body)

# 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())

pixel_w,pixel_h = pixel_dimensions = 640,480
camera_w,camera_h = camera_position = pixel_w/2,pixel_h/2

def cameraOffset(x,y):
    return camera_w+x, camera_h+y

# Initialize pygame
pygame.init()

# Open a display
srf = pygame.display.set_mode(pixel_dimensions)

# 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 = []

# 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()

# create some objects

sphere1 = create_sphere(
    world=world,
    space=space,
    position=(-150,-100,0),
    density=2500,
    radius=50,
    color=(50,0,200)
)
add_body(sphere1)

sphere2 = create_sphere(
    world=world,
    space=space,
    position=(0,-100,0),
    density=2500,
    radius=50,
    color=(255,255,255)
)
add_body(sphere2)

sphere3 = create_sphere(
    world=world,
    space=space,
    position=(150,-100,0),
    density=2500,
    radius=50,
    color=(0,200,50)
)
add_body(sphere3)

box1 = create_box(
    world=world,
    space=space,
    position=(-75,0,0),
    density=500,
    lengths=(200,200,200),
    color=(50,0,200)
)
add_body(box1)
#box1.addForce( (0,-5000,0) )

# Simulation loop...
fps = 30
dt = 1.0/fps
loopFlag = True
clk = pygame.time.Clock()
while loopFlag:
    events = pygame.event.get()
    for e in events:
        if e.type==QUIT:
            loopFlag=False
        if e.type==KEYDOWN:
            loopFlag=False

    # Clear the screen
    #srf.fill((255,255,255))
    srf.fill((0,0,0))

    # Draw the bodies
    for body in bodies:
        if body.shape == 'sphere':
            x1,y1,z1 = body.getPosition()
            #color = (55,0,200)
            pygame.draw.circle(srf, body.color, cameraOffset(x1,y1),
body.radius, 1)
        elif body.shape == 'box':
            x1,y1,z1 = body.getPosition()
            x1,y1 = cameraOffset(x1,y1)
            lx,ly,lz = body.boxsize
            print 'drawing box',body.getPosition(),body.boxsize
            width = 0
            pygame.draw.rect(srf, body.color, Rect(x1,y1,lx,ly), width)
        else:
            print 'unknown body',body

    # Simulate
    n = 2
    for i in xrange(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()

    pygame.display.flip()

    # Next simulation step
    world.step(dt)

    # Try to keep the specified framerate
    clk.tick(fps)