def reset(self):
self._destroy()
self.reward = 0.0
self.prev_reward = 0.0
self.tile_visited_count = 0
self.t = 0.0
self.road_poly = []
self.human_render = False
while True:
success = self._create_track()
if success: break
print("retry to generate track (normal if there are not many of this messages)")
# self.car = Car(self.world, *self.track[0][1:4])
# changed to 0 angle and x, y from my track)
self.car = Car(self.world, -math.pi/2, self.track[0][0]+5, self.track[0][1]+5)
self.car2 = Car(self.world, math.pi/2, self.track[0][0]+25, self.track[0][1]+5, color=(0, 0.5, 0))
# self.car2 = Car(self.world, math.pi/2, self.track[-1][-1], self.track[-1][-2], color=(0, 0.5, 0))
self.car3 = Car(self.world, math.pi, TRACK_WIDTH-5, PLAYFIELD, color=(0, 0.75, 1))
return self.step(None)[0]
def run(self):
car = Car(self.window_width / 20, self.window_height / 20)
track = Track('track3.svg')
track_drawer = TrackDrawer(track)
car.position.x, car.position.y = track.path[0][0] / 10 - 1366 / 20, track.path[0][1] / 10 - 768 / 20
car_drawer = CarDrawer()
input_provider = JoystickInputProvider()
if not input_provider.joysticks:
input_provider = KeyboardInputProvider()
car_data_display = CarDataDisplay(car)
trace = [(car.position.x * 10 + 1366/2, car.position.y * 10 + 768/2) for _ in range(3)]
while not self.exit:
dt = self.clock.tick(self.fps) / 1000
# Event queue
for event in pygame.event.get():
if event.type == pygame.QUIT:
self.exit = True
trace.pop(2)
trace.insert(0, (car.position.x * 10 + 1366/2, car.position.y * 10 + 768/2))
# User input
car_input = input_provider.get_input()
car.get_driver_input(car_input[0], car_input[1], car_input[2], car_input[3])
car.update(dt)
# Drawing
self.screen.fill((0, 0, 0))
track_drawer.draw(self.screen, car.position * 10, trace)
car_drawer.draw(self.screen, car)
car_data_display.display_data(self.screen)
pygame.display.flip()
pygame.quit()
import sys
import os.path
sys.path.append(os.path.dirname(sys.path[0]))
from car_model import Car
global car
# Tests to check that the variables are properly set
car = Car()
def test_no_velocity():
assert (car.force_req(0) == 10.594800000000001)
def test_10_ms():
assert (car.force_req(10) == 19.7823)
def test_energy_used():
assert (car.energy_used([5, 7, 9, 13, 9.5, 7, 5],
[-1, 1, 3, 3, 1, 1, 1.5]) == 28267.47205752044)
def car_min_0_max_1000():
car = Car()
car.speed_min_ms = 0
car.speed_max_ms = 1000
return car
def car_min_15_max_45():
car = Car()
car.speed_min_ms = 15
car.speed_max_ms = 45
return car
def run(self, dt, solution, car=None):
if car is None:
car = Car(1366 / 20, 768 / 20)
car.position.x, car.position.y = self.track.path[0][
0] / 10 - 1366 / 20, self.track.path[0][1] / 10 - 768 / 20
self.track.apply_deformations(list(solution.Deformation))
input_provider = AutonomousDriver(solution)
time = 0
trace = [(car.position.x * 10 + 1366 / 2,
car.position.y * 10 + 768 / 2) for _ in range(3)]
# Indicators
track_path_length = LineString(
self.track.path).length / 10 # length of path loaded from solution
regulation_quality_indicator_square = 0 # integral of squared error values
regulation_quality_indicator_abs = 0 # integral of abs values
max_line_error = 0 # error in m
average_slip = 0 # average of abs lateral speed in m / s
slip_indicator = 0 # integral of squared speed in m / s^2
max_speed_long = 0 # m / s^2
max_speed_lat = 0 # m / s^2
average_speed_long = 0 # average speed longitudinal in m
average_rpm = 0
max_rpm = 0
number_of_iterations = 0 # helper variable for computing averages
while True:
time += dt
if time > self.timeout:
break
# User input
indexes = self.track.check_car_position(trace)
input_provider.index = indexes[-1]
car_input = input_provider.get_input()
car.get_driver_input(car_input[0], car_input[1], car_input[2],
car_input[3])
car.update(dt)
vector = Vector2(40, 0).rotate(-car.angle)
vector = np.array((vector.x + 1366 / 2, vector.y + 768 / 2))
front_center = Point(
np.array((car.position.x * 10, car.position.y * 10)) + vector)
if Polygon(self.track.path).contains(front_center):
input_provider.line_error = -LineString(
self.track.path).distance(front_center)
else:
input_provider.line_error = LineString(
self.track.path).distance(front_center)
# Update solution indicators
number_of_iterations += 1
average_rpm = average_rpm * (
number_of_iterations -
1) / number_of_iterations + car.rpm / number_of_iterations
average_speed_long = average_speed_long * (
number_of_iterations - 1
) / number_of_iterations + car.velocity.x / number_of_iterations
average_slip = average_slip * (
number_of_iterations - 1) / number_of_iterations + abs(
car.velocity.y) / number_of_iterations
regulation_quality_indicator_square += (input_provider.line_error /
10)**2
regulation_quality_indicator_abs += abs(input_provider.line_error /
10)
if abs(input_provider.line_error) / 10 > max_line_error:
max_line_error = abs(input_provider.line_error) / 10
slip_indicator += car.velocity.y**2
if car.velocity.x > max_speed_long:
max_speed_long = car.velocity.x
if abs(car.velocity.y) > max_speed_lat:
max_speed_lat = abs(car.velocity.y)
if car.rpm > max_rpm:
max_rpm = car.rpm
trace.pop(2)
trace.insert(0, (car.position.x * 10 + 1366 / 2,
car.position.y * 10 + 768 / 2))
if input_provider.index == len(self.track.track_chunks) - 1:
break
finished = True
for chunk in self.track.track_chunks:
if not chunk.is_active:
finished = False
break
for index in range(1, len(self.track.track_chunks)):
self.track.track_chunks[index].is_active = False
if finished and time < self.timeout:
return time, (track_path_length, average_rpm, average_speed_long,
average_slip, regulation_quality_indicator_abs,
regulation_quality_indicator_square, max_line_error,
slip_indicator, max_speed_long, max_speed_lat,
max_rpm)
else:
return 99999, (track_path_length, average_rpm, average_speed_long,
average_slip, regulation_quality_indicator_abs,
regulation_quality_indicator_square, max_line_error,
slip_indicator, max_speed_long, max_speed_lat,
max_rpm)
def run_pid_controller(self, track_path, solution_path='solutionOpt.csv', dt=0.05):
car = Car(self.window_width / 20, self.window_height / 20)
track = Track(track_path)
track_drawer = TrackDrawer(track)
solution = pd.read_csv(solution_path, index_col=0)
track.apply_deformations(list(solution.Deformation))
car.position.x, car.position.y = track.path[0][0] / 10 - 1366 / 20, track.path[0][1] / 10 - 768 / 20
car_drawer = CarDrawer()
car_data_display = CarDataDisplay(car)
input_provider = AutonomousDriver(solution)
time = 0
trace = [(car.position.x * 10 + 1366/2, car.position.y * 10 + 768/2) for _ in range(3)]
while not self.exit:
# Handling time
self.clock.tick(1 / dt)
time += dt
# Event queue
for event in pygame.event.get():
if event.type == pygame.QUIT:
self.exit = True
# Input from solution file and PID controller
input_provider.index = track_drawer.chunk_indexes[-1] # Incrementing row index of solution matrix
car_input = input_provider.get_input()
car.get_driver_input(car_input[0], car_input[1], car_input[2], car_input[3])
car.update(dt)
# Calculating position of a front center of a car
vector = Vector2(40, 0).rotate(-car.angle)
vector = np.array((vector.x + 1366 / 2, vector.y + 768 / 2))
front_center = Point(np.array((car.position.x * 10, car.position.y * 10)) + vector)
# Calculating line error as a distance from front center to a given line (path)
if Polygon(track.path).contains(front_center):
input_provider.line_error = - LineString(track.path).distance(front_center)
else:
input_provider.line_error = LineString(track.path).distance(front_center)
# Updating trace
trace.pop(2)
trace.insert(0, (car.position.x * 10 + 1366 / 2, car.position.y * 10 + 768 / 2))
# Drawing
self.screen.fill((0, 0, 0))
self.background.set_location([- car.position.x * 10, 768 - car.position.y * 10])
self.screen.blit(self.background.image, self.background.rect)
self.background.set_location([1200 - car.position.x * 10, 768 - car.position.y * 10])
self.screen.blit(self.background.image, self.background.rect)
self.background.set_location([2400 - car.position.x * 10, 768 - car.position.y * 10])
self.screen.blit(self.background.image, self.background.rect)
self.background.set_location([- car.position.x * 10, 768 + 1200 - car.position.y * 10])
self.screen.blit(self.background.image, self.background.rect)
self.background.set_location([1200 - car.position.x * 10, 768 + 1200 - car.position.y * 10])
self.screen.blit(self.background.image, self.background.rect)
self.background.set_location([2400 - car.position.x * 10, 768 + 1200 - car.position.y * 10])
self.screen.blit(self.background.image, self.background.rect)
self.background.set_location([- car.position.x * 10, 768 + 2400 - car.position.y * 10])
self.screen.blit(self.background.image, self.background.rect)
self.background.set_location([1200 - car.position.x * 10, 768 + 2400 - car.position.y * 10])
self.screen.blit(self.background.image, self.background.rect)
self.background.set_location([2400 - car.position.x * 10, 768 + 2400 - car.position.y * 10])
self.screen.blit(self.background.image, self.background.rect)
track_drawer.draw(self.screen, car.position * 10, trace)
car_drawer.draw(self.screen, car)
car_data_display.display_data(self.screen)
rect = pygame.Rect(front_center.x - car.position.x * 10, front_center.y - car.position.y * 10, 5, 5)
pygame.draw.rect(self.screen, (0, 0, 255), rect)
pygame.display.flip()
# Checking if car has passed finishing-line
if input_provider.index == len(track.track_chunks) - 1:
break
# Checking if car completed track
completed = True
for chunk in track.track_chunks:
if not chunk.is_active:
completed = False
break
print("Time:", time, "Completed:", completed)
pygame.quit()
class CarRacing(gym.Env, EzPickle):
metadata = {
'render.modes': ['human', 'rgb_array', 'state_pixels'],
'video.frames_per_second' : FPS
}
def __init__(self):
EzPickle.__init__(self)
self.seed()
self.contactListener_keepref = FrictionDetector(self)
self.world = Box2D.b2World((0,0), contactListener=self.contactListener_keepref)
self.viewer = None
self.invisible_state_window = None
self.invisible_video_window = None
self.road = None
self.car = None
self.car2 = None
self.reward = 0.0
self.prev_reward = 0.0
self.action_space = spaces.Box( np.array([-1,0,0]), np.array([+1,+1,+1]), dtype=np.float32) # steer, gas, brake
self.observation_space = spaces.Box(low=0, high=255, shape=(STATE_H, STATE_W, 3), dtype=np.uint8)
def seed(self, seed=None):
self.np_random, seed = seeding.np_random(seed)
return [seed]
def _destroy(self):
if not self.road: return
for t in self.road:
self.world.DestroyBody(t)
self.road = []
self.car.destroy()
self.car2.destroy()
def _create_track(self):
########################################################################
t_p = PLAYFIELD
t_w = TRACK_WIDTH
# My cross_road interscections:
my_r1 = [(-t_p, -t_w), (-t_p, t_w), (t_p, t_w), (t_p, -t_w)]
my_r2 = [(-t_w, -t_w), (-t_w, -t_p), (t_w, -t_p), (t_w, -t_w)]
my_r3 = [(t_w, t_w), (t_w, t_p), (-t_w, t_p), (-t_w, t_w)]
self.road_poly = [(my_r1, ROAD_COLOR), (my_r2, ROAD_COLOR), (my_r3, ROAD_COLOR)]
self.road = []
# We create road with 10x10 tiles over all fiels and we change colors a little bit if outside road;
track = [(i, j) for i in np.arange(-t_p, t_p, 20/SCALE) for j in np.arange(-t_w, t_w, 20/SCALE)]
track.extend([(i, j) for i in np.arange(-t_w, t_w, 20/SCALE) for j in np.arange(-t_p, -t_w, 20/SCALE)])
track.extend([(i, j) for i in np.arange(-t_w, t_w, 20/SCALE) for j in np.arange(t_w, t_p, 20/SCALE)])
for i, (x, y) in enumerate(track):
road1_l = (x, y)
road1_r = (x, y+20/SCALE)
road2_l = (x+20/SCALE, y)
road2_r = (x+20/SCALE, y+20/SCALE)
t = self.world.CreateStaticBody( fixtures = fixtureDef(
shape=polygonShape(vertices=[road1_l, road1_r, road2_r, road2_l])
))
t.userData = t
c = 0.01*(i%3)
t.color = [ROAD_COLOR[0] + c, ROAD_COLOR[1] + c, ROAD_COLOR[2] + c]
t.road_visited = False
t.road_friction = 1.0
t.fixtures[0].sensor = True
self.road_poly.append(( [road1_l, road1_r, road2_r, road2_l], t.color ))
self.road.append(t)
self.track = track
########################################################################
return True
def reset(self):
self._destroy()
self.reward = 0.0
self.prev_reward = 0.0
self.tile_visited_count = 0
self.t = 0.0
self.road_poly = []
self.human_render = False
while True:
success = self._create_track()
if success: break
print("retry to generate track (normal if there are not many of this messages)")
# self.car = Car(self.world, *self.track[0][1:4])
# changed to 0 angle and x, y from my track)
self.car = Car(self.world, -math.pi/2, self.track[0][0]+5, self.track[0][1]+5)
self.car2 = Car(self.world, math.pi/2, self.track[0][0]+25, self.track[0][1]+5, color=(0, 0.5, 0))
# self.car2 = Car(self.world, math.pi/2, self.track[-1][-1], self.track[-1][-2], color=(0, 0.5, 0))
self.car3 = Car(self.world, math.pi, TRACK_WIDTH-5, PLAYFIELD, color=(0, 0.75, 1))
return self.step(None)[0]
def step(self, action):
if action is not None:
self.car.steer(-action[0])
self.car.gas(action[1])
self.car.brake(action[2])
for i, c in enumerate(self.car.hull.contacts):
if c.contact.manifold.pointCount != 0:
print("Collision")
#car2 movements:
self.car2.steer(0.01)
self.car2.gas(0.1)
self.car2.brake((np.random.rand()<0.3)*0.2)
#car3 movements:
self.car3.steer(-0.01)
self.car3.gas(0.1)
self.car3.brake((np.random.rand()<0.3)*0.2)
self.car.step(1.0/FPS)
self.car2.step(1.0/FPS)
self.car3.step(1.0/FPS)
self.world.Step(1.0/FPS, 6*30, 2*30)
self.t += 1.0/FPS
self.state = self.render("state_pixels")
step_reward = 0
done = False
if action is not None: # First step without action, called from reset()
self.reward -= 0.1
# We actually don't want to count fuel spent, we want car to be faster.
#self.reward -= 10 * self.car.fuel_spent / ENGINE_POWER
self.car.fuel_spent = 0.0
step_reward = self.reward - self.prev_reward
self.prev_reward = self.reward
if self.tile_visited_count==len(self.track):
done = True
x, y = self.car.hull.position
if abs(x) > PLAYFIELD or abs(y) > PLAYFIELD:
done = True
step_reward = -100
return self.state, step_reward, done, {}
def render(self, mode='human'):
if self.viewer is None:
from gym.envs.classic_control import rendering
self.viewer = rendering.Viewer(WINDOW_W, WINDOW_H)
self.score_label = pyglet.text.Label('0000', font_size=36,
x=20, y=WINDOW_H*2.5/40.00, anchor_x='left', anchor_y='center',
color=(255,255,255,255))
self.transform = rendering.Transform()
if "t" not in self.__dict__: return # reset() not called yet
zoom = 0.1*SCALE*max(1-self.t, 0) + ZOOM*SCALE*min(self.t, 1) # Animate zoom first second
zoom_state = ZOOM*SCALE*STATE_W/WINDOW_W
zoom_video = ZOOM*SCALE*VIDEO_W/WINDOW_W
scroll_x = 0 #self.car.hull.position[0] #0
scroll_y = -30 #self.car.hull.position[1] #-30
angle = 0 #-self.car.hull.angle #0
vel = 0 #self.car.hull.linearVelocity #0
if np.linalg.norm(vel) > 0.5:
angle = math.atan2(vel[0], vel[1])
self.transform.set_scale(zoom, zoom)
self.transform.set_translation(
WINDOW_W/2 - (scroll_x*zoom*math.cos(angle) - scroll_y*zoom*math.sin(angle)),
WINDOW_H/4 - (scroll_x*zoom*math.sin(angle) + scroll_y*zoom*math.cos(angle)) )
self.transform.set_rotation(angle)
self.car.draw(self.viewer, mode!="state_pixels")
self.car2.draw(self.viewer, mode!="state_pixels")
self.car3.draw(self.viewer, mode!="state_pixels")
arr = None
win = self.viewer.window
if mode != 'state_pixels':
win.switch_to()
win.dispatch_events()
if mode=="rgb_array" or mode=="state_pixels":
win.clear()
t = self.transform
if mode=='rgb_array':
VP_W = VIDEO_W
VP_H = VIDEO_H
else:
VP_W = STATE_W
VP_H = STATE_H
gl.glViewport(0, 0, VP_W, VP_H)
t.enable()
self.render_road()
for geom in self.viewer.onetime_geoms:
geom.render()
t.disable()
self.render_indicators(WINDOW_W, WINDOW_H) # TODO: find why 2x needed, wtf
image_data = pyglet.image.get_buffer_manager().get_color_buffer().get_image_data()
arr = np.fromstring(image_data.data, dtype=np.uint8, sep='')
arr = arr.reshape(VP_H, VP_W, 4)
arr = arr[::-1, :, 0:3]
if mode=="rgb_array" and not self.human_render: # agent can call or not call env.render() itself when recording video.
win.flip()
if mode=='human':
self.human_render = True
win.clear()
t = self.transform
gl.glViewport(0, 0, WINDOW_W, WINDOW_H)
t.enable()
self.render_road()
for geom in self.viewer.onetime_geoms:
geom.render()
t.disable()
self.render_indicators(WINDOW_W, WINDOW_H)
win.flip()
self.viewer.onetime_geoms = []
return arr
def close(self):
if self.viewer is not None:
self.viewer.close()
self.viewer = None
def render_road(self):
gl.glBegin(gl.GL_QUADS)
gl.glColor4f(1, 1, 1, 1.0) #gl.glColor4f(0.4, 0.8, 0.4, 1.0)
gl.glVertex3f(-PLAYFIELD, +PLAYFIELD, 0)
gl.glVertex3f(+PLAYFIELD, +PLAYFIELD, 0)
gl.glVertex3f(+PLAYFIELD, -PLAYFIELD, 0)
gl.glVertex3f(-PLAYFIELD, -PLAYFIELD, 0)
# gl.glColor4f(0, 0, 0, 1.0) #gl.glColor4f(0.4, 0.9, 0.4, 1.0)
# k = PLAYFIELD/20.0
# for x in range(-20, 20, 2):
# for y in range(-20, 20, 2):
# gl.glVertex3f(k*x + k, k*y + 0, 0)
# gl.glVertex3f(k*x + 0, k*y + 0, 0)
# gl.glVertex3f(k*x + 0, k*y + k, 0)
# gl.glVertex3f(k*x + k, k*y + k, 0)
for poly, color in self.road_poly:
gl.glColor4f(color[0], color[1], color[2], 1)
for p in poly:
gl.glVertex3f(p[0], p[1], 0)
gl.glEnd()
def render_indicators(self, W, H):
gl.glBegin(gl.GL_QUADS)
s = W/40.0
h = H/40.0
gl.glColor4f(0,0,0,1)
gl.glVertex3f(W, 0, 0)
gl.glVertex3f(W, 5*h, 0)
gl.glVertex3f(0, 5*h, 0)
gl.glVertex3f(0, 0, 0)
def vertical_ind(place, val, color):
gl.glColor4f(color[0], color[1], color[2], 1)
gl.glVertex3f((place+0)*s, h + h*val, 0)
gl.glVertex3f((place+1)*s, h + h*val, 0)
gl.glVertex3f((place+1)*s, h, 0)
gl.glVertex3f((place+0)*s, h, 0)
def horiz_ind(place, val, color):
gl.glColor4f(color[0], color[1], color[2], 1)
gl.glVertex3f((place+0)*s, 4*h , 0)
gl.glVertex3f((place+val)*s, 4*h, 0)
gl.glVertex3f((place+val)*s, 2*h, 0)
gl.glVertex3f((place+0)*s, 2*h, 0)
true_speed = np.sqrt(np.square(self.car.hull.linearVelocity[0]) + np.square(self.car.hull.linearVelocity[1]))
vertical_ind(5, 0.02*true_speed, (1,1,1))
vertical_ind(7, 0.01*self.car.wheels[0].omega, (0.0,0,1)) # ABS sensors
vertical_ind(8, 0.01*self.car.wheels[1].omega, (0.0,0,1))
vertical_ind(9, 0.01*self.car.wheels[2].omega, (0.2,0,1))
vertical_ind(10,0.01*self.car.wheels[3].omega, (0.2,0,1))
horiz_ind(20, -10.0*self.car.wheels[0].joint.angle, (0,1,0))
horiz_ind(30, -0.8*self.car.hull.angularVelocity, (1,0,0))
gl.glEnd()
self.score_label.text = "%04i" % self.reward
self.score_label.draw()