def __set_vehicle(self): self.vehicle = BulletVehicle(PhysMgr().root, self.pnode) self.vehicle.setCoordinateSystem(ZUp) self.vehicle.setPitchControl(self.pitch_control) tuning = self.vehicle.getTuning() tuning.setSuspensionCompression(self.suspension_compression) tuning.setSuspensionDamping(self.suspension_damping) PhysMgr().attach_vehicle(self.vehicle)
def setupVehicle(self, bulletWorld): # Chassis shape = BulletBoxShape(Vec3(1, 2.2, 0.5)) ts = TransformState.makePos(Point3(0, 0, .7)) if self.isMainChar: self.chassisNode = BulletRigidBodyNode(self.username) else: self.chassisNode = BulletRigidBodyNode('vehicle') self.chassisNode.setTag("username", str(self.username)) self.chassisNP = render.attachNewNode(self.chassisNode) self.chassisNP.node().addShape(shape, ts) if self.isMainChar: self.chassisNP.node().notifyCollisions(True) else: self.chassisNP.node().notifyCollisions(False) self.setVehiclePos(self.pos[0], self.pos[1], self.pos[2], self.pos[3], self.pos[4], self.pos[5]) # self.chassisNP.setPos(-5.34744, 114.773, 6) #self.chassisNP.setPos(49.2167, 64.7968, 10) self.chassisNP.node().setMass(800.0) self.chassisNP.node().setDeactivationEnabled(False) bulletWorld.attachRigidBody(self.chassisNP.node()) #np.node().setCcdSweptSphereRadius(1.0) #np.node().setCcdMotionThreshold(1e-7) # Vehicle self.vehicle = BulletVehicle(bulletWorld, self.chassisNP.node()) self.vehicle.setCoordinateSystem(ZUp) bulletWorld.attachVehicle(self.vehicle) self.carNP = loader.loadModel('models/batmobile-chassis.egg') #self.yugoNP.setScale(.7) self.carNP.reparentTo(self.chassisNP) # Right front wheel np = loader.loadModel('models/batmobile-wheel-right.egg') np.reparentTo(render) self.addWheel(Point3(1, 1.1, .7), True, np) # Left front wheel np = loader.loadModel('models/batmobile-wheel-left.egg') np.reparentTo(render) self.addWheel(Point3(-1, 1.1, .7), True, np) # Right rear wheel np = loader.loadModel('models/batmobile-wheel-right.egg') np.reparentTo(render) self.addWheel(Point3(1, -2, .7), False, np) # Left rear wheel np = loader.loadModel('models/batmobile-wheel-left.egg') np.reparentTo(render) self.addWheel(Point3(-1, -2, .7), False, np)
def __init__(self, tempworld, bulletWorld, type, playerId): self.world = tempworld self.speed = 0 self.acceleration = 1.5 self.brakes = .7 self.min_speed = 0 self.max_speed = 150 self.reverse_speed = 20 self.reverse_limit = -40 self.armor = 100 self.health = 100 self.a_timer_start = time.time() self.a_timer_end = time.time() self.power_ups = [0, 0, 0] self.playerId = playerId if type == 0: self.actor = Actor("models/batmobile-chassis") self.actor.setScale(0.7) carRadius = 3 elif type == 1: self.actor = Actor("models/policecarpainted", {}) self.actor.setScale(0.30) self.actor.setH(180) # elif type == 2: # self.actor = loader.loadModel("knucklehead.egg") # self.tex = loader.loadTexture("knucklehead.jpg") # self.actor.setTexture(self.car_tex, 1) shape = BulletBoxShape(Vec3(1.0, 1.5, 0.4)) ts = TransformState.makePos(Point3(0, 0, 0.6)) self.chassisNP = render.attachNewNode(BulletRigidBodyNode('Vehicle')) self.chassisNP.node().addShape(shape, ts) self.chassisNP.setPos(50 * random.random(), 50 * random.random(), 1) self.chassisNP.setH(180) self.chassisNP.node().setMass(800.0) self.chassisNP.node().setDeactivationEnabled(False) bulletWorld.attachRigidBody(self.chassisNP.node()) self.actor.reparentTo(self.chassisNP) self.actor.setH(180) # Vehicle self.vehicle = BulletVehicle(bulletWorld, self.chassisNP.node()) self.vehicle.setCoordinateSystem(ZUp) bulletWorld.attachVehicle(self.vehicle) for fb, y in (("F", 1.1), ("B", -1.1)): for side, x in (("R", 0.75), ("L", -0.75)): np = loader.loadModel("models/tire%s.egg" % side) np.reparentTo(render) isFront = fb == "F" self.addWheel(Point3(x, y, 0.55), isFront, np)
def __set_vehicle(self): self.vehicle = BulletVehicle(self.eng.phys_mgr.root._wld, self.pnode) # access to a protected member self.vehicle.set_coordinate_system(ZUp) self.vehicle.set_pitch_control(self.pitch_control) tuning = self.vehicle.get_tuning() tuning.set_suspension_compression(self.suspension_compression) # default .83 tuning.set_suspension_damping(self.suspension_damping) # default .88 self.eng.phys_mgr.attach_vehicle(self.vehicle)
def _add_chassis(self, pg_physics_world: PGPhysicsWorld): para = self.get_config() self.LENGTH = self.vehicle_config["vehicle_length"] self.WIDTH = self.vehicle_config["vehicle_width"] chassis = BaseVehicleNode(BodyName.Base_vehicle, self) chassis.setIntoCollideMask(BitMask32.bit(CollisionGroup.EgoVehicle)) chassis_shape = BulletBoxShape( Vec3(self.WIDTH / 2, self.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 spawn now self.chassis_np.setPos(Vec3(*self.spawn_place, 1)) self.chassis_np.setQuat( LQuaternionf(math.cos(heading / 2), 0, 0, math.sin(heading / 2))) chassis.setDeactivationEnabled(False) chassis.notifyCollisions( True ) # advance collision check, do callback in pg_collision_callback self.dynamic_nodes.append(chassis) chassis_beneath = BulletGhostNode(BodyName.Base_vehicle_beneath) chassis_beneath.setIntoCollideMask( BitMask32.bit(CollisionGroup.EgoVehicleBeneath)) 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 if self.render: if self.MODEL is None: model_path = 'models/ferra/scene.gltf' self.MODEL = self.loader.loadModel( AssetLoader.file_path(model_path)) self.MODEL.setZ(para[Parameter.vehicle_vis_z]) self.MODEL.setY(para[Parameter.vehicle_vis_y]) self.MODEL.setH(para[Parameter.vehicle_vis_h]) self.MODEL.set_scale(para[Parameter.vehicle_vis_scale]) self.MODEL.instanceTo(self.chassis_np)
def __init__(self, game, pos, vel): self.initCordPos = numpy.array([pos[0], pos[1]]) line = game.segLine lineLen = 0 prevPoint = line[0] self.initNum = 0 for point in line: norm = numpy.linalg.norm(point - prevPoint) if pos[0] >= lineLen and pos[0] < lineLen + norm: ldirec = (point - prevPoint) / norm rdirec = numpy.array([ldirec[1], -ldirec[0]]) post = prevPoint + (pos[0] - lineLen) * ldirec + rdirec * pos[1] self.initPos = Vec3(post[0], post[1], pos[2]) break lineLen = lineLen + norm prevPoint = point self.initNum = self.initNum + 1 chassis = setChassis(game, self.initPos, vel) BulletVehicle.__init__(self, game.world, chassis.node()) self.setCoordinateSystem(ZUp) game.world.attachVehicle(self) self.yugoNP = loader.loadModel('models/yugo/yugo.egg') self.yugoNP.reparentTo(chassis) # Right front wheel np = loader.loadModel('models/yugo/yugotireR.egg') np.reparentTo(game.worldNP) self.addWheel(Point3(0.70, 1.05, 0.3), True, np) # Left front wheel np = loader.loadModel('models/yugo/yugotireL.egg') np.reparentTo(game.worldNP) self.addWheel(Point3(-0.70, 1.05, 0.3), True, np) # Right rear wheel np = loader.loadModel('models/yugo/yugotireR.egg') np.reparentTo(game.worldNP) self.addWheel(Point3(0.70, -1.05, 0.3), False, np) # Left rear wheel np = loader.loadModel('models/yugo/yugotireL.egg') np.reparentTo(game.worldNP) self.addWheel(Point3(-0.70, -1.05, 0.3), False, np) # Steering info self.steering = 0.0 # degree self.steeringClamp = 45.0 # degree self.steeringIncrement = 10.0 # degree per second
def test_get_steering(): world = BulletWorld() # Chassis shape = BulletBoxShape(Vec3(0.6, 1.4, 0.5)) body = BulletRigidBodyNode('Vehicle') body.addShape(shape) world.attach(body) # Vehicle vehicle = BulletVehicle(world, body) world.attachVehicle(vehicle) # Wheel wheel = vehicle.createWheel() wheel.setSteering(30.0) # Returns the steering angle in degrees. assert wheel.getSteering() == approx(30.0)
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 __init__(self,game,pos,vel): self.initCordPos=numpy.array([pos[0],pos[1]]) line=game.segLine lineLen=0 prevPoint=line[0] self.initNum=0 for point in line: norm=numpy.linalg.norm(point-prevPoint) if pos[0]>=lineLen and pos[0]<lineLen+norm: ldirec=(point-prevPoint)/norm rdirec=numpy.array([ldirec[1],-ldirec[0]]) post=prevPoint+(pos[0]-lineLen)*ldirec+rdirec*pos[1] self.initPos=Vec3(post[0],post[1],pos[2]) break lineLen=lineLen+norm prevPoint=point self.initNum=self.initNum+1 chassis=setChassis(game,self.initPos,vel) BulletVehicle.__init__(self,game.world,chassis.node()) self.setCoordinateSystem(ZUp) game.world.attachVehicle(self) self.yugoNP = loader.loadModel('models/yugo/yugo.egg') self.yugoNP.reparentTo(chassis) # Right front wheel np = loader.loadModel('models/yugo/yugotireR.egg') np.reparentTo(game.worldNP) self.addWheel(Point3( 0.70, 1.05, 0.3), True, np) # Left front wheel np = loader.loadModel('models/yugo/yugotireL.egg') np.reparentTo(game.worldNP) self.addWheel(Point3(-0.70, 1.05, 0.3), True, np) # Right rear wheel np = loader.loadModel('models/yugo/yugotireR.egg') np.reparentTo(game.worldNP) self.addWheel(Point3( 0.70, -1.05, 0.3), False, np) # Left rear wheel np = loader.loadModel('models/yugo/yugotireL.egg') np.reparentTo(game.worldNP) self.addWheel(Point3(-0.70, -1.05, 0.3), False, np) # Steering info self.steering = 0.0 # degree self.steeringClamp = 45.0 # degree self.steeringIncrement = 10.0 # degree per second
def __createVehicle(self,bulletWorld): ''' Creates a vehicle, sets up wheels and does all the things ''' self._nodePath.node().setMass(800.0) # Chassis geometry np = loader.loadModel('media/tankBody.x') np.setHpr(90,0,0) np.reparentTo(self._nodePath) #np.setScale(self._tankSize*2) np.setPos(-self._tankSize+Vec3(1, 0, .5)) # Vehicle self.vehicle = BulletVehicle(bulletWorld, self._nodePath.node()) self.vehicle.setCoordinateSystem(2) bulletWorld.attachVehicle(self.vehicle) wheelNP = loader.loadModel('box') wheelNP.setScale(.01,.01,.01) wheelPos = [Point3(1, 1, 0),Point3(-1, 1, 0), Point3(1, -1, 0),Point3(-1, -1, 0)] for i in range(4): wheel = self.vehicle.createWheel() wheel.setWheelAxleCs(Vec3(-2*(i%2)+1, 0, 0)) wheel.setChassisConnectionPointCs(wheelPos[i]) self.__createWheel(wheel) self.vehicle.setSteeringValue(0,i) wheel.setRollInfluence((-2*(i%2)+1)*0.2)
def setupVehicle(self, main): scale = 0.5 # Chassis shape = BulletBoxShape(Vec3(0.6, 1.4, 0.5)) ts = TransformState.makePos(Point3(0, 0, 0.5 * scale)) name = self.username self.chassisNode = BulletRigidBodyNode(name) self.chassisNode.setTag('username', str(name)) self.chassisNP = main.worldNP.attachNewNode(self.chassisNode) self.chassisNP.setName(str(name)) self.chassisNP.node().addShape(shape, ts) self.chassisNP.setScale(scale) self.chassisNP.setPos(self.pos) if self.isCurrentPlayer: self.chassisNP.node().notifyCollisions(True) self.chassisNP.node().setMass(800.0) else: self.chassisNP.node().notifyCollisions(True) self.chassisNP.node().setMass(400.0) self.chassisNP.node().setDeactivationEnabled(False) main.world.attachRigidBody(self.chassisNP.node()) #np.node().setCcdSweptSphereRadius(1.0) #np.node().setCcdMotionThreshold(1e-7) # Vehicle self.vehicle = BulletVehicle(main.world, self.chassisNP.node()) self.vehicle.setCoordinateSystem(ZUp) main.world.attachVehicle(self.vehicle) self.yugoNP = loader.loadModel('models/yugo/yugo.egg') self.yugoNP.reparentTo(self.chassisNP) #self.carNP = loader.loadModel('models/batmobile-chassis.egg') #self.yugoNP.setScale(.7) #self.carNP.reparentTo(self.chassisNP) # Right front wheel np = loader.loadModel('models/yugo/yugotireR.egg') np.reparentTo(main.worldNP) self.addWheel(Point3( 0.70 * scale, 1.05 * scale, 0.3), True, np) # Left front wheel np = loader.loadModel('models/yugo/yugotireL.egg') np.reparentTo(main.worldNP) self.addWheel(Point3(-0.70 * scale, 1.05 * scale, 0.3), True, np) # Right rear wheel np = loader.loadModel('models/yugo/yugotireR.egg') np.reparentTo(main.worldNP) self.addWheel(Point3( 0.70 * scale, -1.05 * scale, 0.3), False, np) # Left rear wheel np = loader.loadModel('models/yugo/yugotireL.egg') np.reparentTo(main.worldNP) self.addWheel(Point3(-0.70 * scale, -1.05 * scale, 0.3), False, np)
def reset(self): car = loader.loadModel("models/car.egg") car.flattenLight() car_bounds = car.getTightBounds() car_shape = BulletBoxShape(Vec3((car_bounds[1].x - car_bounds[0].x) / 2, (car_bounds[1].y - car_bounds[0].y) / 2, (car_bounds[1].z - car_bounds[0].z) / 2)) car_ts = TransformState.makePos(Point3(0, 0, 0.5)) if self.car_node is not None: self.car_node.removeNode() self.car_node = render.attachNewNode(BulletRigidBodyNode('Car')) self.car_node.node().setDeactivationEnabled(False) self.car_node.node().setMass(600) self.car_node.node().addShape(car_shape, car_ts) self.world.attachRigidBody(self.car_node.node()) car.reparentTo(self.car_node) self.car_node.setPos(0, 6, 1) self.car = BulletVehicle(self.world, self.car_node.node()) self.car.setCoordinateSystem(ZUp) self.world.attachVehicle(self.car) self.car_node.setPos(0, 6, 1) self.car_node.setHpr(0, 0, 0) self.car.resetSuspension() self.car_node.node().clearForces() wheel_fl = loader.loadModel("models/wheelL") wheel_fl.reparentTo(render) self.make_wheel(Point3(-0.4, 1.28, 0), True, wheel_fl) wheel_fr = loader.loadModel("models/wheelR") wheel_fr.reparentTo(render) self.make_wheel(Point3(0.4, 1.28, 0), True, wheel_fr) wheel_rl = loader.loadModel("models/wheelL") wheel_rl.reparentTo(render) self.make_wheel(Point3(-0.4, -1.35, 0), False, wheel_rl) wheel_rr = loader.loadModel("models/wheelR") wheel_rr.reparentTo(render) self.make_wheel(Point3(0.4, -1.35, 0), False, wheel_rr)
def setupVehicle(self, bulletWorld): # Chassis shape = BulletBoxShape(Vec3(1, 2.2, 0.5)) ts = TransformState.makePos(Point3(0, 0, .7)) self.chassisNode = BulletRigidBodyNode('Vehicle') self.chassisNP = render.attachNewNode(self.chassisNode) self.chassisNP.node().addShape(shape, ts) self.chassisNP.node().notifyCollisions(True) self.chassisNP.setPosHpr(self.pos[0], self.pos[1], self.pos[2], self.pos[3], self.pos[4], self.pos[5]) # self.chassisNP.setPos(-5.34744, 114.773, 6) # self.chassisNP.setPos(49.2167, 64.7968, 10) self.chassisNP.node().setMass(800.0) self.chassisNP.node().setDeactivationEnabled(False) bulletWorld.attachRigidBody(self.chassisNP.node()) # np.node().setCcdSweptSphereRadius(1.0) # np.node().setCcdMotionThreshold(1e-7) # Vehicle self.vehicle = BulletVehicle(bulletWorld, self.chassisNP.node()) self.vehicle.setCoordinateSystem(ZUp) bulletWorld.attachVehicle(self.vehicle) self.carNP = loader.loadModel('models/swiftstar-chassis') tex = loader.loadTexture("models/tex/Futuristic_Car_C.jpg") self.carNP.setTexture(tex) self.carNP.setScale(1, 1, .9) self.carNP.setCollideMask(BitMask32.allOn()) self.carNP.reparentTo(self.chassisNP) # Right front wheel np = loader.loadModel('models/swiftstar-fr-tire') np.setCollideMask(BitMask32.allOff()) np.reparentTo(render) self.addWheel(Point3(1, 1.1, 1), True, np) # Left front wheel np = loader.loadModel('models/swiftstar-fl-tire') np.setCollideMask(BitMask32.allOff()) np.reparentTo(render) self.addWheel(Point3(-1, 1.1, 1), True, np) # Right rear wheel np = loader.loadModel('models/swiftstar-rr-tire') np.setCollideMask(BitMask32.allOff()) np.reparentTo(render) self.addWheel(Point3(1.2, -2, .8), False, np) # Left rear wheel np = loader.loadModel('models/swiftstar-rl-tire') np.setCollideMask(BitMask32.allOff()) np.reparentTo(render) self.addWheel(Point3(-1.2, -2, .8), False, np)
def _create_vehicle_chassis(self): self.LENGTH = type(self).LENGTH self.WIDTH = type(self).WIDTH self.HEIGHT = type(self).HEIGHT assert self.LENGTH < BaseVehicle.MAX_LENGTH, "Vehicle is too large!" assert self.WIDTH < BaseVehicle.MAX_WIDTH, "Vehicle is too large!" chassis = BaseRigidBodyNode(self.name, BodyName.Vehicle) chassis.setIntoCollideMask(CollisionGroup.Vehicle) chassis_shape = BulletBoxShape( Vec3(self.WIDTH / 2, self.LENGTH / 2, self.HEIGHT / 2)) ts = TransformState.makePos(Vec3(0, 0, self.HEIGHT / 2)) chassis.addShape(chassis_shape, ts) chassis.setMass(self.MASS) chassis.setDeactivationEnabled(False) chassis.notifyCollisions( True ) # advance collision check, do callback in pg_collision_callback physics_world = get_engine().physics_world vehicle_chassis = BulletVehicle(physics_world.dynamic_world, chassis) vehicle_chassis.setCoordinateSystem(ZUp) self.dynamic_nodes.append(vehicle_chassis) return vehicle_chassis
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)
class HeightfieldVehicle(ShowBase): def __init__(self, heightfield_fn="heightfield.png"): # Store the heightfield's filename. self.heightfield_fn = heightfield_fn """ Load some configuration variables, it's important for this to happen before ShowBase is initialized """ load_prc_file_data("", """ sync-video #t textures-power-2 none ###gl-coordinate-system default notify-level-gobj warning notify-level-grutil debug notify-level-shader_terrain debug notify-level-bullet debug ### model paths model-path /usr/share/panda3d model-path /home/juzzuj/code/prereq/bullet-samples/bullet-samples """) ShowBase.__init__(self) base.set_background_color(0.1, 0.1, 0.8, 1) base.set_frame_rate_meter(True) # Increase camera Field Of View and set near and far planes base.camLens.set_fov(90) base.camLens.set_near_far(0.1, 50000) # Lights alight = AmbientLight('ambientLight') alight.set_color(LVector4(0.5, 0.5, 0.5, 1)) alightNP = render.attach_new_node(alight) dlight = DirectionalLight('directionalLight') dlight.set_direction(LVector3(1, 1, -1)) dlight.set_color(LVector4(0.7, 0.7, 0.7, 1)) dlightNP = render.attach_new_node(dlight) render.clear_light() render.set_light(alightNP) render.set_light(dlightNP) # Basic game controls self.accept('escape', self.do_exit) self.accept('f1', base.toggle_wireframe) self.accept('f2', base.toggle_texture) self.accept('f3', self.toggle_debug) self.accept('f5', self.do_screenshot) self.accept('r', self.do_reset) # Vehicle controls inputState.watchWithModifiers('forward', 'w') inputState.watchWithModifiers('turnLeft', 'a') inputState.watchWithModifiers('reverse', 's') inputState.watchWithModifiers('turnRight', 'd') inputState.watchWithModifiers('forward', 'arrow_up') inputState.watchWithModifiers('turnLeft', 'arrow_left') inputState.watchWithModifiers('reverse', 'arrow_down') inputState.watchWithModifiers('turnRight', 'arrow_right') self.accept('space', self.reset_vehicle) # Controls to do with the terrain #self.accept('t', self.rise_in_front) self.accept('t', self.deform_terrain, ["raise"]) self.accept('g', self.deform_terrain, ["depress"]) self.accept('b', self.drop_boxes) # Some debugging and miscellaneous controls self.accept('e', self.query_elevation) self.accept('c', self.convert_coordinates) self.accept('p', self.save) self.accept('h', self.hide_terrain) # Task taskMgr.add(self.update, 'updateWorld') self.setup() """ Macro-like function used to reduce the amount to code needed to create the on screen instructions """ def genLabelText(self, i, text, parent="a2dTopLeft"): return OnscreenText(text=text, parent=getattr(base, parent), scale=.05, pos=(0.06, -.065 * i), fg=(1, 1, 1, 1), align=TextNode.ALeft) # _____HANDLER_____ def cleanup(self): self.world = None self.worldNP.remove_node() self.terrain.remove_node() self.skybox.remove_node() del self.terrain_node def do_exit(self): self.cleanup() sys.exit(1) def do_reset(self): self.cleanup() self.setup() def toggle_debug(self): if self.debugNP.is_hidden(): self.debugNP.show() else: self.debugNP.hide() def do_screenshot(self): base.screenshot('HeightfieldVehicle') # ____TASK___ def update(self, task): # Get the time passed since the last frame dt = globalClock.get_dt() # Pass dt as parameter (we need it for sensible steering calculations) self.process_input(dt) """ Basically, we want to put our camera where the camera floater is. We need the floater's world (=render-relative) position (it's parented to the vehicle) """ floater_pos = render.get_relative_point(self.camera_floater, LVector3(0)) """ If the camera floater is under the terrain surface, adjust it, so that it stays above the terrain. """ elevation_at_floater_pos = self.query_elevation(floater_pos) if elevation_at_floater_pos.z >= floater_pos.z: floater_pos.z = elevation_at_floater_pos.z + 1. # Now we set our camera's position and make it look at the vehicle. base.cam.set_pos(floater_pos) base.cam.look_at(self.vehicleNP) # Let the Bullet library do physics calculations. self.world.do_physics(dt, 10, 0.008) return task.cont def process_input(self, dt): # Relax steering towards straight self.steering *= 1 - self.steering_relax_factor * dt engine_force = 0.0 brake_force = 0.0 if inputState.isSet('forward'): engine_force = 1000.0 brake_force = 0.0 if inputState.isSet('reverse'): engine_force = 0.0 brake_force = 100.0 if inputState.isSet('turnLeft'): self.steering += dt * self.steering_increment self.steering = min(self.steering, self.steering_clamp) if inputState.isSet('turnRight'): self.steering -= dt * self.steering_increment self.steering = max(self.steering, -self.steering_clamp) # Lower steering intensity for high speeds self.steering *= 1. - (self.vehicle.get_current_speed_km_hour() * self.steering_speed_reduction_factor) # Apply steering to front wheels #self.vehicle.get_wheels()[0].set_steering(self.steering) #self.vehicle.get_wheels()[1].set_steering(self.steering) #self.vehicle.wheels[0].steering = self.steering #self.vehicle.wheels[1].steering = self.steering self.vehicle.set_steering_value(self.steering, 0) self.vehicle.set_steering_value(self.steering, 1) # Apply engine and brake to rear wheels #self.vehicle.wheels[2].engine_force = engine_force #self.vehicle.wheels[3].engine_force = engine_force #self.vehicle.wheels[2].brake = brake_force #self.vehicle.wheels[3].brake = brake_force self.vehicle.apply_engine_force(engine_force, 2) self.vehicle.apply_engine_force(engine_force, 3) self.vehicle.set_brake(brake_force, 2) self.vehicle.set_brake(brake_force, 3) def setup(self): # Bullet physics world self.worldNP = render.attach_new_node('World') self.debugNP = self.worldNP.attach_new_node(BulletDebugNode('Debug')) self.world = BulletWorld() self.world.set_gravity(LVector3(0, 0, -9.81)) self.world.set_debug_node(self.debugNP.node()) # Vehicle # Steering info self.steering = 0.0 # degrees self.steering_clamp = 45.0 # degrees self.steering_increment = 120.0 # degrees per second # How fast steering relaxes to straight ahead self.steering_relax_factor = 2.0 # How much steering intensity decreases with higher speeds self.steering_speed_reduction_factor = 0.003 # Chassis collision box (note: Bullet uses half-measures) shape = BulletBoxShape(LVector3(0.6, 1.4, 0.5)) ts = TransformState.make_pos(LPoint3(0, 0, 0.5)) self.vehicleNP = self.worldNP.attach_new_node( BulletRigidBodyNode('Vehicle')) self.vehicleNP.node().add_shape(shape, ts) # Set initial position self.vehicleNP.set_pos(0, 70, -10) self.vehicleNP.node().set_mass(800.0) self.vehicleNP.node().set_deactivation_enabled(False) self.world.attach(self.vehicleNP.node()) # Camera floater self.attach_camera_floater() # BulletVehicle self.vehicle = BulletVehicle(self.world, self.vehicleNP.node()) self.world.attach(self.vehicle) # Vehicle model self.yugoNP = loader.load_model('models/yugo/yugo.egg') self.yugoNP.reparent_to(self.vehicleNP) # Right front wheel np = loader.load_model('models/yugo/yugotireR.egg') np.reparent_to(self.worldNP) self.add_wheel(LPoint3( 0.70, 1.05, 0.3), True, np) # Left front wheel np = loader.load_model('models/yugo/yugotireL.egg') np.reparent_to(self.worldNP) self.add_wheel(LPoint3(-0.70, 1.05, 0.3), True, np) # Right rear wheel np = loader.load_model('models/yugo/yugotireR.egg') np.reparent_to(self.worldNP) self.add_wheel(LPoint3( 0.70, -1.05, 0.3), False, np) # Left rear wheel np = loader.load_model('models/yugo/yugotireL.egg') np.reparent_to(self.worldNP) self.add_wheel(LPoint3(-0.70, -1.05, 0.3), False, np) # Load a skybox self.skybox = self.loader.load_model( "samples/shader-terrain/models/skybox.bam") self.skybox.reparent_to(self.render) self.skybox.set_scale(20000) skybox_texture = self.loader.load_texture( "samples/shader-terrain/textures/skybox.jpg") skybox_texture.set_minfilter(SamplerState.FT_linear) skybox_texture.set_magfilter(SamplerState.FT_linear) skybox_texture.set_wrap_u(SamplerState.WM_repeat) skybox_texture.set_wrap_v(SamplerState.WM_mirror) skybox_texture.set_anisotropic_degree(16) self.skybox.set_texture(skybox_texture) skybox_shader = Shader.load(Shader.SL_GLSL, "samples/shader-terrain/skybox.vert.glsl", "samples/shader-terrain/skybox.frag.glsl") self.skybox.set_shader(skybox_shader) # Terrain self.setup_terrain() def add_wheel(self, pos, front, np): wheel = self.vehicle.create_wheel() wheel.set_node(np.node()) wheel.set_chassis_connection_point_cs(pos) wheel.set_front_wheel(front) wheel.set_wheel_direction_cs(LVector3(0, 0, -1)) wheel.set_wheel_axle_cs(LVector3(1, 0, 0)) wheel.set_wheel_radius(0.25) wheel.set_max_suspension_travel_cm(40.0) wheel.set_suspension_stiffness(40.0) wheel.set_wheels_damping_relaxation(2.3) wheel.set_wheels_damping_compression(4.4) wheel.set_friction_slip(100.0) wheel.set_roll_influence(0.1) def attach_camera_floater(self): """ Set up an auxiliary camera floater, which is parented to the vehicle. Every frame base.cam's position will be set to the camera floater's. """ camera_behind = 8 camera_above = 3 self.camera_floater = NodePath("camera_floater") self.camera_floater.reparent_to(self.vehicleNP) self.camera_floater.set_y(-camera_behind) self.camera_floater.set_z(camera_above) def reset_vehicle(self): reset_pos = self.vehicleNP.get_pos() reset_pos.z += 3 self.vehicleNP.node().clear_forces() self.vehicleNP.node().set_linear_velocity(LVector3(0)) self.vehicleNP.node().set_angular_velocity(LVector3(0)) self.vehicleNP.set_pos(reset_pos) self.vehicleNP.set_hpr(LVector3(0)) def drop_boxes(self): """ Puts a stack of boxes at a distance in front of the vehicle. The boxes will not necessarily spawn above the terrain and will not be cleaned up. """ model = loader.load_model('models/box.egg') model.set_pos(-0.5, -0.5, -0.5) model.flatten_light() shape = BulletBoxShape(LVector3(0.5, 0.5, 0.5)) ahead = self.vehicleNP.get_pos() + self.vehicle.get_forward_vector()*15 for i in range(6): node = BulletRigidBodyNode('Box') node.set_mass(5.0) node.add_shape(shape) node.set_deactivation_enabled(False) np = render.attach_new_node(node) np.set_pos(ahead.x, ahead.y, ahead.z + i*2) self.world.attach(node) model.copy_to(np) def query_elevation(self, xy_pos=None): """ Query elevation for xy_pos if present, else for vehicle position. No xy_pos means verbose mode. """ query_pos = xy_pos or self.vehicleNP.get_pos() """ This method is accurate and may be useful for placing objects on the terrain surface. """ result = self.world.ray_test_closest( LPoint3(query_pos.x, query_pos.y, -10000), LPoint3(query_pos.x, query_pos.y, 10000)) if result.has_hit(): hit_pos = result.get_hit_pos() if not xy_pos: print("Bullet heightfield elevation at " "X {:.2f} | Y {:.2f} is {:.3f}".format( hit_pos.x, hit_pos.y, hit_pos.z)) else: hit_pos = None if not xy_pos: print("Could not query elevation at {}".format(xy_pos)) """ This method is less accurate than the one above. Under heavy ray-testing stress (ray tests are performed for all vehicle wheels, the above elevation query etc.) Bullet sometimes seems to be a little unreliable. """ texspace_pos = self.terrain.get_relative_point(render, query_pos) stm_pos = self.terrain_node.uv_to_world( LTexCoord(texspace_pos.x, texspace_pos.y)) if not xy_pos: print("ShaderTerrainMesh elevation at " "X {:.2f} | Y {:.2f} is {:.3f}".format( stm_pos.x, stm_pos.y, stm_pos.z)) return hit_pos or stm_pos def setup_terrain(self): """ Terrain info Units are meters, which is preferable when working with Bullet. """ self.terrain_scale = LVector3(512, 512, 100) self.terrain_pos = LVector3(-256, -256, -70) # sample values for a 4096 x 4096px heightmap. #self.terrain_scale = LVector3(4096, 4096, 1000) #self.terrain_pos = LVector3(-2048, -2048, -70) """ Diamond_subdivision is an alternating triangulation scheme and may produce better results. """ use_diamond_subdivision = True """ Construct the terrain Without scaling, any ShaderTerrainMesh is 1x1x1 units. """ self.terrain_node = ShaderTerrainMesh() """ Set a heightfield, the heightfield should be a 16-bit png and have a quadratic size of a power of two. """ heightfield = Texture() heightfield.read(self.heightfield_fn) heightfield.set_keep_ram_image(True) self.terrain_node.heightfield = heightfield # Display characteristic values of the heightfield texture #minpoint, maxpoint, avg = LPoint3(), LPoint3(), LPoint3() #heightfield.calc_min_max(minpoint, maxpoint) #heightfield.calc_average_point(avg, 0.5, 0.5, 0.5) #print("avg: {} min: {} max: {}".format(avg.x, minpoint.x, maxpoint.x)) """ Set the target triangle width. For a value of 10.0 for example, the ShaderTerrainMesh will attempt to make every triangle 10 pixels wide on screen. """ self.terrain_node.target_triangle_width = 10.0 if use_diamond_subdivision: """ This has to be specified before calling .generate() The default is false. """ load_prc_file_data("", "stm-use-hexagonal-layout true") self.terrain_node.generate() """ Attach the terrain to the main scene and set its scale. With no scale set, the terrain ranges from (0, 0, 0) to (1, 1, 1) """ self.terrain = self.render.attach_new_node(self.terrain_node) self.terrain.set_scale(self.terrain_scale) self.terrain.set_pos(self.terrain_pos) """ Set a vertex and a fragment shader on the terrain. The ShaderTerrainMesh only works with an applied shader. """ terrain_shader = Shader.load(Shader.SL_GLSL, "samples/shader-terrain/terrain.vert.glsl", "samples/shader-terrain/terrain.frag.glsl") self.terrain.set_shader(terrain_shader) self.terrain.set_shader_input("camera", base.camera) # Set some texture on the terrain grass_tex = self.loader.load_texture( "samples/shader-terrain/textures/grass.png") grass_tex.set_minfilter(SamplerState.FT_linear_mipmap_linear) grass_tex.set_anisotropic_degree(16) self.terrain.set_texture(grass_tex) """ Set up the DynamicHeightfield (it's a type of PfmFile). We load the same heightfield image as with ShaderTerrainMesh. """ self.DHF = DynamicHeightfield() self.DHF.read(self.heightfield_fn) """ Set up empty PfmFiles to prepare stuff in that is going to dynamically modify our terrain. """ self.StagingPFM = PfmFile() self.RotorPFM = PfmFile() """ Set up the BulletHeightfieldShape (=collision terrain) and give it some sensible physical properties. """ self.HFS = BulletHeightfieldShape(self.DHF, self.terrain_scale.z, STM=True) if use_diamond_subdivision: self.HFS.set_use_diamond_subdivision(True) HFS_rigidbody = BulletRigidBodyNode("BulletTerrain") HFS_rigidbody.set_static(True) friction = 2.0 HFS_rigidbody.set_anisotropic_friction( LVector3(friction, friction, friction/1.3)) HFS_rigidbody.set_restitution(0.3) HFS_rigidbody.add_shape(self.HFS) self.world.attach(HFS_rigidbody) HFS_NP = NodePath(HFS_rigidbody) HFS_NP.reparent_to(self.worldNP) """ This aligns the Bullet terrain with the ShaderTerrainMesh rendered terrain. It will be exact as long as the terrain vertex shader from the STM sample is used and no additional tessellation shader. For Bullet (as for other physics engines) the origin of objects is at the center. """ HFS_NP.set_pos(self.terrain_pos + self.terrain_scale/2) HFS_NP.set_sx(self.terrain_scale.x / heightfield.get_x_size()) HFS_NP.set_sy(self.terrain_scale.y / heightfield.get_y_size()) # Disables Bullet debug rendering for the terrain, because it is slow. #HFS_NP.node().set_debug_enabled(False) """ Finally, link the ShaderTerrainMesh and the BulletHeightfieldShape to the DynamicHeightfield. From now on changes to the DynamicHeightfield will propagate to the (visible) ShaderTerrainMesh and the (collidable) BulletHeightfieldShape. """ self.HFS.set_dynamic_heightfield(self.DHF) self.terrain_node.set_dynamic_heightfield(self.DHF) def deform_terrain(self, mode="raise", duration=1.0): self.deform_duration = duration """ Calculate distance to the spot at which we want to raise the terrain. At its current speed the vehicle would reach it in 2.5 seconds. [km/h] / 3.6 = [m/s] * [s] = [m] """ distance = self.vehicle.get_current_speed_km_hour() / 3.6 * 2.5 spot_pos_world = (self.vehicleNP.get_pos() + self.vehicle.get_forward_vector() * distance) spot_pos_heightfield = self.terrain_node.world_to_heightfield( spot_pos_world) xcenter = spot_pos_heightfield.x ycenter = spot_pos_heightfield.y """ To create a smooth hill/depression we call PfmFile.pull_spot to create a sort of gradient. You can use self.cardmaker_debug to view it. From the Panda3D API documentation of PfmFile.pull_spot: Applies delta * t to the point values within radius (xr, yr) distance of (xc, yc). The t value is scaled from 1.0 at the center to 0.0 at radius (xr, yr), and this scale follows the specified exponent. Returns the number of points affected. """ # Delta to apply to the point values in DynamicHeightfield array. delta = LVector4(0.001) # Make the raised spot elliptical. xradius = 10.0 # meters yradius = 6.0 # meters # Choose an exponent exponent = 0.6 # Counter-clockwise angle between Y-axis angle = self.vehicle.get_forward_vector().signed_angle_deg( LVector3.forward(), LVector3.down()) # Define all we need to repeatedly deform the terrain using a task. self.spot_params = [delta, xcenter, ycenter, xradius, yradius, exponent, mode] # Clear staging area to a size that fits our raised region. self.StagingPFM.clear(int(xradius)*2, int(yradius)*2, num_channels=1) """ There are two options: (1) Rotate our hill/depression to be perpendicular to the vehicle's trajectory. This is the default. (2) Just put it in the terrain unrotated. """ # Option (1) self.StagingPFM.pull_spot(delta, xradius-0.5, yradius-0.5, xradius, yradius, exponent) # Rotate wider side so it's perpendicular to vehicle's trajectory. self.RotorPFM.rotate_from(self.StagingPFM, angle) self.raise_start_time = globalClock.get_real_time() taskMgr.do_method_later(0.03, self.deform_perpendicular, "DeformPerpendicularSpot") # Option (2) #taskMgr.do_method_later(0.03, self.deform_regular, "RaiseASpot") def deform_perpendicular(self, task): if (globalClock.get_real_time() - self.raise_start_time < self.deform_duration): (delta, xcenter, ycenter, xradius, yradius, exponent, mode) = self.spot_params """ Copy rotated hill to our DynamicHeightfield. The values from RotorPFM are added to the ones found in the DynamicHeightfield. """ self.DHF.add_sub_image(self.RotorPFM, int(xcenter - self.RotorPFM.get_x_size()/2), int(ycenter - self.RotorPFM.get_y_size()/2), 0, 0, self.RotorPFM.get_x_size(), self.RotorPFM.get_y_size(), 1.0 if mode == "raise" else -1.0) # Output images of our PfmFiles. #self.RotorPFM.write("dbg_RotorPFM.png") #self.StagingPFM.write("dbg_StagingPFM.png") return task.again self.cardmaker_debug() return task.done def deform_regular(self, task): if (globalClock.get_real_time() - self.raise_start_time < self.deform_duration): (delta, xcenter, ycenter, xradius, yradius, exponent, mode) = self.spot_params self.DHF.pull_spot(delta * (1.0 if mode == "raise" else -1.0), xcenter, ycenter, xradius, yradius, exponent) return task.again return task.done def convert_coordinates(self): vpos = self.vehicleNP.get_pos() print("VPOS world: ", vpos) W2H = self.terrain_node.world_to_heightfield(vpos) print("W2H: ", W2H) H2W = self.terrain_node.heightfield_to_world(LTexCoord(W2H.x, W2H.y)) print("H2W: ", H2W) W2U = self.terrain_node.world_to_uv(vpos) print("W2U: ", W2U) U2W = self.terrain_node.uv_to_world(LTexCoord(W2U.x, W2U.y)) print("U2W: ", U2W) def hide_terrain(self): if self.terrain.is_hidden(): self.terrain.show() else: self.terrain.hide() def save(self): self.DHF.write("dbg_dynamicheightfield.png") def pfmvizzer_debug(self, pfm): vizzer = PfmVizzer(pfm) """ To align the mesh we generate with PfmVizzer with our ShaderTerrainMesh, BulletHeightfieldShape, and DynamicHeightfield, we need to set a negative scale on the Y axis. This reverses the winding order of the triangles in the mesh, which is why we choose PfmVizzer.MF_back Note that this does not accurately match up with our mesh because the interpolation differs. Still, PfmVizzer can be useful when inspecting, distorting, etc. PfmFiles. """ self.vizNP = vizzer.generate_vis_mesh(PfmVizzer.MF_back) self.vizNP.set_texture(loader.load_texture("maps/grid.rgb")) self.vizNP.reparent_to(render) if pfm == self.DHF or pfm == self.terrain_node.heightfield: self.vizNP.set_pos(self.terrain_pos.x, -self.terrain_pos.y, self.terrain_pos.z) self.vizNP.set_scale(self.terrain_scale.x, -self.terrain_scale.y, self.terrain_scale.z) def cardmaker_debug(self): for node in render2d.find_all_matches("pfm"): node.remove_node() for text in base.a2dBottomLeft.find_all_matches("*"): text.remove_node() width = 0.2 # render2d coordinates range: [-1..1] # Pseudo-normalize our PfmFile for better contrast. normalized_pfm = PfmFile(self.RotorPFM) max_p = LVector3() normalized_pfm.calc_min_max(LVector3(), max_p) normalized_pfm *= 1.0 / max_p.x # Put it in a texture tex = Texture() tex.load(normalized_pfm) # Apply the texture to a quad and put it in the lower left corner. cm = CardMaker("pfm") cm.set_frame(0, width, 0, width / normalized_pfm.get_x_size() * normalized_pfm.get_y_size()) card = base.render2d.attach_new_node(cm.generate()) card.set_pos(-1, 0, -1) card.set_texture(tex) # Display max value text self.genLabelText(-3, "Max value: {:.3f} == {:.2f}m".format(max_p.x, max_p.x * self.terrain_scale.z), parent="a2dBottomLeft")
def setup(self): self.worldNP = render.attachNewNode('World') # World self.debugNP = self.worldNP.attachNewNode(BulletDebugNode('Debug')) self.world = BulletWorld() self.world.setGravity(Vec3(0, 0, -9.81)) self.world.setDebugNode(self.debugNP.node()) # Plane shape = BulletPlaneShape(Vec3(0, 0, 1), 0) np = self.ground = self.worldNP.attachNewNode( BulletRigidBodyNode('Ground')) np.node().addShape(shape) np.setPos(0, 0, -1) np.setCollideMask(BitMask32.allOn()) self.world.attachRigidBody(np.node()) # collision self.maze = [] for pos in [(0.0, 72.0, 0.0), (-11.0, 60.0, 0.0), (11.0, 60.0, 0.0), (-11.0, 48.0, 0.0), (11.0, 48.0, 0.0), (-11.0, 36.0, 0.0), (11.0, 36.0, 0.0), (-11.0, 24.0, 0.0), (11.0, 24.0, 0.0), (-11.0, 12.0, 0.0), (11.0, 12.0, 0.0), (-11.0, 0.0, 0.0), (11.0, 0.0, 0.0), (0.0, -12.0, 0.0), (0.5, 12.0, 1.0), (-0.5, 12.0, 1.0)]: translate = False if (abs(pos[0]) == 0.5): translate = True visNP = loader.loadModel('../models/ball.egg') else: visNP = loader.loadModel('../models/maze.egg') visNP.clearModelNodes() visNP.reparentTo(self.ground) visNP.setPos(pos[0], pos[1], pos[2]) bodyNPs = BulletHelper.fromCollisionSolids(visNP, True) for bodyNP in bodyNPs: bodyNP.reparentTo(render) if translate: bodyNP.setPos(pos[0], pos[1], pos[2] - 1) else: bodyNP.setPos(pos[0], pos[1], pos[2]) if isinstance(bodyNP.node(), BulletRigidBodyNode): bodyNP.node().setMass(0.0) bodyNP.node().setKinematic(True) self.maze.append(bodyNP) for bodyNP in self.maze: self.world.attachRigidBody(bodyNP.node()) # Chassis mass = rospy.get_param('~mass') #chassis_shape = rospy.get_param('~chassis_shape') shape = BulletBoxShape(Vec3(0.6, 1.4, 0.5)) ts = TransformState.makePos(Point3(0, 0, 0.5)) np = self.worldNP.attachNewNode(BulletRigidBodyNode('Vehicle')) np.node().addShape(shape, ts) rand_vals = numpy.random.random(2) * 8 - 4.0 np.setPos(rand_vals[0], 0.0, -0.6) np.node().setMass(mass) np.node().setDeactivationEnabled(False) first_person = rospy.get_param('~first_person') self.camera_sensor = Panda3dCameraSensor(base, color=True, depth=True, size=(160, 90)) self.camera_node = self.camera_sensor.cam if first_person: self.camera_node.reparentTo(np) self.camera_node.setPos(0.0, 1.0, 1.0) self.camera_node.lookAt(0.0, 6.0, 0.0) else: self.camera_node.reparentTo(np) self.camera_node.setPos(0.0, -10.0, 5.0) self.camera_node.lookAt(0.0, 5.0, 0.0) base.cam.reparentTo(np) base.cam.setPos(0.0, -10.0, 5.0) base.cam.lookAt(0.0, 5.0, 0.0) self.world.attachRigidBody(np.node()) np.node().setCcdSweptSphereRadius(1.0) np.node().setCcdMotionThreshold(1e-7) # Vehicle self.vehicle_node = np.node() self.vehicle = BulletVehicle(self.world, np.node()) self.vehicle.setCoordinateSystem(ZUp) self.world.attachVehicle(self.vehicle) self.yugoNP = loader.loadModel('../models/yugo/yugo.egg') self.yugoNP.reparentTo(np) # Right front wheel np = loader.loadModel('../models/yugo/yugotireR.egg') np.reparentTo(self.worldNP) self.addWheel(Point3(0.70, 1.05, 0.3), True, np) # Left front wheel np = loader.loadModel('../models/yugo/yugotireL.egg') np.reparentTo(self.worldNP) self.addWheel(Point3(-0.70, 1.05, 0.3), True, np) # Right rear wheel np = loader.loadModel('../models/yugo/yugotireR.egg') np.reparentTo(self.worldNP) self.addWheel(Point3(0.70, -1.05, 0.3), False, np) # Left rear wheel np = loader.loadModel('../models/yugo/yugotireL.egg') np.reparentTo(self.worldNP) self.addWheel(Point3(-0.70, -1.05, 0.3), False, np)
class Tank(DynamicWorldObject): '''Child of WorldObject, with all of the things that makes a Tank a tank. Includes a Weapon ''' def __init__(self, world, attach, name = '', position = Vec3(0,0,0), orientation = Vec3(0,0,0), turretPitch = 0): #Constant Relevant Instatiation Parameters self._tankSize = Vec3(1, 1.5, .5) # Actually a half-size self._tankSideLength = max(self._tankSize) friction = .3 tankMass = 800.0 # Rewrite constructor to include these? self._maxVel = 4 self._maxRotVel = 1 self._maxThrusterAccel = 5000 self._breakForce = 1000 turretRelPos = (0, 0, 0) #Relative to tank self._shape = BulletBoxShape(Vec3(1,1.5,.5)) #chassis self._transformState = TransformState.makePos(Point3(0, 0, 0)) #offset DynamicWorldObject.__init__(self, world, attach, name, position, self._shape, orientation, Vec3(0,0,0), mass = tankMass) #Initial velocity must be 0 self.__createVehicle(self._tankWorld.getPhysics()) self._collisionCounter = 0 self._taskTimer = 0 self._nodePath.node().setFriction(friction) self._nodePath.setPos(position) # Set up turret nodepath # (Nodepaths are how objects are managed in Panda3d) self.onTask = 0 # Make collide mask (What collides with what) self._nodePath.setCollideMask(0xFFFF0000) #self._nodePath.setCollideMask(BitMask32.allOff()) self.movementPoint = Point3(10,10,0) #print "Tank.__init__: " + name # Set up the def __createVehicle(self,bulletWorld): ''' Creates a vehicle, sets up wheels and does all the things ''' self._nodePath.node().setMass(800.0) # Chassis geometry np = loader.loadModel('media/tankBody.x') np.setHpr(90,0,0) np.reparentTo(self._nodePath) #np.setScale(self._tankSize*2) np.setPos(-self._tankSize+Vec3(1, 0, .5)) # Vehicle self.vehicle = BulletVehicle(bulletWorld, self._nodePath.node()) self.vehicle.setCoordinateSystem(2) bulletWorld.attachVehicle(self.vehicle) wheelNP = loader.loadModel('box') wheelNP.setScale(.01,.01,.01) wheelPos = [Point3(1, 1, 0),Point3(-1, 1, 0), Point3(1, -1, 0),Point3(-1, -1, 0)] for i in range(4): wheel = self.vehicle.createWheel() wheel.setWheelAxleCs(Vec3(-2*(i%2)+1, 0, 0)) wheel.setChassisConnectionPointCs(wheelPos[i]) self.__createWheel(wheel) self.vehicle.setSteeringValue(0,i) wheel.setRollInfluence((-2*(i%2)+1)*0.2) def __createWheel(self,wheel): ''' sets up properties for wheel. ''' wheel.setWheelDirectionCs(Vec3(0, 0, -1)) wheel.setFrontWheel(False) wheel.setWheelRadius(0.15) wheel.setMaxSuspensionTravelCm(40.0) wheel.setSuspensionStiffness(40.0) wheel.setWheelsDampingRelaxation(2.3) wheel.setWheelsDampingCompression(4.4) wheel.setFrictionSlip(100.0) def getWheels(self): ''' returns a list of wheelPos ''' return self.vehicle.getWheels() def setWeaponHp(self, heading, pitch): ''' float heading float pitch ''' self._weapon.setHp(heading,pitch) def moveWeapon(self, heading = 0, pitch = 0): rot = self.getHpr() self.setWeaponHp(rot[0] + heading, rot[1] + pitch) def distanceScan(self): ''' This scan projects rays from the objects in the field toward the tank in question. This scan does not perform as well as scan when the objects are bunched together. When small objects are spread out reasonably (more than 5 at a viewing range of 50), this scan performs better. Using this scan, large objects can hide behind small objects This scan has the feature that it will pick up a lone object guaranteed at any distance. ''' self.wait(.1) potentialNPs = self._tankWorld.render.getChildren() found = [] for np in potentialNPs: if type(np.node()) == BulletRigidBodyNode and np != self and not np.isHidden(): pFrom = np.getPos(render) #Fix for cubeObjects (z = 0). They collide with the floor if pFrom[2] < 1: pFrom[2] = 1.1 pTo = self.getPos() + self.getPos() - pFrom #pTo is a Vec3, turned to a point for rayTest result = self._tankWorld.getPhysics().rayTestClosest( pFrom, Point3(pTo[0], pTo[1], pTo[2])) if result.hasHit() and result.getNode() == self._nodePath.node(): #found.append((np.node().getPrevTransform().getPos(), # np.node().getName())) found.append((np.getPos(render), np.node().getName())) elif result.hasHit(): #print "Tank.distanceScan: ", #print np, result.getNode(), pFrom, pTo #print "Neigh" x = 1 return found def __bulletRays(self, numPoints, relAngleRange, height): '''Helper function for scans and pings. Runs through the relAngleRange (two ints, in degrees) at the given numPoints and height. Runs a Bullet rayTestClosest to find the nearest hit. Returns a list of BulletClosestHitRayResult objects ''' distanceOfMap = 100000 results = [] #scanResolution = numPoints / 360.0 angleSweep = relAngleRange[1] - relAngleRange[0] + 0.0 pos = self._nodePath.getPos() heading = self._nodePath.getH() for i in range(numPoints): if angleSweep == 0: angle = (heading + relAngleRange[0]) * (math.pi / 180) else: angle = (i / angleSweep + heading + relAngleRange[0]) * (math.pi / 180) pFrom = Point3(math.sin(angle) * self._tankSideLength + pos[0], math.cos(angle) * self._tankSideLength + pos[1], height) pTo = Point3(math.sin(angle) * distanceOfMap + pos[0], math.cos(angle) * distanceOfMap + pos[1], height) result = self._tankWorld.getPhysics().rayTestClosest(pFrom, pTo) results.append((result, angle * 180 / math.pi - heading)) return results def scan(self, numPoints = 360, relAngleRange = (-180, 180), height = 1): ''' This function scans the map to find the other objects on it. The scan works iteratively, based on the angle range (given relative to the tank's current heading) and the number of points given. This is a more realistic scan, but does not work as well with smaller objects and larger distances ''' found = [] numFound = 0 results = self.__bulletRays(numPoints, relAngleRange, height) prevNodes = dict() for item in results: result = item[0] if result.hasHit(): newNode = result.getNode() if newNode not in prevNodes: found.append((newNode.getPrevTransform().getPos(), newNode.getName())) prevNodes[newNode] = 0 numFound = numFound + 1 return found def pingPointsAbs(self, numPoints = 360, relAngleRange = (-180, 180), height = 1): '''Returns a list of absolute coordinate points on each of the ''' found = [] results = self.__bulletRays(numPoints, relAngleRange, height) for item in results: result = item[0] if result.hasHit(): newNode = result.getNode() found.append((result.getHitPos(), item[1], newNode.getName())) return found def pingPoints(self, numPoints = 360, relAngleRange = (-180, 180), height = 1): found = self.pingPointsAbs(numPoints, relAngleRange, height) pos = self.getPos() for i in range(len(found)): hitPos = found[i][0] relPos = Point3(hitPos[0] - pos[0], hitPos[1] - pos[1], hitPos[2] - pos[2]) found[i] = (relPos, found[i][1], found[i][2]) return found def pingDistance(self, numPoints = 360, relAngleRange = (-180, 180), height = 1): found = self.pingPoints(numPoints, relAngleRange, height) for i in range(len(found)): relPos = found[i][0] distance = math.sqrt(relPos[0]**2 + relPos[1]**2) found[i] = (distance, found[i][1], found[i][2]) return found def applyThrusters(self, right = 1, left = 1): #set acceleration '''change acceleration to a percent of the maximum acceleration''' #if right > 1 or amt < 0: # raise ValueError("amt must be between 0 and 1") # tankNode = self._nodePath.node() # angle = self.nodePath.getH() #Apply force in current direction # magnitude = amt * (self._maxThrusterAccel) + (tankNode.getFriction() * # self._nodePath.node().getMass()) # force = Vec3(magnitude * math.cos(angle), # magnitude * math.sin(angle), 0) # self.nodePath.node().applyForce(force) self.vehicle.reset_suspension() self.applyBrakes(0) vel = self.vehicle.getChassis().getLinearVelocity() for i in range(4): self.vehicle.applyEngineForce(0,i) self.vehicle.setBrake(0,i) if vel.length() < self._maxVel: self.vehicle.applyEngineForce(-left*self._maxThrusterAccel,0) self.vehicle.applyEngineForce(right*self._maxThrusterAccel,1) self.vehicle.applyEngineForce(-left*self._maxThrusterAccel,2) self.vehicle.applyEngineForce(right*self._maxThrusterAccel,3) #for i in range(0,1): # self.vehicle.applyEngineForce((left*(i)%2+right*(i+1)%2)*self._maxThrusterAccel,i) #for i in range(2): # self.vehicle.applyEngineForce((left*(i)%2+right*(i+1)%2)*self._maxThrusterAccel,i+2) #self.vehicle.applyEngineForce((2*i%2-1)*engineForce,i) ## for 1 and 3useleft ## for 0 and 2 use right else: for i in range(4): #self.vehicle.applyEngineForce((2*i%2-1)*engineForce,i) self.vehicle.applyEngineForce(0,i) def applyBrakes(self, amt=1): for i in range(4): #self.vehicle.applyEngineForce((2*i%2-1)*engineForce,i) self.vehicle.applyEngineForce(0,i) self.vehicle.setBrake(amt*self._breakForce,i) def setVel(self, goal ): pass def move(self, dist): '''Moves using a distance. Adds an updateMoveLoc task to the taskMgr. ''' heading = self._nodePath.getH() * math.pi/180 pos = self.getPos() self._stop = False #Of the form (goalLoc, startLoc, distance) self._moveLoc = (Point3(pos[0] + math.sin(heading) * dist, pos[1] - math.cos(heading) * dist, pos[2]), pos, dist) self._tankWorld.taskMgr.add(self.updateMoveLoc,'userTank '+self.getName(),uponDeath=self.nextTask) def rotate(self, angle): '''Rotate function. All angles given between 0 and 360 Angle changes are between -180 and 180 ''' angle = angle % 360 if angle > 180: angle -= 360 heading = self.fixAngle(self._nodePath.getH()) newH = self.fixAngle(heading + angle) self._moveLoc = (newH, heading, angle) self._stop = False self._tankWorld.taskMgr.add(self.updateRotateLoc, 'userTank '+self.getName(), uponDeath=self.nextTask) def moveTime(self, moveTime): self._taskTimer = moveTime self._tankWorld.taskMgr.add(self.updateMove,'userTank '+self.getName(),uponDeath=self.nextTask) def rotateTime(self, rotateTime): self._taskTimer = rotateTime self._tankWorld.taskMgr.add(self.updateRotate,'userTank '+self.getName(),uponDeath=self.nextTask) def wait(self, waitTime): self._taskTimer = waitTime self._tankWorld.taskMgr.add(self.updateWait,'userTank '+self.getName(),uponDeath=self.nextTask) def updateWait(self, task): ''' Tasks called to wait ''' try: if self._tankWorld.isRealTime(): dt = globalClock.getDt() else: dt = 1.0/60.0 #print "Tank.updateWait: ", dt self._taskTimer -= dt if self._taskTimer < 0: return task.done return task.cont except: print "error, tank.wait" def updateRotateLoc(self, task): heading = self.fixAngle(self._nodePath.getH()) toHeading = self._moveLoc[0] w = self._nodePath.node().getAngularVelocity()[2] #Right wheel direction for rotating in the direction of goal rFor = self._moveLoc[2]/abs(self._moveLoc[2]) slowTheta = 1.5 * rFor brakePercent = w**2 / (2 * slowTheta * self._maxThrusterAccel) theta = self.fixAngle(toHeading - heading) if theta > 180: theta -= 360 thetaFromStart = heading - self._moveLoc[1] thetaFromStart = thetaFromStart % 360 if thetaFromStart > 180: thetaFromStart -= 360 if self._stop: if abs(w) < .1: self._nodePath.node().setAngularVelocity(Vec3(0,0,0)) self._nodePath.setH(self._moveLoc[0]) return task.done self.applyBrakes() return task.cont if abs(theta) < slowTheta - .25: self.applyThrusters(-rFor * brakePercent, rFor * brakePercent) elif abs(theta) > slowTheta - .25: if w < self._maxRotVel: self.applyThrusters(rFor, -1 * rFor) else: self.applyThrusters(0,0) else: self.applyBrakes(brakePercent) if abs(theta) < .1: self._stop = True if abs(thetaFromStart + .1) > abs(self._moveLoc[2]): self._stop = True #emergency stop based on a long time. if task.time > 3: #print "Tank.updateRotateLoc", "your rotate could not be completed, sorry" return task.done return task.cont def updateMoveLoc(self, task): '''Bases how much to slow down on a = v^2/2x. x is slowDist, chosen to play nice. Generally accurate to 5cm, rarely 20cm or worse Stops at an arbitrary low velocity because it will never exit at a lower minimum ''' pos = self.getPos() toLoc = self._moveLoc[0] distance = math.sqrt((pos[0] - toLoc[0])**2 + (pos[1] - toLoc[1])**2) v = self._nodePath.node().getLinearVelocity().length() slowAccel = 2 slowDist = v**2 / (2 * slowAccel * self._breakForce) brakePercent = slowAccel * self._nodePath.node().getMass() / self._breakForce deltaPos = (self.getPos() - self._moveLoc[1]) distFromStart = deltaPos.length() if self._stop: if v < .4: #self._nodePath.node().setLinearVelocity(Vec3(0,0,0)) #self._nodePath.setPos(self._moveLoc[0]) return task.done self.applyBrakes() return task.cont if distance < slowDist: self.applyBrakes(brakePercent * 1.1) elif distance > slowDist: if self._moveLoc[2] > 0: self.applyThrusters(1,1) else: self.applyThrusters(-1, -1) else: self.applyBrakes(brakePercent) if distance < .01: self._stop = True if abs(distFromStart) > abs(self._moveLoc[2]): self._stop = True return task.cont def nextTask(self,task): self._nodePath.node().setActive(True) self.onTask += 1 if(self._tankWorld.isDead or self._tankWorld.isOver): return #if self.onTask >= len(self.taskList): # return def getNumUserTask(): taskAmount = 0 for t in self._tankWorld.taskMgr.getTasks(): if t.getName() == 'userTank '+self.getName(): taskAmount +=1 return taskAmount pre = getNumUserTask() try: self.taskList.next() if(getNumUserTask() == pre): self.nextTask(task) except StopIteration: #print 'Tank.nextTask error' pass #self.taskList[self.onTask][0](self.taskList[self.onTask][1]) def runTasks(self): self.onTask = 0 self.nextTask(None) def setGenerator(self, gen): self.taskList = gen self.runTasks() def updateMove(self, task): ''' Task Called to do movement. This is called once perframe ''' try: #small hack to prevent the first frame from doing all the tasks. dt = globalClock.getDt() if dt > .1: return task.cont self._taskTimer -= dt if self._taskTimer < 0: self.applyBrakes(1) else: self.applyThrusters(1,1) if self._taskTimer < -1: #one second to stop return task.done except: print "ERROR in tank.updateMove" return task.cont def updateRotate(self, task): ''' called to do rotation. This is called once per frame ''' try: dt = globalClock.getDt() #small hack to prevent the first frame from doing all the tasks. if dt > .1: return task.cont self._taskTimer -= dt if self._taskTimer < 0: self.applyBrakes(1) else: self.applyThrusters(1,-1) if self._taskTimer < -1: #one second to stop return task.done except: print "ERROR in tank.updateRotate" return task.cont def update(self, task): pass def aimAt(self, point, amt = 1, aimLow = True): return self._weapon.aimAt(point, aimLow) def setWeapon(self, weopwn): self._weapon = weopwn def fire(self, amt = 1): x = self._weapon.fire(amt) self.wait(.1) return x def deleteAfter(self, task = None): if not self._nodePath.is_empty(): x = self._nodePath.node() self._tankWorld.removeRigidBody(x) self.hide() self._tankWorld.lose() #self._nodePath.detachNode() def handleCollision(self, collide, taskName): self._collisionCounter += 1 forReal = True hitBy = collide.getNode0() if hitBy.getName() == self._nodePath.node().getName(): hitBy = collide.getNode1() allowedNames = ['floor', 'wall', 'State'] for name in allowedNames: if name in hitBy.getName(): forReal = False if not self._nodePath.is_empty(): if forReal: print "Tank.handleCollision:(pos) ", self.getPos(), 'obj 1', collide.getNode0().getName(), 'obj 2', collide.getNode1().getName() self._tankWorld.taskMgr.remove(taskName) self._tankWorld.doMethodLater(.01, self.deleteAfter, 'deleteAfter') else: print "Tank.handleCollision Failed to have _nodepath" self._tankWorld.taskMgr.remove(taskName)
class BaseVehicle(DynamicElement): MODEL = None """ Vehicle chassis and its wheels index 0 1 II-----II | | <---chassis | II-----II 2 3 """ PARAMETER_SPACE = PGSpace( VehicleParameterSpace.BASE_VEHICLE ) # it will not sample config from parameter space COLLISION_MASK = CollisionGroup.EgoVehicle STEERING_INCREMENT = 0.05 LENGTH = None WIDTH = None def __init__( self, pg_world: PGWorld, vehicle_config: Union[dict, PGConfig] = None, physics_config: dict = None, random_seed: int = 0, name: str = None, ): """ This Vehicle Config is different from self.get_config(), and it is used to define which modules to use, and module parameters. And self.physics_config defines the physics feature of vehicles, such as length/width :param pg_world: PGWorld :param vehicle_config: mostly, vehicle module config :param physics_config: vehicle height/width/length, find more physics para in VehicleParameterSpace :param random_seed: int """ self.vehicle_config = PGConfig(vehicle_config) # self.vehicle_config = self.get_vehicle_config(vehicle_config) \ # if vehicle_config is not None else self._default_vehicle_config() # observation, action self.action_space = self.get_action_space_before_init( extra_action_dim=self.vehicle_config["extra_action_dim"]) super(BaseVehicle, self).__init__(random_seed, name=name) # config info self.set_config(self.PARAMETER_SPACE.sample()) if physics_config is not None: self.set_config(physics_config) self.increment_steering = self.vehicle_config["increment_steering"] self.enable_reverse = self.vehicle_config["enable_reverse"] self.max_speed = self.vehicle_config["max_speed"] self.max_steering = self.vehicle_config["max_steering"] self.pg_world = pg_world self.node_path = NodePath("vehicle") # create self.spawn_place = (0, 0) self._add_chassis(pg_world.physics_world) self.wheels = self._create_wheel() # modules self.image_sensors = {} self.lidar: Optional[Lidar] = None self.side_detector: Optional[SideDetector] = None self.lane_line_detector: Optional[LaneLineDetector] = None self.routing_localization: Optional[RoutingLocalizationModule] = None self.lane: Optional[AbstractLane] = None self.lane_index = None self.vehicle_panel = VehiclePanel(self.pg_world) if ( self.pg_world.mode == RENDER_MODE_ONSCREEN) else None # state info self.throttle_brake = 0.0 self.steering = 0 self.last_current_action = deque([(0.0, 0.0), (0.0, 0.0)], maxlen=2) self.last_position = self.spawn_place self.last_heading_dir = self.heading self.dist_to_left_side = None self.dist_to_right_side = None # collision info render self.collision_info_np = self._init_collision_info_render(pg_world) self.collision_banners = {} # to save time self.current_banner = None self.attach_to_pg_world(self.pg_world.pbr_render, self.pg_world.physics_world) # step info self.out_of_route = None self.on_lane = None # self.step_info = None self._init_step_info() # others self._add_modules_for_vehicle(self.vehicle_config["use_render"]) self.takeover = False self._expert_takeover = False self.energy_consumption = 0 # overtake_stat self.front_vehicles = set() self.back_vehicles = set() def _add_modules_for_vehicle(self, use_render: bool): # add self module for training according to config vehicle_config = self.vehicle_config self.add_routing_localization( vehicle_config["show_navi_mark"]) # default added if self.vehicle_config["side_detector"]["num_lasers"] > 0: self.side_detector = SideDetector( self.pg_world.render, self.vehicle_config["side_detector"]["num_lasers"], self.vehicle_config["side_detector"]["distance"], self.vehicle_config["show_side_detector"]) if self.vehicle_config["lane_line_detector"]["num_lasers"] > 0: self.lane_line_detector = LaneLineDetector( self.pg_world.render, self.vehicle_config["lane_line_detector"]["num_lasers"], self.vehicle_config["lane_line_detector"]["distance"], self.vehicle_config["show_lane_line_detector"]) if not self.vehicle_config["use_image"]: if vehicle_config["lidar"]["num_lasers"] > 0 and vehicle_config[ "lidar"]["distance"] > 0: self.add_lidar( num_lasers=vehicle_config["lidar"]["num_lasers"], distance=vehicle_config["lidar"]["distance"], show_lidar_point=vehicle_config["show_lidar"]) else: import logging logging.warning( "You have set the lidar config to: {}, which seems to be invalid!" .format(vehicle_config["lidar"])) if use_render: rgb_cam_config = vehicle_config["rgb_cam"] rgb_cam = RGBCamera(rgb_cam_config[0], rgb_cam_config[1], self.chassis_np, self.pg_world) self.add_image_sensor("rgb_cam", rgb_cam) mini_map = MiniMap(vehicle_config["mini_map"], self.chassis_np, self.pg_world) self.add_image_sensor("mini_map", mini_map) return if vehicle_config["use_image"]: # 3 types image observation if vehicle_config["image_source"] == "rgb_cam": rgb_cam_config = vehicle_config["rgb_cam"] rgb_cam = RGBCamera(rgb_cam_config[0], rgb_cam_config[1], self.chassis_np, self.pg_world) self.add_image_sensor("rgb_cam", rgb_cam) elif vehicle_config["image_source"] == "mini_map": mini_map = MiniMap(vehicle_config["mini_map"], self.chassis_np, self.pg_world) self.add_image_sensor("mini_map", mini_map) elif vehicle_config["image_source"] == "depth_cam": cam_config = vehicle_config["depth_cam"] depth_cam = DepthCamera(*cam_config, self.chassis_np, self.pg_world) self.add_image_sensor("depth_cam", depth_cam) else: raise ValueError("No module named {}".format( vehicle_config["image_source"])) # load more sensors for visualization when render, only for beauty... if use_render: if vehicle_config["image_source"] == "mini_map": rgb_cam_config = vehicle_config["rgb_cam"] rgb_cam = RGBCamera(rgb_cam_config[0], rgb_cam_config[1], self.chassis_np, self.pg_world) self.add_image_sensor("rgb_cam", rgb_cam) else: mini_map = MiniMap(vehicle_config["mini_map"], self.chassis_np, self.pg_world) self.add_image_sensor("mini_map", mini_map) def _init_step_info(self): # done info will be initialized every frame self.chassis_np.node().getPythonTag( BodyName.Base_vehicle).init_collision_info() self.out_of_route = False # re-route is required if is false self.on_lane = True # on lane surface or not # self.step_info = {"reward": 0, "cost": 0} def _preprocess_action(self, action): if self.vehicle_config["action_check"]: assert self.action_space.contains( action ), "Input {} is not compatible with action space {}!".format( action, self.action_space) # protect agent from nan error action = safe_clip_for_small_array(action, min_val=self.action_space.low[0], max_val=self.action_space.high[0]) return action, {'raw_action': (action[0], action[1])} def prepare_step(self, action): """ Save info and make decision before action """ # init step info to store info before each step self._init_step_info() action, step_info = self._preprocess_action(action) self.last_position = self.position self.last_heading_dir = self.heading self.last_current_action.append( action ) # the real step of physics world is implemented in taskMgr.step() if self.increment_steering: self.set_incremental_action(action) else: self.set_act(action) if self.vehicle_panel is not None: self.vehicle_panel.renew_2d_car_para_visualization(self) return step_info def update_state(self, pg_world=None, detector_mask="WRONG"): # lidar if self.lidar is not None: self.lidar.perceive(self.position, self.heading_theta, self.pg_world.physics_world.dynamic_world, extra_filter_node={self.chassis_np.node()}, detector_mask=detector_mask) if self.routing_localization is not None: self.lane, self.lane_index, = self.routing_localization.update_navigation_localization( self) if self.side_detector is not None: self.side_detector.perceive( self.position, self.heading_theta, self.pg_world.physics_world.static_world) if self.lane_line_detector is not None: self.lane_line_detector.perceive( self.position, self.heading_theta, self.pg_world.physics_world.static_world) self._state_check() self.update_dist_to_left_right() step_energy, episode_energy = self._update_energy_consumption() self.out_of_route = self._out_of_route() step_info = self._update_overtake_stat() step_info.update({ "velocity": float(self.speed), "steering": float(self.steering), "acceleration": float(self.throttle_brake), "step_energy": step_energy, "episode_energy": episode_energy }) return step_info def _out_of_route(self): left, right = self._dist_to_route_left_right() return True if right < 0 or left < 0 else False def _update_energy_consumption(self): """ The calculation method is from https://www.researchgate.net/publication/262182035_Reduction_of_Fuel_Consumption_and_Exhaust_Pollutant_Using_Intelligent_Transport_System default: 3rd gear, try to use ae^bx to fit it, dp: (90, 8), (130, 12) :return: None """ distance = norm(*(self.last_position - self.position)) / 1000 # km step_energy = 3.25 * math.pow(np.e, 0.01 * self.speed) * distance / 100 # step_energy is in Liter, we return mL step_energy = step_energy * 1000 self.energy_consumption += step_energy # L/100 km return step_energy, self.energy_consumption def reset(self, map: Map, pos: np.ndarray = None, heading: float = 0.0): """ pos is a 2-d array, and heading is a float (unit degree) if pos is not None, vehicle will be reset to the position else, vehicle will be reset to spawn place """ if pos is None: lane = map.road_network.get_lane( self.vehicle_config["spawn_lane_index"]) pos = lane.position(self.vehicle_config["spawn_longitude"], self.vehicle_config["spawn_lateral"]) heading = np.rad2deg( lane.heading_at(self.vehicle_config["spawn_longitude"])) self.spawn_place = pos heading = -np.deg2rad(heading) - np.pi / 2 self.set_static(False) self.chassis_np.setPos(panda_position(Vec3(*pos, 1))) self.chassis_np.setQuat( LQuaternionf(math.cos(heading / 2), 0, 0, math.sin(heading / 2))) self.update_map_info(map) self.chassis_np.node().clearForces() self.chassis_np.node().setLinearVelocity(Vec3(0, 0, 0)) self.chassis_np.node().setAngularVelocity(Vec3(0, 0, 0)) self.system.resetSuspension() # np.testing.assert_almost_equal(self.position, pos, decimal=4) # done info self._init_step_info() # other info self.throttle_brake = 0.0 self.steering = 0 self.last_current_action = deque([(0.0, 0.0), (0.0, 0.0)], maxlen=2) self.last_position = self.spawn_place self.last_heading_dir = self.heading self.update_dist_to_left_right() self.takeover = False self.energy_consumption = 0 # overtake_stat self.front_vehicles = set() self.back_vehicles = set() # for render if self.vehicle_panel is not None: self.vehicle_panel.renew_2d_car_para_visualization(self) if "depth_cam" in self.image_sensors and self.image_sensors[ "depth_cam"].view_ground: for block in map.blocks: block.node_path.hide(CamMask.DepthCam) assert self.routing_localization # Please note that if you respawn agent to some new place and might have a new destination, # you should reset the routing localization too! Via: vehicle.routing_localization.set_route or # vehicle.update """------------------------------------------- act -------------------------------------------------""" def set_act(self, action): steering = action[0] self.throttle_brake = action[1] self.steering = steering self.system.setSteeringValue(self.steering * self.max_steering, 0) self.system.setSteeringValue(self.steering * self.max_steering, 1) self._apply_throttle_brake(action[1]) def set_incremental_action(self, action: np.ndarray): self.throttle_brake = action[1] self.steering += action[0] * self.STEERING_INCREMENT self.steering = clip(self.steering, -1, 1) steering = self.steering * self.max_steering self.system.setSteeringValue(steering, 0) self.system.setSteeringValue(steering, 1) self._apply_throttle_brake(action[1]) def _apply_throttle_brake(self, throttle_brake): max_engine_force = self.vehicle_config["max_engine_force"] max_brake_force = self.vehicle_config["max_brake_force"] for wheel_index in range(4): if throttle_brake >= 0: self.system.setBrake(2.0, wheel_index) if self.speed > self.max_speed: self.system.applyEngineForce(0.0, wheel_index) else: self.system.applyEngineForce( max_engine_force * throttle_brake, wheel_index) else: if self.enable_reverse: self.system.applyEngineForce( max_engine_force * throttle_brake, wheel_index) self.system.setBrake(0, wheel_index) else: self.system.applyEngineForce(0.0, wheel_index) self.system.setBrake( abs(throttle_brake) * max_brake_force, wheel_index) """---------------------------------------- vehicle info ----------------------------------------------""" def update_dist_to_left_right(self): self.dist_to_left_side, self.dist_to_right_side = self._dist_to_route_left_right( ) def _dist_to_route_left_right(self): current_reference_lane = self.routing_localization.current_ref_lanes[0] _, lateral_to_reference = current_reference_lane.local_coordinates( self.position) lateral_to_left = lateral_to_reference + self.routing_localization.get_current_lane_width( ) / 2 lateral_to_right = self.routing_localization.get_current_lateral_range( self.position, self.pg_world) - lateral_to_left return lateral_to_left, lateral_to_right @property def position(self): return pgdrive_position(self.chassis_np.getPos()) @property def speed(self): """ km/h """ velocity = self.chassis_np.node().get_linear_velocity() speed = norm(velocity[0], velocity[1]) * 3.6 return clip(speed, 0.0, 100000.0) @property def heading(self): real_heading = self.heading_theta # heading = np.array([math.cos(real_heading), math.sin(real_heading)]) heading = PGVector((math.cos(real_heading), math.sin(real_heading))) return heading @property def heading_theta(self): """ Get the heading theta of vehicle, unit [rad] :return: heading in rad """ return (pgdrive_heading(self.chassis_np.getH()) - 90) / 180 * math.pi @property def velocity(self) -> np.ndarray: return self.speed * self.velocity_direction @property def velocity_direction(self): direction = self.system.getForwardVector() return np.asarray([direction[0], -direction[1]]) @property def current_road(self): return Road(*self.lane_index[0:-1]) """---------------------------------------- some math tool ----------------------------------------------""" def heading_diff(self, target_lane): lateral = None if isinstance(target_lane, StraightLane): lateral = np.asarray( get_vertical_vector(target_lane.end - target_lane.start)[1]) elif isinstance(target_lane, CircularLane): if target_lane.direction == -1: lateral = self.position - target_lane.center else: lateral = target_lane.center - self.position else: raise ValueError("Unknown target lane type: {}".format( type(target_lane))) lateral_norm = norm(lateral[0], lateral[1]) forward_direction = self.heading # print(f"Old forward direction: {self.forward_direction}, new heading {self.heading}") forward_direction_norm = norm(forward_direction[0], forward_direction[1]) if not lateral_norm * forward_direction_norm: return 0 cos = ((forward_direction[0] * lateral[0] + forward_direction[1] * lateral[1]) / (lateral_norm * forward_direction_norm)) # return cos # Normalize to 0, 1 return clip(cos, -1.0, 1.0) / 2 + 0.5 def projection(self, vector): # Projected to the heading of vehicle # forward_vector = self.vehicle.get_forward_vector() # forward_old = (forward_vector[0], -forward_vector[1]) forward = self.heading # print(f"[projection] Old forward {forward_old}, new heading {forward}") norm_velocity = norm(forward[0], forward[1]) + 1e-6 project_on_heading = (vector[0] * forward[0] + vector[1] * forward[1]) / norm_velocity side_direction = get_vertical_vector(forward)[1] side_norm = norm(side_direction[0], side_direction[1]) + 1e-6 project_on_side = (vector[0] * side_direction[0] + vector[1] * side_direction[1]) / side_norm return project_on_heading, project_on_side def lane_distance_to(self, vehicle, lane: AbstractLane = None) -> float: assert self.routing_localization is not None, "a routing and localization module should be added " \ "to interact with other vehicles" if not vehicle: return np.nan if not lane: lane = self.lane return lane.local_coordinates( vehicle.position)[0] - lane.local_coordinates(self.position)[0] """-------------------------------------- for vehicle making ------------------------------------------""" def _add_chassis(self, pg_physics_world: PGPhysicsWorld): para = self.get_config() self.LENGTH = self.vehicle_config["vehicle_length"] self.WIDTH = self.vehicle_config["vehicle_width"] chassis = BaseVehicleNode(BodyName.Base_vehicle, self) chassis.setIntoCollideMask(BitMask32.bit(CollisionGroup.EgoVehicle)) chassis_shape = BulletBoxShape( Vec3(self.WIDTH / 2, self.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 spawn now self.chassis_np.setPos(Vec3(*self.spawn_place, 1)) self.chassis_np.setQuat( LQuaternionf(math.cos(heading / 2), 0, 0, math.sin(heading / 2))) chassis.setDeactivationEnabled(False) chassis.notifyCollisions( True ) # advance collision check, do callback in pg_collision_callback self.dynamic_nodes.append(chassis) chassis_beneath = BulletGhostNode(BodyName.Base_vehicle_beneath) chassis_beneath.setIntoCollideMask( BitMask32.bit(CollisionGroup.EgoVehicleBeneath)) 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 if self.render: if self.MODEL is None: model_path = 'models/ferra/scene.gltf' self.MODEL = self.loader.loadModel( AssetLoader.file_path(model_path)) self.MODEL.setZ(para[Parameter.vehicle_vis_z]) self.MODEL.setY(para[Parameter.vehicle_vis_y]) self.MODEL.setH(para[Parameter.vehicle_vis_h]) self.MODEL.set_scale(para[Parameter.vehicle_vis_scale]) self.MODEL.instanceTo(self.chassis_np) def _create_wheel(self): para = self.get_config() f_l = para[Parameter.front_tire_longitude] r_l = -para[Parameter.rear_tire_longitude] lateral = para[Parameter.tire_lateral] axis_height = para[Parameter.tire_radius] + 0.05 radius = para[Parameter.tire_radius] wheels = [] for k, pos in enumerate([ Vec3(lateral, f_l, axis_height), Vec3(-lateral, f_l, axis_height), Vec3(lateral, r_l, axis_height), Vec3(-lateral, r_l, axis_height) ]): wheel = self._add_wheel(pos, radius, True if k < 2 else False, True if k == 0 or k == 2 else False) wheels.append(wheel) return wheels def _add_wheel(self, pos: Vec3, radius: float, front: bool, left): wheel_np = self.node_path.attachNewNode("wheel") if self.render: # TODO something wrong with the wheel render model_path = 'models/yugo/yugotireR.egg' if left else 'models/yugo/yugotireL.egg' wheel_model = self.loader.loadModel( AssetLoader.file_path(model_path)) wheel_model.reparentTo(wheel_np) wheel_model.set_scale(1.4, radius / 0.25, radius / 0.25) wheel = self.system.create_wheel() wheel.setNode(wheel_np.node()) wheel.setChassisConnectionPointCs(pos) wheel.setFrontWheel(front) wheel.setWheelDirectionCs(Vec3(0, 0, -1)) wheel.setWheelAxleCs(Vec3(1, 0, 0)) # TODO add them to PGConfig in the future wheel.setWheelRadius(radius) wheel.setMaxSuspensionTravelCm(40) wheel.setSuspensionStiffness(30) wheel.setWheelsDampingRelaxation(4.8) wheel.setWheelsDampingCompression(1.2) wheel.setFrictionSlip(self.vehicle_config["wheel_friction"]) wheel.setRollInfluence(1.5) return wheel def add_image_sensor(self, name: str, sensor: ImageBuffer): self.image_sensors[name] = sensor def add_lidar(self, num_lasers=240, distance=50, show_lidar_point=False): assert num_lasers > 0 assert distance > 0 self.lidar = Lidar(self.pg_world.render, num_lasers, distance, show_lidar_point) def add_routing_localization(self, show_navi_mark: bool = False): config = self.vehicle_config self.routing_localization = RoutingLocalizationModule( self.pg_world, show_navi_mark=show_navi_mark, random_navi_mark_color=config["random_navi_mark_color"], show_dest_mark=config["show_dest_mark"], show_line_to_dest=config["show_line_to_dest"]) def update_map_info(self, map): """ Update map info after reset() :param map: new map :return: None """ lane, new_l_index = ray_localization(np.array(self.heading.tolist()), np.array(self.spawn_place), self.pg_world) self.routing_localization.update(map, current_lane_index=new_l_index) assert lane is not None, "spawn place is not on road!" self.lane_index = new_l_index self.lane = lane def _state_check(self): """ Check States and filter to update info """ result_1 = self.pg_world.physics_world.static_world.contactTest( self.chassis_beneath_np.node(), True) result_2 = self.pg_world.physics_world.dynamic_world.contactTest( self.chassis_beneath_np.node(), True) contacts = set() for contact in result_1.getContacts() + result_2.getContacts(): node0 = contact.getNode0() node1 = contact.getNode1() name = [node0.getName(), node1.getName()] name.remove(BodyName.Base_vehicle_beneath) if name[0] == "Ground" or name[0] == BodyName.Lane: continue if name[0] == BodyName.Sidewalk: self.chassis_np.node().getPythonTag( BodyName.Base_vehicle).crash_sidewalk = True elif name[0] == BodyName.White_continuous_line: self.chassis_np.node().getPythonTag( BodyName.Base_vehicle).on_white_continuous_line = True elif name[0] == BodyName.Yellow_continuous_line: self.chassis_np.node().getPythonTag( BodyName.Base_vehicle).on_yellow_continuous_line = True elif name[0] == BodyName.Broken_line: self.chassis_np.node().getPythonTag( BodyName.Base_vehicle).on_broken_line = True contacts.add(name[0]) if self.render: self.render_collision_info(contacts) @staticmethod def _init_collision_info_render(pg_world): if pg_world.mode == "onscreen": info_np = NodePath("Collision info nodepath") info_np.reparentTo(pg_world.aspect2d) else: info_np = None return info_np def render_collision_info(self, contacts): contacts = sorted(list(contacts), key=lambda c: COLLISION_INFO_COLOR[COLOR[c]][0]) text = contacts[0] if len(contacts) != 0 else None if text is None: text = "Normal" if time.time( ) - self.pg_world._episode_start_time > 10 else "Press H to see help message" self.render_banner(text, COLLISION_INFO_COLOR["green"][1]) else: if text == BodyName.Base_vehicle_beneath: text = BodyName.Traffic_vehicle self.render_banner(text, COLLISION_INFO_COLOR[COLOR[text]][1]) def render_banner(self, text, color=COLLISION_INFO_COLOR["green"][1]): """ Render the banner in the left bottom corner. """ if self.collision_info_np is None: return if self.current_banner is not None: self.current_banner.detachNode() if text in self.collision_banners: self.collision_banners[text].reparentTo(self.collision_info_np) self.current_banner = self.collision_banners[text] else: new_banner = NodePath(TextNode("collision_info:{}".format(text))) self.collision_banners[text] = new_banner text_node = new_banner.node() text_node.setCardColor(color) text_node.setText(text) text_node.setCardActual(-5 * self.pg_world.w_scale, 5.1 * self.pg_world.w_scale, -0.3, 1) text_node.setCardDecal(True) text_node.setTextColor(1, 1, 1, 1) text_node.setAlign(TextNode.A_center) new_banner.setScale(0.05) new_banner.setPos(-0.75 * self.pg_world.w_scale, 0, -0.8 * self.pg_world.h_scale) new_banner.reparentTo(self.collision_info_np) self.current_banner = new_banner def destroy(self, _=None): self.chassis_np.node().getPythonTag(BodyName.Base_vehicle).destroy() if self.chassis_np.node() in self.dynamic_nodes: self.dynamic_nodes.remove(self.chassis_np.node()) super(BaseVehicle, self).destroy(self.pg_world) self.pg_world.physics_world.dynamic_world.clearContactAddedCallback() self.routing_localization.destroy() self.routing_localization = None if self.side_detector is not None: self.side_detector.destroy() if self.lane_line_detector is not None: self.lane_line_detector.destroy() self.side_detector = None self.lane_line_detector = None if self.lidar is not None: self.lidar.destroy() self.lidar = None if len(self.image_sensors) != 0: for sensor in self.image_sensors.values(): sensor.destroy(self.pg_world) self.image_sensors = None if self.vehicle_panel is not None: self.vehicle_panel.destroy(self.pg_world) self.pg_world = None def set_position(self, position, height=0.4): """ Should only be called when restore traffic from episode data :param position: 2d array or list :return: None """ self.chassis_np.setPos(panda_position(position, height)) def set_heading(self, heading_theta) -> None: """ Should only be called when restore traffic from episode data :param heading_theta: float in rad :return: None """ self.chassis_np.setH((panda_heading(heading_theta) * 180 / np.pi) - 90) def get_state(self): return { "heading": self.heading_theta, "position": self.position.tolist(), "done": self.crash_vehicle or self.out_of_route or self.crash_sidewalk or not self.on_lane } def set_state(self, state: dict): self.set_heading(state["heading"]) self.set_position(state["position"]) def _update_overtake_stat(self): if self.vehicle_config["overtake_stat"]: surrounding_vs = self.lidar.get_surrounding_vehicles() routing = self.routing_localization ckpt_idx = routing._target_checkpoints_index for surrounding_v in surrounding_vs: if surrounding_v.lane_index[:-1] == ( routing.checkpoints[ckpt_idx[0]], routing.checkpoints[ckpt_idx[1]]): if self.lane.local_coordinates(self.position)[0] - \ self.lane.local_coordinates(surrounding_v.position)[0] < 0: self.front_vehicles.add(surrounding_v) if surrounding_v in self.back_vehicles: self.back_vehicles.remove(surrounding_v) else: self.back_vehicles.add(surrounding_v) return {"overtake_vehicle_num": self.get_overtake_num()} def get_overtake_num(self): return len(self.front_vehicles.intersection(self.back_vehicles)) @classmethod def get_action_space_before_init(cls, extra_action_dim: int = 0): return gym.spaces.Box(-1.0, 1.0, shape=(2 + extra_action_dim, ), dtype=np.float32) def remove_display_region(self): if self.render: self.vehicle_panel.remove_display_region(self.pg_world) self.vehicle_panel.buffer.set_active(False) self.collision_info_np.detachNode() self.routing_localization._arrow_node_path.detachNode() for sensor in self.image_sensors.values(): sensor.remove_display_region(self.pg_world) sensor.buffer.set_active(False) def add_to_display(self): if self.render: self.vehicle_panel.add_to_display( self.pg_world, self.vehicle_panel.default_region) self.vehicle_panel.buffer.set_active(True) self.collision_info_np.reparentTo(self.pg_world.aspect2d) self.routing_localization._arrow_node_path.reparentTo( self.pg_world.aspect2d) for sensor in self.image_sensors.values(): sensor.add_to_display(self.pg_world, sensor.default_region) sensor.buffer.set_active(True) def __del__(self): super(BaseVehicle, self).__del__() self.pg_world = None self.lidar = None self.mini_map = None self.rgb_cam = None self.routing_localization = None self.wheels = None @property def arrive_destination(self): long, lat = self.routing_localization.final_lane.local_coordinates( self.position) flag = (self.routing_localization.final_lane.length - 5 < long < self.routing_localization.final_lane.length + 5) and ( self.routing_localization.get_current_lane_width() / 2 >= lat >= (0.5 - self.routing_localization.get_current_lane_num()) * self.routing_localization.get_current_lane_width()) return flag @property def crash_vehicle(self): return self.chassis_np.node().getPythonTag( BodyName.Base_vehicle).crash_vehicle @property def crash_object(self): return self.chassis_np.node().getPythonTag( BodyName.Base_vehicle).crash_object @property def crash_sidewalk(self): return self.chassis_np.node().getPythonTag( BodyName.Base_vehicle).crash_sidewalk @property def on_yellow_continuous_line(self): return self.chassis_np.node().getPythonTag( BodyName.Base_vehicle).on_yellow_continuous_line @property def on_white_continuous_line(self): return self.chassis_np.node().getPythonTag( BodyName.Base_vehicle).on_white_continuous_line @property def on_broken_line(self): return self.chassis_np.node().getPythonTag( BodyName.Base_vehicle).on_broken_line def set_static(self, flag): self.chassis_np.node().setStatic(flag) @property def reference_lanes(self): return self.routing_localization.current_ref_lanes def set_wheel_friction(self, new_friction): raise DeprecationWarning("Bug exists here") for wheel in self.wheels: wheel.setFrictionSlip(new_friction)
def setup(self): self.worldNP = render.attachNewNode('World') # World self.debugNP = self.worldNP.attachNewNode(BulletDebugNode('Debug')) self.debugNP.show() self.world = BulletWorld() self.world.setGravity(Vec3(0, 0, -9.81)) self.world.setDebugNode(self.debugNP.node()) #terrain = GeoMipTerrain("mySimpleTerrain") #terrain.setHeightfield("./models/heightfield_2.png") #terrain.getRoot().reparentTo(self.worldNP)#render) #terrain.generate() # Plane shape = BulletPlaneShape(Vec3(0, 0, 1), 0) np = self.worldNP.attachNewNode(BulletRigidBodyNode('Ground')) np.node().addShape(shape) np.setPos(0, 0, -1) np.setCollideMask(BitMask32.allOn()) self.world.attachRigidBody(np.node()) np = self.worldNP.attachNewNode(BulletRigidBodyNode('Track')) np.node().setMass(5000.0) np.setPos(3, 0, 10) np.setCollideMask(BitMask32.allOn()) #(0x0f)) #self.track = BulletVehicle(self.world, np.node()) #self.track.setCoordinateSystem(ZUp) self.track_np = loader.loadModel('models/race_track.egg') self.track_np.setScale(100) self.track_np.reparentTo(np) self.track_np.setCollideMask(BitMask32.allOn()) self.world.attachRigidBody(np.node()) self.track_np = np #self.track_np.show() # Chassis shape = BulletBoxShape(Vec3(0.6, 1.4, 0.5)) ts = TransformState.makePos(Point3(0, 0, 0.5)) np = self.worldNP.attachNewNode(BulletRigidBodyNode('Vehicle')) np.node().addShape(shape, ts) np.setPos(0, 0, 1) np.node().setMass(800.0) np.node().setDeactivationEnabled(False) self.world.attachRigidBody(np.node()) #np.node().setCcdSweptSphereRadius(1.0) #np.node().setCcdMotionThreshold(1e-7) self.cTrav = CollisionTraverser() # Vehicle self.vehicle = BulletVehicle(self.world, np.node()) self.vehicle.setCoordinateSystem(ZUp) self.yugoNP = loader.loadModel('models/yugo/yugo.egg') self.yugoNP.reparentTo(np) self.colHandler = CollisionHandlerQueue() self.ray_col_np = {} self.ray_col_vec_dict = {} for ray_dir in range(-1, 2): # populate collision rays self.ray = CollisionRay() self.ray.setOrigin(ray_dir, 0.5, 0.5) self.ray.setDirection(ray_dir, 1, 0) self.ray_col = CollisionNode('ray%d' % (ray_dir + 1)) self.ray_col.addSolid(self.ray) self.ray_col.setFromCollideMask( BitMask32.allOn()) #(0x0f))#CollideMask.bit(0) #self.ray_col.setIntoCollideMask(CollideMask.allOff()) self.ray_col_np['ray%d' % (ray_dir + 1)] = self.yugoNP.attachNewNode( self.ray_col) self.cTrav.addCollider(self.ray_col_np['ray%d' % (ray_dir + 1)], self.colHandler) self.ray_col_np['ray%d' % (ray_dir + 1)].show() self.ray_col_vec_dict['ray%d' % (ray_dir + 1)] = [] self.world.attachVehicle(self.vehicle) self.cTrav.showCollisions(render) # FIXME base.camera.reparentTo(self.yugoNP) # Right front wheel np = loader.loadModel('models/yugo/yugotireR.egg') np.reparentTo(self.worldNP) self.addWheel(Point3(0.70, 1.05, 0.3), True, np) # Left front wheel np = loader.loadModel('models/yugo/yugotireL.egg') np.reparentTo(self.worldNP) self.addWheel(Point3(-0.70, 1.05, 0.3), True, np) # Right rear wheel np = loader.loadModel('models/yugo/yugotireR.egg') np.reparentTo(self.worldNP) self.addWheel(Point3(0.70, -1.05, 0.3), False, np) # Left rear wheel np = loader.loadModel('models/yugo/yugotireL.egg') np.reparentTo(self.worldNP) self.addWheel(Point3(-0.70, -1.05, 0.3), False, np) # Steering info self.steering = 0.0 # degree self.steeringClamp = 45.0 # degree self.steeringIncrement = 120.0 # degree per second # Box for i, j in [(0, 8), (-3, 5), (6, -5), (8, 3), (-4, -4)]: shape = BulletBoxShape(Vec3(0.5, 0.5, 0.5)) # https://discourse.panda3d.org/t/wall-collision-help/23606 np = self.worldNP.attachNewNode(BulletRigidBodyNode('Box')) np.node().setMass(1.0) np.node().addShape(shape) np.setPos(i, j, 2) np.setCollideMask(BitMask32.allOn()) #(0x0f)) self.world.attachRigidBody(np.node()) self.boxNP = np #self.colHandler2 = CollisionHandlerQueue() visualNP = loader.loadModel('models/box.egg') visualNP.reparentTo(self.boxNP) #self.cTrav.addCollider(self.boxNP,self.colHandler) """ aNode = CollisionNode("TheRay") self.ray = CollisionRay() self.ray.setOrigin( self.yugoNP.getPos() ) self.ray.setDirection( Vec3(0, 10, 0) ) #self.ray.show() aNodePath = self.yugoNP.attachNewNode( CollisionNode("TheRay") ) aNodePath.node().addSolid(self.ray) aNodePath.show() """ #aNode.addSolid(self.ray) #self.ray = CollisionRay(0,0,0,10,0,0) #self.ray.reparentTo(self.yugoNP) #self.rayColl = CollisionNode('PlayerRay') #self.rayColl.addSolid(self.ray) #self.playerRayNode = self.yugoNP.attachNewNode( self.rayColl ) #self.playerRayNode.show() #base.myTraverser.addCollider (self.playerRayNode, base.floor) #base.floor.addCollider( self.playerRayNode, self.yugoNP) """
def load_vehicle(self, pos=(0.0, -20.0, -0.6), quat=None): # Chassis self._mass = self.params['mass'] #chassis_shape = self.params['chassis_shape'] shape = BulletBoxShape(Vec3(0.6, 1.4, 0.5)) ts = TransformState.makePos(Point3(0, 0, 0.5)) self.vehicle_pointer = self.worldNP.attachNewNode( BulletRigidBodyNode('Vehicle')) self.vehicle_node = self.vehicle_pointer.node() self.vehicle_node.addShape(shape, ts) self.previous_pos = pos self.vehicle_pointer.setPos(pos[0], pos[1], pos[2]) if quat is not None: self.vehicle_pointer.setQuat(quat) self.previous_quat = self.vehicle_pointer.getQuat() self.vehicle_node.setMass(self._mass) self.vehicle_node.setDeactivationEnabled(False) # first_person = self.params['first_person'] self.camera_sensor = Panda3dCameraSensor(base, color=True, depth=True, size=(160, 90)) self.camera_node = self.camera_sensor.cam # if first_person: # self.camera_node.setPos(0.0, 1.0, 1.0) # self.camera_node.lookAt(0.0, 6.0, 0.0) # else: # self.camera_node.setPos(0.0, -10.0, 5.0) # self.camera_node.lookAt(0.0, 5.0, 0.0) self.camera_node.reparentTo(self.vehicle_pointer) self.camera_node.setPos(0.0, 1.0, 1.0) self.camera_node.lookAt(0.0, 6.0, 0.0) self.back_camera_sensor = Panda3dCameraSensor(base, color=True, depth=True, size=(160, 90)) self.back_camera_node = self.back_camera_sensor.cam # if first_person: # self.camera_node.setPos(0.0, 1.0, 1.0) # self.camera_node.lookAt(0.0, 6.0, 0.0) # else: # self.camera_node.setPos(0.0, -10.0, 5.0) # self.camera_node.lookAt(0.0, 5.0, 0.0) self.back_camera_node.reparentTo(self.vehicle_pointer) self.back_camera_node.setPos(0.0, -1.0, 1.0) self.back_camera_node.lookAt(0.0, -6.0, 0.0) self.world.attachRigidBody(self.vehicle_node) self.vehicle_node.setCcdSweptSphereRadius(1.0) self.vehicle_node.setCcdMotionThreshold(1e-7) # Vehicle self.vehicle = BulletVehicle(self.world, self.vehicle_node) self.vehicle.setCoordinateSystem(ZUp) self.world.attachVehicle(self.vehicle) self.yugoNP = loader.loadModel('../models/yugo/yugo.egg') self.yugoNP.reparentTo(self.vehicle_pointer) self._wheels = [] # Right front wheel np = loader.loadModel('../models/yugo/yugotireR.egg') np.reparentTo(self.worldNP) self.addWheel(Point3(0.70, 1.05, 0.3), True, np) # Left front wheel np = loader.loadModel('../models/yugo/yugotireL.egg') np.reparentTo(self.worldNP) self.addWheel(Point3(-0.70, 1.05, 0.3), True, np) # Right rear wheel np = loader.loadModel('../models/yugo/yugotireR.egg') np.reparentTo(self.worldNP) self.addWheel(Point3(0.70, -1.05, 0.3), False, np) # Left rear wheel np = loader.loadModel('../models/yugo/yugotireL.egg') np.reparentTo(self.worldNP) self.addWheel(Point3(-0.70, -1.05, 0.3), False, np)
def setup(self): # Steering info self.steering = 0.0 # degree self.steeringClamp = 30.0 # degree self.steeringIncrement = 80.0 # degree per second self.worldNP = render.attachNewNode('World') # World self.debugNP = self.worldNP.attachNewNode(BulletDebugNode('Debug')) #self.debugNP.show() self.world = BulletWorld() self.world.setGravity(Vec3(0, 0, -9.81)) self.world.setDebugNode(self.debugNP.node()) if(self.gameLevel == 1): #set score print('GameLevel') self.score = 1000 self.distanceTravelled = 0 self.time = 0 # Plane img = PNMImage(Filename('media/terrain/SIMP_Assignment_2_Terrain_1.png')) shape = BulletHeightfieldShape(img, 50.0, ZUp) np = self.worldNP.attachNewNode(BulletRigidBodyNode('Ground')) np.node().addShape(shape) np.setPos(0, 0, 0) np.setCollideMask(BitMask32.allOn()) self.world.attachRigidBody(np.node()) #skybox skybox = loader.loadModel('media/models/skybox/skybox_01.X') skybox.reparentTo(render) # Chassis shape = BulletBoxShape(Vec3(0.6, 1.4, 0.5)) ts = TransformState.makePos(Point3(0, 0, 1.0)) self.vehicleNP = self.worldNP.attachNewNode(BulletRigidBodyNode('Vehicle')) self.vehicleNP.node().addShape(shape, ts) self.vehicleNP.setPos(-93, -88, -7)#-93, -88, -7) #(-82,65.8,-8) #(55,8.38,-6)#(45, -19, -8)#(-93, -88, -7) self.vehicleNP.setHpr(-90,0,0) self.vehicleNP.node().setMass(5.0) self.vehicleNP.node().setDeactivationEnabled(False) base.cam.setPos(self.vehicleNP.getPos().getX()+2,self.vehicleNP.getPos().getY()+2,self.vehicleNP.getPos().getZ()+2) self.world.attachRigidBody(self.vehicleNP.node()) # Vehicle self.vehicle = BulletVehicle(self.world, self.vehicleNP.node()) self.vehicle.setCoordinateSystem(ZUp) self.world.attachVehicle(self.vehicle) self.hummerNP = loader.loadModel('media/models/vehicle/body.X') self.hummerNP.reparentTo(self.vehicleNP) # Right front wheel np = loader.loadModel('media/models/vehicle/front_right.X') np.reparentTo(self.worldNP) self.addWheel(Point3( 0.8, 0.9, 0.8), True, np) # Left front wheel np = loader.loadModel('media/models/vehicle/front_left.X') np.reparentTo(self.worldNP) self.addWheel(Point3(-0.8, 0.9, 0.8), True, np) # Right rear wheel np = loader.loadModel('media/models/vehicle/back_right.X') np.reparentTo(self.worldNP) self.addWheel(Point3( 0.8, -0.7, 0.8), False, np) # Left rear wheel np = loader.loadModel('media/models/vehicle/back_left.X') np.reparentTo(self.worldNP) self.addWheel(Point3(-0.8, -0.7, 0.8), False, np) #Obstacles self.setupObstacleOne(Vec3(50, -5, -4), 1.8, Vec3(60, 0, 0)) self.setupObstacleFour(Vec3(63.3, 59.2, -10), 1.5, Vec3(0,0,0)) self.setupObstacleFour(Vec3(41, 57, -10), 1.5, Vec3(0,0,0)) self.setupObstacleFour(Vec3(7.5, 53.8, -10), 1.5, Vec3(0,0,0)) self.setupObstacleFour(Vec3(-28, 81.4, -10), 1.5, Vec3(0,0,0)) self.setupObstacleSix(Vec3(-91, 81 , -6), 1, Vec3(60,0,0)) #Goal self.setupGoal(Vec3(-101,90.6,-6.5)) #self.vehicleNP.setPos(Vec3(6,52,-6)) self.setupTerrain() elif(self.gameLevel == 2): self.distanceTravelled = 0 self.time = 0 # Plane img = PNMImage(Filename('media/terrain/SIMP_Assignment_2_Terrain_2.png')) shape = BulletHeightfieldShape(img, 50.0, ZUp) np = self.worldNP.attachNewNode(BulletRigidBodyNode('Ground')) np.node().addShape(shape) np.setPos(0, 0, 0) np.setCollideMask(BitMask32.allOn()) self.world.attachRigidBody(np.node()) #skybox skybox = loader.loadModel('media/models/skybox/skybox_01.X') skybox.reparentTo(render) # Chassis shape = BulletBoxShape(Vec3(0.6, 1.4, 0.5)) ts = TransformState.makePos(Point3(0, 0, 1.0)) self.vehicleNP = self.worldNP.attachNewNode(BulletRigidBodyNode('Vehicle')) self.vehicleNP.node().addShape(shape, ts) self.vehicleNP.setPos(-99.6,105,-11.8)#(88, 21, -11)#(34.3,8.4,-11.8)#(-99.6,105,-11.8)#(86.4,41.2,-12) self.vehicleNP.setHpr(-130,0,0) self.vehicleNP.node().setMass(5.0) self.vehicleNP.node().setDeactivationEnabled(False) base.cam.setPos(self.vehicleNP.getPos().getX()+2,self.vehicleNP.getPos().getY()+2,self.vehicleNP.getPos().getZ()+2) self.world.attachRigidBody(self.vehicleNP.node()) # Vehicle self.vehicle = BulletVehicle(self.world, self.vehicleNP.node()) self.vehicle.setCoordinateSystem(ZUp) self.world.attachVehicle(self.vehicle) self.hummerNP = loader.loadModel('media/models/vehicle/body.X') self.hummerNP.reparentTo(self.vehicleNP) # Right front wheel np = loader.loadModel('media/models/vehicle/front_right.X') np.reparentTo(self.worldNP) self.addWheel(Point3( 0.8, 0.9, 0.8), True, np) # Left front wheel np = loader.loadModel('media/models/vehicle/front_left.X') np.reparentTo(self.worldNP) self.addWheel(Point3(-0.8, 0.9, 0.8), True, np) # Right rear wheel np = loader.loadModel('media/models/vehicle/back_right.X') np.reparentTo(self.worldNP) self.addWheel(Point3( 0.8, -0.7, 0.8), False, np) # Left rear wheel np = loader.loadModel('media/models/vehicle/back_left.X') np.reparentTo(self.worldNP) self.addWheel(Point3(-0.8, -0.7, 0.8), False, np) self.setupObstacleFive(Vec3(91, 3, -9),1,Vec3(90,0,0)) self.setupObstacleFive(Vec3(94,-19, -10),0.9,Vec3(90,0,0)) self.setupObstacleFive(Vec3(85,-40, -10),1,Vec3(90,0,0)) self.setupObstacleFour(Vec3(-33.5, 23.4,-14.5),1,Vec3(0,0,0)) self.setupObstacleFour(Vec3(-43.3, 24.2,-14.5),1,Vec3(0,0,0)) self.setupObstacleTwo(Vec3(34.7,20.9,-8.5),1,Vec3(90,0,0)) self.setupObstacleTwo(Vec3(26.8,20.3,-8.5),1,Vec3(90,0,0)) self.setupObstacleTwo(Vec3(42.1,22.5,-8.5),1,Vec3(90,0,0)) #self.setupObstacleFive(Vec3(91,0.2, -8),2.1,Vec3(90,0,0)) #Goal self.setupGoal(Vec3(94,-89.7,-10)) self.setupTerrain() elif(self.gameLevel == 3): self.distanceTravelled = 0 self.time = 0 # Plane img = PNMImage(Filename('media/terrain/SIMP_Assignment_2_Terrain_3.png')) shape = BulletHeightfieldShape(img, 50.0, ZUp) np = self.worldNP.attachNewNode(BulletRigidBodyNode('Ground')) np.node().addShape(shape) np.setPos(0, 0, 0) np.setCollideMask(BitMask32.allOn()) self.world.attachRigidBody(np.node()) #skybox skybox = loader.loadModel('media/models/skybox/skybox_01.X') skybox.reparentTo(render) # Chassis shape = BulletBoxShape(Vec3(0.6, 1.4, 0.5)) ts = TransformState.makePos(Point3(0, 0, 1.0)) self.vehicleNP = self.worldNP.attachNewNode(BulletRigidBodyNode('Vehicle')) self.vehicleNP.node().addShape(shape, ts) self.vehicleNP.setPos(-110, -110, 0) self.vehicleNP.setHpr(-40,0,0) self.vehicleNP.node().setMass(5.0) self.vehicleNP.node().setDeactivationEnabled(False) base.cam.setPos(self.vehicleNP.getPos().getX()+2,self.vehicleNP.getPos().getY()+2,self.vehicleNP.getPos().getZ()+2) self.world.attachRigidBody(self.vehicleNP.node()) # Vehicle self.vehicle = BulletVehicle(self.world, self.vehicleNP.node()) self.vehicle.setCoordinateSystem(ZUp) self.world.attachVehicle(self.vehicle) self.hummerNP = loader.loadModel('media/models/vehicle/body.X') self.hummerNP.reparentTo(self.vehicleNP) # Right front wheel np = loader.loadModel('media/models/vehicle/front_right.X') np.reparentTo(self.worldNP) self.addWheel(Point3( 0.8, 0.9, 0.8), True, np) # Left front wheel np = loader.loadModel('media/models/vehicle/front_left.X') np.reparentTo(self.worldNP) self.addWheel(Point3(-0.8, 0.9, 0.8), True, np) # Right rear wheel np = loader.loadModel('media/models/vehicle/back_right.X') np.reparentTo(self.worldNP) self.addWheel(Point3( 0.8, -0.7, 0.8), False, np) # Left rear wheel np = loader.loadModel('media/models/vehicle/back_left.X') np.reparentTo(self.worldNP) self.addWheel(Point3(-0.8, -0.7, 0.8), False, np) self.setupTerrain() #Goal self.setupGoal(Vec3(114,100,-13)) #Obstacles self.setupObstacleFour(Vec3(-60, -73, -9), 1, Vec3(0, 0, 0)) self.setupObstacleFour(Vec3(-63, -77, -9), 1, Vec3(0, 0, 0)) self.setupObstacleTwo(Vec3(-15, -40, -3), 1, Vec3(0, 0, 0)) self.setupObstacleFour(Vec3(-60, 12, -11), 1, Vec3(0, 0, 0)) self.setupObstacleSix(Vec3(-15, 90, -6), 1.5, Vec3(-30, 0, 0)) self.setupObstacleFour(Vec3(28, 87, -11), 1, Vec3(0, 0, 0)) self.setupObstacleFour(Vec3(32, 90, -11), 1, Vec3(0, 0, 0))
class RonnieRacer(DirectObject): gameState = 'INIT' gameLevel = 1 distanceTravelled = 0 speed = 0 score = 0 triesLeft = 3 count = 0 rot = 0 time = 0 pause = False def __init__(self): self.imageObject = OnscreenImage(image = 'media/images/splashscreen.png', pos=(0,0,0), scale=(1.4,1,1)) self.loseScreen = OnscreenImage(image = 'media/images/gameover.png', pos=(0,0,0), scale=(1,1,0.8)) self.loseScreen.hide() self.retryScreen = OnscreenImage(image = 'media/images/retry.png', pos=(0,0,0), scale=(1,1,0.8)) self.retryScreen.hide() self.congratScreen = OnscreenImage(image = 'media/images/congratulations.png', pos=(0,0,0), scale = (1,1,0.8)) self.congratScreen.hide() self.winScreen = OnscreenImage(image = 'media/images/victory.png', pos=(0,0,0), scale = (1,1,0.8)) self.winScreen.hide() self.pauseScreen = OnscreenImage(image = 'media/images/pause.png', pos=(0,0,0), scale = (1,1,0.8)) self.pauseScreen.hide() self.instructionScreen = OnscreenImage(image = 'media/images/instructions.png', pos=(0,0,0), scale = (1,1,0.8)) self.instructionScreen.hide() preloader = Preloader() base.setBackgroundColor(0, 0, 0, 1) base.setFrameRateMeter(True) # Audio self.loseSound = base.loader.loadSfx("media/audio/sfxboo.wav") self.winSound = base.loader.loadSfx("media/audio/cheer2.aif") self.menuMusic = base.loader.loadSfx("media/audio/Scattershot.mp3") self.gameMusic = base.loader.loadSfx("media/audio/Ghostpocalypse - 7 Master.mp3") self.menuMusic.setLoop(True) self.menuMusic.setLoopCount(0) self.gameMusic.setLoop(True) self.gameMusic.setLoopCount(0) #setup buttons self.retryBtn = DirectButton(text="Retry", scale = 0.1, pos = (0,0,0), command = self.doRetry) self.retryBtn.hide() self.menuBtn = DirectButton(text="Main Menu", scale = 0.1, pos = (0,0,0), command = self.doMenu) self.menuBtn.hide() self.nextBtn = DirectButton(text='Next', scale = 0.1, pos = (0,0,0), command = self.doNext) self.nextBtn.hide() self.backBtn = DirectButton(text='back', scale = 0.1, pos = (-0.7,0,-0.7), command = self.doBack) self.backBtn.hide() #setup font self.font = loader.loadFont('media/SHOWG.TTF') self.font.setPixelsPerUnit(60) #setup text self.text = OnscreenText(text = '', pos = (0, 0), scale = 0.07, font = self.font) self.rpmText = OnscreenText(text = '', pos = (-0.9, -0.9), scale = 0.07, font = self.font) self.speedText = OnscreenText(text = '', pos = (0, -0.9), scale = 0.07, font = self.font) self.distanceText = OnscreenText(text = '', pos = (0.9, -0.9), scale = 0.07, font = self.font) self.triesLeftText = OnscreenText(text = '', pos = (1.0, 0.9), scale = 0.07, font = self.font) self.gameLevelText = OnscreenText(text = '', pos = (-1.0, 0.9), scale = 0.07, font = self.font) self.timeText = OnscreenText(text = '', pos = (0, 0.9), scale = 0.07, font = self.font) self.scoreText = OnscreenText(text = '', pos = (1.0, 0.8), scale = 0.07, font = self.font) self.finalScoreText = OnscreenText(text = '', pos = (0, 0.2), scale = 0.07, font = self.font) # Light alight = AmbientLight('ambientLight') alight.setColor(Vec4(0.5, 0.5, 0.5, 1)) alightNP = render.attachNewNode(alight) dlight = DirectionalLight('directionalLight') dlight.setDirection(Vec3(1, 1, -1)) dlight.setColor(Vec4(0.7, 0.7, 0.7, 1)) dlightNP = render.attachNewNode(dlight) render.clearLight() render.setLight(alightNP) render.setLight(dlightNP) # Input self.accept('escape', self.doExit) self.accept('r', self.doReset) self.accept('f1', self.toggleWireframe) self.accept('f2', self.toggleTexture) self.accept('f3', self.toggleDebug) self.accept('f5', self.doScreenshot) inputState.watchWithModifiers('forward', 'w') inputState.watchWithModifiers('left', 'a') inputState.watchWithModifiers('reverse', 's') inputState.watchWithModifiers('right', 'd') inputState.watchWithModifiers('turnLeft', 'a') inputState.watchWithModifiers('turnRight', 'd') # Task taskMgr.add(self.update, 'updateWorld') # _____HANDLER_____ def doExit(self): sys.exit(1) def doReset(self): self.cleanup() self.terrain.getRoot().removeNode() self.setup() def doBack(self): self.backBtn.hide() self.instructionScreen.hide() self.imageObject.show() self.helpBtn.show() self.startBtn.show() self.exitBtn.show() def toggleWireframe(self): base.toggleWireframe() def toggleTexture(self): base.toggleTexture() def toggleDebug(self): if self.debugNP.isHidden(): self.debugNP.show() else: self.debugNP.hide() def doScreenshot(self): base.screenshot('Bullet') # ____TASK___ def processInput(self, dt): # Process input engineForce = 0.0 brakeForce = 0.0 self.accept('p', self.doPause) if inputState.isSet('forward'): engineForce = 15.0 brakeForce = 0.0 if inputState.isSet('reverse'): engineForce = -25.0 brakeForce = 25.0 if inputState.isSet('turnLeft'): self.steering += dt * self.steeringIncrement self.steering = min(self.steering, self.steeringClamp) if inputState.isSet('turnRight'): self.steering -= dt * self.steeringIncrement self.steering = max(self.steering, -self.steeringClamp) # Apply steering to front wheels self.vehicle.setSteeringValue(self.steering, 0) self.vehicle.setSteeringValue(self.steering, 1) # Apply engine and brake to rear wheels self.vehicle.applyEngineForce(engineForce, 2) self.vehicle.applyEngineForce(engineForce, 3) self.vehicle.setBrake(brakeForce, 2) self.vehicle.setBrake(brakeForce, 3) def processContacts(self, dt): result = self.world.contactTestPair(self.vehicleNP.node(), self.flagNP.node()) if(result.getNumContacts() > 0): self.gameState = 'WIN' self.doContinue() def doContinue(self): if(self.gameState == 'INIT'): self.gameState = 'MENU' self.menuMusic.play() self.text.hide() self.startBtn = DirectButton(text=("Play"), scale = 0.1, pos = (0.5,0,0),command=self.playGame) self.helpBtn = DirectButton(text=("Help"), scale = 0.1, pos = (0.5,0,-0.2),command=self.help) self.exitBtn = DirectButton(text=("Exit"), scale = 0.1, pos = (0.5,0,-0.4), command = self.doExit) return if(self.gameState == 'RETRY'): self.retryScreen.show() self.retryBtn.show() self.loseSound.play() return if(self.gameState == 'LOSE'): self.loseScreen.show() self.menuBtn.show() return if(self.gameState == 'WIN'): if(self.gameLevel < 3): self.congratScreen.show() self.nextBtn.show() elif(self.gameLevel >= 3): self.winScreen.show() self.menuBtn.show() self.finalScoreText.setText('Your Score: '+str(int(self.score))) self.finalScoreText.show() self.winSound.play() def help(self): self.gameState = 'HELP' self.startBtn.hide() self.exitBtn.hide() self.helpBtn.hide() self.imageObject.hide() self.instructionScreen.show() self.backBtn.show() def doNext(self): self.nextBtn.hide() self.finalScoreText.hide() self.congratScreen.hide() self.gameLevel += 1 if(self.gameLevel == 2): self.score += 2000 elif(self.gameLevel == 3): self.score += 3000 self.doReset() self.triesLeft = 3 self.gameState = 'PLAY' def doRetry(self): self.doReset() self.gameState = 'PLAY' self.retryScreen.hide() self.retryBtn.hide() self.triesLeft -= 1 def doMenu(self): self.cleanup() self.terrain.getRoot().removeNode() self.gameState = 'MENU' self.score = 0 self.imageObject.show() self.startBtn.show() self.exitBtn.show() self.helpBtn.show() self.loseScreen.hide() self.menuBtn.hide() self.winScreen.hide() self.finalScoreText.hide() self.speedText.hide() self.distanceText.hide() self.rpmText.hide() self.scoreText.hide() self.gameLevelText.hide() self.timeText.hide() self.triesLeftText.hide() self.gameMusic.stop() self.menuMusic.play() def doPause(self): self.pause = not self.pause if(self.pause): self.pauseScreen.show() else: self.pauseScreen.hide() def playGame(self): self.gameState = 'PLAY' self.triesLeft = 3 self.gameLevel = 1 self.imageObject.hide() self.startBtn.hide() self.exitBtn.hide() self.helpBtn.hide() self.menuMusic.stop() self.gameMusic.play() self.speedText.show() self.distanceText.show() self.rpmText.show() self.scoreText.show() self.gameLevelText.show() self.triesLeftText.show() self.timeText.show() # Physics self.setup() def update(self, task): dt = globalClock.getDt() if(not self.pause): if(self.gameState == 'RETRY'): return task.cont if (self.gameState == 'INIT'): self.accept('space', self.doContinue) self.text.setText('Press Space to Continue') if(self.gameState == 'PLAY'): if (self.steering > 0): self.steering -= dt * 50 if (self.steering < 0): self.steering += dt * 50 playerOldSpeed = self.vehicle.getCurrentSpeedKmHour() self.processInput(dt) self.processContacts(dt) self.world.doPhysics(dt, 10, 0.008) #calculate speed,rpm,distance and display text self.speed = self.vehicle.getCurrentSpeedKmHour() if(self.speed<0): self.speed = -self.speed self.speedText.setText('Speed: ' + str(int(self.speed)) + 'Km/h') self.distanceTravelled += self.speed*(dt/3600) self.distanceText.setText('Distance: '+str(float(int(self.distanceTravelled * 1000))/1000) + 'Km') playerNewSpeed = self.vehicle.getCurrentSpeedKmHour() playerAcceleration = (playerNewSpeed - playerOldSpeed) / (dt/60) #playerPosText = self.vehicleNP.getPos() #self.text.setText('Player position: %s'%playerPosText) self.rpmText.setText('Engine RPM: ' + str(int(((self.vehicle.getCurrentSpeedKmHour() / 60) * 1000) / (2 * 0.4 * 3.14159265))) + ' Rpm') self.triesLeftText.setText('Tries Left: ' + str(self.triesLeft)) self.gameLevelText.setText('Level: '+ str(self.gameLevel)) #update camera #position d = self.vehicleNP.getPos() - base.cam.getPos() if(d.length() > 8): base.cam.setX(base.cam.getX() + d.getX()*dt) base.cam.setY(base.cam.getY() + d.getY()*dt) base.cam.setZ(self.vehicleNP.getZ() + 4) #lookat base.cam.lookAt(self.vehicleNP.getPos()+Vec3(0,0,1)) if(self.gameLevel == 1): if(self.vehicleNP.getZ() < -17): if(self.triesLeft > 0): self.gameState = 'RETRY' else: self.gameState = 'LOSE' self.doContinue() elif(self.gameLevel == 2): if(self.vehicleNP.getZ() < -20): if(self.triesLeft > 0): self.gameState = 'RETRY' else: self.gameState = 'LOSE' self.doContinue() elif(self.gameLevel == 3): if(self.vehicleNP.getZ() < -17): if(self.triesLeft > 0): self.gameState = 'RETRY' else: self.gameState = 'LOSE' self.doContinue() if(self.speed < 5): self.steeringIncrement = 120 elif(self.speed >= 5 and self.speed < 10): self.steeringIncrement = 100 elif(self.speed >= 10 and self.speed < 15): self.steeringIncrement = 80 elif(self.speed >=15 and self.speed < 30): self.steeringIncrement = 60 #spin the flag self.rot += 1 self.flagNP.setHpr(self.rot,0,0) #time self.time += dt self.timeText.setText('Time: ' + str(int(self.time))) if(self.score > 0): self.score -= dt self.scoreText.setText('Score: '+str(int(self.score))) return task.cont def cleanup(self): self.world = None self.worldNP.removeNode() def setup(self): # Steering info self.steering = 0.0 # degree self.steeringClamp = 30.0 # degree self.steeringIncrement = 80.0 # degree per second self.worldNP = render.attachNewNode('World') # World self.debugNP = self.worldNP.attachNewNode(BulletDebugNode('Debug')) #self.debugNP.show() self.world = BulletWorld() self.world.setGravity(Vec3(0, 0, -9.81)) self.world.setDebugNode(self.debugNP.node()) if(self.gameLevel == 1): #set score print('GameLevel') self.score = 1000 self.distanceTravelled = 0 self.time = 0 # Plane img = PNMImage(Filename('media/terrain/SIMP_Assignment_2_Terrain_1.png')) shape = BulletHeightfieldShape(img, 50.0, ZUp) np = self.worldNP.attachNewNode(BulletRigidBodyNode('Ground')) np.node().addShape(shape) np.setPos(0, 0, 0) np.setCollideMask(BitMask32.allOn()) self.world.attachRigidBody(np.node()) #skybox skybox = loader.loadModel('media/models/skybox/skybox_01.X') skybox.reparentTo(render) # Chassis shape = BulletBoxShape(Vec3(0.6, 1.4, 0.5)) ts = TransformState.makePos(Point3(0, 0, 1.0)) self.vehicleNP = self.worldNP.attachNewNode(BulletRigidBodyNode('Vehicle')) self.vehicleNP.node().addShape(shape, ts) self.vehicleNP.setPos(-93, -88, -7)#-93, -88, -7) #(-82,65.8,-8) #(55,8.38,-6)#(45, -19, -8)#(-93, -88, -7) self.vehicleNP.setHpr(-90,0,0) self.vehicleNP.node().setMass(5.0) self.vehicleNP.node().setDeactivationEnabled(False) base.cam.setPos(self.vehicleNP.getPos().getX()+2,self.vehicleNP.getPos().getY()+2,self.vehicleNP.getPos().getZ()+2) self.world.attachRigidBody(self.vehicleNP.node()) # Vehicle self.vehicle = BulletVehicle(self.world, self.vehicleNP.node()) self.vehicle.setCoordinateSystem(ZUp) self.world.attachVehicle(self.vehicle) self.hummerNP = loader.loadModel('media/models/vehicle/body.X') self.hummerNP.reparentTo(self.vehicleNP) # Right front wheel np = loader.loadModel('media/models/vehicle/front_right.X') np.reparentTo(self.worldNP) self.addWheel(Point3( 0.8, 0.9, 0.8), True, np) # Left front wheel np = loader.loadModel('media/models/vehicle/front_left.X') np.reparentTo(self.worldNP) self.addWheel(Point3(-0.8, 0.9, 0.8), True, np) # Right rear wheel np = loader.loadModel('media/models/vehicle/back_right.X') np.reparentTo(self.worldNP) self.addWheel(Point3( 0.8, -0.7, 0.8), False, np) # Left rear wheel np = loader.loadModel('media/models/vehicle/back_left.X') np.reparentTo(self.worldNP) self.addWheel(Point3(-0.8, -0.7, 0.8), False, np) #Obstacles self.setupObstacleOne(Vec3(50, -5, -4), 1.8, Vec3(60, 0, 0)) self.setupObstacleFour(Vec3(63.3, 59.2, -10), 1.5, Vec3(0,0,0)) self.setupObstacleFour(Vec3(41, 57, -10), 1.5, Vec3(0,0,0)) self.setupObstacleFour(Vec3(7.5, 53.8, -10), 1.5, Vec3(0,0,0)) self.setupObstacleFour(Vec3(-28, 81.4, -10), 1.5, Vec3(0,0,0)) self.setupObstacleSix(Vec3(-91, 81 , -6), 1, Vec3(60,0,0)) #Goal self.setupGoal(Vec3(-101,90.6,-6.5)) #self.vehicleNP.setPos(Vec3(6,52,-6)) self.setupTerrain() elif(self.gameLevel == 2): self.distanceTravelled = 0 self.time = 0 # Plane img = PNMImage(Filename('media/terrain/SIMP_Assignment_2_Terrain_2.png')) shape = BulletHeightfieldShape(img, 50.0, ZUp) np = self.worldNP.attachNewNode(BulletRigidBodyNode('Ground')) np.node().addShape(shape) np.setPos(0, 0, 0) np.setCollideMask(BitMask32.allOn()) self.world.attachRigidBody(np.node()) #skybox skybox = loader.loadModel('media/models/skybox/skybox_01.X') skybox.reparentTo(render) # Chassis shape = BulletBoxShape(Vec3(0.6, 1.4, 0.5)) ts = TransformState.makePos(Point3(0, 0, 1.0)) self.vehicleNP = self.worldNP.attachNewNode(BulletRigidBodyNode('Vehicle')) self.vehicleNP.node().addShape(shape, ts) self.vehicleNP.setPos(-99.6,105,-11.8)#(88, 21, -11)#(34.3,8.4,-11.8)#(-99.6,105,-11.8)#(86.4,41.2,-12) self.vehicleNP.setHpr(-130,0,0) self.vehicleNP.node().setMass(5.0) self.vehicleNP.node().setDeactivationEnabled(False) base.cam.setPos(self.vehicleNP.getPos().getX()+2,self.vehicleNP.getPos().getY()+2,self.vehicleNP.getPos().getZ()+2) self.world.attachRigidBody(self.vehicleNP.node()) # Vehicle self.vehicle = BulletVehicle(self.world, self.vehicleNP.node()) self.vehicle.setCoordinateSystem(ZUp) self.world.attachVehicle(self.vehicle) self.hummerNP = loader.loadModel('media/models/vehicle/body.X') self.hummerNP.reparentTo(self.vehicleNP) # Right front wheel np = loader.loadModel('media/models/vehicle/front_right.X') np.reparentTo(self.worldNP) self.addWheel(Point3( 0.8, 0.9, 0.8), True, np) # Left front wheel np = loader.loadModel('media/models/vehicle/front_left.X') np.reparentTo(self.worldNP) self.addWheel(Point3(-0.8, 0.9, 0.8), True, np) # Right rear wheel np = loader.loadModel('media/models/vehicle/back_right.X') np.reparentTo(self.worldNP) self.addWheel(Point3( 0.8, -0.7, 0.8), False, np) # Left rear wheel np = loader.loadModel('media/models/vehicle/back_left.X') np.reparentTo(self.worldNP) self.addWheel(Point3(-0.8, -0.7, 0.8), False, np) self.setupObstacleFive(Vec3(91, 3, -9),1,Vec3(90,0,0)) self.setupObstacleFive(Vec3(94,-19, -10),0.9,Vec3(90,0,0)) self.setupObstacleFive(Vec3(85,-40, -10),1,Vec3(90,0,0)) self.setupObstacleFour(Vec3(-33.5, 23.4,-14.5),1,Vec3(0,0,0)) self.setupObstacleFour(Vec3(-43.3, 24.2,-14.5),1,Vec3(0,0,0)) self.setupObstacleTwo(Vec3(34.7,20.9,-8.5),1,Vec3(90,0,0)) self.setupObstacleTwo(Vec3(26.8,20.3,-8.5),1,Vec3(90,0,0)) self.setupObstacleTwo(Vec3(42.1,22.5,-8.5),1,Vec3(90,0,0)) #self.setupObstacleFive(Vec3(91,0.2, -8),2.1,Vec3(90,0,0)) #Goal self.setupGoal(Vec3(94,-89.7,-10)) self.setupTerrain() elif(self.gameLevel == 3): self.distanceTravelled = 0 self.time = 0 # Plane img = PNMImage(Filename('media/terrain/SIMP_Assignment_2_Terrain_3.png')) shape = BulletHeightfieldShape(img, 50.0, ZUp) np = self.worldNP.attachNewNode(BulletRigidBodyNode('Ground')) np.node().addShape(shape) np.setPos(0, 0, 0) np.setCollideMask(BitMask32.allOn()) self.world.attachRigidBody(np.node()) #skybox skybox = loader.loadModel('media/models/skybox/skybox_01.X') skybox.reparentTo(render) # Chassis shape = BulletBoxShape(Vec3(0.6, 1.4, 0.5)) ts = TransformState.makePos(Point3(0, 0, 1.0)) self.vehicleNP = self.worldNP.attachNewNode(BulletRigidBodyNode('Vehicle')) self.vehicleNP.node().addShape(shape, ts) self.vehicleNP.setPos(-110, -110, 0) self.vehicleNP.setHpr(-40,0,0) self.vehicleNP.node().setMass(5.0) self.vehicleNP.node().setDeactivationEnabled(False) base.cam.setPos(self.vehicleNP.getPos().getX()+2,self.vehicleNP.getPos().getY()+2,self.vehicleNP.getPos().getZ()+2) self.world.attachRigidBody(self.vehicleNP.node()) # Vehicle self.vehicle = BulletVehicle(self.world, self.vehicleNP.node()) self.vehicle.setCoordinateSystem(ZUp) self.world.attachVehicle(self.vehicle) self.hummerNP = loader.loadModel('media/models/vehicle/body.X') self.hummerNP.reparentTo(self.vehicleNP) # Right front wheel np = loader.loadModel('media/models/vehicle/front_right.X') np.reparentTo(self.worldNP) self.addWheel(Point3( 0.8, 0.9, 0.8), True, np) # Left front wheel np = loader.loadModel('media/models/vehicle/front_left.X') np.reparentTo(self.worldNP) self.addWheel(Point3(-0.8, 0.9, 0.8), True, np) # Right rear wheel np = loader.loadModel('media/models/vehicle/back_right.X') np.reparentTo(self.worldNP) self.addWheel(Point3( 0.8, -0.7, 0.8), False, np) # Left rear wheel np = loader.loadModel('media/models/vehicle/back_left.X') np.reparentTo(self.worldNP) self.addWheel(Point3(-0.8, -0.7, 0.8), False, np) self.setupTerrain() #Goal self.setupGoal(Vec3(114,100,-13)) #Obstacles self.setupObstacleFour(Vec3(-60, -73, -9), 1, Vec3(0, 0, 0)) self.setupObstacleFour(Vec3(-63, -77, -9), 1, Vec3(0, 0, 0)) self.setupObstacleTwo(Vec3(-15, -40, -3), 1, Vec3(0, 0, 0)) self.setupObstacleFour(Vec3(-60, 12, -11), 1, Vec3(0, 0, 0)) self.setupObstacleSix(Vec3(-15, 90, -6), 1.5, Vec3(-30, 0, 0)) self.setupObstacleFour(Vec3(28, 87, -11), 1, Vec3(0, 0, 0)) self.setupObstacleFour(Vec3(32, 90, -11), 1, Vec3(0, 0, 0)) def addWheel(self, pos, front, np): wheel = self.vehicle.createWheel() wheel.setNode(np.node()) wheel.setChassisConnectionPointCs(pos) wheel.setFrontWheel(front) wheel.setWheelDirectionCs(Vec3(0, 0, -1)) wheel.setWheelAxleCs(Vec3(1, 0, 0)) wheel.setWheelRadius(0.4) wheel.setMaxSuspensionTravelCm(40.0) wheel.setSuspensionStiffness(40.0) wheel.setWheelsDampingRelaxation(2.3) wheel.setWheelsDampingCompression(4.4) wheel.setFrictionSlip(100.0); wheel.setRollInfluence(0.1) def setupTerrain(self): if(self.gameLevel == 1): #terrain setting img = PNMImage(Filename('media/terrain/SIMP_Assignment_2_Terrain_1.png')) self.terrain = GeoMipTerrain("myTerrain") self.terrain.setHeightfield(img) self.terrain.getRoot().setSz(50) self.terrain.setBlockSize(4) #self.terrain.setFactor(10) #self.terrain.setMinLevel(0) self.terrain.setNear(50) self.terrain.setFar(1000) self.terrain.setFocalPoint(base.camera) self.terrain.getRoot().reparentTo(render) offset = img.getXSize() / 2.0 - 0.5 self.terrain.getRoot().setPos(-offset, -offset, -50 / 2.0) self.terrain.generate() #load textures tex0 = loader.loadTexture("media/terrain/SIMP_Assignment_2_Terrain_1_d.png") tex0.setMinfilter(Texture.FTLinearMipmapLinear) tex1 = loader.loadTexture("media/terrain/longGrass.png") tex1.setMinfilter(Texture.FTLinearMipmapLinear) tex2 = loader.loadTexture("media/terrain/bigRockFace.png") tex2.setMinfilter(Texture.FTLinearMipmapLinear) tex3 = loader.loadTexture("media/terrain/greenrough.png") tex3.setMinfilter(Texture.FTLinearMipmapLinear) tex4 = loader.loadTexture("media/terrain/grayRock.png") tex4.setMinfilter(Texture.FTLinearMipmapLinear) tex5 = loader.loadTexture("media/terrain/SIMP_Assignment_2_Terrain_1_c.png") tex5.setMinfilter(Texture.FTLinearMipmapLinear) tex6 = loader.loadTexture("media/terrain/SIMP_Assignment_2_Terrain_1_l.png") tex6.setMinfilter(Texture.FTLinearMipmapLinear) #set mutiltextures self.terrain.getRoot().setTexture( TextureStage('tex0'),tex0 ) self.terrain.getRoot().setTexture( TextureStage('tex1'),tex1 ) self.terrain.getRoot().setTexture( TextureStage('tex2'),tex2 ) self.terrain.getRoot().setTexture( TextureStage('tex3'),tex3 ) self.terrain.getRoot().setTexture( TextureStage('tex4'),tex4 ) self.terrain.getRoot().setTexture( TextureStage('tex5'),tex5 ) self.terrain.getRoot().setTexture( TextureStage('tex6'),tex6 ) #load shader self.terrain.getRoot().setShader(loader.loadShader('terraintexture.sha')) elif(self.gameLevel == 2): #terrain setting img = PNMImage(Filename('media/terrain/SIMP_Assignment_2_Terrain_2.png')) self.terrain = GeoMipTerrain("myTerrain") self.terrain.setHeightfield(img) self.terrain.getRoot().setSz(50) self.terrain.setBlockSize(4) #self.terrain.setFactor(10) #self.terrain.setMinLevel(0) self.terrain.setNear(50) self.terrain.setFar(100) self.terrain.setFocalPoint(base.camera) self.terrain.getRoot().reparentTo(render) offset = img.getXSize() / 2.0 - 0.5 self.terrain.getRoot().setPos(-offset, -offset, -50 / 2.0) self.terrain.generate() #load textures tex0 = loader.loadTexture("media/terrain/SIMP_Assignment_2_Terrain_2_d.png") tex0.setMinfilter(Texture.FTLinearMipmapLinear) tex1 = loader.loadTexture("media/terrain/sandripple.png") tex1.setMinfilter(Texture.FTLinearMipmapLinear) tex2 = loader.loadTexture("media/terrain/orangesand.png") tex2.setMinfilter(Texture.FTLinearMipmapLinear) tex3 = loader.loadTexture("media/terrain/grayRock.png") tex3.setMinfilter(Texture.FTLinearMipmapLinear) tex4 = loader.loadTexture("media/terrain/bigRockFace.png") tex4.setMinfilter(Texture.FTLinearMipmapLinear) tex5 = loader.loadTexture("media/terrain/SIMP_Assignment_2_Terrain_2_c.png") tex5.setMinfilter(Texture.FTLinearMipmapLinear) tex6 = loader.loadTexture("media/terrain/SIMP_Assignment_2_Terrain_2_l.png") tex6.setMinfilter(Texture.FTLinearMipmapLinear) #set mutiltextures self.terrain.getRoot().setTexture( TextureStage('tex0'),tex0 ) self.terrain.getRoot().setTexture( TextureStage('tex1'),tex1 ) self.terrain.getRoot().setTexture( TextureStage('tex2'),tex2 ) self.terrain.getRoot().setTexture( TextureStage('tex3'),tex3 ) self.terrain.getRoot().setTexture( TextureStage('tex4'),tex4 ) self.terrain.getRoot().setTexture( TextureStage('tex5'),tex5 ) self.terrain.getRoot().setTexture( TextureStage('tex6'),tex6 ) #load shader self.terrain.getRoot().setShader(loader.loadShader('terraintexture.sha')) elif(self.gameLevel == 3): #terrain setting img = PNMImage(Filename('media/terrain/SIMP_Assignment_2_Terrain_3.png')) self.terrain = GeoMipTerrain("myTerrain") self.terrain.setHeightfield(img) self.terrain.getRoot().setSz(50) self.terrain.setBlockSize(4) #self.terrain.setFactor(10) #self.terrain.setMinLevel(0) self.terrain.setNear(50) self.terrain.setFar(100) self.terrain.setFocalPoint(base.camera) self.terrain.getRoot().reparentTo(render) offset = img.getXSize() / 2.0 - 0.5 self.terrain.getRoot().setPos(-offset, -offset, -50 / 2.0) self.terrain.generate() #load textures tex0 = loader.loadTexture("media/terrain/SIMP_Assignment_2_Terrain_3_d.png") tex0.setMinfilter(Texture.FTLinearMipmapLinear) tex1 = loader.loadTexture("media/terrain/hardDirt.png") tex1.setMinfilter(Texture.FTLinearMipmapLinear) tex2 = loader.loadTexture("media/terrain/littlerocks.png") tex2.setMinfilter(Texture.FTLinearMipmapLinear) tex3 = loader.loadTexture("media/terrain/greenrough.png") tex3.setMinfilter(Texture.FTLinearMipmapLinear) tex4 = loader.loadTexture("media/terrain/bigRockFace.png") tex4.setMinfilter(Texture.FTLinearMipmapLinear) tex5 = loader.loadTexture("media/terrain/SIMP_Assignment_2_Terrain_3_c.png") tex5.setMinfilter(Texture.FTLinearMipmapLinear) tex6 = loader.loadTexture("media/terrain/SIMP_Assignment_2_Terrain_3_l.png") tex6.setMinfilter(Texture.FTLinearMipmapLinear) #set mutiltextures self.terrain.getRoot().setTexture( TextureStage('tex0'),tex0 ) self.terrain.getRoot().setTexture( TextureStage('tex1'),tex1 ) self.terrain.getRoot().setTexture( TextureStage('tex2'),tex2 ) self.terrain.getRoot().setTexture( TextureStage('tex3'),tex3 ) self.terrain.getRoot().setTexture( TextureStage('tex4'),tex4 ) self.terrain.getRoot().setTexture( TextureStage('tex5'),tex5 ) self.terrain.getRoot().setTexture( TextureStage('tex6'),tex6 ) #load shader self.terrain.getRoot().setShader(loader.loadShader('terraintexture.sha')) def setupObstacleOne(self, pos, scale, turn): #box A shape = BulletBoxShape(Vec3(3, 0.1, 0.1) * scale) bodyA = BulletRigidBodyNode('Box A') bodyNP= self.worldNP.attachNewNode(bodyA) bodyNP.node().addShape(shape) bodyNP.setCollideMask(BitMask32.allOn()) bodyNP.setPos(pos) bodyNP.setHpr(turn) visNP = loader.loadModel('media/models/box.egg') visNP.setScale(Vec3(3, 0.1, 0.1)*2 * scale) visNP.clearModelNodes() visNP.reparentTo(bodyNP) self.world.attachRigidBody(bodyA) # Box C shape = BulletBoxShape(Vec3(0.1, 0.1, 0.9)*scale) bodyC = BulletRigidBodyNode('Box C') bodyNP = self.worldNP.attachNewNode(bodyC) bodyNP.node().addShape(shape) bodyNP.node().setMass(1.0) bodyNP.node().setLinearDamping(0.5) bodyNP.node().setDeactivationEnabled(False) bodyNP.setCollideMask(BitMask32.allOn()) bodyNP.setPos(pos) bodyNP.setHpr(turn) visNP = loader.loadModel('media/models/box.egg') visNP.setScale(Vec3(0.1, 0.1, 0.9)*2*scale) visNP.clearModelNodes() visNP.reparentTo(bodyNP) self.world.attachRigidBody(bodyC) pivotA = Point3(0, 0, -0.1 * scale) pivotB = Point3(0, 0, 1 * scale) axisA = Vec3(1, 0, 0) axisB = Vec3(1, 0, 0) hinge = BulletHingeConstraint(bodyA, bodyC, pivotA, pivotB, axisA, axisB, True) hinge.setDebugDrawSize(2.0) hinge.setLimit(-90,90, softness=1.0, bias=0.3, relaxation=1.0) self.world.attachConstraint(hinge) # Box B shape = BulletBoxShape(Vec3(3, 2, 0.1)*scale) bodyB = BulletRigidBodyNode('Box B') bodyNP = self.worldNP.attachNewNode(bodyB) bodyNP.node().addShape(shape) bodyNP.node().setMass(1.0) bodyNP.node().setLinearDamping(0.5) bodyNP.node().setDeactivationEnabled(False) bodyNP.setCollideMask(BitMask32.allOn()) bodyNP.setPos(pos) bodyNP.setHpr(turn); visNP = loader.loadModel('media/models/box.egg') visNP.setScale(Vec3(3, 2, 0.1)*2*scale) visNP.clearModelNodes() visNP.reparentTo(bodyNP) self.world.attachRigidBody(bodyB) # Hinge pivotA = Point3(0, 0, 0) pivotB = Point3(0, 0, -1 * scale) hinge = BulletHingeConstraint(bodyB, bodyC, pivotA, pivotB, axisA, axisB, True) hinge.setLimit(0,360, softness=1.0, bias=0.3, relaxation=1.0) self.world.attachConstraint(hinge) def setupObstacleTwo(self,pos,scale,turn): #box A shape = BulletBoxShape(Vec3(3, 0.1, 0.1)*scale) bodyA = BulletRigidBodyNode('Box A') bodyNP= self.worldNP.attachNewNode(bodyA) bodyNP.node().addShape(shape) bodyNP.setCollideMask(BitMask32.allOn()) bodyNP.setPos(pos) bodyNP.setHpr(turn) visNP = loader.loadModel('media/models/box.egg') visNP.setScale(Vec3(3, 0.1, 0.1)*2*scale) visNP.clearModelNodes() visNP.reparentTo(bodyNP) self.world.attachRigidBody(bodyA) # Box B shape = BulletBoxShape(Vec3(0.1, 1, 1)*scale) bodyB = BulletRigidBodyNode('Box B') bodyNP = self.worldNP.attachNewNode(bodyB) bodyNP.node().addShape(shape) bodyNP.node().setMass(100.0) bodyNP.node().setDeactivationEnabled(False) bodyNP.setCollideMask(BitMask32.allOn()) bodyNP.setPos(pos) bodyNP.setHpr(turn) visNP = loader.loadModel('media/models/box.egg') visNP.setScale(Vec3(0.1, 1, 1)*2*scale) visNP.clearModelNodes() visNP.reparentTo(bodyNP) self.world.attachRigidBody(bodyB) # Hinge pivotA = Point3(2, 0, 0) pivotB = Point3(0, 0, 2) axisA = Vec3(1, 0, 0) axisB = Vec3(1, 0, 0) hinge = BulletHingeConstraint(bodyA, bodyB, pivotA, pivotB, axisA, axisB, True) hinge.setDebugDrawSize(2.0) hinge.setLimit(-90,90, softness=1.0, bias=0.3, relaxation=1.0) self.world.attachConstraint(hinge) # Box C shape = BulletBoxShape(Vec3(0.1, 1, 1)*scale) bodyC = BulletRigidBodyNode('Box C') bodyNP = self.worldNP.attachNewNode(bodyC) bodyNP.node().addShape(shape) bodyNP.node().setMass(100.0) bodyNP.node().setDeactivationEnabled(False) bodyNP.setCollideMask(BitMask32.allOn()) bodyNP.setPos(pos) bodyNP.setHpr(turn) visNP = loader.loadModel('media/models/box.egg') visNP.setScale(Vec3(0.1, 1, 1)*2*scale) visNP.clearModelNodes() visNP.reparentTo(bodyNP) self.world.attachRigidBody(bodyC) pivotA = Point3(-2, 0, 0) pivotB = Point3(0, 0, 2) hinge = BulletHingeConstraint(bodyA, bodyC, pivotA, pivotB, axisA, axisB, True) self.world.attachConstraint(hinge) def setupObstacleThree(self, pos, scale, turn): # Box A shape = BulletBoxShape(Vec3(0.1, 0.1, 0.1)) bodyA = BulletRigidBodyNode('Box A') bodyA.setRestitution(1.0) bodyNP = self.worldNP.attachNewNode(bodyA) bodyNP.node().addShape(shape) bodyNP.setCollideMask(BitMask32.allOn()) bodyNP.setPos(pos) bodyNP.setHpr(turn) visNP = loader.loadModel('media/models/box.egg') visNP.setScale(Vec3(0.1, 0.1, 0.1)*2*scale) visNP.clearModelNodes() visNP.reparentTo(bodyNP) self.world.attachRigidBody(bodyA) #Box B shape = BulletBoxShape(Vec3(0.1,0.1,0.1)) bodyB = BulletRigidBodyNode('Box B') bodyB.setRestitution(1.0) bodyNP = self.worldNP.attachNewNode(bodyB) bodyNP.node().addShape(shape) bodyNP.setCollideMask(BitMask32.allOn()) bodyNP.setPos(pos) bodyNP.setHpr(turn) visNP = loader.loadModel('media/models/box.egg') visNP.setScale(Vec3(0.1,0.1,0.1)*2*scale) visNP.clearModelNodes() visNP.reparentTo(bodyNP) self.world.attachRigidBody(bodyB) # Slider frameA = TransformState.makePosHpr(Point3(0, 0, 0), Vec3(0, 0, 0)) frameB = TransformState.makePosHpr(Point3(0, 0, 0), Vec3(0, 0, 0)) slider = BulletSliderConstraint(bodyA, bodyB, frameA, frameB, True) slider.setDebugDrawSize(2.0) slider.setLowerLinearLimit(0) slider.setUpperLinearLimit(12) slider.setLowerAngularLimit(-90) slider.setUpperAngularLimit(-85) self.world.attachConstraint(slider) # Box C shape = BulletBoxShape(Vec3(1, 3, 0.1)) bodyC = BulletRigidBodyNode('Box C') bodyC.setRestitution(1.0) bodyNP = self.worldNP.attachNewNode(bodyC) bodyNP.node().addShape(shape) bodyNP.node().setMass(0.1) bodyNP.node().setDeactivationEnabled(False) bodyNP.setCollideMask(BitMask32.allOn()) bodyNP.setPos(Vec3(pos.getX() + 3, pos.getY() - 4, pos.getZ())) bodyNP.setHpr(turn) visNP = loader.loadModel('media/models/box.egg') visNP.setScale(Vec3(1, 3, 0.1)*2*scale) visNP.clearModelNodes() visNP.reparentTo(bodyNP) self.world.attachRigidBody(bodyC) bodyNP.node().setLinearVelocity(-100) # Slider frameA = TransformState.makePosHpr(Point3(0, 0, 0), Vec3(0, 0, 0)) frameB = TransformState.makePosHpr(Point3(0, 0, 0), Vec3(0, 0, 0)) slider = BulletSliderConstraint(bodyA, bodyC, frameA, frameB, True) slider.setDebugDrawSize(2.0) slider.setLowerLinearLimit(2) slider.setUpperLinearLimit(6) slider.setLowerAngularLimit(-90) slider.setUpperAngularLimit(-85) self.world.attachConstraint(slider) def setupObstacleFour(self, pos, scale, turn): #Start Here # Box A shape = BulletBoxShape(Vec3(0.01, 0.01, 0.01) * scale) bodyA = BulletRigidBodyNode('Box A') bodyNP = self.worldNP.attachNewNode(bodyA) bodyNP.node().addShape(shape) bodyNP.setCollideMask(BitMask32.allOn()) bodyNP.setPos(pos.getX(), pos.getY(), pos.getZ() + 4) #(0, 0, 4) visNP = loader.loadModel('media/models/box.egg') visNP.setScale(Vec3(0.01, 0.01, 0.01)*2*scale) visNP.clearModelNodes() visNP.reparentTo(bodyNP) self.world.attachRigidBody(bodyA) # Box B shape = BulletSphereShape(0.5*scale) bodyB = BulletRigidBodyNode('Sphere B') bodyNP = self.worldNP.attachNewNode(bodyB) bodyNP.node().addShape(shape) bodyNP.node().setMass(10.0) bodyNP.node().setDeactivationEnabled(False) bodyNP.setCollideMask(BitMask32.allOn()) bodyNP.setPos(pos.getX(), pos.getY(), pos.getZ() + 5) #(0, 0, 0.001) visNP = loader.loadModel('media/models/ball.egg') visNP.clearModelNodes() visNP.setScale(1.25*scale) visNP.reparentTo(bodyNP) bodyNP.node().setLinearVelocity(100) self.world.attachRigidBody(bodyB) # Cone frameA = TransformState.makePosHpr(Point3(0, 0, 0), Vec3(0, 0, 90)) frameB = TransformState.makePosHpr(Point3(2, 0, 0)*scale, Vec3(0, 0, 0)) cone = BulletConeTwistConstraint(bodyA, bodyB, frameA, frameB) cone.setDebugDrawSize(2.0) cone.setLimit(30, 90, 270, softness=1.0, bias=0.3, relaxation=10.0) self.world.attachConstraint(cone) # Box C shape = BulletBoxShape(Vec3(0.1, 0.1, 1)*scale) bodyC = BulletRigidBodyNode('Box C') bodyNP = self.worldNP.attachNewNode(bodyC) bodyNP.node().addShape(shape) bodyNP.node().setDeactivationEnabled(False) bodyNP.setCollideMask(BitMask32.allOn()) bodyNP.setPos(pos.getX(), pos.getY(), pos.getZ() + 3) self.world.attachRigidBody(bodyC) visNP = loader.loadModel('media/models/box.egg') visNP.setScale(Vec3(0.1, 0.1, 1)*2*scale) visNP.clearModelNodes() visNP.reparentTo(bodyNP) def setupObstacleSix(self, pos, scale, turn): #box A shape = BulletBoxShape(Vec3(0.1, 0.1, 0.1)*scale) bodyA = BulletRigidBodyNode('Box A') bodyNP= self.worldNP.attachNewNode(bodyA) bodyNP.node().addShape(shape) bodyNP.setCollideMask(BitMask32.allOff()) bodyNP.setPos(pos.getX()-2,pos.getY(),pos.getZ()+2.5)#-2,0,2.5) bodyNP.setHpr(turn) # Box B shape = BulletBoxShape(Vec3(2, 0.1, 3)*scale) bodyB = BulletRigidBodyNode('Box B') bodyNP = self.worldNP.attachNewNode(bodyB) bodyNP.node().addShape(shape) bodyNP.node().setMass(1.0) bodyNP.node().setLinearDamping(0.5) bodyNP.node().setDeactivationEnabled(False) bodyNP.setCollideMask(BitMask32.allOn()) bodyNP.setPos(pos.getX()-3,pos.getY(), pos.getZ())#, 0, 0) bodyNP.setHpr(turn) visNP = loader.loadModel('media/models/box.egg') visNP.setScale(Vec3(2, 0.1, 3)*2*scale) visNP.clearModelNodes() visNP.reparentTo(bodyNP) self.world.attachRigidBody(bodyB) # Hinge pivotA = Point3(-2, 0, -3) pivotB = Point3(-2, 0, -3) axisA = Vec3(0, 0, 1) axisB = Vec3(0, 0, 1) hinge = BulletHingeConstraint(bodyA, bodyB, pivotA, pivotB, axisA, axisB, True) hinge.setDebugDrawSize(2.0) hinge.setLimit(0,90, softness=1.0, bias=0.3, relaxation=1.0) self.world.attachConstraint(hinge) #box A shape = BulletBoxShape(Vec3(0.1, 0.1, 0.1)*scale) bodyA = BulletRigidBodyNode('Box A') bodyNP= self.worldNP.attachNewNode(bodyA) bodyNP.node().addShape(shape) bodyNP.setCollideMask(BitMask32.allOff()) bodyNP.setPos(pos.getX()+2,pos.getY(),pos.getZ()+2.5)#2,0,2.5) bodyNP.setHpr(turn) # Box B shape = BulletBoxShape(Vec3(2, 0.1, 3)*scale) bodyB = BulletRigidBodyNode('Box B') bodyNP = self.worldNP.attachNewNode(bodyB) bodyNP.node().addShape(shape) bodyNP.node().setMass(1.0) bodyNP.node().setLinearDamping(0.5) bodyNP.node().setDeactivationEnabled(False) bodyNP.setCollideMask(BitMask32.allOn()) bodyNP.setPos(pos.getX()+4, pos.getY(), pos.getZ())# 0, 0) bodyNP.setHpr(turn) visNP = loader.loadModel('media/models/box.egg') visNP.setScale(Vec3(2, 0.1, 3)*2*scale) visNP.clearModelNodes() visNP.reparentTo(bodyNP) self.world.attachRigidBody(bodyB) pivotA = Point3(2, 0, -3) pivotB = Point3(2, 0, -3) hinge = BulletHingeConstraint(bodyA, bodyB, pivotA, pivotB, axisA, axisB, True) hinge.setLimit(-90,0, softness=1.0, bias=0.3, relaxation=1.0) self.world.attachConstraint(hinge) def setupObstacleFive(self, pos, scale, turn): #box A shape = BulletBoxShape(Vec3(3, 0.1, 0.1)*scale) bodyA = BulletRigidBodyNode('Box A') bodyNP= self.worldNP.attachNewNode(bodyA) bodyNP.node().addShape(shape) bodyNP.setCollideMask(BitMask32.allOn()) bodyNP.setPos(pos) bodyNP.setHpr(turn) visNP = loader.loadModel('media/models/box.egg') visNP.setScale(Vec3(3, 0.1, 0.1)*2*scale) visNP.clearModelNodes() visNP.reparentTo(bodyNP) self.world.attachRigidBody(bodyA) # Box B shape = BulletBoxShape(Vec3(3, 2, 0.1)*scale) bodyB = BulletRigidBodyNode('Box B') bodyNP = self.worldNP.attachNewNode(bodyB) bodyNP.node().addShape(shape) bodyNP.node().setMass(1.0) bodyNP.node().setDeactivationEnabled(False) bodyNP.setCollideMask(BitMask32.allOn()) bodyNP.setPos(pos) bodyNP.setHpr(turn) visNP = loader.loadModel('media/models/box.egg') visNP.setScale(Vec3(3, 2, 0.1)*2*scale) visNP.clearModelNodes() visNP.reparentTo(bodyNP) self.world.attachRigidBody(bodyB) # Hinge pivotA = Point3(0, 0, 0) pivotB = Point3(0, 0, 5) axisA = Vec3(1, 0, 0) axisB = Vec3(1, 0, 0) hinge = BulletHingeConstraint(bodyA, bodyB, pivotA, pivotB, axisA, axisB, True) hinge.setDebugDrawSize(2.0) hinge.setLimit(-50,50, softness=0.5, bias=0.3, relaxation=0.6) self.world.attachConstraint(hinge) # Box C shape = BulletBoxShape(Vec3(0.1, 0.1, 0.9)*scale) bodyC = BulletRigidBodyNode('Box C') bodyNP = self.worldNP.attachNewNode(bodyC) bodyNP.node().addShape(shape) bodyNP.node().setMass(1.0) bodyNP.node().setDeactivationEnabled(False) bodyNP.setCollideMask(BitMask32.allOn()) bodyNP.setPos(pos) bodyNP.setHpr(turn) visNP = loader.loadModel('media/models/box.egg') visNP.setScale(Vec3(0.1, 0.1, 0.9)*2*scale) visNP.clearModelNodes() visNP.reparentTo(bodyNP) self.world.attachRigidBody(bodyC) pivotA = Point3(0, 0, -1.1) pivotB = Point3(0, 0, 1) hinge = BulletHingeConstraint(bodyA, bodyC, pivotA, pivotB, axisA, axisB, True) hinge.setLimit(-90,90, softness=1.0, bias=0.3, relaxation=1.0) self.world.attachConstraint(hinge) def setupGoal(self, pos): # Goal shape = BulletBoxShape(Vec3(1, 1, 1)) body = BulletRigidBodyNode('Flag') self.flagNP = self.worldNP.attachNewNode(body) self.flagNP.node().addShape(shape) self.flagNP.setCollideMask(BitMask32.allOn()) self.flagNP.setPos(pos) visNP = loader.loadModel('media/models/Flag.X') visNP.clearModelNodes() visNP.reparentTo(self.flagNP) self.world.attachRigidBody(body)
class CarPhys(PhysColleague): def __init__(self, mediator, car_props, tuning, players): PhysColleague.__init__(self, mediator) self.pnode = self.vehicle = self.__track_phys = self.coll_mesh = \ self.max_speed = self.friction_slip = \ self.friction_slip_rear = self.cfg = None self.turbo = False self._tuning = tuning self._players = players self.roll_influence = [] self.ai_meshes = [] self.curr_speed_mul = 1.0 self.roll_influence_k = self.friction_slip_k = 1.0 self.__prev_speed = 0 self.__last_drift_time = 0 self.__finds = {} # cache for find's results self.__whl2flytime = {} self.cprops = car_props self._load_phys() self.__set_collision_mesh() self.__set_ai_meshes() self.__set_phys_node() self.__set_vehicle() self.__set_wheels() self.eng.attach_obs(self.on_end_frame) def _load_phys(self): ppath = self.cprops.race_props.season_props.gameprops.phys_path fpath = ppath % self.cprops.name with open(fpath) as phys_file: self.cfg = load(phys_file) # they may be changed by drivers and tuning self.cfg['max_speed'] = self.get_speed() self.cfg['friction_slip'] = self.get_friction_static()[0] self.cfg['friction_slip_rear'] = self.get_friction_static()[1] self.cfg['roll_influence'] = self.get_roll_influence_static() self.friction_slip = self.cfg['friction_slip'] self.friction_slip_rear = self.cfg['friction_slip_rear'] self.__log_props() set_a = lambda field: setattr(self, field, self.cfg[field]) list(map(set_a, self.cfg.keys())) def __log_props(self, starting=True): s_s = self.cfg['max_speed'] if starting else self.max_speed s_f = self.cfg['friction_slip'] if starting else \ self.get_friction_static()[0] s_fr = self.cfg['friction_slip_rear'] if starting else \ self.get_friction_static()[1] s_r = self.cfg['roll_influence'] if starting else \ self.get_roll_influence_static() log_info = [('speed', self.cprops.name, round(s_s, 2), self.cprops.driver_engine), ('friction 0', self.cprops.name, round(s_f[0], 2), self.cprops.driver_tires), ('friction 1', self.cprops.name, round(s_f[1], 2), self.cprops.driver_tires), ('friction_rear 0', self.cprops.name, round(s_fr[0], 2), self.cprops.driver_tires), ('friction_rear 1', self.cprops.name, round(s_fr[1], 2), self.cprops.driver_tires), ('roll min', self.cprops.name, round(s_r[0], 2), self.cprops.driver_suspensions), ('roll max', self.cprops.name, round(s_r[1], 2), self.cprops.driver_suspensions)] for l_i in log_info: info('%s %s: %s (%s)' % l_i) def __set_collision_mesh(self): fpath = self.cprops.race_props.coll_path % self.cprops.name try: self.coll_mesh = self.eng.load_model(fpath) except OSError: # new cars don't have collision meshes self.coll_mesh = self.eng.load_model( fpath.replace('capsule', 'car')) # chassis_shape = BulletConvexHullShape() # for geom in self.eng.lib.find_geoms( # self.coll_mesh, self.cprops.race_props.coll_name): # chassis_shape.add_geom(geom.node().get_geom(0), # geom.get_transform()) # self.mediator.gfx.nodepath.get_node().add_shape(chassis_shape) chassis_shape = BulletBoxShape(tuple(self.cfg['box_size'])) boxpos = self.cfg['box_pos'] boxpos[2] += self.cfg['center_mass_offset'] pos = TransformState.makePos(tuple(boxpos)) self.mediator.gfx.nodepath.p3dnode.add_shape(chassis_shape, pos) car_names = [player.car for player in self._players] car_idx = car_names.index(self.cprops.name) car_bit = BitMask32.bit(BitMasks.car(car_idx)) ghost_bit = BitMask32.bit(BitMasks.ghost) track_bit = BitMask32.bit(BitMasks.track_merged) mask = car_bit | ghost_bit | track_bit self.mediator.gfx.nodepath.set_collide_mask(mask) def __set_ai_meshes(self): return # if we attach these meshes (or even only one mesh, box, sphere, # whatever) then the collision between the goal and the vehicle doesn't # work properly h = .5 boxsz = self.cfg['box_size'] hs = [] hs += [ h / 2 + boxsz[2] * 2 + self.cfg['box_pos'][2] + self.cfg['center_mass_offset'] ] hs += [ -h / 2 - boxsz[2] * 2 + self.cfg['box_pos'][2] + self.cfg['center_mass_offset'] ] for _h in hs: shape = BulletBoxShape((boxsz[0], boxsz[1], h)) ghost = GhostNode('Vehicle') pos = TransformState.makePos((0, 0, _h)) ghost.node.addShape(shape, pos) self.ai_meshes += [self.eng.attach_node(ghost.node)] car_names = self.cprops.race_props.season_props.car_names car_idx = car_names.index(self.cprops.name) car_bit = BitMask32.bit(BitMasks.car(car_idx)) ghost_bit = BitMask32.bit(BitMasks.ghost) mask = car_bit | ghost_bit self.ai_meshes[-1].set_collide_mask(mask) self.eng.phys_mgr.attach_ghost(ghost.node) def __set_phys_node(self): self.pnode = self.mediator.gfx.nodepath.p3dnode self.pnode.set_mass(self.mass) # default 0 self.pnode.set_deactivation_enabled(False) self.eng.phys_mgr.attach_rigid_body(self.pnode) self.eng.phys_mgr.add_collision_obj(self.pnode) def __set_vehicle(self): self.vehicle = BulletVehicle(self.eng.phys_mgr.root._wld, self.pnode) # access to a protected member self.vehicle.set_coordinate_system(ZUp) self.vehicle.set_pitch_control(self.pitch_control) tuning = self.vehicle.get_tuning() tuning.set_suspension_compression(self.suspension_compression) # default .83 tuning.set_suspension_damping(self.suspension_damping) # default .88 self.eng.phys_mgr.attach_vehicle(self.vehicle) def __set_wheels(self): wheels = self.mediator.gfx.wheels f_bounds = wheels['fr'].tight_bounds f_radius = (f_bounds[1][2] - f_bounds[0][2]) / 2.0 + .01 r_bounds = wheels['rr'].tight_bounds r_radius = (r_bounds[1][2] - r_bounds[0][2]) / 2.0 + .01 wheel_names = self.cprops.race_props.wheel_names ffr = self.coll_mesh.find('**/' + wheel_names.frontrear.fr) ffl = self.coll_mesh.find('**/' + wheel_names.frontrear.fl) rrr = self.coll_mesh.find('**/' + wheel_names.frontrear.rr) rrl = self.coll_mesh.find('**/' + wheel_names.frontrear.rl) meth = self.coll_mesh.find fr_node = ffr if ffr else meth('**/' + wheel_names.both.fr) fl_node = ffl if ffl else meth('**/' + wheel_names.both.fl) rr_node = rrr if rrr else meth('**/' + wheel_names.both.rr) rl_node = rrl if rrl else meth('**/' + wheel_names.both.rl) if not fr_node: # new cars fr_node = meth('**/w_fr') fl_node = meth('**/w_fl') rr_node = meth('**/w_rr') rl_node = meth('**/w_rl') offset = self.cfg['center_mass_offset'] fr_pos = fr_node.get_pos() + (0, 0, f_radius + offset) fl_pos = fl_node.get_pos() + (0, 0, f_radius + offset) rr_pos = rr_node.get_pos() + (0, 0, r_radius + offset) rl_pos = rl_node.get_pos() + (0, 0, r_radius + offset) wheels_info = [(fr_pos, True, wheels['fr'], f_radius), (fl_pos, True, wheels['fl'], f_radius), (rr_pos, False, wheels['rr'], r_radius), (rl_pos, False, wheels['rl'], r_radius)] for i, (pos, front, nodepath, radius) in enumerate(wheels_info): self.__add_wheel(pos, front, nodepath.p3dnode, radius, i) def __add_wheel(self, pos, is_front, node, radius, i): whl = self.vehicle.create_wheel() whl.set_node(node) whl.set_chassis_connection_point_cs(LPoint3f(*pos)) whl.set_front_wheel(is_front) whl.set_wheel_direction_cs((0, 0, -1)) whl.set_wheel_axle_cs((1, 0, 0)) whl.set_wheel_radius(radius) whl.set_suspension_stiffness(self.suspension_stiffness[0]) # default 5.88 whl.set_wheels_damping_relaxation(self.wheels_damping_relaxation[0]) # default .88 whl.set_wheels_damping_compression(self.wheels_damping_compression[0]) # default .83 idx = 0 if is_front else 1 whl.set_friction_slip(self.get_friction_static()[idx][0]) # default 10.5 # friction slip high -> more adherence whl.set_roll_influence(self.roll_influence[0]) # low -> more stability # default .1 whl.set_max_suspension_force(self.max_suspension_force) # default 6000 whl.set_max_suspension_travel_cm(self.max_suspension_travel_cm) # default 500 whl.set_skid_info(self.skid_info) # default 0 self.__whl2flytime[i] = 0 @property def lateral_force(self): vel = self.vehicle.get_chassis().get_linear_velocity() rot_mat = Mat4() rot_mat.setRotateMat(-90, (0, 0, 1)) car_lat = rot_mat.xformVec(self.mediator.logic.car_vec._vec) # access to a protected member proj_frc = vel.project(car_lat) return proj_frc.length() @property def lin_vel(self): return self.vehicle.get_chassis().get_linear_velocity().length() * 3.6 @property def is_flying(self): # no need to be cached rays = [whl.get_raycast_info() for whl in self.vehicle.get_wheels()] return not any(ray.is_in_contact() for ray in rays) @property def is_drifting(self): return self.lateral_force > 4.0 @property def last_drift_time(self): return self.__last_drift_time @property def prev_speed(self): return self.__prev_speed @property def prev_speed_ratio(self): return max(0, min(1.0, self.prev_speed / self.max_speed)) def on_end_frame(self): self.__prev_speed = self.speed @property def speed(self): if self.mediator.fsm.getCurrentOrNextState() == 'Countdown': return 0 # getCurrentSpeedKmHour returns odd values otherwise return self.vehicle.get_current_speed_km_hour() @property def speed_ratio(self): return max(0, min(1.0, self.speed / self.max_speed)) @property def lin_vel_ratio(self): return max(0, min(1.0, self.lin_vel / self.max_speed)) def set_forces(self, eng_frc, brake_frc, brk_ratio, steering): idx = 1 if self.mediator.logic.is_drifting else 0 eng_frc_ratio = self.engine_acc_frc_ratio[idx] self.vehicle.set_steering_value(steering, 0) self.vehicle.set_steering_value(steering, 1) self.vehicle.apply_engine_force(eng_frc * eng_frc_ratio, 0) self.vehicle.apply_engine_force(eng_frc * eng_frc_ratio, 1) self.vehicle.apply_engine_force(eng_frc * (1 - eng_frc_ratio), 2) self.vehicle.apply_engine_force(eng_frc * (1 - eng_frc_ratio), 3) self.vehicle.set_brake((1 - brk_ratio) * brake_frc, 2) self.vehicle.set_brake((1 - brk_ratio) * brake_frc, 3) self.vehicle.set_brake(brk_ratio * brake_frc, 0) self.vehicle.set_brake(brk_ratio * brake_frc, 1) def update_car_props(self): wheels = zip(self.vehicle.get_wheels(), range(4)) speeds = list(map(lambda whi: self.__update_whl_props(*whi), wheels)) speeds = [speed for speed in speeds if speed] if self.is_drifting: self.__last_drift_time = self.eng.curr_time self.curr_speed_mul = (sum(speeds) / len(speeds)) if speeds else 1.0 def __update_whl_props(self, whl, i): susp_min = self.suspension_stiffness[0] susp_max = self.suspension_stiffness[1] susp_diff = susp_max - susp_min whl.set_suspension_stiffness(susp_min + self.speed_ratio * susp_diff) relax_min = self.wheels_damping_relaxation[0] relax_max = self.wheels_damping_relaxation[1] relax_diff = relax_max - relax_min relax = relax_min + self.speed_ratio * relax_diff whl.set_wheels_damping_relaxation(relax) compr_min = self.wheels_damping_compression[0] compr_max = self.wheels_damping_compression[1] compr_diff = compr_max - compr_min compr = compr_min + self.speed_ratio * compr_diff whl.set_wheels_damping_compression(compr) roll_infl_min = self.roll_influence[0] roll_infl_max = self.roll_influence[1] roll_infl_diff = roll_infl_max - roll_infl_min roll_infl = roll_infl_min + self.speed_ratio * roll_infl_diff whl.set_roll_influence(self.roll_influence_k * roll_infl) contact_pt = whl.get_raycast_info().getContactPointWs() gnd_name = self.gnd_name(contact_pt) if not gnd_name or gnd_name in ['Vehicle', 'Wall', 'Respawn']: return if gnd_name not in self.__finds: gnd = self.cprops.race.track.phys.model.find('**/' + gnd_name) self.__finds[gnd_name] = gnd gfx_node = self.__finds[gnd_name] if not gfx_node: print('ground error', gnd_name) return fric = 1.0 if gfx_node.has_tag('friction'): fric = float(gfx_node.get_tag('friction')) if not whl.get_raycast_info().is_in_contact(): self.__whl2flytime[i] = self.eng.curr_time gnd_time = self.eng.curr_time - self.__whl2flytime[i] gnd_recovery_time = .2 if whl.is_front_wheel() else .1 gnd_factor = min(1, gnd_time / gnd_recovery_time) idx = 0 if whl.is_front_wheel() else 1 turbo_factor = 1.24 if self.turbo else 1.0 whl.setFrictionSlip(self.get_friction()[idx] * fric * gnd_factor * turbo_factor) if gfx_node.has_tag('speed'): return float(gfx_node.get_tag('speed')) @property def gnd_names(self): # no need to be cached whls = self.vehicle.get_wheels() pos = list( map(lambda whl: whl.get_raycast_info().get_contact_point_ws(), whls)) return list(map(self.gnd_name, pos)) @staticmethod def gnd_name(pos): top, bottom = pos + (0, 0, 20), pos + (0, 0, -20) result = CarPhys.eng.phys_mgr.ray_test_closest(bottom, top) ground = result.get_node() return ground.get_name() if ground else '' @staticmethod def gnd_height(pos): # this should be a method of the track top, bottom = pos + (0, 0, 20), pos + (0, 0, -20) result = CarPhys.eng.phys_mgr.ray_test_closest(bottom, top) hit_pos = result.get_hit_pos() return hit_pos.z if hit_pos else None def apply_damage(self, reset=False): # wheels = self.vehicle.get_wheels() if reset: self.max_speed = self.get_speed() self.friction_slip_k = 1.0 self.roll_influence_k = 1.0 else: self.max_speed *= .95 self.friction_slip_k *= .95 self.roll_influence_k *= 1.05 # map(lambda whl: whl.set_friction_slip(self.friction_slip), wheels) self.__log_props(False) def get_speed(self): return self.cfg['max_speed'] * (1 + .01 * self.cprops.driver_engine) def get_friction(self): k = (1 + .01 * self.cprops.driver_tires) return self.friction_slip[0] * k, self.friction_slip_rear[0] * k def get_roll_influence_static(self): min_r = self.cfg['roll_influence'][0] max_r = self.cfg['roll_influence'][1] k = 1 + .01 * self.cprops.driver_suspensions return [min_r * k, max_r * k] def get_friction_static(self): k = 1 + .01 * self.cprops.driver_tires fstr = 'friction_slip' return [(self.cfg[fstr][0] * k, self.cfg[fstr][1] * k), (self.cfg[fstr + '_rear'][0] * k, self.cfg[fstr + '_rear'][1] * k)] def get_roll_influence(self): min_r = self.cfg['roll_influence'][0] max_r = self.cfg['roll_influence'][1] diff_r = max_r - min_r curr_r = min_r + self.speed_ratio * diff_r return curr_r * (1 + .01 * self.cprops.driver_suspensions) def rotate(self): self.pnode.apply_torque((0, 0, 80000)) self.mediator.logic.applied_torque = True def destroy(self): self.eng.detach_obs(self.on_end_frame) self.eng.phys_mgr.remove_vehicle(self.vehicle) self.pnode = self.vehicle = self.__finds = self.__track_phys = \ self.coll_mesh = None PhysColleague.destroy(self)
class Vehicle(object): COUNT = 0 def __init__(self, main, username, pos = LVecBase3(-5, -5, 1), isCurrentPlayer = False, carId=3): self.username = username self.main = main self.isCurrentPlayer = isCurrentPlayer self.boostCount = 0 self.boostActive = False self.boostStep = 2 self.boostDuration = 0 self.moveStartTime = self.startTime = self.boostStartTime = time.time() self.pos = pos self.boostFactor = 1.2 self.specs = {"mass": 800.0, "maxWheelForce": 2000.0, "brakeForce": 100.0, "steeringLock": 45.0, "maxSpeed": 33.0, "maxReverseSpeed": 10.0} self.vehicleControlState = {"throttle": 0, "reverse": False, "brake": 0.0, "steering": 0.0, "health": 1} # Steering change per second, normalised to steering lock # Eg. 45 degrees lock and 1.0 rate means 45 degrees per second self.steeringRate = 0.8 self.centreingRate = 1.2 self.speed = 0 self.setupVehicle(main) self.props = VehicleProps(carId) self.currentPowerups = {"powerup1": None, "powerup2": None, "powerup3": None} if isCurrentPlayer: #This command is required for Panda to render particles base.enableParticles() self.p = ParticleEffect() # self.loadParticleConfig('steam.ptf') def setPropHealth(self, health): self.props.setHealth(health) if not self.isCurrentPlayer: self.main.updateStatusBars(self.username, self.props.health) def loadParticleConfig(self, file): #Start of the code from steam.ptf self.p.cleanup() self.p = ParticleEffect() self.p.loadConfig(Filename(file)) # print type(main.worldNp) self.p.softStart() self.p.start(self.yugoNP) # self.p.setPos(0.000, -0.700, 0.250) self.p.setPos(0.000, -0.700, 0) #self.setupVehicle(bulletWorld) self.startTime = time.time() #COUNT = 1 def move(self, steering, wheelForce, brakeForce, x, y, z, h, p, r): self.applyForcesAndSteering(steering, wheelForce, brakeForce) self.endTime = time.time() #print self.endTime elapsed = self.endTime - self.moveStartTime #self.startTime = self.endTime #if elapsed > 1: self.moveStartTime = self.endTime if not self.isCurrentPlayer: self.setVehiclePos(x, y, z, h, p, r) #print "Do Move" def applyForcesAndSteering(self, steering, wheelForce, brakeForce): # 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); def addBoost(self): if self.boostCount > 0: self.boostCount -= 1 if not self.boostActive: self.boostStartTime = time.time() self.boostActive = True self.boostDuration += self.boostStep def checkDisableBoost(self): if time.time() - self.boostStartTime > self.boostDuration: self.boostActive = False def reset(self): self.chassisNP.setP(0) self.chassisNP.setR(0) def processInput(self, inputState, dt): # print self.chassisNP.getPos() #print self.chassisNP.getH() """Use controls to update the player's car""" # For keyboard throttle and brake are either 0 or 1 self.checkDisableBoost() if inputState.isSet('forward'): self.vehicleControlState["throttle"] = 1.0 else: self.vehicleControlState["throttle"] = 0.0 velocity = self.chassisNode.getLinearVelocity() speed = math.sqrt(sum(v ** 2 for v in velocity)) self.speed = speed # Update braking and reversing if inputState.isSet('brake'): if speed < 0.5 or self.vehicleControlState["reverse"]: # If we're stopped, then start reversing # Also keep reversing if we already were self.vehicleControlState["reverse"] = True self.vehicleControlState["throttle"] = 1.0 self.vehicleControlState["brake"] = 0.0 else: self.vehicleControlState["reverse"] = False self.vehicleControlState["brake"] = 1.0 else: self.vehicleControlState["reverse"] = False self.vehicleControlState["brake"] = 0.0 # steering is normalised from -1 to 1, corresponding # to the steering lock right and left steering = self.vehicleControlState["steering"] if inputState.isSet('left'): steering += dt * self.steeringRate steering = min(steering, 1.0) elif inputState.isSet('right'): steering -= dt * self.steeringRate steering = max(steering, -1.0) else: # gradually re-center the steering if steering > 0.0: steering -= dt * self.centreingRate if steering < 0.0: steering = 0.0 elif steering < 0.0: steering += dt * self.centreingRate if steering > 0.0: steering = 0.0 self.vehicleControlState["steering"] = steering # """Updates acceleration, braking and steering # These are all passed in through a controlState dictionary # """ # Update acceleration and braking self.reversing = self.vehicleControlState["reverse"] brakeForce = self.vehicleControlState["brake"] * self.specs["brakeForce"] if self.reversing and self.speed > self.specs["maxReverseSpeed"]: self.applyForcesAndSteering(steering, 0, brakeForce) return if not self.reversing and self.speed > self.specs["maxSpeed"]: self.applyForcesAndSteering(steering, 0, brakeForce) return wheelForce = self.vehicleControlState["throttle"] * self.specs["maxWheelForce"] if self.reversing: # Make reversing a bit slower than moving forward wheelForce *= -0.5 # Update steering # Steering control state is from -1 to 1 steering = self.vehicleControlState["steering"] * self.specs["steeringLock"] if self.boostActive: wheelForce *= self.boostFactor self.applyForcesAndSteering(steering, wheelForce, brakeForce) return [steering, wheelForce, brakeForce] def getSpeed(self): velocity = self.chassisNode.getLinearVelocity() speed = math.sqrt(sum(v ** 2 for v in velocity)) return speed, speed/self.specs["maxSpeed"] def getBoost(self): maxBoost = 3.0 currentScaledBoost = self.boostCount / maxBoost return currentScaledBoost def updateHealth(self, damage): self.vehicleControlState["health"] -= 0.25 if self.vehicleControlState["health"] <= 0.0: self.killVehicle("Lost health") def killVehicle(self, reason = ""): print "Sent request to server for killing this player because: ", reason def updateMovement(self, move, dt): """Use controls to update the player's car""" # For keyboard throttle and brake are either 0 or 1 if move == 'f': self.vehicleControlState["throttle"] = 1.0 else: self.vehicleControlState["throttle"] = 0.0 velocity = self.chassisNode.getLinearVelocity() speed = math.sqrt(sum(v ** 2 for v in velocity)) # Update braking and reversing if move == 'b': if speed < 0.5 or self.vehicleControlState["reverse"]: # If we're stopped, then start reversing # Also keep reversing if we already were self.vehicleControlState["reverse"] = True self.vehicleControlState["throttle"] = 1.0 self.vehicleControlState["brake"] = 0.0 else: self.vehicleControlState["reverse"] = False self.vehicleControlState["brake"] = 1.0 else: self.vehicleControlState["reverse"] = False self.vehicleControlState["brake"] = 0.0 # steering is normalised from -1 to 1, corresponding # to the steering lock right and left steering = self.vehicleControlState["steering"] if move == 'l': steering += dt * self.steeringRate steering = min(steering, 1.0) elif move == 'r': steering -= dt * self.steeringRate steering = max(steering, -1.0) else: # gradually re-center the steering if steering > 0.0: steering -= dt * self.centreingRate if steering < 0.0: steering = 0.0 elif steering < 0.0: steering += dt * self.centreingRate if steering > 0.0: steering = 0.0 self.vehicleControlState["steering"] = steering # """Updates acceleration, braking and steering # These are all passed in through a controlState dictionary # """ # Update acceleration and braking wheelForce = self.vehicleControlState["throttle"] * self.specs["maxWheelForce"] self.reversing = self.vehicleControlState["reverse"] if self.reversing: # Make reversing a bit slower than moving forward wheelForce *= -0.5 brakeForce = self.vehicleControlState["brake"] * self.specs["brakeForce"] # Update steering # Steering control state is from -1 to 1 steering = self.vehicleControlState["steering"] * self.specs["steeringLock"] self.applyForcesAndSteering(steering, wheelForce, brakeForce) return [steering, wheelForce, brakeForce] def setVehiclePos(self, x,y, z, h, p, r): #self.chassisNP.setX(x) #self.chassisNP.setY(y) #self.chassisNP.setP(p) #self.chassisNP.setR(r) self.chassisNP.setPosHpr(x, y, z, h, p, r) return def setupVehicle(self, main): scale = 0.5 # Chassis shape = BulletBoxShape(Vec3(0.6, 1.4, 0.5)) ts = TransformState.makePos(Point3(0, 0, 0.5 * scale)) name = self.username self.chassisNode = BulletRigidBodyNode(name) self.chassisNode.setTag('username', str(name)) self.chassisNP = main.worldNP.attachNewNode(self.chassisNode) self.chassisNP.setName(str(name)) self.chassisNP.node().addShape(shape, ts) self.chassisNP.setScale(scale) self.chassisNP.setPos(self.pos) if self.isCurrentPlayer: self.chassisNP.node().notifyCollisions(True) self.chassisNP.node().setMass(800.0) else: self.chassisNP.node().notifyCollisions(True) self.chassisNP.node().setMass(400.0) self.chassisNP.node().setDeactivationEnabled(False) main.world.attachRigidBody(self.chassisNP.node()) #np.node().setCcdSweptSphereRadius(1.0) #np.node().setCcdMotionThreshold(1e-7) # Vehicle self.vehicle = BulletVehicle(main.world, self.chassisNP.node()) self.vehicle.setCoordinateSystem(ZUp) main.world.attachVehicle(self.vehicle) self.yugoNP = loader.loadModel('models/yugo/yugo.egg') self.yugoNP.reparentTo(self.chassisNP) #self.carNP = loader.loadModel('models/batmobile-chassis.egg') #self.yugoNP.setScale(.7) #self.carNP.reparentTo(self.chassisNP) # Right front wheel np = loader.loadModel('models/yugo/yugotireR.egg') np.reparentTo(main.worldNP) self.addWheel(Point3( 0.70 * scale, 1.05 * scale, 0.3), True, np) # Left front wheel np = loader.loadModel('models/yugo/yugotireL.egg') np.reparentTo(main.worldNP) self.addWheel(Point3(-0.70 * scale, 1.05 * scale, 0.3), True, np) # Right rear wheel np = loader.loadModel('models/yugo/yugotireR.egg') np.reparentTo(main.worldNP) self.addWheel(Point3( 0.70 * scale, -1.05 * scale, 0.3), False, np) # Left rear wheel np = loader.loadModel('models/yugo/yugotireL.egg') np.reparentTo(main.worldNP) self.addWheel(Point3(-0.70 * scale, -1.05 * scale, 0.3), False, np) def addWheel(self, pos, front, np): wheel = self.vehicle.createWheel() wheel.setNode(np.node()) wheel.setChassisConnectionPointCs(pos) wheel.setFrontWheel(front) wheel.setWheelDirectionCs(Vec3(0, 0, -1)) wheel.setWheelAxleCs(Vec3(1, 0, 0)) wheel.setWheelRadius(0.25) wheel.setMaxSuspensionTravelCm(40.0) wheel.setSuspensionStiffness(40.0) wheel.setWheelsDampingRelaxation(2.3) wheel.setWheelsDampingCompression(4.4) wheel.setFrictionSlip(100.0); wheel.setRollInfluence(0.1) def reset(self): #self.chassisNP.setP(0) #self.chassisNP.setR(0) #print "kegwe", self.chassisNP.getX(),self.chassisNP.getY(),self.chassisNP.getZ(),self.chassisNP.getH(),0,0 self.chassisNP.setPosHpr(self.chassisNP.getX(),self.chassisNP.getY(),self.chassisNP.getZ(),self.chassisNP.getH(),0,0) def pickedPowerup(self, powerup): if not powerup.pickable: powerup.useAbility() else: if self.currentPowerups["powerup1"] is None: self.currentPowerups["powerup1"] = powerup elif self.currentPowerups["powerup2"] is None: self.currentPowerups["powerup2"] = powerup elif self.currentPowerups["powerup3"] is None: self.currentPowerups["powerup3"] = powerup def canPickUpPowerup(self): return (self.currentPowerups["powerup1"] is None or self.currentPowerups["powerup2"] is None or self.currentPowerups["powerup3"] is None) def usePowerup(self, powerupIndex): # Move usePowerupN to this function if powerupIndex == 0 and self.currentPowerups["powerup1"] is not None: self.currentPowerups["powerup1"].useAbility() self.currentPowerups["powerup1"] = None elif powerupIndex == 1 and self.currentPowerups["powerup2"] is not None: self.currentPowerups["powerup2"].useAbility() self.currentPowerups["powerup2"] = None elif powerupIndex == 2 and self.currentPowerups["powerup3"] is not None: self.currentPowerups["powerup3"].useAbility() self.currentPowerups["powerup3"] = None
def setup(self): self.worldNP = render.attachNewNode('World') # World self.debugNP = self.worldNP.attachNewNode(BulletDebugNode('Debug')) self.debugNP.show() self.world = BulletWorld() self.world.setGravity(Vec3(0, 0, -9.81)) self.world.setDebugNode(self.debugNP.node()) # Plane shape = BulletPlaneShape(Vec3(0, 0, 1), 0) np = self.worldNP.attachNewNode(BulletRigidBodyNode('Ground')) np.node().addShape(shape) np.setPos(0, 0, -1) np.setCollideMask(BitMask32.allOn()) self.world.attachRigidBody(np.node()) # Chassis shape = BulletBoxShape(Vec3(0.6, 1.4, 0.5)) ts = TransformState.makePos(Point3(0, 0, 0.5)) np = self.worldNP.attachNewNode(BulletRigidBodyNode('Vehicle')) np.node().addShape(shape, ts) np.setPos(0, 0, 1) np.node().setMass(800.0) np.node().setDeactivationEnabled(False) self.world.attachRigidBody(np.node()) #np.node().setCcdSweptSphereRadius(1.0) #np.node().setCcdMotionThreshold(1e-7) # Vehicle self.vehicle = BulletVehicle(self.world, np.node()) self.vehicle.setCoordinateSystem(ZUp) self.world.attachVehicle(self.vehicle) self.yugoNP = loader.loadModel('models/yugo/yugo.egg') self.yugoNP.reparentTo(np) # Right front wheel np = loader.loadModel('models/yugo/yugotireR.egg') np.reparentTo(self.worldNP) self.addWheel(Point3( 0.70, 1.05, 0.3), True, np) # Left front wheel np = loader.loadModel('models/yugo/yugotireL.egg') np.reparentTo(self.worldNP) self.addWheel(Point3(-0.70, 1.05, 0.3), True, np) # Right rear wheel np = loader.loadModel('models/yugo/yugotireR.egg') np.reparentTo(self.worldNP) self.addWheel(Point3( 0.70, -1.05, 0.3), False, np) # Left rear wheel np = loader.loadModel('models/yugo/yugotireL.egg') np.reparentTo(self.worldNP) self.addWheel(Point3(-0.70, -1.05, 0.3), False, np) # Steering info self.steering = 0.0 # degree self.steeringClamp = 45.0 # degree self.steeringIncrement = 120.0 # degree per second
class CarPhys(Phys): def __init__(self, mdt, carphys_props): Phys.__init__(self, mdt) self.pnode = None self.vehicle = None self.curr_speed_factor = 1.0 self.__prev_speed = 0 self.__finds = {} # cache for find's results self.props = carphys_props self._load_phys() self.__set_collision_mesh() self.__set_phys_node() self.__set_vehicle() self.__set_wheels() eng.attach_obs(self.on_end_frame) def _load_phys(self): fpath = self.props.phys_file % self.mdt.name with open(fpath) as phys_file: # pass phys props as a class self.cfg = load(phys_file) self.cfg['max_speed'] = self.get_speed() self.cfg['friction_slip'] = self.get_friction() self.cfg['roll_influence'] = self.get_roll_influence() s_a = (self.mdt.name, round(self.cfg['max_speed'], 2), self.props.driver_engine) LogMgr().log('speed %s: %s (%s)' % s_a) fr_slip = round(self.cfg['friction_slip'], 2) f_a = (self.mdt.name, fr_slip, self.props.driver_tires) LogMgr().log('friction %s: %s (%s)' % f_a) r_a = (self.mdt.name, round(self.cfg['roll_influence'], 2), self.props.driver_suspensions) LogMgr().log('roll %s: %s (%s)' % r_a) s_a = lambda field: setattr(self, field, self.cfg[field]) map(s_a, self.cfg.keys()) def __set_collision_mesh(self): fpath = self.props.coll_path % self.mdt.name self.coll_mesh = loader.loadModel(fpath) chassis_shape = BulletConvexHullShape() for geom in PhysMgr().find_geoms(self.coll_mesh, self.props.coll_name): chassis_shape.addGeom(geom.node().getGeom(0), geom.getTransform()) self.mdt.gfx.nodepath.node().addShape(chassis_shape) self.mdt.gfx.nodepath.setCollideMask( BitMask32.bit(1) | BitMask32.bit(2 + self.props.cars.index(self.mdt.name))) #nodepath = self.mdt.gfx.nodepath.attachNewNode(BulletGhostNode('car ghost')) #nodepath.node().addShape(BulletCapsuleShape(4, 5, ZUp)) #eng.attach_ghost(nodepath.node()) #nodepath.node().notifyCollisions(False) def __set_phys_node(self): self.pnode = self.mdt.gfx.nodepath.node() self.pnode.setMass(self.mass) self.pnode.setDeactivationEnabled(False) PhysMgr().attach_rigid_body(self.pnode) PhysMgr().add_collision_obj(self.pnode) def __set_vehicle(self): self.vehicle = BulletVehicle(PhysMgr().root, self.pnode) self.vehicle.setCoordinateSystem(ZUp) self.vehicle.setPitchControl(self.pitch_control) tuning = self.vehicle.getTuning() tuning.setSuspensionCompression(self.suspension_compression) tuning.setSuspensionDamping(self.suspension_damping) PhysMgr().attach_vehicle(self.vehicle) def __set_wheels(self): fwheel_bounds = self.mdt.gfx.wheels['fr'].get_tight_bounds() f_radius = (fwheel_bounds[1][2] - fwheel_bounds[0][2]) / 2.0 + .01 rwheel_bounds = self.mdt.gfx.wheels['rr'].get_tight_bounds() r_radius = (rwheel_bounds[1][2] - rwheel_bounds[0][2]) / 2.0 + .01 ffr = self.coll_mesh.find('**/' + self.props.wheel_names[0][0]) ffl = self.coll_mesh.find('**/' + self.props.wheel_names[0][1]) rrr = self.coll_mesh.find('**/' + self.props.wheel_names[0][2]) rrl = self.coll_mesh.find('**/' + self.props.wheel_names[0][3]) meth = self.coll_mesh.find fr_node = ffr if ffr else meth('**/' + self.props.wheel_names[1][0]) fl_node = ffl if ffl else meth('**/' + self.props.wheel_names[1][1]) rr_node = rrr if rrr else meth('**/' + self.props.wheel_names[1][2]) rl_node = rrl if rrl else meth('**/' + self.props.wheel_names[1][3]) wheel_fr_pos = fr_node.get_pos() + (0, 0, f_radius) wheel_fl_pos = fl_node.get_pos() + (0, 0, f_radius) wheel_rr_pos = rr_node.get_pos() + (0, 0, r_radius) wheel_rl_pos = rl_node.get_pos() + (0, 0, r_radius) frw = self.mdt.gfx.wheels['fr'] flw = self.mdt.gfx.wheels['fl'] rrw = self.mdt.gfx.wheels['rr'] rlw = self.mdt.gfx.wheels['rl'] wheels_info = [(wheel_fr_pos, True, frw, f_radius), (wheel_fl_pos, True, flw, f_radius), (wheel_rr_pos, False, rrw, r_radius), (wheel_rl_pos, False, rlw, r_radius)] for (pos, front, nodepath, radius) in wheels_info: self.__add_wheel(pos, front, nodepath.node(), radius) def __add_wheel(self, pos, front, node, radius): whl = self.vehicle.createWheel() whl.setNode(node) whl.setChassisConnectionPointCs(LPoint3f(*pos)) whl.setFrontWheel(front) whl.setWheelDirectionCs((0, 0, -1)) whl.setWheelAxleCs((1, 0, 0)) whl.setWheelRadius(radius) whl.setSuspensionStiffness(self.suspension_stiffness) whl.setWheelsDampingRelaxation(self.wheels_damping_relaxation) whl.setWheelsDampingCompression(self.wheels_damping_compression) whl.setFrictionSlip(self.friction_slip) # high -> more adherence whl.setRollInfluence(self.roll_influence) # low -> more stability whl.setMaxSuspensionForce(self.max_suspension_force) whl.setMaxSuspensionTravelCm(self.max_suspension_travel_cm) whl.setSkidInfo(self.skid_info) @property def lateral_force(self): vel = self.vehicle.get_chassis().get_linear_velocity() vel.normalize() dir = self.mdt.logic.car_vec lat = dir.dot(vel) lat_force = 0 if lat > .5: lat_force = min(1, (lat - 1.0) / -.2) return lat_force @property def is_flying(self): # no need to be cached rays = [whl.getRaycastInfo() for whl in self.vehicle.get_wheels()] return not any(ray.isInContact() for ray in rays) @property def prev_speed(self): return self.__prev_speed @property def prev_speed_ratio(self): return max(0, min(1.0, self.prev_speed / self.max_speed)) def on_end_frame(self): self.__prev_speed = self.speed @property def speed(self): if self.mdt.fsm.getCurrentOrNextState() == 'Countdown': return 0 # getCurrentSpeedKmHour returns odd values otherwise return self.vehicle.getCurrentSpeedKmHour() @property def speed_ratio(self): return max(0, min(1.0, self.speed / self.max_speed)) def set_forces(self, eng_frc, brake_frc, steering): self.vehicle.setSteeringValue(steering, 0) self.vehicle.setSteeringValue(steering, 1) self.vehicle.applyEngineForce(eng_frc, 2) self.vehicle.applyEngineForce(eng_frc, 3) self.vehicle.setBrake(brake_frc, 2) self.vehicle.setBrake(brake_frc, 3) def update_car_props(self): speeds = [] for whl in self.vehicle.get_wheels(): contact_pt = whl.get_raycast_info().getContactPointWs() gnd_name = self.gnd_name(contact_pt) if not gnd_name: continue if gnd_name not in self.__finds: gnd = self.props.track_phys.find('**/' + gnd_name) self.__finds[gnd_name] = gnd gfx_node = self.__finds[gnd_name] if gfx_node.has_tag('speed'): speeds += [float(gfx_node.get_tag('speed'))] if gfx_node.has_tag('friction'): fric = float(gfx_node.get_tag('friction')) whl.setFrictionSlip(self.friction_slip * fric) self.curr_speed_factor = (sum(speeds) / len(speeds)) if speeds else 1.0 @property def gnd_names(self): # no need to be cached whls = self.vehicle.get_wheels() pos = map(lambda whl: whl.get_raycast_info().getContactPointWs(), whls) return map(self.gnd_name, pos) @staticmethod def gnd_name(pos): top = pos + (0, 0, 20) bottom = pos + (0, 0, -20) result = PhysMgr().ray_test_closest(bottom, top) ground = result.get_node() return ground.get_name() if ground else '' def apply_damage(self, reset=False): if reset: self.max_speed = self.get_speed() self.friction_slip = self.get_friction() self.roll_influence = self.get_roll_influence() else: self.max_speed *= .95 self.friction_slip *= .95 self.roll_influence *= 1.05 fric = lambda whl: whl.setFrictionSlip(self.friction_slip) map(fric, self.vehicle.get_wheels()) roll = lambda whl: whl.setRollInfluence(self.roll_influence) map(roll, self.vehicle.get_wheels()) s_a = (str(round(self.max_speed, 2)), self.props.driver_engine) LogMgr().log('speed: %s (%s)' % s_a) f_a = (str(round(self.friction_slip, 2)), self.props.driver_tires) LogMgr().log('friction: %s (%s)' % f_a) r_a = (str(round(self.roll_influence, 2)), self.props.driver_suspensions) LogMgr().log('roll: %s (%s)' % r_a) def get_speed(self): return self.cfg['max_speed'] * (1 + .01 * self.props.driver_engine) def get_friction(self): return self.cfg['friction_slip'] * (1 + .01 * self.props.driver_tires) def get_roll_influence(self): return self.cfg['roll_influence'] * ( 1 + .01 * self.props.driver_suspensions) def destroy(self): eng.detach_obs(self.on_end_frame) eng.remove_vehicle(self.vehicle) self.pnode = self.vehicle = self.__finds = self.__track_phys = \ self.coll_mesh = None Phys.destroy(self)
class BaseVehicle(DynamicElement): Ego_state_obs_dim = 9 """ Vehicle chassis and its wheels index 0 1 II-----II | | <---chassis | II-----II 2 3 """ PARAMETER_SPACE = PGSpace( VehicleParameterSpace.BASE_VEHICLE ) # it will not sample config from parameter space COLLISION_MASK = CollisionGroup.EgoVehicle STEERING_INCREMENT = 0.05 default_vehicle_config = PGConfig( dict( lidar=(240, 50, 4), # laser num, distance, other vehicle info num mini_map=(84, 84, 250), # buffer length, width rgb_cam=(160, 120), # buffer length, width depth_cam=(84, 84, True), # buffer length, width, view_ground show_navi_mark=False, increment_steering=False, wheel_friction=0.6, )) born_place = (5, 0) LENGTH = None WIDTH = None def __init__(self, pg_world: PGWorld, vehicle_config: dict = None, random_seed: int = 0, config: dict = None): """ This Vehicle Config is different from self.get_config(), and it is used to define which modules to use, and module parameters. """ super(BaseVehicle, self).__init__(random_seed) self.pg_world = pg_world self.node_path = NodePath("vehicle") # config info self.set_config(self.PARAMETER_SPACE.sample()) if config is not None: self.set_config(config) self.vehicle_config = self.get_vehicle_config( vehicle_config ) if vehicle_config is not None else self.default_vehicle_config self.increment_steering = self.vehicle_config["increment_steering"] self.max_speed = self.get_config()[Parameter.speed_max] self.max_steering = self.get_config()[Parameter.steering_max] # create self._add_chassis(pg_world.physics_world) self.wheels = self._create_wheel() # modules self.image_sensors = {} self.lidar = None self.routing_localization = None self.lane = None self.lane_index = None self.vehicle_panel = VehiclePanel(self.pg_world) if ( self.pg_world.mode == RENDER_MODE_ONSCREEN) else None # other info self.throttle_brake = 0.0 self.steering = 0 self.last_current_action = deque([(0.0, 0.0), (0.0, 0.0)], maxlen=2) self.last_position = self.born_place self.last_heading_dir = self.heading # collision info render self.collision_info_np = self._init_collision_info_render(pg_world) self.collision_banners = {} # to save time self.current_banner = None # done info self.crash = False self.out_of_road = False self.attach_to_pg_world(self.pg_world.pbr_render, self.pg_world.physics_world) @classmethod def get_vehicle_config(cls, new_config): default = copy.deepcopy(cls.default_vehicle_config) default.update(new_config) return default def prepare_step(self, action): """ Save info and make decision before action """ self.last_position = self.position self.last_heading_dir = self.heading self.last_current_action.append( action ) # the real step of physics world is implemented in taskMgr.step() if self.increment_steering: self.set_incremental_action(action) else: self.set_act(action) if self.vehicle_panel is not None: self.vehicle_panel.renew_2d_car_para_visualization(self) def update_state(self, pg_world=None): if self.lidar is not None: self.lidar.perceive(self.position, self.heading_theta, self.pg_world.physics_world) if self.routing_localization is not None: self.lane, self.lane_index = self.routing_localization.update_navigation_localization( self) return self._state_check() def reset(self, map: Map, pos: np.ndarray, heading: float): """ pos is a 2-d array, and heading is a float (unit degree) """ heading = -np.deg2rad(heading) - np.pi / 2 self.chassis_np.setPos(Vec3(*pos, 1)) self.chassis_np.setQuat( LQuaternionf(np.cos(heading / 2), 0, 0, np.sin(heading / 2))) self.update_map_info(map) self.chassis_np.node().clearForces() self.chassis_np.node().setLinearVelocity(Vec3(0, 0, 0)) self.chassis_np.node().setAngularVelocity(Vec3(0, 0, 0)) self.system.resetSuspension() # done info self.crash = False self.out_of_road = False # other info self.throttle_brake = 0.0 self.steering = 0 self.last_current_action = deque([(0.0, 0.0), (0.0, 0.0)], maxlen=2) self.last_position = self.born_place self.last_heading_dir = self.heading if "depth_cam" in self.image_sensors and self.image_sensors[ "depth_cam"].view_ground: for block in map.blocks: block.node_path.hide(CamMask.DepthCam) """------------------------------------------- act -------------------------------------------------""" def set_act(self, action): para = self.get_config() steering = action[0] self.throttle_brake = action[1] self.steering = steering self.system.setSteeringValue( self.steering * para[Parameter.steering_max], 0) self.system.setSteeringValue( self.steering * para[Parameter.steering_max], 1) self._apply_throttle_brake(action[1]) def set_incremental_action(self, action: np.ndarray): self.throttle_brake = action[1] self.steering += action[0] * self.STEERING_INCREMENT self.steering = clip(self.steering, -1, 1) steering = self.steering * self.max_steering self.system.setSteeringValue(steering, 0) self.system.setSteeringValue(steering, 1) self._apply_throttle_brake(action[1]) def _apply_throttle_brake(self, throttle_brake): para = self.get_config() max_engine_force = para[Parameter.engine_force_max] max_brake_force = para[Parameter.brake_force_max] for wheel_index in range(4): if throttle_brake >= 0: self.system.setBrake(2.0, wheel_index) if self.speed > self.max_speed: self.system.applyEngineForce(0.0, wheel_index) else: self.system.applyEngineForce( max_engine_force * throttle_brake, wheel_index) else: self.system.applyEngineForce(0.0, wheel_index) self.system.setBrake( abs(throttle_brake) * max_brake_force, wheel_index) """---------------------------------------- vehicle info ----------------------------------------------""" @property def position(self): return pgdrive_position(self.chassis_np.getPos()) @property def speed(self): """ km/h """ speed = self.chassis_np.node().get_linear_velocity().length() * 3.6 return clip(speed, 0.0, 100000.0) @property def heading(self): real_heading = self.heading_theta heading = np.array([np.cos(real_heading), np.sin(real_heading)]) return heading @property def heading_theta(self): """ Get the heading theta of vehicle, unit [rad] :return: heading in rad """ return (pgdrive_heading(self.chassis_np.getH()) - 90) / 180 * math.pi @property def velocity(self) -> np.ndarray: return self.speed * self.velocity_direction @property def velocity_direction(self): direction = self.system.get_forward_vector() return np.asarray([direction[0], -direction[1]]) @property def forward_direction(self): raise ValueError("This function id deprecated.") # print("This function id deprecated.") # direction = self.vehicle.get_forward_vector() # return np.array([direction[0], -direction[1]]) @property def current_road(self): return self.lane_index[0:-1] """---------------------------------------- some math tool ----------------------------------------------""" def heading_diff(self, target_lane): lateral = None if isinstance(target_lane, StraightLane): lateral = np.asarray( get_vertical_vector(target_lane.end - target_lane.start)[1]) elif isinstance(target_lane, CircularLane): if target_lane.direction == -1: lateral = self.position - target_lane.center else: lateral = target_lane.center - self.position else: raise ValueError("Unknown target lane type: {}".format( type(target_lane))) lateral_norm = norm(lateral[0], lateral[1]) forward_direction = self.heading # print(f"Old forward direction: {self.forward_direction}, new heading {self.heading}") forward_direction_norm = norm(forward_direction[0], forward_direction[1]) if not lateral_norm * forward_direction_norm: return 0 # cos = self.forward_direction.dot(lateral) / (np.linalg.norm(lateral) * np.linalg.norm(self.forward_direction)) cos = ((forward_direction[0] * lateral[0] + forward_direction[1] * lateral[1]) / (lateral_norm * forward_direction_norm)) # return cos # Normalize to 0, 1 return clip(cos, -1.0, 1.0) / 2 + 0.5 def projection(self, vector): # Projected to the heading of vehicle # forward_vector = self.vehicle.get_forward_vector() # forward_old = (forward_vector[0], -forward_vector[1]) forward = self.heading # print(f"[projection] Old forward {forward_old}, new heading {forward}") norm_velocity = norm(forward[0], forward[1]) + 1e-6 project_on_heading = (vector[0] * forward[0] + vector[1] * forward[1]) / norm_velocity side_direction = get_vertical_vector(forward)[1] side_norm = norm(side_direction[0], side_direction[1]) + 1e-6 project_on_side = (vector[0] * side_direction[0] + vector[1] * side_direction[1]) / side_norm return project_on_heading, project_on_side def lane_distance_to(self, vehicle, lane: AbstractLane = None) -> float: assert self.routing_localization is not None, "a routing and localization module should be added " \ "to interact with other vehicles" if not vehicle: return np.nan if not lane: lane = self.lane return lane.local_coordinates( vehicle.position)[0] - lane.local_coordinates(self.position)[0] """-------------------------------------- for vehicle making ------------------------------------------""" 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 _create_wheel(self): para = self.get_config() f_l = para[Parameter.front_tire_longitude] r_l = -para[Parameter.rear_tire_longitude] lateral = para[Parameter.tire_lateral] axis_height = para[Parameter.tire_radius] + 0.05 radius = para[Parameter.tire_radius] wheels = [] for k, pos in enumerate([ Vec3(lateral, f_l, axis_height), Vec3(-lateral, f_l, axis_height), Vec3(lateral, r_l, axis_height), Vec3(-lateral, r_l, axis_height) ]): wheel = self._add_wheel(pos, radius, True if k < 2 else False, True if k == 0 or k == 2 else False) wheels.append(wheel) return wheels def _add_wheel(self, pos: Vec3, radius: float, front: bool, left): wheel_np = self.node_path.attachNewNode("wheel") if self.render: model_path = 'models/yugo/yugotireR.egg' if left else 'models/yugo/yugotireL.egg' wheel_model = self.loader.loadModel( AssetLoader.file_path(model_path)) wheel_model.reparentTo(wheel_np) wheel_model.set_scale(1.4, radius / 0.25, radius / 0.25) wheel = self.system.create_wheel() wheel.setNode(wheel_np.node()) wheel.setChassisConnectionPointCs(pos) wheel.setFrontWheel(front) wheel.setWheelDirectionCs(Vec3(0, 0, -1)) wheel.setWheelAxleCs(Vec3(1, 0, 0)) # TODO add them to PGConfig in the future wheel.setWheelRadius(radius) wheel.setMaxSuspensionTravelCm(40) wheel.setSuspensionStiffness(30) wheel.setWheelsDampingRelaxation(4.8) wheel.setWheelsDampingCompression(1.2) wheel.setFrictionSlip(self.vehicle_config["wheel_friction"]) wheel.setRollInfluence(1.5) return wheel def add_image_sensor(self, name: str, sensor: ImageBuffer): self.image_sensors[name] = sensor def add_lidar(self, laser_num=240, distance=50): self.lidar = Lidar(self.pg_world.render, laser_num, distance) def add_routing_localization(self, show_navi_point: bool): self.routing_localization = RoutingLocalizationModule( self.pg_world, show_navi_point) def update_map_info(self, map): """ Update map info after reset() :param map: new map :return: None """ self.routing_localization.update(map) lane, new_l_index = ray_localization((self.born_place), self.pg_world) assert lane is not None, "Born place is not on road!" self.lane_index = new_l_index self.lane = lane def _state_check(self): """ Check States and filter to update info """ result = self.pg_world.physics_world.dynamic_world.contactTest( self.chassis_beneath_np.node(), True) contacts = set() for contact in result.getContacts(): node0 = contact.getNode0() node1 = contact.getNode1() name = [node0.getName(), node1.getName()] name.remove(BodyName.Ego_vehicle) if name[0] == "Ground" or name[0] == BodyName.Lane: continue elif name[0] == BodyName.Side_walk: self.out_of_road = True contacts.add(name[0]) if self.render: self.render_collision_info(contacts) return contacts def _collision_check(self, contact): """ It may lower the performance if overdone """ node0 = contact.getNode0().getName() node1 = contact.getNode1().getName() name = [node0, node1] name.remove(BodyName.Ego_vehicle_top) if name[0] == BodyName.Traffic_vehicle: self.crash = True logging.debug("Crash with {}".format(name[0])) def _init_collision_info_render(self, pg_world): if pg_world.mode == "onscreen": info_np = NodePath("Collision info nodepath") info_np.reparentTo(pg_world.aspect2d) else: info_np = None return info_np def render_collision_info(self, contacts): contacts = sorted(list(contacts), key=lambda c: COLLISION_INFO_COLOR[COLOR[c]][0]) text = contacts[0] if len(contacts) != 0 else None if text is None: text = "Normal" if time.time( ) - self.pg_world._episode_start_time > 10 else "Press H to see help message" self.render_banner(text, COLLISION_INFO_COLOR["green"][1]) else: self.render_banner(text, COLLISION_INFO_COLOR[COLOR[text]][1]) def render_banner(self, text, color=COLLISION_INFO_COLOR["green"][1]): """ Render the banner in the left bottom corner. """ if self.collision_info_np is None: return if self.current_banner is not None: self.current_banner.detachNode() if text in self.collision_banners: self.collision_banners[text].reparentTo(self.collision_info_np) self.current_banner = self.collision_banners[text] else: new_banner = NodePath(TextNode("collision_info:{}".format(text))) self.collision_banners[text] = new_banner text_node = new_banner.node() text_node.setCardColor(color) text_node.setText(text) text_node.setCardActual(-5 * self.pg_world.w_scale, 5.1 * self.pg_world.w_scale, -0.3, 1) text_node.setCardDecal(True) text_node.setTextColor(1, 1, 1, 1) text_node.setAlign(TextNode.A_center) new_banner.setScale(0.05) new_banner.setPos(-0.75 * self.pg_world.w_scale, 0, -0.8 * self.pg_world.h_scale) new_banner.reparentTo(self.collision_info_np) self.current_banner = new_banner def destroy(self, _=None): self.dynamic_nodes.remove(self.chassis_np.node()) super(BaseVehicle, self).destroy(self.pg_world) self.pg_world.physics_world.dynamic_world.clearContactAddedCallback() self.routing_localization.destroy() self.routing_localization = None if self.lidar is not None: self.lidar.destroy() self.lidar = None if len(self.image_sensors) != 0: for sensor in self.image_sensors.values(): sensor.destroy(self.pg_world) self.image_sensors = None if self.vehicle_panel is not None: self.vehicle_panel.destroy(self.pg_world) self.pg_world = None def set_position(self, position): """ Should only be called when restore traffic from episode data :param position: 2d array or list :return: None """ self.chassis_np.setPos(panda_position(position, 0.4)) def set_heading(self, heading_theta) -> None: """ Should only be called when restore traffic from episode data :param heading_theta: float in rad :return: None """ self.chassis_np.setH((panda_heading(heading_theta) * 180 / np.pi) - 90) def get_state(self): return { "heading": self.heading_theta, "position": self.position.tolist(), "done": self.crash or self.out_of_road } def set_state(self, state: dict): self.set_heading(state["heading"]) self.set_position(state["position"]) def __del__(self): super(BaseVehicle, self).__del__() self.pg_world = None self.lidar = None self.mini_map = None self.rgb_cam = None self.routing_localization = None self.wheels = None
class CarSrv(DirectObject): def __init__(self): base.setBackgroundColor(0.1, 0.1, 0.8, 1) assert ("offscreen" == base.config.GetString("window-type", "offscreen")) # Light alight = AmbientLight('ambientLight') alight.setColor(Vec4(0.5, 0.5, 0.5, 1)) alightNP = render.attachNewNode(alight) dlight = DirectionalLight('directionalLight') dlight.setDirection(Vec3(1, 1, -1)) dlight.setColor(Vec4(0.7, 0.7, 0.7, 1)) dlightNP = render.attachNewNode(dlight) render.clearLight() render.setLight(alightNP) render.setLight(dlightNP) # Input self.accept('escape', self.doExit) self.accept('r', self.doReset) self.accept('f1', self.toggleWireframe) self.accept('f2', self.toggleTexture) self.accept('f3', self.toggleDebug) self.accept('f5', self.doScreenshot) self.params = rospy.get_param('~sim') # Car Simulator self.dt = rospy.get_param('~dt') self.setup() self.load_vehicle() # ROS self.crash_pub = rospy.Publisher('crash', std_msgs.msg.Empty, queue_size=1) self.bridge = cv_bridge.CvBridge() # taskMgr.add(self.update, 'updateWorld') self.start_update_server() # _____HANDLER_____ def doExit(self): self.cleanup() sys.exit(1) def doReset(self, pos=None, quat=None): self.cleanup() self.setup() if pos is None or quat is None: self.load_vehicle() else: self.load_vehicle(pos=pos, quat=quat) def toggleWireframe(self): base.toggleWireframe() def toggleTexture(self): base.toggleTexture() def toggleDebug(self): if self.debugNP.isHidden(): self.debugNP.show() else: self.debugNP.hide() def doScreenshot(self): base.screenshot('Bullet') def get_ros_image(self, cv_image, image_format="rgb8"): return self.bridge.cv2_to_imgmsg(cv_image, image_format) def get_handler(self): def sim_env_handler(req): start_time = time.time() cmd_steer = req.steer motor = req.motor vel = req.vel reset = req.reset pose = req.pose # If motor is default then use velocity if motor == 0.0: cmd_motor = numpy.clip(vel * 3 + 49.5, 0., 99.) else: cmd_motor = numpy.clip(motor, 0., 99.) self.steering = self.steeringClamp * \ ((cmd_steer - 49.5) / 49.5) self.engineForce = self.engineClamp * \ ((cmd_motor - 49.5) / 49.5) if reset: self.steering = 0.0 # degree self.engineForce = 0.0 pos = pose.position.x, pose.position.y, pose.position.z quat = pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w if numpy.all(numpy.array(pos) == 0.): self.doReset() else: self.doReset(pos=pos, quat=quat) self.vehicle.setSteeringValue(self.steering, 0) self.vehicle.setSteeringValue(self.steering, 1) self.vehicle.setBrake(100.0, 2) self.vehicle.setBrake(100.0, 3) self.vehicle.applyEngineForce(self.engineForce, 2) self.vehicle.applyEngineForce(self.engineForce, 3) pos = numpy.array(self.vehicle_pointer.getPos()) np_quat = self.vehicle_pointer.getQuat() quat = numpy.array(np_quat) self.previous_pos = pos self.previous_quat = np_quat # TODO maybe change number of timesteps self.world.doPhysics(self.dt, 10, 0.05) # Collision detection result = self.world.contactTest(self.vehicle_node) collision = result.getNumContacts() > 0 if collision: # TODO figure out why this causes problems # self.crash_pub.publish(std_msgs.msg.Empty()) self.doReset(pos=self.previous_pos, quat=self.previous_quat) state = geometry_msgs.msg.Pose() pos = numpy.array(self.vehicle_pointer.getPos()) np_quat = self.vehicle_pointer.getQuat() quat = numpy.array(np_quat) self.previous_pos = pos self.previous_quat = np_quat state.position.x, state.position.y, state.position.z = pos state.orientation.x, state.orientation.y, \ state.orientation.z, state.orientation.w = quat # Get observation obs = self.camera_sensor.observe() back_obs = self.back_camera_sensor.observe() cam_image = self.get_ros_image(obs[0]) cam_depth = self.get_ros_image(obs[1], image_format="passthrough") self.camera_pub.publish_image(obs[0]) self.depth_pub.publish_image(obs[1], image_format="passthrough") back_cam_image = self.get_ros_image(back_obs[0]) back_cam_depth = self.get_ros_image(back_obs[1], image_format="passthrough") self.back_camera_pub.publish_image(back_obs[0]) self.back_depth_pub.publish_image(back_obs[1], image_format="passthrough") return [ collision, cam_image, cam_depth, back_cam_image, back_cam_depth, state ] return sim_env_handler def load_vehicle(self, pos=(0.0, -20.0, -0.6), quat=None): # Chassis self._mass = self.params['mass'] #chassis_shape = self.params['chassis_shape'] shape = BulletBoxShape(Vec3(0.6, 1.4, 0.5)) ts = TransformState.makePos(Point3(0, 0, 0.5)) self.vehicle_pointer = self.worldNP.attachNewNode( BulletRigidBodyNode('Vehicle')) self.vehicle_node = self.vehicle_pointer.node() self.vehicle_node.addShape(shape, ts) self.previous_pos = pos self.vehicle_pointer.setPos(pos[0], pos[1], pos[2]) if quat is not None: self.vehicle_pointer.setQuat(quat) self.previous_quat = self.vehicle_pointer.getQuat() self.vehicle_node.setMass(self._mass) self.vehicle_node.setDeactivationEnabled(False) # first_person = self.params['first_person'] self.camera_sensor = Panda3dCameraSensor(base, color=True, depth=True, size=(160, 90)) self.camera_node = self.camera_sensor.cam # if first_person: # self.camera_node.setPos(0.0, 1.0, 1.0) # self.camera_node.lookAt(0.0, 6.0, 0.0) # else: # self.camera_node.setPos(0.0, -10.0, 5.0) # self.camera_node.lookAt(0.0, 5.0, 0.0) self.camera_node.reparentTo(self.vehicle_pointer) self.camera_node.setPos(0.0, 1.0, 1.0) self.camera_node.lookAt(0.0, 6.0, 0.0) self.back_camera_sensor = Panda3dCameraSensor(base, color=True, depth=True, size=(160, 90)) self.back_camera_node = self.back_camera_sensor.cam # if first_person: # self.camera_node.setPos(0.0, 1.0, 1.0) # self.camera_node.lookAt(0.0, 6.0, 0.0) # else: # self.camera_node.setPos(0.0, -10.0, 5.0) # self.camera_node.lookAt(0.0, 5.0, 0.0) self.back_camera_node.reparentTo(self.vehicle_pointer) self.back_camera_node.setPos(0.0, -1.0, 1.0) self.back_camera_node.lookAt(0.0, -6.0, 0.0) self.world.attachRigidBody(self.vehicle_node) self.vehicle_node.setCcdSweptSphereRadius(1.0) self.vehicle_node.setCcdMotionThreshold(1e-7) # Vehicle self.vehicle = BulletVehicle(self.world, self.vehicle_node) self.vehicle.setCoordinateSystem(ZUp) self.world.attachVehicle(self.vehicle) self.yugoNP = loader.loadModel('../models/yugo/yugo.egg') self.yugoNP.reparentTo(self.vehicle_pointer) self._wheels = [] # Right front wheel np = loader.loadModel('../models/yugo/yugotireR.egg') np.reparentTo(self.worldNP) self.addWheel(Point3(0.70, 1.05, 0.3), True, np) # Left front wheel np = loader.loadModel('../models/yugo/yugotireL.egg') np.reparentTo(self.worldNP) self.addWheel(Point3(-0.70, 1.05, 0.3), True, np) # Right rear wheel np = loader.loadModel('../models/yugo/yugotireR.egg') np.reparentTo(self.worldNP) self.addWheel(Point3(0.70, -1.05, 0.3), False, np) # Left rear wheel np = loader.loadModel('../models/yugo/yugotireL.egg') np.reparentTo(self.worldNP) self.addWheel(Point3(-0.70, -1.05, 0.3), False, np) def addWheel(self, pos, front, np): wheel = self.vehicle.createWheel() wheel.setNode(np.node()) wheel.setChassisConnectionPointCs(pos) wheel.setFrontWheel(front) wheel.setWheelDirectionCs(Vec3(0, 0, -1)) wheel.setWheelAxleCs(Vec3(1, 0, 0)) wheel.setWheelRadius(0.25) wheel.setMaxSuspensionTravelCm(40.0) wheel.setSuspensionStiffness(40.0) wheel.setWheelsDampingRelaxation(2.3) wheel.setWheelsDampingCompression(4.4) wheel.setFrictionSlip(1e3) wheel.setRollInfluence(0.0) self._wheels.append(np.node()) def start_update_server(self): self.steering = 0.0 # degree self.steeringClamp = self.params['steeringClamp'] self.engineForce = 0.0 self.engineClamp = self.params['engineClamp'] self.camera_pub = ImageROSPublisher("image") self.depth_pub = ImageROSPublisher("depth") self.back_camera_pub = ImageROSPublisher("back_image") self.back_depth_pub = ImageROSPublisher("back_depth") s = rospy.Service('sim_env', bair_car.srv.sim_env, self.get_handler()) rospy.spin() def update(self, task): dt = globalClock.getDt() self.world.doPhysics(dt, 10, 0.008) obs = self.camera_sensor.observe() obs = self.back_camera_sensor.observe() return task.cont def cleanup(self): self.world = None self.worldNP.removeNode() @abc.abstractmethod def setup(self): pass
class Game(DirectObject): def __init__(self, model): self.model = model base.setBackgroundColor(0.1, 0.1, 0.8, 1) base.setFrameRateMeter(True) base.cam.setPos(0, -20, 4) base.cam.lookAt(0, 0, 0) # Light alight = AmbientLight('ambientLight') alight.setColor(Vec4(0.5, 0.5, 0.5, 1)) alightNP = render.attachNewNode(alight) dlight = DirectionalLight('directionalLight') dlight.setDirection(Vec3(1, 1, -1)) dlight.setColor(Vec4(0.7, 0.7, 0.7, 1)) dlightNP = render.attachNewNode(dlight) render.clearLight() render.setLight(alightNP) render.setLight(dlightNP) # Input self.accept('escape', self.doExit) self.accept('r', self.doReset) self.accept('f1', self.toggleWireframe) self.accept('f2', self.toggleTexture) self.accept('f3', self.toggleDebug) self.accept('f5', self.doScreenshot) inputState.watchWithModifiers('forward', 'w') inputState.watchWithModifiers('left', 'a') inputState.watchWithModifiers('reverse', 's') inputState.watchWithModifiers('right', 'd') inputState.watchWithModifiers('turnLeft', 'q') inputState.watchWithModifiers('turnRight', 'e') # Task taskMgr.add(self.update, 'updateWorld') # Physics self.setup() # _____HANDLER_____ def doExit(self): self.cleanup() sys.exit(1) def doReset(self): self.cleanup() self.setup() def toggleWireframe(self): base.toggleWireframe() def toggleTexture(self): base.toggleTexture() def toggleDebug(self): if self.debugNP.isHidden(): self.debugNP.show() else: self.debugNP.hide() def doScreenshot(self): base.screenshot('Bullet') # ____TASK___ def calculate_moves(self): self.y = self.model.predict(self.x) self.moves = self.y > 0 # 0.5 def processInput(self, dt): engineForce = 0.0 brakeForce = 0.0 if self.moves[0]: #inputState.isSet('forward'): engineForce = 1000.0 brakeForce = 0.0 if not self.moves[0]: #inputState.isSet('reverse'): engineForce = 0.0 brakeForce = 100.0 if self.moves[1]: #inputState.isSet('turnLeft'): self.steering += dt * self.steeringIncrement self.steering = min(self.steering, self.steeringClamp) if not self.moves[1]: #inputState.isSet('turnRight'): self.steering -= dt * self.steeringIncrement self.steering = max(self.steering, -self.steeringClamp) """ if inputState.isSet('forward'): engineForce = 1000.0 brakeForce = 0.0 if inputState.isSet('reverse'): engineForce = 0.0 brakeForce = 100.0 if inputState.isSet('turnLeft'): self.steering += dt * self.steeringIncrement self.steering = min(self.steering, self.steeringClamp) if inputState.isSet('turnRight'): self.steering -= dt * self.steeringIncrement self.steering = max(self.steering, -self.steeringClamp) """ # Apply steering to front wheels self.vehicle.setSteeringValue(self.steering, 0) self.vehicle.setSteeringValue(self.steering, 1) # Apply engine and brake to rear wheels self.vehicle.applyEngineForce(engineForce, 2) self.vehicle.applyEngineForce(engineForce, 3) self.vehicle.setBrake(brakeForce, 2) self.vehicle.setBrake(brakeForce, 3) def raycast(self): """pFrom = render.getRelativePoint(self.yugoNP,Point3(0,0,0))#Point3(0,0,0) pFrom -= Point3(0,0,pFrom[2]) pRel = render.getRelativePoint(base.cam,self.yugoNP.getPos()) # FIXME THIS IS IT!! get rid of z component pRel -= Point3(0,0,pRel[2]) p45 = Point3(pRel[0] - pRel[1], pRel[1] + pRel[0],0) pn45 = Point3(pRel[0] + pRel[1], pRel[1] - pRel[0],0) #print(render.getRelativePoint(self.yugoNP,Point3(0,0,0))) #print(dir(self.yugoNP)) pTo = [pFrom + pn45, pFrom + pRel, pFrom + p45]#[pFrom + Vec3(-10,10,0)*999,pFrom + Vec3(0,10,0)*999,pFrom + Vec3(10,10,0)*999]# FIXME should be relative to front of car, getting cloe! #self.yugoNP.getPosDelta()*99999]#[Point3(-10,10,0) * 99999,Point3(0,10,0) * 99999,Point3(10,10,0) * 99999] #self.ray = CollisionRay(0,0,0,100,0,0) result = [self.world.rayTestClosest(pFrom,pt) for pt in pTo] #print(dir(self.yugoNP)) #print(result.getHitPos()) return tuple([res.getHitPos().length() for res in result]) """#queue = CollisionHandlerQueue() #traverser.addCollider(fromObject, queue) #traverser.traverse(render) #queue.sortEntries() #for entry in queue.getEntries(): #print(entry) #print(result.getHitPos()) #if result.getNode() != None: #print(self.yugoNP.getPos(result.getNode())) #print(self.cTrav) self.cTrav.traverse(render) entries = list(self.colHandler.getEntries()) entries.sort(key=lambda y: y.getSurfacePoint(render).getY()) #for entry in entries: print(entry.getFromNodePath().getName()) if entries: # and len(result) > 1: for r in entries: if r.getIntoNodePath().getName( ) == 'Box' and r.getFromNodePath().getName() in [ 'ray%d' % i for i in range(3) ]: self.ray_col_vec_dict[ r.getFromNodePath().getName()].append( numpy.linalg.norm( list(r.getSurfacePoint( r.getFromNodePath()))[:-1])) self.ray_col_vec_dict = { k: (min(self.ray_col_vec_dict[k]) if len(self.ray_col_vec_dict[k]) >= 1 else 10000) for k in self.ray_col_vec_dict } self.x = numpy.array(list(self.ray_col_vec_dict.values())) #return entries def update(self, task): dt = globalClock.getDt() self.raycast() self.calculate_moves() self.ray_col_vec_dict = {k: [] for k in self.ray_col_vec_dict} self.processInput(dt) self.world.doPhysics(dt, 10, 0.008) #print(dir(result[1])) #print(numpy.linalg.norm(list(result[1].getSurfacePoint(result[1].getFromNodePath()))[:-1])) #base.camera.setPos(0,-40,10) #print self.vehicle.getWheel(0).getRaycastInfo().isInContact() #print self.vehicle.getWheel(0).getRaycastInfo().getContactPointWs() #print self.vehicle.getChassis().isKinematic() return task.cont def cleanup(self): self.world = None self.worldNP.removeNode() def setup(self): self.worldNP = render.attachNewNode('World') # World self.debugNP = self.worldNP.attachNewNode(BulletDebugNode('Debug')) self.debugNP.show() self.world = BulletWorld() self.world.setGravity(Vec3(0, 0, -9.81)) self.world.setDebugNode(self.debugNP.node()) #terrain = GeoMipTerrain("mySimpleTerrain") #terrain.setHeightfield("./models/heightfield_2.png") #terrain.getRoot().reparentTo(self.worldNP)#render) #terrain.generate() # Plane shape = BulletPlaneShape(Vec3(0, 0, 1), 0) np = self.worldNP.attachNewNode(BulletRigidBodyNode('Ground')) np.node().addShape(shape) np.setPos(0, 0, -1) np.setCollideMask(BitMask32.allOn()) self.world.attachRigidBody(np.node()) np = self.worldNP.attachNewNode(BulletRigidBodyNode('Track')) np.node().setMass(5000.0) np.setPos(3, 0, 10) np.setCollideMask(BitMask32.allOn()) #(0x0f)) #self.track = BulletVehicle(self.world, np.node()) #self.track.setCoordinateSystem(ZUp) self.track_np = loader.loadModel('models/race_track.egg') self.track_np.setScale(100) self.track_np.reparentTo(np) self.track_np.setCollideMask(BitMask32.allOn()) self.world.attachRigidBody(np.node()) self.track_np = np #self.track_np.show() # Chassis shape = BulletBoxShape(Vec3(0.6, 1.4, 0.5)) ts = TransformState.makePos(Point3(0, 0, 0.5)) np = self.worldNP.attachNewNode(BulletRigidBodyNode('Vehicle')) np.node().addShape(shape, ts) np.setPos(0, 0, 1) np.node().setMass(800.0) np.node().setDeactivationEnabled(False) self.world.attachRigidBody(np.node()) #np.node().setCcdSweptSphereRadius(1.0) #np.node().setCcdMotionThreshold(1e-7) self.cTrav = CollisionTraverser() # Vehicle self.vehicle = BulletVehicle(self.world, np.node()) self.vehicle.setCoordinateSystem(ZUp) self.yugoNP = loader.loadModel('models/yugo/yugo.egg') self.yugoNP.reparentTo(np) self.colHandler = CollisionHandlerQueue() self.ray_col_np = {} self.ray_col_vec_dict = {} for ray_dir in range(-1, 2): # populate collision rays self.ray = CollisionRay() self.ray.setOrigin(ray_dir, 0.5, 0.5) self.ray.setDirection(ray_dir, 1, 0) self.ray_col = CollisionNode('ray%d' % (ray_dir + 1)) self.ray_col.addSolid(self.ray) self.ray_col.setFromCollideMask( BitMask32.allOn()) #(0x0f))#CollideMask.bit(0) #self.ray_col.setIntoCollideMask(CollideMask.allOff()) self.ray_col_np['ray%d' % (ray_dir + 1)] = self.yugoNP.attachNewNode( self.ray_col) self.cTrav.addCollider(self.ray_col_np['ray%d' % (ray_dir + 1)], self.colHandler) self.ray_col_np['ray%d' % (ray_dir + 1)].show() self.ray_col_vec_dict['ray%d' % (ray_dir + 1)] = [] self.world.attachVehicle(self.vehicle) self.cTrav.showCollisions(render) # FIXME base.camera.reparentTo(self.yugoNP) # Right front wheel np = loader.loadModel('models/yugo/yugotireR.egg') np.reparentTo(self.worldNP) self.addWheel(Point3(0.70, 1.05, 0.3), True, np) # Left front wheel np = loader.loadModel('models/yugo/yugotireL.egg') np.reparentTo(self.worldNP) self.addWheel(Point3(-0.70, 1.05, 0.3), True, np) # Right rear wheel np = loader.loadModel('models/yugo/yugotireR.egg') np.reparentTo(self.worldNP) self.addWheel(Point3(0.70, -1.05, 0.3), False, np) # Left rear wheel np = loader.loadModel('models/yugo/yugotireL.egg') np.reparentTo(self.worldNP) self.addWheel(Point3(-0.70, -1.05, 0.3), False, np) # Steering info self.steering = 0.0 # degree self.steeringClamp = 45.0 # degree self.steeringIncrement = 120.0 # degree per second # Box for i, j in [(0, 8), (-3, 5), (6, -5), (8, 3), (-4, -4)]: shape = BulletBoxShape(Vec3(0.5, 0.5, 0.5)) # https://discourse.panda3d.org/t/wall-collision-help/23606 np = self.worldNP.attachNewNode(BulletRigidBodyNode('Box')) np.node().setMass(1.0) np.node().addShape(shape) np.setPos(i, j, 2) np.setCollideMask(BitMask32.allOn()) #(0x0f)) self.world.attachRigidBody(np.node()) self.boxNP = np #self.colHandler2 = CollisionHandlerQueue() visualNP = loader.loadModel('models/box.egg') visualNP.reparentTo(self.boxNP) #self.cTrav.addCollider(self.boxNP,self.colHandler) """ aNode = CollisionNode("TheRay") self.ray = CollisionRay() self.ray.setOrigin( self.yugoNP.getPos() ) self.ray.setDirection( Vec3(0, 10, 0) ) #self.ray.show() aNodePath = self.yugoNP.attachNewNode( CollisionNode("TheRay") ) aNodePath.node().addSolid(self.ray) aNodePath.show() """ #aNode.addSolid(self.ray) #self.ray = CollisionRay(0,0,0,10,0,0) #self.ray.reparentTo(self.yugoNP) #self.rayColl = CollisionNode('PlayerRay') #self.rayColl.addSolid(self.ray) #self.playerRayNode = self.yugoNP.attachNewNode( self.rayColl ) #self.playerRayNode.show() #base.myTraverser.addCollider (self.playerRayNode, base.floor) #base.floor.addCollider( self.playerRayNode, self.yugoNP) """ MyEvent=CollisionHandlerFloor() MyEvent.setReach(100) MyEvent.setOffset(15.0) aNode = CollisionNode("TheRay") ray = CollisionRay() ray.setOrigin( self.boxNP.getPos() ) ray.setDirection( Vec3(10, 0, 0) ) aNode.addSolid(ray) aNodePath = MyModel.attachNewNode( aNode ) Collision = ( aNode, "TheRay" ) Collision[0].setFromCollideMask( BitMask32.bit( 1 ) ) """ def addWheel(self, pos, front, np): wheel = self.vehicle.createWheel() wheel.setNode(np.node()) wheel.setChassisConnectionPointCs(pos) wheel.setFrontWheel(front) wheel.setWheelDirectionCs(Vec3(0, 0, -1)) wheel.setWheelAxleCs(Vec3(1, 0, 0)) wheel.setWheelRadius(0.25) wheel.setMaxSuspensionTravelCm(40.0) wheel.setSuspensionStiffness(40.0) wheel.setWheelsDampingRelaxation(2.3) wheel.setWheelsDampingCompression(4.4) wheel.setFrictionSlip(100.0) wheel.setRollInfluence(0.1)
class Game(DirectObject): def __init__(self): base.setBackgroundColor(0.1, 0.1, 0.8, 1) base.setFrameRateMeter(True) base.cam.setPos(0, -20, 4) base.cam.lookAt(0, 0, 0) # Light alight = AmbientLight('ambientLight') alight.setColor(Vec4(0.5, 0.5, 0.5, 1)) alightNP = render.attachNewNode(alight) dlight = DirectionalLight('directionalLight') dlight.setDirection(Vec3(1, 1, -1)) dlight.setColor(Vec4(0.7, 0.7, 0.7, 1)) dlightNP = render.attachNewNode(dlight) render.clearLight() render.setLight(alightNP) render.setLight(dlightNP) # Input self.accept('escape', self.doExit) self.accept('r', self.doReset) self.accept('f1', self.toggleWireframe) self.accept('f2', self.toggleTexture) self.accept('f3', self.toggleDebug) self.accept('f5', self.doScreenshot) inputState.watchWithModifiers('forward', 'w') inputState.watchWithModifiers('left', 'a') inputState.watchWithModifiers('reverse', 's') inputState.watchWithModifiers('right', 'd') inputState.watchWithModifiers('turnLeft', 'q') inputState.watchWithModifiers('turnRight', 'e') # Task taskMgr.add(self.update, 'updateWorld') # Physics self.setup() # _____HANDLER_____ def doExit(self): self.cleanup() sys.exit(1) def doReset(self): self.cleanup() self.setup() def toggleWireframe(self): base.toggleWireframe() def toggleTexture(self): base.toggleTexture() def toggleDebug(self): if self.debugNP.isHidden(): self.debugNP.show() else: self.debugNP.hide() def doScreenshot(self): base.screenshot('Bullet') # ____TASK___ def processInput(self, dt): engineForce = 0.0 brakeForce = 0.0 if inputState.isSet('forward'): engineForce = 1000.0 brakeForce = 0.0 if inputState.isSet('reverse'): engineForce = 0.0 brakeForce = 100.0 if inputState.isSet('turnLeft'): self.steering += dt * self.steeringIncrement self.steering = min(self.steering, self.steeringClamp) if inputState.isSet('turnRight'): self.steering -= dt * self.steeringIncrement self.steering = max(self.steering, -self.steeringClamp) # Apply steering to front wheels self.vehicle.setSteeringValue(self.steering, 0) self.vehicle.setSteeringValue(self.steering, 1) # Apply engine and brake to rear wheels self.vehicle.applyEngineForce(engineForce, 2) self.vehicle.applyEngineForce(engineForce, 3) self.vehicle.setBrake(brakeForce, 2) self.vehicle.setBrake(brakeForce, 3) def update(self, task): dt = globalClock.getDt() self.processInput(dt) self.world.doPhysics(dt, 10, 0.008) #print self.vehicle.getWheel(0).getRaycastInfo().isInContact() #print self.vehicle.getWheel(0).getRaycastInfo().getContactPointWs() #print self.vehicle.getChassis().isKinematic() return task.cont def cleanup(self): self.world = None self.worldNP.removeNode() def setup(self): self.worldNP = render.attachNewNode('World') # World self.debugNP = self.worldNP.attachNewNode(BulletDebugNode('Debug')) self.debugNP.show() self.world = BulletWorld() self.world.setGravity(Vec3(0, 0, -9.81)) self.world.setDebugNode(self.debugNP.node()) # Plane shape = BulletPlaneShape(Vec3(0, 0, 1), 0) np = self.worldNP.attachNewNode(BulletRigidBodyNode('Ground')) np.node().addShape(shape) np.setPos(0, 0, -1) np.setCollideMask(BitMask32.allOn()) self.world.attachRigidBody(np.node()) # Chassis shape = BulletBoxShape(Vec3(0.6, 1.4, 0.5)) ts = TransformState.makePos(Point3(0, 0, 0.5)) np = self.worldNP.attachNewNode(BulletRigidBodyNode('Vehicle')) np.node().addShape(shape, ts) np.setPos(0, 0, 1) np.node().setMass(800.0) np.node().setDeactivationEnabled(False) self.world.attachRigidBody(np.node()) #np.node().setCcdSweptSphereRadius(1.0) #np.node().setCcdMotionThreshold(1e-7) # Vehicle self.vehicle = BulletVehicle(self.world, np.node()) self.vehicle.setCoordinateSystem(ZUp) self.world.attachVehicle(self.vehicle) self.yugoNP = loader.loadModel('car/yugo.egg') self.yugoNP.reparentTo(np) # Right front wheel np = loader.loadModel('car/yugotireR.egg') np.reparentTo(self.worldNP) self.addWheel(Point3(0.70, 1.05, 0.3), True, np) # Left front wheel np = loader.loadModel('car/yugotireL.egg') np.reparentTo(self.worldNP) self.addWheel(Point3(-0.70, 1.05, 0.3), True, np) # Right rear wheel np = loader.loadModel('car/yugotireR.egg') np.reparentTo(self.worldNP) self.addWheel(Point3(0.70, -1.05, 0.3), False, np) # Left rear wheel np = loader.loadModel('car/yugotireL.egg') np.reparentTo(self.worldNP) self.addWheel(Point3(-0.70, -1.05, 0.3), False, np) # Steering info self.steering = 0.0 # degree self.steeringClamp = 45.0 # degree self.steeringIncrement = 120.0 # degree per second def addWheel(self, pos, front, np): wheel = self.vehicle.createWheel() wheel.setNode(np.node()) wheel.setChassisConnectionPointCs(pos) wheel.setFrontWheel(front) wheel.setWheelDirectionCs(Vec3(0, 0, -1)) wheel.setWheelAxleCs(Vec3(1, 0, 0)) wheel.setWheelRadius(0.25) wheel.setMaxSuspensionTravelCm(40.0) wheel.setSuspensionStiffness(40.0) wheel.setWheelsDampingRelaxation(2.3) wheel.setWheelsDampingCompression(4.4) wheel.setFrictionSlip(100.0) wheel.setRollInfluence(0.1)
def setup(self): self.worldNP = render.attachNewNode('World') self.distance_text = OnscreenText( text='Distance=0', pos=(0.75, 0.85), scale=0.08, mayChange=1) #Directxxxxxx(distance='Distance=%d'%(0)) self.speed_text = OnscreenText( text='Speed=0', pos=(0.75, 0.78), scale=0.08, mayChange=1) #Directxxxxxx(distance='Distance=%d'%(0)) self.time_text = OnscreenText( text='TotalTime=0', pos=(0.75, 0.71), scale=0.08, mayChange=1) #Directxxxxxx(distance='Distance=%d'%(0)) #self.time_maxsteer_text = OnscreenText(text='TotalTimeMaxSteer=0', pos = (0.85,0.70), scale = 0.05, mayChange=1)#Directxxxxxx(distance='Distance=%d'%(0)) #self.nn_image = OnscreenImage(image='blank.png', pos= (0.85,0,0.15), scale=0.45) # http://dev-wiki.gestureworks.com/index.php/GestureWorksCore:Python_%26_Panda3D:_Getting_Started_II_(Hello_Multitouch)#8._Create_a_method_to_draw_touchpoint_data self.total_time = 0. self.time_max_steering = 0. # World self.debugNP = self.worldNP.attachNewNode(BulletDebugNode('Debug')) self.debugNP.show() self.world = BulletWorld() self.world.setGravity(Vec3(0, 0, -9.81)) self.world.setDebugNode(self.debugNP.node()) #terrain = GeoMipTerrain("mySimpleTerrain") #terrain.setHeightfield("./models/heightfield_2.png") #terrain.getRoot().reparentTo(self.worldNP)#render) #terrain.generate() # Plane shape = BulletPlaneShape(Vec3(0, 0, 1), 0) np = self.worldNP.attachNewNode(BulletRigidBodyNode('Ground')) np.node().addShape(shape) np.setPos(0, 0, -1) np.setCollideMask(BitMask32.allOn()) self.world.attachRigidBody(np.node()) #np = self.worldNP.attachNewNode(BulletRigidBodyNode('Track')) #np.node().setMass(5000.0) #np.setPos(3, 0, 10) #np.setCollideMask(BitMask32.allOn())#(0x0f)) #self.track = BulletVehicle(self.world, np.node()) #self.track.setCoordinateSystem(ZUp) self.track_np = loader.loadModel( 'models/race_track_2.egg' ) # https://discourse.panda3d.org/t/panda3d-and-bullet-physics/15724/10 self.track_np.setPos(-72, -7, -3.5) self.track_np.setScale(10) self.track_np.reparentTo(render) self.track_np.setCollideMask(BitMask32.allOn()) #(0))#.allOn()) self.world.attachRigidBody(np.node()) self.track_np = np #self.track_np.show() # Chassis shape = BulletBoxShape(Vec3(0.6, 1.4, 0.5)) ts = TransformState.makePos(Point3(0, 0, 0.5)) np = self.worldNP.attachNewNode(BulletRigidBodyNode('Vehicle')) np.node().addShape(shape, ts) np.setPos(0, 0, 0.05) np.node().setMass(800.0) np.node().setDeactivationEnabled(False) self.world.attachRigidBody(np.node()) #np.node().setCcdSweptSphereRadius(1.0) #np.node().setCcdMotionThreshold(1e-7) self.cTrav = CollisionTraverser() # Vehicle self.vehicle = BulletVehicle(self.world, np.node()) self.vehicle.setCoordinateSystem(ZUp) self.yugoNP = loader.loadModel('models/yugo/yugo.egg') self.yugoNP.setCollideMask(BitMask32(0)) #.allOn()) self.yugoNP.reparentTo(np) self.colHandler = CollisionHandlerQueue() # travel distance self.distance = 0. """self.sphere = CollisionSphere(0,0,0,2) self.sphere_col = CollisionNode('yugo') self.sphere_col.addSolid(self.sphere) self.sphere_col.setFromCollideMask(BitMask32.allOn()) self.sphere_col_np = self.yugoNP.attachNewNode(self.sphere_col) self.cTrav.addCollider(self.sphere_col_np,self.colHandler) self.sphere_col_np.show()""" self.yugo_col = CollisionNode('yugo_box') self.yugo_col.addSolid(CollisionBox(Point3(0, 0, 0.7), 0.9, 1.6, 0.05)) self.yugo_col.setFromCollideMask(BitMask32(1)) self.box_col_np = self.yugoNP.attachNewNode(self.yugo_col) self.cTrav.addCollider(self.box_col_np, self.colHandler) self.box_col_np.show() self.ray_col_np = {} self.ray_col_vec_dict = {} self.n_rays = self.model.shape[0] for i, ray_dir in enumerate( numpy.linspace(-numpy.pi / 4, numpy.pi / 4, self.n_rays)): # populate collision rays #print(ray_dir) self.ray = CollisionRay() y_dir, x_dir = numpy.cos(ray_dir), numpy.sin(ray_dir) self.ray.setOrigin(1.3 * x_dir, 1.3 * y_dir, 0.5) self.ray.setDirection(x_dir, y_dir, 0) self.ray_col = CollisionNode('ray%d' % (i)) self.ray_col.addSolid(self.ray) self.ray_col.setFromCollideMask( BitMask32.allOn()) #(0x0f))#CollideMask.bit(0) #self.ray_col.setIntoCollideMask(CollideMask.allOff()) self.ray_col_np['ray%d' % (i)] = self.yugoNP.attachNewNode( self.ray_col) self.cTrav.addCollider(self.ray_col_np['ray%d' % (i)], self.colHandler) self.ray_col_np['ray%d' % (i)].show() self.ray_col_vec_dict['ray%d' % (i)] = [] self.world.attachVehicle(self.vehicle) self.cTrav.showCollisions(render) # FIXME base.camera.reparentTo(self.yugoNP) # Right front wheel np = loader.loadModel('models/yugo/yugotireR.egg') np.reparentTo(self.worldNP) self.addWheel(Point3(0.70, 1.05, 0.3), True, np) # Left front wheel np = loader.loadModel('models/yugo/yugotireL.egg') np.reparentTo(self.worldNP) self.addWheel(Point3(-0.70, 1.05, 0.3), True, np) # Right rear wheel np = loader.loadModel('models/yugo/yugotireR.egg') np.reparentTo(self.worldNP) self.addWheel(Point3(0.70, -1.05, 0.3), False, np) # Left rear wheel np = loader.loadModel('models/yugo/yugotireL.egg') np.reparentTo(self.worldNP) self.addWheel(Point3(-0.70, -1.05, 0.3), False, np) # Steering info self.steering = 0.0 # degree self.steeringClamp = 38.0 #45.0 # degree self.steeringIncrement = 105.0 #120.0 # degree per second # add previous positions self.prevPos = [] self.prevPos.append(self.yugoNP.getPos(render)) self.model_offset = 0.5 if self.model.activation == 'relu' else 0. # Box """ for i,j in [(0,8),(-3,5),(6,-5),(8,3),(-4,-4),(0,0)]: shape = BulletBoxShape(Vec3(0.5, 0.5, 0.5)) # https://discourse.panda3d.org/t/wall-collision-help/23606 np = self.worldNP.attachNewNode(BulletRigidBodyNode('Box')) np.node().setMass(1.0) np.node().addShape(shape) np.setPos(i, j, 2) np.setCollideMask(BitMask32.allOn())#(0x0f)) self.world.attachRigidBody(np.node()) self.boxNP = np #self.colHandler2 = CollisionHandlerQueue() visualNP = loader.loadModel('models/box.egg') visualNP.reparentTo(self.boxNP) #self.cTrav.addCollider(self.boxNP,self.colHandler) """ """ aNode = CollisionNode("TheRay") self.ray = CollisionRay() self.ray.setOrigin( self.yugoNP.getPos() ) self.ray.setDirection( Vec3(0, 10, 0) ) #self.ray.show() aNodePath = self.yugoNP.attachNewNode( CollisionNode("TheRay") ) aNodePath.node().addSolid(self.ray) aNodePath.show() """ #aNode.addSolid(self.ray) #self.ray = CollisionRay(0,0,0,10,0,0) #self.ray.reparentTo(self.yugoNP) #self.rayColl = CollisionNode('PlayerRay') #self.rayColl.addSolid(self.ray) #self.playerRayNode = self.yugoNP.attachNewNode( self.rayColl ) #self.playerRayNode.show() #base.myTraverser.addCollider (self.playerRayNode, base.floor) #base.floor.addCollider( self.playerRayNode, self.yugoNP) """
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()
def __init__(self): ShowBase.__init__(self) #Setup scene = BulletWorld() scene.setGravity(Vec3(0, 0, -9.81)) base.setBackgroundColor(0.6,0.9,0.9) fog = Fog("The Fog") fog.setColor(0.9,0.9,1.0) fog.setExpDensity(0.003) render.setFog(fog) #Lighting #Sun light sun = DirectionalLight("The Sun") sun_np = render.attachNewNode(sun) sun_np.setHpr(0,-60,0) render.setLight(sun_np) #Ambient light amb = AmbientLight("The Ambient Light") amb.setColor(VBase4(0.39,0.39,0.39, 1)) amb_np = render.attachNewNode(amb) render.setLight(amb_np) #Variables self.gear = 0 self.start = 0 self.Pbrake = 0 self.terrain_var = 1 self.time = 0 self.headlight_var = 0 self.RPM = 0 self.clutch = 0 self.carmaxspeed = 100 #KPH self.carmaxreversespeed = -40 #KPH self.steering = 0 #Functions def V1(): camera.setPos(0.25,-1.2,0.5) camera.setHpr(0,-13,0) def V2(): camera.setPos(0,-15,3) camera.setHpr(0,-10,0) def V3(): camera.setPos(0,0,9) camera.setHpr(0,-90,0) def up(): self.gear = self.gear -1 if self.gear < -1: self.gear = -1 def down(): self.gear = self.gear +1 if self.gear > 1: self.gear = 1 def start_function(): self.start = 1 self.start_sound.play() self.engine_idle_sound.play() self.RPM = 1000 def stop_function(): self.start = 0 self.engine_idle_sound.stop() def parkingbrake(): self.Pbrake = (self.Pbrake + 1) % 2 def rotate(): Car_np.setHpr(0, 0, 0) def horn(): self.horn_sound.play() def set_time(): if self.time == -1: sun.setColor(VBase4(0.4, 0.3, 0.3, 1)) base.setBackgroundColor(0.8,0.7,0.7) if self.time == 0: sun.setColor(VBase4(0.7, 0.7, 0.7, 1)) base.setBackgroundColor(0.6,0.9,0.9) if self.time == 1: sun.setColor(VBase4(0.2, 0.2, 0.2, 1)) base.setBackgroundColor(0.55,0.5,0.5) if self.time == 2: sun.setColor(VBase4(0.02, 0.02, 0.05, 1)) base.setBackgroundColor(0.3,0.3,0.3) if self.time == -2: self.time = -1 if self.time == 3: self.time = 2 def time_forward(): self.time = self.time + 1 def time_backward(): self.time = self.time -1 def set_terrain(): if self.terrain_var == 1: self.ground_model.setTexture(self.ground_tex, 1) self.ground_model.setScale(3) if self.terrain_var == 2: self.ground_model.setTexture(self.ground_tex2, 1) self.ground_model.setScale(3) if self.terrain_var == 3: self.ground_model.setTexture(self.ground_tex3, 1) self.ground_model.setScale(4) if self.terrain_var == 4: self.terrain_var = 1 if self.terrain_var == 0: self.terrain_var = 3 def next_terrain(): self.terrain_var = self.terrain_var + 1 def previous_terrain(): self.terrain_var = self.terrain_var - 1 def show_menu(): self.menu_win.show() self.a1.show() self.a2.show() self.a3.show() self.a4.show() self.t1.show() self.t2.show() self.ok.show() self.exit_button.show() def hide_menu(): self.menu_win.hide() self.a1.hide() self.a2.hide() self.a3.hide() self.a4.hide() self.ok.hide() self.t1.hide() self.t2.hide() self.exit_button.hide() def Menu(): self.menu_win = OnscreenImage(image = "Textures/menu.png", pos = (0.9,0,0), scale = (0.5)) self.menu_win.setTransparency(TransparencyAttrib.MAlpha) #The Arrow Buttons self.a1 = DirectButton(text = "<", scale = 0.2, pos = (0.55,0,0.25), command = previous_terrain) self.a2 = DirectButton(text = ">", scale = 0.2, pos = (1.15,0,0.25), command = next_terrain) self.a3 = DirectButton(text = "<", scale = 0.2, pos = (0.55,0,0.0), command = time_backward) self.a4 = DirectButton(text = ">", scale = 0.2, pos = (1.15,0,0.0), command = time_forward) #The Text self.t1 = OnscreenText(text = "Terrain", pos = (0.85,0.25,0), scale = 0.1, fg = (0.4,0.4,0.5,1)) self.t2 = OnscreenText(text = "Time", pos = (0.85,0,0), scale = 0.1, fg = (0.4,0.4,0.5,1)) #The Buttons self.ok = DirectButton(text = "Okay", scale = 0.11, pos = (0.87,0,-0.25), command = hide_menu) self.exit_button = DirectButton(text = "Quit", scale = 0.11, pos = (0.87,0,-0.42), command = sys.exit) Menu() def take_screenshot(): base.screenshot("Screenshot") def set_headlights(): if self.headlight_var == 1: Headlight1.setColor(VBase4(9.0,8.9,8.9,1)) Headlight2.setColor(VBase4(9.0,8.9,8.9,1)) if self.headlight_var == 0: Headlight1.setColor(VBase4(0,0,0,1)) Headlight2.setColor(VBase4(0,0,0,1)) def headlights(): self.headlight_var = (self.headlight_var + 1) % 2 def update_rpm(): #Simulate RPM if self.start == 1: if self.gear == 0: self.RPM = self.RPM - self.RPM / 400 else: self.RPM = self.RPM + self.carspeed / 9 self.RPM = self.RPM - self.RPM / 200 #Reset RPM to 0 when engine is off if self.start == 0: if self.RPM > 0.0: self.RPM = self.RPM - 40 if self.RPM < 10: self.RPM = 0.0 #Idle RPM power if self.start == 1: if self.RPM < 650: self.RPM = self.RPM + 4 if self.RPM < 600: self.clutch = 1 else: self.clutch = 0 #RPM limit if self.RPM > 6000: self.RPM = 6000 #Controls inputState.watchWithModifiers("F", "arrow_up") inputState.watchWithModifiers("B", "arrow_down") inputState.watchWithModifiers("L", "arrow_left") inputState.watchWithModifiers("R", "arrow_right") do = DirectObject() do.accept("escape", show_menu) do.accept("1", V1) do.accept("2", V2) do.accept("3", V3) do.accept("page_up", up) do.accept("page_down", down) do.accept("x-repeat", start_function) do.accept("x", stop_function) do.accept("p", parkingbrake) do.accept("backspace", rotate) do.accept("enter", horn) do.accept("f12", take_screenshot) do.accept("h", headlights) #The ground self.ground = BulletPlaneShape(Vec3(0, 0, 1,), 1) self.ground_node = BulletRigidBodyNode("The ground") self.ground_node.addShape(self.ground) self.ground_np = render.attachNewNode(self.ground_node) self.ground_np.setPos(0, 0, -2) scene.attachRigidBody(self.ground_node) self.ground_model = loader.loadModel("Models/plane.egg") self.ground_model.reparentTo(render) self.ground_model.setPos(0,0,-1) self.ground_model.setScale(3) self.ground_tex = loader.loadTexture("Textures/ground.png") self.ground_tex2 = loader.loadTexture("Textures/ground2.png") self.ground_tex3 = loader.loadTexture("Textures/ground3.png") self.ground_model.setTexture(self.ground_tex, 1) #The car Car_shape = BulletBoxShape(Vec3(1, 2.0, 1.0)) Car_node = BulletRigidBodyNode("The Car") Car_node.setMass(1200.0) Car_node.addShape(Car_shape) Car_np = render.attachNewNode(Car_node) Car_np.setPos(0,0,3) Car_np.setHpr(0,0,0) Car_np.node().setDeactivationEnabled(False) scene.attachRigidBody(Car_node) Car_model = loader.loadModel("Models/Car.egg") Car_model.reparentTo(Car_np) Car_tex = loader.loadTexture("Textures/Car1.png") Car_model.setTexture(Car_tex, 1) self.Car_sim = BulletVehicle(scene, Car_np.node()) self.Car_sim.setCoordinateSystem(ZUp) scene.attachVehicle(self.Car_sim) #The inside of the car Car_int = loader.loadModel("Models/inside.egg") Car_int.reparentTo(Car_np) Car_int_tex = loader.loadTexture("Textures/inside.png") Car_int.setTexture(Car_int_tex, 1) Car_int.setTransparency(TransparencyAttrib.MAlpha) #The steering wheel Sw = loader.loadModel("Models/Steering wheel.egg") Sw.reparentTo(Car_np) Sw.setPos(0.25,0,-0.025) #The first headlight Headlight1 = Spotlight("Headlight1") lens = PerspectiveLens() lens.setFov(180) Headlight1.setLens(lens) Headlight1np = render.attachNewNode(Headlight1) Headlight1np.reparentTo(Car_np) Headlight1np.setPos(-0.8,2.5,-0.5) Headlight1np.setP(-15) render.setLight(Headlight1np) #The second headlight Headlight2 = Spotlight("Headlight2") Headlight2.setLens(lens) Headlight2np = render.attachNewNode(Headlight2) Headlight2np.reparentTo(Car_np) Headlight2np.setPos(0.8,2.5,-0.5) Headlight2np.setP(-15) render.setLight(Headlight2np) #Sounds self.horn_sound = loader.loadSfx("Sounds/horn.ogg") self.start_sound = loader.loadSfx("Sounds/enginestart.ogg") self.engine_idle_sound = loader.loadSfx("Sounds/engineidle.ogg") self.engine_idle_sound.setLoop(True) self.accelerate_sound = loader.loadSfx("Sounds/enginethrottle.ogg") #Camera base.disableMouse() camera.reparentTo(Car_np) camera.setPos(0,-15,3) camera.setHpr(0,-10,0) #Wheel function def Wheel(pos, np, r, f): w = self.Car_sim.createWheel() w.setNode(np.node()) w.setChassisConnectionPointCs(pos) w.setFrontWheel(f) w.setWheelDirectionCs(Vec3(0, 0, -1)) w.setWheelAxleCs(Vec3(1, 0, 0)) w.setWheelRadius(r) w.setMaxSuspensionTravelCm(40) w.setSuspensionStiffness(120) w.setWheelsDampingRelaxation(2.3) w.setWheelsDampingCompression(4.4) w.setFrictionSlip(50) w.setRollInfluence(0.1) #Wheels w1_np = loader.loadModel("Models/Lwheel") w1_np.reparentTo(render) w1_np.setColorScale(0,6) Wheel(Point3(-1,1,-0.6), w1_np, 0.4, False) w2_np = loader.loadModel("Models/Rwheel") w2_np.reparentTo(render) w2_np.setColorScale(0,6) Wheel(Point3(-1.1,-1.2,-0.6), w2_np, 0.4, True) w3_np = loader.loadModel("Models/Lwheel") w3_np.reparentTo(render) w3_np.setColorScale(0,6) Wheel(Point3(1.1,-1,-0.6), w3_np, 0.4, True) w4_np = loader.loadModel("Models/Rwheel") w4_np.reparentTo(render) w4_np.setColorScale(0,6) Wheel(Point3(1,1,-0.6), w4_np, 0.4, False) #The engine and steering def processInput(dt): #Vehicle properties self.steeringClamp = 35.0 self.steeringIncrement = 70 engineForce = 0.0 brakeForce = 0.0 #Get the vehicle's current speed self.carspeed = self.Car_sim.getCurrentSpeedKmHour() #Engage clutch when in gear 0 if self.gear == 0: self.clutch = 1 #Slow the steering when at higher speeds self.steeringIncrement = self.steeringIncrement - self.carspeed / 1.5 #Reset the steering if not inputState.isSet("L") and not inputState.isSet("R"): if self.steering < 0.00: self.steering = self.steering + 0.6 if self.steering > 0.00: self.steering = self.steering - 0.6 if self.steering < 1.0 and self.steering > -1.0: self.steering = 0 #Slow the car down while it's moving if self.clutch == 0: brakeForce = brakeForce + self.carspeed / 5 else: brakeForce = brakeForce + self.carspeed / 15 #Forward if self.start == 1: if inputState.isSet("F"): self.RPM = self.RPM + 35 self.accelerate_sound.play() if self.clutch == 0: if self.gear == -1: if self.carspeed > self.carmaxreversespeed: engineForce = -self.RPM / 3 if self.gear == 1: if self.carspeed < self.carmaxspeed: engineForce = self.RPM / 1 #Brake if inputState.isSet("B"): engineForce = 0.0 brakeForce = 12.0 if self.gear != 0 and self.clutch == 0: self.RPM = self.RPM - 20 #Left if inputState.isSet("L"): if self.steering < 0.0: #This makes the steering reset at the correct speed when turning from right to left self.steering += dt * self.steeringIncrement + 0.6 self.steering = min(self.steering, self.steeringClamp) else: #Normal steering self.steering += dt * self.steeringIncrement self.steering = min(self.steering, self.steeringClamp) #Right if inputState.isSet("R"): if self.steering > 0.0: #This makes the steering reset at the correct speed when turning from left to right self.steering -= dt * self.steeringIncrement + 0.6 self.steering = max(self.steering, -self.steeringClamp) else: #Normal steering self.steering -= dt * self.steeringIncrement self.steering = max(self.steering, -self.steeringClamp) #Park if self.Pbrake == 1: brakeForce = 10.0 if self.gear != 0 and self. clutch == 0: self.RPM = self.RPM - 20 #Apply forces to wheels self.Car_sim.applyEngineForce(engineForce, 0); self.Car_sim.applyEngineForce(engineForce, 3); self.Car_sim.setBrake(brakeForce, 1); self.Car_sim.setBrake(brakeForce, 2); self.Car_sim.setSteeringValue(self.steering, 0); self.Car_sim.setSteeringValue(self.steering, 3); #Steering wheel Sw.setHpr(0,0,-self.steering*10) #The HUD self.gear_hud = OnscreenImage(image = "Textures/gear_hud.png", pos = (-1,0,-0.85), scale = (0.2)) self.gear_hud.setTransparency(TransparencyAttrib.MAlpha) self.gear2_hud = OnscreenImage(image = "Textures/gear2_hud.png", pos = (-1,0,-0.85), scale = (0.2)) self.gear2_hud.setTransparency(TransparencyAttrib.MAlpha) self.starter = OnscreenImage(image = "Textures/starter.png", pos = (-1.2,0,-0.85), scale = (0.15)) self.starter.setTransparency(TransparencyAttrib.MAlpha) self.park = OnscreenImage(image = "Textures/pbrake.png", pos = (-0.8,0,-0.85), scale = (0.1)) self.park.setTransparency(TransparencyAttrib.MAlpha) self.rev_counter = OnscreenImage(image = "Textures/dial.png", pos = (-1.6, 0.0, -0.70), scale = (0.6,0.6,0.4)) self.rev_counter.setTransparency(TransparencyAttrib.MAlpha) self.rev_needle = OnscreenImage(image = "Textures/needle.png", pos = (-1.6, 0.0, -0.70), scale = (0.5)) self.rev_needle.setTransparency(TransparencyAttrib.MAlpha) self.rev_text = OnscreenText(text = " ", pos = (-1.6, -0.90, 0), scale = 0.05) self.speedometer = OnscreenImage(image = "Textures/dial.png", pos = (-1.68, 0.0, -0.10), scale = (0.7,0.7,0.5)) self.speedometer.setTransparency(TransparencyAttrib.MAlpha) self.speedometer_needle = OnscreenImage(image = "Textures/needle.png", pos = (-1.68, 0.0, -0.10), scale = (0.5)) self.speedometer_needle.setTransparency(TransparencyAttrib.MAlpha) self.speedometer_text = OnscreenText(text = " ", pos = (-1.68, -0.35, 0), scale = 0.05) #Update the HUD def Update_HUD(): #Move gear selector if self.gear == -1: self.gear2_hud.setPos(-1,0,-0.785) if self.gear == 0: self.gear2_hud.setPos(-1,0,-0.85) if self.gear == 1: self.gear2_hud.setPos(-1,0,-0.91) #Rotate starter if self.start == 0: self.starter.setHpr(0,0,0) else: self.starter.setHpr(0,0,45) #Update the parking brake light if self.Pbrake == 1: self.park.setImage("Textures/pbrake2.png") self.park.setTransparency(TransparencyAttrib.MAlpha) else: self.park.setImage("Textures/pbrake.png") self.park.setTransparency(TransparencyAttrib.MAlpha) #Update the rev counter self.rev_needle.setR(self.RPM/22) rev_string = str(self.RPM)[:4] self.rev_text.setText(rev_string+" RPM") #Update the speedometer if self.carspeed > 0.0: self.speedometer_needle.setR(self.carspeed*2.5) if self.carspeed < 0.0: self.speedometer_needle.setR(-self.carspeed*2.5) speed_string = str(self.carspeed)[:3] self.speedometer_text.setText(speed_string+" KPH") #Update the program def update(task): dt = globalClock.getDt() processInput(dt) Update_HUD() set_time() set_terrain() set_headlights() update_rpm() scene.doPhysics(dt, 5, 1.0/180.0) return task.cont taskMgr.add(update, "Update")
class Game(DirectObject): def __init__(self, model): self.model = model base.setBackgroundColor(0.1, 0.1, 0.8, 1) base.setFrameRateMeter(True) base.cam.setPos(0, -20, 4) base.cam.lookAt(0, 0, 0) w = WindowProperties() w.setFullscreen(False) w.setOrigin(5, 20) w.setSize(865, 800) base.win.requestProperties(w) # Light alight = AmbientLight('ambientLight') alight.setColor(Vec4(0.5, 0.5, 0.5, 1)) alightNP = render.attachNewNode(alight) dlight = DirectionalLight('directionalLight') dlight.setDirection(Vec3(1, 1, -1)) dlight.setColor(Vec4(0.7, 0.7, 0.7, 1)) dlightNP = render.attachNewNode(dlight) render.clearLight() render.setLight(alightNP) render.setLight(dlightNP) # Input self.accept('escape', self.doExit) self.accept('r', self.doReset) self.accept('f1', self.toggleWireframe) self.accept('f2', self.toggleTexture) self.accept('f3', self.toggleDebug) self.accept('f5', self.doScreenshot) inputState.watchWithModifiers('forward', 'w') inputState.watchWithModifiers('left', 'a') inputState.watchWithModifiers('reverse', 's') inputState.watchWithModifiers('right', 'd') inputState.watchWithModifiers('turnLeft', 'q') inputState.watchWithModifiers('turnRight', 'e') # Task taskMgr.add(self.update, 'updateWorld') # Physics self.setup() # _____HANDLER_____ def doExit(self): self.cleanup() sys.exit(1) def endLoop(self): self.penalized_distance = self.distance * (numpy.exp( -self.time_max_steering / self.total_time)) pickle.dump(self.penalized_distance, open('distance.p', 'wb')) sys.exit() #quit() #print("Distance was: ",self.distance) #os.execv(sys.executable,['python']+[__file__]) #taskMgr.running = False def doReset(self): self.cleanup() self.setup() def toggleWireframe(self): base.toggleWireframe() def toggleTexture(self): base.toggleTexture() def toggleDebug(self): if self.debugNP.isHidden(): self.debugNP.show() else: self.debugNP.hide() def doScreenshot(self): base.screenshot('Bullet') # ____TASK___ def calculate_moves(self): self.y = self.model.predict(self.x) #print(self.y) self.moves = self.y > 0 #+ self.model_offset # 0.5 #self.moves[0] = True # FIXME test def processInput(self, dt): engineForce = 0.0 brakeForce = 0.0 if self.moves[ 0]: #inputState.isSet('forward'): FIXME maybe engine and brake can be linked to self.y, rectified, continuous engineForce = 2000.0 # 1000. brakeForce = 0.0 if not self.moves[0]: #inputState.isSet('reverse'): engineForce = 200.0 #0.0 brakeForce = 100.0 self.steering = self.y[1] if self.moves[1]: self.steering = min(self.steering, self.steeringClamp) if not self.moves[1]: self.steering = max(self.steering, -self.steeringClamp) """ # FIXME option may be better if self.steering is fed into self.y[3] for 4th element if not self.moves[2]: # enabled steering lock if self.moves[1]:#inputState.isSet('turnLeft'): self.steering += dt * self.steeringIncrement self.steering = min(self.steering, self.steeringClamp) if not self.moves[1]:#inputState.isSet('turnRight'): self.steering -= dt * self.steeringIncrement self.steering = max(self.steering, -self.steeringClamp) """ """ if inputState.isSet('forward'): engineForce = 1000.0 brakeForce = 0.0 if inputState.isSet('reverse'): engineForce = 0.0 brakeForce = 100.0 if inputState.isSet('turnLeft'): self.steering += dt * self.steeringIncrement self.steering = min(self.steering, self.steeringClamp) if inputState.isSet('turnRight'): self.steering -= dt * self.steeringIncrement self.steering = max(self.steering, -self.steeringClamp) """ # Apply steering to front wheels self.vehicle.setSteeringValue(self.steering, 0) self.vehicle.setSteeringValue(self.steering, 1) # Apply engine and brake to rear wheels self.vehicle.applyEngineForce(engineForce, 2) self.vehicle.applyEngineForce(engineForce, 3) self.vehicle.setBrake(brakeForce, 2) self.vehicle.setBrake(brakeForce, 3) def check_collisions(self): """pFrom = render.getRelativePoint(self.yugoNP,Point3(0,0,0))#Point3(0,0,0) pFrom -= Point3(0,0,pFrom[2]) pRel = render.getRelativePoint(base.cam,self.yugoNP.getPos()) # FIXME THIS IS IT!! get rid of z component pRel -= Point3(0,0,pRel[2]) p45 = Point3(pRel[0] - pRel[1], pRel[1] + pRel[0],0) pn45 = Point3(pRel[0] + pRel[1], pRel[1] - pRel[0],0) #print(render.getRelativePoint(self.yugoNP,Point3(0,0,0))) #print(dir(self.yugoNP)) pTo = [pFrom + pn45, pFrom + pRel, pFrom + p45]#[pFrom + Vec3(-10,10,0)*999,pFrom + Vec3(0,10,0)*999,pFrom + Vec3(10,10,0)*999]# FIXME should be relative to front of car, getting cloe! #self.yugoNP.getPosDelta()*99999]#[Point3(-10,10,0) * 99999,Point3(0,10,0) * 99999,Point3(10,10,0) * 99999] #self.ray = CollisionRay(0,0,0,100,0,0) result = [self.world.rayTestClosest(pFrom,pt) for pt in pTo] #print(dir(self.yugoNP)) #print(result.getHitPos()) return tuple([res.getHitPos().length() for res in result]) """#queue = CollisionHandlerQueue() #traverser.addCollider(fromObject, queue) #traverser.traverse(render) #queue.sortEntries() #for entry in queue.getEntries(): #print(entry) #print(result.getHitPos()) #if result.getNode() != None: #print(self.yugoNP.getPos(result.getNode())) #print(self.cTrav) self.cTrav.traverse(render) entries = list(self.colHandler.getEntries()) #print(entries) entries.sort(key=lambda y: y.getSurfacePoint(render).getY()) #for entry in entries: print(entry.getFromNodePath().getName()) if entries: # and len(result) > 1: #print(entries) for r in entries: #print(r.getIntoNodePath().getName()) if r.getIntoNodePath().getName( ) == 'Plane' and r.getFromNodePath().getName() == 'yugo_box': self.endLoop() if r.getIntoNodePath().getName( ) == 'Plane' and r.getFromNodePath().getName() in [ 'ray%d' % i for i in range(self.n_rays) ]: #Box self.ray_col_vec_dict[ r.getFromNodePath().getName()].append( numpy.linalg.norm( list(r.getSurfacePoint( r.getFromNodePath()))[:-1])) self.ray_col_vec_dict = { k: (min(self.ray_col_vec_dict[k]) if len(self.ray_col_vec_dict[k]) >= 1 else 10000) for k in self.ray_col_vec_dict } self.x = numpy.array(list(self.ray_col_vec_dict.values())) #print(self.x) result = self.world.contactTest(self.yugoNP.node()) #print(result.getNumContacts()) #print(dir(self.yugoNP)) #return entries def check_prevPos(self): if len(self.prevPos) > 80: #print(self.prevPos) #print(numpy.linalg.norm(self.prevPos[-1] - self.prevPos[0])) if numpy.linalg.norm(self.prevPos[-1] - self.prevPos[0]) < 4.5: #print("ERROR") self.endLoop() del self.prevPos[0:len(self.prevPos) - 80] def update(self, task): self.prevPos.append(self.yugoNP.getPos(render)) dx = numpy.linalg.norm(self.prevPos[-1] - self.prevPos[-2]) self.distance += dx self.distance_text.setText('Distance=%.3f' % (self.distance)) #print(len(self.prevPos)) dt = globalClock.getDt() self.total_time += dt if abs(self.steering) == abs(self.steeringClamp): self.time_max_steering += dt self.time_text.setText('TotalTime=%.3f' % (self.total_time)) #self.time_maxsteer_text.setText('TotalTimeMaxSteer=%f'%(self.time_max_steering)) #self.penalized_distance = self.distance*(1.-numpy.exp(-self.time_max_steering/self.total_time)) if self.distance > 10000: self.endLoop() self.check_prevPos() self.speed = dx / dt self.speed_text.setText('Speed=%.3f' % (self.speed)) self.check_collisions() self.calculate_moves() self.model.plot_NN() #self.nn_image.setImage('neural_net_vis.png') self.ray_col_vec_dict = {k: [] for k in self.ray_col_vec_dict} self.processInput(dt) self.world.doPhysics(dt, 10, 0.008) # FIXME KEEP TRACK OF TOTAL DEGREES TURNED AND PENALIZE #self.doReset() #print(dir(result[1])) #print(numpy.linalg.norm(list(result[1].getSurfacePoint(result[1].getFromNodePath()))[:-1])) #base.camera.setPos(0,-40,10) #print self.vehicle.getWheel(0).getRaycastInfo().isInContact() #print self.vehicle.getWheel(0).getRaycastInfo().getContactPointWs() #print self.vehicle.getChassis().isKinematic() return task.cont def cleanup(self): self.world = None self.worldNP.removeNode() def setup(self): self.worldNP = render.attachNewNode('World') self.distance_text = OnscreenText( text='Distance=0', pos=(0.75, 0.85), scale=0.08, mayChange=1) #Directxxxxxx(distance='Distance=%d'%(0)) self.speed_text = OnscreenText( text='Speed=0', pos=(0.75, 0.78), scale=0.08, mayChange=1) #Directxxxxxx(distance='Distance=%d'%(0)) self.time_text = OnscreenText( text='TotalTime=0', pos=(0.75, 0.71), scale=0.08, mayChange=1) #Directxxxxxx(distance='Distance=%d'%(0)) #self.time_maxsteer_text = OnscreenText(text='TotalTimeMaxSteer=0', pos = (0.85,0.70), scale = 0.05, mayChange=1)#Directxxxxxx(distance='Distance=%d'%(0)) #self.nn_image = OnscreenImage(image='blank.png', pos= (0.85,0,0.15), scale=0.45) # http://dev-wiki.gestureworks.com/index.php/GestureWorksCore:Python_%26_Panda3D:_Getting_Started_II_(Hello_Multitouch)#8._Create_a_method_to_draw_touchpoint_data self.total_time = 0. self.time_max_steering = 0. # World self.debugNP = self.worldNP.attachNewNode(BulletDebugNode('Debug')) self.debugNP.show() self.world = BulletWorld() self.world.setGravity(Vec3(0, 0, -9.81)) self.world.setDebugNode(self.debugNP.node()) #terrain = GeoMipTerrain("mySimpleTerrain") #terrain.setHeightfield("./models/heightfield_2.png") #terrain.getRoot().reparentTo(self.worldNP)#render) #terrain.generate() # Plane shape = BulletPlaneShape(Vec3(0, 0, 1), 0) np = self.worldNP.attachNewNode(BulletRigidBodyNode('Ground')) np.node().addShape(shape) np.setPos(0, 0, -1) np.setCollideMask(BitMask32.allOn()) self.world.attachRigidBody(np.node()) #np = self.worldNP.attachNewNode(BulletRigidBodyNode('Track')) #np.node().setMass(5000.0) #np.setPos(3, 0, 10) #np.setCollideMask(BitMask32.allOn())#(0x0f)) #self.track = BulletVehicle(self.world, np.node()) #self.track.setCoordinateSystem(ZUp) self.track_np = loader.loadModel( 'models/race_track_2.egg' ) # https://discourse.panda3d.org/t/panda3d-and-bullet-physics/15724/10 self.track_np.setPos(-72, -7, -3.5) self.track_np.setScale(10) self.track_np.reparentTo(render) self.track_np.setCollideMask(BitMask32.allOn()) #(0))#.allOn()) self.world.attachRigidBody(np.node()) self.track_np = np #self.track_np.show() # Chassis shape = BulletBoxShape(Vec3(0.6, 1.4, 0.5)) ts = TransformState.makePos(Point3(0, 0, 0.5)) np = self.worldNP.attachNewNode(BulletRigidBodyNode('Vehicle')) np.node().addShape(shape, ts) np.setPos(0, 0, 0.05) np.node().setMass(800.0) np.node().setDeactivationEnabled(False) self.world.attachRigidBody(np.node()) #np.node().setCcdSweptSphereRadius(1.0) #np.node().setCcdMotionThreshold(1e-7) self.cTrav = CollisionTraverser() # Vehicle self.vehicle = BulletVehicle(self.world, np.node()) self.vehicle.setCoordinateSystem(ZUp) self.yugoNP = loader.loadModel('models/yugo/yugo.egg') self.yugoNP.setCollideMask(BitMask32(0)) #.allOn()) self.yugoNP.reparentTo(np) self.colHandler = CollisionHandlerQueue() # travel distance self.distance = 0. """self.sphere = CollisionSphere(0,0,0,2) self.sphere_col = CollisionNode('yugo') self.sphere_col.addSolid(self.sphere) self.sphere_col.setFromCollideMask(BitMask32.allOn()) self.sphere_col_np = self.yugoNP.attachNewNode(self.sphere_col) self.cTrav.addCollider(self.sphere_col_np,self.colHandler) self.sphere_col_np.show()""" self.yugo_col = CollisionNode('yugo_box') self.yugo_col.addSolid(CollisionBox(Point3(0, 0, 0.7), 0.9, 1.6, 0.05)) self.yugo_col.setFromCollideMask(BitMask32(1)) self.box_col_np = self.yugoNP.attachNewNode(self.yugo_col) self.cTrav.addCollider(self.box_col_np, self.colHandler) self.box_col_np.show() self.ray_col_np = {} self.ray_col_vec_dict = {} self.n_rays = self.model.shape[0] for i, ray_dir in enumerate( numpy.linspace(-numpy.pi / 4, numpy.pi / 4, self.n_rays)): # populate collision rays #print(ray_dir) self.ray = CollisionRay() y_dir, x_dir = numpy.cos(ray_dir), numpy.sin(ray_dir) self.ray.setOrigin(1.3 * x_dir, 1.3 * y_dir, 0.5) self.ray.setDirection(x_dir, y_dir, 0) self.ray_col = CollisionNode('ray%d' % (i)) self.ray_col.addSolid(self.ray) self.ray_col.setFromCollideMask( BitMask32.allOn()) #(0x0f))#CollideMask.bit(0) #self.ray_col.setIntoCollideMask(CollideMask.allOff()) self.ray_col_np['ray%d' % (i)] = self.yugoNP.attachNewNode( self.ray_col) self.cTrav.addCollider(self.ray_col_np['ray%d' % (i)], self.colHandler) self.ray_col_np['ray%d' % (i)].show() self.ray_col_vec_dict['ray%d' % (i)] = [] self.world.attachVehicle(self.vehicle) self.cTrav.showCollisions(render) # FIXME base.camera.reparentTo(self.yugoNP) # Right front wheel np = loader.loadModel('models/yugo/yugotireR.egg') np.reparentTo(self.worldNP) self.addWheel(Point3(0.70, 1.05, 0.3), True, np) # Left front wheel np = loader.loadModel('models/yugo/yugotireL.egg') np.reparentTo(self.worldNP) self.addWheel(Point3(-0.70, 1.05, 0.3), True, np) # Right rear wheel np = loader.loadModel('models/yugo/yugotireR.egg') np.reparentTo(self.worldNP) self.addWheel(Point3(0.70, -1.05, 0.3), False, np) # Left rear wheel np = loader.loadModel('models/yugo/yugotireL.egg') np.reparentTo(self.worldNP) self.addWheel(Point3(-0.70, -1.05, 0.3), False, np) # Steering info self.steering = 0.0 # degree self.steeringClamp = 38.0 #45.0 # degree self.steeringIncrement = 105.0 #120.0 # degree per second # add previous positions self.prevPos = [] self.prevPos.append(self.yugoNP.getPos(render)) self.model_offset = 0.5 if self.model.activation == 'relu' else 0. # Box """ for i,j in [(0,8),(-3,5),(6,-5),(8,3),(-4,-4),(0,0)]: shape = BulletBoxShape(Vec3(0.5, 0.5, 0.5)) # https://discourse.panda3d.org/t/wall-collision-help/23606 np = self.worldNP.attachNewNode(BulletRigidBodyNode('Box')) np.node().setMass(1.0) np.node().addShape(shape) np.setPos(i, j, 2) np.setCollideMask(BitMask32.allOn())#(0x0f)) self.world.attachRigidBody(np.node()) self.boxNP = np #self.colHandler2 = CollisionHandlerQueue() visualNP = loader.loadModel('models/box.egg') visualNP.reparentTo(self.boxNP) #self.cTrav.addCollider(self.boxNP,self.colHandler) """ """ aNode = CollisionNode("TheRay") self.ray = CollisionRay() self.ray.setOrigin( self.yugoNP.getPos() ) self.ray.setDirection( Vec3(0, 10, 0) ) #self.ray.show() aNodePath = self.yugoNP.attachNewNode( CollisionNode("TheRay") ) aNodePath.node().addSolid(self.ray) aNodePath.show() """ #aNode.addSolid(self.ray) #self.ray = CollisionRay(0,0,0,10,0,0) #self.ray.reparentTo(self.yugoNP) #self.rayColl = CollisionNode('PlayerRay') #self.rayColl.addSolid(self.ray) #self.playerRayNode = self.yugoNP.attachNewNode( self.rayColl ) #self.playerRayNode.show() #base.myTraverser.addCollider (self.playerRayNode, base.floor) #base.floor.addCollider( self.playerRayNode, self.yugoNP) """ MyEvent=CollisionHandlerFloor() MyEvent.setReach(100) MyEvent.setOffset(15.0) aNode = CollisionNode("TheRay") ray = CollisionRay() ray.setOrigin( self.boxNP.getPos() ) ray.setDirection( Vec3(10, 0, 0) ) aNode.addSolid(ray) aNodePath = MyModel.attachNewNode( aNode ) Collision = ( aNode, "TheRay" ) Collision[0].setFromCollideMask( BitMask32.bit( 1 ) ) """ def addWheel(self, pos, front, np): wheel = self.vehicle.createWheel() wheel.setNode(np.node()) wheel.setChassisConnectionPointCs(pos) wheel.setFrontWheel(front) wheel.setWheelDirectionCs(Vec3(0, 0, -1)) wheel.setWheelAxleCs(Vec3(1, 0, 0)) wheel.setWheelRadius(0.25) wheel.setMaxSuspensionTravelCm(40.0) wheel.setSuspensionStiffness(40.0) wheel.setWheelsDampingRelaxation(2.3) wheel.setWheelsDampingCompression(4.4) wheel.setFrictionSlip(100.0) wheel.setRollInfluence(0.1)
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 Vehicle(object): COUNT = 0 def __init__(self, bulletWorld, pos, username, isMainChar=False): self.specs = { "mass": 400.0, "maxWheelForce": 2000.0, "brakeForce": 100.0, "steeringLock": 45.0 } self.vehicleControlState = { "throttle": 0, "reverse": False, "brake": 0.0, "steering": 0.0 } self.username = username self.isMainChar = isMainChar # Steering change per second, normalised to steering lock # Eg. 45 degrees lock and 1.0 rate means 45 degrees per second self.steeringRate = 0.8 self.centreingRate = 1.2 self.pos = pos self.currentPowerups = { "powerup1": None, "powerup2": None, "powerup3": None } self.setupVehicle(bulletWorld) self.startTime = time.time() COUNT = 1 def move(self, steering, wheelForce, brakeForce, x, y, z, h, p, r): self.applyForcesAndSteering(steering, wheelForce, brakeForce) self.endTime = time.time() #print self.endTime elapsed = self.endTime - self.startTime #if elapsed > .1: #print "Set Pos Now" self.setVehiclePos(x, y, z, h, p, r) self.startTime = self.endTime #print "Do Move" def applyForcesAndSteering(self, steering, wheelForce, brakeForce): # 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) def processInput(self, inputState, dt): # print self.chassisNP.getPos() #print self.chassisNP.getH() """Use controls to update the player's car""" # For keyboard throttle and brake are either 0 or 1 if inputState.isSet('forward'): self.vehicleControlState["throttle"] = 1.0 else: self.vehicleControlState["throttle"] = 0.0 velocity = self.chassisNode.getLinearVelocity() speed = math.sqrt(sum(v**2 for v in velocity)) # Update braking and reversing if inputState.isSet('brake'): if speed < 0.5 or self.vehicleControlState["reverse"]: # If we're stopped, then start reversing # Also keep reversing if we already were self.vehicleControlState["reverse"] = True self.vehicleControlState["throttle"] = 1.0 self.vehicleControlState["brake"] = 0.0 else: self.vehicleControlState["reverse"] = False self.vehicleControlState["brake"] = 1.0 else: self.vehicleControlState["reverse"] = False self.vehicleControlState["brake"] = 0.0 # steering is normalised from -1 to 1, corresponding # to the steering lock right and left steering = self.vehicleControlState["steering"] if inputState.isSet('left'): steering += dt * self.steeringRate steering = min(steering, 1.0) elif inputState.isSet('right'): steering -= dt * self.steeringRate steering = max(steering, -1.0) else: # gradually re-center the steering if steering > 0.0: steering -= dt * self.centreingRate if steering < 0.0: steering = 0.0 elif steering < 0.0: steering += dt * self.centreingRate if steering > 0.0: steering = 0.0 self.vehicleControlState["steering"] = steering # """Updates acceleration, braking and steering # These are all passed in through a controlState dictionary # """ # Update acceleration and braking wheelForce = self.vehicleControlState["throttle"] * self.specs[ "maxWheelForce"] self.reversing = self.vehicleControlState["reverse"] if self.reversing: # Make reversing a bit slower than moving forward wheelForce *= -0.5 brakeForce = self.vehicleControlState["brake"] * self.specs[ "brakeForce"] # Update steering # Steering control state is from -1 to 1 steering = self.vehicleControlState["steering"] * self.specs[ "steeringLock"] self.applyForcesAndSteering(steering, wheelForce, brakeForce) return [steering, wheelForce, brakeForce] def setVehiclePos(self, x, y, z, h, p, r): self.chassisNP.setPosHpr(x, y, z, h, p, r) def setupVehicle(self, bulletWorld): # Chassis shape = BulletBoxShape(Vec3(1, 2.2, 0.5)) ts = TransformState.makePos(Point3(0, 0, .7)) if self.isMainChar: self.chassisNode = BulletRigidBodyNode(self.username) else: self.chassisNode = BulletRigidBodyNode('vehicle') self.chassisNode.setTag("username", str(self.username)) self.chassisNP = render.attachNewNode(self.chassisNode) self.chassisNP.node().addShape(shape, ts) if self.isMainChar: self.chassisNP.node().notifyCollisions(True) else: self.chassisNP.node().notifyCollisions(False) self.setVehiclePos(self.pos[0], self.pos[1], self.pos[2], self.pos[3], self.pos[4], self.pos[5]) # self.chassisNP.setPos(-5.34744, 114.773, 6) #self.chassisNP.setPos(49.2167, 64.7968, 10) self.chassisNP.node().setMass(800.0) self.chassisNP.node().setDeactivationEnabled(False) bulletWorld.attachRigidBody(self.chassisNP.node()) #np.node().setCcdSweptSphereRadius(1.0) #np.node().setCcdMotionThreshold(1e-7) # Vehicle self.vehicle = BulletVehicle(bulletWorld, self.chassisNP.node()) self.vehicle.setCoordinateSystem(ZUp) bulletWorld.attachVehicle(self.vehicle) self.carNP = loader.loadModel('models/batmobile-chassis.egg') #self.yugoNP.setScale(.7) self.carNP.reparentTo(self.chassisNP) # Right front wheel np = loader.loadModel('models/batmobile-wheel-right.egg') np.reparentTo(render) self.addWheel(Point3(1, 1.1, .7), True, np) # Left front wheel np = loader.loadModel('models/batmobile-wheel-left.egg') np.reparentTo(render) self.addWheel(Point3(-1, 1.1, .7), True, np) # Right rear wheel np = loader.loadModel('models/batmobile-wheel-right.egg') np.reparentTo(render) self.addWheel(Point3(1, -2, .7), False, np) # Left rear wheel np = loader.loadModel('models/batmobile-wheel-left.egg') np.reparentTo(render) self.addWheel(Point3(-1, -2, .7), False, np) def addWheel(self, pos, front, np): wheel = self.vehicle.createWheel() wheel.setNode(np.node()) wheel.setChassisConnectionPointCs(pos) wheel.setFrontWheel(front) wheel.setWheelDirectionCs(Vec3(0, 0, -1)) wheel.setWheelAxleCs(Vec3(1, 0, 0)) wheel.setWheelRadius(0.33) wheel.setMaxSuspensionTravelCm(40.0) wheel.setSuspensionStiffness(40.0) wheel.setWheelsDampingRelaxation(2.3) wheel.setWheelsDampingCompression(4.4) wheel.setFrictionSlip(100.0) wheel.setRollInfluence(0.1) def reset(self): self.chassisNP.setP(0) self.chassisNP.setR(0) def pickedPowerup(self, powerup): if not powerup.pickable: powerup.useAbility() else: if self.currentPowerups["powerup1"] is None: self.currentPowerups["powerup1"] = powerup elif self.currentPowerups["powerup2"] is None: self.currentPowerups["powerup2"] = powerup elif self.currentPowerups["powerup3"] is None: self.currentPowerups["powerup3"] = powerup def canPickUpPowerup(self): return (self.currentPowerups["powerup1"] is None or self.currentPowerups["powerup2"] is None or self.currentPowerups["powerup3"] is None) def usePowerup(self, powerupIndex): # Move usePowerupN to this function if powerupIndex == 0 and self.currentPowerups["powerup1"] is not None: self.currentPowerups["powerup1"].useAbility() self.currentPowerups["powerup1"] = None elif powerupIndex == 1 and self.currentPowerups["powerup2"] is not None: self.currentPowerups["powerup2"].useAbility() self.currentPowerups["powerup2"] = None elif powerupIndex == 2 and self.currentPowerups["powerup3"] is not None: self.currentPowerups["powerup3"].useAbility() self.currentPowerups["powerup3"] = None
class Game(DirectObject): def __init__(self): base.setBackgroundColor(0.1, 0.1, 0.8, 1) base.setFrameRateMeter(True) # Light alight = AmbientLight('ambientLight') alight.setColor(Vec4(0.5, 0.5, 0.5, 1)) alightNP = render.attachNewNode(alight) dlight = DirectionalLight('directionalLight') dlight.setDirection(Vec3(1, 1, -1)) dlight.setColor(Vec4(0.7, 0.7, 0.7, 1)) dlightNP = render.attachNewNode(dlight) render.clearLight() render.setLight(alightNP) render.setLight(dlightNP) # Input self.accept('escape', self.doExit) self.accept('r', self.doReset) self.accept('f1', self.toggleWireframe) self.accept('f2', self.toggleTexture) self.accept('f3', self.toggleDebug) self.accept('f5', self.doScreenshot) # ROS self.crash_pub = rospy.Publisher('crash', std_msgs.msg.Empty, queue_size=100) ### subscribers (info sent to Arduino) self.cmd_steer_sub = rospy.Subscriber( 'cmd/steer', std_msgs.msg.Float32, callback=self._cmd_steer_callback) self.cmd_motor_sub = rospy.Subscriber( 'cmd/motor', std_msgs.msg.Float32, callback=self._cmd_motor_callback) self.cmd_vel_sub = rospy.Subscriber('cmd/vel', std_msgs.msg.Float32, callback=self._cmd_vel_callback) self.reset_sub = rospy.Subscriber('reset', std_msgs.msg.Empty, callback=self._reset_callback) self.cmd_steer_queue = Queue.Queue(maxsize=1) self.cmd_motor_queue = Queue.Queue(maxsize=1) # Task taskMgr.add(self.update, 'updateWorld') # Physics self.setup() print('Starting ROS thread') threading.Thread(target=self._ros_servos_thread).start() threading.Thread(target=self._ros_crash_thread).start() threading.Thread(target=self._ros_image_thread).start() # Callbacks def _cmd_steer_callback(self, msg): if msg.data >= 0 and msg.data <= 99.0: self.cmd_steer_queue.put(msg.data) def _cmd_motor_callback(self, msg): if msg.data >= 0 and msg.data <= 99.0: self.cmd_motor_queue.put(msg.data) def _cmd_vel_callback(self, msg): data = numpy.clip(msg.data * 3 + 49.5, 0, 99.0) self.cmd_motor_queue.put(data) def _reset_callback(self, msg): self.doReset() # ROS thread def _ros_servos_thread(self): """ Publishes/subscribes to Ros. """ self.steering = 0.0 # degree self.steeringClamp = rospy.get_param('~steeringClamp') self.engineForce = 0.0 self.engineClamp = rospy.get_param('~engineClamp') r = rospy.Rate(60) while not rospy.is_shutdown(): r.sleep() for var, queue in (('steer', self.cmd_steer_queue), ('motor', self.cmd_motor_queue)): if not queue.empty(): try: if var == 'steer': self.steering = self.steeringClamp * ( (queue.get() - 49.5) / 49.5) self.vehicle.setSteeringValue(self.steering, 0) self.vehicle.setSteeringValue(self.steering, 1) elif var == 'motor': self.vehicle.setBrake(100.0, 2) self.vehicle.setBrake(100.0, 3) self.engineForce = self.engineClamp * ( (queue.get() - 49.5) / 49.5) self.vehicle.applyEngineForce(self.engineForce, 2) self.vehicle.applyEngineForce(self.engineForce, 3) except Exception as e: print(e) def _ros_crash_thread(self): crash = 0 r = rospy.Rate(30) while not rospy.is_shutdown(): r.sleep() try: result = self.world.contactTest(self.vehicle_node) if result.getNumContacts() > 0: self.crash_pub.publish(std_msgs.msg.Empty()) except Exception as e: print(e) def _ros_image_thread(self): camera_pub = ImageROSPublisher("image") depth_pub = ImageROSPublisher("depth") r = rospy.Rate(30) i = 0 while not rospy.is_shutdown(): r.sleep() # sometimes fails to observe try: obs = self.camera_sensor.observe() camera_pub.publish_image(obs[0]) depth_pub.publish_image(obs[1], image_format="passthrough") except: pass # _____HANDLER_____ def doExit(self): self.cleanup() sys.exit(1) def doReset(self): self.cleanup() self.setup() def toggleWireframe(self): base.toggleWireframe() def toggleTexture(self): base.toggleTexture() def toggleDebug(self): if self.debugNP.isHidden(): self.debugNP.show() else: self.debugNP.hide() def doScreenshot(self): base.screenshot('Bullet') # ____TASK___ def update(self, task): dt = globalClock.getDt() self.world.doPhysics(dt, 10, 0.008) #print self.vehicle.getWheel(0).getRaycastInfo().isInContact() #print self.vehicle.getWheel(0).getRaycastInfo().getContactPointWs() #print self.vehicle.getChassis().isKinematic() return task.cont def cleanup(self): self.world = None self.worldNP.removeNode() def setup(self): self.worldNP = render.attachNewNode('World') # World self.debugNP = self.worldNP.attachNewNode(BulletDebugNode('Debug')) self.world = BulletWorld() self.world.setGravity(Vec3(0, 0, -9.81)) self.world.setDebugNode(self.debugNP.node()) # Plane shape = BulletPlaneShape(Vec3(0, 0, 1), 0) np = self.ground = self.worldNP.attachNewNode( BulletRigidBodyNode('Ground')) np.node().addShape(shape) np.setPos(0, 0, -1) np.setCollideMask(BitMask32.allOn()) self.world.attachRigidBody(np.node()) # collision self.maze = [] for pos in [(0.0, 72.0, 0.0), (-11.0, 60.0, 0.0), (11.0, 60.0, 0.0), (-11.0, 48.0, 0.0), (11.0, 48.0, 0.0), (-11.0, 36.0, 0.0), (11.0, 36.0, 0.0), (-11.0, 24.0, 0.0), (11.0, 24.0, 0.0), (-11.0, 12.0, 0.0), (11.0, 12.0, 0.0), (-11.0, 0.0, 0.0), (11.0, 0.0, 0.0), (0.0, -12.0, 0.0), (0.5, 12.0, 1.0), (-0.5, 12.0, 1.0)]: translate = False if (abs(pos[0]) == 0.5): translate = True visNP = loader.loadModel('../models/ball.egg') else: visNP = loader.loadModel('../models/maze.egg') visNP.clearModelNodes() visNP.reparentTo(self.ground) visNP.setPos(pos[0], pos[1], pos[2]) bodyNPs = BulletHelper.fromCollisionSolids(visNP, True) for bodyNP in bodyNPs: bodyNP.reparentTo(render) if translate: bodyNP.setPos(pos[0], pos[1], pos[2] - 1) else: bodyNP.setPos(pos[0], pos[1], pos[2]) if isinstance(bodyNP.node(), BulletRigidBodyNode): bodyNP.node().setMass(0.0) bodyNP.node().setKinematic(True) self.maze.append(bodyNP) for bodyNP in self.maze: self.world.attachRigidBody(bodyNP.node()) # Chassis mass = rospy.get_param('~mass') #chassis_shape = rospy.get_param('~chassis_shape') shape = BulletBoxShape(Vec3(0.6, 1.4, 0.5)) ts = TransformState.makePos(Point3(0, 0, 0.5)) np = self.worldNP.attachNewNode(BulletRigidBodyNode('Vehicle')) np.node().addShape(shape, ts) rand_vals = numpy.random.random(2) * 8 - 4.0 np.setPos(rand_vals[0], 0.0, -0.6) np.node().setMass(mass) np.node().setDeactivationEnabled(False) first_person = rospy.get_param('~first_person') self.camera_sensor = Panda3dCameraSensor(base, color=True, depth=True, size=(160, 90)) self.camera_node = self.camera_sensor.cam if first_person: self.camera_node.reparentTo(np) self.camera_node.setPos(0.0, 1.0, 1.0) self.camera_node.lookAt(0.0, 6.0, 0.0) else: self.camera_node.reparentTo(np) self.camera_node.setPos(0.0, -10.0, 5.0) self.camera_node.lookAt(0.0, 5.0, 0.0) base.cam.reparentTo(np) base.cam.setPos(0.0, -10.0, 5.0) base.cam.lookAt(0.0, 5.0, 0.0) self.world.attachRigidBody(np.node()) np.node().setCcdSweptSphereRadius(1.0) np.node().setCcdMotionThreshold(1e-7) # Vehicle self.vehicle_node = np.node() self.vehicle = BulletVehicle(self.world, np.node()) self.vehicle.setCoordinateSystem(ZUp) self.world.attachVehicle(self.vehicle) self.yugoNP = loader.loadModel('../models/yugo/yugo.egg') self.yugoNP.reparentTo(np) # Right front wheel np = loader.loadModel('../models/yugo/yugotireR.egg') np.reparentTo(self.worldNP) self.addWheel(Point3(0.70, 1.05, 0.3), True, np) # Left front wheel np = loader.loadModel('../models/yugo/yugotireL.egg') np.reparentTo(self.worldNP) self.addWheel(Point3(-0.70, 1.05, 0.3), True, np) # Right rear wheel np = loader.loadModel('../models/yugo/yugotireR.egg') np.reparentTo(self.worldNP) self.addWheel(Point3(0.70, -1.05, 0.3), False, np) # Left rear wheel np = loader.loadModel('../models/yugo/yugotireL.egg') np.reparentTo(self.worldNP) self.addWheel(Point3(-0.70, -1.05, 0.3), False, np) # Collision handle #base.cTrav = CollisionTraverser() #self.notifier = CollisionHandlerEvent() #self.notifier.addInPattern("%fn") #self.accept("Vehicle", self.onCollision) def onCollision(self): print("crash") def addWheel(self, pos, front, np): wheel = self.vehicle.createWheel() wheel.setNode(np.node()) wheel.setChassisConnectionPointCs(pos) wheel.setFrontWheel(front) wheel.setWheelDirectionCs(Vec3(0, 0, -1)) wheel.setWheelAxleCs(Vec3(1, 0, 0)) wheel.setWheelRadius(0.25) wheel.setMaxSuspensionTravelCm(40.0) wheel.setSuspensionStiffness(40.0) wheel.setWheelsDampingRelaxation(2.3) wheel.setWheelsDampingCompression(4.4) wheel.setFrictionSlip(1e3) wheel.setRollInfluence(0.0)
def __init__(self, game, pos, vel, length, width, height, axisDis, wheelDis, radius, wheelH): self.initCordPos = numpy.array([pos[0], pos[1]]) '''self.length=length self.width=width self.height=height self.axisDis=axisDis self.wheelDis=wheelDis self.radius=radius self.wheelH=wheelH''' line = game.segLine lineLen = 0 prevPoint = line[0] self.initNum = 0 for point in line: norm = numpy.linalg.norm(point - prevPoint) if pos[0] >= lineLen and pos[0] < lineLen + norm: ldirec = (point - prevPoint) / norm rdirec = numpy.array([ldirec[1], -ldirec[0]]) post = prevPoint + (pos[0] - lineLen) * ldirec + rdirec * pos[1] self.initPos = Vec3(post[0], post[1], pos[2]) break lineLen = lineLen + norm prevPoint = point self.initNum = self.initNum + 1 chassis = setChassis(game, self.initPos, vel, length, width, height) BulletVehicle.__init__(self, game.world, chassis.node()) self.setCoordinateSystem(ZUp) game.world.attachVehicle(self) #self.yugoNP = loader.loadModel('models/yugo/yugo.egg') self.yugoNP = loader.loadModel('models/car.egg') self.yugoNP.reparentTo(chassis) # Right front wheel np = loader.loadModel('models/yugo/yugotireR.egg') np.reparentTo(game.worldNP) self.addWheel(Point3(wheelDis / 2, axisDis / 2, wheelH), True, np, radius) # Left front wheel np = loader.loadModel('models/yugo/yugotireL.egg') np.reparentTo(game.worldNP) self.addWheel(Point3(-wheelDis / 2, axisDis / 2, wheelH), True, np, radius) # Right rear wheel np = loader.loadModel('models/yugo/yugotireR.egg') np.reparentTo(game.worldNP) self.addWheel(Point3(wheelDis / 2, -axisDis / 2, wheelH), False, np, radius) # Left rear wheel np = loader.loadModel('models/yugo/yugotireL.egg') np.reparentTo(game.worldNP) self.addWheel(Point3(-wheelDis / 2, -axisDis / 2, wheelH), False, np, radius) # Steering info self.steering = 0.0 # degree self.steeringClamp = 45.0 # degree self.steeringIncrement = 10.0 # degree per second
def setup(self): # Bullet physics world self.worldNP = render.attach_new_node('World') self.debugNP = self.worldNP.attach_new_node(BulletDebugNode('Debug')) self.world = BulletWorld() self.world.set_gravity(LVector3(0, 0, -9.81)) self.world.set_debug_node(self.debugNP.node()) # Vehicle # Steering info self.steering = 0.0 # degrees self.steering_clamp = 45.0 # degrees self.steering_increment = 120.0 # degrees per second # How fast steering relaxes to straight ahead self.steering_relax_factor = 2.0 # How much steering intensity decreases with higher speeds self.steering_speed_reduction_factor = 0.003 # Chassis collision box (note: Bullet uses half-measures) shape = BulletBoxShape(LVector3(0.6, 1.4, 0.5)) ts = TransformState.make_pos(LPoint3(0, 0, 0.5)) self.vehicleNP = self.worldNP.attach_new_node( BulletRigidBodyNode('Vehicle')) self.vehicleNP.node().add_shape(shape, ts) # Set initial position self.vehicleNP.set_pos(0, 70, -10) self.vehicleNP.node().set_mass(800.0) self.vehicleNP.node().set_deactivation_enabled(False) self.world.attach(self.vehicleNP.node()) # Camera floater self.attach_camera_floater() # BulletVehicle self.vehicle = BulletVehicle(self.world, self.vehicleNP.node()) self.world.attach(self.vehicle) # Vehicle model self.yugoNP = loader.load_model('models/yugo/yugo.egg') self.yugoNP.reparent_to(self.vehicleNP) # Right front wheel np = loader.load_model('models/yugo/yugotireR.egg') np.reparent_to(self.worldNP) self.add_wheel(LPoint3( 0.70, 1.05, 0.3), True, np) # Left front wheel np = loader.load_model('models/yugo/yugotireL.egg') np.reparent_to(self.worldNP) self.add_wheel(LPoint3(-0.70, 1.05, 0.3), True, np) # Right rear wheel np = loader.load_model('models/yugo/yugotireR.egg') np.reparent_to(self.worldNP) self.add_wheel(LPoint3( 0.70, -1.05, 0.3), False, np) # Left rear wheel np = loader.load_model('models/yugo/yugotireL.egg') np.reparent_to(self.worldNP) self.add_wheel(LPoint3(-0.70, -1.05, 0.3), False, np) # Load a skybox self.skybox = self.loader.load_model( "samples/shader-terrain/models/skybox.bam") self.skybox.reparent_to(self.render) self.skybox.set_scale(20000) skybox_texture = self.loader.load_texture( "samples/shader-terrain/textures/skybox.jpg") skybox_texture.set_minfilter(SamplerState.FT_linear) skybox_texture.set_magfilter(SamplerState.FT_linear) skybox_texture.set_wrap_u(SamplerState.WM_repeat) skybox_texture.set_wrap_v(SamplerState.WM_mirror) skybox_texture.set_anisotropic_degree(16) self.skybox.set_texture(skybox_texture) skybox_shader = Shader.load(Shader.SL_GLSL, "samples/shader-terrain/skybox.vert.glsl", "samples/shader-terrain/skybox.frag.glsl") self.skybox.set_shader(skybox_shader) # Terrain self.setup_terrain()
class Character: playerId = 1 type = 0 def __init__(self, tempworld, bulletWorld, type, playerId): self.world = tempworld self.speed = 0 self.acceleration = 1.5 self.brakes = .7 self.min_speed = 0 self.max_speed = 150 self.reverse_speed = 20 self.reverse_limit = -40 self.armor = 100 self.health = 100 self.a_timer_start = time.time() self.a_timer_end = time.time() self.power_ups = [0, 0, 0] self.playerId = playerId if type == 0: self.actor = Actor("models/batmobile-chassis") self.actor.setScale(0.7) carRadius = 3 elif type == 1: self.actor = Actor("models/policecarpainted", {}) self.actor.setScale(0.30) self.actor.setH(180) # elif type == 2: # self.actor = loader.loadModel("knucklehead.egg") # self.tex = loader.loadTexture("knucklehead.jpg") # self.actor.setTexture(self.car_tex, 1) shape = BulletBoxShape(Vec3(1.0, 1.5, 0.4)) ts = TransformState.makePos(Point3(0, 0, 0.6)) self.chassisNP = render.attachNewNode(BulletRigidBodyNode('Vehicle')) self.chassisNP.node().addShape(shape, ts) self.chassisNP.setPos(50 * random.random(), 50 * random.random(), 1) self.chassisNP.setH(180) self.chassisNP.node().setMass(800.0) self.chassisNP.node().setDeactivationEnabled(False) bulletWorld.attachRigidBody(self.chassisNP.node()) self.actor.reparentTo(self.chassisNP) self.actor.setH(180) # Vehicle self.vehicle = BulletVehicle(bulletWorld, self.chassisNP.node()) self.vehicle.setCoordinateSystem(ZUp) bulletWorld.attachVehicle(self.vehicle) for fb, y in (("F", 1.1), ("B", -1.1)): for side, x in (("R", 0.75), ("L", -0.75)): np = loader.loadModel("models/tire%s.egg" % side) np.reparentTo(render) isFront = fb == "F" self.addWheel(Point3(x, y, 0.55), isFront, np) def addWheel(self, position, isFront, np): wheel = self.vehicle.createWheel() wheel.setNode(np.node()) wheel.setChassisConnectionPointCs(position) wheel.setFrontWheel(isFront) wheel.setWheelDirectionCs(Vec3(0, 0, -1)) wheel.setWheelAxleCs(Vec3(1, 0, 0)) wheel.setWheelRadius(0.2) wheel.setMaxSuspensionTravelCm(0.4 * 100.0) wheel.setSuspensionStiffness(40.0) wheel.setWheelsDampingRelaxation(2.3) wheel.setWheelsDampingCompression(4.4) wheel.setFrictionSlip(100) wheel.setRollInfluence(0.1) def getActor(self): return self.actor def get_speed(self): return self.speed def accelerate(self): # check how long you were accelerating time_elapsed = time.time() - self.a_timer_end if time_elapsed > 1: # in seconds last accelerated self.a_timer_start = time.time() if self.speed < self.max_speed: self.speed += self.acceleration * (time.time() - self.a_timer_start) # print(time_elapsed) else: self.speed = self.max_speed # reset timer self.a_timer_end = time.time() def friction(self, friction): if self.speed > 0: self.speed -= friction else: self.speed = 0 def brake(self): if self.speed > self.min_speed: self.speed -= self.brakes # reset acceleration timer self.a_timer_start = time.time() def reverse(self): if self.speed > self.reverse_limit: self.speed -= self.reverse_speed def walk(self): self.actor.stop() self.actor.pose("walk", 5) self.isMoving = False def run(self): self.actor.loop("run") self.isMoving = True
def setup(self): self.worldNP = render.attachNewNode('World') # World self.debugNP = self.worldNP.attachNewNode(BulletDebugNode('Debug')) self.debugNP.show() self.world = BulletWorld() self.world.setGravity(Vec3(0, 0, -9.81)) self.world.setDebugNode(self.debugNP.node()) # Plane shape = BulletPlaneShape(Vec3(0, 0, 1), 0) np = self.worldNP.attachNewNode(BulletRigidBodyNode('Ground')) np.node().addShape(shape) np.setPos(0, 0, -1) np.setCollideMask(BitMask32.allOn()) self.world.attachRigidBody(np.node()) # Chassis shape = BulletBoxShape(Vec3(0.6, 1.4, 0.5)) ts = TransformState.makePos(Point3(0, 0, 0.5)) np = self.worldNP.attachNewNode(BulletRigidBodyNode('Vehicle')) np.node().addShape(shape, ts) np.setPos(0, 0, 1) np.node().setMass(800.0) np.node().setDeactivationEnabled(False) self.world.attachRigidBody(np.node()) #np.node().setCcdSweptSphereRadius(1.0) #np.node().setCcdMotionThreshold(1e-7) # Vehicle self.vehicle = BulletVehicle(self.world, np.node()) self.vehicle.setCoordinateSystem(ZUp) self.world.attachVehicle(self.vehicle) self.yugoNP = loader.loadModel('car/yugo.egg') self.yugoNP.reparentTo(np) # Right front wheel np = loader.loadModel('car/yugotireR.egg') np.reparentTo(self.worldNP) self.addWheel(Point3(0.70, 1.05, 0.3), True, np) # Left front wheel np = loader.loadModel('car/yugotireL.egg') np.reparentTo(self.worldNP) self.addWheel(Point3(-0.70, 1.05, 0.3), True, np) # Right rear wheel np = loader.loadModel('car/yugotireR.egg') np.reparentTo(self.worldNP) self.addWheel(Point3(0.70, -1.05, 0.3), False, np) # Left rear wheel np = loader.loadModel('car/yugotireL.egg') np.reparentTo(self.worldNP) self.addWheel(Point3(-0.70, -1.05, 0.3), False, np) # Steering info self.steering = 0.0 # degree self.steeringClamp = 45.0 # degree self.steeringIncrement = 120.0 # degree per second
class Game(DirectObject): def __init__(self): base.setBackgroundColor(0.1, 0.1, 0.8, 1) base.setFrameRateMeter(True) base.cam.setPos(0, -20, 4) base.cam.lookAt(0, 0, 0) # Light alight = AmbientLight('ambientLight') alight.setColor(Vec4(0.5, 0.5, 0.5, 1)) alightNP = render.attachNewNode(alight) dlight = DirectionalLight('directionalLight') dlight.setDirection(Vec3(1, 1, -1)) dlight.setColor(Vec4(0.7, 0.7, 0.7, 1)) dlightNP = render.attachNewNode(dlight) render.clearLight() render.setLight(alightNP) render.setLight(dlightNP) # Input self.accept('escape', self.doExit) self.accept('r', self.doReset) self.accept('f1', self.toggleWireframe) self.accept('f2', self.toggleTexture) self.accept('f3', self.toggleDebug) self.accept('f5', self.doScreenshot) inputState.watchWithModifiers('forward', 'w') inputState.watchWithModifiers('left', 'a') inputState.watchWithModifiers('reverse', 's') inputState.watchWithModifiers('right', 'd') inputState.watchWithModifiers('turnLeft', 'q') inputState.watchWithModifiers('turnRight', 'e') # Task taskMgr.add(self.update, 'updateWorld') # Physics self.setup() # _____HANDLER_____ def doExit(self): self.cleanup() sys.exit(1) def doReset(self): self.cleanup() self.setup() def toggleWireframe(self): base.toggleWireframe() def toggleTexture(self): base.toggleTexture() def toggleDebug(self): if self.debugNP.isHidden(): self.debugNP.show() else: self.debugNP.hide() def doScreenshot(self): base.screenshot('Bullet') # ____TASK___ def processInput(self, dt): engineForce = 0.0 brakeForce = 0.0 if inputState.isSet('forward'): engineForce = 1000.0 brakeForce = 0.0 if inputState.isSet('reverse'): engineForce = 0.0 brakeForce = 100.0 if inputState.isSet('turnLeft'): self.steering += dt * self.steeringIncrement self.steering = min(self.steering, self.steeringClamp) if inputState.isSet('turnRight'): self.steering -= dt * self.steeringIncrement self.steering = max(self.steering, -self.steeringClamp) # Apply steering to front wheels self.vehicle.setSteeringValue(self.steering, 0); self.vehicle.setSteeringValue(self.steering, 1); # Apply engine and brake to rear wheels self.vehicle.applyEngineForce(engineForce, 2); self.vehicle.applyEngineForce(engineForce, 3); self.vehicle.setBrake(brakeForce, 2); self.vehicle.setBrake(brakeForce, 3); def update(self, task): dt = globalClock.getDt() self.processInput(dt) self.world.doPhysics(dt, 10, 0.008) #print self.vehicle.getWheel(0).getRaycastInfo().isInContact() #print self.vehicle.getWheel(0).getRaycastInfo().getContactPointWs() #print self.vehicle.getChassis().isKinematic() return task.cont def cleanup(self): self.world = None self.worldNP.removeNode() def setup(self): self.worldNP = render.attachNewNode('World') # World self.debugNP = self.worldNP.attachNewNode(BulletDebugNode('Debug')) self.debugNP.show() self.world = BulletWorld() self.world.setGravity(Vec3(0, 0, -9.81)) self.world.setDebugNode(self.debugNP.node()) # Plane shape = BulletPlaneShape(Vec3(0, 0, 1), 0) np = self.worldNP.attachNewNode(BulletRigidBodyNode('Ground')) np.node().addShape(shape) np.setPos(0, 0, -1) np.setCollideMask(BitMask32.allOn()) self.world.attachRigidBody(np.node()) # Chassis shape = BulletBoxShape(Vec3(0.6, 1.4, 0.5)) ts = TransformState.makePos(Point3(0, 0, 0.5)) np = self.worldNP.attachNewNode(BulletRigidBodyNode('Vehicle')) np.node().addShape(shape, ts) np.setPos(0, 0, 1) np.node().setMass(800.0) np.node().setDeactivationEnabled(False) self.world.attachRigidBody(np.node()) #np.node().setCcdSweptSphereRadius(1.0) #np.node().setCcdMotionThreshold(1e-7) # Vehicle self.vehicle = BulletVehicle(self.world, np.node()) self.vehicle.setCoordinateSystem(ZUp) self.world.attachVehicle(self.vehicle) self.yugoNP = loader.loadModel('models/yugo/yugo.egg') self.yugoNP.reparentTo(np) # Right front wheel np = loader.loadModel('models/yugo/yugotireR.egg') np.reparentTo(self.worldNP) self.addWheel(Point3( 0.70, 1.05, 0.3), True, np) # Left front wheel np = loader.loadModel('models/yugo/yugotireL.egg') np.reparentTo(self.worldNP) self.addWheel(Point3(-0.70, 1.05, 0.3), True, np) # Right rear wheel np = loader.loadModel('models/yugo/yugotireR.egg') np.reparentTo(self.worldNP) self.addWheel(Point3( 0.70, -1.05, 0.3), False, np) # Left rear wheel np = loader.loadModel('models/yugo/yugotireL.egg') np.reparentTo(self.worldNP) self.addWheel(Point3(-0.70, -1.05, 0.3), False, np) # Steering info self.steering = 0.0 # degree self.steeringClamp = 45.0 # degree self.steeringIncrement = 120.0 # degree per second def addWheel(self, pos, front, np): wheel = self.vehicle.createWheel() wheel.setNode(np.node()) wheel.setChassisConnectionPointCs(pos) wheel.setFrontWheel(front) wheel.setWheelDirectionCs(Vec3(0, 0, -1)) wheel.setWheelAxleCs(Vec3(1, 0, 0)) wheel.setWheelRadius(0.25) wheel.setMaxSuspensionTravelCm(40.0) wheel.setSuspensionStiffness(40.0) wheel.setWheelsDampingRelaxation(2.3) wheel.setWheelsDampingCompression(4.4) wheel.setFrictionSlip(100.0); wheel.setRollInfluence(0.1)
class Vehicle(object): COUNT = 0 def __init__(self, bulletWorld, pos, username): self.world = bulletWorld self.acceleration = 1.5 self.brakeForce = 100.0 self.mass = 800 self.max_speed = 150 self.reverse_limit = -40 self.mass = 800.0 # kg self.max_speed = 150 # km self.armor = 100 self.health = 100 self.friction = 0.2 #slows the car when not accelerating (based on the brakes) self.maxWheelForce = 2000.0 self.maxWheelForce = 2000.0 # acceleration self.power_ups = [0, 0, 0] self.specs = {"mass": self.mass, "maxWheelForce": self.maxWheelForce, "brakeForce": self.brakeForce, "steeringLock": 20.0} self.vehicleControlState = {"throttle": 0, "reverse": False, "brake": 0.0, "steering": 0.0} self.username = username # Steering change per second, normalised to steering lock # Eg. 45 degrees lock and 1.0 rate means 45 degrees per second self.steeringRate = 0.7 self.centreingRate = 5.0 self.pos = pos self.currentPowerups = {"powerup1": None, "powerup2": None, "powerup3": None} self.setupVehicle(bulletWorld) COUNT = 1 def processInput(self, inputState, dt): # print self.chassisNP.getPos() # print self.chassisNP.getH() """Use controls to update the player's car""" # For keyboard throttle and brake are either 0 or 1 if inputState.isSet('forward') and self.vehicle.getCurrentSpeedKmHour() <= self.max_speed: self.vehicleControlState["throttle"] = 1.0 else: self.vehicleControlState["throttle"] = 0.0 velocity = self.chassisNode.getLinearVelocity() speed = math.sqrt(sum(v ** 2 for v in velocity)) # Update braking and reversing if inputState.isSet('brake'): if speed < 0.5 or self.vehicleControlState["reverse"]: # If we're stopped, then start reversing # Also keep reversing if we already were self.vehicleControlState["reverse"] = True self.vehicleControlState["throttle"] = 1.0 self.vehicleControlState["brake"] = 0.0 else: self.vehicleControlState["reverse"] = False self.vehicleControlState["brake"] = 1.0 else: self.vehicleControlState["reverse"] = False self.vehicleControlState["brake"] = 0.0 # steering is normalised from -1 to 1, corresponding # to the steering lock right and left steering = self.vehicleControlState["steering"] if inputState.isSet('left'): steering += dt * self.steeringRate steering = min(steering, 1.0) elif inputState.isSet('right'): steering -= dt * self.steeringRate steering = max(steering, -1.0) else: # gradually re-center the steering if steering > 0.0: steering -= dt * self.centreingRate if steering < 0.0: steering = 0.0 elif steering < 0.0: steering += dt * self.centreingRate if steering > 0.0: steering = 0.0 self.vehicleControlState["steering"] = steering # """Updates acceleration, braking and steering # These are all passed in through a controlState dictionary # """ # Update acceleration and braking wheelForce = self.vehicleControlState["throttle"] * self.specs["maxWheelForce"] self.reversing = self.vehicleControlState["reverse"] if self.reversing: # Make reversing a bit slower than moving forward wheelForce *= -0.5 brakeForce = self.vehicleControlState["brake"] * self.specs["brakeForce"] #slows down vehicle when no key pressed if self.vehicleControlState["throttle"] != 1.0 and self.vehicleControlState["reverse"] != 1.0 and self.vehicleControlState["brake"] != 1.0: brakeForce = self.friction * self.specs["brakeForce"] # Update steering # Steering control state is from -1 to 1 steering = self.vehicleControlState["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); def setupVehicle(self, bulletWorld): # Chassis shape = BulletBoxShape(Vec3(1, 2.2, 0.5)) ts = TransformState.makePos(Point3(0, 0, .7)) self.chassisNode = BulletRigidBodyNode('Vehicle') self.chassisNP = render.attachNewNode(self.chassisNode) self.chassisNP.node().addShape(shape, ts) self.chassisNP.node().notifyCollisions(True) self.chassisNP.setPosHpr(self.pos[0], self.pos[1], self.pos[2], self.pos[3], self.pos[4], self.pos[5]) # self.chassisNP.setPos(-5.34744, 114.773, 6) # self.chassisNP.setPos(49.2167, 64.7968, 10) self.chassisNP.node().setMass(800.0) self.chassisNP.node().setDeactivationEnabled(False) bulletWorld.attachRigidBody(self.chassisNP.node()) # np.node().setCcdSweptSphereRadius(1.0) # np.node().setCcdMotionThreshold(1e-7) # Vehicle self.vehicle = BulletVehicle(bulletWorld, self.chassisNP.node()) self.vehicle.setCoordinateSystem(ZUp) bulletWorld.attachVehicle(self.vehicle) self.carNP = loader.loadModel('models/batmobile-chassis.egg') # self.yugoNP.setScale(.7) self.carNP.reparentTo(self.chassisNP) # Right front wheel np = loader.loadModel('models/batmobile-wheel-right.egg') np.reparentTo(render) self.addWheel(Point3(1, 1.1, .7), True, np) # Left front wheel np = loader.loadModel('models/batmobile-wheel-left.egg') np.reparentTo(render) self.addWheel(Point3(-1, 1.1, .7), True, np) # Right rear wheel np = loader.loadModel('models/batmobile-wheel-right.egg') np.reparentTo(render) self.addWheel(Point3(1, -2, .7), False, np) # Left rear wheel np = loader.loadModel('models/batmobile-wheel-left.egg') np.reparentTo(render) self.addWheel(Point3(-1, -2, .7), False, np) def addWheel(self, pos, front, np): wheel = self.vehicle.createWheel() wheel.setNode(np.node()) wheel.setChassisConnectionPointCs(pos) wheel.setFrontWheel(front) wheel.setWheelDirectionCs(Vec3(0, 0, -1)) wheel.setWheelAxleCs(Vec3(1, 0, 0)) wheel.setWheelRadius(0.33) wheel.setMaxSuspensionTravelCm(40.0) wheel.setSuspensionStiffness(40.0) wheel.setWheelsDampingRelaxation(2.3) wheel.setWheelsDampingCompression(4.4) wheel.setFrictionSlip(100.0); wheel.setRollInfluence(0.1) def reset(self): self.chassisNP.setP(0) self.chassisNP.setR(0) def pickedPowerup(self, powerup): if not powerup.pickable: powerup.useAbility() else: if self.currentPowerups["powerup1"] is None: self.currentPowerups["powerup1"] = powerup elif self.currentPowerups["powerup2"] is None: self.currentPowerups["powerup2"] = powerup elif self.currentPowerups["powerup3"] is None: self.currentPowerups["powerup3"] = powerup def canPickUpPowerup(self): return (self.currentPowerups["powerup1"] is None or self.currentPowerups["powerup2"] is None or self.currentPowerups["powerup3"] is None) def usePowerup(self, powerupIndex): # Move usePowerupN to this function if powerupIndex == 0 and self.currentPowerups["powerup1"] is not None: self.currentPowerups["powerup1"].useAbility() self.currentPowerups["powerup1"] = None elif powerupIndex == 1 and self.currentPowerups["powerup2"] is not None: self.currentPowerups["powerup2"].useAbility() self.currentPowerups["powerup2"] = None elif powerupIndex == 2 and self.currentPowerups["powerup3"] is not None: self.currentPowerups["powerup3"].useAbility() self.currentPowerups["powerup3"] = None
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