def extract_track(filename, manhattan_heuristic): f = open(filename) line = f.readline() track = [] while line != "": line = f.readline() split_line = [] for x in line: if x != "\n": split_line.append(x) if len(split_line) != 0: track.append(split_line) width = len(track[0]) length = len(track) index_to_states = dict([]) states_to_indexes = dict([]) goal_indexes = [] start_state = None for i in range(width * length): l_index = i / width w_index = i - l_index * width if track[l_index][w_index] != 'x': ### zero speed s0 = state() ### up direction s1 = state() s2 = state() ### down direction s3 = state() s4 = state() ### right direction s5 = state() s6 = state() ### left direction s7 = state() s8 = state() index_to_states[i] = [s0, s1, s2, s3, s4, s5, s6, s7, s8] states_to_indexes[s0] = i states_to_indexes[s1] = i states_to_indexes[s2] = i states_to_indexes[s3] = i states_to_indexes[s4] = i states_to_indexes[s5] = i states_to_indexes[s6] = i states_to_indexes[s7] = i states_to_indexes[s8] = i if track[l_index][w_index] == "s": start_state = s0 if track[l_index][w_index] == "g": s0.set_goal() s1.set_goal() s2.set_goal() s3.set_goal() s4.set_goal() s5.set_goal() s6.set_goal() s7.set_goal() s8.set_goal() goal_indexes.append(i) ### target_states are 1 step from origin_states def connect_track(origin_states, target_states): [s0, s1, s2] = origin_states [t0, t1, t2] = target_states ### first s0: possible actions, stay or accelerate a1 = action(1, [1], [s0]) ### stay a2 = action(1, [1], [t1]) ### accelerate s0.append_action(a1) s0.append_action(a2) ### second s1: possible actions, descelerate, stay, or accelerate a1 = action(1, [1], [t0]) ### descelerate a2 = action(1, [1], [t1]) ### stay a3 = action(1, [1], [t2]) ### accelerate s1.append_action(a1) s1.append_action(a2) s1.append_action(a2) ### third s2: possible actions, descelerate, stay a1 = action(1, [1], [t1]) ### descelerate s2.append_action(a1) origin_states = [s0, s1, s2] target_states = [t0, t1, t2] return (origin_states, target_states) ### target_states are 2 steps from origin_states def connect_track_double(origin_states, target_states): [s0, s1, s2] = origin_states [t0, t1, t2] = target_states ### first s0: possible actions, stay or accelerate ### there are no possible actions ### second s1: possible actions, descelerate, stay, or accelerate ### there are no possible actions ### third s2: possible actions, descelerate, stay a1 = action(1, [1], [t1]) ### descelerate a2 = action(1, [1], [t2]) ### stay s2.append_action(a1) s2.append_action(a2) origin_states = [s0, s1, s2] target_states = [t0, t1, t2] return (origin_states, target_states) ### origin_states with themselves def connect_track_break(origin_states): [s0, s1, s2] = origin_states ### all go si -> s0 ### first s0 a1 = action(.1, [1], [s0]) ### stay s0.append_action(a1) ### second s1 a1 = action(.1, [1], [s0]) ### descelerate s1.append_action(a1) ### third s2: possible actions, descelerate, stay a1 = action(.1, [1], [s0]) ### descelerate s2.append_action(a1) origin_states = [s0, s1, s2] return (origin_states, None) for index in index_to_states: if index - width >= 0: ### if square above is in track if index - width in index_to_states: upper_states = index_to_states[index - width] upper_states = upper_states[0:1] + upper_states[1:3] origin_states = index_to_states[index] origin_states = origin_states[0:1] + origin_states[1:3] r = connect_track(origin_states, upper_states) ### if square above is not in track else: ### it colided with wall, put speed zero origin_states = index_to_states[index] origin_states = origin_states[0:1] + origin_states[1:3] r = connect_track_break(origin_states) ### if 2 squares above is in track if index - 2 * width >= 0 and index - 2 * width in index_to_states: upper_states = index_to_states[index - width] upper_states = upper_states[0:1] + upper_states[1:3] origin_states = index_to_states[index] origin_states = origin_states[0:1] + origin_states[1:3] r = connect_track_double(origin_states, upper_states) if index + width < width * length: ### if square below is in track if index + width in index_to_states: lower_states = index_to_states[index + width] lower_states = lower_states[0:1] + lower_states[3:5] origin_states = index_to_states[index] origin_states = origin_states[0:1] + origin_states[3:5] r = connect_track(origin_states, lower_states) ### if square above is not in track else: ### it colided with wall, put speed zero origin_states = index_to_states[index] origin_states = origin_states[0:1] + origin_states[3:5] r = connect_track_break(origin_states) ### if 2 squares below is in track if index + 2 * width < width * length and index + 2 * width in index_to_states: lower_states = index_to_states[index + 2 * width] lower_states = lower_states[0:1] + lower_states[3:5] origin_states = index_to_states[index] origin_states = origin_states[0:1] + origin_states[3:5] r = connect_track_double(origin_states, lower_states) if (index + 1) % width != 0: ### if square to right is in track if index + 1 in index_to_states: right_states = index_to_states[index + 1] right_states = right_states[0:1] + right_states[5:7] origin_states = index_to_states[index] origin_states = origin_states[0:1] + origin_states[5:7] r = connect_track(origin_states, right_states) ### if square above is not in track else: ### it colided with wall, put speed zero origin_states = index_to_states[index] origin_states = origin_states[0:1] + origin_states[5:7] r = connect_track_break(origin_states) ### if 2 squares to right is in track if (index + 2) % width != 0 and index + 2 in index_to_states: right_states = index_to_states[index + 2] right_states = right_states[0:1] + right_states[5:7] origin_states = index_to_states[index] origin_states = origin_states[0:1] + origin_states[5:7] r = connect_track_double(origin_states, right_states) if (index) % width != 0: ### if square to left is in track if index - 1 in index_to_states: left_states = index_to_states[index - 1] left_states = left_states[0:1] + left_states[7:9] origin_states = index_to_states[index] origin_states = origin_states[0:1] + origin_states[7:9] r = connect_track(origin_states, left_states) ### if square above is not in track else: ### it colided with wall, put speed zero origin_states = index_to_states[index] origin_states = origin_states[0:1] + origin_states[7:9] r = connect_track_break(origin_states) ### if 2 squares to left is in track if (index - 1) % width != 0 and index - 2 in index_to_states: left_states = index_to_states[index - 2] left_states = left_states[0:1] + left_states[7:9] origin_states = index_to_states[index] origin_states = origin_states[0:1] + origin_states[7:9] r = connect_track(origin_states, left_states) ### object that converts the states of the track and their relations ### into a string to output in the GUI class trackDisplay(): def __init__(self, Width, Length, Index_to_states): self.print_board = [["x" for i in range(Width)] for j in range(Length)] self.index_to_states = Index_to_states def update(self, newstate): for index in self.index_to_states: char = ' ' if newstate in self.index_to_states[index]: char = '0' self.print_board[(index / width)][index - index / width * width] = char def getdisplay(self): toPrint = "" for row in self.print_board: line = "" for char in row: line += char toPrint += line + "\n" return toPrint def printdisplay(self): print self.getdisplay() t = trackDisplay(width, length, index_to_states) t.update(start_state) states = [] for states_pack in index_to_states.values(): states.append(states_pack[0]) states.append(states_pack[1]) states.append(states_pack[2]) states.append(states_pack[3]) states.append(states_pack[4]) states.append(states_pack[5]) states.append(states_pack[6]) states.append(states_pack[7]) states.append(states_pack[8]) def get_distance_goal(index, goal_i, Width): l_index = index / Width w_index = index - l_index * Width result = float("inf") for goal_index in goal_i: gl_index = goal_index / Width gw_index = goal_index - gl_index * Width distance = math.fabs(gl_index - l_index) + math.fabs(gw_index - w_index) if distance < result: result = distance return result ### set up the manhattan heuristic if manhattan_heuristic: for index in index_to_states: states_pack = index_to_states[index] distance = get_distance_goal(index, goal_indexes, width) states_pack[0].h = distance states_pack[1].h = distance states_pack[2].h = distance states_pack[3].h = distance states_pack[4].h = distance states_pack[5].h = distance states_pack[6].h = distance states_pack[7].h = distance states_pack[8].h = distance return (states, start_state, t)
def extract_track(filename, manhattan_heuristic): f = open(filename) line = f.readline() track = [] while line != "": line = f.readline() split_line = [] for x in line: if x != "\n": split_line.append(x) if len(split_line) != 0: track.append(split_line) width = len(track[0]) length = len(track) index_to_states = dict([]) states_to_indexes = dict([]) goal_indexes = [] start_state = None for i in range(width*length): l_index = i/width w_index = i - l_index*width if track[l_index][w_index] != 'x': ### zero speed s0 = state() ### up direction s1 = state() s2 = state() ### down direction s3 = state() s4 = state() ### right direction s5 = state() s6 = state() ### left direction s7 = state() s8 = state() index_to_states[i] = [s0, s1, s2, s3, s4, s5, s6, s7, s8] states_to_indexes[s0] = i states_to_indexes[s1] = i states_to_indexes[s2] = i states_to_indexes[s3] = i states_to_indexes[s4] = i states_to_indexes[s5] = i states_to_indexes[s6] = i states_to_indexes[s7] = i states_to_indexes[s8] = i if track[l_index][w_index] == "s": start_state = s0 if track[l_index][w_index] == "g": s0.set_goal() s1.set_goal() s2.set_goal() s3.set_goal() s4.set_goal() s5.set_goal() s6.set_goal() s7.set_goal() s8.set_goal() goal_indexes.append(i) ### target_states are 1 step from origin_states def connect_track(origin_states, target_states): [s0, s1, s2] = origin_states [t0, t1, t2] = target_states ### first s0: possible actions, stay or accelerate a1 = action(1, [1], [s0]) ### stay a2 = action(1, [1], [t1]) ### accelerate s0.append_action(a1) s0.append_action(a2) ### second s1: possible actions, descelerate, stay, or accelerate a1 = action(1, [1], [t0]) ### descelerate a2 = action(1, [1], [t1]) ### stay a3 = action(1, [1], [t2]) ### accelerate s1.append_action(a1) s1.append_action(a2) s1.append_action(a2) ### third s2: possible actions, descelerate, stay a1 = action(1, [1], [t1]) ### descelerate s2.append_action(a1) origin_states = [s0, s1, s2] target_states = [t0, t1, t2] return (origin_states, target_states) ### target_states are 2 steps from origin_states def connect_track_double(origin_states, target_states): [s0, s1, s2] = origin_states [t0, t1, t2] = target_states ### first s0: possible actions, stay or accelerate ### there are no possible actions ### second s1: possible actions, descelerate, stay, or accelerate ### there are no possible actions ### third s2: possible actions, descelerate, stay a1 = action(1, [1], [t1]) ### descelerate a2 = action(1, [1], [t2]) ### stay s2.append_action(a1) s2.append_action(a2) origin_states = [s0, s1, s2] target_states = [t0, t1, t2] return (origin_states, target_states) ### origin_states with themselves def connect_track_break(origin_states): [s0, s1, s2] = origin_states ### all go si -> s0 ### first s0 a1 = action(.1, [1], [s0]) ### stay s0.append_action(a1) ### second s1 a1 = action(.1, [1], [s0]) ### descelerate s1.append_action(a1) ### third s2: possible actions, descelerate, stay a1 = action(.1, [1], [s0]) ### descelerate s2.append_action(a1) origin_states = [s0, s1, s2] return (origin_states, None) for index in index_to_states: if index - width >= 0: ### if square above is in track if index - width in index_to_states: upper_states = index_to_states[index-width] upper_states = upper_states[0:1] + upper_states[1:3] origin_states = index_to_states[index] origin_states = origin_states[0:1] + origin_states[1:3] r = connect_track(origin_states, upper_states) ### if square above is not in track else: ### it colided with wall, put speed zero origin_states = index_to_states[index] origin_states = origin_states[0:1] + origin_states[1:3] r = connect_track_break(origin_states) ### if 2 squares above is in track if index - 2*width >= 0 and index - 2*width in index_to_states: upper_states = index_to_states[index-width] upper_states = upper_states[0:1] + upper_states[1:3] origin_states = index_to_states[index] origin_states = origin_states[0:1] + origin_states[1:3] r = connect_track_double(origin_states, upper_states) if index + width < width*length: ### if square below is in track if index+width in index_to_states: lower_states = index_to_states[index+width] lower_states = lower_states[0:1] + lower_states[3:5] origin_states = index_to_states[index] origin_states = origin_states[0:1] + origin_states[3:5] r = connect_track(origin_states, lower_states) ### if square above is not in track else: ### it colided with wall, put speed zero origin_states = index_to_states[index] origin_states = origin_states[0:1] + origin_states[3:5] r = connect_track_break(origin_states) ### if 2 squares below is in track if index + 2*width < width*length and index+2*width in index_to_states: lower_states = index_to_states[index+2*width] lower_states = lower_states[0:1] + lower_states[3:5] origin_states = index_to_states[index] origin_states = origin_states[0:1] + origin_states[3:5] r = connect_track_double(origin_states, lower_states) if (index + 1) % width != 0: ### if square to right is in track if index + 1 in index_to_states: right_states = index_to_states[index+1] right_states = right_states[0:1] + right_states[5:7] origin_states = index_to_states[index] origin_states = origin_states[0:1] + origin_states[5:7] r = connect_track(origin_states, right_states) ### if square above is not in track else: ### it colided with wall, put speed zero origin_states = index_to_states[index] origin_states = origin_states[0:1] + origin_states[5:7] r = connect_track_break(origin_states) ### if 2 squares to right is in track if (index + 2) % width != 0 and index + 2 in index_to_states: right_states = index_to_states[index+2] right_states = right_states[0:1] + right_states[5:7] origin_states = index_to_states[index] origin_states = origin_states[0:1] + origin_states[5:7] r = connect_track_double(origin_states, right_states) if (index) % width != 0: ### if square to left is in track if index - 1 in index_to_states: left_states = index_to_states[index-1] left_states = left_states[0:1] + left_states[7:9] origin_states = index_to_states[index] origin_states = origin_states[0:1] + origin_states[7:9] r = connect_track(origin_states, left_states) ### if square above is not in track else: ### it colided with wall, put speed zero origin_states = index_to_states[index] origin_states = origin_states[0:1] + origin_states[7:9] r = connect_track_break(origin_states) ### if 2 squares to left is in track if (index-1) % width != 0 and index - 2 in index_to_states: left_states = index_to_states[index-2] left_states = left_states[0:1] + left_states[7:9] origin_states = index_to_states[index] origin_states = origin_states[0:1] + origin_states[7:9] r = connect_track(origin_states, left_states) ### object that converts the states of the track and their relations ### into a string to output in the GUI class trackDisplay(): def __init__(self, Width, Length, Index_to_states): self.print_board = [["x" for i in range(Width)] for j in range(Length)] self.index_to_states = Index_to_states def update(self, newstate): for index in self.index_to_states: char = ' ' if newstate in self.index_to_states[index]: char = '0' self.print_board[(index/width)][index-index/width*width] = char def getdisplay(self): toPrint = "" for row in self.print_board: line = "" for char in row: line += char toPrint+= line + "\n" return toPrint def printdisplay(self): print self.getdisplay() t = trackDisplay(width, length, index_to_states) t.update(start_state) states = [] for states_pack in index_to_states.values(): states.append(states_pack[0]) states.append(states_pack[1]) states.append(states_pack[2]) states.append(states_pack[3]) states.append(states_pack[4]) states.append(states_pack[5]) states.append(states_pack[6]) states.append(states_pack[7]) states.append(states_pack[8]) def get_distance_goal(index, goal_i, Width): l_index = index/Width w_index = index - l_index*Width result = float("inf") for goal_index in goal_i: gl_index = goal_index/Width gw_index = goal_index - gl_index*Width distance = math.fabs(gl_index-l_index) + math.fabs(gw_index-w_index) if distance < result: result = distance return result ### set up the manhattan heuristic if manhattan_heuristic: for index in index_to_states: states_pack = index_to_states[index] distance = get_distance_goal(index, goal_indexes, width) states_pack[0].h = distance states_pack[1].h = distance states_pack[2].h = distance states_pack[3].h = distance states_pack[4].h = distance states_pack[5].h = distance states_pack[6].h = distance states_pack[7].h = distance states_pack[8].h = distance return (states, start_state, t)
def extract_track(filename, manhattan_heuristic): f = open(filename) line = f.readline() track = [] while line != "": line = f.readline() split_line = [] for x in line: if x != "\n": split_line.append(x) if len(split_line) != 0: track.append(split_line) width = len(track[0]) length = len(track) index_to_states = dict([]) states_to_indexes = dict([]) goal_indexes = [] start_state = None class Grid: def __init__(self): self.grid_to_states = dict([]) self.states_to_grid = dict([]) def add_state(self, position_velocity, st): self.grid_to_states[position_velocity] = st self.states_to_grid[st] = position_velocity ### direction is a vector def get_state(self, position, velocity): ### returns the state in position (x_p, y_p) with velocity ### returns none if no such state position_velocity = tuple(list(position) + list(velocity)) if position_velocity in self.grid_to_states: return self.grid_to_states[position_velocity] else: return None ### returns the parameters of the given state ### i.e. position (x_p, y_p) and velocity def get_state_parameters(self, st): return self.states_to_grid[st] def get_next_position(self, initial_position, velocity): new_x = initial_position[0] + velocity[0] new_y = initial_position[1] + velocity[1] return (new_x, new_y) ### assumes a well formed state was given? def get_next_state(self, st, accel): max_speed = 2 grid_vals = self.get_state_parameters(st) position = (grid_vals[0], grid_vals[1]) velocity = (grid_vals[2], grid_vals[3]) x_vel_new = min(max(-max_speed,velocity[0]+accel[0]),max_speed) y_vel_new = min(max(-max_speed,velocity[1]+accel[1]),max_speed) new_velocity = (x_vel_new , y_vel_new) new_position = self.get_next_position(position, velocity) successor = self.get_state(new_position, new_velocity) return successor ### gets the last free position in the direction ### the state is looking at. This means, the place of colision. ### returns position def get_last_free(self, st): grid_vals = self.get_state_parameters(st) position = (grid_vals[0], grid_vals[1]) velocity = (grid_vals[2], grid_vals[3]) ### needs to be velocity normalized next_position = self.get_next_position(position, velocity) old_position = position ### while the next position is part of the grid while next_position + (0,0) in self.grid_to_states: old_position = next_position next_position = self.get_next_position(next_position, velocity) return old_position grid = Grid() for i in range(width*length): l_index = i/width w_index = i - l_index*width if track[l_index][w_index] != 'x': position_states = [] for j in range(-2, 3): ### change this for parameters given, instead of hard coded for k in range(-2, 3): s = state() position_states.append(s) position_velocity = (w_index, l_index, j, k) grid.add_state(position_velocity, s) index_to_states[i] = position_states for st in position_states: states_to_indexes[st] = i if track[l_index][w_index] == "s": start_state = position_states[0] if track[l_index][w_index] == "g": for st in position_states: st.set_goal() goal_indexes.append(i) ### CONNECT THE GRAPH for x in range(width): for y in range(length): for dx in range(-2,3): for dy in range(-2,3): position = (x,y) velocity = (dx, dy) st = grid.get_state(position, velocity) ### the state isn't a border or similar if st != None: for a1 in range(-1, 2): for a2 in range(-1, 2): accel = (a1, a2) successor = grid.get_next_state(st, accel) ### the successor is well formed if successor != None: a = action(1, [1], [successor]) ### connect the states now st.append_action(a) ### the next state is a border else: coll_pos = grid.get_last_free(st) coll_state = grid.get_state(coll_pos, (0,0)) ### connect the states now a = action(1, [1], [coll_state]) st.append_action(a) ### object that converts the states of the track and their relations ### into a string to output in the GUI class trackDisplay(): def __init__(self, Width, Length, Index_to_states): self.print_board = [["x" for i in range(Width)] for j in range(Length)] self.index_to_states = Index_to_states def update(self, newstate): for index in self.index_to_states: char = ' ' if newstate in self.index_to_states[index]: char = '0' self.print_board[(index/width)][index-index/width*width] = char def getdisplay(self): toPrint = "" for row in self.print_board: line = "" for char in row: line += char toPrint+= line + "\n" return toPrint def printdisplay(self): print self.getdisplay() t = trackDisplay(width, length, index_to_states) t.update(start_state) states = [] for states_pack in index_to_states.values(): for st in states_pack: states.append(st) def get_distance_goal(index, goal_i, Width): l_index = index/Width w_index = index - l_index*Width result = float("inf") for goal_index in goal_i: gl_index = goal_index/Width gw_index = goal_index - gl_index*Width distance = math.fabs(gl_index-l_index) + math.fabs(gw_index-w_index) if distance < result: result = distance return result ### set up the manhattan heuristic if manhattan_heuristic: for index in index_to_states: states_pack = index_to_states[index] distance = get_distance_goal(index, goal_indexes, width) for st in states_pack: st.h = distance else: for index in index_to_states: states_pack = index_to_states[index] for st in states_pack: st.h = 0 return (states, start_state, t)
width = len(track[0]) length = len(track) index_to_states = dict([]) states_to_indexes = dict([]) start_state = None for i in range(width*length): l_index = i/width w_index = i - l_index*width if track[l_index][w_index] != 'x': s0 = state() s1 = state() s2 = state() index_to_states[i] = [s0, s1, s2] states_to_indexes[s0] = i states_to_indexes[s1] = i states_to_indexes[s2] = i if track[l_index][w_index] == "s": start_state = s0 if track[l_index][w_index] == "g": s0.set_goal()