def lidar(self, name, vx, vy, vz) :
"""
With a LIDAR-type instrument, query the distance to a solid object
in a direction
"""
body = self.bodies[name]
# create a ray facing the specified direction
ray = ode.GeomRay(self.space, rlen=100.0)
ray.set(body.getPosition(), (vx, vy, vz))
contacts = ode.collide(self.terrain_geom, ray)
for geom in self.bodies.itervalues() :
if geom != body :
contacts.extend( ode.collide(geom, ray) )
if contacts == [] : return -1.0 # no collisions
distances = []
px, py, pz = body.getPosition()
for contact in contacts :
pos, normal, depth, geom1, geom2 = contact.getContactGeomParams()
x, y, z = pos
distances.append((
(x - px) ** 2 + (y - py) ** 2 + (z - pz) ** 2
))
return min(distances) # returns closest collision
def update(theta, sos=443.0, pos=(0,0,0)): for i, v in enumerate(rays): angle = (2*math.pi*i/float(count)+theta) v.set(pos, (math.sin(angle), math.cos(angle), 0)) positions = [None]*count ray_lengths=[None]*count for i in xrange(count): collision=ode.collide(box, rays[i])+\ ode.collide(corridors[0], rays[i])+ode.collide(corridors[1], rays[i])+\ ode.collide(corridors[2], rays[i])+ode.collide(corridors[3], rays[i]) if len(collision) == 0: length = 1e6 #something absurdly large. positions[i] = None else: possibilities = [j.getContactGeomParams()[0] for j in collision] distances = [math.sqrt(sum((j[k]-pos[k])**2 for k in xrange(3))) for j in possibilities] needed_index = min([(j[1], j[0]) for j in enumerate(distances)])[1] positions[i] = possibilities[needed_index] length = distances[needed_index] ray_lengths[i]=length if length > 0 else 0.01 times=[i/float(sos) for i in ray_lengths] for time, delay, length, panner, i in zip(times, delays, ray_lengths, panners, xrange(count)): delay.delay = min(time, 9.0) delay.delay+=delay.delay*.20*random.random() delay.mul = min(1.0/length, 1.0) delay.feedback=feedback filters[i].frequency = freq_start+freq_range*delay.mul filters[i].q=freq_q panner.azimuth = 360.0*i/count #we must now compute and update the fdn feedback matrix and delays. new_feedbacks = [0.0]*(count**2) new_delays = [0.0]*count for i in xrange(count): #we calculate the distance between the points, divide by the speed of sound, and use that as the delay. p1 = positions[i] p2=positions[(i+1)%count] if p1 is not None and p2 is not None: dist = math.sqrt(sum([(p2[j]-p1[j])**2 for j in xrange(3)])) else: dist = 0.0 if dist == 0: dist = 0.01 new_delays[i] = dist/sos #same algorithm as the initial lines. new_feedbacks[i*count+((i+1)%count)]=min(1/dist**2, 0.4) # new_feedbacks[i*count+(i-1)%count] = min(1/dist**2, 0.4) fdn.set_delays(len(new_delays), new_delays) fdn.set_feedback_matrix(len(new_feedbacks), new_feedbacks) fdn.set_output_gains(count, [1.0/count]*count)
def _ode_collide_callback(self, args, geom1, geom2):
# Check if the objects do collide
contacts = ode.collide(geom1, geom2)
# Create contact joints
world, contactgroup = args
for c in contacts:
body1 = geom1.getBody()
body2 = geom2.getBody()
if hasattr(body1, 'is_output') and len(self.outputs()) > 0: # TODO flow logic needs to move out of Newton
self._remove_list.append(geom2.particle)
continue
if hasattr(body2, 'is_output') and len(self.outputs()) > 0:
self._remove_list.append(geom1.particle)
continue
c.setBounce(0.1) # 0.5
c.setMu(0.1) # 0.1
j = ode.ContactJoint(world, contactgroup, c)
j.attach(body1, body2)
# callback into container
if hasattr(body1, 'owner'): # get the Squiggle particle belonging to this ode body
#print "b1", body1, body1.owner
body1 = body1.owner
if hasattr(body2, 'owner'):
#print "b2", body2, body2.owner
body2 = body2.owner
self.collision(body1, body2)
def sphere_static_callback(game, sphere, static):
''' Callback function for collisions between spheres
and the static environment. This function checks
if the given geoms do collide and creates contact
joints if they do. '''
contact_group = game.get_contact_group()
world = game.get_world()
sphere_shape = sphere.object
static_shape = static.object
sphere_body = sphere.getBody()
static_body = static.getBody()
# Check if the objects do collide
contacts = ode.collide(sphere, static)
# Create contact joints
for c in contacts:
bounce = sqrt(sphere_shape.get_bounce() * static_shape.get_bounce())
friction = sqrt(sphere_shape.get_friction() * static_shape.get_friction()) * 1000
c.setBounce(bounce)
c.setMu(friction)
j = ode.ContactJoint(world, contact_group, c)
j.attach(sphere_body, static_body)
# Rolling friction
if contacts:
ang_vel = Vector(sphere_body.getAngularVel())
rolling_friction = sqrt(sphere_shape.get_rolling_friction() * static_shape.get_friction())
sphere_body.addTorque((-ang_vel * rolling_friction).value)
def __call__(self, args, geom1, geom2, gamma=1):
# raise NotImplementedError
if geom1.body_ref in geom2.no_contact:
return
world = args[0]
base_tensor = world.bodies[0].p
# pdb.set_trace()
contacts = ode.collide(geom1.body_ref, geom2.body_ref)
for c in contacts:
point, normal, penetration, geom1, geom2 = c.getContactGeomParams()
# XXX Simple disambiguation of 3D repetition of contacts
if point[2] > 0:
continue
normal = base_tensor.new_tensor(normal[:DIM])
point = base_tensor.new_tensor(point)
penetration = base_tensor.new_tensor([penetration])
penetration -= 2 * world.eps
if penetration.item() < -2 * world.eps:
return
p1 = point - base_tensor.new_tensor(geom1.getPosition())
p2 = point - base_tensor.new_tensor(geom2.getPosition())
world.contacts.append(((normal, p1[:DIM], p2[:DIM], penetration),
geom1.body, geom2.body))
world.contacts_debug = world.contacts # XXX
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):
"""
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 raiseGeoms(self):
"Raise all geoms above the ground"
total_offset = 0.0
min_z = None
# big raise
for g in self.space:
if g != self.ground:
(x, y, z) = g.getPosition()
if min_z == None:
min_z = z
else:
min_z = min(min_z, z)
if min_z != None:
self.moveBps(0, 0, -min_z)
total_offset += -min_z
log.debug('big model raise by %f', -min_z)
# small raises until all geoms above ground
incontact = 1
while incontact:
# is model in contact with the ground?
incontact = 0
for i in range(self.space.getNumGeoms()):
g = self.space.getGeom(i)
if g != self.ground:
(x, y, z) = g.getPosition()
incontact = ode.collide(self.ground, g)
if incontact:
break
if incontact:
self.moveBps(0, 0, 0.1)
total_offset += 0.1
log.debug('raised model to %f', total_offset)
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."""
# only check parse list, if objects have name
if geom1.name != None and geom2.name != None:
# Preliminary checking, only collide with certain objects
for p in self.passpairs:
g1 = False
g2 = False
for x in p:
g1 = g1 or (geom1.name.find(x) != -1)
g2 = g2 or (geom2.name.find(x) != -1)
if g1 and g2:
return()
# Check if the objects do collide
contacts = ode.collide(geom1, geom2)
# Create contact joints
world, contactgroup = args
for c in contacts:
p = c.getContactGeomParams()
# parameters from Niko Wolf
c.setBounce(0.2)
c.setBounceVel(0.05) #Set the minimum incoming velocity necessary for bounce
c.setSoftERP(0.6) #Set the contact normal "softness" parameter
c.setSoftCFM(0.00005) #Set the contact normal "softness" parameter
c.setSlip1(0.02) #Set the coefficient of force-dependent-slip (FDS) for friction direction 1
c.setSlip2(0.02) #Set the coefficient of force-dependent-slip (FDS) for friction direction 2
c.setMu(self.FricMu) #Set the Coulomb friction coefficient
j = ode.ContactJoint(world, contactgroup, c)
j.name = None
j.attach(geom1.getBody(), geom2.getBody())
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):
"""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()
# Contacts in the same group are ignored
# e.g. foot and lower leg
for group in allGroups:
if body1 in group and body2 in group:
return
# Check if the objects do collide
contacts = ode.collide(geom1, geom2)
# Create contact joints
self.world, self.contactgroup = args
for c in contacts:
c.setBounce(0.2)
c.setMu(5000)
j = ode.ContactJoint(self.world, self.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()
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 insertCollision(self, geom):
#objs = iter(self.obj)
_collide = lambda o: ode.collide(geom, o.geom)
for obj in self.obj:
if _collide(obj):
return True
return 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."""
# only check parse list, if objects have name
if geom1.name != None and geom2.name != None:
# Preliminary checking, only collide with certain objects
for p in self.passpairs:
g1 = False
g2 = False
for x in p:
g1 = g1 or (geom1.name.find(x) != -1)
g2 = g2 or (geom2.name.find(x) != -1)
if g1 and g2:
return()
# Check if the objects do collide
contacts = ode.collide(geom1, geom2)
# Create contact joints
world, contactgroup = args
for c in contacts:
p = c.getContactGeomParams()
# parameters from Niko Wolf
c.setBounce(0.2)
c.setBounceVel(0.05) #Set the minimum incoming velocity necessary for bounce
c.setSoftERP(0.6) #Set the contact normal "softness" parameter
c.setSoftCFM(0.00005) #Set the contact normal "softness" parameter
c.setSlip1(0.02) #Set the coefficient of force-dependent-slip (FDS) for friction direction 1
c.setSlip2(0.02) #Set the coefficient of force-dependent-slip (FDS) for friction direction 2
c.setMu(self.FricMu) #Set the Coulomb friction coefficient
j = ode.ContactJoint(world, contactgroup, c)
j.name = None
j.attach(geom1.getBody(), geom2.getBody())
def _near_callback(self, args, geom1, geom2):
world, contactgroup = args
try:
i = self.ball_geoms.index(geom1)
if not self.on_table[i]:
return
except:
pass
try:
j = self.ball_geoms.index(geom2)
if not self.on_table[j]:
return
except:
pass
contacts = ode.collide(geom1, geom2)
if contacts:
body1, body2 = geom1.getBody(), geom2.getBody()
for c in contacts:
if isinstance(geom1, ode.GeomPlane) or isinstance(
geom2, ode.GeomPlane):
c.setBounce(0.24)
c.setMu(0.16)
c.setBounceVel(0.033)
c.setSoftERP(0.4)
c.setSoftCFM(1e4)
c.setSlip1(0.05)
elif isinstance(geom1, ode.GeomTriMesh) or isinstance(
geom2, ode.GeomTriMesh):
c.setBounce(0.86)
c.setMu(0.16)
c.setBounceVel(0.02)
c.setSoftERP(0.4)
c.setSoftCFM(1e2)
c.setSlip1(0.04)
elif isinstance(geom1, ode.GeomCylinder) or isinstance(
geom2, ode.GeomCylinder):
c.setBounce(0.69)
c.setMu(0.25)
c.setSoftERP(0.2)
c.setSoftCFM(1e1)
pos, normal, depth, g1, g2 = c.getContactGeomParams()
v_n = np.array(normal).dot(
np.array(body1.getPointVel(pos)) -
np.array(body2.getPointVel(pos)))
if self._on_cue_ball_collide:
self._on_cue_ball_collide(impact_speed=abs(v_n))
else:
c.setBounce(0.93)
c.setMu(0.13)
pos, normal, depth, g1, g2 = c.getContactGeomParams()
v_n = abs(
np.array(normal).dot(
np.array(body1.getLinearVel()) -
np.array(body2.getLinearVel())))
vol = max(0.02, min(0.8, 0.45 * v_n + 0.55 * v_n**2))
if vol > 0.02:
play_ball_ball_collision_sound(vol=vol)
j = ode.ContactJoint(world, contactgroup, c)
j.attach(body1, body2)
def near_callback(args, g0, g1):
print "Click!"
contacts = ode.collide(g0, g1)
world, contactgroup = args
for c in contacts:
# Various collision parameters can be set here
j = ode.ContactJoint(world, contactgroup, c)
j.attach(g0.getBody(), g1.getBody())
def _rayCollideCallback(self, args, geom1, geom2):
contacts = ode.collide(geom1, geom2)
if len(contacts) > 0:
if isinstance(geom1, ode.GeomRay):
theRay = geom1
elif isinstance(geom2, ode.GeomRay):
raise RuntimeError('I didn' 't think ray==geom2 was possible')
self.currentRayContacts[theRay] += (contacts)
def nearCallback(self, args, geom1, geom2):
contacts = ode.collide(geom1, geom2)
for c in contacts:
c.setBounce(0.2)
c.setMu(5000)
j = ode.ContactJoint(self.world, self.contactGroup, c)
j.attach(geom1.getBody(), geom2.getBody())
def nearCallback(self, args, geom1, geom2):
contacts = ode.collide(geom1, geom2)
for c in contacts:
c.setBounce(0.2)
c.setMu(5000)
j = ode.ContactJoint(self.world, self.contactGroup, c)
j.attach(geom1.getBody(), geom2.getBody())
def near_callback(args, geom1, geom2):
contacts = ode.collide(geom1, geom2)
for contact in contacts:
contact.setMode(ode.ContactBounce)
contact.setMu(ode.Infinity)
contact.setBounce(0.5)
joint = ode.ContactJoint(world, contactgroup, contact)
joint.attach(geom1.getBody(), geom2.getBody())
def near_callback(args, geom1, geom2):
contacts = ode.collide(geom1, geom2)
world,contactgroup = args
for c in contacts:
c.setBounce(0.1)
c.setMu(10000)
j = ode.ContactJoint(world, contactgroup, c)
j.attach(geom1.getBody(), geom2.getBody())
def callback(args,g1,g2):
contacts = ode.collide(g1,g2)
world,cgroup = args
for c in contacts:
c.setBounce(.9)
c.setMu(1000)
j = ode.ContactJoint(world,cgroup,c)
j.attach(g1.getBody(),g2.getBody())
def collide(self, args, geom1, geom2):
contacts = ode.collide(geom1, geom2)
world,contact_group = args
for c in contacts:
c.setBounce(0.0)
c.setMu(5000)
j = ode.ContactJoint(world, contact_group, c)
j.attach(geom1.getBody(), geom2.getBody())
def nearCallback(self, args, geom1, geom2):
contacts = ode.collide(geom1, geom2)
for c in contacts:
if self.collideCallback is not None:
self.collideCallback(c)
j = ode.ContactJoint(self.oWorld, self.oContactGroup, c)
j.attach(geom1.getBody(), geom2.getBody())
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 _near_callback(self, args, geom1, geom2) :
contacts = ode.collide(geom1, geom2)
# Create contact joints
world,contactgroup = args
for c in contacts:
c.setBounce(0.02)
c.setMu(1000.0)
j = ode.ContactJoint(world, contactgroup, c)
j.attach(geom1.getBody(), geom2.getBody())
def _onCollision(self, args, geom1, geom2):
contacts = ode.collide(geom1, geom2)
world, contact_group = args
for c in contacts:
c.setBounce(self._contact_bounce)
c.setMu(self._contact_friction)
j = ode.ContactJoint(world, contact_group, c)
j.attach(geom1.getBody(), geom2.getBody())
def near_callback(args, g0, g1):
contacts = ode.collide(g0, g1)
world, contactgroup = args
for c in contacts:
c.setBounce(1)
c.setMu(0)
c.setMu2(0)
j = ode.ContactJoint(world, contactgroup, c)
j.attach(g0.getBody(), g1.getBody())
def Collision_Callback(args, geom1, geom2):
contacts = ode.collide(geom1, geom2)
world, contactgroup = args
for c in contacts:
c.setBounce(0) # 反発係数
c.setMu(2) # クーロン摩擦係数
j = ode.ContactJoint(world, contactgroup, c)
j.attach(geom1.getBody(), geom2.getBody())
global Col
Col=True
def _collidecallback(cls, args, geom1, geom2):
go1 = geom1.gameobject
go2 = geom2.gameobject
go1.oncollision(go2)
go2.oncollision(go1)
for contact in ode.collide(geom1, geom2):
contact.setBounce(0)
contact.setMu(10000)
j = ode.ContactJoint(cls.world, cls.contactgroup, contact)
j.attach(geom1.getBody(), geom2.getBody())
def near_callback(self, _, geom1, geom2):
# Check if the objects do collide
contacts = collide(geom1, geom2)
# Create contact joints
for contact in contacts:
contact.setBounce(0.01)
contact.setMu(2000)
joint = ContactJoint(self.world, self.contact_group, contact)
joint.attach(geom1.getBody(), geom2.getBody())
def _collision(self, args, geom1, geom2):
# Check if the objects do collide
contacts = ode.collide(geom1, geom2)
# Create contact joints
world, contactgroup = args
for c in contacts:
c.setBounce(0.1)
c.setMu(50000)
j = ode.ContactJoint(world, contactgroup, c)
j.attach(geom1.getBody(), geom2.getBody())
def player_interactive_object_callback(game, player_geom, obj_geom):
obj = obj_geom.object
contacts = ode.collide(player_geom, obj_geom)
obj.set_pressed(False)
if contacts:
obj.set_pressed(True)
def near_callback(args, geometria1, geometria2):
contacto = ode.collide(geometria1, geometria2)
mundo, grupo_contacto = args
for c in contacto:
c.setBounce(0.2)
c.setMu(5000)
j = ode.ContactJoint(mundo, grupo_contacto, c)
j.attach(geometria1.getBody(), geometria2.getBody())
def near_callback(args, geom1, geom2):
if not (type(geom1) == ode.GeomSphere or type(geom2) == ode.GeomSphere):
return
contacts = ode.collide(geom1, geom2)
world, contactgroup = args
for c in contacts:
c.setBounce(1.00)
c.setMu(0)
j = ode.ContactJoint(world, contactgroup, c)
j.attach(geom1.getBody(), geom2.getBody())
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 near_callback(self, args, geom1, geom2):
# Check if the objects do collide
contacts = ode.collide(geom1, geom2)
# Create contact joints
self.world, self.contactgroup = args
for c in contacts:
c.setBounce(0.2)
c.setMu(5000)
j = ode.ContactJoint(self.world, self.contactgroup, c)
j.attach(geom1.getBody(), geom2.getBody())
def wheel_contact_callback(args, geom1,
geom2): #GENERIC collision (like webots)
if geom1 == geom2: #geom2 is the wheel!
return
contacts = ode.collide(geom1, geom2) # Check if the objects do collide
world, contactgroup = args
for c in contacts: # Create contact joints
c.setBounce(0.2)
c.setMu(10) #is friction?
j = ode.ContactJoint(world, contactgroup, c)
j.attach(None, geom2.getBody())
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.
"""
# Check if the objects do collide
contacts = ode.collide(geom1, geom2)
if(len(contacts)>0):
self.targetTime+=self.dt
else:
self.targetTime=0
def find_collision(self): """EDIT: Runs through all particles in particle_list to look for overlapping particles Needs to check and see if particle has a rod connection and if it does, call check_rod_constraint """ self.has_collided = [] #Reset collisions self.boundary() #See if any particles collide with the wall for p in combinations(self.particle_list,2): contact = ode.collide(p[0].geom, p[1].geom) if contact != []: rc = contact[0].getContactGeomParams()[0] n_hat = contact[0].getContactGeomParams()[1] dx = contact[0].getContactGeomParams()[2]
def near_callback(args, geom1, geom2):
"""Callback function for the collide() method below.
This function checks if the given geoms do collide and
creates contact joints if they do.
"""
contacts = ode.collide(geom1, geom2)
world,contactgroup = args
for c in contacts:
c.setBounce(0.01)
c.setMu(60000)
j = ode.ContactJoint(world, contactgroup, c)
j.attach(geom1.getBody(), geom2.getBody())
def contact_callback(args, geom1, geom2): #GENERIC collision (like webots)
if geom1 == geom2:
return
#print "generic two-way contact"
contacts = ode.collide(geom1, geom2) # Check if the objects do collide
world, contactgroup = args
for c in contacts: # Create contact joints
c.setBounce(0.2)
c.setMu(5000) #is friction?
j = ode.ContactJoint(world, contactgroup, c)
j.attach(geom1.getBody(), geom2.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 near_callback(self, xxx_todo_changeme, geom1, geom2):
'''Callback function for the collide() method.
This function checks if the given geoms do collide and
creates contact joints if they do.
'''
(world, contact_group) = xxx_todo_changeme
contacts = ode.collide(geom1, geom2)
# Create contact joints
for c in contacts:
c.setBounce(.4)
c.setMu(5000)
j = ode.ContactJoint(world, contact_group, c)
j.attach(geom1.getBody(), geom2.getBody())
def oneway_contact_callback(args, geom1,
geom2): #ignore opposite force on environment
if geom1 == geom2: #geom2 is the object to experience the force
return
#print "oneway contact"
contacts = ode.collide(geom1, geom2) # Check if the objects do collide
world, contactgroup = args
for c in contacts: # Create contact joints
c.setBounce(0.2)
c.setMu(0) #no friction
j = ode.ContactJoint(world, contactgroup, c)
j.attach(None, geom2.getBody(
)) #None=link to environment instead of other body (see ODE FAQ)
def near_callback(args, geometria1, geometria2):
contacto = ode.collide(geometria1, geometria2)
mundo, grupo_contacto = args
for c in contacto:
c.setBounce(REBOTE)
c.setMu(MU)
c.setMu2(MU2)
c.setBounceVel(REBOTE_VEL)
c.setSoftCFM(SOFT_CFM)
j = ode.ContactJoint(mundo, grupo_contacto, c)
j.attach(geometria1.getBody(), geometria2.getBody())
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())
def onNearCollision(args, geom1, geom2):
"""
taken directly from pyode tutorial3
"""
# Check if the objects do collide
contacts = ode.collide(geom1, geom2)
# Create contact joints
world,contactgroup = args
for c in contacts:
c.setBounce(0.05)
c.setMu(5000)
j = ode.ContactJoint(world, contactgroup, c)
j.attach(geom1.getBody(), geom2.getBody())
def collide(self, args, geom1, geom2):
contacts = ode.collide(geom1, geom2)
# print type(geom1), type(geom2)
if type(geom1) == ode.GeomBox:
self.detect_interaction(geom1, geom2)
elif type(geom2) == ode.GeomBox:
self.detect_interaction(geom2, geom1)
else:
self.detect_death(geom1, geom2)
for c in contacts:
c.setBounce(0.2)
c.setMu(5000)
j = ode.ContactJoint(self.world, self.contact_group, c)
j.attach(geom1.getBody(), geom2.getBody())
def ode_collide_callback(self, args, geom1, geom2):
# Check if the objects do collide
contacts = ode.collide(geom1, geom2)
# Create contact joints
world, contactgroup = args
for c in contacts:
#c.setBounce(0.99)
c.setBounce(0.25)
c.setMu(0.1)
j = ode.ContactJoint(world, contactgroup, c)
j.attach(geom1.getBody(), geom2.getBody())
def _near_callback(self, geom1, geom2):
"""Callback function for the collide() method.
This function checks if the given self.geoms do collide and
creates contact joints if they do.
"""
# Set of geoms
gset = {geom1, geom2}
# Check if the objects do collide
contacts = ode.collide(geom1, geom2)
# Create contact joints
for c in contacts:
c.setBounce(0.77) # standard bounce pingpong labda + asztal kozott
c.setMu(0.25) # approx mu
j = ode.ContactJoint(self.world, self.contactGroup, c)
j.attach(geom1.getBody(), geom2.getBody())
if {self.lastBall} <= gset:
if not {self.tableNet, self.hitFloor}.isdisjoint(gset):
self.hitState = self.BallState.invalidBounce
elif {self.hitWall} <= gset:
if self.hitState == self.BallState.nearHit:
self.hitState = self.BallState.wallHit
else:
self.hitState = self.BallState.invalidBounce
elif {self.tableFar} <= gset:
if self.hitState == self.BallState.noneHit:
self.hitState = self.BallState.farHit
elif self.hitState == self.BallState.batHit:
self.hitState = self.BallState.farSecondHit
else:
self.hitState = self.BallState.invalidBounce
elif {self.tableNear} <= gset:
if self.hitState == self.BallState.farHit:
self.hitState = self.BallState.nearHit
elif self.hitState == self.BallState.batHit:
self.hitState = self.BallState.nearSecondHit
else:
self.hitState = self.BallState.invalidBounce
elif {self.bat} <= gset:
if self.hitState == self.BallState.nearHit:
self.hitState = self.BallState.batHit
else:
self.hitState = self.BallState.invalidHit
elif {self.bat, self.tableNear} == gset:
self.hitState = self.BallState.invalidHit
def nearCallback(self, args, geom1, geom2):
try:
# Force collision event to appear in (mat1, mat2) order
# (i.e. the order in which the material pair was defined in contactprops)
if not (geom1.material, geom2.material) in self.contactprops:
(geom1, geom2) = (geom2, geom1)
except:
#print "ups"
pass
# Check if the objects do collide
contacts = ode.collide(geom1, geom2)
# No contacts? then return immediately
if len(contacts) == 0:
return
# print len(contacts),"contacts"
# Get the contact properties
cp = self.getContactProperties((geom1.material, geom2.material))
# Create a collision event?
if self.collision_events:
obj1 = getattr(geom1, "worldobj", None)
obj2 = getattr(geom2, "worldobj", None)
evt = ODECollisionEvent(obj1, obj2, contacts, cp)
self.eventmanager.event(ODE_COLLISION, evt)
# Some event handler could change the number of contacts (simplify them)
self.numcontacts += len(contacts)
cp = cp.getSurfaceParamsList()
# Create contact joints
for c in contacts:
if self.show_contacts:
p, n, d, g1, g2 = c.getContactGeomParams()
cmds.drawMarker(p,
size=self.contactmarkersize,
color=(1, 0, 0))
cmds.drawLine(p,
vec3(p) + self.contactnormalsize * vec3(n),
color=(1, 0.5, 0.5))
# print p,n,d
ode.CreateContactJointQ(self.world, self.contactgroup, c, cp,
geom1, geom2)
def get_dvl_range(self):
"""Returns the range of each ray, otherwise 0 represents failure to contact the floor
For each dvl sensor ray object, collisions with the floor are detected,
and parameters from the contact.getContactGeomParams() method
are used to calculate the range of the dvl sensor to the floor
"""
ranges = np.array([0.0, 0.0, 0.0, 0.0])
for n, ray in enumerate(self.dvl_rays):
for contact in ode.collide(ray, self.space):
pos, normal, depth, geom1, geom2 = contact.getContactGeomParams()
assert geom1 is ray, geom1
if (geom2 is self.geom):
continue
ranges[n] = np.min(depth, ranges[n]) # The closest object!
return ranges
def _collision_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.
"""
#print 'Collision detected'
# Check if the objects do collide
contacts = ode.collide(geom1, geom2)
# Create contact joints
world,contactgroup = args
for c in contacts:
c.setBounce(self.bounce) # Restitution parameter
c.setMu(self.friction) # Coulomb friction
j = ode.ContactJoint(world, contactgroup, c)
j.attach(geom1.getBody(), geom2.getBody())
