コード例 #1
0
ファイル: main.py プロジェクト: Cg-boy/open-driving-3d
class Drive(ShowBase):
	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")
コード例 #2
0
ファイル: Tank.py プロジェクト: ursulawolz/tanCS
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)
コード例 #3
0
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
コード例 #4
0
ファイル: phys.py プロジェクト: PlumpMath/yyagl
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)
コード例 #5
0
class CarEnv(DirectObject):
    def __init__(self, params={}):
        self._params = params
        if 'random_seed' in self._params:
            np.random.seed(self._params['random_seed'])
        self._use_vel = self._params.get('use_vel', True)
        self._run_as_task = self._params.get('run_as_task', False)
        self._do_back_up = self._params.get('do_back_up', False)
        self._use_depth = self._params.get('use_depth', False)
        self._use_back_cam = self._params.get('use_back_cam', False)

        self._collision_reward_only = self._params.get('collision_reward_only',
                                                       False)
        self._collision_reward = self._params.get('collision_reward', -10.0)
        self._obs_shape = self._params.get('obs_shape', (64, 36))
        self._steer_limits = params.get('steer_limits', (-30., 30.))
        self._speed_limits = params.get('speed_limits', (-4.0, 4.0))
        self._motor_limits = params.get('motor_limits', (-0.5, 0.5))
        self._fixed_speed = (self._speed_limits[0] == self._speed_limits[1]
                             and self._use_vel)
        if not self._params.get('visualize', False):
            loadPrcFileData('', 'window-type offscreen')

        # Defines base, render, loader

        try:
            ShowBase()
        except:
            pass

        base.setBackgroundColor(0.0, 0.0, 0.0, 1)

        # World
        self._worldNP = render.attachNewNode('World')
        self._world = BulletWorld()
        self._world.setGravity(Vec3(0, 0, -9.81))
        self._dt = params.get('dt', 0.25)
        self._step = 0.05

        # Vehicle
        shape = BulletBoxShape(Vec3(0.6, 1.0, 0.25))
        ts = TransformState.makePos(Point3(0., 0., 0.25))
        self._vehicle_node = BulletRigidBodyNode('Vehicle')
        self._vehicle_node.addShape(shape, ts)
        self._mass = self._params.get('mass', 10.)
        self._vehicle_node.setMass(self._mass)
        self._vehicle_node.setDeactivationEnabled(False)
        self._vehicle_node.setCcdSweptSphereRadius(1.0)
        self._vehicle_node.setCcdMotionThreshold(1e-7)
        self._vehicle_pointer = self._worldNP.attachNewNode(self._vehicle_node)

        self._world.attachRigidBody(self._vehicle_node)

        self._vehicle = BulletVehicle(self._world, self._vehicle_node)
        self._vehicle.setCoordinateSystem(ZUp)
        self._world.attachVehicle(self._vehicle)
        self._addWheel(Point3(0.3, 0.5, 0.07), True, 0.07)
        self._addWheel(Point3(-0.3, 0.5, 0.07), True, 0.07)
        self._addWheel(Point3(0.3, -0.5, 0.07), False, 0.07)
        self._addWheel(Point3(-0.3, -0.5, 0.07), False, 0.07)

        # Camera
        size = self._params.get('size', [160, 90])
        hfov = self._params.get('hfov', 120)
        near_far = self._params.get('near_far', [0.1, 100.])
        self._camera_sensor = Panda3dCameraSensor(base,
                                                  color=not self._use_depth,
                                                  depth=self._use_depth,
                                                  size=size,
                                                  hfov=hfov,
                                                  near_far=near_far,
                                                  title='front cam')
        self._camera_node = self._camera_sensor.cam
        self._camera_node.setPos(0.0, 0.5, 0.375)
        self._camera_node.lookAt(0.0, 6.0, 0.0)
        self._camera_node.reparentTo(self._vehicle_pointer)

        if self._use_back_cam:
            self._back_camera_sensor = Panda3dCameraSensor(
                base,
                color=not self._use_depth,
                depth=self._use_depth,
                size=size,
                hfov=hfov,
                near_far=near_far,
                title='back cam')

            self._back_camera_node = self._back_camera_sensor.cam
            self._back_camera_node.setPos(0.0, -0.5, 0.375)
            self._back_camera_node.lookAt(0.0, -6.0, 0.0)
            self._back_camera_node.reparentTo(self._vehicle_pointer)

        # Car Simulator
        self._des_vel = None
        self._setup()

        # Input
        self.accept('escape', self._doExit)
        self.accept('r', self.reset)
        self.accept('f1', self._toggleWireframe)
        self.accept('f2', self._toggleTexture)
        self.accept('f3', self._view_image)
        self.accept('f5', self._doScreenshot)
        self.accept('q', self._forward_0)
        self.accept('w', self._forward_1)
        self.accept('e', self._forward_2)
        self.accept('a', self._left)
        self.accept('s', self._stop)
        self.accept('x', self._backward)
        self.accept('d', self._right)
        self.accept('m', self._mark)

        self._steering = 0.0  # degree
        self._engineForce = 0.0
        self._brakeForce = 0.0
        self._env_time_step = 0
        self._p = self._params.get('p', 1.25)
        self._d = self._params.get('d', 0.0)
        self._last_err = 0.0
        self._curr_time = 0.0
        self._accelClamp = self._params.get('accelClamp', 0.5)
        self._engineClamp = self._accelClamp * self._mass
        self._collision = False

        self._setup_spec()

        self.spec = EnvSpec(observation_im_space=self.observation_im_space,
                            action_space=self.action_space,
                            action_selection_space=self.action_selection_space,
                            observation_vec_spec=self.observation_vec_spec,
                            action_spec=self.action_spec,
                            action_selection_spec=self.action_selection_spec,
                            goal_spec=self.goal_spec)

        if self._run_as_task:
            self._mark_d = 0.0
            taskMgr.add(self._update_task, 'updateWorld')
            base.run()

    def _setup_spec(self):
        self.action_spec = OrderedDict()
        self.action_selection_spec = OrderedDict()
        self.observation_vec_spec = OrderedDict()
        self.goal_spec = OrderedDict()

        self.action_spec['steer'] = Box(low=-45., high=45.)
        self.action_selection_spec['steer'] = Box(low=self._steer_limits[0],
                                                  high=self._steer_limits[1])

        if self._use_vel:
            self.action_spec['speed'] = Box(low=-4., high=4.)
            self.action_space = Box(low=np.array([
                self.action_spec['steer'].low[0],
                self.action_spec['speed'].low[0]
            ]),
                                    high=np.array([
                                        self.action_spec['steer'].high[0],
                                        self.action_spec['speed'].high[0]
                                    ]))

            self.action_selection_spec['speed'] = Box(
                low=self._speed_limits[0], high=self._speed_limits[1])
            self.action_selection_space = Box(
                low=np.array([
                    self.action_selection_spec['steer'].low[0],
                    self.action_selection_spec['speed'].low[0]
                ]),
                high=np.array([
                    self.action_selection_spec['steer'].high[0],
                    self.action_selection_spec['speed'].high[0]
                ]))
        else:
            self.action_spec['motor'] = Box(low=-self._accelClamp,
                                            high=self._accelClamp)
            self.action_space = Box(low=np.array([
                self.action_spec['steer'].low[0],
                self.action_spec['motor'].low[0]
            ]),
                                    high=np.array([
                                        self.action_spec['steer'].high[0],
                                        self.action_spec['motor'].high[0]
                                    ]))

            self.action_selection_spec['motor'] = Box(
                low=self._motor_limits[0], high=self._motor_limits[1])
            self.action_selection_space = Box(
                low=np.array([
                    self.action_selection_spec['steer'].low[0],
                    self.action_selection_spec['motor'].low[0]
                ]),
                high=np.array([
                    self.action_selection_spec['steer'].high[0],
                    self.action_selection_spec['motor'].high[0]
                ]))

        assert (np.logical_and(
            self.action_selection_space.low >= self.action_space.low - 1e-4,
            self.action_selection_space.high <=
            self.action_space.high + 1e-4).all())

        self.observation_im_space = Box(low=0,
                                        high=255,
                                        shape=tuple(
                                            self._get_observation()[0].shape))
        self.observation_vec_spec['coll'] = Discrete(1)
        self.observation_vec_spec['heading'] = Box(low=0, high=2 * 3.14)
        self.observation_vec_spec['speed'] = Box(low=-4.0, high=4.0)

    @property
    def _base_dir(self):
        return os.path.join(os.path.dirname(os.path.abspath(__file__)),
                            'models')

    @property
    def horizon(self):
        return np.inf

    @property
    def max_reward(self):
        return np.inf

    # _____HANDLER_____

    def _doExit(self):
        sys.exit(1)

    def _toggleWireframe(self):
        base.toggleWireframe()

    def _toggleTexture(self):
        base.toggleTexture()

    def _doScreenshot(self):
        base.screenshot('Bullet')

    def _forward_0(self):
        self._des_vel = 1
        self._brakeForce = 0.0

    def _forward_1(self):
        self._des_vel = 2
        self._brakeForce = 0.0

    def _forward_2(self):
        self._des_vel = 4
        self._brakeForce = 0.0

    def _stop(self):
        self._des_vel = 0.0
        self._brakeForce = 0.0

    def _backward(self):
        self._des_vel = -4
        self._brakeForce = 0.0

    def _right(self):
        self._steering = np.min([np.max([-30, self._steering - 5]), 0.0])

    def _left(self):
        self._steering = np.max([np.min([30, self._steering + 5]), 0.0])

    def _view_image(self):
        from matplotlib import pyplot as plt
        image = self._camera_sensor.observe()[0]
        if self._use_depth:
            plt.imshow(image[:, :, 0], cmap='gray')
        else:

            def rgb2gray(rgb):
                return np.dot(rgb[..., :3], [0.299, 0.587, 0.114])

            image = rgb2gray(image)
            im = cv2.resize(image, (64, 36), interpolation=cv2.INTER_AREA
                            )  # TODO how does this deal with aspect ratio
            plt.imshow(im.astype(np.uint8), cmap='Greys_r')
        plt.show()

    def _mark(self):
        self._mark_d = 0.0

    # Setup

    def _setup(self):
        self._setup_map()
        self._place_vehicle()
        self._setup_light()
        self._setup_restart_pos()

    def _setup_map(self):
        if hasattr(self, '_model_path'):
            # Collidable objects
            self._setup_collision_object(self._model_path)
        else:
            ground = self._worldNP.attachNewNode(BulletRigidBodyNode('Ground'))
            shape = BulletPlaneShape(Vec3(0, 0, 1), 0)
            ground.node().addShape(shape)
            ground.setCollideMask(BitMask32.allOn())
            self._world.attachRigidBody(ground.node())

    def _setup_collision_object(self,
                                path,
                                pos=(0.0, 0.0, 0.0),
                                hpr=(0.0, 0.0, 0.0),
                                scale=1):
        visNP = loader.loadModel(path)
        visNP.clearModelNodes()
        visNP.reparentTo(render)
        visNP.setPos(pos[0], pos[1], pos[2])
        visNP.setHpr(hpr[0], hpr[1], hpr[2])
        visNP.set_scale(scale, scale, scale)
        bodyNPs = BulletHelper.fromCollisionSolids(visNP, True)
        for bodyNP in bodyNPs:
            bodyNP.reparentTo(render)
            bodyNP.setPos(pos[0], pos[1], pos[2])
            bodyNP.setHpr(hpr[0], hpr[1], hpr[2])
            bodyNP.set_scale(scale, scale, scale)
            if isinstance(bodyNP.node(), BulletRigidBodyNode):
                bodyNP.node().setMass(0.0)
                bodyNP.node().setKinematic(True)
                bodyNP.setCollideMask(BitMask32.allOn())
                self._world.attachRigidBody(bodyNP.node())
            else:
                print("Issue")

    def _setup_restart_pos(self):
        self._restart_index = 0
        self._restart_pos = self._default_restart_pos()

    def _next_restart_pos_hpr(self):
        num = len(self._restart_pos)
        if num == 0:
            return None, None
        else:
            pos_hpr = self._restart_pos[self._restart_index]
            self._restart_index = (self._restart_index + 1) % num
            return pos_hpr[:3], pos_hpr[3:]

    def _setup_light(self):
        #        alight = AmbientLight('ambientLight')
        #        alight.setColor(Vec4(0.5, 0.5, 0.5, 1))
        #        alightNP = render.attachNewNode(alight)
        #        render.clearLight()
        #        render.setLight(alightNP)
        pass

    # Vehicle
    def _default_pos(self):
        return (0.0, 0.0, 0.3)

    def _default_hpr(self):
        return (0.0, 0.0, 0.0)

    def _default_restart_pos(self):
        return [self._default_pos() + self._default_hpr()]

    def _get_speed(self):
        vel = self._vehicle.getCurrentSpeedKmHour() / 3.6
        return vel

    def _get_heading(self):
        h = np.array(self._vehicle_pointer.getHpr())[0]
        ori = h * (pi / 180.)
        while (ori > 2 * pi):
            ori -= 2 * pi
        while (ori < 0):
            ori += 2 * pi
        return ori

    def _update(self, dt=1.0, coll_check=True):
        self._vehicle.setSteeringValue(self._steering, 0)
        self._vehicle.setSteeringValue(self._steering, 1)
        self._vehicle.setBrake(self._brakeForce, 0)
        self._vehicle.setBrake(self._brakeForce, 1)
        self._vehicle.setBrake(self._brakeForce, 2)
        self._vehicle.setBrake(self._brakeForce, 3)
        if dt >= self._step:
            # TODO maybe change number of timesteps
            #            for i in range(int(dt/self._step)):
            if self._des_vel is not None:
                vel = self._get_speed()
                err = self._des_vel - vel
                d_err = (err - self._last_err) / self._step
                self._last_err = err
                self._engineForce = np.clip(self._p * err + self._d * d_err,
                                            -self._accelClamp,
                                            self._accelClamp) * self._mass
            self._vehicle.applyEngineForce(self._engineForce, 0)
            self._vehicle.applyEngineForce(self._engineForce, 1)
            self._vehicle.applyEngineForce(self._engineForce, 2)
            self._vehicle.applyEngineForce(self._engineForce, 3)
            for _ in range(int(dt / self._step)):
                self._world.doPhysics(self._step, 1, self._step)
            self._collision = self._is_contact()
        elif self._run_as_task:
            self._curr_time += dt
            if self._curr_time > 0.05:
                if self._des_vel is not None:
                    vel = self._get_speed()
                    self._mark_d += vel * self._curr_time
                    print(vel, self._mark_d, self._is_contact())
                    err = self._des_vel - vel
                    d_err = (err - self._last_err) / 0.05
                    self._last_err = err
                    self._engineForce = np.clip(
                        self._p * err + self._d * d_err, -self._accelClamp,
                        self._accelClamp) * self._mass
                self._curr_time = 0.0
                self._vehicle.applyEngineForce(self._engineForce, 0)
                self._vehicle.applyEngineForce(self._engineForce, 1)
                self._vehicle.applyEngineForce(self._engineForce, 2)
                self._vehicle.applyEngineForce(self._engineForce, 3)
            self._world.doPhysics(dt, 1, dt)
            self._collision = self._is_contact()
        else:
            raise ValueError(
                "dt {0} s is too small for velocity control".format(dt))

    def _stop_car(self):
        self._steering = 0.0
        self._engineForce = 0.0
        self._vehicle.setSteeringValue(0.0, 0)
        self._vehicle.setSteeringValue(0.0, 1)
        self._vehicle.applyEngineForce(0.0, 0)
        self._vehicle.applyEngineForce(0.0, 1)
        self._vehicle.applyEngineForce(0.0, 2)
        self._vehicle.applyEngineForce(0.0, 3)

        if self._des_vel is not None:
            self._des_vel = 0

        self._vehicle_node.setLinearVelocity(Vec3(0.0, 0.0, 0.0))
        self._vehicle_node.setAngularVelocity(Vec3(0.0, 0.0, 0.0))
        for i in range(self._vehicle.getNumWheels()):
            wheel = self._vehicle.getWheel(i)
            wheel.setRotation(0.0)
        self._vehicle_node.clearForces()

    def _place_vehicle(self, pos=None, hpr=None):
        if pos is None:
            pos = self._default_pos()
        if hpr is None:
            hpr = self._default_hpr()
        self._vehicle_pointer.setPos(pos[0], pos[1], pos[2])
        self._vehicle_pointer.setHpr(hpr[0], hpr[1], hpr[2])
        self._stop_car()

    def _addWheel(self, pos, front, radius=0.25):
        wheel = self._vehicle.createWheel()
        wheel.setChassisConnectionPointCs(pos)
        wheel.setFrontWheel(front)
        wheel.setWheelDirectionCs(Vec3(0, 0, -1))
        wheel.setWheelAxleCs(Vec3(1, 0, 0))
        wheel.setWheelRadius(radius)
        wheel.setMaxSuspensionTravelCm(40.0)
        wheel.setSuspensionStiffness(40.0)
        wheel.setWheelsDampingRelaxation(2.3)
        wheel.setWheelsDampingCompression(4.4)
        wheel.setFrictionSlip(1e2)
        wheel.setRollInfluence(0.1)

    # Task

    def _update_task(self, task):
        dt = globalClock.getDt()
        self._update(dt=dt)
        self._get_observation()
        return task.cont

    # Helper functions

    def _get_observation(self):
        self._obs = self._camera_sensor.observe()
        observation = []
        observation.append(self.process(self._obs[0], self._obs_shape))
        if self._use_back_cam:
            self._back_obs = self._back_camera_sensor.observe()
            observation.append(self.process(self._back_obs[0],
                                            self._obs_shape))
        observation_im = np.concatenate(observation, axis=2)
        coll = self._collision
        heading = self._get_heading()
        speed = self._get_speed()
        observation_vec = np.array([coll, heading, speed])
        return observation_im, observation_vec

    def _get_goal(self):
        return np.array([])

    def process(self, image, obs_shape):
        if self._use_depth:
            im = np.reshape(image, (image.shape[0], image.shape[1]))
            if im.shape != obs_shape:
                im = cv2.resize(im, obs_shape, interpolation=cv2.INTER_AREA)
            return im.astype(np.uint8)
        else:
            image = np.dot(image[..., :3], [0.299, 0.587, 0.114])
            im = cv2.resize(image, obs_shape, interpolation=cv2.INTER_AREA
                            )  #TODO how does this deal with aspect ratio
            # TODO might not be necessary
            im = np.expand_dims(im, 2)
            return im.astype(np.uint8)

    def _get_reward(self):
        if self._collision_reward_only:
            if self._collision:
                reward = self._collision_reward
            else:
                reward = 0.0
        else:
            if self._collision:
                reward = self._collision_reward
            else:
                reward = self._get_speed()
        assert (reward <= self.max_reward)
        return reward

    def _get_done(self):
        return self._collision

    def _get_info(self):
        info = {}
        info['pos'] = np.array(self._vehicle_pointer.getPos())
        info['hpr'] = np.array(self._vehicle_pointer.getHpr())
        info['vel'] = self._get_speed()
        info['coll'] = self._collision
        info['env_time_step'] = self._env_time_step
        return info

    def _back_up(self):
        assert (self._use_vel)
        #        logger.debug('Backing up!')
        self._params['back_up'] = self._params.get('back_up', {})
        back_up_vel = self._params['back_up'].get('vel', -1.0)
        self._des_vel = back_up_vel
        back_up_steer = self._params['back_up'].get('steer', (-5.0, 5.0))
        self._steering = np.random.uniform(*back_up_steer)
        self._brakeForce = 0.
        duration = self._params['back_up'].get('duration', 3.0)
        self._update(dt=duration)
        self._des_vel = 0.
        self._steering = 0.
        self._update(dt=duration)
        self._brakeForce = 0.

    def _is_contact(self):
        result = self._world.contactTest(self._vehicle_node)
        return result.getNumContacts() > 0

    # Environment functions

    def reset(self, pos=None, hpr=None, hard_reset=False):
        if self._do_back_up and not hard_reset and \
                pos is None and hpr is None:
            if self._collision:
                self._back_up()
        else:
            if hard_reset:
                logger.debug('Hard resetting!')
            if pos is None and hpr is None:
                pos, hpr = self._next_restart_pos_hpr()
            self._place_vehicle(pos=pos, hpr=hpr)
        self._collision = False
        self._env_time_step = 0
        return self._get_observation(), self._get_goal()

    def step(self, action):
        self._steering = action[0]
        if action[1] == 0.0:
            self._brakeForce = 1000.
        else:
            self._brakeForce = 0.
        if self._use_vel:
            # Convert from m/s to km/h
            self._des_vel = action[1]
        else:
            self._engineForce = self._mass * action[1]

        self._update(dt=self._dt)
        observation = self._get_observation()
        goal = self._get_goal()
        reward = self._get_reward()
        done = self._get_done()
        info = self._get_info()
        self._env_time_step += 1
        return observation, goal, reward, done, info
コード例 #6
0
ファイル: game.py プロジェクト: Red-Gravel/Red-Gravel
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)
コード例 #7
0
ファイル: car.py プロジェクト: TheEnbyperor/racing_sim
class Car(DirectObject):
    def __init__(self, base, world, track):
        super().__init__()

        self.world = world
        self.track = track

        self.steering = 0
        self.accelerator = 0
        self.brake = 0

        self.gear_ratios = [-4, 0, 3.9, 2.9, 2.3, 1.87, 1.68, 1.54, 1.46]
        self.gear = 1
        self.differential_ratio = 4.5
        self.transmission_efficiency = 0.95
        self.wheel_radius = 0.4
        self.drag_coefficient = 0.48
        self.engine_torque_curve = [
            (0, 466),
            (564, 469),
            (1612, 469),
            (2822, 518),
            (3870, 517),
            (4516, 597),
            (5000, 613),
            (5564, 600),
            (6048, 655),
            (6693, 681),
            (7177, 716),
            (7822, 696),
            (8306, 696),
            (11048, 569),
            (13951, 391),
            (15000, 339),
            (15483, 301),
            (16612, 247),
            (17177, 65),
            (18306, 55)
        ]
        self.fw_wingspan = 0.64
        self.fw_cord = 1.01
        self.fw_clift = 0.7
        self.rw_wingspan = 0.64
        self.rw_cord = 0.51
        self.rw_clift = 0.2

        self.car_node = None
        self.car = None

        dial_scale = 0.2

        speed_dial = OnscreenImage(image='tex/speed360.rgb', pos=(-dial_scale, 0, dial_scale),
                                   scale=(dial_scale, 1, dial_scale),
                                   parent=base.a2dBottomCenter)
        speed_dial.setTransparency(TransparencyAttrib.MAlpha)
        self.speed_dial = OnscreenImage(image='tex/dial.rgb', parent=speed_dial)

        rpm_dial = OnscreenImage(image='tex/rpm20000.rgb', pos=(dial_scale, 0, dial_scale),
                                 scale=(dial_scale, 1, dial_scale),
                                 parent=base.a2dBottomCenter)
        rpm_dial.setTransparency(TransparencyAttrib.MAlpha)
        self.rpm_dial = OnscreenImage(image='tex/dial.rgb', parent=rpm_dial)
        self.gear_text = OnscreenText(text='N', pos=(-0.02, -0.67), scale=0.4, parent=rpm_dial, fg=(255, 0, 0, 1))

        self.reset()
        taskMgr.add(self.update, 'update')

    def make_wheel(self, pos, front, np):
        wheel = self.car.createWheel(0.02)

        wheel.setNode(np.node())
        wheel.setChassisConnectionPointCs(pos)
        wheel.setFrontWheel(front)
        wheel.setWheelDirectionCs(Vec3(0, 0, -1))
        wheel.setWheelAxleCs(Vec3(1, 0, 0))
        wheel.setWheelRadius(self.wheel_radius)
        wheel.setSuspensionStiffness(200)
        wheel.setWheelsDampingRelaxation(23)
        wheel.setWheelsDampingCompression(44)
        wheel.setRollInfluence(0.1)
        wheel.setMaxSuspensionTravelCm(10)
        wheel.setFrictionSlip(1.2)
        # wheel.setMaxSuspensionForce(1000)

    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 get_engine_torque(self, rpm):
        min_rpm = min(self.engine_torque_curve, key=lambda p: p[0])
        max_rpm = max(self.engine_torque_curve, key=lambda p: p[0])

        if rpm <= min_rpm[0]:
            return min_rpm[1]
        elif rpm >= max_rpm[0]:
            return 0

        less_rpm = filter(lambda p: p[0] <= rpm, self.engine_torque_curve)
        more_rpm = filter(lambda p: p[0] >= rpm, self.engine_torque_curve)
        max_less_rpm = max(less_rpm, key=lambda p: p[0])
        min_more_rpm = min(more_rpm, key=lambda p: p[0])

        rpm_diff = min_more_rpm[0] - max_less_rpm[0]
        torque_diff = min_more_rpm[1] - max_less_rpm[1]
        slope = torque_diff / rpm_diff
        diff = rpm - max_less_rpm[0]

        return max_less_rpm[1] + (slope * diff)

    @property
    def pos(self):
        return self.car_node.getPos()

    @property
    def forward_vector(self):
        return self.car.getForwardVector()

    def update(self, task):
        car_pos = self.pos
        car_vec = self.forward_vector
        track_bounds = self.track.tight_bounds

        if car_pos.x < track_bounds[0].x or \
                car_pos.x > track_bounds[1].x or \
                car_pos.y < track_bounds[0].y or \
                car_pos.y > track_bounds[1].y or \
                car_pos.z < track_bounds[0].z:
            self.reset()
            return task.cont

        car_speed = self.car.getCurrentSpeedKmHour()
        car_speed_ms = car_speed * 1000 / 3600
        car_speed_abs = abs(car_speed)

        self.car_node.node().clearForces()

        self.apply_wing_force(car_speed_ms)
        self.apply_drag_force(car_speed_ms, car_vec)
        rpm = self.apply_engine_force(car_speed_ms)
        self.apply_brake_force()
        self.apply_steering_moment(car_speed_abs)
        self.update_dials(rpm, car_speed_abs)

        return task.cont

    def apply_wing_force(self, car_speed_ms):
        fw_downforce = 0.5 * self.fw_cord * self.fw_wingspan * self.fw_clift * 1.29 * car_speed_ms ** 2
        rw_downforce = 0.5 * self.rw_cord * self.rw_wingspan * self.rw_clift * 1.29 * car_speed_ms ** 2

        self.car_node.node().applyForce(Vec3(0, 0, -fw_downforce), Point3(-0.61, 2.32, 0.23))
        self.car_node.node().applyForce(Vec3(0, 0, -fw_downforce), Point3(0.61, 2.32, 0.23))
        self.car_node.node().applyForce(Vec3(0, 0, -rw_downforce), Point3(-0.61, -2.47, 1.2))
        self.car_node.node().applyForce(Vec3(0, 0, -rw_downforce), Point3(0.61, -2.47, 1.2))

    def apply_drag_force(self, car_speed_ms, car_vec):
        drag_coefficient = 0.5 * self.drag_coefficient * 1.29 * 5
        drag_force = drag_coefficient * car_speed_ms ** 2
        rr_force = drag_coefficient * 30 * car_speed_ms
        if car_speed_ms < 0:
            drag_force = -drag_force

        self.car_node.node().applyCentralForce(car_vec * -drag_force)
        self.car_node.node().applyCentralForce(car_vec * -rr_force)

    def apply_engine_force(self, car_speed_ms):
        angular_velocity = car_speed_ms / self.wheel_radius
        rpm = angular_velocity * self.gear_ratios[self.gear] * self.differential_ratio * 60 / (2 * math.pi)
        rpm = max(0, rpm)
        engine_torque = self.get_engine_torque(rpm)

        max_engine_force = engine_torque * self.gear_ratios[self.gear] * self.differential_ratio * \
                           self.transmission_efficiency / self.wheel_radius
        engine_force = max_engine_force * (self.accelerator / 128)

        self.car.applyEngineForce(engine_force, 2)
        self.car.applyEngineForce(engine_force, 3)
        return rpm

    def apply_brake_force(self):
        brake_adj = 5
        self.car.setBrake(self.brake * brake_adj, 0)
        self.car.setBrake(self.brake * brake_adj, 1)
        self.car.setBrake(self.brake * brake_adj, 2)
        self.car.setBrake(self.brake * brake_adj, 3)

    def apply_steering_moment(self, car_speed_abs):
        steering_ratio = max((car_speed_abs / 3), 30)
        self.car.setSteeringValue(self.steering / steering_ratio, 0)
        self.car.setSteeringValue(self.steering / steering_ratio, 1)

    def update_dials(self, rpm, car_speed_abs):
        min_rpm = min(self.engine_torque_curve, key=lambda p: p[0])[0]
        if self.gear == 1:
            min_rpm = max(self.engine_torque_curve, key=lambda p: p[0])[0]

        self.speed_dial.setHpr(0, 0, car_speed_abs * (270 / 360))
        self.rpm_dial.setHpr(0, 0, max(rpm, min_rpm) * (270 / 20000) * (self.accelerator / 128))

    def update_gear_text(self):
        if self.gear >= 2:
            text = str(self.gear - 1)
        elif self.gear == 0:
            text = "R"
        elif self.gear == 1:
            text = "N"
        else:
            text = ""
        self.gear_text.setText(text)

    def down_gear(self):
        self.gear -= 1
        if self.gear < 0:
            self.gear = 0
        self.update_gear_text()

    def up_gear(self):
        self.gear += 1
        if self.gear >= len(self.gear_ratios):
            self.gear = len(self.gear_ratios) - 1
        self.update_gear_text()