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