-
Notifications
You must be signed in to change notification settings - Fork 0
/
KueiRaceway0521.py
76 lines (57 loc) · 2.38 KB
/
KueiRaceway0521.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
import math
def reward_function(params):
waypoints = params['waypoints']
closest_waypoints = params['closest_waypoints']
heading = params['heading']
track_width = params['track_width']
distance_from_center = params['distance_from_center']
all_wheels_on_track = params['all_wheels_on_track']
speed = params['speed']
SPEED_THRESHOLD = 2.0
steering = abs(params['steering_angle']) # We don't care whether it is left or right steering
# Calculate 3 markers that are at varying distances away from the center line
marker_1 = 0.1 * track_width
# marker_2 = 0.5 * track_width
# marker_3 = 0.5 * track_width
# Give higher reward if the car is closer to center line and vice versa
if distance_from_center <= marker_1:
reward = 2.0
# elif distance_from_center <= marker_2:
# reward = 0.5
# elif distance_from_center <= marker_3:
# reward = 0.1
else:
reward = 1e-3 # likely crashed/ close to off track
if not all_wheels_on_track:
# Penalize if the car goes off track
reward = 1e-3
elif speed < SPEED_THRESHOLD:
# Penalize if the car goes too slow
reward = 0.5
else:
# High reward if the car stays on track and goes fast
reward = 1.0
# Initialize the reward with typical value
reward = 1.0
# Calculate the direction of the center line based on the closest waypoints
next_point = waypoints[closest_waypoints[1]]
prev_point = waypoints[closest_waypoints[0]]
# Calculate the direction in radius, arctan2(dy, dx), the result is (-pi, pi) in radians
track_direction = math.atan2(next_point[1] - prev_point[1], next_point[0] - prev_point[0])
# Convert to degree
track_direction = math.degrees(track_direction)
# Calculate the difference between the track direction and the heading direction of the car
direction_diff = abs(track_direction - heading)
if direction_diff > 180:
direction_diff = 360 - direction_diff
# Penalize the reward if the difference is too large
DIRECTION_THRESHOLD = 10.0
if direction_diff > DIRECTION_THRESHOLD:
reward *= 0.5
# Initialize the reward with typical value
reward = 1.0
# Penalize if car steer too much to prevent zigzag
STEERING_THRESHOLD = 20.0
if steering > STEERING_THRESHOLD:
reward *= 0.8
return float(reward)