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 _initAgents(self):
# Load agents
for agent in self.scene.scene.findAllMatches('**/agents/agent*'):
transform = TransformState.makeIdentity()
if self.agentMode == 'capsule':
shape = BulletCapsuleShape(
self.agentRadius, self.agentHeight - 2 * self.agentRadius)
elif self.agentMode == 'sphere':
shape = BulletCapsuleShape(self.agentRadius,
2 * self.agentRadius)
# XXX: use BulletCharacterControllerNode class, which already handles local transform?
node = BulletRigidBodyNode('physics')
node.setMass(self.agentMass)
node.setStatic(False)
node.setFriction(self.defaultMaterialFriction)
node.setRestitution(self.defaultMaterialRestitution)
node.addShape(shape)
self.bulletWorld.attach(node)
# Constrain the agent to have fixed position on the Z-axis
node.setLinearFactor(Vec3(1.0, 1.0, 0.0))
# Constrain the agent not to be affected by rotations
node.setAngularFactor(Vec3(0.0, 0.0, 0.0))
node.setIntoCollideMask(BitMask32.allOn())
node.setDeactivationEnabled(True)
# Enable continuous collision detection (CCD)
node.setCcdMotionThreshold(1e-7)
node.setCcdSweptSphereRadius(0.50)
if node.isStatic():
agent.setTag('physics-mode', 'static')
else:
agent.setTag('physics-mode', 'dynamic')
# Attach the physic-related node to the scene graph
physicsNp = NodePath(node)
physicsNp.setTransform(transform)
# Reparent all child nodes below the new physic node
for child in agent.getChildren():
child.reparentTo(physicsNp)
physicsNp.reparentTo(agent)
# NOTE: we need this to update the transform state of the internal bullet node
physicsNp.node().setTransformDirty()
# Validation
assert np.allclose(
mat4ToNumpyArray(physicsNp.getNetTransform().getMat()),
mat4ToNumpyArray(agent.getNetTransform().getMat()),
atol=1e-6)
def createPlane(self):
rb = BulletRigidBodyNode('Ground')
rb.addShape(BulletPlaneShape(Vec3(0, 0, 1), 1))
rb.setFriction(1.0)
np = self.render.attachNewNode(rb)
np.setPos(Vec3(0, 0, -100))
np.setCollideMask(BitMask32.bit(0))
self.world.attachRigidBody(rb)
return np
def _initLayoutModels(self):
# Load layout objects as meshes
for model in self.scene.scene.findAllMatches('**/layouts/object*/model*'):
# NOTE: ignore models that have no geometry defined
if model.getTightBounds() is None:
logger.warning('Object %s has no geometry defined and will be ignored for physics!' % (str(model)))
continue
shape, transform = getCollisionShapeFromModel(
model, mode='mesh', defaultCentered=False)
node = BulletRigidBodyNode('physics')
node.setMass(0.0)
node.setFriction(self.defaultMaterialFriction)
node.setRestitution(self.defaultMaterialRestitution)
node.setStatic(True)
node.addShape(shape)
node.setDeactivationEnabled(True)
node.setIntoCollideMask(BitMask32.allOn())
self.bulletWorld.attach(node)
# Attach the physic-related node to the scene graph
physicsNp = model.getParent().attachNewNode(node)
physicsNp.setTransform(transform)
if node.isStatic():
model.getParent().setTag('physics-mode', 'static')
else:
model.getParent().setTag('physics-mode', 'dynamic')
# Reparent render and acoustics nodes (if any) below the new physic node
# XXX: should be less error prone to just reparent all children
# (except the hidden model)
renderNp = model.getParent().find('**/render')
if not renderNp.isEmpty():
renderNp.reparentTo(physicsNp)
acousticsNp = model.getParent().find('**/acoustics')
if not acousticsNp.isEmpty():
acousticsNp.reparentTo(physicsNp)
semanticsNp = model.getParent().find('**/semantics')
if not semanticsNp.isEmpty():
semanticsNp.reparentTo(physicsNp)
# NOTE: we need this to update the transform state of the internal
# bullet node
physicsNp.node().setTransformDirty()
# Validation
assert np.allclose(mat4ToNumpyArray(physicsNp.getNetTransform().getMat()),
mat4ToNumpyArray(model.getNetTransform().getMat()), atol=1e-6)
def createPlane(self, pos):
rb = BulletRigidBodyNode("plane")
rb.addShape(BulletPlaneShape(Vec3(0, 0, 1), 1))
rb.setFriction(1.0)
rb.setAnisotropicFriction(1.0)
rb.setRestitution(1.0)
np = self.render.attachNewNode(rb)
np.setPos(pos)
np.setCollideMask(BitMask32.bit(0))
self.world.attachRigidBody(rb)
return np
def spawn_block():
global world
node = BulletRigidBodyNode('Block')
node.setFriction(1.0)
block_np = render.attachNewNode(node)
shape = BulletBoxShape(Vec3(0.0254 * 4, 0.0254 * 24, 0.0254 * 2))
node.setMass(1.0)
block_np.setPos(-3.7, 0.0, 2.0)
block_np.setHpr(random.uniform(-60, 60), 0.0, 0.0)
node.addShape(shape)
world.attachRigidBody(node)
model = loader.loadModel('assets/bullet-samples/models/box.egg')
model.setH(90)
model.setSy(0.0254 * 4 * 2)
model.setSx(0.0254 * 24 * 2)
model.setSz(0.0254 * 2 * 2)
model.flattenLight()
model.reparentTo(block_np)
def spawn_block(self, location):
node = BulletRigidBodyNode('Block')
node.setFriction(1.0)
block_np = self.render.attachNewNode(node)
block_np.setAntialias(AntialiasAttrib.MMultisample)
shape = BulletBoxShape(Vec3(0.0254 * 4, 0.0254 * 24, 0.0254 * 2))
node.setMass(1.0)
#block_np.setPos(-3.7, 0.0, 2.0)
block_np.setPos(location)
block_np.setHpr(random.uniform(-60, 60), 0.0, 0.0)
node.addShape(shape)
self.world.attachRigidBody(node)
model = loader.loadModel('assets/bullet-samples/models/box.egg')
model.setH(90)
model.setSy(0.0254 * 4 * 2)
model.setSx(0.0254 * 24 * 2)
model.setSz(0.0254 * 2 * 2)
model.flattenLight()
model.reparentTo(block_np)
return block_np
def createPlane(self, pos):
rb = BulletRigidBodyNode("GroundPlane")
rb.addShape(BulletPlaneShape(Vec3(0, 0, 1), 1))
rb.setFriction(1.0)
rb.setAnisotropicFriction(1.0)
rb.setRestitution(1.0)
np = self.render.attachNewNode(rb)
np.setPos(pos)
np.setCollideMask(BitMask32.bit(0))
plane = self.loader.loadModel("cube")
plane.setScale(Vec3(100, 100, 1))
plane.setPos(Vec3(0, 0, -0.5))
plane.setColor(VBase4(0.8, 0.8, 0.8, 1.0))
plane.reparentTo(np)
self.world.attachRigidBody(rb)
return np
def __init__(self):
super(Terrain, self).__init__(random_seed=0)
shape = BulletPlaneShape(Vec3(0, 0, 1), 0)
node = BulletRigidBodyNode(BodyName.Ground)
node.setFriction(.9)
node.addShape(shape)
node.setIntoCollideMask(self.COLLISION_MASK)
self.dynamic_nodes.append(node)
self.origin.attachNewNode(node)
if self.render:
self.origin.hide(CamMask.MiniMap | CamMask.Shadow
| CamMask.DepthCam | CamMask.ScreenshotCam)
# self.terrain_normal = self.loader.loadTexture(
# AssetLoader.file_path( "textures", "grass2", "normal.jpg")
# )
self.terrain_texture = self.loader.loadTexture(
AssetLoader.file_path("textures", "ground.png"))
self.terrain_texture.setWrapU(Texture.WM_repeat)
self.terrain_texture.setWrapV(Texture.WM_repeat)
self.ts_color = TextureStage("color")
self.ts_normal = TextureStage("normal")
self.ts_normal.set_mode(TextureStage.M_normal)
self.origin.setPos(0, 0, self.HEIGHT)
cm = CardMaker('card')
scale = 20000
cm.setUvRange((0, 0), (scale / 10, scale / 10))
card = self.origin.attachNewNode(cm.generate())
# scale = 1 if self.use_hollow else 20000
card.set_scale(scale)
card.setPos(-scale / 2, -scale / 2, -0.1)
card.setZ(-.05)
card.setTexture(self.ts_color, self.terrain_texture)
# card.setTexture(self.ts_normal, self.terrain_normal)
self.terrain_texture.setMinfilter(
SamplerState.FT_linear_mipmap_linear)
self.terrain_texture.setAnisotropicDegree(8)
card.setQuat(
LQuaternionf(math.cos(-math.pi / 4), math.sin(-math.pi / 4), 0,
0))
def spawn_block(self, location, z_inches, y_inches, x_inches):
"""
Spawns a block
"""
node = BulletRigidBodyNode('Block')
node.setFriction(1.0)
block_np = self.render.attachNewNode(node)
shape = BulletBoxShape(Vec3(0.0254*y_inches, 0.0254*x_inches, 0.0254*z_inches))
node.setMass(1.0)
block_np.setPos(location)
block_np.setHpr(random.uniform(-60, 60), 0.0, 0.0)
node.addShape(shape)
self.world.attachRigidBody(node)
model = loader.loadModel('assets/bullet-samples/models/box.egg')
model.setH(90)
model.setSx(0.0254*x_inches*2)
model.setSy(0.0254*y_inches*2)
model.setSz(0.0254*z_inches*2)
model.flattenLight()
model.reparentTo(block_np)
block_np.node().setTag('scrambled', 'False')
return block_np
def createBox(self, mass, pos, scale, color):
rb = BulletRigidBodyNode('box')
rb.addShape(BulletBoxShape(scale))
rb.setMass(mass)
rb.setLinearDamping(0.2)
rb.setAngularDamping(0.9)
rb.setFriction(1.0)
rb.setAnisotropicFriction(1.0)
rb.setRestitution(0.0)
self.world.attachRigidBody(rb)
np = self.render.attachNewNode(rb)
np.setPos(pos)
np.setCollideMask(BitMask32.bit(1))
cube = self.loader.loadModel('cube')
cube.setScale(scale)
cube.setColor(color)
cube.reparentTo(np)
return np
class BulletNPC(DynamicObject):
def __init__(self, name, game, pos):
self.game = game
self.name = name
self.jumping = 0
self.crouching = 0
# maximum speed when only walking
self.groundSpeed = 4.0
# acceleration used when walking to rise to self.groundSpeed
self.groundAccel = 150.0
self.groundFriction = 0.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(self.name)
self.node.setMass(1.0)
self.node.addShape(self.shape)
self.node.setActive(True, True)
self.node.setDeactivationEnabled(False, True)
self.node.setFriction(self.groundFriction)
#self.node.setGravity(10)
#self.node.setFallSpeed(200)
self.node.setCcdMotionThreshold(1e-7)
# do not use setCcdSweptSphereRadius
# it messes up the bite contact test
#self.node.setCcdSweptSphereRadius(0.5)
self.node.setAngularFactor(Vec3(0, 0, 1))
self.np = self.game.render.attachNewNode(self.node)
self.setPos(pos)
self.setH(0)
#self.np.setCollideMask(BitMask32.allOn())
self.game.world.attachRigidBody(self.node)
self.playerModel = Actor(
"models/monsters/ghoul2.egg", {
"idle": "models/monsters/ghoul2-idle.egg",
"walk": "models/monsters/ghoul2-walk.egg"
})
self.playerModel.setScale(0.15)
self.playerModel.setZ(0.15)
self.playerModel.reparentTo(self.np)
interval = Wait(random.uniform(0, 1))
i2 = Func(self.startWalkAnim)
seq = Sequence()
seq.append(interval)
seq.append(i2)
seq.start()
self.growlTimer = 5.0
self.sound = None
self.alive = True
self.targetNp = NodePath(self.name)
self.brainTimer = 0.0
self.brainDelay = 1.2
self.currentForce = Vec3(0, 0, 0)
def startWalkAnim(self):
self.playerModel.loop("walk")
def stopWalkAnim(self):
self.playerModel.loop("idle")
def checkFront(self):
p1 = Point3(self.getPos())
direction = self.game.playerNp.getPos() - self.getPos()
direction.normalize()
direction = direction * 2.0
p2 = Point3(self.getPos() + direction)
result = self.game.world.rayTestClosest(p1, p2)
#ts1 = TransformState.makePos(p1)
#ts2 = TransformState.makePos(p2)
#result = self.game.world.sweepTestClosest(self.shape, ts1, ts2, 0)
if result.hasHit():
pos = result.getHitPos()
n = result.getHitNormal()
frac = result.getHitFraction()
node = result.getNode()
return node.getName()
return None
def checkRight(self):
p1 = Point3(self.getPos())
direction = self.game.playerNp.getPos() - self.getPos()
direction.normalize()
right = direction.cross(Vec3(0, 0, 1))
right = right * 2.0
p2 = Point3(self.getPos() + right)
result = self.game.world.rayTestClosest(p1, p2)
if result.hasHit():
pos = result.getHitPos()
n = result.getHitNormal()
frac = result.getHitFraction()
node = result.getNode()
return node.getName()
return None
def checkLeft(self):
p1 = Point3(self.getPos())
direction = self.game.playerNp.getPos() - self.getPos()
direction.normalize()
left = -direction.cross(Vec3(0, 0, 1))
left = left * 2.0
p2 = Point3(self.getPos() + left)
result = self.game.world.rayTestClosest(p1, p2)
if result.hasHit():
pos = result.getHitPos()
n = result.getHitNormal()
frac = result.getHitFraction()
node = result.getNode()
return node.getName()
return None
def updateDirection(self):
direction = self.game.playerNp.getPos() - self.getPos()
direction.setZ(0)
direction.normalize()
direction = direction * self.groundAccel
right = direction.cross(Vec3(0, 0, 1))
left = -right
inFront = self.checkFront()
inRight = self.checkRight()
inLeft = self.checkLeft()
if inFront == "boxpack":
if inRight == "boxpack":
if inLeft == "boxPack":
self.currentForce = -direction
else:
self.currentForce = left
else:
self.currentForce = right
else:
self.currentForce = direction
def update(self, dt=0.0):
if not self.alive:
return
self.brainTimer -= dt
if self.brainTimer <= 0.0:
self.updateDirection()
self.brainTimer = self.brainDelay
self.setForce(self.currentForce)
self.capSpeedXY()
speedVec = self.getSpeedVec()
self.targetNp.setPos(self.getPos() + speedVec)
self.lookAt(self.targetNp)
# growl!
self.growlTimer -= dt
if self.growlTimer <= 0.0:
if self.sound:
self.game.soundDic3D[self.sound].stop()
self.game.detachSound(self.sound)
growlIndex = random.randint(1, len(self.game.zombieGrowlSounds))
soundName = "monster-" + str(growlIndex)
self.sound = soundName
self.game.attachSound(soundName, self.np)
self.game.playSound3D(soundName)
self.growlTimer = 1.0 + random.uniform(0.0, 1.0)
#print "Growl from %s" % (self.name)
# bite! # player is the only one checking for that now
#res = self.getContacts()
#if "player" in res:
#print "%s took a bite on player at %s!" % (self.name, self.game.globalClock.getFrameTime())
#self.game.onDie()
def destroy(self):
self.alive = False
self.game.world.removeRigidBody(self.node)
self.stopWalkAnim()
self.np.setTransparency(True)
self.np.setColor(1, 1, 1, 1)
self.setGlowOff()
i1 = Wait(random.uniform(0, 1))
i2 = LerpColorInterval(self.np, 1.0, (1, 1, 1, 0))
i3 = Func(self.np.remove)
seq = Sequence()
seq.append(i1)
seq.append(i2)
seq.append(i3)
seq.start()
def _initObjects(self):
# Load objects
for model in self.scene.scene.findAllMatches(
'**/objects/object*/model*'):
modelId = model.getParent().getTag('model-id')
# XXX: we could create BulletGhostNode instance for non-collidable objects, but we would need to filter out the collisions later on
if not modelId in self.openedDoorModelIds:
shape, transform = getCollisionShapeFromModel(
model, self.objectMode, defaultCentered=True)
node = BulletRigidBodyNode('physics')
node.addShape(shape)
node.setFriction(self.defaultMaterialFriction)
node.setRestitution(self.defaultMaterialRestitution)
node.setIntoCollideMask(BitMask32.allOn())
node.setDeactivationEnabled(True)
if self.suncgDatasetRoot is not None:
# Check if it is a movable object
category = self.modelCatMapping.getCoarseGrainedCategoryForModelId(
modelId)
if category in self.movableObjectCategories:
# Estimate mass of object based on volumetric data and default material density
objVoxFilename = os.path.join(self.suncgDatasetRoot,
'object_vox',
'object_vox_data',
modelId,
modelId + '.binvox')
voxelData = ObjectVoxelData.fromFile(objVoxFilename)
mass = Panda3dBulletPhysics.defaultDensity * voxelData.getFilledVolume(
)
node.setMass(mass)
else:
node.setMass(0.0)
node.setStatic(True)
else:
node.setMass(0.0)
node.setStatic(True)
if node.isStatic():
model.getParent().setTag('physics-mode', 'static')
else:
model.getParent().setTag('physics-mode', 'dynamic')
# Attach the physic-related node to the scene graph
physicsNp = model.getParent().attachNewNode(node)
physicsNp.setTransform(transform)
# Reparent render and acoustics nodes (if any) below the new physic node
#XXX: should be less error prone to just reparent all children (except the hidden model)
renderNp = model.getParent().find('**/render')
if not renderNp.isEmpty():
renderNp.reparentTo(physicsNp)
acousticsNp = model.getParent().find('**/acoustics')
if not acousticsNp.isEmpty():
acousticsNp.reparentTo(physicsNp)
# NOTE: we need this to update the transform state of the internal bullet node
node.setTransformDirty()
# NOTE: we need to add the node to the bullet engine only after setting all attributes
self.bulletWorld.attach(node)
# Validation
assert np.allclose(
mat4ToNumpyArray(physicsNp.getNetTransform().getMat()),
mat4ToNumpyArray(
model.getParent().getNetTransform().getMat()),
atol=1e-6)
else:
logger.debug('Object %s ignored from physics' % (modelId))
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 BulletNPC(DynamicObject):
def __init__(self, name, game, pos):
self.game = game
self.name = name
self.jumping = 0
self.crouching = 0
# maximum speed when only walking
self.groundSpeed = 4.0
# acceleration used when walking to rise to self.groundSpeed
self.groundAccel = 150.0
self.groundFriction = 0.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(self.name)
self.node.setMass(1.0)
self.node.addShape(self.shape)
self.node.setActive(True, True)
self.node.setDeactivationEnabled(False, True)
self.node.setFriction(self.groundFriction)
# self.node.setGravity(10)
# self.node.setFallSpeed(200)
self.node.setCcdMotionThreshold(1e-7)
# do not use setCcdSweptSphereRadius
# it messes up the bite contact test
# self.node.setCcdSweptSphereRadius(0.5)
self.node.setAngularFactor(Vec3(0, 0, 1))
self.np = self.game.render.attachNewNode(self.node)
self.setPos(pos)
self.setH(0)
# self.np.setCollideMask(BitMask32.allOn())
self.game.world.attachRigidBody(self.node)
self.playerModel = Actor(
"models/monsters/ghoul2.egg",
{"idle": "models/monsters/ghoul2-idle.egg", "walk": "models/monsters/ghoul2-walk.egg"},
)
self.playerModel.setScale(0.15)
self.playerModel.setZ(0.15)
self.playerModel.reparentTo(self.np)
interval = Wait(random.uniform(0, 1))
i2 = Func(self.startWalkAnim)
seq = Sequence()
seq.append(interval)
seq.append(i2)
seq.start()
self.growlTimer = 5.0
self.sound = None
self.alive = True
self.targetNp = NodePath(self.name)
self.brainTimer = 0.0
self.brainDelay = 1.2
self.currentForce = Vec3(0, 0, 0)
def startWalkAnim(self):
self.playerModel.loop("walk")
def stopWalkAnim(self):
self.playerModel.loop("idle")
def checkFront(self):
p1 = Point3(self.getPos())
direction = self.game.playerNp.getPos() - self.getPos()
direction.normalize()
direction = direction * 2.0
p2 = Point3(self.getPos() + direction)
result = self.game.world.rayTestClosest(p1, p2)
# ts1 = TransformState.makePos(p1)
# ts2 = TransformState.makePos(p2)
# result = self.game.world.sweepTestClosest(self.shape, ts1, ts2, 0)
if result.hasHit():
pos = result.getHitPos()
n = result.getHitNormal()
frac = result.getHitFraction()
node = result.getNode()
return node.getName()
return None
def checkRight(self):
p1 = Point3(self.getPos())
direction = self.game.playerNp.getPos() - self.getPos()
direction.normalize()
right = direction.cross(Vec3(0, 0, 1))
right = right * 2.0
p2 = Point3(self.getPos() + right)
result = self.game.world.rayTestClosest(p1, p2)
if result.hasHit():
pos = result.getHitPos()
n = result.getHitNormal()
frac = result.getHitFraction()
node = result.getNode()
return node.getName()
return None
def checkLeft(self):
p1 = Point3(self.getPos())
direction = self.game.playerNp.getPos() - self.getPos()
direction.normalize()
left = -direction.cross(Vec3(0, 0, 1))
left = left * 2.0
p2 = Point3(self.getPos() + left)
result = self.game.world.rayTestClosest(p1, p2)
if result.hasHit():
pos = result.getHitPos()
n = result.getHitNormal()
frac = result.getHitFraction()
node = result.getNode()
return node.getName()
return None
def updateDirection(self):
direction = self.game.playerNp.getPos() - self.getPos()
direction.setZ(0)
direction.normalize()
direction = direction * self.groundAccel
right = direction.cross(Vec3(0, 0, 1))
left = -right
inFront = self.checkFront()
inRight = self.checkRight()
inLeft = self.checkLeft()
if inFront == "boxpack":
if inRight == "boxpack":
if inLeft == "boxPack":
self.currentForce = -direction
else:
self.currentForce = left
else:
self.currentForce = right
else:
self.currentForce = direction
def update(self, dt=0.0):
if not self.alive:
return
self.brainTimer -= dt
if self.brainTimer <= 0.0:
self.updateDirection()
self.brainTimer = self.brainDelay
self.setForce(self.currentForce)
self.capSpeedXY()
speedVec = self.getSpeedVec()
self.targetNp.setPos(self.getPos() + speedVec)
self.lookAt(self.targetNp)
# growl!
self.growlTimer -= dt
if self.growlTimer <= 0.0:
if self.sound:
self.game.soundDic3D[self.sound].stop()
self.game.detachSound(self.sound)
growlIndex = random.randint(1, len(self.game.zombieGrowlSounds))
soundName = "monster-" + str(growlIndex)
self.sound = soundName
self.game.attachSound(soundName, self.np)
self.game.playSound3D(soundName)
self.growlTimer = 1.0 + random.uniform(0.0, 1.0)
# print "Growl from %s" % (self.name)
# bite! # player is the only one checking for that now
# res = self.getContacts()
# if "player" in res:
# print "%s took a bite on player at %s!" % (self.name, self.game.globalClock.getFrameTime())
# self.game.onDie()
def destroy(self):
self.alive = False
self.game.world.removeRigidBody(self.node)
self.stopWalkAnim()
self.np.setTransparency(True)
self.np.setColor(1, 1, 1, 1)
self.setGlowOff()
i1 = Wait(random.uniform(0, 1))
i2 = LerpColorInterval(self.np, 1.0, (1, 1, 1, 0))
i3 = Func(self.np.remove)
seq = Sequence()
seq.append(i1)
seq.append(i2)
seq.append(i3)
seq.start()
class MyApp(ShowBase):
def __init__(self, screen_size=84, DEBUGGING=False, human_playable=False):
ShowBase.__init__(self)
self.forward_button = KeyboardButton.ascii_key(b'w')
self.backward_button = KeyboardButton.ascii_key(b's')
self.fps = 20
self.human_playable = human_playable
self.actions = 3
self.last_frame_start_time = time.time()
self.action_buffer = [1, 1, 1]
self.last_teleport_time = 0.0
self.time_to_teleport = False
if self.human_playable is False:
winprops = WindowProperties.size(screen_size, screen_size)
fbprops = FrameBufferProperties()
fbprops.set_rgba_bits(8, 8, 8, 0)
fbprops.set_depth_bits(24)
self.pipe = GraphicsPipeSelection.get_global_ptr().make_module_pipe('pandagl')
self.imageBuffer = self.graphicsEngine.makeOutput(
self.pipe,
"image buffer",
1,
fbprops,
winprops,
GraphicsPipe.BFRefuseWindow)
self.camera = Camera('cam')
self.cam = NodePath(self.camera)
self.cam.reparentTo(self.render)
self.dr = self.imageBuffer.makeDisplayRegion()
self.dr.setCamera(self.cam)
self.render.setShaderAuto()
self.cam.setPos(0.5, 0, 6)
self.cam.lookAt(0.5, 0, 0)
# Create Ambient Light
self.ambientLight = AmbientLight('ambientLight')
self.ambientLight.setColor((0.2, 0.2, 0.2, 1))
self.ambientLightNP = self.render.attachNewNode(self.ambientLight)
self.render.setLight(self.ambientLightNP)
# Spotlight
self.light = Spotlight('light')
self.light.setColor((0.9, 0.9, 0.9, 1))
self.lightNP = self.render.attachNewNode(self.light)
self.lightNP.setPos(0, 10, 10)
self.lightNP.lookAt(0, 0, 0)
self.lightNP.node().getLens().setFov(40)
self.lightNP.node().getLens().setNearFar(10, 100)
self.lightNP.node().setShadowCaster(True, 1024, 1024)
self.render.setLight(self.lightNP)
self.world = BulletWorld()
self.world.setGravity(Vec3(0, 0, -9.81))
if DEBUGGING is True:
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())
self.finger_speed_mps = 0.0
self.penalty_applied = False
self.teleport_cooled_down = True
self.fps = 20
self.framecount = 0
self.reset()
def reset(self):
namelist = ['Ground',
'Conveyor',
'Finger',
'Block',
'Scrambled Block',
'Not Rewardable',
'Teleport Me']
for child in self.render.getChildren():
for test in namelist:
if child.node().name == test:
self.world.remove(child.node())
child.removeNode()
break
# Plane
self.plane_shape = BulletPlaneShape(Vec3(0, 0, 1), 1)
self.plane_node = BulletRigidBodyNode('Ground')
self.plane_node.addShape(self.plane_shape)
self.plane_np = self.render.attachNewNode(self.plane_node)
self.plane_np.setPos(0.0, 0.0, -1.0)
self.world.attachRigidBody(self.plane_node)
# Conveyor
self.conv_node = BulletRigidBodyNode('Conveyor')
self.conv_node.setFriction(1.0)
self.conv_np = self.render.attachNewNode(self.conv_node)
self.conv_shape = BulletBoxShape(Vec3(100.0, 1.0, 0.05))
self.conv_node.setMass(1000.0)
self.conv_np.setPos(-95.0, 0.0, 0.1)
self.conv_node.addShape(self.conv_shape)
self.world.attachRigidBody(self.conv_node)
self.model = loader.loadModel('assets/conv.egg')
self.model.flattenLight()
self.model.reparentTo(self.conv_np)
# Finger
self.finger_node = BulletRigidBodyNode('Finger')
self.finger_node.setFriction(1.0)
self.finger_np = self.render.attachNewNode(self.finger_node)
self.finger_shape = BulletCylinderShape(0.1, 0.25, ZUp)
self.finger_node.setMass(0)
self.finger_np.setPos(1.8, 0.0, 0.24 + 0.0254*3.5)
self.finger_node.addShape(self.finger_shape)
self.world.attachRigidBody(self.finger_node)
self.model = loader.loadModel('assets/finger.egg')
self.model.flattenLight()
self.model.reparentTo(self.finger_np)
self.blocks = []
for block_num in range(15):
new_block = self.spawn_block(Vec3(28, random.uniform(-3, 3), (0.2 * block_num) + 2.0),
2, random.choice([4, 4,
6]), random.choice([10,
11,
12,
13,
14,
15, 15,
16, 16,
17, 17,
18, 18, 18, 18,
19, 19, 19, 19,
20, 20, 20, 20, 20,
21, 21, 21, 21,
22, 22, 22, 23,
23, 23, 23, 23,
24]))
# new_block = self.spawn_block(Vec3(18, 0, (0.2 * block_num) + 2.0),
# 2, 4, 24)
self.blocks.append(new_block)
self.finger_speed_mps = 0.0
self.penalty_applied = False
self.teleport_cooled_down = True
self.fps = 20
self.framecount = 0
self.last_teleport_time = 0.0
self.time_to_teleport = False
return self.step(1)[0]
def spawn_block(self, location, z_inches, y_inches, x_inches):
"""
Spawns a block
"""
node = BulletRigidBodyNode('Block')
node.setFriction(1.0)
block_np = self.render.attachNewNode(node)
shape = BulletBoxShape(Vec3(0.0254*y_inches, 0.0254*x_inches, 0.0254*z_inches))
node.setMass(1.0)
block_np.setPos(location)
block_np.setHpr(random.uniform(-60, 60), 0.0, 0.0)
node.addShape(shape)
self.world.attachRigidBody(node)
model = loader.loadModel('assets/bullet-samples/models/box.egg')
model.setH(90)
model.setSx(0.0254*x_inches*2)
model.setSy(0.0254*y_inches*2)
model.setSz(0.0254*z_inches*2)
model.flattenLight()
model.reparentTo(block_np)
block_np.node().setTag('scrambled', 'False')
return block_np
def get_camera_image(self, requested_format=None):
"""
Returns the camera's image, which is of type uint8 and has values
between 0 and 255.
The 'requested_format' argument should specify in which order the
components of the image must be. For example, valid format strings are
"RGBA" and "BGRA". By default, Panda's internal format "BGRA" is used,
in which case no data is copied over.
"""
tex = self.dr.getScreenshot()
if requested_format is None:
data = tex.getRamImage()
else:
data = tex.getRamImageAs(requested_format)
image = np.frombuffer(data, np.uint8) # use data.get_data() instead of data in python 2
image.shape = (tex.getYSize(), tex.getXSize(), tex.getNumComponents())
image = np.flipud(image)
return image[:,:,:3]
def reset_conv(self):
conveyor_dist_left = 1 - self.conv_np.getPos()[0]
if conveyor_dist_left < 10:
self.conv_np.setX(-95.0)
self.conv_np.setY(0.0)
def check_rewards(self):
reward = 0
for block in self.blocks:
block_x, block_y, block_z = block.getPos()
if block_z > 0.16 and block_x > -1 and block_x < 0:
block.node().setTag('scrambled', 'True')
if block_x < 2.3 and block_z < 0.16 and block.node().getTag('scrambled') == 'True':
block.node().setTag('scrambled', 'False')
reward = 1
return reward
def check_teleportable(self, blocks_per_minute):
self.time = self.framecount/self.fps
# if self.time % (1/(blocks_per_minute/60)) < 0.1:
# self.time_to_teleport = True
# else:
# self.time_to_teleport = False
# self.teleport_cooled_down = True
teleport_cooled_down = True if self.last_teleport_time + 0.4 < self.time else False
if random.choice([True,
False, False, False]) and teleport_cooled_down:
self.last_teleport_time = self.time
self.time_to_teleport = True
for block in self.blocks:
block_x = block.getPos()[0]
if block_x > 5:
block.node().setTag('scrambled', 'False')
if self.time_to_teleport is True:
self.time_to_teleport = False
block.setX(-3)
block.setY(0.0)
block.setZ(2.0)
block.setHpr(random.uniform(-60, 60), 0.0, 0.0)
def step(self, action):
dt = 1/self.fps
self.framecount += 1
max_dist = 1.1
# Move finger
finger_max_speed = 2
finger_accel = 10.0
finger_deccel = 10.0
self.action_buffer.pop(0)
self.action_buffer.append(action)
action = self.action_buffer[0]
if action == 0:
self.finger_speed_mps += dt * finger_accel
if self.finger_speed_mps > finger_max_speed:
self.finger_speed_mps = 2
if action == 1:
if self.finger_speed_mps > 0.01:
self.finger_speed_mps -= finger_deccel * dt
if self.finger_speed_mps < -0.01:
self.finger_speed_mps += finger_deccel * dt
if action == 2:
self.finger_speed_mps -= dt * finger_accel
if self.finger_speed_mps < -finger_max_speed:
self.finger_speed_mps = -finger_max_speed
real_displacement = self.finger_speed_mps * dt
self.finger_np.setY(self.finger_np.getY() + real_displacement)
if self.finger_np.getY() > max_dist:
self.finger_np.setY(max_dist)
self.finger_speed_mps = 0
if self.finger_np.getY() < -max_dist:
self.finger_np.setY(-max_dist)
self.finger_speed_mps = 0
# self.world.doPhysics(dt, 5, 1.0/120.0)
self.world.doPhysics(dt, 20, 1.0/240.0)
self.reset_conv()
self.check_teleportable(blocks_per_minute=59)
# Keep the conveyor moving
self.conv_np.node().setLinearVelocity(Vec3(1.0, 0.0, 0.0))
if self.human_playable is False:
self.graphicsEngine.renderFrame()
TransformState.garbageCollect()
RenderState.garbageCollect()
image = self.get_camera_image()
else:
image = None
score = 0
score += self.check_rewards()
done = False
return image, score, done
def update(self, task):
is_down = self.mouseWatcherNode.is_button_down
next_act = 1
if is_down(self.forward_button):
next_act = 0
if is_down(self.backward_button):
next_act = 2
_, reward, _ = self.step(next_act)
if reward != 0:
print(reward)
last_frame_duration = time.time() - self.last_frame_start_time
if last_frame_duration < (1/self.fps):
time.sleep((1/self.fps) - last_frame_duration)
self.last_frame_start_time = time.time()
return task.cont
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 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 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 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 MyApp(ShowBase):
def __init__(self, screen_size=84, DEBUGGING=False):
ShowBase.__init__(self)
self.render_stuff = True
self.actions = 3
self.render.setShaderAuto()
self.cam.setPos(0, 0, 7)
self.cam.lookAt(0, 0, 0)
wp = WindowProperties()
window_size = screen_size
wp.setSize(window_size, window_size)
self.win.requestProperties(wp)
# Create Ambient Light
self.ambientLight = AmbientLight('ambientLight')
self.ambientLight.setColor((0.2, 0.2, 0.2, 1))
self.ambientLightNP = self.render.attachNewNode(self.ambientLight)
self.render.setLight(self.ambientLightNP)
# Spotlight
self.light = Spotlight('light')
self.light.setColor((0.9, 0.9, 0.9, 1))
self.lightNP = self.render.attachNewNode(self.light)
self.lightNP.setPos(0, 10, 10)
self.lightNP.lookAt(0, 0, 0)
self.lightNP.node().getLens().setFov(40)
self.lightNP.node().getLens().setNearFar(10, 100)
self.lightNP.node().setShadowCaster(True, 1024, 1024)
self.render.setLight(self.lightNP)
self.world = BulletWorld()
self.world.setGravity(Vec3(0, 0, -9.81))
if DEBUGGING is True:
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())
# Reward zone
self.rzone_shape = BulletBoxShape(Vec3(.8, 1, 0.5))
self.rzone_ghost = BulletGhostNode('Reward Zone')
self.rzone_ghost.addShape(self.rzone_shape)
self.rzone_ghostNP = self.render.attachNewNode(self.rzone_ghost)
self.rzone_ghostNP.setPos(2.2, 0.0, 0.86)
self.rzone_ghostNP.setCollideMask(BitMask32(0x0f))
self.world.attachGhost(self.rzone_ghost)
# Needed for camera image
self.dr = self.camNode.getDisplayRegion(0)
# Needed for camera depth image
winprops = WindowProperties.size(self.win.getXSize(),
self.win.getYSize())
fbprops = FrameBufferProperties()
fbprops.setDepthBits(1)
self.depthBuffer = self.graphicsEngine.makeOutput(
self.pipe, "depth buffer", -2, fbprops, winprops,
GraphicsPipe.BFRefuseWindow, self.win.getGsg(), self.win)
self.depthTex = Texture()
self.depthTex.setFormat(Texture.FDepthComponent)
self.depthBuffer.addRenderTexture(self.depthTex,
GraphicsOutput.RTMCopyRam,
GraphicsOutput.RTPDepth)
lens = self.cam.node().getLens()
# the near and far clipping distances can be changed if desired
# lens.setNear(5.0)
# lens.setFar(500.0)
self.depthCam = self.makeCamera(self.depthBuffer,
lens=lens,
scene=render)
self.depthCam.reparentTo(self.cam)
def reset(self):
namelist = [
'Ground', 'Conveyor', 'Finger', 'Block', 'Scrambled Block',
'Not Rewardable', 'Teleport Me'
]
for child in render.getChildren():
for test in namelist:
if child.node().name == test:
self.world.remove(child.node())
child.removeNode()
break
# Plane
self.plane_shape = BulletPlaneShape(Vec3(0, 0, 1), 1)
self.plane_node = BulletRigidBodyNode('Ground')
self.plane_node.addShape(self.plane_shape)
self.plane_np = self.render.attachNewNode(self.plane_node)
self.plane_np.setPos(0.0, 0.0, -1.0)
self.world.attachRigidBody(self.plane_node)
# Conveyor
self.conv_node = BulletRigidBodyNode('Conveyor')
self.conv_node.setFriction(1.0)
self.conv_np = self.render.attachNewNode(self.conv_node)
self.conv_shape = BulletBoxShape(Vec3(100.0, 1.0, 0.05))
self.conv_node.setMass(1000.0)
self.conv_np.setPos(-95.0, 0.0, 0.1)
self.conv_node.addShape(self.conv_shape)
self.world.attachRigidBody(self.conv_node)
self.model = loader.loadModel('assets/conv.egg')
self.model.flattenLight()
self.model.reparentTo(self.conv_np)
# Finger
self.finger_node = BulletRigidBodyNode('Finger')
self.finger_node.setFriction(1.0)
self.finger_np = self.render.attachNewNode(self.finger_node)
self.finger_shape = BulletCylinderShape(0.1, 0.25, ZUp)
self.finger_node.setMass(0)
self.finger_np.setPos(1.8, 0.0, 0.24 + 0.0254 * 3.5)
self.finger_node.addShape(self.finger_shape)
self.world.attachRigidBody(self.finger_node)
self.model = loader.loadModel('assets/finger.egg')
self.model.flattenLight()
self.model.reparentTo(self.finger_np)
self.blocks = []
for block_num in range(15):
new_block = self.spawn_block(Vec3(18, 0, (0.2 * block_num) + 2.0))
self.blocks.append(new_block)
self.have_scramble = False
self.penalty_applied = False
self.spawnned = False
self.score = 10
self.teleport_cooled_down = True
self.fps = 20
self.framecount = 0
return self.step(1)[0]
def spawn_block(self, location):
node = BulletRigidBodyNode('Block')
node.setFriction(1.0)
block_np = self.render.attachNewNode(node)
block_np.setAntialias(AntialiasAttrib.MMultisample)
shape = BulletBoxShape(Vec3(0.0254 * 4, 0.0254 * 24, 0.0254 * 2))
node.setMass(1.0)
#block_np.setPos(-3.7, 0.0, 2.0)
block_np.setPos(location)
block_np.setHpr(random.uniform(-60, 60), 0.0, 0.0)
node.addShape(shape)
self.world.attachRigidBody(node)
model = loader.loadModel('assets/bullet-samples/models/box.egg')
model.setH(90)
model.setSy(0.0254 * 4 * 2)
model.setSx(0.0254 * 24 * 2)
model.setSz(0.0254 * 2 * 2)
model.flattenLight()
model.reparentTo(block_np)
return block_np
def get_camera_image(self, requested_format=None):
"""
Returns the camera's image, which is of type uint8 and has values
between 0 and 255.
The 'requested_format' argument should specify in which order the
components of the image must be. For example, valid format strings are
"RGBA" and "BGRA". By default, Panda's internal format "BGRA" is used,
in which case no data is copied over.
"""
tex = self.dr.getScreenshot()
if requested_format is None:
data = tex.getRamImage()
else:
data = tex.getRamImageAs(requested_format)
image = np.frombuffer(
data, np.uint8) # use data.get_data() instead of data in python 2
image.shape = (tex.getYSize(), tex.getXSize(), tex.getNumComponents())
image = np.flipud(image)
return image[:, :, :3]
def reset_conv(self):
conveyor_dist_left = 1 - self.conv_np.getPos()[0]
if conveyor_dist_left < 10:
self.conv_np.setX(-95.0)
self.conv_np.setY(0.0)
# self.conv_np.setY(0.0)
# self.conv_np.setHpr(0.0, 0.0, 0.0)
def check_penalty(self):
penalty = 0
self.pzone_ghost = self.pzone_ghostNP.node()
for node in self.pzone_ghost.getOverlappingNodes():
if node.name == 'Block':
penalty = 1
node.name = 'Scramble'
self.have_scramble = False
return penalty
def check_rewards(self):
reward = 0
# Check for reward blocks (recently cleared scrambles)
rzone_ghost = self.rzone_ghostNP.node()
scrambled = False
for node in rzone_ghost.getOverlappingNodes():
if node.name == 'Block' or node.name == 'Scrambled Block':
node.name = 'Scrambled Block'
scrambled = True
# Rename blocks that are not eligable for reward due to being too late
for block in self.blocks:
block_x = block.getPos()[0]
block_name = block.node().name
if block_x > 2.4 and block_name == 'Scrambled Block':
self.have_scramble = False
scrambled = False
block.node().name = 'Not Rewardable'
if scrambled is True:
self.have_scramble = True
else:
if self.have_scramble is True:
reward = 1
self.have_scramble = False
return reward
def check_teleportable(self, blocks_per_minute):
self.time = self.framecount / self.fps
if self.time % (1 / (blocks_per_minute / 60)) < 0.1:
self.time_to_teleport = True
else:
self.time_to_teleport = False
self.teleport_cooled_down = True
for block in self.blocks:
block_x = block.getPos()[0]
if block_x > 5:
if block.node().name == 'Scrambled Block':
self.have_scramble = False
block.node().name = 'Teleport Me'
if self.time_to_teleport is True and self.teleport_cooled_down is True:
self.teleport_cooled_down = False
block.setX(-4)
block.setY(0.0)
block.setZ(2.0)
block.setHpr(random.uniform(-60, 60), 0.0, 0.0)
block.node().name = 'Block'
def step(self, action):
dt = 1 / self.fps
self.framecount += 1
finger_meters_per_second = 2
max_dist = 1.1
real_displacement = finger_meters_per_second * dt
# Move finger
if action == 0:
self.finger_np.setY(self.finger_np.getY() + real_displacement)
if self.finger_np.getY() > max_dist:
self.finger_np.setY(max_dist)
if action == 2:
self.finger_np.setY(self.finger_np.getY() - real_displacement)
if self.finger_np.getY() < -max_dist:
self.finger_np.setY(-max_dist)
self.world.doPhysics(dt, 5, 1.0 / 120.0)
self.reset_conv()
self.check_teleportable(blocks_per_minute=1.1 * 60)
# Keep the conveyor moving
self.conv_np.node().setLinearVelocity(Vec3(1.0, 0.0, 0.0))
if self.render_stuff == True:
self.graphicsEngine.renderFrame()
image = self.get_camera_image()
# image = cv2.resize(image, (84, 84), interpolation=cv2.INTER_CUBIC)
score = 0
score += self.check_rewards()
#score -= self.check_penalty()
done = False
return image, score, done
ambientLight = AmbientLight('ambientLight')
ambientLight.setColor((0.2, 0.2, 0.2, 1))
ambientLightNP = render.attachNewNode(ambientLight)
render.setLight(ambientLightNP)
# Plane
plane_shape = BulletPlaneShape(Vec3(0, 0, 1), 1)
plane_node = BulletRigidBodyNode('Ground')
plane_node.addShape(plane_shape)
plane_np = render.attachNewNode(plane_node)
plane_np.setPos(0.0, 0.0, -1.0)
world.attachRigidBody(plane_node)
# Conveyor
conv_node = BulletRigidBodyNode('Conveyor')
conv_node.setFriction(1.0)
conv_np = render.attachNewNode(conv_node)
conv_shape = BulletBoxShape(Vec3(100.0, 1.0, 0.05))
conv_node.setMass(1000.0)
conv_np.setPos(-95.0, 0.0, 0.1)
conv_node.addShape(conv_shape)
world.attachRigidBody(conv_node)
model = loader.loadModel('assets/conv.egg')
model.flattenLight()
model.reparentTo(conv_np)
# Finger
finger_node = BulletRigidBodyNode('Finger')
finger_node.setFriction(1.0)
finger_np = render.attachNewNode(finger_node)
finger_shape = BulletCylinderShape(0.1, 0.25, ZUp)
