Hi,
I'm new to ODE/PyODE and I've been playing around with the tutorial
scripts over the last week to get a feel for the library. However,
there are still a few features I've had problems accessing.
The attached script is based on the 2nd and 3rd PyODE
tutorials, and makes use of PyGame to visualize the results in 2D, as
well as Numpy to do some simple matrix multiplication to compute the
rotation of polygons.
Basically, the script drops a few spheres and boxes onto planes to the
left, right, and bottom simulating walls and a floor. It attempts to
simulate collision detection, both to stop objects from passing
through each other and the planes. It also tries to detect objects in
a certain area, identified by a bodiless geom box in the center of the
screen. When a geom passes through that area, the collision detection
code *should* turn the geom blue.
If you run the script, you will notice several problems.
First, while basic collision detection seems to work well enough to
bounce objects off one another, there's still a lot of "mushiness",
with objects routinely going partly or full way through one another.
I've tried tweaking ERF/CFM but I haven't been able to improve things
much. It seems at one setting the objects act mushy, and at another
the simulation becomes completely unstable. I can't find a middle
ground. Is there some way around this?
Second, I'm unable to detect when objects enter my bodiless geom. I'm
using collide2, which should return all geoms in contact with the
target geom, but the results are chaotic. The callback signals a
contact by changing the color of the contacting geom to blue. Some
objects do change blue, but at positions that don't correspond to the
target geom. Is there an error in my drawing code or am I incorrectly
implementing collision detection?
Third, I'm not sure I'm interpreting a geom's rotation data correctly.
I allow geoms to rotate about the Z axis, and so I attempt to draw
this rotation by using the rotation matrix returned by
geom.getRotation(), but the results are a little confusing. The
rectangle in the scene seems to correctly rotate somewhat, but there's
enough error that I'm unsure whether my code's wrong or if it's simply
"mushiness". Again, is this a problem with my drawing code, or with
how I'm interpreting the rotation data?
On a side note, I attempted to use plane2DJoint to restrict movement
in the XY plane, according to
http://opende.sourceforge.net/wiki/index.php/HOWTO_constrain_objects_to_2d
but I was unable to get it to work. I achieved the same effect by
reseting the Z position at each iteration, but I fear this may be
inefficient. Is there any trick to using plane2DJoint in PyODE?
I've been working on this for days now, but I'm reaching a plateau. If
anyone could give me some tips or pointers, I'd be immensely grateful.
Thanks,
Chris
'''
todo: Use plane2DJoint to restrict Z movement.
The method listed at http://opende.sourceforge.net/wiki/index.php/HOWTO_constrain_objects_to_2d
does not appear to work.
todo: Detect objects in geomBox. Collision detection works with geomPlane, but not geomBox
for some reason.
todo: stop objects from going through each other
'''
import sys, os, random, time
from math import sqrt
import pygame
from pygame.locals import *
import ode
from numpy import matrix
bodies = []
def create_box(world, space, position, density, lengths, color, body=1):
"""Create a box body and its corresponding geom."""
# Create a box geom for collision detection
geom = ode.GeomBox(space, lengths=lengths)
if body:
# Create body
body = ode.Body(world)
M = ode.Mass()
M.setBox(density, *lengths)
body.setMass(M)
geom.setBody(body)
# constrain to 2D
# todo: fix, this does nothing!!!
planeJoint.attach(body, None)
# Set parameters for drawing the body
geom.shape = "box"
geom.color = color
geom.setPosition(position)
global bodies
bodies.append(geom)
return geom
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)
# Create a box geom for collision detection
geom = ode.GeomSphere(space, radius)
geom.setBody(body)
# Set parameters for drawing the body
geom.shape = body.shape = "sphere"
geom.color = body.color = color
body.setPosition(position)
# constrain to 2D
# todo: fix, this does nothing!!!
planeJoint.attach(body, None)
global bodies
bodies.append(geom)
return geom
# Collision callback
viewboxContacts = set()
contactDict = {}
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)
#if contacts:
# print '%i contacts!!!!!!!!!!!!!!!!' % len(contacts)
# Create contact joints
world,contactgroup = args
for c in contacts:
# viewbox should be bodiless...
# but explicitly ignore anyways since it seems to be effecting collisions
if geom1 is viewbox1 or geom2 is viewbox1:
continue
c.setBounce(1)
c.setMu(5000)
#contactDict.setdefault(contactgroup, set())
#contactDict[contactgroup].update([geom1,geom2])
j = ode.ContactJoint(world, contactgroup, c)
j.attach(geom1.getBody(), geom2.getBody())
'''
# mark geoms in contact with viewbox (blue)
if geom1 is viewbox1 or geom2 is viewbox1:
#print time.time(),'intersecting viewbox!!!!!!!!!!!!',geom1,geom2
#viewboxContacts.update([g for g in (geom1,geom2) if g is not viewbox1])
#print viewboxContacts
for g in [g for g in (geom1,geom2) if g is not viewbox1]:
g.color = (0,0,255)
'''
'''
# mark geoms in contact with floor (red)
if geom1 is floor or geom2 is floor:
for g in [g for g in (geom1,geom2) if g is not viewbox1]:
g.color = (255,0,0)
'''
def viewbox_callback(args, geom1, geom2):
if geom1 is not viewbox1:
geom1.color = (0,0,255)
if geom2 is not viewbox1:
geom2.color = (0,0,255)
def alignToZAxis(body):
rot = body.getAngularVel()
quat = list(body.getQuaternion())
quat[1] = 0
quat[2] = 0
quat_len = sqrt( quat[0] * quat[0] + quat[3] * quat[3] )
quat[0] /= quat_len
quat[3] /= quat_len
body.setQuaternion(quat)
body.setAngularVel((0, 0, rot[2]))
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)
# A list with ODE bodies
bodies = []
# restrict positions to these ranges
x_min,x_max = 50,590
y_min,y_max = 50,430
z_min,z_max = 0,0
# Create a world object
world = ode.World()
world.setGravity( (0,9.81,0) )
world.setERP(0.8)
world.setCFM(1E-9)
# Create a space object
space = ode.Space(type=1)
# Create a plane geom which prevent the objects from falling forever
floor = ode.GeomPlane(space, (0,-1,0), -430)
floor.shape = 'plane'
bodies.append(floor)
#'''
floor2 = ode.GeomPlane(space, (1,0,0), 50)
floor2.shape = 'plane'
bodies.append(floor2)
#'''
# these don't work as expected...
# causes spheres to fly to extremes...
# generates "ODE Message 2: vector has zero size in dNormalize4() File odemath.cpp Line 129"
floor3 = ode.GeomPlane(space, (-1,0,0), -590)
floor3.shape = 'plane'
bodies.append(floor3)
# A joint group for the contact joints that are generated whenever
# two bodies collide
contactgroup = ode.JointGroup()
planeJoint = ode.Plane2DJoint(world) # restricts movement to 2D
# 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=(270,50,0),
density=220,
radius=50,
color=(255,255,255)
)
sphere2 = create_sphere(
world=world,
space=space,
position=(320,170,0),
density=220,
radius=50,
color=(255,255,255)
)
sphere3 = create_sphere(
world=world,
space=space,
position=(370,50,0),
density=220,
radius=50,
color=(255,255,255)
)
w,h = 200,20
box1 = create_box(
world=world,
space=space,
position=(320-w/2,240-h/2,-100),
density=220,
lengths=(w,h,50),
color=(255,255,255)
)
# create bounding box to detect shapes in user's view
w,h = 200,200
viewbox1 = create_box(
world=world,
space=space,
position=(320-w/2,240-h/2,-100),
density=220,
lengths=(w,h,100),
color=(255,255,255),
body=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((0,0,0))
# Draw the bodies
for geom in bodies:
if geom.getBody():
alignToZAxis(geom.getBody())
if geom.shape == 'sphere':
x1,y1,z1 = geom.getPosition()
#print 'z',z1
if geom.getBody():
x1 = min(max(x_min, x1), x_max)
y1 = min(max(y_min, y1), y_max)
z1 = min(max(z_min, z1), z_max)
geom.setPosition((x1,y1,0.0))
pygame.draw.circle(srf, geom.color, (x1,y1), geom.getRadius(), 1)
elif geom.shape == 'box':
x1,y1,z1 = geom.getPosition()
lx,ly,lz = geom.getLengths()
#print 'z',z1
#print geom.getRotation()
if geom.getBody():
#print 'no body',geom
x1 = min(max(x_min, x1), x_max)
y1 = min(max(y_min, y1), y_max)
z1 = min(max(z_min, z1), z_max)
geom.setPosition((x1,y1,0))
#print Rect(x1,y1,lx,ly) # this will error if units get too large
#x1,y1 = cameraOffset(x1,y1)
p1 = x1,y1
p2 = x1+lx,y1+ly
p3 = p1[0],p2[1]
p4 = p2[0],p1[1]
points = p1,p4,p2,p3
pos = geom.getPosition()
#print 'raw:',points
# untranslate
points = map(lambda p: (p[0]-pos[0],p[1]-pos[1]), points)
#print 'untran:',points
if geom.placeable():
r = geom.getRotation()
R = matrix([r[i:i+2] for i in xrange(0,6,3)]) # find rotation matrix (take only x,y components)
points = map(matrix, points) # convert to matrix
points = map(lambda p: p*R, points) # transform points
points = map(lambda p: p.tolist()[0], points) # convert to list
#print 'roted:',points
# retranslate
points = map(lambda p: (p[0]+pos[0],p[1]+pos[1]), points)
#print 'retran:',points
#pygame.draw.rect(srf, geom.color, Rect(x1,y1,lx,ly), 1)
pygame.draw.polygon(srf, (255,255,255), points, 1)
elif geom.shape == 'plane':
normal,dist = geom.getParams()
nx,ny,nz = normal
if ny:
f = lambda x: (dist-nx*x)/ny
p1 = 0, f(0)
p2 = pixel_w, f(pixel_w)
else:
f = lambda y: (dist-ny*y)/nx
p1 = f(0), 0
p2 = f(pixel_h), pixel_h
#print p1,p2
pygame.draw.line(srf, (255,255,255), p1, p2, 1)
else:
print 'unknown shape',geom
# clear prior marking
geom.color = (255,255,255)
# Remove all contact joints and unmark bodies no longer in contact
contactgroup.empty()
viewboxContacts.clear()
# 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)
world.quickStep(dt/n)
# detect objects in viewing box
ode.collide2(viewbox1, space, None, viewbox_callback)
pygame.display.flip()
# Next simulation step
world.step(dt)
# Try to keep the specified framerate
clk.tick(fps)
-------------------------------------------------------------------------
This SF.net email is sponsored by DB2 Express
Download DB2 Express C - the FREE version of DB2 express and take
control of your XML. No limits. Just data. Click to get it now.
http://sourceforge.net/powerbar/db2/
_______________________________________________
Pyode-user mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/pyode-user
