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)