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")
class CarEnv(DirectObject):
def __init__(self, params={}):
self._params = params
if 'random_seed' in self._params:
np.random.seed(self._params['random_seed'])
self._use_vel = self._params.get('use_vel', True)
self._run_as_task = self._params.get('run_as_task', False)
self._do_back_up = self._params.get('do_back_up', False)
self._use_depth = self._params.get('use_depth', False)
self._use_back_cam = self._params.get('use_back_cam', False)
self._collision_reward = self._params.get('collision_reward', 0.)
if not self._params.get('visualize', False):
loadPrcFileData('', 'window-type offscreen')
# Defines base, render, loader
try:
ShowBase()
except:
pass
base.setBackgroundColor(0.0, 0.0, 0.0, 1)
# World
self._worldNP = render.attachNewNode('World')
self._world = BulletWorld()
self._world.setGravity(Vec3(0, 0, -9.81))
self._dt = params.get('dt', 0.25)
self._step = 0.05
# Vehicle
shape = BulletBoxShape(Vec3(0.6, 1.0, 0.25))
ts = TransformState.makePos(Point3(0., 0., 0.25))
self._vehicle_node = BulletRigidBodyNode('Vehicle')
self._vehicle_node.addShape(shape, ts)
self._mass = self._params.get('mass', 10.)
self._vehicle_node.setMass(self._mass)
self._vehicle_node.setDeactivationEnabled(False)
self._vehicle_node.setCcdSweptSphereRadius(1.0)
self._vehicle_node.setCcdMotionThreshold(1e-7)
self._vehicle_pointer = self._worldNP.attachNewNode(self._vehicle_node)
self._world.attachRigidBody(self._vehicle_node)
self._vehicle = BulletVehicle(self._world, self._vehicle_node)
self._vehicle.setCoordinateSystem(ZUp)
self._world.attachVehicle(self._vehicle)
self._addWheel(Point3(0.3, 0.5, 0.07), True, 0.07)
self._addWheel(Point3(-0.3, 0.5, 0.07), True, 0.07)
self._addWheel(Point3(0.3, -0.5, 0.07), False, 0.07)
self._addWheel(Point3(-0.3, -0.5, 0.07), False, 0.07)
# Camera
size = self._params.get('size', [160, 90])
hfov = self._params.get('hfov', 60)
near_far = self._params.get('near_far', [0.1, 100.])
self._camera_sensor = Panda3dCameraSensor(base,
color=not self._use_depth,
depth=self._use_depth,
size=size,
hfov=hfov,
near_far=near_far,
title='front cam')
self._camera_node = self._camera_sensor.cam
self._camera_node.setPos(0.0, 0.5, 0.375)
self._camera_node.lookAt(0.0, 6.0, 0.0)
self._camera_node.reparentTo(self._vehicle_pointer)
if self._use_back_cam:
self._back_camera_sensor = Panda3dCameraSensor(
base,
color=not self._use_depth,
depth=self._use_depth,
size=size,
hfov=hfov,
near_far=near_far,
title='back cam')
self._back_camera_node = self._back_camera_sensor.cam
self._back_camera_node.setPos(0.0, -0.5, 0.375)
self._back_camera_node.lookAt(0.0, -6.0, 0.0)
self._back_camera_node.reparentTo(self._vehicle_pointer)
# Car Simulator
self._des_vel = None
self._setup()
# Input
self.accept('escape', self._doExit)
self.accept('r', self.reset)
self.accept('f1', self._toggleWireframe)
self.accept('f2', self._toggleTexture)
self.accept('f3', self._view_image)
self.accept('f5', self._doScreenshot)
self.accept('q', self._forward_0)
self.accept('w', self._forward_1)
self.accept('e', self._forward_2)
self.accept('a', self._left)
self.accept('s', self._stop)
self.accept('x', self._backward)
self.accept('d', self._right)
self.accept('m', self._mark)
self._steering = 0.0 # degree
self._engineForce = 0.0
self._brakeForce = 0.0
self._p = self._params.get('p', 1.25)
self._d = self._params.get('d', 0.0)
self._last_err = 0.0
self._curr_time = 0.0
self._accelClamp = self._params.get('accelClamp', 2.0)
self._engineClamp = self._accelClamp * self._mass
self._collision = False
if self._run_as_task:
self._mark_d = 0.0
taskMgr.add(self._update_task, 'updateWorld')
base.run()
# _____HANDLER_____
def _doExit(self):
sys.exit(1)
def _toggleWireframe(self):
base.toggleWireframe()
def _toggleTexture(self):
base.toggleTexture()
def _doScreenshot(self):
base.screenshot('Bullet')
def _forward_0(self):
self._des_vel = 1
self._brakeForce = 0.0
def _forward_1(self):
self._des_vel = 2
self._brakeForce = 0.0
def _forward_2(self):
self._des_vel = 4
self._brakeForce = 0.0
def _stop(self):
self._des_vel = 0.0
self._brakeForce = 0.0
def _backward(self):
self._des_vel = -4
self._brakeForce = 0.0
def _right(self):
self._steering = np.min([np.max([-30, self._steering - 5]), 0.0])
def _left(self):
self._steering = np.max([np.min([30, self._steering + 5]), 0.0])
def _view_image(self):
from matplotlib import pyplot as plt
image = self._camera_sensor.observe()[0]
if self._use_depth:
plt.imshow(image[:, :, 0], cmap='gray')
else:
import cv2
def rgb2gray(rgb):
return np.dot(rgb[..., :3], [0.299, 0.587, 0.114])
image = rgb2gray(image)
im = cv2.resize(image, (64, 36), interpolation=cv2.INTER_AREA
) # TODO how does this deal with aspect ratio
plt.imshow(im.astype(np.uint8), cmap='Greys_r')
plt.show()
def _mark(self):
self._mark_d = 0.0
# Setup
def _setup(self):
if hasattr(self, '_model_path'):
# Collidable objects
visNP = loader.loadModel(self._model_path)
visNP.clearModelNodes()
visNP.reparentTo(render)
pos = (0., 0., 0.)
visNP.setPos(pos[0], pos[1], pos[2])
bodyNPs = BulletHelper.fromCollisionSolids(visNP, True)
for bodyNP in bodyNPs:
bodyNP.reparentTo(render)
bodyNP.setPos(pos[0], pos[1], pos[2])
if isinstance(bodyNP.node(), BulletRigidBodyNode):
bodyNP.node().setMass(0.0)
bodyNP.node().setKinematic(True)
bodyNP.setCollideMask(BitMask32.allOn())
self._world.attachRigidBody(bodyNP.node())
else:
ground = self._worldNP.attachNewNode(BulletRigidBodyNode('Ground'))
shape = BulletPlaneShape(Vec3(0, 0, 1), 0)
ground.node().addShape(shape)
ground.setCollideMask(BitMask32.allOn())
self._world.attachRigidBody(ground.node())
self._place_vehicle()
self._setup_light()
self._setup_restart_pos()
def _setup_restart_pos(self):
self._restart_pos = []
self._restart_index = 0
if self._params.get('position_ranges', None) is not None:
ranges = self._params['position_ranges']
num_pos = self._params['num_pos']
if self._params.get('range_type', 'random') == 'random':
for _ in range(num_pos):
ran = ranges[np.random.randint(len(ranges))]
self._restart_pos.append(np.random.uniform(ran[0], ran[1]))
elif self._params['range_type'] == 'fix_spacing':
num_ran = len(ranges)
num_per_ran = num_pos // num_ran
for i in range(num_ran):
ran = ranges[i]
low = np.array(ran[0])
diff = np.array(ran[1]) - np.array(ran[0])
for j in range(num_per_ran):
val = diff * ((j + 0.0) / num_per_ran) + low
self._restart_pos.append(val)
elif self._params.get('positions', None) is not None:
self._restart_pos = self._params['positions']
else:
self._restart_pos = self._default_restart_pos()
def _next_restart_pos_hpr(self):
num = len(self._restart_pos)
if num == 0:
return None, None
else:
pos_hpr = self._restart_pos[self._restart_index]
self._restart_index = (self._restart_index + 1) % num
return pos_hpr[:3], pos_hpr[3:]
def _next_random_restart_pos_hpr(self):
num = len(self._restart_pos)
if num == 0:
return None, None
else:
index = np.random.randint(num)
pos_hpr = self._restart_pos[index]
self._restart_index = (self._restart_index + 1) % num
return pos_hpr[:3], pos_hpr[3:]
def _setup_light(self):
alight = AmbientLight('ambientLight')
alight.setColor(Vec4(0.5, 0.5, 0.5, 1))
alightNP = render.attachNewNode(alight)
render.clearLight()
render.setLight(alightNP)
# Vehicle
def _default_pos(self):
return (0.0, 0.0, 0.3)
def _default_hpr(self):
return (0.0, 0.0, 3.14)
def _default_restart_pos():
return [self._default_pos() + self._default_hpr()]
def _get_speed(self):
vel = self._vehicle.getCurrentSpeedKmHour() / 3.6
return vel
def _update(self, dt=1.0, coll_check=True):
self._vehicle.setSteeringValue(self._steering, 0)
self._vehicle.setSteeringValue(self._steering, 1)
self._vehicle.setBrake(self._brakeForce, 0)
self._vehicle.setBrake(self._brakeForce, 1)
self._vehicle.setBrake(self._brakeForce, 2)
self._vehicle.setBrake(self._brakeForce, 3)
if dt >= self._step:
# TODO maybe change number of timesteps
for i in range(int(dt / self._step)):
if self._des_vel is not None:
vel = self._get_speed()
err = self._des_vel - vel
d_err = (err - self._last_err) / self._step
self._last_err = err
self._engineForce = np.clip(
self._p * err + self._d * d_err, -self._accelClamp,
self._accelClamp) * self._mass
self._vehicle.applyEngineForce(self._engineForce, 0)
self._vehicle.applyEngineForce(self._engineForce, 1)
self._vehicle.applyEngineForce(self._engineForce, 2)
self._vehicle.applyEngineForce(self._engineForce, 3)
self._world.doPhysics(self._step, 1, self._step)
self._collision = self._is_contact()
elif self._run_as_task:
self._curr_time += dt
if self._curr_time > 0.05:
if self._des_vel is not None:
vel = self._get_speed()
self._mark_d += vel * self._curr_time
print(vel, self._mark_d, self._is_contact())
err = self._des_vel - vel
d_err = (err - self._last_err) / 0.05
self._last_err = err
self._engineForce = np.clip(
self._p * err + self._d * d_err, -self._accelClamp,
self._accelClamp) * self._mass
self._curr_time = 0.0
self._vehicle.applyEngineForce(self._engineForce, 0)
self._vehicle.applyEngineForce(self._engineForce, 1)
self._vehicle.applyEngineForce(self._engineForce, 2)
self._vehicle.applyEngineForce(self._engineForce, 3)
self._world.doPhysics(dt, 1, dt)
self._collision = self._is_contact()
else:
raise ValueError(
"dt {0} s is too small for velocity control".format(dt))
def _stop_car(self):
self._steering = 0.0
self._engineForce = 0.0
self._vehicle.setSteeringValue(0.0, 0)
self._vehicle.setSteeringValue(0.0, 1)
self._vehicle.applyEngineForce(0.0, 0)
self._vehicle.applyEngineForce(0.0, 1)
self._vehicle.applyEngineForce(0.0, 2)
self._vehicle.applyEngineForce(0.0, 3)
if self._des_vel is not None:
self._des_vel = 0
self._vehicle_node.setLinearVelocity(Vec3(0.0, 0.0, 0.0))
self._vehicle_node.setAngularVelocity(Vec3(0.0, 0.0, 0.0))
for i in range(self._vehicle.getNumWheels()):
wheel = self._vehicle.getWheel(i)
wheel.setRotation(0.0)
self._vehicle_node.clearForces()
def _place_vehicle(self, pos=None, hpr=None):
if pos is None:
pos = self._default_pos()
if hpr is None:
hpr = self._default_hpr()
self._vehicle_pointer.setPos(pos[0], pos[1], pos[2])
self._vehicle_pointer.setHpr(hpr[0], hpr[1], hpr[2])
self._stop_car()
def _addWheel(self, pos, front, radius=0.25):
wheel = self._vehicle.createWheel()
wheel.setChassisConnectionPointCs(pos)
wheel.setFrontWheel(front)
wheel.setWheelDirectionCs(Vec3(0, 0, -1))
wheel.setWheelAxleCs(Vec3(1, 0, 0))
wheel.setWheelRadius(radius)
wheel.setMaxSuspensionTravelCm(40.0)
wheel.setSuspensionStiffness(40.0)
wheel.setWheelsDampingRelaxation(2.3)
wheel.setWheelsDampingCompression(4.4)
wheel.setFrictionSlip(1e2)
wheel.setRollInfluence(0.1)
# Task
def _update_task(self, task):
dt = globalClock.getDt()
self._update(dt=dt)
self._get_observation()
return task.cont
# Helper functions
def _get_observation(self):
self._obs = self._camera_sensor.observe()
observation = []
observation.append(self._obs[0])
if self._use_back_cam:
self._back_obs = self._back_camera_sensor.observe()
observation.append(self._back_obs[0])
observation = np.concatenate(observation, axis=2)
return observation
def _get_reward(self):
reward = self._collision_reward if self._collision else self._get_speed(
)
return reward
def _get_done(self):
return self._collision
def _get_info(self):
info = {}
info['pos'] = np.array(self._vehicle_pointer.getPos())
info['hpr'] = np.array(self._vehicle_pointer.getHpr())
info['vel'] = self._get_speed()
info['coll'] = self._collision
return info
def _back_up(self):
assert (self._use_vel)
back_up_vel = self._params['back_up'].get('vel', -2.0)
self._des_vel = back_up_vel
back_up_steer = self._params['back_up'].get('steer', (-5.0, 5.0))
# TODO
self._steering = np.random.uniform(*back_up_steer)
self._brakeForce = 0.
duration = self._params['back_up'].get('duration', 1.0)
self._update(dt=duration)
self._des_vel = 0.0
self._steering = 0.0
self._update(dt=duration)
self._brakeForce = 0.
def _is_contact(self):
result = self._world.contactTest(self._vehicle_node)
num_contacts = result.getNumContacts()
return result.getNumContacts() > 0
# Environment functions
def reset(self, pos=None, hpr=None, hard_reset=False, random_reset=False):
if self._do_back_up and not hard_reset and \
pos is None and hpr is None:
if self._collision:
self._back_up()
else:
if pos is None and hpr is None:
if random_reset:
pos, hpr = self._next_random_restart_pos_hpr()
else:
pos, hpr = self._next_restart_pos_hpr()
self._place_vehicle(pos=pos, hpr=hpr)
self._collision = False
return self._get_observation()
def step(self, action):
self._steering = action[0]
if action[1] == 0.0:
self._brakeForce = 1000.
else:
self._brakeForce = 0.
if self._use_vel:
# Convert from m/s to km/h
self._des_vel = action[1]
else:
self._engineForce = self._engineClamp * \
((action[1] - 49.5) / 49.5)
self._update(dt=self._dt)
observation = self._get_observation()
reward = self._get_reward()
done = self._get_done()
info = self._get_info()
return observation, reward, done, info
class CarPhys(Phys):
def __init__(self, mdt, carphys_props):
Phys.__init__(self, mdt)
self.pnode = None
self.vehicle = None
self.curr_speed_factor = 1.0
self.__prev_speed = 0
self.__finds = {} # cache for find's results
self.props = carphys_props
self._load_phys()
self.__set_collision_mesh()
self.__set_phys_node()
self.__set_vehicle()
self.__set_wheels()
eng.attach_obs(self.on_end_frame)
def _load_phys(self):
fpath = self.props.phys_file % self.mdt.name
with open(fpath) as phys_file: # pass phys props as a class
self.cfg = load(phys_file)
self.cfg['max_speed'] = self.get_speed()
self.cfg['friction_slip'] = self.get_friction()
self.cfg['roll_influence'] = self.get_roll_influence()
s_a = (self.mdt.name, round(self.cfg['max_speed'],
2), self.props.driver_engine)
LogMgr().log('speed %s: %s (%s)' % s_a)
fr_slip = round(self.cfg['friction_slip'], 2)
f_a = (self.mdt.name, fr_slip, self.props.driver_tires)
LogMgr().log('friction %s: %s (%s)' % f_a)
r_a = (self.mdt.name, round(self.cfg['roll_influence'],
2), self.props.driver_suspensions)
LogMgr().log('roll %s: %s (%s)' % r_a)
s_a = lambda field: setattr(self, field, self.cfg[field])
map(s_a, self.cfg.keys())
def __set_collision_mesh(self):
fpath = self.props.coll_path % self.mdt.name
self.coll_mesh = loader.loadModel(fpath)
chassis_shape = BulletConvexHullShape()
for geom in PhysMgr().find_geoms(self.coll_mesh, self.props.coll_name):
chassis_shape.addGeom(geom.node().getGeom(0), geom.getTransform())
self.mdt.gfx.nodepath.node().addShape(chassis_shape)
self.mdt.gfx.nodepath.setCollideMask(
BitMask32.bit(1)
| BitMask32.bit(2 + self.props.cars.index(self.mdt.name)))
#nodepath = self.mdt.gfx.nodepath.attachNewNode(BulletGhostNode('car ghost'))
#nodepath.node().addShape(BulletCapsuleShape(4, 5, ZUp))
#eng.attach_ghost(nodepath.node())
#nodepath.node().notifyCollisions(False)
def __set_phys_node(self):
self.pnode = self.mdt.gfx.nodepath.node()
self.pnode.setMass(self.mass)
self.pnode.setDeactivationEnabled(False)
PhysMgr().attach_rigid_body(self.pnode)
PhysMgr().add_collision_obj(self.pnode)
def __set_vehicle(self):
self.vehicle = BulletVehicle(PhysMgr().root, self.pnode)
self.vehicle.setCoordinateSystem(ZUp)
self.vehicle.setPitchControl(self.pitch_control)
tuning = self.vehicle.getTuning()
tuning.setSuspensionCompression(self.suspension_compression)
tuning.setSuspensionDamping(self.suspension_damping)
PhysMgr().attach_vehicle(self.vehicle)
def __set_wheels(self):
fwheel_bounds = self.mdt.gfx.wheels['fr'].get_tight_bounds()
f_radius = (fwheel_bounds[1][2] - fwheel_bounds[0][2]) / 2.0 + .01
rwheel_bounds = self.mdt.gfx.wheels['rr'].get_tight_bounds()
r_radius = (rwheel_bounds[1][2] - rwheel_bounds[0][2]) / 2.0 + .01
ffr = self.coll_mesh.find('**/' + self.props.wheel_names[0][0])
ffl = self.coll_mesh.find('**/' + self.props.wheel_names[0][1])
rrr = self.coll_mesh.find('**/' + self.props.wheel_names[0][2])
rrl = self.coll_mesh.find('**/' + self.props.wheel_names[0][3])
meth = self.coll_mesh.find
fr_node = ffr if ffr else meth('**/' + self.props.wheel_names[1][0])
fl_node = ffl if ffl else meth('**/' + self.props.wheel_names[1][1])
rr_node = rrr if rrr else meth('**/' + self.props.wheel_names[1][2])
rl_node = rrl if rrl else meth('**/' + self.props.wheel_names[1][3])
wheel_fr_pos = fr_node.get_pos() + (0, 0, f_radius)
wheel_fl_pos = fl_node.get_pos() + (0, 0, f_radius)
wheel_rr_pos = rr_node.get_pos() + (0, 0, r_radius)
wheel_rl_pos = rl_node.get_pos() + (0, 0, r_radius)
frw = self.mdt.gfx.wheels['fr']
flw = self.mdt.gfx.wheels['fl']
rrw = self.mdt.gfx.wheels['rr']
rlw = self.mdt.gfx.wheels['rl']
wheels_info = [(wheel_fr_pos, True, frw, f_radius),
(wheel_fl_pos, True, flw, f_radius),
(wheel_rr_pos, False, rrw, r_radius),
(wheel_rl_pos, False, rlw, r_radius)]
for (pos, front, nodepath, radius) in wheels_info:
self.__add_wheel(pos, front, nodepath.node(), radius)
def __add_wheel(self, pos, front, node, radius):
whl = self.vehicle.createWheel()
whl.setNode(node)
whl.setChassisConnectionPointCs(LPoint3f(*pos))
whl.setFrontWheel(front)
whl.setWheelDirectionCs((0, 0, -1))
whl.setWheelAxleCs((1, 0, 0))
whl.setWheelRadius(radius)
whl.setSuspensionStiffness(self.suspension_stiffness)
whl.setWheelsDampingRelaxation(self.wheels_damping_relaxation)
whl.setWheelsDampingCompression(self.wheels_damping_compression)
whl.setFrictionSlip(self.friction_slip) # high -> more adherence
whl.setRollInfluence(self.roll_influence) # low -> more stability
whl.setMaxSuspensionForce(self.max_suspension_force)
whl.setMaxSuspensionTravelCm(self.max_suspension_travel_cm)
whl.setSkidInfo(self.skid_info)
@property
def lateral_force(self):
vel = self.vehicle.get_chassis().get_linear_velocity()
vel.normalize()
dir = self.mdt.logic.car_vec
lat = dir.dot(vel)
lat_force = 0
if lat > .5:
lat_force = min(1, (lat - 1.0) / -.2)
return lat_force
@property
def is_flying(self): # no need to be cached
rays = [whl.getRaycastInfo() for whl in self.vehicle.get_wheels()]
return not any(ray.isInContact() for ray in rays)
@property
def prev_speed(self):
return self.__prev_speed
@property
def prev_speed_ratio(self):
return max(0, min(1.0, self.prev_speed / self.max_speed))
def on_end_frame(self):
self.__prev_speed = self.speed
@property
def speed(self):
if self.mdt.fsm.getCurrentOrNextState() == 'Countdown':
return 0 # getCurrentSpeedKmHour returns odd values otherwise
return self.vehicle.getCurrentSpeedKmHour()
@property
def speed_ratio(self):
return max(0, min(1.0, self.speed / self.max_speed))
def set_forces(self, eng_frc, brake_frc, steering):
self.vehicle.setSteeringValue(steering, 0)
self.vehicle.setSteeringValue(steering, 1)
self.vehicle.applyEngineForce(eng_frc, 2)
self.vehicle.applyEngineForce(eng_frc, 3)
self.vehicle.setBrake(brake_frc, 2)
self.vehicle.setBrake(brake_frc, 3)
def update_car_props(self):
speeds = []
for whl in self.vehicle.get_wheels():
contact_pt = whl.get_raycast_info().getContactPointWs()
gnd_name = self.gnd_name(contact_pt)
if not gnd_name:
continue
if gnd_name not in self.__finds:
gnd = self.props.track_phys.find('**/' + gnd_name)
self.__finds[gnd_name] = gnd
gfx_node = self.__finds[gnd_name]
if gfx_node.has_tag('speed'):
speeds += [float(gfx_node.get_tag('speed'))]
if gfx_node.has_tag('friction'):
fric = float(gfx_node.get_tag('friction'))
whl.setFrictionSlip(self.friction_slip * fric)
self.curr_speed_factor = (sum(speeds) / len(speeds)) if speeds else 1.0
@property
def gnd_names(self): # no need to be cached
whls = self.vehicle.get_wheels()
pos = map(lambda whl: whl.get_raycast_info().getContactPointWs(), whls)
return map(self.gnd_name, pos)
@staticmethod
def gnd_name(pos):
top = pos + (0, 0, 20)
bottom = pos + (0, 0, -20)
result = PhysMgr().ray_test_closest(bottom, top)
ground = result.get_node()
return ground.get_name() if ground else ''
def apply_damage(self, reset=False):
if reset:
self.max_speed = self.get_speed()
self.friction_slip = self.get_friction()
self.roll_influence = self.get_roll_influence()
else:
self.max_speed *= .95
self.friction_slip *= .95
self.roll_influence *= 1.05
fric = lambda whl: whl.setFrictionSlip(self.friction_slip)
map(fric, self.vehicle.get_wheels())
roll = lambda whl: whl.setRollInfluence(self.roll_influence)
map(roll, self.vehicle.get_wheels())
s_a = (str(round(self.max_speed, 2)), self.props.driver_engine)
LogMgr().log('speed: %s (%s)' % s_a)
f_a = (str(round(self.friction_slip, 2)), self.props.driver_tires)
LogMgr().log('friction: %s (%s)' % f_a)
r_a = (str(round(self.roll_influence,
2)), self.props.driver_suspensions)
LogMgr().log('roll: %s (%s)' % r_a)
def get_speed(self):
return self.cfg['max_speed'] * (1 + .01 * self.props.driver_engine)
def get_friction(self):
return self.cfg['friction_slip'] * (1 + .01 * self.props.driver_tires)
def get_roll_influence(self):
return self.cfg['roll_influence'] * (
1 + .01 * self.props.driver_suspensions)
def destroy(self):
eng.detach_obs(self.on_end_frame)
eng.remove_vehicle(self.vehicle)
self.pnode = self.vehicle = self.__finds = self.__track_phys = \
self.coll_mesh = None
Phys.destroy(self)
class CarEnv(DirectObject):
def __init__(self, params={}):
self._params = params
if 'random_seed' in self._params:
np.random.seed(self._params['random_seed'])
self._use_vel = self._params.get('use_vel', True)
self._run_as_task = self._params.get('run_as_task', False)
self._do_back_up = self._params.get('do_back_up', False)
self._use_depth = self._params.get('use_depth', False)
self._use_back_cam = self._params.get('use_back_cam', False)
self._collision_reward_only = self._params.get('collision_reward_only',
False)
self._collision_reward = self._params.get('collision_reward', -10.0)
self._obs_shape = self._params.get('obs_shape', (64, 36))
self._steer_limits = params.get('steer_limits', (-30., 30.))
self._speed_limits = params.get('speed_limits', (-4.0, 4.0))
self._motor_limits = params.get('motor_limits', (-0.5, 0.5))
self._fixed_speed = (self._speed_limits[0] == self._speed_limits[1]
and self._use_vel)
if not self._params.get('visualize', False):
loadPrcFileData('', 'window-type offscreen')
# Defines base, render, loader
try:
ShowBase()
except:
pass
base.setBackgroundColor(0.0, 0.0, 0.0, 1)
# World
self._worldNP = render.attachNewNode('World')
self._world = BulletWorld()
self._world.setGravity(Vec3(0, 0, -9.81))
self._dt = params.get('dt', 0.25)
self._step = 0.05
# Vehicle
shape = BulletBoxShape(Vec3(0.6, 1.0, 0.25))
ts = TransformState.makePos(Point3(0., 0., 0.25))
self._vehicle_node = BulletRigidBodyNode('Vehicle')
self._vehicle_node.addShape(shape, ts)
self._mass = self._params.get('mass', 10.)
self._vehicle_node.setMass(self._mass)
self._vehicle_node.setDeactivationEnabled(False)
self._vehicle_node.setCcdSweptSphereRadius(1.0)
self._vehicle_node.setCcdMotionThreshold(1e-7)
self._vehicle_pointer = self._worldNP.attachNewNode(self._vehicle_node)
self._world.attachRigidBody(self._vehicle_node)
self._vehicle = BulletVehicle(self._world, self._vehicle_node)
self._vehicle.setCoordinateSystem(ZUp)
self._world.attachVehicle(self._vehicle)
self._addWheel(Point3(0.3, 0.5, 0.07), True, 0.07)
self._addWheel(Point3(-0.3, 0.5, 0.07), True, 0.07)
self._addWheel(Point3(0.3, -0.5, 0.07), False, 0.07)
self._addWheel(Point3(-0.3, -0.5, 0.07), False, 0.07)
# Camera
size = self._params.get('size', [160, 90])
hfov = self._params.get('hfov', 120)
near_far = self._params.get('near_far', [0.1, 100.])
self._camera_sensor = Panda3dCameraSensor(base,
color=not self._use_depth,
depth=self._use_depth,
size=size,
hfov=hfov,
near_far=near_far,
title='front cam')
self._camera_node = self._camera_sensor.cam
self._camera_node.setPos(0.0, 0.5, 0.375)
self._camera_node.lookAt(0.0, 6.0, 0.0)
self._camera_node.reparentTo(self._vehicle_pointer)
if self._use_back_cam:
self._back_camera_sensor = Panda3dCameraSensor(
base,
color=not self._use_depth,
depth=self._use_depth,
size=size,
hfov=hfov,
near_far=near_far,
title='back cam')
self._back_camera_node = self._back_camera_sensor.cam
self._back_camera_node.setPos(0.0, -0.5, 0.375)
self._back_camera_node.lookAt(0.0, -6.0, 0.0)
self._back_camera_node.reparentTo(self._vehicle_pointer)
# Car Simulator
self._des_vel = None
self._setup()
# Input
self.accept('escape', self._doExit)
self.accept('r', self.reset)
self.accept('f1', self._toggleWireframe)
self.accept('f2', self._toggleTexture)
self.accept('f3', self._view_image)
self.accept('f5', self._doScreenshot)
self.accept('q', self._forward_0)
self.accept('w', self._forward_1)
self.accept('e', self._forward_2)
self.accept('a', self._left)
self.accept('s', self._stop)
self.accept('x', self._backward)
self.accept('d', self._right)
self.accept('m', self._mark)
self._steering = 0.0 # degree
self._engineForce = 0.0
self._brakeForce = 0.0
self._env_time_step = 0
self._p = self._params.get('p', 1.25)
self._d = self._params.get('d', 0.0)
self._last_err = 0.0
self._curr_time = 0.0
self._accelClamp = self._params.get('accelClamp', 0.5)
self._engineClamp = self._accelClamp * self._mass
self._collision = False
self._setup_spec()
self.spec = EnvSpec(observation_im_space=self.observation_im_space,
action_space=self.action_space,
action_selection_space=self.action_selection_space,
observation_vec_spec=self.observation_vec_spec,
action_spec=self.action_spec,
action_selection_spec=self.action_selection_spec,
goal_spec=self.goal_spec)
if self._run_as_task:
self._mark_d = 0.0
taskMgr.add(self._update_task, 'updateWorld')
base.run()
def _setup_spec(self):
self.action_spec = OrderedDict()
self.action_selection_spec = OrderedDict()
self.observation_vec_spec = OrderedDict()
self.goal_spec = OrderedDict()
self.action_spec['steer'] = Box(low=-45., high=45.)
self.action_selection_spec['steer'] = Box(low=self._steer_limits[0],
high=self._steer_limits[1])
if self._use_vel:
self.action_spec['speed'] = Box(low=-4., high=4.)
self.action_space = Box(low=np.array([
self.action_spec['steer'].low[0],
self.action_spec['speed'].low[0]
]),
high=np.array([
self.action_spec['steer'].high[0],
self.action_spec['speed'].high[0]
]))
self.action_selection_spec['speed'] = Box(
low=self._speed_limits[0], high=self._speed_limits[1])
self.action_selection_space = Box(
low=np.array([
self.action_selection_spec['steer'].low[0],
self.action_selection_spec['speed'].low[0]
]),
high=np.array([
self.action_selection_spec['steer'].high[0],
self.action_selection_spec['speed'].high[0]
]))
else:
self.action_spec['motor'] = Box(low=-self._accelClamp,
high=self._accelClamp)
self.action_space = Box(low=np.array([
self.action_spec['steer'].low[0],
self.action_spec['motor'].low[0]
]),
high=np.array([
self.action_spec['steer'].high[0],
self.action_spec['motor'].high[0]
]))
self.action_selection_spec['motor'] = Box(
low=self._motor_limits[0], high=self._motor_limits[1])
self.action_selection_space = Box(
low=np.array([
self.action_selection_spec['steer'].low[0],
self.action_selection_spec['motor'].low[0]
]),
high=np.array([
self.action_selection_spec['steer'].high[0],
self.action_selection_spec['motor'].high[0]
]))
assert (np.logical_and(
self.action_selection_space.low >= self.action_space.low - 1e-4,
self.action_selection_space.high <=
self.action_space.high + 1e-4).all())
self.observation_im_space = Box(low=0,
high=255,
shape=tuple(
self._get_observation()[0].shape))
self.observation_vec_spec['coll'] = Discrete(1)
self.observation_vec_spec['heading'] = Box(low=0, high=2 * 3.14)
self.observation_vec_spec['speed'] = Box(low=-4.0, high=4.0)
@property
def _base_dir(self):
return os.path.join(os.path.dirname(os.path.abspath(__file__)),
'models')
@property
def horizon(self):
return np.inf
@property
def max_reward(self):
return np.inf
# _____HANDLER_____
def _doExit(self):
sys.exit(1)
def _toggleWireframe(self):
base.toggleWireframe()
def _toggleTexture(self):
base.toggleTexture()
def _doScreenshot(self):
base.screenshot('Bullet')
def _forward_0(self):
self._des_vel = 1
self._brakeForce = 0.0
def _forward_1(self):
self._des_vel = 2
self._brakeForce = 0.0
def _forward_2(self):
self._des_vel = 4
self._brakeForce = 0.0
def _stop(self):
self._des_vel = 0.0
self._brakeForce = 0.0
def _backward(self):
self._des_vel = -4
self._brakeForce = 0.0
def _right(self):
self._steering = np.min([np.max([-30, self._steering - 5]), 0.0])
def _left(self):
self._steering = np.max([np.min([30, self._steering + 5]), 0.0])
def _view_image(self):
from matplotlib import pyplot as plt
image = self._camera_sensor.observe()[0]
if self._use_depth:
plt.imshow(image[:, :, 0], cmap='gray')
else:
def rgb2gray(rgb):
return np.dot(rgb[..., :3], [0.299, 0.587, 0.114])
image = rgb2gray(image)
im = cv2.resize(image, (64, 36), interpolation=cv2.INTER_AREA
) # TODO how does this deal with aspect ratio
plt.imshow(im.astype(np.uint8), cmap='Greys_r')
plt.show()
def _mark(self):
self._mark_d = 0.0
# Setup
def _setup(self):
self._setup_map()
self._place_vehicle()
self._setup_light()
self._setup_restart_pos()
def _setup_map(self):
if hasattr(self, '_model_path'):
# Collidable objects
self._setup_collision_object(self._model_path)
else:
ground = self._worldNP.attachNewNode(BulletRigidBodyNode('Ground'))
shape = BulletPlaneShape(Vec3(0, 0, 1), 0)
ground.node().addShape(shape)
ground.setCollideMask(BitMask32.allOn())
self._world.attachRigidBody(ground.node())
def _setup_collision_object(self,
path,
pos=(0.0, 0.0, 0.0),
hpr=(0.0, 0.0, 0.0),
scale=1):
visNP = loader.loadModel(path)
visNP.clearModelNodes()
visNP.reparentTo(render)
visNP.setPos(pos[0], pos[1], pos[2])
visNP.setHpr(hpr[0], hpr[1], hpr[2])
visNP.set_scale(scale, scale, scale)
bodyNPs = BulletHelper.fromCollisionSolids(visNP, True)
for bodyNP in bodyNPs:
bodyNP.reparentTo(render)
bodyNP.setPos(pos[0], pos[1], pos[2])
bodyNP.setHpr(hpr[0], hpr[1], hpr[2])
bodyNP.set_scale(scale, scale, scale)
if isinstance(bodyNP.node(), BulletRigidBodyNode):
bodyNP.node().setMass(0.0)
bodyNP.node().setKinematic(True)
bodyNP.setCollideMask(BitMask32.allOn())
self._world.attachRigidBody(bodyNP.node())
else:
print("Issue")
def _setup_restart_pos(self):
self._restart_index = 0
self._restart_pos = self._default_restart_pos()
def _next_restart_pos_hpr(self):
num = len(self._restart_pos)
if num == 0:
return None, None
else:
pos_hpr = self._restart_pos[self._restart_index]
self._restart_index = (self._restart_index + 1) % num
return pos_hpr[:3], pos_hpr[3:]
def _setup_light(self):
# alight = AmbientLight('ambientLight')
# alight.setColor(Vec4(0.5, 0.5, 0.5, 1))
# alightNP = render.attachNewNode(alight)
# render.clearLight()
# render.setLight(alightNP)
pass
# Vehicle
def _default_pos(self):
return (0.0, 0.0, 0.3)
def _default_hpr(self):
return (0.0, 0.0, 0.0)
def _default_restart_pos(self):
return [self._default_pos() + self._default_hpr()]
def _get_speed(self):
vel = self._vehicle.getCurrentSpeedKmHour() / 3.6
return vel
def _get_heading(self):
h = np.array(self._vehicle_pointer.getHpr())[0]
ori = h * (pi / 180.)
while (ori > 2 * pi):
ori -= 2 * pi
while (ori < 0):
ori += 2 * pi
return ori
def _update(self, dt=1.0, coll_check=True):
self._vehicle.setSteeringValue(self._steering, 0)
self._vehicle.setSteeringValue(self._steering, 1)
self._vehicle.setBrake(self._brakeForce, 0)
self._vehicle.setBrake(self._brakeForce, 1)
self._vehicle.setBrake(self._brakeForce, 2)
self._vehicle.setBrake(self._brakeForce, 3)
if dt >= self._step:
# TODO maybe change number of timesteps
# for i in range(int(dt/self._step)):
if self._des_vel is not None:
vel = self._get_speed()
err = self._des_vel - vel
d_err = (err - self._last_err) / self._step
self._last_err = err
self._engineForce = np.clip(self._p * err + self._d * d_err,
-self._accelClamp,
self._accelClamp) * self._mass
self._vehicle.applyEngineForce(self._engineForce, 0)
self._vehicle.applyEngineForce(self._engineForce, 1)
self._vehicle.applyEngineForce(self._engineForce, 2)
self._vehicle.applyEngineForce(self._engineForce, 3)
for _ in range(int(dt / self._step)):
self._world.doPhysics(self._step, 1, self._step)
self._collision = self._is_contact()
elif self._run_as_task:
self._curr_time += dt
if self._curr_time > 0.05:
if self._des_vel is not None:
vel = self._get_speed()
self._mark_d += vel * self._curr_time
print(vel, self._mark_d, self._is_contact())
err = self._des_vel - vel
d_err = (err - self._last_err) / 0.05
self._last_err = err
self._engineForce = np.clip(
self._p * err + self._d * d_err, -self._accelClamp,
self._accelClamp) * self._mass
self._curr_time = 0.0
self._vehicle.applyEngineForce(self._engineForce, 0)
self._vehicle.applyEngineForce(self._engineForce, 1)
self._vehicle.applyEngineForce(self._engineForce, 2)
self._vehicle.applyEngineForce(self._engineForce, 3)
self._world.doPhysics(dt, 1, dt)
self._collision = self._is_contact()
else:
raise ValueError(
"dt {0} s is too small for velocity control".format(dt))
def _stop_car(self):
self._steering = 0.0
self._engineForce = 0.0
self._vehicle.setSteeringValue(0.0, 0)
self._vehicle.setSteeringValue(0.0, 1)
self._vehicle.applyEngineForce(0.0, 0)
self._vehicle.applyEngineForce(0.0, 1)
self._vehicle.applyEngineForce(0.0, 2)
self._vehicle.applyEngineForce(0.0, 3)
if self._des_vel is not None:
self._des_vel = 0
self._vehicle_node.setLinearVelocity(Vec3(0.0, 0.0, 0.0))
self._vehicle_node.setAngularVelocity(Vec3(0.0, 0.0, 0.0))
for i in range(self._vehicle.getNumWheels()):
wheel = self._vehicle.getWheel(i)
wheel.setRotation(0.0)
self._vehicle_node.clearForces()
def _place_vehicle(self, pos=None, hpr=None):
if pos is None:
pos = self._default_pos()
if hpr is None:
hpr = self._default_hpr()
self._vehicle_pointer.setPos(pos[0], pos[1], pos[2])
self._vehicle_pointer.setHpr(hpr[0], hpr[1], hpr[2])
self._stop_car()
def _addWheel(self, pos, front, radius=0.25):
wheel = self._vehicle.createWheel()
wheel.setChassisConnectionPointCs(pos)
wheel.setFrontWheel(front)
wheel.setWheelDirectionCs(Vec3(0, 0, -1))
wheel.setWheelAxleCs(Vec3(1, 0, 0))
wheel.setWheelRadius(radius)
wheel.setMaxSuspensionTravelCm(40.0)
wheel.setSuspensionStiffness(40.0)
wheel.setWheelsDampingRelaxation(2.3)
wheel.setWheelsDampingCompression(4.4)
wheel.setFrictionSlip(1e2)
wheel.setRollInfluence(0.1)
# Task
def _update_task(self, task):
dt = globalClock.getDt()
self._update(dt=dt)
self._get_observation()
return task.cont
# Helper functions
def _get_observation(self):
self._obs = self._camera_sensor.observe()
observation = []
observation.append(self.process(self._obs[0], self._obs_shape))
if self._use_back_cam:
self._back_obs = self._back_camera_sensor.observe()
observation.append(self.process(self._back_obs[0],
self._obs_shape))
observation_im = np.concatenate(observation, axis=2)
coll = self._collision
heading = self._get_heading()
speed = self._get_speed()
observation_vec = np.array([coll, heading, speed])
return observation_im, observation_vec
def _get_goal(self):
return np.array([])
def process(self, image, obs_shape):
if self._use_depth:
im = np.reshape(image, (image.shape[0], image.shape[1]))
if im.shape != obs_shape:
im = cv2.resize(im, obs_shape, interpolation=cv2.INTER_AREA)
return im.astype(np.uint8)
else:
image = np.dot(image[..., :3], [0.299, 0.587, 0.114])
im = cv2.resize(image, obs_shape, interpolation=cv2.INTER_AREA
) #TODO how does this deal with aspect ratio
# TODO might not be necessary
im = np.expand_dims(im, 2)
return im.astype(np.uint8)
def _get_reward(self):
if self._collision_reward_only:
if self._collision:
reward = self._collision_reward
else:
reward = 0.0
else:
if self._collision:
reward = self._collision_reward
else:
reward = self._get_speed()
assert (reward <= self.max_reward)
return reward
def _get_done(self):
return self._collision
def _get_info(self):
info = {}
info['pos'] = np.array(self._vehicle_pointer.getPos())
info['hpr'] = np.array(self._vehicle_pointer.getHpr())
info['vel'] = self._get_speed()
info['coll'] = self._collision
info['env_time_step'] = self._env_time_step
return info
def _back_up(self):
assert (self._use_vel)
# logger.debug('Backing up!')
self._params['back_up'] = self._params.get('back_up', {})
back_up_vel = self._params['back_up'].get('vel', -1.0)
self._des_vel = back_up_vel
back_up_steer = self._params['back_up'].get('steer', (-5.0, 5.0))
self._steering = np.random.uniform(*back_up_steer)
self._brakeForce = 0.
duration = self._params['back_up'].get('duration', 3.0)
self._update(dt=duration)
self._des_vel = 0.
self._steering = 0.
self._update(dt=duration)
self._brakeForce = 0.
def _is_contact(self):
result = self._world.contactTest(self._vehicle_node)
return result.getNumContacts() > 0
# Environment functions
def reset(self, pos=None, hpr=None, hard_reset=False):
if self._do_back_up and not hard_reset and \
pos is None and hpr is None:
if self._collision:
self._back_up()
else:
if hard_reset:
logger.debug('Hard resetting!')
if pos is None and hpr is None:
pos, hpr = self._next_restart_pos_hpr()
self._place_vehicle(pos=pos, hpr=hpr)
self._collision = False
self._env_time_step = 0
return self._get_observation(), self._get_goal()
def step(self, action):
self._steering = action[0]
if action[1] == 0.0:
self._brakeForce = 1000.
else:
self._brakeForce = 0.
if self._use_vel:
# Convert from m/s to km/h
self._des_vel = action[1]
else:
self._engineForce = self._mass * action[1]
self._update(dt=self._dt)
observation = self._get_observation()
goal = self._get_goal()
reward = self._get_reward()
done = self._get_done()
info = self._get_info()
self._env_time_step += 1
return observation, goal, reward, done, info
class RoadRally(ShowBase):
def __init__(self):
ShowBase.__init__(self)
scene = BulletWorld()
scene.setGravity(Vec3(0, 0, -9.81))
base.setBackgroundColor(0.6, 0.9, 0.9)
#Variables
self.steering = 0
#Controls
inputState.watchWithModifiers("F", "arrow_up")
inputState.watchWithModifiers("B", "arrow_down")
inputState.watchWithModifiers("L", "arrow_left")
inputState.watchWithModifiers("R", "arrow_right")
#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.track_model = loader.loadModel("Models/Track.egg")
self.track_model.reparentTo(self.render)
self.track_model.setPos(0, 0, -7)
self.track_tex = loader.loadTexture("Textures/Road.jpg")
self.track_model.setTexture(self.track_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, 0)
Car_np.setHpr(0, 0, 0)
Car_np.node().setDeactivationEnabled(False)
scene.attachRigidBody(Car_node)
#Load and transform the Car Actor
self.car_model = loader.loadModel("Models/Car.egg")
self.car_model.setPos(0, 20, -3)
self.car_model.setHpr(180, 0, 0)
self.car_model.reparentTo(Car_np)
self.Car_sim = BulletVehicle(scene, Car_np.node())
self.Car_sim.setCoordinateSystem(ZUp)
scene.attachVehicle(self.Car_sim)
#Camera
#base.disableMouse()
camera.reparentTo(Car_np)
camera.setPos(0, 0, 0)
camera.setHpr(0, 0, 0)
def Wheel(pos, r, f):
w = self.Car_sim.createWheel()
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
Wheel(Point3(-1, 1, -0.6), 0.4, False)
Wheel(Point3(-1.1, -1.2, -0.6), 0.4, True)
Wheel(Point3(1.1, -1, -0.6), 0.4, True)
Wheel(Point3(1, 1, -0.6), 0.4, False)
def ProcessInput(dt):
engineForce = 0.0
self.steeringClamp = 35.0
self.steeringIncrement = 70
#Get the vehicle's current speed
self.carspeed = self.Car_sim.getCurrentSpeedKmHour()
#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
if inputState.isSet("F"):
engineForce = 35
if inputState.isSet("B"):
engineForce = -35
#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)
#Apply forces to wheels
self.Car_sim.applyEngineForce(engineForce, 0)
self.Car_sim.applyEngineForce(engineForce, 3)
def Update(task):
dt = globalClock.getDt()
ProcessInput(dt)
scene.doPhysics(dt, 5, 1.0 / 180.0)
return task.cont
taskMgr.add(Update, "Update")
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()
