class RocketPhys(Phys): def __init__(self, mdt, parent): Phys.__init__(self, mdt) self.parent = parent def fire(self): self.node = BulletRigidBodyNode('Box') self.node.setMass(10000) self.node.addShape(BulletSphereShape(.5)) self.np = self.parent.attachNewNode(self.node) self.np.setPos(0, 0, 1.5) eng.attach_rigid_body(self.node) self.mdt.gfx.gfx_np.reparentTo(self.np) self.mdt.gfx.gfx_np.setPos(0, 0, 0) rot_mat = Mat4() rot_mat.setRotateMat(self.parent.get_h(), (0, 0, 1)) self.node.setLinearVelocity(rot_mat.xformVec((0, 30, 0))) # continue to apply after firing def destroy(self): if hasattr(self, 'node'): # has not been fired eng.remove_rigid_body(self.node) self.np = self.np.remove_node() self.parent = None Phys.destroy(self)
class Shoot(): def __init__(self, showbase): self.showbase = showbase self.actor = showbase.actor print('Press "q" to shoot') showbase.accept('q', self.createBall) def createBall(self): self.actor.play("launch") # Sphere self.shape = BulletSphereShape(0.15) self.node = BulletRigidBodyNode('Sphere') self.node.setMass(1) self.node.addShape(self.shape) self.np = self.showbase.render.attachNewNode(self.node) self.np.setPos(self.actor.getX(), self.actor.getY(), self.actor.getZ() + 2.5) # Smiley self.model = self.showbase.loader.loadModel("./models/smiley") self.model.setTexture( self.showbase.loader.loadTexture(imagePath + 'metal1.jpg'), 1) self.model.flattenLight() self.model.setScale(0.15, 0.15, 0.15) self.model.reparentTo(self.np) # Set speed h = self.actor.getH() if (h == 0): x = 0 y = 8 if (h == 90): x = -7 y = 0 if (h == 180): x = 0 y = -8 if (h == -90): x = 8 y = 0 self.node.setLinearVelocity(Vec3(x, y, 6.2)) #self.actor.getX()/3, 11, 6.2) self.showbase.world.attachRigidBody(self.node)
class Shoot( ) : def __init__(self, showbase) : self.showbase = showbase self.actor = showbase.actor print('Press "q" to shoot') showbase.accept('q', self.createBall) def createBall(self) : self.actor.play("launch") # Sphere self.shape = BulletSphereShape(0.15) self.node = BulletRigidBodyNode('Sphere') self.node.setMass(1) self.node.addShape(self.shape) self.np = self.showbase.render.attachNewNode(self.node) self.np.setPos(self.actor.getX(), self.actor.getY(), self.actor.getZ() + 2.5) # Smiley self.model = self.showbase.loader.loadModel("./models/smiley") self.model.setTexture(self.showbase.loader.loadTexture(imagePath + 'metal1.jpg'), 1) self.model.flattenLight() self.model.setScale(0.15, 0.15, 0.15) self.model.reparentTo(self.np) # Set speed h = self.actor.getH() if(h == 0) : x = 0 y = 8 if(h == 90) : x = -7 y = 0 if(h == 180) : x = 0 y = -8 if(h == -90) : x = 8 y = 0 self.node.setLinearVelocity(Vec3(x, y, 6.2)) #self.actor.getX()/3, 11, 6.2) self.showbase.world.attachRigidBody(self.node)
class EscapeFromJCMB(object, DirectObject): def __init__(self): print "Loading..." self.init_window() self.init_music() self.init_bullet() self.init_key() self.load_world() self.init_player() self.init_objects() render.prepareScene(base.win.getGsg()) print "Done" self.start_physics() def init_window(self): # Hide the mouse cursor props = WindowProperties() props.setCursorHidden(True) base.win.requestProperties(props) def init_music(self): music = base.loader.loadSfx("../data/snd/Bent_and_Broken.mp3") music.play() self.playferscream = base.loader.loadSfx("../data/snd/deadscrm.wav") def init_bullet(self): self.world = BulletWorld() self.world.setGravity(Vec3(0, 0, -9.81)) # debugNode = BulletDebugNode('Debug') # debugNode.showWireframe(True) # debugNode.showConstraints(True) # debugNode.showBoundingBoxes(False) # debugNode.showNormals(False) # debugNP = render.attachNewNode(debugNode) # debugNP.show() # self.world.setDebugNode(debugNP.node()) alight = AmbientLight('alight') alight.setColor(VBase4(0.05, 0.05, 0.05, 1)) alnp = render.attachNewNode(alight) render.setLight(alnp) def init_key(self): # Stores the state of the keys, 1 for pressed and 0 for unpressed self.key_state = { 'up': False, 'right': False, 'down': False, 'left': False } # Assign the key event handler self.accept('w', self.set_key_state, ['up', True]) self.accept('w-up', self.set_key_state, ['up', False]) self.accept('d', self.set_key_state, ['right', True]) self.accept('d-up', self.set_key_state, ['right', False]) self.accept('s', self.set_key_state, ['down', True]) self.accept('s-up', self.set_key_state, ['down', False]) self.accept('a', self.set_key_state, ['left', True]) self.accept('a-up', self.set_key_state, ['left', False]) # Stores the state of the mouse buttons, 1 for pressed and 0 for unpressed self.mouse_state = {'left_click': False, 'right_click': False} # Assign the mouse click event handler self.accept('mouse1', self.set_mouse_state, ['left_click', True]) self.accept('mouse1-up', self.set_mouse_state, ['left_click', False]) self.accept('mouse2', self.set_mouse_state, ['right_click', True]) self.accept('mouse2-up', self.set_mouse_state, ['right_click', False]) self.accept('z', self.print_pos) # Esc self.accept('escape', sys.exit) def set_key_state(self, key, state): self.key_state[key] = state def set_mouse_state(self, button, state): self.mouse_state[button] = state def print_pos(self): print self.playernp.getPos() def egg_to_BulletTriangleMeshShape(self, modelnp): mesh = BulletTriangleMesh() for collisionNP in modelnp.findAllMatches('**/+CollisionNode'): collisionNode = collisionNP.node() for collisionPolygon in collisionNode.getSolids(): tri_points = collisionPolygon.getPoints() mesh.addTriangle(tri_points[0], tri_points[1], tri_points[2]) shape = BulletTriangleMeshShape(mesh, dynamic=False) return shape def load_world(self): stairwell = loader.loadModel('../data/mod/jcmbstairs.egg') stairwell.reparentTo(render) stairwell_shape = self.egg_to_BulletTriangleMeshShape(stairwell) stairwellnode = BulletRigidBodyNode('stairwell') stairwellnode.addShape(stairwell_shape) self.world.attachRigidBody(stairwellnode) def init_player(self): # Stop the default mouse behaviour base.disableMouse() # Character has a capsule shape shape = BulletCapsuleShape(0.2, 1, ZUp) self.player = BulletRigidBodyNode('Player') self.player.setMass(80.0) self.player.addShape(shape) self.playernp = render.attachNewNode(self.player) self.playernp.setPos(0, 0, 1) self.world.attachRigidBody(self.player) self.player.setLinearSleepThreshold(0.0) self.player.setAngularFactor(0.0) self.player_speed = 3 self.player_is_grabbing = False # self.head = BulletRigidBodyNode('Player Head') # self.head.addShape(BulletSphereShape(0.2)) # self.head.setMass(10) # self.head.setInertia(Vec3(1,1,1)) # self.head.setAngularFactor(1.0) # self.head.setLinearFactor(0.0) # self.headnp = self.playernp.attachNewNode(self.head) # self.headnp.setPos(0,0,0) # self.headnp.setCollideMask(BitMask32.allOff()) # self.world.attachRigidBody(self.head) # Attach the camera to the player's head base.camera.reparentTo(self.playernp) # base.camera.setPos(0,0,.5) base.camLens.setFov(80) base.camLens.setNear(0.01) # Make the torch and attach it to our character torch = Spotlight('torch') torch.setColor(VBase4(1, 1, 1, 1)) lens = PerspectiveLens() lens.setFov(80) lens.setNearFar(20, 100) torch.setLens(lens) self.torchnp = base.camera.attachNewNode(torch) self.torchnp.setPos(0, 0, 0) # Allow the world to be illuminated by our torch render.setLight(self.torchnp) self.cs = None # Player's "hand" in the world hand_model = loader.loadModel('../data/mod/hand.egg') hand_model.setScale(1) hand_model.setBillboardPointEye() self.hand = BulletRigidBodyNode('Hand') self.hand.addShape(BulletSphereShape(0.1)) self.hand.setLinearSleepThreshold(0.0) self.hand.setLinearDamping(0.9999999) self.hand.setAngularFactor(0.0) self.hand.setMass(1.0) self.world.attachRigidBody(self.hand) self.handnp = render.attachNewNode(self.hand) self.handnp.setCollideMask(BitMask32.allOff()) # hand_model.reparentTo(self.handnp) # Create a text node to display object identification information and attach it to the player's "hand" self.hand_text = TextNode('Hand Text') self.hand_text.setText("Ch-ch-chek yoself befo yo rek yoself, bro.") self.hand_text.setAlign(TextNode.ACenter) self.hand_text.setTextColor(1, 1, 1, 1) self.hand_text.setShadow(0.05, 0.05) self.hand_text.setShadowColor(0, 0, 0, 1) self.hand_text_np = render.attachNewNode(self.hand_text) self.hand_text_np.setScale(0.03) self.hand_text_np.setBillboardPointEye() self.hand_text_np.hide() # Disable the depth testing for the hand and the text - we always want these things on top, with no clipping self.handnp.setDepthTest(False) self.hand_text_np.setDepthTest(False) # Add the player update task taskMgr.add(self.update, 'update_player_task') def init_objects(self): # Make Playfer Box shape = BulletBoxShape(Vec3(0.25, 0.25, 0.25)) node = BulletRigidBodyNode('Playfer Box') node.setMass(110.0) node.setFriction(1.0) node.addShape(shape) node.setAngularDamping(0.0) np = render.attachNewNode(node) np.setPos(-1.4, 1.7, -1.7) self.world.attachRigidBody(node) playferboxmodel = loader.loadModel('../data/mod/playferbox.egg') playferboxmodel.reparentTo(np) # Make Pendlepot shape = BulletBoxShape(Vec3(0.2, 0.15, 0.1)) node = BulletRigidBodyNode('Pendlepot') node.setMass(5.0) node.setFriction(1.0) node.addShape(shape) node.setAngularDamping(0.0) np = render.attachNewNode(node) np.setPos(-1.4, 1.7, -1.5) self.world.attachRigidBody(node) pendlepotmodel = loader.loadModel('../data/mod/pendlepot.egg') pendlepotmodel.reparentTo(np) def update(self, task): # Update camera orientation md = base.win.getPointer(0) mouse_x = md.getX() mouse_y = md.getY() centre_x = base.win.getXSize() / 2 centre_y = base.win.getYSize() / 2 if base.win.movePointer(0, centre_x, centre_y): new_H = base.camera.getH() + (centre_x - mouse_x) new_P = base.camera.getP() + (centre_y - mouse_y) if new_P < -90: new_P = -90 elif new_P > 90: new_P = 90 base.camera.setH(new_H) base.camera.setP(new_P) # Update player position speed = 3 self.player_is_moving = False if (self.key_state["up"] == True): self.player_is_moving = True dir = 0 if (self.key_state["down"] == True): self.player_is_moving = True dir = 180 if (self.key_state["left"] == True): self.player_is_moving = True dir = 90 if (self.key_state["right"] == True): self.player_is_moving = True dir = 270 self.player.clearForces() old_vel = self.player.getLinearVelocity() new_vel = Vec3(0, 0, 0) if self.player_is_moving == True: new_vel.setX(-speed * math.sin( (base.camera.getH() + dir) * 3.1415 / 180.0)) new_vel.setY(speed * math.cos( (base.camera.getH() + dir) * 3.1415 / 180.0)) timescale = 0.001 linear_force = (new_vel - old_vel) / (timescale) linear_force.setZ(0.0) self.player.applyCentralForce(linear_force) if self.player_is_grabbing == False: new_hand_pos = LPoint3f( render.getRelativePoint(base.camera, Vec3(0, 0.2, 0))) self.handnp.setPos(new_hand_pos) else: new_hand_pos = LPoint3f( render.getRelativePoint(base.camera, Vec3(0, 0.5, 0))) diff = new_hand_pos - self.handnp.getPos() self.hand.applyCentralForce(diff * 1000 - self.hand.getLinearVelocity() * 100) if diff.length() > .5: self.player.setLinearVelocity(Vec3(0, 0, 0)) # Identify what lies beneath the player's hand (unless player is holding something) if self.player_is_grabbing == False: ray_from = self.playernp.getPos() ray_to = LPoint3f( render.getRelativePoint(base.camera, Vec3(0, 1, 0))) result = self.world.rayTestClosest(ray_from, ray_to) if result.hasHit() == True: self.hand_text.setText(result.getNode().getName()) self.hand_text_np.setPos(result.getHitPos()) self.hand_text_np.show() # If player clicks, grab the object by the nearest point (as chosen by ray) if self.mouse_state["left_click"] == True: if result.getNode().getNumChildren() == 1: obj = NodePath(result.getNode().getChild(0)) if self.player_is_grabbing == False: self.player_is_grabbing = True # Find the position of contact in terms of the object's local coordinates. # Parent the player's hand to the grabbed object at that position. pos = obj.getRelativePoint(render, result.getHitPos()) self.grabbed_node = result.getNode() self.grabbed_node.setActive(True) print self.grabbed_node frameA = TransformState.makePosHpr( Vec3(0, 0, 0), Vec3(0, 0, 0)) frameB = TransformState.makePosHpr( Vec3(0, 0, 0), Vec3(0, 0, 0)) swing1 = 20 # degrees swing2 = 20 # degrees twist = 20 # degrees self.cs = BulletConeTwistConstraint( self.hand, result.getNode(), frameA, frameB) self.cs.setLimit(swing1, swing2, twist) self.world.attachConstraint(self.cs) # Stop the held object swinging all over the place result.getNode().setAngularDamping(0.7) else: self.hand_text_np.hide() self.player_is_grabbing = False if self.mouse_state["left_click"] == False: self.player_is_grabbing = False if self.cs != None: self.world.remove_constraint(self.cs) self.cs = None self.grabbed_node.setAngularDamping(0.0) if self.player_is_grabbing == True and self.mouse_state[ "right_click"] == True: self.world.remove_constraint(self.cs) self.cs = None self.grabbed_node.setAngularDamping(0.0) self.grabbed_node.setActive(True) self.grabbed_node.applyCentralImpulse(1000, 0, 0) if self.player_is_grabbing == True: self.hand_text_np.hide() return task.cont def update_physics(self, task): dt = globalClock.getDt() self.world.doPhysics(dt) return task.cont def start_physics(self): taskMgr.add(self.update_physics, 'update_physics')
class Flame(Entity): """ The thing that comes out the end of the thing you hold """ animspeed = 0.1 depth = 20 playerWidth = 3 speed = 20 def __init__(self, world, pos, hpr): super(Flame, self).__init__() self.shape = BulletBoxShape(Vec3(0.1,0.05,0.05)) self.bnode = BulletRigidBodyNode() self.bnode.setMass(1.0) self.bnode.addShape(self.shape) self.np = utilities.app.render.attachNewNode(self.bnode) self.world =world self.anim = list() self.anim.append(utilities.loadObject("flame1", depth=0)) self.anim.append(utilities.loadObject("flame2", depth=0)) self.anim.append(utilities.loadObject("flame3", depth=0)) world.bw.attachRigidBody(self.bnode) self.curspr = 0 self.obj = self.anim[self.curspr] self.obj.show() self.livetime = 0 self.delta = 0 self.pos = pos self.pos.y = Flame.depth #self.pos.z -= 0.2 self.hpr = hpr self.vel = Point2() self.vel.x = cos(world.player.angle)*Flame.speed self.vel.y = sin(world.player.angle)*Flame.speed tv = Vec3(self.vel.x, 0, self.vel.y) # this makes the shot miss the target if the player has any velocity tv += world.player.bnode.getLinearVelocity() self.bnode.setLinearVelocity(tv) tv.normalize() # initial position of RB and draw plane self.np.setHpr(hpr) self.np.setPos(pos+tv/2) self.bnode.setAngularFactor(Vec3(0,0,0)) self.bnode.setLinearFactor(Vec3(1,0,1)) self.bnode.setGravity(Vec3(0,0,0)) self.bnode.setCcdMotionThreshold(1e-7) self.bnode.setCcdSweptSphereRadius(0.10) self.bnode.notifyCollisions(True) self.bnode.setIntoCollideMask(BitMask32.bit(1)) self.bnode.setPythonTag("Entity", self) self.noCollideFrames = 4 for a in self.anim: a.hide() a.reparentTo(self.np) a.setScale(0.25, 1, 0.25) a.setPos(0, -0.1,0) def update(self, timer): #animation self.delta += timer self.livetime += timer if self.noCollideFrames == 0: self.bnode.setIntoCollideMask(BitMask32.allOn()) if self.noCollideFrames > -1: self.noCollideFrames -= 1 if self.delta > Flame.animspeed: self.delta = 0 self.obj.hide() self.curspr += 1 if self.curspr > len(self.anim)-1: self.curspr = 0 self.obj = self.anim[self.curspr] self.obj.show()
class Flame(Entity): """ The thing that comes out the end of the thing you hold """ animspeed = 0.1 depth = 20 playerWidth = 3 speed = 30 maxlife = 10 damage = 30 def __init__(self, world, pos, hpr): super(Flame, self).__init__() self.shape = BulletBoxShape(Vec3(0.1, 0.05, 0.05)) self.bnode = BulletRigidBodyNode() self.bnode.setMass(0.00001) self.bnode.addShape(self.shape) self.np = utilities.app.render.attachNewNode(self.bnode) self.remove = False self.world = world self.anim = list() self.anim.append(utilities.loadObject("flame1", depth=0)) self.anim.append(utilities.loadObject("flame2", depth=0)) self.anim.append(utilities.loadObject("flame3", depth=0)) world.bw.attachRigidBody(self.bnode) self.curspr = 0 self.livetime = 0 self.delta = 0 self.pos = pos self.pos.y = Flame.depth self.hpr = hpr self.vel = Point2() self.vel.x = cos(world.player.angle) * Flame.speed self.vel.y = sin(world.player.angle) * Flame.speed tv = Vec3(self.vel.x, 0, self.vel.y) # this makes the shot miss the target if the player has any velocity tv += world.player.bnode.getLinearVelocity() self.bnode.setLinearVelocity(tv) tv.normalize() # initial position of RB and draw plane self.np.setHpr(hpr) self.np.setPos(pos + tv / 2) self.bnode.setAngularFactor(Vec3(0, 0, 0)) self.bnode.setLinearFactor(Vec3(1, 0, 1)) self.bnode.setGravity(Vec3(0, 0, 0)) #self.bnode.setCcdMotionThreshold(1e-7) #self.bnode.setCcdSweptSphereRadius(0.50) self.bnode.notifyCollisions(True) self.bnode.setIntoCollideMask(BitMask32.bit(1)) self.bnode.setPythonTag("Entity", self) self.noCollideFrames = 4 for a in self.anim: a.hide() a.reparentTo(self.np) a.setScale(0.25, 1, 0.25) a.setPos(0, -0.1, 0) self.obj = self.anim[self.curspr] self.obj.show() self.bnode.setPythonTag("entity", self) def update(self, timer): #animation self.delta += timer self.livetime += timer if self.remove: self.obj.hide() return if self.noCollideFrames == 0: self.bnode.setIntoCollideMask(BitMask32.allOn()) if self.noCollideFrames > -1: self.noCollideFrames -= 1 if self.delta > Flame.animspeed: self.delta = 0 self.obj.hide() self.curspr += 1 if self.curspr > len(self.anim) - 1: self.curspr = 0 self.obj = self.anim[self.curspr] self.obj.show() if self.livetime > Flame.maxlife: self.remove = True def hitby(self, index, projectile): return def destroy(self): self.remove = True self.obj.hide() for model in self.anim: model.remove() self.world.bw.removeRigidBody(self.bnode) def removeOnHit(self): self.remove = True
class Chunk(Entity): chunkmass = 5.0 def __init__(self, world, blocks, pos, hpr=Point3(0,0,0), diff = Vec3(0,0,0), angVel = Vec3(0,0,0), linVel = Vec3(0,0,0)): super(Chunk, self).__init__() self.mass = len(blocks)*Chunk.chunkmass self.world = world self.blocks = blocks self.bnode = BulletRigidBodyNode() self.bnode.setMass(self.mass) self.bnode.setAngularFactor(Vec3(0,1,0)) self.bnode.setLinearSleepThreshold(20) self.bnode.setAngularSleepThreshold(20) self.bnode.setAngularVelocity(angVel) self.bnode.setLinearVelocity(linVel) self.np = utilities.app.render.attachNewNode(self.bnode) self.np.setPos(pos.x,20,pos.y) self.np.setHpr(hpr) self.np.setPos(self.np, diff) self.bnode.setPythonTag("entity", self) self.inScope = False # culling not done yet # centre the blocks around the np and add their shapes in. self.minMax() cog = Point2((self.minX+self.maxX) / 2.0, (self.minY+self.maxY) / 2.0) for block in blocks: block.rebuild(self, cog) world.bw.attachRigidBody(self.bnode) self.hitlist = dict() def update(self, timer): for index in self.hitlist: # returns true if the wall is destroyed by the hit if self.blocks[index].hitby(self.hitlist[index]): self.blocks[index].destroy() self.rebuild(index) self.hitlist.clear() if self.playerDistance() > 40.0: self.bnode.setAngularSleepThreshold(1) self.bnode.setLinearSleepThreshold(1) else: self.bnode.setAngularSleepThreshold(0) self.bnode.setLinearSleepThreshold(0) def playerDistance(self): sp = self.np.getPos() pp = self.world.player.node.getPos() distance = hypot(sp.x - pp.x, sp.z - pp.z) return distance # remove an element and rebuild # TODO add another method for removing multiple # blocks in a single go def rebuild(self, index): deadblock = self.blocks[index] out = list() for block in self.blocks: for edge in block.edges: if edge == deadblock: block.edges.remove(edge) for block in deadblock.edges: chunk = list() self.searchBlock(block, chunk, deadblock) out.append(chunk) #out is a list of lists of populated blocks #remove duplicate chunks results = list() for chunk in out: chunk.sort(compareBlock) if not chunk in results and len(chunk) > 0: results.append(chunk) for result in results: self.minMax(result) #diff = Point2((self.minX+self.maxX) / 2.0, (self.minY+self.maxY) / 2.0) diff = Vec3((self.minX+self.maxX) / 2.0, 0, (self.minY+self.maxY) / 2.0) p = self.np.getPos() pos = Point2(p.x, p.z) h = self.np.getHpr() self.world.entities.append(Chunk(self.world, result, pos, h, diff, self.bnode.getAngularVelocity(), self.bnode.getLinearVelocity())) self.destroyNow() self.remove = True def searchBlock(self, block, lst, deleted): if block in lst: return else: lst.append(block) for newblock in block.edges: self.searchBlock(newblock, lst, deleted) def minMax(self, blocks=None): if blocks == None: blocks = self.blocks self.minX = blocks[0].pos.x self.minY = blocks[0].pos.y self.maxX = blocks[0].pos.x self.maxY = blocks[0].pos.y for point in blocks: self.minX = min(point.pos.x, self.minX) self.minY = min(point.pos.y, self.minY) self.maxX = max(point.pos.x, self.maxX) self.maxY = max(point.pos.y, self.maxY) # Need to clear the bulletrigidbody immediately # so that the phys object isn't present during next sim phase # which occurs before cleanup normally. Otherwise shit will # fly everywhere since two things are inside each other def destroyNow(self): self.world.bw.removeRigidBody(self.bnode) return # since this is handled earlier nothing happens def destroy(self): return def hitby(self, projectile, index): self.hitlist[index] = projectile
class Chunk(Entity): chunkmass = 5.0 def __init__(self, world, blocks, pos, hpr=Point3(0, 0, 0), diff=Vec3(0, 0, 0), angVel=Vec3(0, 0, 0), linVel=Vec3(0, 0, 0)): super(Chunk, self).__init__() self.mass = len(blocks) * Chunk.chunkmass self.world = world self.blocks = blocks self.bnode = BulletRigidBodyNode() self.bnode.setMass(self.mass) self.bnode.setAngularFactor(Vec3(0, 1, 0)) self.bnode.setLinearSleepThreshold(20) self.bnode.setAngularSleepThreshold(20) self.bnode.setAngularVelocity(angVel) self.bnode.setLinearVelocity(linVel) self.np = utilities.app.render.attachNewNode(self.bnode) self.np.setPos(pos.x, 20, pos.y) self.np.setHpr(hpr) self.np.setPos(self.np, diff) self.bnode.setPythonTag("entity", self) self.inScope = False # culling not done yet # centre the blocks around the np and add their shapes in. self.minMax() cog = Point2((self.minX + self.maxX) / 2.0, (self.minY + self.maxY) / 2.0) for block in blocks: block.rebuild(self, cog) world.bw.attachRigidBody(self.bnode) self.hitlist = dict() def update(self, timer): for index in self.hitlist: # returns true if the wall is destroyed by the hit if self.blocks[index].hitby(self.hitlist[index]): self.blocks[index].destroy() self.rebuild(index) self.hitlist.clear() if self.playerDistance() > 40.0: self.bnode.setAngularSleepThreshold(1) self.bnode.setLinearSleepThreshold(1) else: self.bnode.setAngularSleepThreshold(0) self.bnode.setLinearSleepThreshold(0) def playerDistance(self): sp = self.np.getPos() pp = self.world.player.node.getPos() distance = hypot(sp.x - pp.x, sp.z - pp.z) return distance # remove an element and rebuild # TODO add another method for removing multiple # blocks in a single go def rebuild(self, index): deadblock = self.blocks[index] out = list() for block in self.blocks: for edge in block.edges: if edge == deadblock: block.edges.remove(edge) for block in deadblock.edges: chunk = list() self.searchBlock(block, chunk, deadblock) out.append(chunk) #out is a list of lists of populated blocks #remove duplicate chunks results = list() for chunk in out: chunk.sort(compareBlock) if not chunk in results and len(chunk) > 0: results.append(chunk) for result in results: self.minMax(result) #diff = Point2((self.minX+self.maxX) / 2.0, (self.minY+self.maxY) / 2.0) diff = Vec3((self.minX + self.maxX) / 2.0, 0, (self.minY + self.maxY) / 2.0) p = self.np.getPos() pos = Point2(p.x, p.z) h = self.np.getHpr() self.world.entities.append( Chunk(self.world, result, pos, h, diff, self.bnode.getAngularVelocity(), self.bnode.getLinearVelocity())) self.destroyNow() self.remove = True def searchBlock(self, block, lst, deleted): if block in lst: return else: lst.append(block) for newblock in block.edges: self.searchBlock(newblock, lst, deleted) def minMax(self, blocks=None): if blocks == None: blocks = self.blocks self.minX = blocks[0].pos.x self.minY = blocks[0].pos.y self.maxX = blocks[0].pos.x self.maxY = blocks[0].pos.y for point in blocks: self.minX = min(point.pos.x, self.minX) self.minY = min(point.pos.y, self.minY) self.maxX = max(point.pos.x, self.maxX) self.maxY = max(point.pos.y, self.maxY) # Need to clear the bulletrigidbody immediately # so that the phys object isn't present during next sim phase # which occurs before cleanup normally. Otherwise shit will # fly everywhere since two things are inside each other def destroyNow(self): self.world.bw.removeRigidBody(self.bnode) return # since this is handled earlier nothing happens def destroy(self): return def hitby(self, projectile, index): self.hitlist[index] = projectile
class CarEnv(DirectObject): def __init__(self, params={}): self._params = params if 'random_seed' in self._params: np.random.seed(self._params['random_seed']) self._use_vel = self._params.get('use_vel', True) self._run_as_task = self._params.get('run_as_task', False) self._do_back_up = self._params.get('do_back_up', False) self._use_depth = self._params.get('use_depth', False) self._use_back_cam = self._params.get('use_back_cam', False) self._collision_reward = self._params.get('collision_reward', 0.) if not self._params.get('visualize', False): loadPrcFileData('', 'window-type offscreen') # Defines base, render, loader try: ShowBase() except: pass base.setBackgroundColor(0.0, 0.0, 0.0, 1) # World self._worldNP = render.attachNewNode('World') self._world = BulletWorld() self._world.setGravity(Vec3(0, 0, -9.81)) self._dt = params.get('dt', 0.25) self._step = 0.05 # Vehicle shape = BulletBoxShape(Vec3(0.6, 1.0, 0.25)) ts = TransformState.makePos(Point3(0., 0., 0.25)) self._vehicle_node = BulletRigidBodyNode('Vehicle') self._vehicle_node.addShape(shape, ts) self._mass = self._params.get('mass', 10.) self._vehicle_node.setMass(self._mass) self._vehicle_node.setDeactivationEnabled(False) self._vehicle_node.setCcdSweptSphereRadius(1.0) self._vehicle_node.setCcdMotionThreshold(1e-7) self._vehicle_pointer = self._worldNP.attachNewNode(self._vehicle_node) self._world.attachRigidBody(self._vehicle_node) self._vehicle = BulletVehicle(self._world, self._vehicle_node) self._vehicle.setCoordinateSystem(ZUp) self._world.attachVehicle(self._vehicle) self._addWheel(Point3(0.3, 0.5, 0.07), True, 0.07) self._addWheel(Point3(-0.3, 0.5, 0.07), True, 0.07) self._addWheel(Point3(0.3, -0.5, 0.07), False, 0.07) self._addWheel(Point3(-0.3, -0.5, 0.07), False, 0.07) # Camera size = self._params.get('size', [160, 90]) hfov = self._params.get('hfov', 60) near_far = self._params.get('near_far', [0.1, 100.]) self._camera_sensor = Panda3dCameraSensor(base, color=not self._use_depth, depth=self._use_depth, size=size, hfov=hfov, near_far=near_far, title='front cam') self._camera_node = self._camera_sensor.cam self._camera_node.setPos(0.0, 0.5, 0.375) self._camera_node.lookAt(0.0, 6.0, 0.0) self._camera_node.reparentTo(self._vehicle_pointer) if self._use_back_cam: self._back_camera_sensor = Panda3dCameraSensor( base, color=not self._use_depth, depth=self._use_depth, size=size, hfov=hfov, near_far=near_far, title='back cam') self._back_camera_node = self._back_camera_sensor.cam self._back_camera_node.setPos(0.0, -0.5, 0.375) self._back_camera_node.lookAt(0.0, -6.0, 0.0) self._back_camera_node.reparentTo(self._vehicle_pointer) # Car Simulator self._des_vel = None self._setup() # Input self.accept('escape', self._doExit) self.accept('r', self.reset) self.accept('f1', self._toggleWireframe) self.accept('f2', self._toggleTexture) self.accept('f3', self._view_image) self.accept('f5', self._doScreenshot) self.accept('q', self._forward_0) self.accept('w', self._forward_1) self.accept('e', self._forward_2) self.accept('a', self._left) self.accept('s', self._stop) self.accept('x', self._backward) self.accept('d', self._right) self.accept('m', self._mark) self._steering = 0.0 # degree self._engineForce = 0.0 self._brakeForce = 0.0 self._p = self._params.get('p', 1.25) self._d = self._params.get('d', 0.0) self._last_err = 0.0 self._curr_time = 0.0 self._accelClamp = self._params.get('accelClamp', 2.0) self._engineClamp = self._accelClamp * self._mass self._collision = False if self._run_as_task: self._mark_d = 0.0 taskMgr.add(self._update_task, 'updateWorld') base.run() # _____HANDLER_____ def _doExit(self): sys.exit(1) def _toggleWireframe(self): base.toggleWireframe() def _toggleTexture(self): base.toggleTexture() def _doScreenshot(self): base.screenshot('Bullet') def _forward_0(self): self._des_vel = 1 self._brakeForce = 0.0 def _forward_1(self): self._des_vel = 2 self._brakeForce = 0.0 def _forward_2(self): self._des_vel = 4 self._brakeForce = 0.0 def _stop(self): self._des_vel = 0.0 self._brakeForce = 0.0 def _backward(self): self._des_vel = -4 self._brakeForce = 0.0 def _right(self): self._steering = np.min([np.max([-30, self._steering - 5]), 0.0]) def _left(self): self._steering = np.max([np.min([30, self._steering + 5]), 0.0]) def _view_image(self): from matplotlib import pyplot as plt image = self._camera_sensor.observe()[0] if self._use_depth: plt.imshow(image[:, :, 0], cmap='gray') else: import cv2 def rgb2gray(rgb): return np.dot(rgb[..., :3], [0.299, 0.587, 0.114]) image = rgb2gray(image) im = cv2.resize(image, (64, 36), interpolation=cv2.INTER_AREA ) # TODO how does this deal with aspect ratio plt.imshow(im.astype(np.uint8), cmap='Greys_r') plt.show() def _mark(self): self._mark_d = 0.0 # Setup def _setup(self): if hasattr(self, '_model_path'): # Collidable objects visNP = loader.loadModel(self._model_path) visNP.clearModelNodes() visNP.reparentTo(render) pos = (0., 0., 0.) visNP.setPos(pos[0], pos[1], pos[2]) bodyNPs = BulletHelper.fromCollisionSolids(visNP, True) for bodyNP in bodyNPs: bodyNP.reparentTo(render) bodyNP.setPos(pos[0], pos[1], pos[2]) if isinstance(bodyNP.node(), BulletRigidBodyNode): bodyNP.node().setMass(0.0) bodyNP.node().setKinematic(True) bodyNP.setCollideMask(BitMask32.allOn()) self._world.attachRigidBody(bodyNP.node()) else: ground = self._worldNP.attachNewNode(BulletRigidBodyNode('Ground')) shape = BulletPlaneShape(Vec3(0, 0, 1), 0) ground.node().addShape(shape) ground.setCollideMask(BitMask32.allOn()) self._world.attachRigidBody(ground.node()) self._place_vehicle() self._setup_light() self._setup_restart_pos() def _setup_restart_pos(self): self._restart_pos = [] self._restart_index = 0 if self._params.get('position_ranges', None) is not None: ranges = self._params['position_ranges'] num_pos = self._params['num_pos'] if self._params.get('range_type', 'random') == 'random': for _ in range(num_pos): ran = ranges[np.random.randint(len(ranges))] self._restart_pos.append(np.random.uniform(ran[0], ran[1])) elif self._params['range_type'] == 'fix_spacing': num_ran = len(ranges) num_per_ran = num_pos // num_ran for i in range(num_ran): ran = ranges[i] low = np.array(ran[0]) diff = np.array(ran[1]) - np.array(ran[0]) for j in range(num_per_ran): val = diff * ((j + 0.0) / num_per_ran) + low self._restart_pos.append(val) elif self._params.get('positions', None) is not None: self._restart_pos = self._params['positions'] else: self._restart_pos = self._default_restart_pos() def _next_restart_pos_hpr(self): num = len(self._restart_pos) if num == 0: return None, None else: pos_hpr = self._restart_pos[self._restart_index] self._restart_index = (self._restart_index + 1) % num return pos_hpr[:3], pos_hpr[3:] def _next_random_restart_pos_hpr(self): num = len(self._restart_pos) if num == 0: return None, None else: index = np.random.randint(num) pos_hpr = self._restart_pos[index] self._restart_index = (self._restart_index + 1) % num return pos_hpr[:3], pos_hpr[3:] def _setup_light(self): alight = AmbientLight('ambientLight') alight.setColor(Vec4(0.5, 0.5, 0.5, 1)) alightNP = render.attachNewNode(alight) render.clearLight() render.setLight(alightNP) # Vehicle def _default_pos(self): return (0.0, 0.0, 0.3) def _default_hpr(self): return (0.0, 0.0, 3.14) def _default_restart_pos(): return [self._default_pos() + self._default_hpr()] def _get_speed(self): vel = self._vehicle.getCurrentSpeedKmHour() / 3.6 return vel def _update(self, dt=1.0, coll_check=True): self._vehicle.setSteeringValue(self._steering, 0) self._vehicle.setSteeringValue(self._steering, 1) self._vehicle.setBrake(self._brakeForce, 0) self._vehicle.setBrake(self._brakeForce, 1) self._vehicle.setBrake(self._brakeForce, 2) self._vehicle.setBrake(self._brakeForce, 3) if dt >= self._step: # TODO maybe change number of timesteps for i in range(int(dt / self._step)): if self._des_vel is not None: vel = self._get_speed() err = self._des_vel - vel d_err = (err - self._last_err) / self._step self._last_err = err self._engineForce = np.clip( self._p * err + self._d * d_err, -self._accelClamp, self._accelClamp) * self._mass self._vehicle.applyEngineForce(self._engineForce, 0) self._vehicle.applyEngineForce(self._engineForce, 1) self._vehicle.applyEngineForce(self._engineForce, 2) self._vehicle.applyEngineForce(self._engineForce, 3) self._world.doPhysics(self._step, 1, self._step) self._collision = self._is_contact() elif self._run_as_task: self._curr_time += dt if self._curr_time > 0.05: if self._des_vel is not None: vel = self._get_speed() self._mark_d += vel * self._curr_time print(vel, self._mark_d, self._is_contact()) err = self._des_vel - vel d_err = (err - self._last_err) / 0.05 self._last_err = err self._engineForce = np.clip( self._p * err + self._d * d_err, -self._accelClamp, self._accelClamp) * self._mass self._curr_time = 0.0 self._vehicle.applyEngineForce(self._engineForce, 0) self._vehicle.applyEngineForce(self._engineForce, 1) self._vehicle.applyEngineForce(self._engineForce, 2) self._vehicle.applyEngineForce(self._engineForce, 3) self._world.doPhysics(dt, 1, dt) self._collision = self._is_contact() else: raise ValueError( "dt {0} s is too small for velocity control".format(dt)) def _stop_car(self): self._steering = 0.0 self._engineForce = 0.0 self._vehicle.setSteeringValue(0.0, 0) self._vehicle.setSteeringValue(0.0, 1) self._vehicle.applyEngineForce(0.0, 0) self._vehicle.applyEngineForce(0.0, 1) self._vehicle.applyEngineForce(0.0, 2) self._vehicle.applyEngineForce(0.0, 3) if self._des_vel is not None: self._des_vel = 0 self._vehicle_node.setLinearVelocity(Vec3(0.0, 0.0, 0.0)) self._vehicle_node.setAngularVelocity(Vec3(0.0, 0.0, 0.0)) for i in range(self._vehicle.getNumWheels()): wheel = self._vehicle.getWheel(i) wheel.setRotation(0.0) self._vehicle_node.clearForces() def _place_vehicle(self, pos=None, hpr=None): if pos is None: pos = self._default_pos() if hpr is None: hpr = self._default_hpr() self._vehicle_pointer.setPos(pos[0], pos[1], pos[2]) self._vehicle_pointer.setHpr(hpr[0], hpr[1], hpr[2]) self._stop_car() def _addWheel(self, pos, front, radius=0.25): wheel = self._vehicle.createWheel() wheel.setChassisConnectionPointCs(pos) wheel.setFrontWheel(front) wheel.setWheelDirectionCs(Vec3(0, 0, -1)) wheel.setWheelAxleCs(Vec3(1, 0, 0)) wheel.setWheelRadius(radius) wheel.setMaxSuspensionTravelCm(40.0) wheel.setSuspensionStiffness(40.0) wheel.setWheelsDampingRelaxation(2.3) wheel.setWheelsDampingCompression(4.4) wheel.setFrictionSlip(1e2) wheel.setRollInfluence(0.1) # Task def _update_task(self, task): dt = globalClock.getDt() self._update(dt=dt) self._get_observation() return task.cont # Helper functions def _get_observation(self): self._obs = self._camera_sensor.observe() observation = [] observation.append(self._obs[0]) if self._use_back_cam: self._back_obs = self._back_camera_sensor.observe() observation.append(self._back_obs[0]) observation = np.concatenate(observation, axis=2) return observation def _get_reward(self): reward = self._collision_reward if self._collision else self._get_speed( ) return reward def _get_done(self): return self._collision def _get_info(self): info = {} info['pos'] = np.array(self._vehicle_pointer.getPos()) info['hpr'] = np.array(self._vehicle_pointer.getHpr()) info['vel'] = self._get_speed() info['coll'] = self._collision return info def _back_up(self): assert (self._use_vel) back_up_vel = self._params['back_up'].get('vel', -2.0) self._des_vel = back_up_vel back_up_steer = self._params['back_up'].get('steer', (-5.0, 5.0)) # TODO self._steering = np.random.uniform(*back_up_steer) self._brakeForce = 0. duration = self._params['back_up'].get('duration', 1.0) self._update(dt=duration) self._des_vel = 0.0 self._steering = 0.0 self._update(dt=duration) self._brakeForce = 0. def _is_contact(self): result = self._world.contactTest(self._vehicle_node) num_contacts = result.getNumContacts() return result.getNumContacts() > 0 # Environment functions def reset(self, pos=None, hpr=None, hard_reset=False, random_reset=False): if self._do_back_up and not hard_reset and \ pos is None and hpr is None: if self._collision: self._back_up() else: if pos is None and hpr is None: if random_reset: pos, hpr = self._next_random_restart_pos_hpr() else: pos, hpr = self._next_restart_pos_hpr() self._place_vehicle(pos=pos, hpr=hpr) self._collision = False return self._get_observation() def step(self, action): self._steering = action[0] if action[1] == 0.0: self._brakeForce = 1000. else: self._brakeForce = 0. if self._use_vel: # Convert from m/s to km/h self._des_vel = action[1] else: self._engineForce = self._engineClamp * \ ((action[1] - 49.5) / 49.5) self._update(dt=self._dt) observation = self._get_observation() reward = self._get_reward() done = self._get_done() info = self._get_info() return observation, reward, done, info
class CarEnv(DirectObject): def __init__(self, params={}): self._params = params if 'random_seed' in self._params: np.random.seed(self._params['random_seed']) self._use_vel = self._params.get('use_vel', True) self._run_as_task = self._params.get('run_as_task', False) self._do_back_up = self._params.get('do_back_up', False) self._use_depth = self._params.get('use_depth', False) self._use_back_cam = self._params.get('use_back_cam', False) self._collision_reward_only = self._params.get('collision_reward_only', False) self._collision_reward = self._params.get('collision_reward', -10.0) self._obs_shape = self._params.get('obs_shape', (64, 36)) self._steer_limits = params.get('steer_limits', (-30., 30.)) self._speed_limits = params.get('speed_limits', (-4.0, 4.0)) self._motor_limits = params.get('motor_limits', (-0.5, 0.5)) self._fixed_speed = (self._speed_limits[0] == self._speed_limits[1] and self._use_vel) if not self._params.get('visualize', False): loadPrcFileData('', 'window-type offscreen') # Defines base, render, loader try: ShowBase() except: pass base.setBackgroundColor(0.0, 0.0, 0.0, 1) # World self._worldNP = render.attachNewNode('World') self._world = BulletWorld() self._world.setGravity(Vec3(0, 0, -9.81)) self._dt = params.get('dt', 0.25) self._step = 0.05 # Vehicle shape = BulletBoxShape(Vec3(0.6, 1.0, 0.25)) ts = TransformState.makePos(Point3(0., 0., 0.25)) self._vehicle_node = BulletRigidBodyNode('Vehicle') self._vehicle_node.addShape(shape, ts) self._mass = self._params.get('mass', 10.) self._vehicle_node.setMass(self._mass) self._vehicle_node.setDeactivationEnabled(False) self._vehicle_node.setCcdSweptSphereRadius(1.0) self._vehicle_node.setCcdMotionThreshold(1e-7) self._vehicle_pointer = self._worldNP.attachNewNode(self._vehicle_node) self._world.attachRigidBody(self._vehicle_node) self._vehicle = BulletVehicle(self._world, self._vehicle_node) self._vehicle.setCoordinateSystem(ZUp) self._world.attachVehicle(self._vehicle) self._addWheel(Point3(0.3, 0.5, 0.07), True, 0.07) self._addWheel(Point3(-0.3, 0.5, 0.07), True, 0.07) self._addWheel(Point3(0.3, -0.5, 0.07), False, 0.07) self._addWheel(Point3(-0.3, -0.5, 0.07), False, 0.07) # Camera size = self._params.get('size', [160, 90]) hfov = self._params.get('hfov', 120) near_far = self._params.get('near_far', [0.1, 100.]) self._camera_sensor = Panda3dCameraSensor(base, color=not self._use_depth, depth=self._use_depth, size=size, hfov=hfov, near_far=near_far, title='front cam') self._camera_node = self._camera_sensor.cam self._camera_node.setPos(0.0, 0.5, 0.375) self._camera_node.lookAt(0.0, 6.0, 0.0) self._camera_node.reparentTo(self._vehicle_pointer) if self._use_back_cam: self._back_camera_sensor = Panda3dCameraSensor( base, color=not self._use_depth, depth=self._use_depth, size=size, hfov=hfov, near_far=near_far, title='back cam') self._back_camera_node = self._back_camera_sensor.cam self._back_camera_node.setPos(0.0, -0.5, 0.375) self._back_camera_node.lookAt(0.0, -6.0, 0.0) self._back_camera_node.reparentTo(self._vehicle_pointer) # Car Simulator self._des_vel = None self._setup() # Input self.accept('escape', self._doExit) self.accept('r', self.reset) self.accept('f1', self._toggleWireframe) self.accept('f2', self._toggleTexture) self.accept('f3', self._view_image) self.accept('f5', self._doScreenshot) self.accept('q', self._forward_0) self.accept('w', self._forward_1) self.accept('e', self._forward_2) self.accept('a', self._left) self.accept('s', self._stop) self.accept('x', self._backward) self.accept('d', self._right) self.accept('m', self._mark) self._steering = 0.0 # degree self._engineForce = 0.0 self._brakeForce = 0.0 self._env_time_step = 0 self._p = self._params.get('p', 1.25) self._d = self._params.get('d', 0.0) self._last_err = 0.0 self._curr_time = 0.0 self._accelClamp = self._params.get('accelClamp', 0.5) self._engineClamp = self._accelClamp * self._mass self._collision = False self._setup_spec() self.spec = EnvSpec(observation_im_space=self.observation_im_space, action_space=self.action_space, action_selection_space=self.action_selection_space, observation_vec_spec=self.observation_vec_spec, action_spec=self.action_spec, action_selection_spec=self.action_selection_spec, goal_spec=self.goal_spec) if self._run_as_task: self._mark_d = 0.0 taskMgr.add(self._update_task, 'updateWorld') base.run() def _setup_spec(self): self.action_spec = OrderedDict() self.action_selection_spec = OrderedDict() self.observation_vec_spec = OrderedDict() self.goal_spec = OrderedDict() self.action_spec['steer'] = Box(low=-45., high=45.) self.action_selection_spec['steer'] = Box(low=self._steer_limits[0], high=self._steer_limits[1]) if self._use_vel: self.action_spec['speed'] = Box(low=-4., high=4.) self.action_space = Box(low=np.array([ self.action_spec['steer'].low[0], self.action_spec['speed'].low[0] ]), high=np.array([ self.action_spec['steer'].high[0], self.action_spec['speed'].high[0] ])) self.action_selection_spec['speed'] = Box( low=self._speed_limits[0], high=self._speed_limits[1]) self.action_selection_space = Box( low=np.array([ self.action_selection_spec['steer'].low[0], self.action_selection_spec['speed'].low[0] ]), high=np.array([ self.action_selection_spec['steer'].high[0], self.action_selection_spec['speed'].high[0] ])) else: self.action_spec['motor'] = Box(low=-self._accelClamp, high=self._accelClamp) self.action_space = Box(low=np.array([ self.action_spec['steer'].low[0], self.action_spec['motor'].low[0] ]), high=np.array([ self.action_spec['steer'].high[0], self.action_spec['motor'].high[0] ])) self.action_selection_spec['motor'] = Box( low=self._motor_limits[0], high=self._motor_limits[1]) self.action_selection_space = Box( low=np.array([ self.action_selection_spec['steer'].low[0], self.action_selection_spec['motor'].low[0] ]), high=np.array([ self.action_selection_spec['steer'].high[0], self.action_selection_spec['motor'].high[0] ])) assert (np.logical_and( self.action_selection_space.low >= self.action_space.low - 1e-4, self.action_selection_space.high <= self.action_space.high + 1e-4).all()) self.observation_im_space = Box(low=0, high=255, shape=tuple( self._get_observation()[0].shape)) self.observation_vec_spec['coll'] = Discrete(1) self.observation_vec_spec['heading'] = Box(low=0, high=2 * 3.14) self.observation_vec_spec['speed'] = Box(low=-4.0, high=4.0) @property def _base_dir(self): return os.path.join(os.path.dirname(os.path.abspath(__file__)), 'models') @property def horizon(self): return np.inf @property def max_reward(self): return np.inf # _____HANDLER_____ def _doExit(self): sys.exit(1) def _toggleWireframe(self): base.toggleWireframe() def _toggleTexture(self): base.toggleTexture() def _doScreenshot(self): base.screenshot('Bullet') def _forward_0(self): self._des_vel = 1 self._brakeForce = 0.0 def _forward_1(self): self._des_vel = 2 self._brakeForce = 0.0 def _forward_2(self): self._des_vel = 4 self._brakeForce = 0.0 def _stop(self): self._des_vel = 0.0 self._brakeForce = 0.0 def _backward(self): self._des_vel = -4 self._brakeForce = 0.0 def _right(self): self._steering = np.min([np.max([-30, self._steering - 5]), 0.0]) def _left(self): self._steering = np.max([np.min([30, self._steering + 5]), 0.0]) def _view_image(self): from matplotlib import pyplot as plt image = self._camera_sensor.observe()[0] if self._use_depth: plt.imshow(image[:, :, 0], cmap='gray') else: def rgb2gray(rgb): return np.dot(rgb[..., :3], [0.299, 0.587, 0.114]) image = rgb2gray(image) im = cv2.resize(image, (64, 36), interpolation=cv2.INTER_AREA ) # TODO how does this deal with aspect ratio plt.imshow(im.astype(np.uint8), cmap='Greys_r') plt.show() def _mark(self): self._mark_d = 0.0 # Setup def _setup(self): self._setup_map() self._place_vehicle() self._setup_light() self._setup_restart_pos() def _setup_map(self): if hasattr(self, '_model_path'): # Collidable objects self._setup_collision_object(self._model_path) else: ground = self._worldNP.attachNewNode(BulletRigidBodyNode('Ground')) shape = BulletPlaneShape(Vec3(0, 0, 1), 0) ground.node().addShape(shape) ground.setCollideMask(BitMask32.allOn()) self._world.attachRigidBody(ground.node()) def _setup_collision_object(self, path, pos=(0.0, 0.0, 0.0), hpr=(0.0, 0.0, 0.0), scale=1): visNP = loader.loadModel(path) visNP.clearModelNodes() visNP.reparentTo(render) visNP.setPos(pos[0], pos[1], pos[2]) visNP.setHpr(hpr[0], hpr[1], hpr[2]) visNP.set_scale(scale, scale, scale) bodyNPs = BulletHelper.fromCollisionSolids(visNP, True) for bodyNP in bodyNPs: bodyNP.reparentTo(render) bodyNP.setPos(pos[0], pos[1], pos[2]) bodyNP.setHpr(hpr[0], hpr[1], hpr[2]) bodyNP.set_scale(scale, scale, scale) if isinstance(bodyNP.node(), BulletRigidBodyNode): bodyNP.node().setMass(0.0) bodyNP.node().setKinematic(True) bodyNP.setCollideMask(BitMask32.allOn()) self._world.attachRigidBody(bodyNP.node()) else: print("Issue") def _setup_restart_pos(self): self._restart_index = 0 self._restart_pos = self._default_restart_pos() def _next_restart_pos_hpr(self): num = len(self._restart_pos) if num == 0: return None, None else: pos_hpr = self._restart_pos[self._restart_index] self._restart_index = (self._restart_index + 1) % num return pos_hpr[:3], pos_hpr[3:] def _setup_light(self): # alight = AmbientLight('ambientLight') # alight.setColor(Vec4(0.5, 0.5, 0.5, 1)) # alightNP = render.attachNewNode(alight) # render.clearLight() # render.setLight(alightNP) pass # Vehicle def _default_pos(self): return (0.0, 0.0, 0.3) def _default_hpr(self): return (0.0, 0.0, 0.0) def _default_restart_pos(self): return [self._default_pos() + self._default_hpr()] def _get_speed(self): vel = self._vehicle.getCurrentSpeedKmHour() / 3.6 return vel def _get_heading(self): h = np.array(self._vehicle_pointer.getHpr())[0] ori = h * (pi / 180.) while (ori > 2 * pi): ori -= 2 * pi while (ori < 0): ori += 2 * pi return ori def _update(self, dt=1.0, coll_check=True): self._vehicle.setSteeringValue(self._steering, 0) self._vehicle.setSteeringValue(self._steering, 1) self._vehicle.setBrake(self._brakeForce, 0) self._vehicle.setBrake(self._brakeForce, 1) self._vehicle.setBrake(self._brakeForce, 2) self._vehicle.setBrake(self._brakeForce, 3) if dt >= self._step: # TODO maybe change number of timesteps # for i in range(int(dt/self._step)): if self._des_vel is not None: vel = self._get_speed() err = self._des_vel - vel d_err = (err - self._last_err) / self._step self._last_err = err self._engineForce = np.clip(self._p * err + self._d * d_err, -self._accelClamp, self._accelClamp) * self._mass self._vehicle.applyEngineForce(self._engineForce, 0) self._vehicle.applyEngineForce(self._engineForce, 1) self._vehicle.applyEngineForce(self._engineForce, 2) self._vehicle.applyEngineForce(self._engineForce, 3) for _ in range(int(dt / self._step)): self._world.doPhysics(self._step, 1, self._step) self._collision = self._is_contact() elif self._run_as_task: self._curr_time += dt if self._curr_time > 0.05: if self._des_vel is not None: vel = self._get_speed() self._mark_d += vel * self._curr_time print(vel, self._mark_d, self._is_contact()) err = self._des_vel - vel d_err = (err - self._last_err) / 0.05 self._last_err = err self._engineForce = np.clip( self._p * err + self._d * d_err, -self._accelClamp, self._accelClamp) * self._mass self._curr_time = 0.0 self._vehicle.applyEngineForce(self._engineForce, 0) self._vehicle.applyEngineForce(self._engineForce, 1) self._vehicle.applyEngineForce(self._engineForce, 2) self._vehicle.applyEngineForce(self._engineForce, 3) self._world.doPhysics(dt, 1, dt) self._collision = self._is_contact() else: raise ValueError( "dt {0} s is too small for velocity control".format(dt)) def _stop_car(self): self._steering = 0.0 self._engineForce = 0.0 self._vehicle.setSteeringValue(0.0, 0) self._vehicle.setSteeringValue(0.0, 1) self._vehicle.applyEngineForce(0.0, 0) self._vehicle.applyEngineForce(0.0, 1) self._vehicle.applyEngineForce(0.0, 2) self._vehicle.applyEngineForce(0.0, 3) if self._des_vel is not None: self._des_vel = 0 self._vehicle_node.setLinearVelocity(Vec3(0.0, 0.0, 0.0)) self._vehicle_node.setAngularVelocity(Vec3(0.0, 0.0, 0.0)) for i in range(self._vehicle.getNumWheels()): wheel = self._vehicle.getWheel(i) wheel.setRotation(0.0) self._vehicle_node.clearForces() def _place_vehicle(self, pos=None, hpr=None): if pos is None: pos = self._default_pos() if hpr is None: hpr = self._default_hpr() self._vehicle_pointer.setPos(pos[0], pos[1], pos[2]) self._vehicle_pointer.setHpr(hpr[0], hpr[1], hpr[2]) self._stop_car() def _addWheel(self, pos, front, radius=0.25): wheel = self._vehicle.createWheel() wheel.setChassisConnectionPointCs(pos) wheel.setFrontWheel(front) wheel.setWheelDirectionCs(Vec3(0, 0, -1)) wheel.setWheelAxleCs(Vec3(1, 0, 0)) wheel.setWheelRadius(radius) wheel.setMaxSuspensionTravelCm(40.0) wheel.setSuspensionStiffness(40.0) wheel.setWheelsDampingRelaxation(2.3) wheel.setWheelsDampingCompression(4.4) wheel.setFrictionSlip(1e2) wheel.setRollInfluence(0.1) # Task def _update_task(self, task): dt = globalClock.getDt() self._update(dt=dt) self._get_observation() return task.cont # Helper functions def _get_observation(self): self._obs = self._camera_sensor.observe() observation = [] observation.append(self.process(self._obs[0], self._obs_shape)) if self._use_back_cam: self._back_obs = self._back_camera_sensor.observe() observation.append(self.process(self._back_obs[0], self._obs_shape)) observation_im = np.concatenate(observation, axis=2) coll = self._collision heading = self._get_heading() speed = self._get_speed() observation_vec = np.array([coll, heading, speed]) return observation_im, observation_vec def _get_goal(self): return np.array([]) def process(self, image, obs_shape): if self._use_depth: im = np.reshape(image, (image.shape[0], image.shape[1])) if im.shape != obs_shape: im = cv2.resize(im, obs_shape, interpolation=cv2.INTER_AREA) return im.astype(np.uint8) else: image = np.dot(image[..., :3], [0.299, 0.587, 0.114]) im = cv2.resize(image, obs_shape, interpolation=cv2.INTER_AREA ) #TODO how does this deal with aspect ratio # TODO might not be necessary im = np.expand_dims(im, 2) return im.astype(np.uint8) def _get_reward(self): if self._collision_reward_only: if self._collision: reward = self._collision_reward else: reward = 0.0 else: if self._collision: reward = self._collision_reward else: reward = self._get_speed() assert (reward <= self.max_reward) return reward def _get_done(self): return self._collision def _get_info(self): info = {} info['pos'] = np.array(self._vehicle_pointer.getPos()) info['hpr'] = np.array(self._vehicle_pointer.getHpr()) info['vel'] = self._get_speed() info['coll'] = self._collision info['env_time_step'] = self._env_time_step return info def _back_up(self): assert (self._use_vel) # logger.debug('Backing up!') self._params['back_up'] = self._params.get('back_up', {}) back_up_vel = self._params['back_up'].get('vel', -1.0) self._des_vel = back_up_vel back_up_steer = self._params['back_up'].get('steer', (-5.0, 5.0)) self._steering = np.random.uniform(*back_up_steer) self._brakeForce = 0. duration = self._params['back_up'].get('duration', 3.0) self._update(dt=duration) self._des_vel = 0. self._steering = 0. self._update(dt=duration) self._brakeForce = 0. def _is_contact(self): result = self._world.contactTest(self._vehicle_node) return result.getNumContacts() > 0 # Environment functions def reset(self, pos=None, hpr=None, hard_reset=False): if self._do_back_up and not hard_reset and \ pos is None and hpr is None: if self._collision: self._back_up() else: if hard_reset: logger.debug('Hard resetting!') if pos is None and hpr is None: pos, hpr = self._next_restart_pos_hpr() self._place_vehicle(pos=pos, hpr=hpr) self._collision = False self._env_time_step = 0 return self._get_observation(), self._get_goal() def step(self, action): self._steering = action[0] if action[1] == 0.0: self._brakeForce = 1000. else: self._brakeForce = 0. if self._use_vel: # Convert from m/s to km/h self._des_vel = action[1] else: self._engineForce = self._mass * action[1] self._update(dt=self._dt) observation = self._get_observation() goal = self._get_goal() reward = self._get_reward() done = self._get_done() info = self._get_info() self._env_time_step += 1 return observation, goal, reward, done, info
class CharacterController(DynamicObject, FSM): def __init__(self, game): self.game = game FSM.__init__(self, "Player") # key states # dx direction left or right, dy direction forward or backward self.kh = self.game.kh self.dx = 0 self.dy = 0 self.jumping = 0 self.crouching = 0 # maximum speed when only walking self.groundSpeed = 5.0 # acceleration used when walking to rise to self.groundSpeed self.groundAccel = 100.0 # maximum speed in the air (air friction) self.airSpeed = 30.0 # player movement control when in the air self.airAccel = 10.0 # horizontal speed on jump start self.jumpSpeed = 5.5 self.turnSpeed = 5 self.moveSpeedVec = Vec3(0, 0, 0) self.platformSpeedVec = Vec3(0, 0, 0) h = 1.75 w = 0.4 self.shape = BulletCapsuleShape(w, h - 2 * w, ZUp) self.node = BulletRigidBodyNode('Player') self.node.setMass(2) self.node.addShape(self.shape) self.node.setActive(True, True) self.node.setDeactivationEnabled(False, True) self.node.setFriction(200) #self.node.setGravity(10) #self.node.setFallSpeed(200) self.node.setCcdMotionThreshold(1e-7) self.node.setCcdSweptSphereRadius(0.5) self.node.setAngularFactor(Vec3(0, 0, 1)) self.np = self.game.render.attachNewNode(self.node) self.np.setPos(0, 0, 0) self.np.setH(0) #self.np.setCollideMask(BitMask32.allOn()) self.game.world.attachRigidBody(self.node) self.playerModel = None self.slice_able = False def setActor(self, modelPath, animDic={}, flip=False, pos=(0, 0, 0), scale=1.0): self.playerModel = Actor(modelPath, animDic) self.playerModel.reparentTo(self.np) self.playerModel.setScale(scale) # 1ft = 0.3048m if flip: self.playerModel.setH(180) self.playerModel.setPos(pos) self.playerModel.setScale(scale) #------------------------------------------------------------------- # Speed def getSpeedVec(self): return self.node.getLinearVelocity() def setSpeedVec(self, speedVec): #print "setting speed to %s" % (speedVec) self.node.setLinearVelocity(speedVec) return speedVec def setPlatformSpeed(self, speedVec): self.platformSpeedVec = speedVec def getSpeed(self): return self.getSpeedVec().length() def setSpeed(self, speed): speedVec = self.getSpeedVec() speedVec.normalize() self.setSpeedVec(speedVec * speed) def getLocalSpeedVec(self): return self.np.getRelativeVector(self.getSpeed()) def getSpeedXY(self): vec = self.getSpeedVec() return Vec3(vec[0], vec[1], 0) def setSpeedXY(self, speedX, speedY): vec = self.getSpeedVec() z = self.getSpeedZ() self.setSpeedVec(Vec3(speedX, speedY, z)) def getSpeedH(self): return self.getSpeedXY().length() def getSpeedZ(self): return self.getSpeedVec()[2] def setSpeedZ(self, zSpeed): vec = self.getSpeedVec() speedVec = Vec3(vec[0], vec[1], zSpeed) return self.setSpeedVec(speedVec) def setLinearVelocity(self, speedVec): return self.setSpeedVec(speedVec) def setAngularVelocity(self, speed): self.node.setAngularVelocity(Vec3(0, 0, speed)) def getFriction(self): return self.node.getFriction() def setFriction(self, friction): return self.node.setFriction(friction) #------------------------------------------------------------------- # Acceleration def doJump(self): #self.setSpeedZ(self.getSpeedZ()+self.jumpSpeed) self.setSpeedZ(self.jumpSpeed) def setForce(self, force): self.node.applyCentralForce(force) #------------------------------------------------------------------- # contacts #------------------------------------------------------------------- # update def update(self, dt, dx=0, dy=0, jumping=0, crouching=0, dRot=0): #self.setR(0) #self.setP(0) self.jumping = jumping self.crouching = crouching self.dx = dx self.dy = dy #self.setAngularVelocity(dRot*self.turnSpeed) #self.setAngularVelocity(0) self.setH(self.game.camHandler.gameNp.getH()) speedVec = self.getSpeedVec() - self.platformSpeedVec speed = speedVec.length() localSpeedVec = self.np.getRelativeVector(self.game.render, speedVec) pushVec = self.game.render.getRelativeVector(self.np, Vec3(self.dx, self.dy, 0)) if self.dx != 0 or self.dy != 0: pushVec.normalize() else: pushVec = Vec3(0, 0, 0) contacts = self.getContacts() contact = self.checkFeet() if self.jumping and contact in contacts: self.setFriction(0) self.doJump() if self.jumping: self.setForce(pushVec * self.airAccel) if speed > self.airSpeed: self.setSpeed(self.airSpeed) else: if contacts: self.setForce(pushVec * self.groundAccel) else: self.setForce(pushVec * self.airAccel) if self.dx == 0 and self.dy == 0: self.setFriction(100) else: self.setFriction(0) if speed > self.groundSpeed: if contacts: self.setSpeed(self.groundSpeed) '''
class Npc(object): aggroDist = Vec3(3, 3, 3) def __init__(self, renderNode, world, collisions, playerNode, Type, x, y, z, numNpc): self.numNpc = numNpc self.world = world self.renderDummy = renderNode self.collisions = collisions self.player = playerNode self.levelNodeBounds = self.renderDummy.getBounds() if Type == "Enemy": self.createEnemy(x, y, z) print("aiudsrfbcweruifhcboieruhfnowi3eu4ghfnrweiugfhresiougfhnresikugfhrwesik") else: print("No Implementation of " + str(Type) + " yet!") def createEnemy(self, x, y, z): #nodes self.enemyBulletNode = BulletRigidBodyNode('PlayerBox') self.enemyBulletNode.setMass(1.0) self.enemyNp = self.renderDummy.attachNewNode(self.enemyBulletNode) self.enemyNp.setPos(x, y, z) print(x, y, z) #shapes shape = BulletBoxShape(Vec3(0.5, 0.5, 0.5)) model = Cube(1, 1, 1) model.reparentTo(self.enemyNp) model.setPos(-.5, -.5, -.5) #nodes self.enemyBulletNode.addShape(shape) material = Material() material.setAmbient(VBase4(random.random(), random.random(), random.random(), random.random())) material.setDiffuse(VBase4(random.random(), random.random(), random.random(), random.random()))# -*- coding: utf-8 *-* material.setEmission(VBase4(random.random(), random.random(), random.random(), random.random())) #material.setSpecular(VBase4(random.random(), random.random(), random.random(), random.random())) material.setShininess(random.random()) myTexture = loader.loadTexture("./textures/enemy.jpg") self.enemyNp.setTexture(myTexture) self.enemyNp.setMaterial(material) #bullet world self.world.attachRigidBody(self.enemyBulletNode) taskMgr.add(self.enemyUpdate, "EnemyUpdateTask#" + str(self.numNpc)) def shoot(self): self.ammoUsed = 0 self.ammoUsed += 1 #nodes node = BulletRigidBodyNode('Box') node.setMass(.1) ammoNode = render.attachNewNode(node) #shapes shape = BulletSphereShape(.2) model = Sphere(.2) model.reparentTo(ammoNode) model.setPos(0, 0, 0) #nodes node.addShape(shape) material = Material() material.setAmbient(VBase4(random.random(), random.random(), random.random(), random.random())) material.setDiffuse(VBase4(random.random(), random.random(), random.random(), random.random())) material.setEmission(VBase4(random.random(), random.random(), random.random(), random.random())) material.setShininess(random.random()) #myTexture = loader.loadTexture("player.png") #ammoNode.setTexture(myTexture) ammoNode.setMaterial(material) ammoNode.setX(self.enemyNp.getX()) ammoNode.setY(self.enemyNp.getY()) ammoNode.setZ(self.enemyNp.getZ() + 1) force = Vec3(0, 0, 0) force.setY(5) force.setZ(1) force*=50 force = render.getRelativeVector(self.player, force) ammoNode.node().applyCentralForce(force) print(force) #bullet world self.world.attachRigidBody(node) #taskMgr.remove('ammoDestroy') task = Task(self.destroyAmmoNode) taskMgr.add(task, 'ammoDestroy', extraArgs = [task, ammoNode, node, self.world]) #self.collisions.initAmmoCollisions(ammoNode, self.np, 0, 0, 0) def destroyAmmoNode(self, task, arg, bulletArg, world): if task.time > 3: print(task.time) arg.removeNode() world.removeRigidBody(bulletArg) print('mmmmm?') return Task.done return Task.cont def removeTask(self): taskMgr.remove("EnemyUpdateTask#" + str(self.numNpc)) def enemyUpdate(self, task): if math.fabs(self.player.getX() - self.enemyNp.getX()) <= 20 and math.fabs(self.player.getY() - self.enemyNp.getY()) <= 20 and math.fabs(self.player.getZ() - self.enemyNp.getZ()) <= 20: #self.r = random.randint(1, 100) #if self.r == random.randint(1, 100): #self.shoot() #self.enemyLerp = LerpPosInterval(self.enemyNp, .4, self.player.getPos()) #self.enemyLerp.start() self.velocity = Vec3(self.player.getPos() - self.enemyNp.getPos()) self.enemyBulletNode.setLinearVelocity(self.velocity * 2) if math.fabs(self.enemyNp.getX()) > math.fabs(self.levelNodeBounds.getRadius()) * 2: self.removeTask() if math.fabs(self.enemyNp.getY()) > math.fabs(self.levelNodeBounds.getRadius()) * 2: self.removeTask() if math.fabs(self.enemyNp.getZ()) > math.fabs(self.levelNodeBounds.getRadius()) * 2: self.removeTask() return Task.cont
class CharacterController(DynamicObject, FSM): def __init__(self, game): self.game = game FSM.__init__(self, "Player") # key states # dx direction left or right, dy direction forward or backward self.kh = self.game.kh self.dx = 0 self.dy = 0 self.jumping = 0 self.crouching = 0 # maximum speed when only walking self.groundSpeed = 5.0 # acceleration used when walking to rise to self.groundSpeed self.groundAccel = 100.0 # maximum speed in the air (air friction) self.airSpeed = 30.0 # player movement control when in the air self.airAccel = 10.0 # horizontal speed on jump start self.jumpSpeed = 5.5 self.turnSpeed = 5 self.moveSpeedVec = Vec3(0,0,0) self.platformSpeedVec = Vec3(0,0,0) h = 1.75 w = 0.4 self.shape = BulletCapsuleShape(w, h - 2 * w, ZUp) self.node = BulletRigidBodyNode('Player') self.node.setMass(2) self.node.addShape(self.shape) self.node.setActive(True, True) self.node.setDeactivationEnabled(False, True) self.node.setFriction(200) #self.node.setGravity(10) #self.node.setFallSpeed(200) self.node.setCcdMotionThreshold(1e-7) self.node.setCcdSweptSphereRadius(0.5) self.node.setAngularFactor(Vec3(0,0,1)) self.np = self.game.render.attachNewNode(self.node) self.np.setPos(0, 0, 0) self.np.setH(0) #self.np.setCollideMask(BitMask32.allOn()) self.game.world.attachRigidBody(self.node) self.playerModel = None self.slice_able = False def setActor(self, modelPath, animDic={}, flip = False, pos = (0,0,0), scale = 1.0): self.playerModel = Actor(modelPath, animDic) self.playerModel.reparentTo(self.np) self.playerModel.setScale(scale) # 1ft = 0.3048m if flip: self.playerModel.setH(180) self.playerModel.setPos(pos) self.playerModel.setScale(scale) #------------------------------------------------------------------- # Speed def getSpeedVec(self): return self.node.getLinearVelocity() def setSpeedVec(self, speedVec): #print "setting speed to %s" % (speedVec) self.node.setLinearVelocity(speedVec) return speedVec def setPlatformSpeed(self, speedVec): self.platformSpeedVec = speedVec def getSpeed(self): return self.getSpeedVec().length() def setSpeed(self, speed): speedVec = self.getSpeedVec() speedVec.normalize() self.setSpeedVec(speedVec*speed) def getLocalSpeedVec(self): return self.np.getRelativeVector(self.getSpeed()) def getSpeedXY(self): vec = self.getSpeedVec() return Vec3(vec[0], vec[1], 0) def setSpeedXY(self, speedX, speedY): vec = self.getSpeedVec() z = self.getSpeedZ() self.setSpeedVec(Vec3(speedX, speedY, z)) def getSpeedH(self): return self.getSpeedXY().length() def getSpeedZ(self): return self.getSpeedVec()[2] def setSpeedZ(self, zSpeed): vec = self.getSpeedVec() speedVec = Vec3(vec[0], vec[1], zSpeed) return self.setSpeedVec(speedVec) def setLinearVelocity(self, speedVec): return self.setSpeedVec(speedVec) def setAngularVelocity(self, speed): self.node.setAngularVelocity(Vec3(0, 0, speed)) def getFriction(self): return self.node.getFriction() def setFriction(self, friction): return self.node.setFriction(friction) #------------------------------------------------------------------- # Acceleration def doJump(self): #self.setSpeedZ(self.getSpeedZ()+self.jumpSpeed) self.setSpeedZ(self.jumpSpeed) def setForce(self, force): self.node.applyCentralForce(force) #------------------------------------------------------------------- # contacts #------------------------------------------------------------------- # update def update(self, dt, dx=0, dy=0, jumping=0, crouching=0, dRot=0): #self.setR(0) #self.setP(0) self.jumping = jumping self.crouching = crouching self.dx = dx self.dy = dy #self.setAngularVelocity(dRot*self.turnSpeed) #self.setAngularVelocity(0) self.setH(self.game.camHandler.gameNp.getH()) speedVec = self.getSpeedVec() - self.platformSpeedVec speed = speedVec.length() localSpeedVec = self.np.getRelativeVector(self.game.render, speedVec) pushVec = self.game.render.getRelativeVector(self.np, Vec3(self.dx,self.dy,0)) if self.dx != 0 or self.dy != 0: pushVec.normalize() else: pushVec = Vec3(0,0,0) contacts = self.getContacts() contact = self.checkFeet() if self.jumping and contact in contacts: self.setFriction(0) self.doJump() if self.jumping: self.setForce(pushVec * self.airAccel) if speed > self.airSpeed: self.setSpeed(self.airSpeed) else: if contacts: self.setForce(pushVec * self.groundAccel) else: self.setForce(pushVec * self.airAccel) if self.dx == 0 and self.dy == 0: self.setFriction(100) else: self.setFriction(0) if speed > self.groundSpeed: if contacts: self.setSpeed(self.groundSpeed) '''
class Window3D: def __init__(self, window_id, rect): print("Creating window ID={}".format(window_id)) self.rect = rect self.window_id = window_id self.box = WindowBox(globals.front_texture, globals.back_texture) self.box.update_vertices(rect, globals.desk_rect) x0 = int(rect[0] * 1.1) - 2000 + int(0.7 * rect[2]) y0 = 1980 z0 = int(rect[1] * 1.1) - 500 self.default_position = Point3(x0, y0, z0) self.box.node.setTag('wid', str(window_id)) size = [0.5 * (a - b) for a, b in zip(self.box.mx, self.box.mn)] self.shape = BulletBoxShape(Vec3(size[0], size[1], size[2] + 10)) self.phy_node = BulletRigidBodyNode('window') volume = size[0] * size[1] * size[2] self.phy_node.setMass(0.001 * volume) self.phy_node.addShape(self.shape) self.node = globals.gui.render.attachNewNode(self.phy_node) self.phy_node.setLinearDamping(0.6) self.phy_node.setAngularDamping(0.6) globals.gui.world.attachRigidBody(self.phy_node) self.node.setCollideMask(BitMask32.bit(1)) self.box.node.reparentTo(self.node) self.box.node.setPos(0, 0, size[2] - 10) self.node.setPos(x0, y0, z0) self.set_physics(False) def reset_position(self): self.node.setPos(self.default_position) self.node.setHpr(0, 0, 0) self.set_physics(False) def get_hook_pos(self): return self.node.getPos() + self.box.node.getPos() def update_rectangle(self, rect): self.rect = rect self.box.update_vertices(rect, globals.desk_rect) def set_physics(self, state): if state: self.phy_node.setMass(1.0) else: self.phy_node.setMass(0.0) self.phy_node.setLinearVelocity(LVector3(0, 0, 0)) self.phy_node.setAngularVelocity(LVector3(0, 0, 0)) def rotate(self, dx, dy): dx = norm_rot(dx) dy = norm_rot(dy) v = self.box.node.getHpr() v = v + LVecBase3(dx, dy, 0) self.box.node.setHpr(v) def move(self, delta): v = self.node.getPos() for i in range(len(delta)): v[i] = v[i] + delta[i] self.node.setPos(v) # self.phy_node.setAngularVelocity(LVector3(0,0,0)) def close(self): self.box.close() def calc_desktop_coords(self, v): if v[2] > 0: return None x = int(self.rect[0] + (0.5 * self.rect[2] + v[0])) y = int(self.rect[1] - v[2]) return x, y
class Drone: # RIGIDBODYMASS = 1 # RIGIDBODYRADIUS = 0.1 # LINEARDAMPING = 0.97 # SENSORRANGE = 0.6 # TARGETFORCE = 3 # AVOIDANCEFORCE = 25 # FORCEFALLOFFDISTANCE = .5 RIGIDBODYMASS = 0.5 RIGIDBODYRADIUS = 0.1 LINEARDAMPING = 0.95 SENSORRANGE = 0.6 TARGETFORCE = 1 AVOIDANCEFORCE = 10 FORCEFALLOFFDISTANCE = .5 def __init__(self, manager, position: Vec3, uri="-1", printDebugInfo=False): self.base = manager.base self.manager = manager # The position of the real drone this virtual drone is connected to. # If a connection is active, this value is updated each frame. self.realDronePosition = Vec3(0, 0, 0) self.canConnect = False # true if the virtual drone has a uri to connect to a real drone self.isConnected = False # true if the connection to a real drone is currently active self.uri = uri if self.uri != "-1": self.canConnect = True self.id = int(uri[-1]) self.randVec = Vec3(random.uniform(-1, 1), random.uniform(-1, 1), random.uniform(-1, 1)) # add the rigidbody to the drone, which has a mass and linear damping self.rigidBody = BulletRigidBodyNode( "RigidSphere") # derived from PandaNode self.rigidBody.setMass(self.RIGIDBODYMASS) # body is now dynamic self.rigidBody.addShape(BulletSphereShape(self.RIGIDBODYRADIUS)) self.rigidBody.setLinearSleepThreshold(0) self.rigidBody.setFriction(0) self.rigidBody.setLinearDamping(self.LINEARDAMPING) self.rigidBodyNP = self.base.render.attachNewNode(self.rigidBody) self.rigidBodyNP.setPos(position) # self.rigidBodyNP.setCollideMask(BitMask32.bit(1)) self.base.world.attach(self.rigidBody) # add a 3d model to the drone to be able to see it in the 3d scene model = self.base.loader.loadModel(self.base.modelDir + "/drones/drone1.egg") model.setScale(0.2) model.reparentTo(self.rigidBodyNP) self.target = position # the long term target that the virtual drones tries to reach self.setpoint = position # the immediate target (setpoint) that the real drone tries to reach, usually updated each frame self.waitingPosition = Vec3(position[0], position[1], 0.7) self.lastSentPosition = self.waitingPosition # the position that this drone last sent around self.printDebugInfo = printDebugInfo if self.printDebugInfo: # put a second drone model on top of drone that outputs debug stuff model = self.base.loader.loadModel(self.base.modelDir + "/drones/drone1.egg") model.setScale(0.4) model.setPos(0, 0, .2) model.reparentTo(self.rigidBodyNP) # initialize line renderers self.targetLineNP = self.base.render.attachNewNode(LineSegs().create()) self.velocityLineNP = self.base.render.attachNewNode( LineSegs().create()) self.forceLineNP = self.base.render.attachNewNode(LineSegs().create()) self.actualDroneLineNP = self.base.render.attachNewNode( LineSegs().create()) self.setpointNP = self.base.render.attachNewNode(LineSegs().create()) def connect(self): """Connects the virtual drone to a real one with the uri supplied at initialization.""" if not self.canConnect: return print(self.uri, "connecting") self.isConnected = True self.scf = SyncCrazyflie(self.uri, cf=Crazyflie(rw_cache='./cache')) self.scf.open_link() self._reset_estimator() self.start_position_printing() # MOVE THIS BACK TO SENDPOSITIONS() IF STUFF BREAKS self.scf.cf.param.set_value('flightmode.posSet', '1') def sendPosition(self): """Sends the position of the virtual drone to the real one.""" cf = self.scf.cf self.setpoint = self.getPos() # send the setpoint cf.commander.send_position_setpoint(self.setpoint[0], self.setpoint[1], self.setpoint[2], 0) def disconnect(self): """Disconnects the real drone.""" print(self.uri, "disconnecting") self.isConnected = False cf = self.scf.cf cf.commander.send_stop_setpoint() time.sleep(0.1) self.scf.close_link() def update(self): """Update the virtual drone.""" # self.updateSentPositionBypass(0) self._updateTargetForce() self._updateAvoidanceForce() self._clampForce() if self.isConnected: self.sendPosition() # draw various lines to get a better idea of whats happening self._drawTargetLine() # self._drawVelocityLine() self._drawForceLine() # self._drawSetpointLine() self._printDebugInfo() def updateSentPosition(self, timeslot): transmissionProbability = 1 if self.id == timeslot: if random.uniform(0, 1) < transmissionProbability: # print(f"drone {self.id} updated position") self.lastSentPosition = self.getPos() else: pass # print(f"drone {self.id} failed!") def updateSentPositionBypass(self, timeslot): self.lastSentPosition = self.getPos() def getPos(self) -> Vec3: return self.rigidBodyNP.getPos() def getLastSentPos(self) -> Vec3: return self.lastSentPosition def _updateTargetForce(self): """Applies a force to the virtual drone which moves it closer to its target.""" dist = (self.target - self.getPos()) if (dist.length() > self.FORCEFALLOFFDISTANCE): force = dist.normalized() else: force = (dist / self.FORCEFALLOFFDISTANCE) self.addForce(force * self.TARGETFORCE) def _updateAvoidanceForce(self): """Applies a force the the virtual drone which makes it avoid other drones.""" # get all drones within the sensors reach and put them in a list nearbyDrones = [] for drone in self.manager.drones: dist = (drone.getLastSentPos() - self.getPos()).length() if drone.id == self.id: # prevent drone from detecting itself continue if dist < self.SENSORRANGE: nearbyDrones.append(drone) # calculate and apply forces for nearbyDrone in nearbyDrones: distVec = nearbyDrone.getLastSentPos() - self.getPos() if distVec.length() < 0.2: print("BONK") distMult = self.SENSORRANGE - distVec.length() avoidanceDirection = self.randVec.normalized( ) * 2 - distVec.normalized() * 10 avoidanceDirection.normalize() self.addForce(avoidanceDirection * distMult * self.AVOIDANCEFORCE) def _clampForce(self): """Clamps the total force acting in the drone.""" totalForce = self.rigidBody.getTotalForce() if totalForce.length() > 2: self.rigidBody.clearForces() self.rigidBody.applyCentralForce(totalForce.normalized()) def targetVector(self) -> Vec3: """Returns the vector from the drone to its target.""" return self.target - self.getPos() def _printDebugInfo(self): if self.printDebugInfo: pass def setTarget(self, target: Vec3): """Sets a new target for the drone.""" self.target = target def setRandomTarget(self): """Sets a new random target for the drone.""" self.target = self.manager.getRandomRoomCoordinate() def setNewRandVec(self): self.randVec = Vec3(random.uniform(-1, 1), random.uniform(-1, 1), random.uniform(-1, 1)) def addForce(self, force: Vec3): self.rigidBody.applyCentralForce(force) def setPos(self, position: Vec3): self.rigidBodyNP.setPos(position) def getVel(self) -> Vec3: return self.rigidBody.getLinearVelocity() def setVel(self, velocity: Vec3): return self.rigidBody.setLinearVelocity(velocity) def _drawTargetLine(self): self.targetLineNP.removeNode() ls = LineSegs() # ls.setThickness(1) ls.setColor(1.0, 0.0, 0.0, 1.0) ls.moveTo(self.getPos()) ls.drawTo(self.target) node = ls.create() self.targetLineNP = self.base.render.attachNewNode(node) def _drawVelocityLine(self): self.velocityLineNP.removeNode() ls = LineSegs() # ls.setThickness(1) ls.setColor(0.0, 0.0, 1.0, 1.0) ls.moveTo(self.getPos()) ls.drawTo(self.getPos() + self.getVel()) node = ls.create() self.velocityLineNP = self.base.render.attachNewNode(node) def _drawForceLine(self): self.forceLineNP.removeNode() ls = LineSegs() # ls.setThickness(1) ls.setColor(0.0, 1.0, 0.0, 1.0) ls.moveTo(self.getPos()) ls.drawTo(self.getPos() + self.rigidBody.getTotalForce() * 0.2) node = ls.create() self.forceLineNP = self.base.render.attachNewNode(node) def _drawSetpointLine(self): self.setpointNP.removeNode() ls = LineSegs() # ls.setThickness(1) ls.setColor(1.0, 1.0, 1.0, 1.0) ls.moveTo(self.getPos()) ls.drawTo(self.setpoint) node = ls.create() self.setpointNP = self.base.render.attachNewNode(node) def _wait_for_position_estimator(self): """Waits until the position estimator reports a consistent location after resetting.""" print('Waiting for estimator to find position...') log_config = LogConfig(name='Kalman Variance', period_in_ms=500) log_config.add_variable('kalman.varPX', 'float') log_config.add_variable('kalman.varPY', 'float') log_config.add_variable('kalman.varPZ', 'float') var_y_history = [1000] * 10 var_x_history = [1000] * 10 var_z_history = [1000] * 10 threshold = 0.001 with SyncLogger(self.scf, log_config) as logger: for log_entry in logger: data = log_entry[1] var_x_history.append(data['kalman.varPX']) var_x_history.pop(0) var_y_history.append(data['kalman.varPY']) var_y_history.pop(0) var_z_history.append(data['kalman.varPZ']) var_z_history.pop(0) min_x = min(var_x_history) max_x = max(var_x_history) min_y = min(var_y_history) max_y = max(var_y_history) min_z = min(var_z_history) max_z = max(var_z_history) if (max_x - min_x) < threshold and ( max_y - min_y) < threshold and (max_z - min_z) < threshold: break def _reset_estimator(self): """Resets the position estimator, this should be run before flying the drones or they might report a wrong position.""" cf = self.scf.cf cf.param.set_value('kalman.resetEstimation', '1') time.sleep(0.1) cf.param.set_value('kalman.resetEstimation', '0') self._wait_for_position_estimator() def position_callback(self, timestamp, data, logconf): """Updates the variable holding the position of the actual drone. It is not called in the update method, but by the drone itself (I think).""" x = data['kalman.stateX'] y = data['kalman.stateY'] z = data['kalman.stateZ'] self.realDronePosition = Vec3(x, y, z) # print('pos: ({}, {}, {})'.format(x, y, z)) def start_position_printing(self): """Activate logging of the position of the real drone.""" log_conf = LogConfig(name='Position', period_in_ms=50) log_conf.add_variable('kalman.stateX', 'float') log_conf.add_variable('kalman.stateY', 'float') log_conf.add_variable('kalman.stateZ', 'float') self.scf.cf.log.add_config(log_conf) log_conf.data_received_cb.add_callback(self.position_callback) log_conf.start()