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
