def prob(p):
p = float(p)
n = 1.0 - p
return {
'S':(('S', p), ('N', n)),
'N':(('N', p), ('S', n)),
'E':(('E', p), ('W', n)),
'W':(('W', p), ('E', n)),
}
STATES = ((0,3),(0,2),(1,2),(0,1),(1,1),(0,0))
if __name__ == '__main__':
print "\nInitial values:"
g = GridWorld(GRID, prob(1), STATES, 1, -4)
print g
i = g.value_iteration(0.1)
print "\nValues after %d iterations:" % i
print g
g = GridWorld(GRID, prob(0.8), STATES, 1, -4)
print "\nValue of (1,2) after first iteration: %.1f" % g.value(0,3)
g = GridWorld(GRID, prob(0.8), STATES, 1, -4)
i = g.value_iteration(0.1)
print "\nValues after %d iterations:" % i
print g
from grid import GridWorld
# from policy_evaluation import evaluate_policy
# from policy_improvement import improve_policy
from value_iteration import iterate_values
from utils import print_value_function_estimates
if __name__ == "__main__":
GRID = GridWorld(4, 4)
THETA = 10e-6
DISCOUNT = 1.0
# initialize V(s)
VALUE_FUNCTION_ESTIMATES = {state: 0 for state in GRID.all_spaces}
# Initialize policy
POLICY = {}
for state in GRID.non_terminal_spaces:
POLICY[state] = list(GRID.action_space.keys())
# Initial policy evaluation
# VALUE_FUNCTION_ESTIMATES = evaluate_policy(
# GRID, VALUE_FUNCTION_ESTIMATES, POLICY, DISCOUNT, THETA
# )
# print_value_function_estimates(VALUE_FUNCTION_ESTIMATES, GRID)
# Iterative policy evaluation
# stable = False
# while not stable:
# VALUE_FUNCTION_ESTIMATES = evaluate_policy(
# GRID, VALUE_FUNCTION_ESTIMATES, POLICY, DISCOUNT, THETA
# )
Y_DIM = 12
VIEWING_RANGE = 3
# Set the HEIGHT and WIDTH of the screen
# Set title of screen
pygame.display.set_caption("D* Lite Path Planning")
# Loop until the user clicks the close button.
done = False
# Used to manage how fast the screen updates
clock = pygame.time.Clock()
if __name__ == "__main__":
graph = GridWorld(X_DIM, Y_DIM)
s_start = 'x1y2'
s_goal = 'x5y4'
graph.goal = s_goal
goal_coords = stateNameToCoords(s_goal)
graph.goal_coords = goal_coords
graph.setStart(s_start)
graph.setGoal(s_goal)
k_m = 0
s_last = s_start
queue = []
s_current = s_start
pos_coords = stateNameToCoords(s_current)
graph.pos_coords = pos_coords
graph, queue, k_m = initDStarLite(graph, queue, s_start, s_goal, k_m)
from grid import GridWorld
from policyEvaluation import evaluatePolicy
from policyImprovement import improvePolicy
from valueIteration import iterateValues
from utils import printV
if __name__ == '__main__':
grid = GridWorld(4,4)
THETA = 10e-6
GAMMA = 1.0
# initialize V(s)
V = {}
for state in grid.stateSpacePlus:
V[state] = 0
# Initialize policy
policy = {}
for state in grid.stateSpace:
policy[state] = [key for key in grid.actionSpace.keys()]
# Initial policy evaluation
V = evaluatePolicy(grid, V, policy, GAMMA, THETA)
printV(V, grid)
# Iterative policy evaluation
stable = False
while not stable:
V = evaluatePolicy(grid, V, policy, GAMMA, THETA)
printV(V, grid)
stable, policy = improvePolicy(grid, V, policy, GAMMA)
def main():
# Loop until the user clicks the close button.
done = False
graph = GridWorld(dimension_x, dimension_y)
s_start = input("Enter start (xnym, where n,m are coordinates) : ")
s_goal = input("Enter goal (xnym, where n,m are coordinates) : ")
# s_start = 'x1y2'
# s_goal = 'x9y7'
goal_coords = stateNameToCoords(s_goal)
start_coords = stateNameToCoords(s_start)
graph.setStart(s_start)
graph.setGoal(s_goal)
k_m = 0
s_last = s_start
queue = []
graph, queue, k_m = initDStarLite(graph, queue, s_start, s_goal, k_m)
s_current = s_start
pos_coords = stateNameToCoords(s_current)
robot_centers = []
# Initialize pygame
pygame.init()
screen = pygame.display.set_mode(WINDOW_SIZE)
# Set title of screen
pygame.display.set_caption("D* Lite Path Planning")
# Used to manage how fast the screen updates
clock = pygame.time.Clock()
basicfont = pygame.font.SysFont('Comic Sans MS', 22)
# -------- Main Program Loop -----------
while done == False:
for event in pygame.event.get(): # User did something
if event.type == pygame.QUIT: # If user clicked close
done = True # Flag that we are done so we exit this loop
elif event.type == pygame.KEYDOWN and event.key == pygame.K_SPACE:
print('move')
s_new, k_m = moveAndRescan(graph, queue, s_current, k_m)
if s_new == 'goal':
print('Goal Reached!')
done = True
else:
# print('setting s_current to ', s_new)
s_current = s_new
pos_coords = stateNameToCoords(s_current)
# print('got pos coords: ', pos_coords)
elif event.type == pygame.MOUSEBUTTONDOWN:
# User clicks the mouse. Get the position
pos = pygame.mouse.get_pos()
# Change the x/y screen coordinates to grid coordinates
column = pos[0] // (width + margin)
row = pos[1] // (height + margin)
# Set that location to one
if (graph.cells[row][column] == 0):
graph.cells[row][column] = -1
# Set the screen background
screen.fill(BLACK)
# Draw the grid
for row in range(dimension_y):
for column in range(dimension_x):
pygame.draw.rect(
screen, colors[graph.cells[row][column]],
[(margin + width) * column + margin,
(margin + height) * row + margin, width, height])
node_name = 'x' + str(column) + 'y' + str(row)
# fill in goal cell with GREEN
pygame.draw.rect(
screen, GREEN,
[(margin + width) * start_coords[0] + margin,
(margin + height) * start_coords[1] + margin, width, height])
# fill in goal cell with RED
pygame.draw.rect(
screen, RED,
[(margin + width) * goal_coords[0] + margin,
(margin + height) * goal_coords[1] + margin, width, height])
# draw moving robot, based on pos_coords
# Set the new robot centre
robot_center = [
int(pos_coords[0] * (width + margin) + width / 2) + margin,
int(pos_coords[1] * (height + margin) + height / 2) + margin
]
draw_arrow(screen, BLACK, robot_center, robot_center)
# maintain a list of all cells traversed
robot_centers.append(robot_center)
if len(robot_centers) > 1:
for i in range(0, len(robot_centers) - 1):
pygame.draw.line(screen, BLACK, robot_centers[i],
robot_centers[i + 1], 3)
# grey out visible boxes for robot
screen.blit(
transparent,
(robot_center[0] - 1.5 * width - margin, robot_center[1] -
1.5 * height - margin)) # (0,0) are the top-left coordinates
# Limit to 60 frames per second
clock.tick(20)
# Go ahead and update the screen with what we've drawn.
pygame.display.flip()
# Be IDLE friendly. If you forget this line, the program will 'hang'
# on exit.
pygame.quit()
from grid import GridWorld
GRID = [[0, 0, 0, 100],
[0, None, 0, -100],
[0, 0, 0, 0]]
PROB = {
'S':(('S', 0.8), ('W', 0.1), ('E', 0.1)),
'N':(('N', 0.8), ('E', 0.1), ('W', 0.1)),
'E':(('E', 0.8), ('S', 0.1), ('N', 0.1)),
'W':(('W', 0.8), ('N', 0.1), ('S', 0.1)),
}
STATES = ((0,2),(0,1),(1,2),(0,0),(2,2),(1,0),(2,1),(2,0),(2,3))
if __name__ == '__main__':
g = GridWorld(GRID, PROB, STATES, 1, -3)
v = g.value(0,2)
print "\nValue of (0,2) after first iteration: %.1f" % v
g.grid[0][2] = v
print "\nValue of (1,2) after first iteration: %.1f" % g.value(1,2)
print "\nInitial values:"
g = GridWorld(GRID, PROB, STATES, 1, -3)
print g
i = g.value_iteration(0.1)
print "\nValues after %d iterations:" % i
print g
if (len(lines[0])) == 0:
del lines[0]
lines = [x.rstrip() for x in lines]
for j, _ in enumerate(lines):
for i, _ in enumerate(lines[0]):
self.world.get_cell(i, j).load(lines[j][i])
def reset_pos(self):
self.agent.x = self.get_init_pos()[0]
self.agent.y = self.get_init_pos()[1]
self.agent.dir = self.get_init_pos()[2]
self.agent.cell = self.world.get_cell(self.agent.x, self.agent.y)
world_cfg = WorldConfig()
world = GridWorld(Cell, map=world_cfg.get_map(), directions=4)
agent = ContinuousAgent()
world_cfg.world = world
world_cfg.agent = agent
world_cfg.set_ind(0)
world_cfg.world.add(
agent,
x=world_cfg.get_init_pos()[0],
y=world_cfg.get_init_pos()[1],
dir=world_cfg.get_init_pos()[2],
)
# ----------- LEARNING & MODEL PARAMETERS -------------------------------------
learn_rate = 1e-4
learn_synapse = 0.030
learn_timeout = 60.0
# Set the HEIGHT and WIDTH of the screen
WINDOW_SIZE = [(WIDTH + MARGIN) * X_DIM + MARGIN,
(HEIGHT + MARGIN) * Y_DIM + MARGIN]
screen = pygame.display.set_mode(WINDOW_SIZE)
# Set title of screen
pygame.display.set_caption("D* Lite in Google UAS Airspace System")
# Loop until the user clicks the close button.
done = False
# Used to manage how fast the screen updates
clock = pygame.time.Clock()
if __name__ == "__main__":
graph = GridWorld(X_DIM, Y_DIM)
s_start = 'x1y2'
s_goal = 'x5y4'
goal_coords = stateNameToCoords(s_goal)
graph.setStart(s_start)
graph.setGoal(s_goal)
k_m = 0
s_last = s_start
queue = []
graph, queue, k_m = initDStarLite(graph, queue, s_start, s_goal, k_m)
s_current = s_start
pos_coords = stateNameToCoords(s_current)
numOb = int(input("Enter number of dynamic obsticals (integer input only): "))
## world = '20,20,20'
## start = '1,1,1'
## goal = '19,19,19'
## numOb = 80
world = world.split(',')
start = start.split(',')
goal = goal.split(',')
s_start_list = [int(x) for x in start]
s_goal_list = [int(x) for x in goal]
worldSize = [int(x) for x in world]
graph = GridWorld(worldSize[0], worldSize[1], worldSize[2])
s_start = "x" + str(s_start_list[0]) + "y" + str(s_start_list[1]) + "z" + str(s_start_list[2])
s_goal = "x" + str(s_goal_list[0]) + "y" + str(s_goal_list[1]) + "z" + str(s_goal_list[2])
path = [s_start]
graph.setStart(s_start)
graph.setGoal(s_goal)
k_m = 10
s_last = s_start
queue = []
obstacles = []
new_obstacles = []
graph, queue, k_m = initDStarLite(graph, queue, s_start, s_goal, k_m)
print Proposition(
lambda a, b, c: implies(a, b) and implies(b, c) and implies(c, a),
"(a => b) and (b => c) and (c => a)"
).satisfiability_report()
print Proposition(
lambda a, b, c: implies(a, b) and (not ((not a) or b)),
"(a => b) and (not ((not a) or b))"
).satisfiability_report()
print Proposition(
lambda a, b, c: (implies(a, b) or implies(b, c)) == implies(a, c),
"((a => b) and (b => c)) == (a => c)"
).satisfiability_report()
print "\n=== Problem 14 ==="
from grid import GridWorld
GRID = [[0, 0, None, 100],
[0, 0, 0, 0]]
PROB = {
'S':(('S', 1.0), ),
'N':(('N', 1.0), ),
'E':(('E', 1.0), ),
'W':(('W', 1.0), ),
}
STATES = ((1,3),(1,2),(1,1),(0,1),(1,0),(0,0))
g = GridWorld(GRID, PROB, STATES, 1, -5)
i = g.value_iteration(0.1)
print "Values after %d iterations:" % i
print g
# Set the HEIGHT and WIDTH of the screen
#WINDOW_SIZE = [(WIDTH + MARGIN) * X_DIM + MARGIN,
# (HEIGHT + MARGIN) * Y_DIM + MARGIN]
screen = pygame.display.set_mode(WINDOW_SIZE)
# Set title of screen
pygame.display.set_caption("D* Lite Path Planning")
# Loop until the user clicks the close button.
done = False
# Used to manage how fast the screen updates
clock = pygame.time.Clock()
if __name__ == "__main__":
GRID = GridWorld(X_DIM, Y_DIM, GAP, RADIUS)
s_start = (1, 0)
s_goal = (5, 6)
goal_coords = s_goal
#goal_coords = stateNameToCoords(s_goal)
GRID.graph.setStart(s_start)
GRID.graph.setGoal(s_goal)
k_m = 0
s_last = s_start
queue = []
graph, queue, k_m = initDStarLite(GRID.graph, queue, s_start, s_goal, k_m)
s_current = s_start
pos_coords = s_current
s_goal = 'x5y4'
goal_coords = stateNameToCoords(s_goal)
#------------------------------------------------------------------------------
# Setup
#------------------------------------------------------------------------------
# Create GUI
pygame.init()
screen = pygame.display.set_mode(WINDOW_SIZE)
pygame.display.set_caption("D* Lite Path Planning")
clock = pygame.time.Clock()
basicfont = pygame.font.SysFont('Comic Sans MS', 36)
# Create grid
graph = GridWorld(X_DIM, Y_DIM, connect8=False)
graph.setStart(s_start)
graph.setGoal(s_goal)
# Initialize D* Lite
k_m = 0
queue = []
graph, queue, k_m = initDStarLite(graph, queue, s_start, s_goal, k_m)
s_current = s_start
pos_coords = stateNameToCoords(s_current)
#------------------------------------------------------------------------------
# Main loop
# Set the HEIGHT and WIDTH of the screen
# Set title of screen
pygame.display.set_caption("D* Lite Path Planning")
# Loop until the user clicks the close button.
done = False
# Used to manage how fast the screen updates
clock = pygame.time.Clock()
d_s_obj = d_star_obj()
if __name__ == "__main__":
graph = GridWorld(X_DIM, Y_DIM)
s_start = 'x3y1'
s_goal = 'x0y6'
graph.goal = s_goal
goal_coords = stateNameToCoords(s_goal)
graph.goal_coords = goal_coords
graph.setStart(s_start)
graph.setGoal(s_goal)
for i in range(len(graph.cells)):
row = graph.cells[i]
for j in range(len(row)):
graph.cells[i][j] = -2
#graph.cells[2][2]=-2
#graph.cells[3][0]=-2
