def test_connect_to(box):
b = pagoda.physics.Box('b', box.world, lengths=(1, 2, 3))
assert not ode.areConnected(box.ode_body, b.ode_body)
assert np.allclose(box.position, (0, 0, 0))
assert np.allclose(b.position, (0, 0, 0))
box.connect_to('hinge', b, (1, 0, 0), (-1, 0, 0), name='j')
assert ode.areConnected(box.ode_body, b.ode_body)
assert np.allclose(box.position, (0, 0, 0))
assert np.allclose(b.position, (1, 0, 0))
j = box.world.get_joint('j')
assert np.allclose(j.anchor, (0.5, 0, 0))
def are_connected(self,b1,b2):
""" Interface to call the ode dBodyAreConnected()
Args:
b1, b2: bodies to check if they are connected.
"""
return True if ode.areConnected(b1,b2) else False
def near_callback(self, 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.
"""
# Are both geoms static? We don't care if they touch.
if geom1.getBody() is None and geom2.getBody() is None:
return
# Are both geoms connected?
if (ode.areConnected(geom1.getBody(), geom2.getBody())):
return
# check if the objects collide
contacts = ode.collide(geom1, geom2)
# create contact joints
self.world, self.contactgroup = args
for c in contacts:
c.setBounce(0.2)
# ode.ContactApprox1 makes maximum frictional force proportional to
# normal force
c.setMode(ode.ContactApprox1)
# Friction coefficient: Max friction (tangent) force = Mu*Normal force
c.setMu(0.5)
j = ode.ContactJoint(self.world, None, c)
# FIXME: Store the position of the contact
j.position = c.getContactGeomParams()[0]
self.contactlist.append(j)
j.attach(geom1.getBody(), geom2.getBody())
j.setFeedback(True)
def near_callback(self, 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.
"""
body1 = geom1.getBody()
body2 = geom2.getBody()
if ode.areConnected(body1, body2):
return
# Check if the objects do collide
contacts = ode.collide(geom1, geom2)
if geom1.name == "object":
for c in contacts:
pos, normal, depth, g1, g2 = c.getContactGeomParams()
self.grasp_contacts.append( (pos[0], pos[1], pos[2], -normal[0], -normal[1], -normal[2]) )
if geom2.name == "object":
for c in contacts:
pos, normal, depth, g1, g2 = c.getContactGeomParams()
self.grasp_contacts.append( (pos[0], pos[1], pos[2], normal[0], normal[1], normal[2]) )
# Create contact joints
world,contactgroup = args
for c in contacts:
c.setBounce(0.0)
c.setMu(5000)
j = ode.ContactJoint(world, contactgroup, c)
j.attach(body1, body2)
def nearcb(self, args, geom1, geom2):
"""Create contact joints between colliding geoms"""
body1, body2 = geom1.getBody(), geom2.getBody()
pos = ()
if body1 is None:
body1 = ode.environment
else:
pos = body1.getPosition()
if body2 is None:
body2 = ode.environment
else:
pos = body2.getPosition()
if ode.areConnected(body1, body2):
return
contacts = ode.collide(geom1, geom2)
for c in contacts:
# c.setBounce(0.2)
# c.setMu(10000)
c.setBounce(1)
c.setMu(0)
j = ode.ContactJoint(self.world, self.contactgroup, c)
j.attach(body1, body2)
# report collision
self._outlet(3,(self.geoms[geom1],self.geoms[geom2])+pos)
def near_callback(self, args, geom1, geom2):
"""
Function invoke when detected collisions
:type geom2: GeomObject
:type geom1: GeomObject
:type args: object
"""
body1, body2 = geom1.getBody(), geom2.getBody()
if (body1 is None):
body1 = ode.environment
if (body2 is None):
body2 = ode.environment
if (ode.areConnected(body1, body2)):
return
contacts = ode.collide(geom1, geom2)
if body1 is not None and contacts:
velocity = body1.getLinearVel()
body1.agent.direction = (max(min(velocity[0], 1.0), -1.0), 0.0, max(min(velocity[2], 1.0), -1.0))
if body2 is not None and contacts:
velocity = body2.getLinearVel()
body2.agent.direction = (max(min(velocity[0], 1.0), -1.0), 0.0, max(min(velocity[2], 1.0), -1.0))
if body1 is not None and body2 is not None and contacts:
self.encounter(body1, body2)
for c in contacts:
self.touch(c,body1,body2)
j = ode.ContactJoint(self.world, self.contactJoints, c)
j.attach(body1, body2)
def nearcb(self, args, geom1, geom2):
"""Create contact joints between colliding geoms"""
body1, body2 = geom1.getBody(), geom2.getBody()
pos = ()
if body1 is None:
body1 = ode.environment
else:
pos = body1.getPosition()
if body2 is None:
body2 = ode.environment
else:
pos = body2.getPosition()
if ode.areConnected(body1, body2):
return
contacts = ode.collide(geom1, geom2)
for c in contacts:
# c.setBounce(0.2)
# c.setMu(10000)
c.setBounce(1)
c.setMu(0)
j = ode.ContactJoint(self.world, self.contactgroup, c)
j.attach(body1, body2)
# report collision
self._outlet(3, (self.geoms[geom1], self.geoms[geom2]) + pos)
def are_connected(self, b1, b2):
""" Interface to call the ode dBodyAreConnected()
Args:
b1, b2: bodies to check if they are connected.
"""
return True if ode.areConnected(b1, b2) else False
def collide(self, args, geom1, geom2):
body1 = geom1.getBody()
body2 = geom2.getBody()
if not ode.areConnected(body1, body2):
for c in ode.collide(geom1, geom2):
c.setBounce(0.2)
c.setMu(0.0)
j = ode.ContactJoint(self.world, self.contacts, c)
j.attach(body1, body2)
def collide_callback(args, obj1: ode.GeomObject, obj2: ode.GeomObject):
if ode.areConnected(obj1.getBody(),obj2.getBody()):
pass
contacts = ode.collide(obj1, obj2)
world, joint_group = args
for c in contacts:
j = ode.ContactJoint(world, joint_group, c)
j.attach(obj1.getBody(), obj2.getBody())
def collide_callback(args, obj1: ode.GeomObject, obj2: ode.GeomObject):
if ode.areConnected(obj1.getBody(), obj2.getBody()):
pass
print("COLLISION!!!!")
contacts = ode.collide(obj1, obj2)
world, joint_group = args
for c in contacts:
j = ode.ContactJoint(world, joint_group, c)
j.attach(obj1.getBody(), obj2.getBody())
def nearcb(self, args, g1, g2): b1, b2 = g1.getBody(), g2.getBody() if (b1 is None): b1=ode.environment if (b2 is None): b2=ode.environment if ode.areConnected(b1, b2): #pass return contacts = ode.collide(g1, g2) for c in contacts: c.setBounce(0.0) c.setMu(10000) j=ode.ContactJoint(self.world, self.cjoints, c) j.attach(b1, b2) self.internalstate['contacts'].append((b1,b2))
def near_callback(args, g1, g2):
try:
if getattr(g1, 'isImmovable', False) and getattr(
g2, 'isImmovable', False):
return
if g1.isSpace() or g2.isSpace():
if g1.isSpace() and not getattr(g1, 'isImmovable', False):
g1.collide(args, near_callback)
if g2.isSpace() and not getattr(g2, 'isImmovable', False):
g2.collide(args, near_callback)
ode.collide2(g1, g2, args, near_callback)
contacts = ode.collide(g1, g2)
if ode.areConnected(g1.getBody(), g2.getBody()):
return
d1 = getattr(g1.getBody(), 'data', None)
d2 = getattr(g2.getBody(), 'data', None)
if d1 is None: d1 = getattr(g1, 'data', None)
if d2 is None: d2 = getattr(g2, 'data', None)
if d1 is not None: d1 = d1()
if d2 is not None: d2 = d2()
# TODO: if d1 and/or d2 are not None, use them to handle collision
r1 = True
r2 = True
if len(contacts) > 0:
if d1 is not None and hasattr(d1, 'onCollision'):
r1 = d1.onCollision(d2)
if d2 is not None and hasattr(d2, 'onCollision'):
r2 = d2.onCollision(d1)
if r1 and r2:
for c in contacts:
c.setBounce(0.8)
c.setMu(250)
c.setMode(ode.ContactApprox1)
objects.DebugPoint(c.getContactGeomParams()[0])
j = ode.ContactJoint(objects.world, objects.contactgroup,
c)
j.attach(g1.getBody(), g2.getBody())
except BaseException, e:
print "Exception in collision handler: ", str(e)
def near_callback(args, g1, g2): try: if getattr(g1, 'isImmovable', False) and getattr(g2, 'isImmovable', False): return if g1.isSpace() or g2.isSpace(): if g1.isSpace() and not getattr(g1, 'isImmovable', False): g1.collide(args, near_callback) if g2.isSpace() and not getattr(g2, 'isImmovable', False): g2.collide(args, near_callback) ode.collide2(g1, g2, args, near_callback) contacts = ode.collide(g1, g2) if ode.areConnected(g1.getBody(), g2.getBody()): return d1 = getattr(g1.getBody(), 'data', None) d2 = getattr(g2.getBody(), 'data', None) if d1 is None: d1 = getattr(g1, 'data', None) if d2 is None: d2 = getattr(g2, 'data', None) if d1 is not None: d1 = d1() if d2 is not None: d2 = d2() # TODO: if d1 and/or d2 are not None, use them to handle collision r1 = True; r2 = True if len(contacts) > 0: if d1 is not None and hasattr(d1, 'onCollision'): r1 = d1.onCollision(d2) if d2 is not None and hasattr(d2, 'onCollision'): r2 = d2.onCollision(d1) if r1 and r2: for c in contacts: c.setBounce(0.8) c.setMu(250) c.setMode(ode.ContactApprox1) objects.DebugPoint(c.getContactGeomParams()[0]) j = ode.ContactJoint(objects.world, objects.contactgroup, c) j.attach(g1.getBody(), g2.getBody()) except BaseException, e: print "Exception in collision handler: ", str(e)
def are_connected(self, body_a, body_b):
'''Determine whether the given bodies are currently connected.
Parameters
----------
body_a : str or :class:`Body`
One body to test for connectedness. If this is a string, it is
treated as the name of a body to look up.
body_b : str or :class:`Body`
One body to test for connectedness. If this is a string, it is
treated as the name of a body to look up.
Returns
-------
connected : bool
Return True iff the two bodies are connected.
'''
return bool(ode.areConnected(
self.get_body(body_a).ode_body,
self.get_body(body_b).ode_body))
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.
"""
if (ode.areConnected(geom1.getBody(), geom2.getBody())):
return
# check if the objects collide
contacts = ode.collide(geom1, geom2)
# create contact joints
world, contactgroup = args
for c in contacts:
c.setBounce(0.2)
c.setMu(500) # 0-5 = very slippery, 50-500 = normal, 5000 = very sticky
j = ode.ContactJoint(world, contactgroup, c)
j.attach(geom1.getBody(), geom2.getBody())
def _nearcb(self, args, geom1, geom2):
"""
Create contact joints between colliding geoms.
"""
body1, body2 = geom1.getBody(), geom2.getBody()
if (body1 is None):
body1 = ode.environment
if (body2 is None):
body2 = ode.environment
if (ode.areConnected(body1, body2)):
return
contacts = ode.collide(geom1, geom2)
for c in contacts:
c.setBounce(0.2)
c.setMu(10000)
j = ode.ContactJoint(self.world, self._cjoints, c)
j.attach(body1, body2)
def _nearcb(self, args, geom1, geom2): """ Create contact joints between colliding geoms. """ body1, body2 = geom1.getBody(), geom2.getBody() if (body1 is None): body1 = ode.environment if (body2 is None): body2 = ode.environment if (ode.areConnected(body1, body2)): return contacts = ode.collide(geom1, geom2) for c in contacts: c.setBounce(0.05) c.setMu(10000) j = ode.ContactJoint(self.world, self._cjoints, c) j.attach(body1, body2)
def are_connected(self, body_a, body_b):
'''Determine whether the given bodies are currently connected.
Parameters
----------
body_a : str or :class:`Body`
One body to test for connectedness. If this is a string, it is
treated as the name of a body to look up.
body_b : str or :class:`Body`
One body to test for connectedness. If this is a string, it is
treated as the name of a body to look up.
Returns
-------
connected : bool
Return True iff the two bodies are connected.
'''
return bool(
ode.areConnected(
self.get_body(body_a).ode_body,
self.get_body(body_b).ode_body))
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. """ if (ode.areConnected(geom1.getBody(), geom2.getBody())): return # check if the objects collide contacts = ode.collide(geom1, geom2) # create contact joints world, contactgroup = args for c in contacts: c.setBounce(0.2) c.setMu(500) # 0-5 = very slippery, 50-500 = normal, 5000 = very sticky j = ode.ContactJoint(world, contactgroup, c) j.attach(geom1.getBody(), geom2.getBody())
def on_collision(self, args, geom_a, geom_b):
'''Callback function for the collide() method.
Parameters
----------
args : None
Arguments passed when the callback was registered. Not used.
geom_a : ODE geometry
The geometry object of one of the bodies that has collided.
geom_b : ODE geometry
The geometry object of one of the bodies that has collided.
'''
body_a = geom_a.getBody()
body_b = geom_b.getBody()
if ode.areConnected(body_a, body_b) or \
(body_a and body_a.isKinematic()) or \
(body_b and body_b.isKinematic()):
return
for c in ode.collide(geom_a, geom_b):
c.setBounce(self.elasticity)
c.setMu(self.friction)
ode.ContactJoint(self.ode_world, self.ode_contactgroup, c).attach(
geom_a.getBody(), geom_b.getBody())
def on_collision(self, args, geom_a, geom_b):
'''Callback function for the collide() method.
Parameters
----------
args : None
Arguments passed when the callback was registered. Not used.
geom_a : ODE geometry
The geometry object of one of the bodies that has collided.
geom_b : ODE geometry
The geometry object of one of the bodies that has collided.
'''
body_a = geom_a.getBody()
body_b = geom_b.getBody()
if ode.areConnected(body_a, body_b) or \
(body_a and body_a.isKinematic()) or \
(body_b and body_b.isKinematic()):
return
for c in ode.collide(geom_a, geom_b):
c.setBounce(self.elasticity)
c.setMu(self.friction)
ode.ContactJoint(self.ode_world, self.ode_contactgroup,
c).attach(geom_a.getBody(), geom_b.getBody())
def test_join_to(box):
b = pagoda.physics.Box('b', box.world, lengths=(1, 2, 3))
assert not ode.areConnected(box.ode_body, b.ode_body)
box.join_to('hinge', b)
assert ode.areConnected(box.ode_body, b.ode_body)
def _nearcb(self, args, geom1, geom2):
"""
Create contact joints between colliding geoms.
"""
body1, body2 = geom1.getBody(), geom2.getBody()
if (body1 is None):
body1 = ode.environment
if (body2 is None):
body2 = ode.environment
if (ode.areConnected(body1, body2)):
return
contacts = ode.collide(geom1, geom2)
floor_found = False
other_geom = None
for geom in (geom1, geom2):
if (floor_found):
other_geom = geom
if (self._floor == geom):
floor_found = True
else:
other_geom = geom
contacts_generated = False
if (floor_found):
allowed = (ode.GeomSphere, ode.GeomCylinder)
ok = False
for klass in allowed:
ok = ok or isinstance(other_geom, klass)
if (ok):
# we have found a wheel touching the floor
body = other_geom.getBody()
wheel_axis_local = (0, 0, 1)
wheel_axis_world = body.vectorToWorld(wheel_axis_local)
#wheel_axis_world = body.getRelPointPos(
#body.vectorToWorld(wheel_axis_local))
#print "wheel_axis_world =", wheel_axis_world
#self._helpers.append((wheel_axis_world, (1, 0, 0)))
#wheel_axis_world = (
#body.getPosition()[0] - wheel_axis_world[0],
#body.getPosition()[1] - wheel_axis_world[1],
#body.getPosition()[2] - wheel_axis_world[2]
#)
#print "wheel_axis_world =", wheel_axis_world
for contact in contacts:
pos, normal, _, _, _ = contact.getContactGeomParams()
axis = maths.cross_product(normal, wheel_axis_world)
axis = maths.normalise(axis)
if (axis is not None):
wheel_axis_floor = maths.cross_product(axis, normal)
self._helpers.append((maths.add(normal, pos),
(0, 1, 0)))
self._helpers.append((maths.add(wheel_axis_world, pos),
(0, 0, 1)))
self._helpers.append((maths.add(axis, pos),
(1, 0, 0)))
contact.setFDir1(wheel_axis_floor)
#contact.setFDir1(axis)
contact.setMu(6000)
contact.setMu2(10000)
contact.setBounce(0.2)
contact.setMode(ode.ContactFDir1 + ode.ContactMu2)
joint = ode.ContactJoint(
self.world, self._cjoints, contact)
joint.attach(body1, body2)
else:
self._make_simple_contact(contact, body1, body2)
contacts_generated = True
if (not contacts_generated):
for c in contacts:
self._make_simple_contact(c, body1, body2)
def __init__(self,world): super(self.__class__,self).__init__() self.world = world def thrust(self): for obj in self: obj.thrust() def render(self): for obj in self: obj.render() contacts_count = 0 def near_callback((world,contactgroup),geom1,geom2): """Handles collisions from the collide() function.""" global contacts_count obj1 = geom1.getBody() if not obj1: obj1 = main.floor obj2 = geom2.getBody() if not obj2: obj2 = main.floor if ode.areConnected(geom1.getBody(),geom2.getBody()): return contacts = ode.collide(geom1,geom2) for contact in contacts: contacts_count += 1 print 'Contact',contacts_count,'between object',obj1.id,'and object',obj2.id contact.setBounce(.3) contact.setMu(20) joint = ode.ContactJoint(world,contactgroup,contact) joint.attach(geom1.getBody(),geom2.getBody())
