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)
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)
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)
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)
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)
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)
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)
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]
origin_states = index_to_states[index]
r = connect_track(origin_states, upper_states)
#index_to_states[index] = r[0]
#index_to_states[index-width] = r[1]
### if square above is not in track
else:
### it colided with wall, put speed zero
a = action(1,[1], [index_to_states[index][0]])
############################################################################################## missing shit
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]
origin_states = index_to_states[index]
r = connect_track(origin_states, lower_states)
#index_to_states[index] = r[0]
#index_to_states[index-width] = r[1]
### if square above is not in track
else:
### it colided with wall, put speed zero
a = action(1,[1], [index_to_states[index][0]])