deepracer_crashlord.py

import math

def _distance(x1, y1, x2, y2):
    adj = x1 - x2
    opp = y1 - y2
    
    return math.sqrt(math.pow(adj,2) + math.pow(opp,2))

def _angle(adj, opp):
    #print(f"adj: {adj} ,opp: {opp}")
    
    return math.degrees(
        math.atan(opp / adj)
    )

def _closest_waypoint(x,y, waypoints, closest_waypoints):
    waypoint_1 = waypoints[closest_waypoints[0]]
    waypoint_2 = waypoints[closest_waypoints[1]]
    
    waypoint_1_distance = _distance(x, y, waypoint_1[0], waypoint_1[1])
    waypoint_2_distance = _distance(x, y, waypoint_2[0], waypoint_2[1])

    if waypoint_1_distance < waypoint_2_distance:
        return waypoint_1
    else:
        return waypoint_2

def _heading(params):
    closest = _closest_waypoint(
        params['x'],
        params['y'], 
        params['waypoints'], 
        params['closest_waypoints']
    )
    
    x = params['x']
    y = params['y']
    
#     print(f"racer x: {x}, y: {y}")
#     print(f"closest: {closest}")
    
    closest_x = closest[0]
    closest_y = closest[1]
    
    adj = x - closest_x
    opp = y - closest_y
    
    racer_y = y < closest_y
    racer_x = x < closest_x
    
#     print(f"racer_x: {racer_x}, racer_y: {racer_y}")
    
    if(racer_x and racer_y):
        correction = 0
    elif(racer_y and not racer_x):
        correction = 180
    elif(not racer_y and not racer_x):
        correction = -180
    else:
        correction = 0
        
#     print(f"correction: {correction}")
    
    angle = None
    
    if(adj == 0 and racer_y):
        angle = -90
    elif(adj == 0 and not racer_y):
        angle = 90    
    elif(opp == 0 and racer_x):
        angle = 0 
    elif(opp == 0 and not racer_x):
        angle = 180
    else:  
        uncorrected_closest_angle = _angle(adj, opp)
        corrected_closest_angle = uncorrected_closest_angle + correction

        angle = corrected_closest_angle
    
    return angle
    
def _no_zig_zag(params):
    '''
    Example of penalize steering, which helps mitigate zig-zag behaviors
    '''
    
    # Read input parameters
    distance_from_center = params['distance_from_center']
    track_width = params['track_width']
    steering = abs(params['steering_angle']) # Only need the absolute steering angle

    # Calculate 3 markers that are at varying distances away from the center line
    marker_1 = 0.1 * track_width
    marker_2 = 0.25 * track_width
    marker_3 = 0.5 * track_width

    # Give higher reward if the agent is closer to center line and vice versa
    if distance_from_center <= marker_1:
        reward = 1
    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

    # Steering penality threshold, change the number based on your action space setting
    ABS_STEERING_THRESHOLD = 15

    # Penalize reward if the agent is steering too much
    if steering > ABS_STEERING_THRESHOLD:
        reward *= 0.8

    return float(reward)
def _center_line(params):    
    # Read input parameters
    track_width = params['track_width']
    distance_from_center = params['distance_from_center']
    
    # Calculate 3 markers that are at varying distances away from the center line
    marker_1 = 0.1 * track_width
    marker_2 = 0.25 * 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 = 1.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
    
    return float(reward)


def _stay_on_track(params):
    # Read input parameters
    all_wheels_on_track = params['all_wheels_on_track']
    distance_from_center = params['distance_from_center']
    track_width = params['track_width']
    
    # Give a very low reward by default
    reward = 1e-3

    # Give a high reward if no wheels go off the track and
    # the agent is somewhere in between the track borders
    if all_wheels_on_track and (0.5*track_width - distance_from_center) >= 0.05:
        reward = 1.0
    
    return float(reward)

def _good_heading(params):
    closest_waypoint_heading = _heading(params)
    heading_difference = params['heading'] - closest_waypoint_heading
    
    if(heading_difference < 15 or heading_difference > -15):
        return True
    else:
        return False

def reward_function(params):
    
    center_line_reward = _center_line(params)
    good_heading = _good_heading(params)
    
    reward_factor = 1
    if(params["speed"] > 0.6 and good_heading):
        reward_factor = 1.8
    
    r1 = _no_zig_zag(params)
    r2 = _stay_on_track(params)
    r3 = _center_line(params)

    avg_reward = (r1 + r2 + r3) / 3
    
    reward = avg_reward * reward_factor
        
    return float(reward)