def setupPlayer(self, x, y, z): yetiHeight = 7 yetiRadius = 2 yetiShape = BulletCapsuleShape(yetiRadius, yetiHeight - 2 * yetiRadius, ZUp) modelPrefix = "../res/models/yeti_" self.yetiModel = Actor("../res/models/yeti.egg", {"idle":"../res/models/yeti_idle.egg", "walk":"../res/models/yeti_walking.egg"}) self.yetiModel.setH(90) self.yetiModel.setPos(0, 0, SNOW_HEIGHT) playerNode = BulletRigidBodyNode("Player") playerNode.setMass(MASS) playerNode.addShape(yetiShape) # Without this set to 0,0,0, the Yeti would wobble like a Weeble but not fall down. playerNode.setAngularFactor(Vec3(0,0,0)) # Without this disabled, things will weld together after a certain amount of time. It's really annoying. playerNode.setDeactivationEnabled(False) playerNP = self.worldNP.attachNewNode(playerNode) playerNP.setPos(x, y, z) playerNP.setH(270) self.yetiModel.reparentTo(playerNP) self.bulletWorld.attachRigidBody(playerNP.node()) # Hopefully Brandon will get those animation files to me so I can convert them. # self.setAnimation('idle') return playerNP
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 _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 makeBall(self, num, pos = (0, 0, 0)): shape = BulletSphereShape(0.5) node = BulletRigidBodyNode('ball_' + str(num)) node.setMass(1.0) node.setRestitution(.9) node.setDeactivationEnabled(False) node.addShape(shape) physics = render.attachNewNode(node) physics.setPos(*pos) self.world.attachRigidBody(node) model = loader.loadModel('models/ball') color = self.makeColor() model.setColor(color) self.ballColors['ball_' + str(num)] = color model.reparentTo(physics)
def _add_chassis(self, pg_physics_world: PGPhysicsWorld): para = self.get_config() chassis = BulletRigidBodyNode(BodyName.Ego_vehicle_top) chassis.setIntoCollideMask(BitMask32.bit(self.COLLISION_MASK)) chassis_shape = BulletBoxShape( Vec3(para[Parameter.vehicle_width] / 2, para[Parameter.vehicle_length] / 2, para[Parameter.vehicle_height] / 2)) ts = TransformState.makePos( Vec3(0, 0, para[Parameter.chassis_height] * 2)) chassis.addShape(chassis_shape, ts) heading = np.deg2rad(-para[Parameter.heading] - 90) chassis.setMass(para[Parameter.mass]) self.chassis_np = self.node_path.attachNewNode(chassis) # not random born now self.chassis_np.setPos(Vec3(*self.born_place, 1)) self.chassis_np.setQuat( LQuaternionf(np.cos(heading / 2), 0, 0, np.sin(heading / 2))) chassis.setDeactivationEnabled(False) chassis.notifyCollisions(True) # advance collision check self.pg_world.physics_world.dynamic_world.setContactAddedCallback( PythonCallbackObject(self._collision_check)) self.dynamic_nodes.append(chassis) chassis_beneath = BulletGhostNode(BodyName.Ego_vehicle) chassis_beneath.setIntoCollideMask(BitMask32.bit(self.COLLISION_MASK)) chassis_beneath.addShape(chassis_shape) self.chassis_beneath_np = self.chassis_np.attachNewNode( chassis_beneath) self.dynamic_nodes.append(chassis_beneath) self.system = BulletVehicle(pg_physics_world.dynamic_world, chassis) self.system.setCoordinateSystem(ZUp) self.dynamic_nodes.append( self.system ) # detach chassis will also detach system, so a waring will generate self.LENGTH = para[Parameter.vehicle_length] self.WIDTH = para[Parameter.vehicle_width] if self.render: model_path = 'models/ferra/scene.gltf' self.chassis_vis = self.loader.loadModel( AssetLoader.file_path(model_path)) self.chassis_vis.setZ(para[Parameter.vehicle_vis_z]) self.chassis_vis.setY(para[Parameter.vehicle_vis_y]) self.chassis_vis.setH(para[Parameter.vehicle_vis_h]) self.chassis_vis.set_scale(para[Parameter.vehicle_vis_scale]) self.chassis_vis.reparentTo(self.chassis_np)
def createElement(self, name, type, start, end=None): if type == "cell": model_file = "sphere.dae" elif type == "bit": model_file = "box.dae" elif type == "segment" or type == "synapse": model_file = "cylinder.dae" # Create the rigid body body_node = BulletRigidBodyNode(name) body_node.setDeactivationEnabled(False) body_np = self.render.attachNewNode(body_node) body_np.setName(name) if type == "segment" or type == "synapse": # Calculate the linear distance between the start and the end position of the segment. length = (Point3(start) - Point3(end)).length() body_np.setPos(start) body_np.lookAt(end) body_np.setScale(1, length / 2, 1) else: body_np.setPos(start) # Load the 3d model file using the asset folder relative path and attach the geom node to rigid body object_np = self.loader.loadModel( Filename.from_os_specific( os.path.join(REPO_DIR, "models", model_file))) object_np.reparentTo(body_np) object_np.setPosHpr((0, 0, 0), (0, 0, 0)) object_np.setName(name + "_geom") object_np.setTexGen(TextureStage.getDefault(), TexGenAttrib.MWorldPosition) # Apply any transforms from modelling sotware to gain performance object_np.flattenStrong() # Create the shape used for collisions geom_nodes = object_np.findAllMatches("**/+GeomNode") mesh = BulletTriangleMesh() for geom in geom_nodes[0].node().getGeoms(): mesh.addGeom(geom) shape = BulletTriangleMeshShape(mesh, dynamic=True) body_node.addShape(shape) self.physics_manager.attachRigidBody(body_node) return body_np
def setupDoor(self, _obj, _eggFile): shape = BulletBoxShape(Vec3(1.7 / 2, 0.6 / 2, 0.2 / 2)) node = BulletRigidBodyNode(_obj.getTag("door")) node.addShape(shape) node.setMass(1) node.setDeactivationEnabled(False) np = self.rootNode.attachNewNode(node) np.setCollideMask(BitMask32.bit(2)) np.setPos(_obj.getPos()) np.setHpr(_obj.getHpr()) self.parent.physics_world.attachRigidBody(node) _obj.reparentTo(np) _obj.setPos(np.getPos() - _obj.getPos()) # Setup hinge if _obj.getTag("door") == "left" and self.isHingeLeftSet != True: pos = Point3(-2.0, 0, 0) #hingeLeft.getPos() axisA = Vec3(0, 1, 0) hinge = BulletHingeConstraint(node, pos, axisA, True) hinge.setDebugDrawSize(0.3) hinge.setLimit(-10, 58, softness=0.9, bias=0.3, relaxation=1.0) self.parent.physics_world.attachConstraint(hinge) self.isHingeLeftSet = True self.parent.game_doors["left"] = np self.parent.game_doors["left_hinge"] = hinge if _obj.getTag("door") == "right" and self.isHingeRightSet != True: pos = Point3(2.0, 0, 0) #hingeLeft.getPos() axisA = Vec3(0, -1, 0) hinge = BulletHingeConstraint(node, pos, axisA, True) hinge.setDebugDrawSize(0.3) hinge.setLimit(-10, 58, softness=0.9, bias=0.3, relaxation=1.0) self.parent.physics_world.attachConstraint(hinge) self.isHingeRightSet = True self.parent.game_doors["right"] = np self.parent.game_doors["right_hinge"] = hinge
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 SMAI(): #This constructs the nods tree needed for an AIChar object. #Takes in a model as a string, a speed double, a world object, a bullet physics world and n x, y and z positions def __init__(self, model, speed, world, worldNP, x, y, z): self.AISpeed = 80000 self.maxSpeed = speed self.AIX = x self.AIY = y self.AIZ = z self.worldNP = worldNP bulletWorld = world self.type = "" self.AIModel = loader.loadModel(model) self.AIModel.setScale(0.5) self.AIModel.reparentTo(render) AIShape = BulletBoxShape(Vec3(3, 3, 1)) self.AINode = BulletRigidBodyNode('AIChar') self.AINode.setMass(100) self.AINode.addShape(AIShape) self.AINode.setDeactivationEnabled(False) self.AINode.setAngularFactor(Vec3(0, 0, 0)) render.attachNewNode(self.AINode) self.AIChar = self.worldNP.attachNewNode(self.AINode) self.AIModel.reparentTo(self.AIChar) self.AIChar.setPos(x, y, z) bulletWorld.attachRigidBody(self.AIChar.node()) #This method needs to be called on your AIChar object to determine what type of AI you want. #Takes in a type string; either flee or seek and also a target object. def setBehavior(self, type, target): if (type == "flee"): self.type = type self.target = target print("Type set to flee") elif (type == "seek"): self.type = type self.target = target print("Type set to seek") else: print("Error @ Incorrect Type!") def moveTo(self, target): self.moveTo = True self.target = target def flee(self): h = 1 hx = 1 hy = 1 if (self.AIChar.getDistance(self.target) < 40.0): if (self.AINode.getLinearVelocity() > self.maxSpeed): if (self.AINode.getLinearVelocity() < 100): self.AIChar.setH(self.target.getH()) h = self.AIChar.getH() else: hx = sin(h * DEG_TO_RAD) * 10 hy = cos(h * DEG_TO_RAD) * 10 self.AINode.applyForce( Vec3(-hx * self.AISpeed * globalClock.getDt(), hy * self.AISpeed * globalClock.getDt(), 0), PNT) # else: # self.AINode.applyForce(Vec3(-hx * self.AISpeed * globalClock.getDt(), hy * self.AISpeed * globalClock.getDt(), 0), PNT) def seek(self): if (self.AIChar.getDistance(self.target) > 20.0): if (self.AINode.getLinearVelocity() > self.maxSpeed): self.AIChar.lookAt(self.target) h = self.AIChar.getH() hx = sin(h * DEG_TO_RAD) * 10 hy = cos(h * DEG_TO_RAD) * 10 self.AINode.applyForce( Vec3(-hx * self.AISpeed * globalClock.getDt(), hy * self.AISpeed * globalClock.getDt(), 0), PNT) def moveToRun(self): self.seek() # if(self.target.getX()+5 <= self.AIChar.getX() and self.target.getX()-5 >= self.AIChar.getX() and self.target.getY()+5 <= self.AIChar.getY() and self.target.getY()-5 >= self.AIChar.getY()): # print("It's stopped!") # self.AIChar.clearForces() # self.AIChar.setLinearVelocity(0) # else: # self.AINode.clearForces() # self.AIChar.lookAt(self.target) # self.AINode.setLinearVelocity(self.AISpeed) #Gets called on every AIChar in the world's update method to allow the AI to function at all. def AIUpdate(self): if (self.moveTo == True): self.moveToRun() if (self.type == "seek"): self.seek() elif (self.type == "flee"): self.flee() else: print("Error @ No AI Type")
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 CarEnv(DirectObject): def __init__(self, params={}): self._params = params if 'random_seed' in self._params: np.random.seed(self._params['random_seed']) self._use_vel = self._params.get('use_vel', True) self._run_as_task = self._params.get('run_as_task', False) self._do_back_up = self._params.get('do_back_up', False) self._use_depth = self._params.get('use_depth', False) self._use_back_cam = self._params.get('use_back_cam', False) self._collision_reward = self._params.get('collision_reward', 0.) if not self._params.get('visualize', False): loadPrcFileData('', 'window-type offscreen') # Defines base, render, loader try: ShowBase() except: pass base.setBackgroundColor(0.0, 0.0, 0.0, 1) # World self._worldNP = render.attachNewNode('World') self._world = BulletWorld() self._world.setGravity(Vec3(0, 0, -9.81)) self._dt = params.get('dt', 0.25) self._step = 0.05 # Vehicle shape = BulletBoxShape(Vec3(0.6, 1.0, 0.25)) ts = TransformState.makePos(Point3(0., 0., 0.25)) self._vehicle_node = BulletRigidBodyNode('Vehicle') self._vehicle_node.addShape(shape, ts) self._mass = self._params.get('mass', 10.) self._vehicle_node.setMass(self._mass) self._vehicle_node.setDeactivationEnabled(False) self._vehicle_node.setCcdSweptSphereRadius(1.0) self._vehicle_node.setCcdMotionThreshold(1e-7) self._vehicle_pointer = self._worldNP.attachNewNode(self._vehicle_node) self._world.attachRigidBody(self._vehicle_node) self._vehicle = BulletVehicle(self._world, self._vehicle_node) self._vehicle.setCoordinateSystem(ZUp) self._world.attachVehicle(self._vehicle) self._addWheel(Point3(0.3, 0.5, 0.07), True, 0.07) self._addWheel(Point3(-0.3, 0.5, 0.07), True, 0.07) self._addWheel(Point3(0.3, -0.5, 0.07), False, 0.07) self._addWheel(Point3(-0.3, -0.5, 0.07), False, 0.07) # Camera size = self._params.get('size', [160, 90]) hfov = self._params.get('hfov', 60) near_far = self._params.get('near_far', [0.1, 100.]) self._camera_sensor = Panda3dCameraSensor(base, color=not self._use_depth, depth=self._use_depth, size=size, hfov=hfov, near_far=near_far, title='front cam') self._camera_node = self._camera_sensor.cam self._camera_node.setPos(0.0, 0.5, 0.375) self._camera_node.lookAt(0.0, 6.0, 0.0) self._camera_node.reparentTo(self._vehicle_pointer) if self._use_back_cam: self._back_camera_sensor = Panda3dCameraSensor( base, color=not self._use_depth, depth=self._use_depth, size=size, hfov=hfov, near_far=near_far, title='back cam') self._back_camera_node = self._back_camera_sensor.cam self._back_camera_node.setPos(0.0, -0.5, 0.375) self._back_camera_node.lookAt(0.0, -6.0, 0.0) self._back_camera_node.reparentTo(self._vehicle_pointer) # Car Simulator self._des_vel = None self._setup() # Input self.accept('escape', self._doExit) self.accept('r', self.reset) self.accept('f1', self._toggleWireframe) self.accept('f2', self._toggleTexture) self.accept('f3', self._view_image) self.accept('f5', self._doScreenshot) self.accept('q', self._forward_0) self.accept('w', self._forward_1) self.accept('e', self._forward_2) self.accept('a', self._left) self.accept('s', self._stop) self.accept('x', self._backward) self.accept('d', self._right) self.accept('m', self._mark) self._steering = 0.0 # degree self._engineForce = 0.0 self._brakeForce = 0.0 self._p = self._params.get('p', 1.25) self._d = self._params.get('d', 0.0) self._last_err = 0.0 self._curr_time = 0.0 self._accelClamp = self._params.get('accelClamp', 2.0) self._engineClamp = self._accelClamp * self._mass self._collision = False if self._run_as_task: self._mark_d = 0.0 taskMgr.add(self._update_task, 'updateWorld') base.run() # _____HANDLER_____ def _doExit(self): sys.exit(1) def _toggleWireframe(self): base.toggleWireframe() def _toggleTexture(self): base.toggleTexture() def _doScreenshot(self): base.screenshot('Bullet') def _forward_0(self): self._des_vel = 1 self._brakeForce = 0.0 def _forward_1(self): self._des_vel = 2 self._brakeForce = 0.0 def _forward_2(self): self._des_vel = 4 self._brakeForce = 0.0 def _stop(self): self._des_vel = 0.0 self._brakeForce = 0.0 def _backward(self): self._des_vel = -4 self._brakeForce = 0.0 def _right(self): self._steering = np.min([np.max([-30, self._steering - 5]), 0.0]) def _left(self): self._steering = np.max([np.min([30, self._steering + 5]), 0.0]) def _view_image(self): from matplotlib import pyplot as plt image = self._camera_sensor.observe()[0] if self._use_depth: plt.imshow(image[:, :, 0], cmap='gray') else: import cv2 def rgb2gray(rgb): return np.dot(rgb[..., :3], [0.299, 0.587, 0.114]) image = rgb2gray(image) im = cv2.resize(image, (64, 36), interpolation=cv2.INTER_AREA ) # TODO how does this deal with aspect ratio plt.imshow(im.astype(np.uint8), cmap='Greys_r') plt.show() def _mark(self): self._mark_d = 0.0 # Setup def _setup(self): if hasattr(self, '_model_path'): # Collidable objects visNP = loader.loadModel(self._model_path) visNP.clearModelNodes() visNP.reparentTo(render) pos = (0., 0., 0.) visNP.setPos(pos[0], pos[1], pos[2]) bodyNPs = BulletHelper.fromCollisionSolids(visNP, True) for bodyNP in bodyNPs: bodyNP.reparentTo(render) bodyNP.setPos(pos[0], pos[1], pos[2]) if isinstance(bodyNP.node(), BulletRigidBodyNode): bodyNP.node().setMass(0.0) bodyNP.node().setKinematic(True) bodyNP.setCollideMask(BitMask32.allOn()) self._world.attachRigidBody(bodyNP.node()) else: ground = self._worldNP.attachNewNode(BulletRigidBodyNode('Ground')) shape = BulletPlaneShape(Vec3(0, 0, 1), 0) ground.node().addShape(shape) ground.setCollideMask(BitMask32.allOn()) self._world.attachRigidBody(ground.node()) self._place_vehicle() self._setup_light() self._setup_restart_pos() def _setup_restart_pos(self): self._restart_pos = [] self._restart_index = 0 if self._params.get('position_ranges', None) is not None: ranges = self._params['position_ranges'] num_pos = self._params['num_pos'] if self._params.get('range_type', 'random') == 'random': for _ in range(num_pos): ran = ranges[np.random.randint(len(ranges))] self._restart_pos.append(np.random.uniform(ran[0], ran[1])) elif self._params['range_type'] == 'fix_spacing': num_ran = len(ranges) num_per_ran = num_pos // num_ran for i in range(num_ran): ran = ranges[i] low = np.array(ran[0]) diff = np.array(ran[1]) - np.array(ran[0]) for j in range(num_per_ran): val = diff * ((j + 0.0) / num_per_ran) + low self._restart_pos.append(val) elif self._params.get('positions', None) is not None: self._restart_pos = self._params['positions'] else: self._restart_pos = self._default_restart_pos() def _next_restart_pos_hpr(self): num = len(self._restart_pos) if num == 0: return None, None else: pos_hpr = self._restart_pos[self._restart_index] self._restart_index = (self._restart_index + 1) % num return pos_hpr[:3], pos_hpr[3:] def _next_random_restart_pos_hpr(self): num = len(self._restart_pos) if num == 0: return None, None else: index = np.random.randint(num) pos_hpr = self._restart_pos[index] self._restart_index = (self._restart_index + 1) % num return pos_hpr[:3], pos_hpr[3:] def _setup_light(self): alight = AmbientLight('ambientLight') alight.setColor(Vec4(0.5, 0.5, 0.5, 1)) alightNP = render.attachNewNode(alight) render.clearLight() render.setLight(alightNP) # Vehicle def _default_pos(self): return (0.0, 0.0, 0.3) def _default_hpr(self): return (0.0, 0.0, 3.14) def _default_restart_pos(): return [self._default_pos() + self._default_hpr()] def _get_speed(self): vel = self._vehicle.getCurrentSpeedKmHour() / 3.6 return vel def _update(self, dt=1.0, coll_check=True): self._vehicle.setSteeringValue(self._steering, 0) self._vehicle.setSteeringValue(self._steering, 1) self._vehicle.setBrake(self._brakeForce, 0) self._vehicle.setBrake(self._brakeForce, 1) self._vehicle.setBrake(self._brakeForce, 2) self._vehicle.setBrake(self._brakeForce, 3) if dt >= self._step: # TODO maybe change number of timesteps for i in range(int(dt / self._step)): if self._des_vel is not None: vel = self._get_speed() err = self._des_vel - vel d_err = (err - self._last_err) / self._step self._last_err = err self._engineForce = np.clip( self._p * err + self._d * d_err, -self._accelClamp, self._accelClamp) * self._mass self._vehicle.applyEngineForce(self._engineForce, 0) self._vehicle.applyEngineForce(self._engineForce, 1) self._vehicle.applyEngineForce(self._engineForce, 2) self._vehicle.applyEngineForce(self._engineForce, 3) self._world.doPhysics(self._step, 1, self._step) self._collision = self._is_contact() elif self._run_as_task: self._curr_time += dt if self._curr_time > 0.05: if self._des_vel is not None: vel = self._get_speed() self._mark_d += vel * self._curr_time print(vel, self._mark_d, self._is_contact()) err = self._des_vel - vel d_err = (err - self._last_err) / 0.05 self._last_err = err self._engineForce = np.clip( self._p * err + self._d * d_err, -self._accelClamp, self._accelClamp) * self._mass self._curr_time = 0.0 self._vehicle.applyEngineForce(self._engineForce, 0) self._vehicle.applyEngineForce(self._engineForce, 1) self._vehicle.applyEngineForce(self._engineForce, 2) self._vehicle.applyEngineForce(self._engineForce, 3) self._world.doPhysics(dt, 1, dt) self._collision = self._is_contact() else: raise ValueError( "dt {0} s is too small for velocity control".format(dt)) def _stop_car(self): self._steering = 0.0 self._engineForce = 0.0 self._vehicle.setSteeringValue(0.0, 0) self._vehicle.setSteeringValue(0.0, 1) self._vehicle.applyEngineForce(0.0, 0) self._vehicle.applyEngineForce(0.0, 1) self._vehicle.applyEngineForce(0.0, 2) self._vehicle.applyEngineForce(0.0, 3) if self._des_vel is not None: self._des_vel = 0 self._vehicle_node.setLinearVelocity(Vec3(0.0, 0.0, 0.0)) self._vehicle_node.setAngularVelocity(Vec3(0.0, 0.0, 0.0)) for i in range(self._vehicle.getNumWheels()): wheel = self._vehicle.getWheel(i) wheel.setRotation(0.0) self._vehicle_node.clearForces() def _place_vehicle(self, pos=None, hpr=None): if pos is None: pos = self._default_pos() if hpr is None: hpr = self._default_hpr() self._vehicle_pointer.setPos(pos[0], pos[1], pos[2]) self._vehicle_pointer.setHpr(hpr[0], hpr[1], hpr[2]) self._stop_car() def _addWheel(self, pos, front, radius=0.25): wheel = self._vehicle.createWheel() wheel.setChassisConnectionPointCs(pos) wheel.setFrontWheel(front) wheel.setWheelDirectionCs(Vec3(0, 0, -1)) wheel.setWheelAxleCs(Vec3(1, 0, 0)) wheel.setWheelRadius(radius) wheel.setMaxSuspensionTravelCm(40.0) wheel.setSuspensionStiffness(40.0) wheel.setWheelsDampingRelaxation(2.3) wheel.setWheelsDampingCompression(4.4) wheel.setFrictionSlip(1e2) wheel.setRollInfluence(0.1) # Task def _update_task(self, task): dt = globalClock.getDt() self._update(dt=dt) self._get_observation() return task.cont # Helper functions def _get_observation(self): self._obs = self._camera_sensor.observe() observation = [] observation.append(self._obs[0]) if self._use_back_cam: self._back_obs = self._back_camera_sensor.observe() observation.append(self._back_obs[0]) observation = np.concatenate(observation, axis=2) return observation def _get_reward(self): reward = self._collision_reward if self._collision else self._get_speed( ) return reward def _get_done(self): return self._collision def _get_info(self): info = {} info['pos'] = np.array(self._vehicle_pointer.getPos()) info['hpr'] = np.array(self._vehicle_pointer.getHpr()) info['vel'] = self._get_speed() info['coll'] = self._collision return info def _back_up(self): assert (self._use_vel) back_up_vel = self._params['back_up'].get('vel', -2.0) self._des_vel = back_up_vel back_up_steer = self._params['back_up'].get('steer', (-5.0, 5.0)) # TODO self._steering = np.random.uniform(*back_up_steer) self._brakeForce = 0. duration = self._params['back_up'].get('duration', 1.0) self._update(dt=duration) self._des_vel = 0.0 self._steering = 0.0 self._update(dt=duration) self._brakeForce = 0. def _is_contact(self): result = self._world.contactTest(self._vehicle_node) num_contacts = result.getNumContacts() return result.getNumContacts() > 0 # Environment functions def reset(self, pos=None, hpr=None, hard_reset=False, random_reset=False): if self._do_back_up and not hard_reset and \ pos is None and hpr is None: if self._collision: self._back_up() else: if pos is None and hpr is None: if random_reset: pos, hpr = self._next_random_restart_pos_hpr() else: pos, hpr = self._next_restart_pos_hpr() self._place_vehicle(pos=pos, hpr=hpr) self._collision = False return self._get_observation() def step(self, action): self._steering = action[0] if action[1] == 0.0: self._brakeForce = 1000. else: self._brakeForce = 0. if self._use_vel: # Convert from m/s to km/h self._des_vel = action[1] else: self._engineForce = self._engineClamp * \ ((action[1] - 49.5) / 49.5) self._update(dt=self._dt) observation = self._get_observation() reward = self._get_reward() done = self._get_done() info = self._get_info() return observation, reward, done, info
class DroneEnv(gym.Env): def __init__(self): self.visualize = False if self.visualize == False: from pandac.PandaModules import loadPrcFileData loadPrcFileData("", "window-type none") import direct.directbase.DirectStart self.ep = 0 self.ep_rew = 0 self.t = 0 self.prevDis = 0 self.action_space = Box(-1, 1, shape=(3, )) self.observation_space = Box(-50, 50, shape=(9, )) self.target = 8 * np.random.rand(3) self.construct() self.percentages = [] self.percentMean = [] self.percentStd = [] taskMgr.add(self.stepTask, 'update') taskMgr.add(self.lightTask, 'lights') self.rotorForce = np.array([0, 0, 9.81], dtype=np.float) def construct(self): if self.visualize: base.cam.setPos(17, 17, 1) base.cam.lookAt(0, 0, 0) wp = WindowProperties() wp.setSize(1200, 500) base.win.requestProperties(wp) # World self.world = BulletWorld() self.world.setGravity(Vec3(0, 0, -9.81)) #skybox skybox = loader.loadModel('../models/skybox.gltf') skybox.setTexGen(TextureStage.getDefault(), TexGenAttrib.MWorldPosition) skybox.setTexProjector(TextureStage.getDefault(), render, skybox) skybox.setTexScale(TextureStage.getDefault(), 1) skybox.setScale(100) skybox.setHpr(0, -90, 0) tex = loader.loadCubeMap('../textures/s_#.jpg') skybox.setTexture(tex) skybox.reparentTo(render) render.setTwoSided(True) #Light plight = PointLight('plight') plight.setColor((1.0, 1.0, 1.0, 1)) plnp = render.attachNewNode(plight) plnp.setPos(0, 0, 0) render.setLight(plnp) # Create Ambient Light ambientLight = AmbientLight('ambientLight') ambientLight.setColor((0.15, 0.05, 0.05, 1)) ambientLightNP = render.attachNewNode(ambientLight) render.setLight(ambientLightNP) # Drone shape = BulletBoxShape(Vec3(0.5, 0.5, 0.5)) self.drone = BulletRigidBodyNode('Box') self.drone.setMass(1.0) self.drone.addShape(shape) self.droneN = render.attachNewNode(self.drone) self.droneN.setPos(0, 0, 3) self.world.attachRigidBody(self.drone) model = loader.loadModel('../models/drone.gltf') model.setHpr(0, 90, 0) model.flattenLight() model.reparentTo(self.droneN) blade = loader.loadModel("../models/blade.gltf") blade.reparentTo(self.droneN) blade.setHpr(0, 90, 0) blade.setPos(Vec3(0.3, -3.0, 1.1)) rotation_interval = blade.hprInterval(0.2, Vec3(180, 90, 0)) rotation_interval.loop() placeholder = self.droneN.attachNewNode("blade-placeholder") placeholder.setPos(Vec3(0, 6.1, 0)) blade.instanceTo(placeholder) placeholder = self.droneN.attachNewNode("blade-placeholder") placeholder.setPos(Vec3(3.05, 3.0, 0)) blade.instanceTo(placeholder) placeholder = self.droneN.attachNewNode("blade-placeholder") placeholder.setPos(Vec3(-3.05, 3.0, 0)) blade.instanceTo(placeholder) #drone ligths self.plight2 = PointLight('plight') self.plight2.setColor((0.9, 0.1, 0.1, 1)) plnp = self.droneN.attachNewNode(self.plight2) plnp.setPos(0, 0, -1) self.droneN.setLight(plnp) #over light plight3 = PointLight('plight') plight3.setColor((0.9, 0.8, 0.8, 1)) plnp = self.droneN.attachNewNode(plight3) plnp.setPos(0, 0, 2) self.droneN.setLight(plnp) #target object self.targetObj = loader.loadModel("../models/target.gltf") self.targetObj.reparentTo(render) self.targetObj.setPos( Vec3(self.target[0], self.target[1], self.target[2])) def lightTask(self, task): count = globalClock.getFrameCount() rest = count % 100 if rest < 10: self.plight2.setColor((0.1, 0.9, 0.1, 1)) elif rest > 30 and rest < 40: self.plight2.setColor((0.9, 0.1, 0.1, 1)) elif rest > 65 and rest < 70: self.plight2.setColor((0.9, 0.9, 0.9, 1)) elif rest > 75 and rest < 80: self.plight2.setColor((0.9, 0.9, 0.9, 1)) else: self.plight2.setColor((0.1, 0.1, 0.1, 1)) return task.cont def getState(self): #vel = self.drone.get_linear_velocity() po = self.drone.transform.pos ang = self.droneN.getHpr() #velocity = np.nan_to_num(np.array([vel[0], vel[1], vel[2]])) position = np.array([po[0], po[1], po[2]]) state = np.array([position, self.target]).reshape(6, ) state = np.around(state, decimals=3) return state def getReward(self): reward = 0 s = self.getState() d = np.linalg.norm(s[0:3] - s[3:6]) if d < 20: reward = 20 - d reward = reward / 40 #/4000 #if d < self.prevDis : # reward *= 1.2 #self.prevDis = np.copy(d) return reward def reset(self): #log self.percentages.append(self.ep_rew) me = np.mean(self.percentages[-500:]) self.percentMean.append(me) self.percentStd.append(np.std(self.percentages[-500:])) s = self.getState() d = np.linalg.norm(np.abs(s[:3] - self.target)) ds = np.linalg.norm(s[:3] - np.array([0, 0, 4])) if self.ep % 50 == 0: self.PlotReward() print( f'{self.ep} {self.t} {self.ep_rew:+8.2f} {me:+6.2f} {d:6.2f} {ds:6.2f} {s}' ) #{s[:6]} #physics reset self.droneN.setPos(0, 0, 4) self.droneN.setHpr(0, 0, 0) self.drone.set_linear_velocity(Vec3(0, 0, 0)) self.drone.setAngularVelocity(Vec3(0, 0, 0)) self.rotorForce = np.array([0, 0, 9.81], dtype=np.float) #define new target: self.target = 8 * (2 * np.random.rand(3) - 1) #self.target = np.zeros((3)) self.target[2] = np.abs(self.target[2]) self.target[1] = np.abs(self.target[1]) self.targetObj.setPos( Vec3(self.target[0], self.target[1], self.target[2])) self.ep += 1 self.t = 0 self.ep_rew = 0 state = self.getState() return state def stepTask(self, task): dt = globalClock.getDt() if self.visualize: self.world.doPhysics(dt) else: self.world.doPhysics(0.1) self.drone.setDeactivationEnabled(False) #application of force force = Vec3(self.rotorForce[0], self.rotorForce[1], self.rotorForce[2]) self.drone.applyCentralForce(force) #should be action po = self.drone.transform.pos position = np.array([po[0], po[1], po[2]]) return task.cont def step(self, action): done = False reward = 0 self.t += 1 reward = self.getReward() self.ep_rew += reward state = self.getState() basis = np.array([0, 0, 9.81], dtype=np.float) self.rotorForce = basis + 0.2 * action #0.1 *action #10 sub steps in each step for i in range(5): c = taskMgr.step() self.rotorForce -= 0.05 * (self.rotorForce - basis) #time constraint if self.t > 150: done = True return state, reward, done, {} def PlotReward(self): c = range(len(self.percentages)) plt.plot(self.percentMean, c='b', alpha=0.8) plt.fill_between( c, np.array(self.percentMean) + np.array(self.percentStd), np.array(self.percentMean) - np.array(self.percentStd), color='g', alpha=0.3, label='Objective 1') plt.grid() plt.savefig('rews.png') plt.close()
class 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 SMAI(): #This constructs the nods tree needed for an AIChar object. #Takes in a model as a string, a speed double, a world object, a bullet physics world and n x, y and z positions def __init__(self, model, speed, world, worldNP, x, y, z): self.AISpeed = 80000 self.maxSpeed = speed self.AIX = x self.AIY = y self.AIZ = z self.worldNP = worldNP bulletWorld = world self.type = "" self.AIModel = loader.loadModel(model) self.AIModel.setScale(0.5) self.AIModel.reparentTo(render) AIShape = BulletBoxShape(Vec3(3, 3, 1)) self.AINode = BulletRigidBodyNode('AIChar') self.AINode.setMass(100) self.AINode.addShape(AIShape) self.AINode.setDeactivationEnabled(False) self.AINode.setAngularFactor(Vec3(0,0,0)) render.attachNewNode(self.AINode) self.AIChar = self.worldNP.attachNewNode(self.AINode) self.AIModel.reparentTo(self.AIChar) self.AIChar.setPos(x, y, z) bulletWorld.attachRigidBody(self.AIChar.node()) #This method needs to be called on your AIChar object to determine what type of AI you want. #Takes in a type string; either flee or seek and also a target object. def setBehavior(self, type, target): if(type == "flee"): self.type = type self.target = target print("Type set to flee") elif(type == "seek"): self.type = type self.target = target print("Type set to seek") else: print("Error @ Incorrect Type!") def moveTo(self, target): self.moveTo = True self.target = target def flee(self): h = 1 hx = 1 hy = 1 if(self.AIChar.getDistance(self.target) < 40.0): if(self.AINode.getLinearVelocity() > self.maxSpeed): if(self.AINode.getLinearVelocity() < 100): self.AIChar.setH(self.target.getH()) h = self.AIChar.getH() else: hx = sin(h * DEG_TO_RAD) * 10 hy = cos(h * DEG_TO_RAD) * 10 self.AINode.applyForce(Vec3(-hx * self.AISpeed * globalClock.getDt(), hy * self.AISpeed * globalClock.getDt(), 0), PNT) # else: # self.AINode.applyForce(Vec3(-hx * self.AISpeed * globalClock.getDt(), hy * self.AISpeed * globalClock.getDt(), 0), PNT) def seek(self): if(self.AIChar.getDistance(self.target) > 20.0): if(self.AINode.getLinearVelocity() > self.maxSpeed): self.AIChar.lookAt(self.target) h = self.AIChar.getH() hx = sin(h * DEG_TO_RAD) * 10 hy = cos(h * DEG_TO_RAD) * 10 self.AINode.applyForce(Vec3(-hx * self.AISpeed * globalClock.getDt(), hy * self.AISpeed * globalClock.getDt(), 0), PNT) def moveToRun(self): self.seek() # if(self.target.getX()+5 <= self.AIChar.getX() and self.target.getX()-5 >= self.AIChar.getX() and self.target.getY()+5 <= self.AIChar.getY() and self.target.getY()-5 >= self.AIChar.getY()): # print("It's stopped!") # self.AIChar.clearForces() # self.AIChar.setLinearVelocity(0) # else: # self.AINode.clearForces() # self.AIChar.lookAt(self.target) # self.AINode.setLinearVelocity(self.AISpeed) #Gets called on every AIChar in the world's update method to allow the AI to function at all. def AIUpdate(self): if(self.moveTo == True): self.moveToRun() if(self.type == "seek"): self.seek() elif(self.type == "flee"): self.flee() else: print("Error @ No AI Type")
class CarEnv(DirectObject): def __init__(self, params={}): self._params = params if 'random_seed' in self._params: np.random.seed(self._params['random_seed']) self._use_vel = self._params.get('use_vel', True) self._run_as_task = self._params.get('run_as_task', False) self._do_back_up = self._params.get('do_back_up', False) self._use_depth = self._params.get('use_depth', False) self._use_back_cam = self._params.get('use_back_cam', False) self._collision_reward_only = self._params.get('collision_reward_only', False) self._collision_reward = self._params.get('collision_reward', -10.0) self._obs_shape = self._params.get('obs_shape', (64, 36)) self._steer_limits = params.get('steer_limits', (-30., 30.)) self._speed_limits = params.get('speed_limits', (-4.0, 4.0)) self._motor_limits = params.get('motor_limits', (-0.5, 0.5)) self._fixed_speed = (self._speed_limits[0] == self._speed_limits[1] and self._use_vel) if not self._params.get('visualize', False): loadPrcFileData('', 'window-type offscreen') # Defines base, render, loader try: ShowBase() except: pass base.setBackgroundColor(0.0, 0.0, 0.0, 1) # World self._worldNP = render.attachNewNode('World') self._world = BulletWorld() self._world.setGravity(Vec3(0, 0, -9.81)) self._dt = params.get('dt', 0.25) self._step = 0.05 # Vehicle shape = BulletBoxShape(Vec3(0.6, 1.0, 0.25)) ts = TransformState.makePos(Point3(0., 0., 0.25)) self._vehicle_node = BulletRigidBodyNode('Vehicle') self._vehicle_node.addShape(shape, ts) self._mass = self._params.get('mass', 10.) self._vehicle_node.setMass(self._mass) self._vehicle_node.setDeactivationEnabled(False) self._vehicle_node.setCcdSweptSphereRadius(1.0) self._vehicle_node.setCcdMotionThreshold(1e-7) self._vehicle_pointer = self._worldNP.attachNewNode(self._vehicle_node) self._world.attachRigidBody(self._vehicle_node) self._vehicle = BulletVehicle(self._world, self._vehicle_node) self._vehicle.setCoordinateSystem(ZUp) self._world.attachVehicle(self._vehicle) self._addWheel(Point3(0.3, 0.5, 0.07), True, 0.07) self._addWheel(Point3(-0.3, 0.5, 0.07), True, 0.07) self._addWheel(Point3(0.3, -0.5, 0.07), False, 0.07) self._addWheel(Point3(-0.3, -0.5, 0.07), False, 0.07) # Camera size = self._params.get('size', [160, 90]) hfov = self._params.get('hfov', 120) near_far = self._params.get('near_far', [0.1, 100.]) self._camera_sensor = Panda3dCameraSensor(base, color=not self._use_depth, depth=self._use_depth, size=size, hfov=hfov, near_far=near_far, title='front cam') self._camera_node = self._camera_sensor.cam self._camera_node.setPos(0.0, 0.5, 0.375) self._camera_node.lookAt(0.0, 6.0, 0.0) self._camera_node.reparentTo(self._vehicle_pointer) if self._use_back_cam: self._back_camera_sensor = Panda3dCameraSensor( base, color=not self._use_depth, depth=self._use_depth, size=size, hfov=hfov, near_far=near_far, title='back cam') self._back_camera_node = self._back_camera_sensor.cam self._back_camera_node.setPos(0.0, -0.5, 0.375) self._back_camera_node.lookAt(0.0, -6.0, 0.0) self._back_camera_node.reparentTo(self._vehicle_pointer) # Car Simulator self._des_vel = None self._setup() # Input self.accept('escape', self._doExit) self.accept('r', self.reset) self.accept('f1', self._toggleWireframe) self.accept('f2', self._toggleTexture) self.accept('f3', self._view_image) self.accept('f5', self._doScreenshot) self.accept('q', self._forward_0) self.accept('w', self._forward_1) self.accept('e', self._forward_2) self.accept('a', self._left) self.accept('s', self._stop) self.accept('x', self._backward) self.accept('d', self._right) self.accept('m', self._mark) self._steering = 0.0 # degree self._engineForce = 0.0 self._brakeForce = 0.0 self._env_time_step = 0 self._p = self._params.get('p', 1.25) self._d = self._params.get('d', 0.0) self._last_err = 0.0 self._curr_time = 0.0 self._accelClamp = self._params.get('accelClamp', 0.5) self._engineClamp = self._accelClamp * self._mass self._collision = False self._setup_spec() self.spec = EnvSpec(observation_im_space=self.observation_im_space, action_space=self.action_space, action_selection_space=self.action_selection_space, observation_vec_spec=self.observation_vec_spec, action_spec=self.action_spec, action_selection_spec=self.action_selection_spec, goal_spec=self.goal_spec) if self._run_as_task: self._mark_d = 0.0 taskMgr.add(self._update_task, 'updateWorld') base.run() def _setup_spec(self): self.action_spec = OrderedDict() self.action_selection_spec = OrderedDict() self.observation_vec_spec = OrderedDict() self.goal_spec = OrderedDict() self.action_spec['steer'] = Box(low=-45., high=45.) self.action_selection_spec['steer'] = Box(low=self._steer_limits[0], high=self._steer_limits[1]) if self._use_vel: self.action_spec['speed'] = Box(low=-4., high=4.) self.action_space = Box(low=np.array([ self.action_spec['steer'].low[0], self.action_spec['speed'].low[0] ]), high=np.array([ self.action_spec['steer'].high[0], self.action_spec['speed'].high[0] ])) self.action_selection_spec['speed'] = Box( low=self._speed_limits[0], high=self._speed_limits[1]) self.action_selection_space = Box( low=np.array([ self.action_selection_spec['steer'].low[0], self.action_selection_spec['speed'].low[0] ]), high=np.array([ self.action_selection_spec['steer'].high[0], self.action_selection_spec['speed'].high[0] ])) else: self.action_spec['motor'] = Box(low=-self._accelClamp, high=self._accelClamp) self.action_space = Box(low=np.array([ self.action_spec['steer'].low[0], self.action_spec['motor'].low[0] ]), high=np.array([ self.action_spec['steer'].high[0], self.action_spec['motor'].high[0] ])) self.action_selection_spec['motor'] = Box( low=self._motor_limits[0], high=self._motor_limits[1]) self.action_selection_space = Box( low=np.array([ self.action_selection_spec['steer'].low[0], self.action_selection_spec['motor'].low[0] ]), high=np.array([ self.action_selection_spec['steer'].high[0], self.action_selection_spec['motor'].high[0] ])) assert (np.logical_and( self.action_selection_space.low >= self.action_space.low - 1e-4, self.action_selection_space.high <= self.action_space.high + 1e-4).all()) self.observation_im_space = Box(low=0, high=255, shape=tuple( self._get_observation()[0].shape)) self.observation_vec_spec['coll'] = Discrete(1) self.observation_vec_spec['heading'] = Box(low=0, high=2 * 3.14) self.observation_vec_spec['speed'] = Box(low=-4.0, high=4.0) @property def _base_dir(self): return os.path.join(os.path.dirname(os.path.abspath(__file__)), 'models') @property def horizon(self): return np.inf @property def max_reward(self): return np.inf # _____HANDLER_____ def _doExit(self): sys.exit(1) def _toggleWireframe(self): base.toggleWireframe() def _toggleTexture(self): base.toggleTexture() def _doScreenshot(self): base.screenshot('Bullet') def _forward_0(self): self._des_vel = 1 self._brakeForce = 0.0 def _forward_1(self): self._des_vel = 2 self._brakeForce = 0.0 def _forward_2(self): self._des_vel = 4 self._brakeForce = 0.0 def _stop(self): self._des_vel = 0.0 self._brakeForce = 0.0 def _backward(self): self._des_vel = -4 self._brakeForce = 0.0 def _right(self): self._steering = np.min([np.max([-30, self._steering - 5]), 0.0]) def _left(self): self._steering = np.max([np.min([30, self._steering + 5]), 0.0]) def _view_image(self): from matplotlib import pyplot as plt image = self._camera_sensor.observe()[0] if self._use_depth: plt.imshow(image[:, :, 0], cmap='gray') else: def rgb2gray(rgb): return np.dot(rgb[..., :3], [0.299, 0.587, 0.114]) image = rgb2gray(image) im = cv2.resize(image, (64, 36), interpolation=cv2.INTER_AREA ) # TODO how does this deal with aspect ratio plt.imshow(im.astype(np.uint8), cmap='Greys_r') plt.show() def _mark(self): self._mark_d = 0.0 # Setup def _setup(self): self._setup_map() self._place_vehicle() self._setup_light() self._setup_restart_pos() def _setup_map(self): if hasattr(self, '_model_path'): # Collidable objects self._setup_collision_object(self._model_path) else: ground = self._worldNP.attachNewNode(BulletRigidBodyNode('Ground')) shape = BulletPlaneShape(Vec3(0, 0, 1), 0) ground.node().addShape(shape) ground.setCollideMask(BitMask32.allOn()) self._world.attachRigidBody(ground.node()) def _setup_collision_object(self, path, pos=(0.0, 0.0, 0.0), hpr=(0.0, 0.0, 0.0), scale=1): visNP = loader.loadModel(path) visNP.clearModelNodes() visNP.reparentTo(render) visNP.setPos(pos[0], pos[1], pos[2]) visNP.setHpr(hpr[0], hpr[1], hpr[2]) visNP.set_scale(scale, scale, scale) bodyNPs = BulletHelper.fromCollisionSolids(visNP, True) for bodyNP in bodyNPs: bodyNP.reparentTo(render) bodyNP.setPos(pos[0], pos[1], pos[2]) bodyNP.setHpr(hpr[0], hpr[1], hpr[2]) bodyNP.set_scale(scale, scale, scale) if isinstance(bodyNP.node(), BulletRigidBodyNode): bodyNP.node().setMass(0.0) bodyNP.node().setKinematic(True) bodyNP.setCollideMask(BitMask32.allOn()) self._world.attachRigidBody(bodyNP.node()) else: print("Issue") def _setup_restart_pos(self): self._restart_index = 0 self._restart_pos = self._default_restart_pos() def _next_restart_pos_hpr(self): num = len(self._restart_pos) if num == 0: return None, None else: pos_hpr = self._restart_pos[self._restart_index] self._restart_index = (self._restart_index + 1) % num return pos_hpr[:3], pos_hpr[3:] def _setup_light(self): # alight = AmbientLight('ambientLight') # alight.setColor(Vec4(0.5, 0.5, 0.5, 1)) # alightNP = render.attachNewNode(alight) # render.clearLight() # render.setLight(alightNP) pass # Vehicle def _default_pos(self): return (0.0, 0.0, 0.3) def _default_hpr(self): return (0.0, 0.0, 0.0) def _default_restart_pos(self): return [self._default_pos() + self._default_hpr()] def _get_speed(self): vel = self._vehicle.getCurrentSpeedKmHour() / 3.6 return vel def _get_heading(self): h = np.array(self._vehicle_pointer.getHpr())[0] ori = h * (pi / 180.) while (ori > 2 * pi): ori -= 2 * pi while (ori < 0): ori += 2 * pi return ori def _update(self, dt=1.0, coll_check=True): self._vehicle.setSteeringValue(self._steering, 0) self._vehicle.setSteeringValue(self._steering, 1) self._vehicle.setBrake(self._brakeForce, 0) self._vehicle.setBrake(self._brakeForce, 1) self._vehicle.setBrake(self._brakeForce, 2) self._vehicle.setBrake(self._brakeForce, 3) if dt >= self._step: # TODO maybe change number of timesteps # for i in range(int(dt/self._step)): if self._des_vel is not None: vel = self._get_speed() err = self._des_vel - vel d_err = (err - self._last_err) / self._step self._last_err = err self._engineForce = np.clip(self._p * err + self._d * d_err, -self._accelClamp, self._accelClamp) * self._mass self._vehicle.applyEngineForce(self._engineForce, 0) self._vehicle.applyEngineForce(self._engineForce, 1) self._vehicle.applyEngineForce(self._engineForce, 2) self._vehicle.applyEngineForce(self._engineForce, 3) for _ in range(int(dt / self._step)): self._world.doPhysics(self._step, 1, self._step) self._collision = self._is_contact() elif self._run_as_task: self._curr_time += dt if self._curr_time > 0.05: if self._des_vel is not None: vel = self._get_speed() self._mark_d += vel * self._curr_time print(vel, self._mark_d, self._is_contact()) err = self._des_vel - vel d_err = (err - self._last_err) / 0.05 self._last_err = err self._engineForce = np.clip( self._p * err + self._d * d_err, -self._accelClamp, self._accelClamp) * self._mass self._curr_time = 0.0 self._vehicle.applyEngineForce(self._engineForce, 0) self._vehicle.applyEngineForce(self._engineForce, 1) self._vehicle.applyEngineForce(self._engineForce, 2) self._vehicle.applyEngineForce(self._engineForce, 3) self._world.doPhysics(dt, 1, dt) self._collision = self._is_contact() else: raise ValueError( "dt {0} s is too small for velocity control".format(dt)) def _stop_car(self): self._steering = 0.0 self._engineForce = 0.0 self._vehicle.setSteeringValue(0.0, 0) self._vehicle.setSteeringValue(0.0, 1) self._vehicle.applyEngineForce(0.0, 0) self._vehicle.applyEngineForce(0.0, 1) self._vehicle.applyEngineForce(0.0, 2) self._vehicle.applyEngineForce(0.0, 3) if self._des_vel is not None: self._des_vel = 0 self._vehicle_node.setLinearVelocity(Vec3(0.0, 0.0, 0.0)) self._vehicle_node.setAngularVelocity(Vec3(0.0, 0.0, 0.0)) for i in range(self._vehicle.getNumWheels()): wheel = self._vehicle.getWheel(i) wheel.setRotation(0.0) self._vehicle_node.clearForces() def _place_vehicle(self, pos=None, hpr=None): if pos is None: pos = self._default_pos() if hpr is None: hpr = self._default_hpr() self._vehicle_pointer.setPos(pos[0], pos[1], pos[2]) self._vehicle_pointer.setHpr(hpr[0], hpr[1], hpr[2]) self._stop_car() def _addWheel(self, pos, front, radius=0.25): wheel = self._vehicle.createWheel() wheel.setChassisConnectionPointCs(pos) wheel.setFrontWheel(front) wheel.setWheelDirectionCs(Vec3(0, 0, -1)) wheel.setWheelAxleCs(Vec3(1, 0, 0)) wheel.setWheelRadius(radius) wheel.setMaxSuspensionTravelCm(40.0) wheel.setSuspensionStiffness(40.0) wheel.setWheelsDampingRelaxation(2.3) wheel.setWheelsDampingCompression(4.4) wheel.setFrictionSlip(1e2) wheel.setRollInfluence(0.1) # Task def _update_task(self, task): dt = globalClock.getDt() self._update(dt=dt) self._get_observation() return task.cont # Helper functions def _get_observation(self): self._obs = self._camera_sensor.observe() observation = [] observation.append(self.process(self._obs[0], self._obs_shape)) if self._use_back_cam: self._back_obs = self._back_camera_sensor.observe() observation.append(self.process(self._back_obs[0], self._obs_shape)) observation_im = np.concatenate(observation, axis=2) coll = self._collision heading = self._get_heading() speed = self._get_speed() observation_vec = np.array([coll, heading, speed]) return observation_im, observation_vec def _get_goal(self): return np.array([]) def process(self, image, obs_shape): if self._use_depth: im = np.reshape(image, (image.shape[0], image.shape[1])) if im.shape != obs_shape: im = cv2.resize(im, obs_shape, interpolation=cv2.INTER_AREA) return im.astype(np.uint8) else: image = np.dot(image[..., :3], [0.299, 0.587, 0.114]) im = cv2.resize(image, obs_shape, interpolation=cv2.INTER_AREA ) #TODO how does this deal with aspect ratio # TODO might not be necessary im = np.expand_dims(im, 2) return im.astype(np.uint8) def _get_reward(self): if self._collision_reward_only: if self._collision: reward = self._collision_reward else: reward = 0.0 else: if self._collision: reward = self._collision_reward else: reward = self._get_speed() assert (reward <= self.max_reward) return reward def _get_done(self): return self._collision def _get_info(self): info = {} info['pos'] = np.array(self._vehicle_pointer.getPos()) info['hpr'] = np.array(self._vehicle_pointer.getHpr()) info['vel'] = self._get_speed() info['coll'] = self._collision info['env_time_step'] = self._env_time_step return info def _back_up(self): assert (self._use_vel) # logger.debug('Backing up!') self._params['back_up'] = self._params.get('back_up', {}) back_up_vel = self._params['back_up'].get('vel', -1.0) self._des_vel = back_up_vel back_up_steer = self._params['back_up'].get('steer', (-5.0, 5.0)) self._steering = np.random.uniform(*back_up_steer) self._brakeForce = 0. duration = self._params['back_up'].get('duration', 3.0) self._update(dt=duration) self._des_vel = 0. self._steering = 0. self._update(dt=duration) self._brakeForce = 0. def _is_contact(self): result = self._world.contactTest(self._vehicle_node) return result.getNumContacts() > 0 # Environment functions def reset(self, pos=None, hpr=None, hard_reset=False): if self._do_back_up and not hard_reset and \ pos is None and hpr is None: if self._collision: self._back_up() else: if hard_reset: logger.debug('Hard resetting!') if pos is None and hpr is None: pos, hpr = self._next_restart_pos_hpr() self._place_vehicle(pos=pos, hpr=hpr) self._collision = False self._env_time_step = 0 return self._get_observation(), self._get_goal() def step(self, action): self._steering = action[0] if action[1] == 0.0: self._brakeForce = 1000. else: self._brakeForce = 0. if self._use_vel: # Convert from m/s to km/h self._des_vel = action[1] else: self._engineForce = self._mass * action[1] self._update(dt=self._dt) observation = self._get_observation() goal = self._get_goal() reward = self._get_reward() done = self._get_done() info = self._get_info() self._env_time_step += 1 return observation, goal, reward, done, info
class CharacterController(DynamicObject, FSM): def __init__(self, game): self.game = game FSM.__init__(self, "Player") # key states # dx direction left or right, dy direction forward or backward self.kh = self.game.kh self.dx = 0 self.dy = 0 self.jumping = 0 self.crouching = 0 # maximum speed when only walking self.groundSpeed = 5.0 # acceleration used when walking to rise to self.groundSpeed self.groundAccel = 100.0 # maximum speed in the air (air friction) self.airSpeed = 30.0 # player movement control when in the air self.airAccel = 10.0 # horizontal speed on jump start self.jumpSpeed = 5.5 self.turnSpeed = 5 self.moveSpeedVec = Vec3(0, 0, 0) self.platformSpeedVec = Vec3(0, 0, 0) h = 1.75 w = 0.4 self.shape = BulletCapsuleShape(w, h - 2 * w, ZUp) self.node = BulletRigidBodyNode('Player') self.node.setMass(2) self.node.addShape(self.shape) self.node.setActive(True, True) self.node.setDeactivationEnabled(False, True) self.node.setFriction(200) #self.node.setGravity(10) #self.node.setFallSpeed(200) self.node.setCcdMotionThreshold(1e-7) self.node.setCcdSweptSphereRadius(0.5) self.node.setAngularFactor(Vec3(0, 0, 1)) self.np = self.game.render.attachNewNode(self.node) self.np.setPos(0, 0, 0) self.np.setH(0) #self.np.setCollideMask(BitMask32.allOn()) self.game.world.attachRigidBody(self.node) self.playerModel = None self.slice_able = False def setActor(self, modelPath, animDic={}, flip=False, pos=(0, 0, 0), scale=1.0): self.playerModel = Actor(modelPath, animDic) self.playerModel.reparentTo(self.np) self.playerModel.setScale(scale) # 1ft = 0.3048m if flip: self.playerModel.setH(180) self.playerModel.setPos(pos) self.playerModel.setScale(scale) #------------------------------------------------------------------- # Speed def getSpeedVec(self): return self.node.getLinearVelocity() def setSpeedVec(self, speedVec): #print "setting speed to %s" % (speedVec) self.node.setLinearVelocity(speedVec) return speedVec def setPlatformSpeed(self, speedVec): self.platformSpeedVec = speedVec def getSpeed(self): return self.getSpeedVec().length() def setSpeed(self, speed): speedVec = self.getSpeedVec() speedVec.normalize() self.setSpeedVec(speedVec * speed) def getLocalSpeedVec(self): return self.np.getRelativeVector(self.getSpeed()) def getSpeedXY(self): vec = self.getSpeedVec() return Vec3(vec[0], vec[1], 0) def setSpeedXY(self, speedX, speedY): vec = self.getSpeedVec() z = self.getSpeedZ() self.setSpeedVec(Vec3(speedX, speedY, z)) def getSpeedH(self): return self.getSpeedXY().length() def getSpeedZ(self): return self.getSpeedVec()[2] def setSpeedZ(self, zSpeed): vec = self.getSpeedVec() speedVec = Vec3(vec[0], vec[1], zSpeed) return self.setSpeedVec(speedVec) def setLinearVelocity(self, speedVec): return self.setSpeedVec(speedVec) def setAngularVelocity(self, speed): self.node.setAngularVelocity(Vec3(0, 0, speed)) def getFriction(self): return self.node.getFriction() def setFriction(self, friction): return self.node.setFriction(friction) #------------------------------------------------------------------- # Acceleration def doJump(self): #self.setSpeedZ(self.getSpeedZ()+self.jumpSpeed) self.setSpeedZ(self.jumpSpeed) def setForce(self, force): self.node.applyCentralForce(force) #------------------------------------------------------------------- # contacts #------------------------------------------------------------------- # update def update(self, dt, dx=0, dy=0, jumping=0, crouching=0, dRot=0): #self.setR(0) #self.setP(0) self.jumping = jumping self.crouching = crouching self.dx = dx self.dy = dy #self.setAngularVelocity(dRot*self.turnSpeed) #self.setAngularVelocity(0) self.setH(self.game.camHandler.gameNp.getH()) speedVec = self.getSpeedVec() - self.platformSpeedVec speed = speedVec.length() localSpeedVec = self.np.getRelativeVector(self.game.render, speedVec) pushVec = self.game.render.getRelativeVector(self.np, Vec3(self.dx, self.dy, 0)) if self.dx != 0 or self.dy != 0: pushVec.normalize() else: pushVec = Vec3(0, 0, 0) contacts = self.getContacts() contact = self.checkFeet() if self.jumping and contact in contacts: self.setFriction(0) self.doJump() if self.jumping: self.setForce(pushVec * self.airAccel) if speed > self.airSpeed: self.setSpeed(self.airSpeed) else: if contacts: self.setForce(pushVec * self.groundAccel) else: self.setForce(pushVec * self.airAccel) if self.dx == 0 and self.dy == 0: self.setFriction(100) else: self.setFriction(0) if speed > self.groundSpeed: if contacts: self.setSpeed(self.groundSpeed) '''
class Vehicle(object): def __init__(self, positionHpr, render, world, base): self.base = base loader = base.loader position, hpr = positionHpr vehicleType = "yugo" self.vehicleDir = "data/vehicles/" + vehicleType + "/" # Load vehicle description and specs with open(self.vehicleDir + "vehicle.json") as vehicleData: data = json.load(vehicleData) self.specs = data["specs"] # Chassis for collisions and mass uses a simple box shape halfExtents = (0.5 * dim for dim in self.specs["dimensions"]) shape = BulletBoxShape(Vec3(*halfExtents)) ts = TransformState.makePos(Point3(0, 0, 0.5)) self.rigidNode = BulletRigidBodyNode("vehicle") self.rigidNode.addShape(shape, ts) self.rigidNode.setMass(self.specs["mass"]) self.rigidNode.setDeactivationEnabled(False) self.np = render.attachNewNode(self.rigidNode) self.np.setPos(position) self.np.setHpr(hpr) world.attachRigidBody(self.rigidNode) # Vehicle physics model self.vehicle = BulletVehicle(world, self.rigidNode) self.vehicle.setCoordinateSystem(ZUp) world.attachVehicle(self.vehicle) # Vehicle graphical model self.vehicleNP = loader.loadModel(self.vehicleDir + "car.egg") self.vehicleNP.reparentTo(self.np) # Create wheels wheelPos = self.specs["wheelPositions"] for fb, y in (("F", wheelPos[1]), ("B", -wheelPos[1])): for side, x in (("R", wheelPos[0]), ("L", -wheelPos[0])): np = loader.loadModel(self.vehicleDir + "tire%s.egg" % side) np.reparentTo(render) isFront = fb == "F" self.addWheel(Point3(x, y, wheelPos[2]), isFront, np) def addWheel(self, position, isFront, np): wheel = self.vehicle.createWheel() wheelSpecs = self.specs["wheels"] wheel.setNode(np.node()) wheel.setChassisConnectionPointCs(position) wheel.setFrontWheel(isFront) wheel.setWheelDirectionCs(Vec3(0, 0, -1)) wheel.setWheelAxleCs(Vec3(1, 0, 0)) wheel.setWheelRadius(wheelSpecs["radius"]) wheel.setMaxSuspensionTravelCm(wheelSpecs["suspensionTravel"] * 100.0) wheel.setSuspensionStiffness(wheelSpecs["suspensionStiffness"]) wheel.setWheelsDampingRelaxation(wheelSpecs["dampingRelaxation"]) wheel.setWheelsDampingCompression(wheelSpecs["dampingCompression"]) wheel.setFrictionSlip(wheelSpecs["frictionSlip"]) wheel.setRollInfluence(wheelSpecs["rollInfluence"]) def initialiseSound(self, audioManager): """Start the engine sound and set collision sounds""" # Set sounds to play for collisions self.collisionSound = CollisionSound( nodePath=self.np, sounds=["data/sounds/09.wav"], thresholdForce=600.0, maxForce=800000.0 ) self.engineSound = audioManager.loadSfx(self.vehicleDir + "engine.wav") audioManager.attachSoundToObject(self.engineSound, self.np) self.engineSound.setLoop(True) self.engineSound.setPlayRate(0.6) self.engineSound.play() self.gearSpacing = self.specs["sound"]["maxExpectedRotationRate"] / self.specs["sound"]["numberOfGears"] self.reversing = False def updateSound(self, dt): """Use vehicle speed to update sound pitch""" soundSpecs = self.specs["sound"] # Use rear wheel rotation speed as some measure of engine revs wheels = (self.vehicle.getWheel(idx) for idx in (2, 3)) # wheelRate is in degrees per second wheelRate = 0.5 * abs(sum(w.getDeltaRotation() / dt for w in wheels)) # Calculate which gear we're in, and what the normalised revs are if self.reversing: numberOfGears = 1 else: numberOfGears = self.specs["sound"]["numberOfGears"] gear = min(int(wheelRate / self.gearSpacing), numberOfGears - 1) posInGear = (wheelRate - gear * self.gearSpacing) / self.gearSpacing targetPlayRate = 0.6 + posInGear * (1.5 - 0.6) currentRate = self.engineSound.getPlayRate() self.engineSound.setPlayRate(0.8 * currentRate + 0.2 * targetPlayRate) def updateControl(self, controlState, dt): """Updates acceleration, braking and steering These are all passed in through a controlState dictionary """ # Update acceleration and braking wheelForce = controlState["throttle"] * self.specs["maxWheelForce"] self.reversing = controlState["reverse"] if self.reversing: # Make reversing a bit slower than moving forward wheelForce *= -0.5 brakeForce = controlState["brake"] * self.specs["brakeForce"] # Update steering # Steering control state is from -1 to 1 steering = controlState["steering"] * self.specs["steeringLock"] # Apply steering to front wheels self.vehicle.setSteeringValue(steering, 0) self.vehicle.setSteeringValue(steering, 1) # Apply engine and brake to rear wheels self.vehicle.applyEngineForce(wheelForce, 2) self.vehicle.applyEngineForce(wheelForce, 3) self.vehicle.setBrake(brakeForce, 2) self.vehicle.setBrake(brakeForce, 3)
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 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