def fire(self, shooterPosition, targetPosition): print("fire balls") # create bullet rigid body node sphereBRBN = BulletRigidBodyNode('Ball') # set physics properties sphereBRBN.setMass(1.0) sphereBRBN.addShape(BulletSphereShape(0.2)) # attach to render and get a nodePath in render hierarchic sphere = self.render.attachNewNode(sphereBRBN) # set starting position of fire ball pos = shooterPosition pos.setZ(pos.getZ() + 1) sphere.setPos(pos) # load texture of fireball and set size then add to nodePath smileyFace = self.loader.loadModel("models/smiley") smileyFace.setScale(0.2) smileyFace.reparentTo(sphere) # add rigid body to physics world self.world.attachRigidBody(sphereBRBN) # apply the force so it moves shootingDirection = targetPosition - pos shootingDirection.normalize() sphereBRBN.applyCentralForce(shootingDirection * 1000) self.fireballs.append(sphereBRBN) self.cleanUp()
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 Drone: """ Class for a single drone in the simulation. """ RIGID_BODY_MASS = .5 # Mass of physics object COLLISION_SPHERE_RADIUS = .1 # Radius of the bullet collision sphere around the drone FRICTION = 0 # No friction between drone and objects, not really necessary TARGET_FORCE_MULTIPLIER = 1 # Allows to change influence of the force applied to get to the target AVOIDANCE_FORCE_MULTIPLIER = 10 # Allows to change influence of the force applied to avoid collisions TARGET_PROXIMITY_RADIUS = .5 # Drone reduces speed when in proximity of a target AVOIDANCE_PROXIMITY_RADIUS = .6 # Distance when an avoidance manoeuvre has to be done # TODO: Check if other values are better here (and find out what they really do as well..) LINEAR_DAMPING = 0.95 LINEAR_SLEEP_THRESHOLD = 0 def __init__(self, manager, number): """ Initialises the drone as a bullet and panda object. :param manager: The drone manager creating this very drone. """ self.manager = manager # Drone manager handling this drone self.base = manager.base # Simulation self.crazyflie = None # object of real drone, if connected to one self.debug = False # If debugging info should be given self.in_flight = False # If currently in flight self.number = number # Number of drone in list # Every drone has its own vector to follow if an avoidance manouver has to be done self.avoidance_vector = LVector3f(random.uniform(-1, 1), random.uniform(-1, 1), random.uniform(-1, 1)).normalize() # Create bullet rigid body for drone drone_collision_shape = BulletSphereShape(self.COLLISION_SPHERE_RADIUS) self.drone_node_bullet = BulletRigidBodyNode("RigidSphere") self.drone_node_bullet.addShape(drone_collision_shape) self.drone_node_bullet.setMass(self.RIGID_BODY_MASS) # Set some values for the physics object self.drone_node_bullet.setLinearSleepThreshold( self.LINEAR_SLEEP_THRESHOLD) self.drone_node_bullet.setFriction(self.FRICTION) self.drone_node_bullet.setLinearDamping(self.LINEAR_DAMPING) # Attach to the simulation self.drone_node_panda = self.base.render.attachNewNode( self.drone_node_bullet) # ...and physics engine self.base.world.attachRigidBody(self.drone_node_bullet) # Add a model to the drone to be actually seen in the simulation drone_model = self.base.loader.loadModel( "models/drones/drone_florian.egg") drone_model.setScale(0.2) drone_model.reparentTo(self.drone_node_panda) # Set the position and target position to their default (origin) default_position = LPoint3f(0, 0, 0) self.drone_node_panda.setPos(default_position) self.target_position = default_position # Create a line renderer to draw a line from center to target point self.line_creator = LineSegs() # Then draw a default line so that the update function works as expected (with the removal) self.target_line_node = self.base.render.attachNewNode( self.line_creator.create(False)) # Create node for text self.drone_text_node_panda = None def get_pos(self) -> LPoint3f: """ Get the position of the drone. :return: Position of the drone as an LPoint3 object """ return self.drone_node_panda.getPos() def set_pos(self, position: LPoint3f): """ Set position of the drone. This directly sets the drone to that position, without any transition or flight. :param position: The position the drone is supposed to be at. """ self.drone_node_panda.setPos(position) def get_target(self) -> LPoint3f: """ Get the current target position of the drone. :return: The target position as a LPoint3 object. """ return self.target_position def set_target(self, position: LPoint3f): """ Set the target position of the drone. :param position: The target position to be set. """ self.target_position = position def set_debug(self, active): """ De-/activate debug information such as lines showing forces and such. :param active: If debugging should be turned on or off. """ self.debug = active if active: # Create a line so the updater can update it self.target_line_node = self.base.render.attachNewNode( self.line_creator.create(False)) # Write address of drone above model self._draw_cf_name(True) else: self.target_line_node.removeNode() self._draw_cf_name(False) def update(self): """ Update the drone and its forces. """ # Update the force needed to get to the target self._update_target_force() # Update the force needed to avoid other drones, if any self._update_avoidance_force() # Combine all acting forces and normalize them to one acting force self._combine_forces() # Update the line drawn to the current target if self.debug: self._draw_target_line() # Update real drone if connected to one if self.crazyflie is not None and self.in_flight: current_pos = self.get_pos() self.crazyflie.cf.commander.send_position_setpoint( current_pos.y, -current_pos.x, current_pos.z, 0) # print("Update!") def _update_target_force(self): """ Calculates force needed to get to the target and applies it. """ distance = (self.get_target() - self.get_pos() ) # Calculate distance to fly # Normalise distance to get an average force for all drones, unless in close proximity of target, # then slowing down is encouraged and the normalisation is no longer needed # If normalisation would always take place overshoots would occur if distance > self.TARGET_PROXIMITY_RADIUS: distance = distance.normalized() # Apply to drone (with force multiplier in mind) self.drone_node_bullet.applyCentralForce(distance * self.TARGET_FORCE_MULTIPLIER) def _update_avoidance_force(self): """ Applies a force to drones that are to close to each other and to avoid crashes. """ # Save all drones in near proximity with their distance near = [] for drone in self.manager.drones: distance = len(drone.get_pos() - self.get_pos()) if 0 < distance < self.AVOIDANCE_PROXIMITY_RADIUS: # Check dist > 0 to prevent drone from detecting itself near.append(drone) # Calculate and apply forces for every opponent for opponent in near: # Vector to other drone opponent_vector = opponent.get_pos() - self.get_pos() # Multiplier to maximise force when opponent gets closer multiplier = self.AVOIDANCE_PROXIMITY_RADIUS - len(opponent_vector) # Calculate a direction follow (multipliers found by testing) avoidance_direction = self.avoidance_vector * 2 - opponent_vector.normalized( ) * 10 avoidance_direction.normalize() # Apply avoidance force self.drone_node_bullet.applyCentralForce( avoidance_direction * multiplier * self.AVOIDANCE_FORCE_MULTIPLIER) def _combine_forces(self): """ Combine all acting forces to one normalised force. """ total_force = self.drone_node_bullet.getTotalForce() # Only do so if force is sufficiently big to retain small movements if total_force.length() > 2: self.drone_node_bullet.clearForces() self.drone_node_bullet.applyCentralForce(total_force.normalized()) def destroy(self): """ Detach drone from the simulation and physics engine, then destroy object. """ self.drone_node_panda.removeNode() self.base.world.removeRigidBody(self.drone_node_bullet) self.target_line_node.removeNode() if self.debug: self.drone_text_node_panda.removeNode() def _draw_target_line(self): """ Draw a line from the center to the target position in the simulation. """ # Remove the former line self.target_line_node.removeNode() # Create a new one self.line_creator.setColor(1.0, 0.0, 0.0, 1.0) self.line_creator.moveTo(self.get_pos()) self.line_creator.drawTo(self.target_position) line = self.line_creator.create(False) # And attach it to the renderer self.target_line_node = self.base.render.attachNewNode(line) def _draw_cf_name(self, draw): """ Show the address of the connected Craziefly, if there is one, above the model. """ if draw: address = 'Not connected' if self.crazyflie is not None: address = self.crazyflie.cf.link_uri text = TextNode('droneInfo') text.setText(str(self.number) + '\n' + address) text.setAlign(TextNode.ACenter) self.drone_text_node_panda = self.base.render.attachNewNode(text) self.drone_text_node_panda.setScale(.1) self.drone_text_node_panda.setPos(self.drone_node_panda, 0, 0, .3) else: self.drone_text_node_panda.removeNode()
class Player(Entity): walkspeed = 50 damping = 0.9 topspeed = 15 leftMove = False rightMove = False jumpToggle = False crouchToggle = False def __init__(self, world): super(Player, self).__init__() self.obj = utilities.loadObject("player", depth=20) self.world = world self.health = 100 self.inventory = dict() self.depth = self.obj.getPos().y self.location = Point2(0,0) self.velocity = Vec3(0) self.pt = 0.0 self.shape = BulletBoxShape(Vec3(0.3, 1.0, 0.49)) self.bnode = BulletRigidBodyNode('Box') self.bnode.setMass(1.0) self.bnode.setAngularVelocity(Vec3(0)) self.bnode.setAngularFactor(Vec3(0)) self.bnode.addShape(self.shape) self.bnode.setLinearDamping(0.95) self.bnode.setLinearSleepThreshold(0) world.bw.attachRigidBody(self.bnode) self.bnode.setPythonTag("Entity", self) self.bnode.setIntoCollideMask(BitMask32.bit(0)) self.node = utilities.app.render.attachNewNode(self.bnode) self.node.setPos(self.obj.getPos()) self.obj.setPos(0,0,0) self.obj.setScale(1) self.obj.reparentTo(self.node) self.node.setPos(self.location.x, self.depth, self.location.y) def initialise(self): self.inventory["LightLaser"] = LightLaser(self.world, self) self.inventory["Blowtorch"] = Blowtorch(self.world, self) self.inventory["Grenade"] = Grenade(self.world, self) for i in self.inventory: self.inventory[i].initialise() self.currentItem = self.inventory["Blowtorch"] self.currentItem.equip() self.armNode = self.obj.attachNewNode("arm") self.armNode.setPos(0.20,0,0.08) self.arm = utilities.loadObject("arm", scaleX = 0.5,scaleY = 0.5, depth = -0.2) self.arm.reparentTo(self.armNode) def activate(self): self.currentItem.activate() def update(self, timer): self.velocity = self.bnode.getLinearVelocity() if (self.leftMove): self.bnode.applyCentralForce(Vec3(-Player.walkspeed,0,0)) if (self.rightMove): self.bnode.applyCentralForce(Vec3(Player.walkspeed,0,0)) if (self.jumpToggle): self.bnode.applyCentralForce(Vec3(0,0,Player.walkspeed)) if (self.crouchToggle): self.bnode.applyCentralForce(Vec3(0,0,-Player.walkspeed)) if (self.velocity.x < -self.topspeed): self.velocity.x = -self.topspeed if (self.velocity.x > self.topspeed): self.velocity.x = self.topspeed mouse = utilities.app.mousePos # extrude test near = Point3() far = Point3() utilities.app.camLens.extrude(mouse, near, far) camp = utilities.app.camera.getPos() near *= 20 # player depth if near.x != 0: angle = atan2(near.z + camp.z - self.node.getPos().z, near.x + camp.x - self.node.getPos().x) #angle = atan2(near.z, near.x) else: angle = 90 self.angle = angle # set current item to point at cursor self.currentItem.update(timer) # move the camera so the player is centred horizontally, # but keep the camera steady vertically utilities.app.camera.setPos(self.node.getPos().x, 0, self.node.getPos().z) #move arm into correct position. gunLength = 2.0 self.gunVector = Point2(cos(angle)*gunLength - self.armNode.getX()*5, sin(angle)*gunLength - self.armNode.getZ()*2) armAngle = atan2(self.gunVector.y, self.gunVector.x) self.arm.setHpr(self.armNode, 0, 0, -1 * degrees(armAngle)) def moveLeft(self, switch): self.leftMove = switch #self.bnode.applyCentralForce(Vec3(-500,0,0)) def moveRight(self, switch): self.rightMove = switch #self.bnode.applyCentralForce(Vec3(500,0,0)) def jump(self, switch): self.jumpToggle = switch def crouch(self, switch): self.crouchToggle = switch
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 DroneEnv(gym.Env): def __init__(self): self.visualize = False if self.visualize == False: from pandac.PandaModules import loadPrcFileData loadPrcFileData("", "window-type none") import direct.directbase.DirectStart self.ep = 0 self.ep_rew = 0 self.t = 0 self.prevDis = 0 self.action_space = Box(-1, 1, shape=(3, )) self.observation_space = Box(-50, 50, shape=(9, )) self.target = 8 * np.random.rand(3) self.construct() self.percentages = [] self.percentMean = [] self.percentStd = [] taskMgr.add(self.stepTask, 'update') taskMgr.add(self.lightTask, 'lights') self.rotorForce = np.array([0, 0, 9.81], dtype=np.float) def construct(self): if self.visualize: base.cam.setPos(17, 17, 1) base.cam.lookAt(0, 0, 0) wp = WindowProperties() wp.setSize(1200, 500) base.win.requestProperties(wp) # World self.world = BulletWorld() self.world.setGravity(Vec3(0, 0, -9.81)) #skybox skybox = loader.loadModel('../models/skybox.gltf') skybox.setTexGen(TextureStage.getDefault(), TexGenAttrib.MWorldPosition) skybox.setTexProjector(TextureStage.getDefault(), render, skybox) skybox.setTexScale(TextureStage.getDefault(), 1) skybox.setScale(100) skybox.setHpr(0, -90, 0) tex = loader.loadCubeMap('../textures/s_#.jpg') skybox.setTexture(tex) skybox.reparentTo(render) render.setTwoSided(True) #Light plight = PointLight('plight') plight.setColor((1.0, 1.0, 1.0, 1)) plnp = render.attachNewNode(plight) plnp.setPos(0, 0, 0) render.setLight(plnp) # Create Ambient Light ambientLight = AmbientLight('ambientLight') ambientLight.setColor((0.15, 0.05, 0.05, 1)) ambientLightNP = render.attachNewNode(ambientLight) render.setLight(ambientLightNP) # Drone shape = BulletBoxShape(Vec3(0.5, 0.5, 0.5)) self.drone = BulletRigidBodyNode('Box') self.drone.setMass(1.0) self.drone.addShape(shape) self.droneN = render.attachNewNode(self.drone) self.droneN.setPos(0, 0, 3) self.world.attachRigidBody(self.drone) model = loader.loadModel('../models/drone.gltf') model.setHpr(0, 90, 0) model.flattenLight() model.reparentTo(self.droneN) blade = loader.loadModel("../models/blade.gltf") blade.reparentTo(self.droneN) blade.setHpr(0, 90, 0) blade.setPos(Vec3(0.3, -3.0, 1.1)) rotation_interval = blade.hprInterval(0.2, Vec3(180, 90, 0)) rotation_interval.loop() placeholder = self.droneN.attachNewNode("blade-placeholder") placeholder.setPos(Vec3(0, 6.1, 0)) blade.instanceTo(placeholder) placeholder = self.droneN.attachNewNode("blade-placeholder") placeholder.setPos(Vec3(3.05, 3.0, 0)) blade.instanceTo(placeholder) placeholder = self.droneN.attachNewNode("blade-placeholder") placeholder.setPos(Vec3(-3.05, 3.0, 0)) blade.instanceTo(placeholder) #drone ligths self.plight2 = PointLight('plight') self.plight2.setColor((0.9, 0.1, 0.1, 1)) plnp = self.droneN.attachNewNode(self.plight2) plnp.setPos(0, 0, -1) self.droneN.setLight(plnp) #over light plight3 = PointLight('plight') plight3.setColor((0.9, 0.8, 0.8, 1)) plnp = self.droneN.attachNewNode(plight3) plnp.setPos(0, 0, 2) self.droneN.setLight(plnp) #target object self.targetObj = loader.loadModel("../models/target.gltf") self.targetObj.reparentTo(render) self.targetObj.setPos( Vec3(self.target[0], self.target[1], self.target[2])) def lightTask(self, task): count = globalClock.getFrameCount() rest = count % 100 if rest < 10: self.plight2.setColor((0.1, 0.9, 0.1, 1)) elif rest > 30 and rest < 40: self.plight2.setColor((0.9, 0.1, 0.1, 1)) elif rest > 65 and rest < 70: self.plight2.setColor((0.9, 0.9, 0.9, 1)) elif rest > 75 and rest < 80: self.plight2.setColor((0.9, 0.9, 0.9, 1)) else: self.plight2.setColor((0.1, 0.1, 0.1, 1)) return task.cont def getState(self): #vel = self.drone.get_linear_velocity() po = self.drone.transform.pos ang = self.droneN.getHpr() #velocity = np.nan_to_num(np.array([vel[0], vel[1], vel[2]])) position = np.array([po[0], po[1], po[2]]) state = np.array([position, self.target]).reshape(6, ) state = np.around(state, decimals=3) return state def getReward(self): reward = 0 s = self.getState() d = np.linalg.norm(s[0:3] - s[3:6]) if d < 20: reward = 20 - d reward = reward / 40 #/4000 #if d < self.prevDis : # reward *= 1.2 #self.prevDis = np.copy(d) return reward def reset(self): #log self.percentages.append(self.ep_rew) me = np.mean(self.percentages[-500:]) self.percentMean.append(me) self.percentStd.append(np.std(self.percentages[-500:])) s = self.getState() d = np.linalg.norm(np.abs(s[:3] - self.target)) ds = np.linalg.norm(s[:3] - np.array([0, 0, 4])) if self.ep % 50 == 0: self.PlotReward() print( f'{self.ep} {self.t} {self.ep_rew:+8.2f} {me:+6.2f} {d:6.2f} {ds:6.2f} {s}' ) #{s[:6]} #physics reset self.droneN.setPos(0, 0, 4) self.droneN.setHpr(0, 0, 0) self.drone.set_linear_velocity(Vec3(0, 0, 0)) self.drone.setAngularVelocity(Vec3(0, 0, 0)) self.rotorForce = np.array([0, 0, 9.81], dtype=np.float) #define new target: self.target = 8 * (2 * np.random.rand(3) - 1) #self.target = np.zeros((3)) self.target[2] = np.abs(self.target[2]) self.target[1] = np.abs(self.target[1]) self.targetObj.setPos( Vec3(self.target[0], self.target[1], self.target[2])) self.ep += 1 self.t = 0 self.ep_rew = 0 state = self.getState() return state def stepTask(self, task): dt = globalClock.getDt() if self.visualize: self.world.doPhysics(dt) else: self.world.doPhysics(0.1) self.drone.setDeactivationEnabled(False) #application of force force = Vec3(self.rotorForce[0], self.rotorForce[1], self.rotorForce[2]) self.drone.applyCentralForce(force) #should be action po = self.drone.transform.pos position = np.array([po[0], po[1], po[2]]) return task.cont def step(self, action): done = False reward = 0 self.t += 1 reward = self.getReward() self.ep_rew += reward state = self.getState() basis = np.array([0, 0, 9.81], dtype=np.float) self.rotorForce = basis + 0.2 * action #0.1 *action #10 sub steps in each step for i in range(5): c = taskMgr.step() self.rotorForce -= 0.05 * (self.rotorForce - basis) #time constraint if self.t > 150: done = True return state, reward, done, {} def PlotReward(self): c = range(len(self.percentages)) plt.plot(self.percentMean, c='b', alpha=0.8) plt.fill_between( c, np.array(self.percentMean) + np.array(self.percentStd), np.array(self.percentMean) - np.array(self.percentStd), color='g', alpha=0.3, label='Objective 1') plt.grid() plt.savefig('rews.png') plt.close()
class Player(object): #global playerNode #playerNode = NodePath('player') F_MOVE = 400.0 def __init__(self, btWorld): self._attachControls() self._initPhysics(btWorld) self._loadModel() taskMgr.add(self.mouseUpdate, 'player-task') taskMgr.add(self.moveUpdate, 'player-task') self._vforce = Vec3(0,0,0) #ItemHandling(self.playerNode) def _attachControls(self): #base.accept( "s" , self.__setattr__,["walk",self.STOP] ) base.accept("w", self.goForward) base.accept("w-up", self.nogoForward) base.accept("s", self.goBack) base.accept("s-up", self.nogoBack) base.accept("a", self.goLeft) base.accept("a-up", self.nogoLeft) base.accept("d", self.goRight) base.accept("d-up", self.nogoRight) base.accept("space", self.goUp) base.accept("space-up", self.nogoUp) def _loadModel(self): #self._model = loader.loadModel("../data/models/d1_sphere.egg") #self._model.reparentTo(self.playerNode) pl = base.cam.node().getLens() pl.setNear(0.2) base.cam.node().setLens(pl) def _initPhysics(self, world): rad = 1 shape = BulletSphereShape(rad) self._rb_node = BulletRigidBodyNode('Box') self._rb_node.setMass(80.0) self._rb_node.setFriction(1.8) self._rb_node.addShape(shape) self._rb_node.setAngularFactor(Vec3(0,0,0)) self._rb_node.setDeactivationEnabled(False, True) self.playerNode = render.attachNewNode(self._rb_node) self.playerNode.setPos(0, 0, 4) self.playerNode.setHpr(0,0,0) world.attachRigidBody(self._rb_node) self.camNode = self.playerNode.attachNewNode("cam node") base.camera.reparentTo(self.camNode) self.camNode.setPos(self.camNode, 0, 0, 1.75-rad) #self.camNode.setPos(self.camNode, 0, -5,0.5) #self.camNode.lookAt(Point3(0,0,0),Vec3(0,1,0)) def mouseUpdate(self,task): md = base.win.getPointer(0) dx = md.getX() - base.win.getXSize()/2.0 dy = md.getY() - base.win.getYSize()/2.0 yaw = dx/(base.win.getXSize()/2.0) * 90 self.camNode.setHpr(self.camNode, -yaw, 0, 0) #base.camera.setHpr(base.camera, yaw, pith, roll) base.win.movePointer(0, base.win.getXSize() / 2, base.win.getYSize() / 2) return task.cont def moveUpdate(self,task): if (self._vforce.length() > 0): #self.rbNode.applyCentralForce(self._vforce) self._rb_node.applyCentralForce(self.playerNode.getRelativeVector(self.camNode, self._vforce)) else: pass return task.cont def goForward(self): self._vforce.setY( self.F_MOVE) def nogoForward(self): self._vforce.setY( 0) def goBack(self): self._vforce.setY(-self.F_MOVE) def nogoBack(self): self._vforce.setY( 0) def goLeft(self): self._vforce.setX(-self.F_MOVE) def nogoLeft(self): self._vforce.setX( 0) def goRight(self): self._vforce.setX( self.F_MOVE) def nogoRight(self): self._vforce.setX( 0) def goUp(self): self._vforce.setZ( self.F_MOVE*1.4) def nogoUp(self): self._vforce.setZ( 0) def getRBNode(self): return self._rb_node
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 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()
class Player(Entity): walkspeed = 5 damping = 0.9 topspeed = 15 leftMove = False rightMove = False jumpToggle = False crouchToggle = False def __init__(self, world): super(Player, self).__init__() self.obj = utilities.loadObject("tdplayer", depth=20) self.world = world self.health = 100 self.inventory = list() self.depth = self.obj.getPos().y self.location = Point2(10,0) self.velocity = Vec3(0) self.shape = BulletBoxShape(Vec3(0.3, 1.0, 0.49)) self.bnode = BulletRigidBodyNode('Box') self.bnode.setMass(0.1) self.bnode.setAngularVelocity(Vec3(0)) self.bnode.setAngularFactor(Vec3(0)) self.bnode.addShape(self.shape) self.bnode.setLinearDamping(0.95) self.bnode.setLinearSleepThreshold(0) world.bw.attachRigidBody(self.bnode) self.bnode.setPythonTag("entity", self) self.bnode.setIntoCollideMask(BitMask32.bit(0)) self.node = utilities.app.render.attachNewNode(self.bnode) self.node.setPos(self.obj.getPos()) self.obj.setPos(0,0,0) self.obj.setScale(1) self.obj.reparentTo(self.node) self.node.setPos(self.location.x, self.depth, self.location.y) def initialise(self): self.inventory.append(LightLaser(self.world, self)) self.inventory.append(Blowtorch(self.world, self)) #self.inventory["Grenade"] = Grenade(self.world, self) for item in self.inventory: item.initialise() self.currentItemIndex = 1 self.currentItem = self.inventory[self.currentItemIndex] self.currentItem.equip() def activate(self): self.currentItem.activate() def update(self, timer): self.velocity = self.bnode.getLinearVelocity() if (self.leftMove): self.bnode.applyCentralForce(Vec3(-Player.walkspeed,0,0)) if (self.rightMove): self.bnode.applyCentralForce(Vec3(Player.walkspeed,0,0)) if (self.jumpToggle): self.bnode.applyCentralForce(Vec3(0,0,Player.walkspeed)) if (self.crouchToggle): self.bnode.applyCentralForce(Vec3(0,0,-Player.walkspeed)) if (self.velocity.x < -self.topspeed): self.velocity.x = -self.topspeed if (self.velocity.x > self.topspeed): self.velocity.x = self.topspeed mouse = utilities.app.mousePos # extrude test near = Point3() far = Point3() utilities.app.camLens.extrude(mouse, near, far) camp = utilities.app.camera.getPos() near *= 20 # player depth if near.x != 0: angle = atan2(near.z + camp.z - self.node.getPos().z, near.x + camp.x - self.node.getPos().x) #angle = atan2(near.z, near.x) else: angle = 90 self.angle = angle # set current item to point at cursor self.currentItem.update(timer) # move the camera so the player is centred horizontally, # but keep the camera steady vertically utilities.app.camera.setPos(self.node.getPos().x, 0, self.node.getPos().z) self.obj.setHpr(0, 0, -1 * degrees(angle)) self.location.x = self.node.getPos().x self.location.y = self.node.getPos().z def moveLeft(self, switch): self.leftMove = switch def moveRight(self, switch): self.rightMove = switch def jump(self, switch): self.jumpToggle = switch def crouch(self, switch): self.crouchToggle = switch def scrollItem(self, number): self.currentItem.unequip() self.currentItemIndex = self.currentItemIndex + number if self.currentItemIndex < 0: self.currentItemIndex = len(self.inventory) - 1 if self.currentItemIndex > len(self.inventory) - 1: self.currentItemIndex = 0 self.currentItem = self.inventory[self.currentItemIndex] self.currentItem.equip() def selectItem(self, number): if (number - 1 < len(self.inventory) and number - 1 >= 0): self.currentItem.unequip() self.currentItemIndex = number - 1 self.currentItem = self.inventory[self.currentItemIndex] self.currentItem.equip() def hitby(self, projectile, index): self.health -= projectile.damage if (self.health < 0): self.obj.setColor(1,0,0,1) return True greyscale = 0.3 + (0.01 * self.health) self.obj.setColor(greyscale, greyscale,greyscale,greyscale) return False
class Catcher(Enemy): damage = 25 def __init__(self, location, player, cmap, world): super(Catcher, self).__init__(location) self.player = player self.cmap = cmap self.obj = utilities.loadObject("robot", depth=20) self.world = world self.health = 100 self.depth = self.obj.getPos().y self.location = location self.velocity = Vec3(0) self.shape = BulletBoxShape(Vec3(0.5, 1.0, 0.5)) self.bnode = BulletRigidBodyNode('Box') self.bnode.setMass(0.1) self.bnode.setAngularVelocity(Vec3(0)) self.bnode.setAngularFactor(Vec3(0)) self.bnode.addShape(self.shape) self.bnode.setLinearDamping(0.75) self.bnode.setLinearSleepThreshold(0) world.bw.attachRigidBody(self.bnode) self.bnode.setPythonTag("entity", self) self.bnode.setIntoCollideMask(BitMask32.bit(0)) self.node = utilities.app.render.attachNewNode(self.bnode) self.node.setPos(self.obj.getPos()) self.obj.setPos(0,0,0) self.obj.setScale(1) self.obj.reparentTo(self.node) self.node.setPos(self.location.x, self.depth, self.location.y) def update(self, time): self.location = Point2(self.node.getPos().x - self.player.location.x, self.node.getPos().z - self.player.location.y) if self.location.x > 20 or self.location.x < -20: return if self.location.y > 20 or self.location.y < -20: return path = findPath(Point2(self.location + Point2(20,20)), Point2(20,20), self.world.cmap) if len(path) > 1: move = path[1] - self.location self.bnode.applyCentralForce(Vec3(move.x-20,0,move.y-20)) def hitby(self, projectile, index): self.health -= projectile.damage if (self.health < 0): self.remove = True greyscale = 0.3 + (0.01 * self.health) self.obj.setColor(1, greyscale,greyscale,1) return False def removeOnHit(self): return def destroy(self): self.obj.hide() self.world.bw.removeRigidBody(self.bnode)
class Catcher(Enemy): damage = 25 def __init__(self, location, player, cmap, world): super(Catcher, self).__init__(location) self.player = player self.cmap = cmap self.obj = utilities.loadObject("robot", depth=20) self.world = world self.health = 100 self.depth = self.obj.getPos().y self.location = location self.velocity = Vec3(0) self.shape = BulletBoxShape(Vec3(0.5, 1.0, 0.5)) self.bnode = BulletRigidBodyNode('Box') self.bnode.setMass(0.1) self.bnode.setAngularVelocity(Vec3(0)) self.bnode.setAngularFactor(Vec3(0)) self.bnode.addShape(self.shape) self.bnode.setLinearDamping(0.75) self.bnode.setLinearSleepThreshold(0) world.bw.attachRigidBody(self.bnode) self.bnode.setPythonTag("entity", self) self.bnode.setIntoCollideMask(BitMask32.bit(0)) self.node = utilities.app.render.attachNewNode(self.bnode) self.node.setPos(self.obj.getPos()) self.obj.setPos(0, 0, 0) self.obj.setScale(1) self.obj.reparentTo(self.node) self.node.setPos(self.location.x, self.depth, self.location.y) def update(self, time): self.location = Point2(self.node.getPos().x - self.player.location.x, self.node.getPos().z - self.player.location.y) if self.location.x > 20 or self.location.x < -20: return if self.location.y > 20 or self.location.y < -20: return path = findPath(Point2(self.location + Point2(20, 20)), Point2(20, 20), self.world.cmap) if len(path) > 1: move = path[1] - self.location self.bnode.applyCentralForce(Vec3(move.x - 20, 0, move.y - 20)) def hitby(self, projectile, index): self.health -= projectile.damage if (self.health < 0): self.remove = True greyscale = 0.3 + (0.01 * self.health) self.obj.setColor(1, greyscale, greyscale, 1) return False def removeOnHit(self): return def destroy(self): self.obj.hide() self.world.bw.removeRigidBody(self.bnode)
class Player(Entity): walkspeed = 5 damping = 0.9 topspeed = 15 leftMove = False rightMove = False jumpToggle = False crouchToggle = False def __init__(self, world): super(Player, self).__init__() self.obj = utilities.loadObject("tdplayer", depth=20) self.world = world self.health = 100 self.inventory = list() self.depth = self.obj.getPos().y self.location = Point2(10, 0) self.velocity = Vec3(0) self.shape = BulletBoxShape(Vec3(0.3, 1.0, 0.49)) self.bnode = BulletRigidBodyNode('Box') self.bnode.setMass(0.1) self.bnode.setAngularVelocity(Vec3(0)) self.bnode.setAngularFactor(Vec3(0)) self.bnode.addShape(self.shape) self.bnode.setLinearDamping(0.95) self.bnode.setLinearSleepThreshold(0) world.bw.attachRigidBody(self.bnode) self.bnode.setPythonTag("entity", self) self.bnode.setIntoCollideMask(BitMask32.bit(0)) self.node = utilities.app.render.attachNewNode(self.bnode) self.node.setPos(self.obj.getPos()) self.obj.setPos(0, 0, 0) self.obj.setScale(1) self.obj.reparentTo(self.node) self.node.setPos(self.location.x, self.depth, self.location.y) def initialise(self): self.inventory.append(LightLaser(self.world, self)) self.inventory.append(Blowtorch(self.world, self)) #self.inventory["Grenade"] = Grenade(self.world, self) for item in self.inventory: item.initialise() self.currentItemIndex = 1 self.currentItem = self.inventory[self.currentItemIndex] self.currentItem.equip() def activate(self): self.currentItem.activate() def update(self, timer): self.velocity = self.bnode.getLinearVelocity() if (self.leftMove): self.bnode.applyCentralForce(Vec3(-Player.walkspeed, 0, 0)) if (self.rightMove): self.bnode.applyCentralForce(Vec3(Player.walkspeed, 0, 0)) if (self.jumpToggle): self.bnode.applyCentralForce(Vec3(0, 0, Player.walkspeed)) if (self.crouchToggle): self.bnode.applyCentralForce(Vec3(0, 0, -Player.walkspeed)) if (self.velocity.x < -self.topspeed): self.velocity.x = -self.topspeed if (self.velocity.x > self.topspeed): self.velocity.x = self.topspeed mouse = utilities.app.mousePos # extrude test near = Point3() far = Point3() utilities.app.camLens.extrude(mouse, near, far) camp = utilities.app.camera.getPos() near *= 20 # player depth if near.x != 0: angle = atan2(near.z + camp.z - self.node.getPos().z, near.x + camp.x - self.node.getPos().x) #angle = atan2(near.z, near.x) else: angle = 90 self.angle = angle # set current item to point at cursor self.currentItem.update(timer) # move the camera so the player is centred horizontally, # but keep the camera steady vertically utilities.app.camera.setPos(self.node.getPos().x, 0, self.node.getPos().z) self.obj.setHpr(0, 0, -1 * degrees(angle)) self.location.x = self.node.getPos().x self.location.y = self.node.getPos().z def moveLeft(self, switch): self.leftMove = switch def moveRight(self, switch): self.rightMove = switch def jump(self, switch): self.jumpToggle = switch def crouch(self, switch): self.crouchToggle = switch def scrollItem(self, number): self.currentItem.unequip() self.currentItemIndex = self.currentItemIndex + number if self.currentItemIndex < 0: self.currentItemIndex = len(self.inventory) - 1 if self.currentItemIndex > len(self.inventory) - 1: self.currentItemIndex = 0 self.currentItem = self.inventory[self.currentItemIndex] self.currentItem.equip() def selectItem(self, number): if (number - 1 < len(self.inventory) and number - 1 >= 0): self.currentItem.unequip() self.currentItemIndex = number - 1 self.currentItem = self.inventory[self.currentItemIndex] self.currentItem.equip() def hitby(self, projectile, index): self.health -= projectile.damage if (self.health < 0): self.obj.setColor(1, 0, 0, 1) return True greyscale = 0.3 + (0.01 * self.health) self.obj.setColor(greyscale, greyscale, greyscale, greyscale) return False