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