def run_all_tests(search_problem):
print(search_problem.maze)
print(bfs_search(search_problem))
print(astar_search(search_problem, null_heuristic))
print(astar_search(search_problem, search_problem.state_len_heuristic))
print(astar_search(search_problem, search_problem.opt_state_len_heuristic))
print(astar_search(search_problem, search_problem.uniq_x_y_heuristic))
def get_path_to_start(self):
# Convert x_pos,y_pos for self.pose and self.start_pos to x,y for use with A* search
x_r, y_r = self.m_s.xy_from_xy_pos(self.pose.position.x, self.pose.position.y)
x_s, y_s = self.m_s.xy_from_xy_pos(self.start_pos[0], self.start_pos[1])
# Find a path from current position to start position
self.xy_path = astar_search(self.m_s, (x_r, y_r), (x_s, y_s))
# Convert the path in to real coordinates
self.convert_current_path()
def query(self, start, goal, manual):
start_point_on_prm = self.local_planner(1, start, manual)
goal_point_on_prm = self.local_planner(1, goal, manual)
#if both start and goal can connect to the road map...
if start_point_on_prm != [] and goal_point_on_prm != []:
#connect them
self.connect_sg(start, goal, start_point_on_prm, goal_point_on_prm)
#then do astar search
solution = astar_search(self.roadmap, start, goal, self.heuristic, self.goal_test)
return solution
def query(self, initial_state, goal_state):
#-- Set up initial and goal state variables for astar search
self.start_state = initial_state
self.goal_state = goal_state
#-- Connect the Initial and Goal state to the graph.
for state in [initial_state, goal_state]:
if not self.workspace.is_collision(state):
self.vertices.append(state)
for q in self.neighborhood(state):
if q != state and self.connect(state, q):
self.edges.add(normalize((tuple(state), tuple(q))))
#-- Run astar search on the graph.
solution = astar_search(self, self.angular_distance_heuristic)
return solution
def get_next_path(self):
# Here we want to find the item in the list which is the closest to the
# current position of the robot.
closest = 999999.9
closest_index = 0
for i in range(len(self.target_list)):
d = dist((self.pose.position.x, self.pose.position.y),
(self.target_list[i][0], self.target_list[i][1]))
if d < closest:
closest = d
closest_index = i
# Get the target that is closest
pos_t = self.target_list.pop(closest_index)
# Find the x,y values for robot position and target
x_t, y_t = self.m_s.xy_from_xy_pos(pos_t[0], pos_t[1])
x_r, y_r = self.m_s.xy_from_xy_pos(self.pose.position.x, self.pose.position.y)
# Search for path
path = astar_search(self.m_s, (x_r, y_r), (x_t, y_t))
return path
from BlindrobotProblem import BlindrobotProblem
from Maze import Maze
from astar_search import astar_search
# null heuristic, useful for testing astar search without heuristic (uniform cost search).
def null_heuristic(state):
return 0
# test_maze2 = Maze("maze2.maz")
# print test_maze2
# test_mp = BlindrobotProblem(test_maze2, (2, 2))
# result = astar_search(test_mp, test_mp.setsize_heuristic)
# print result
# test_mp.animate_path(result.path)
test_maze07 = Maze("maze07.maz")
print test_maze07
test_mp = BlindrobotProblem(test_maze07, (1, 0))
result = astar_search(test_mp, test_mp.setsize_heuristic)
print result
test_mp.animate_path(result.path)
if not maze.is_floor(x, y):
x = x - path[i - 1][0]
y = y - path[i - 1][1]
p.append((x, y))
return p
#############################################################################
# I recommend ignoring everything up there. They are just helper functions to make live easier.
goal = (2, 0)
test_maze = Maze("maze4.maz")
print(test_maze)
test_mp = SensorlessProblem(test_maze, goal)
result = astar_search(test_mp, test_mp.num_heuristic)
result.path = get_path(result)
print(result)
test_all(test_maze, test_mp, result.path, goal)
#test_mp.animate_path(create_path(test_maze, (2, 0), result.path))
result = astar_search(test_mp, test_mp.spam_heuristic)
result.path = get_path(result)
print(result)
test_all(test_maze, test_mp, result.path, goal)
goal = (0, 4)
test_maze = Maze("maze3.maz")
print(test_maze)
test_mp = SensorlessProblem(test_maze, goal)
result = astar_search(test_mp, test_mp.num_heuristic)
#from uninformed_search import bfs_search
from astar_search import astar_search
# null heuristic, useful for testing astar search without heuristic (uniform cost search).
def null_heuristic(state):
return 0
# NOTE the huerstic i used can be fount in the SensorlessProblem.py
# Test on all the mazes given
test_maze3 = Maze("maze3.maz")
test_mp = SensorlessProblem(test_maze3)
result = astar_search(test_mp, null_heuristic)
print(result)
test_mp.animate_path(result.path)
print(result)
result = astar_search(test_mp, test_mp.cardinality_heuristic)
print(result)
test_mp.animate_path(result.path)
print(result)
test_maze1 = Maze("maze1.maz")
test_mp = SensorlessProblem(test_maze1)
result = astar_search(test_mp, test_mp.cardinality_heuristic)
print(result)
test_mp.animate_path(result.path)
test_maze3 = Maze("maze3.maz")
test_mpA = MazeworldProblem(test_maze3, (1, 4, 1, 3, 1, 2))
test_maze4 = Maze("maze4.maz")
test_mpB = MazeworldProblem(test_maze4, (8, 8, 8, 9, 8, 10))
test_maze2 = Maze("maze2.maz")
test_mpC = MazeworldProblem(test_maze2, (3, 0))
test_maze5 = Maze("maze5.maz")
test_mpD = MazeworldProblem(test_maze5, (4, 8, 3, 8, 2, 8))
test_maze6 = Maze("maze6.maz")
test_mpE = MazeworldProblem(test_maze6, (6, 4))
test_maze7 = Maze("maze7.maz")
test_mpF = MazeworldProblem(test_maze7, (8, 7, 1, 7))
# this should explore a lot of nodes; it's just uniform-cost search
result = astar_search(test_mpA, null_heuristic)
print(result)
test_mpA.animate_path(result.path)
# this should explore a lot of nodes; it's just uniform-cost search
result = astar_search(test_mpA, test_mpA.manhattan_heuristic)
print(result)
test_mpA.animate_path(result.path)
# this should do a bit better:
# result = astar_search(test_mpB, test_mpB.manhattan_heuristic)
# print(result)
# test_mpB.animate_path(result.path)
# Your additional tests here:
# result = astar_search(test_mpC, test_mpC.euclid_heuristic)
# You write this:
from SensorlessProblem import SensorlessProblem
from Maze import Maze
from astar_search import astar_search
# null heuristic, useful for testing astar search without heuristic (uniform cost search).
def null_heuristic(state):
return 0
# Test problems
test_maze3 = Maze("maze3.maz")
test_mp = SensorlessProblem(test_maze3, (2, 2))
# this should explore a lot of nodes; it's just uniform-cost search
#result = astar_search(test_mp, null_heuristic)
#print(result)
# this should do a bit better:
result = astar_search(test_mp, test_mp.manhattan_heuristic)
print(result)
test_mp.animate_path(result.path)
from astar_search import astar_search
# null heuristic, useful for testing astar search without heuristic (uniform cost search).
def null_heuristic(state):
return 0
# Test problems
test_maze1 = Maze("maze1.maz")
print(test_maze1)
test_mp = MazeworldProblem(test_maze1, (32, 4))
# this should explore a lot of nodes; it's just uniform-cost search
result = astar_search(test_mp, null_heuristic)
print(result)
# this should do a bit better:
result = astar_search(test_mp, test_mp.manhattan_heuristic)
print(result)
# test_mp.animate_path(result.path)
result = astar_search(test_mp, test_mp.bfs_heuristic)
print(result)
test_maze2 = Maze("maze2.maz")
print(test_maze2)
test_mp = MazeworldProblem(test_maze2, (4, 2, 5, 1, 6, 0))
# this should explore a lot of nodes; it's just uniform-cost search
if __name__ == "__main__":
# initialize obstacles
obstacles = [((-160, -160), (-160, -100), (-100, -100), (-100, -160)),
((100, -160), (100, -100), (160, -100), (160, -160)),
((100, 100), (100, 160), (160, 160), (160, 100)),
((-160, 100), (-160, 160), (-100, 160), (-100, 100))]
before = datetime.now()
# width, height, obstacles, arm number, arm lengths, resolution
test_map = prm_map(800, 800, obstacles, 4, [100, 100, 100, 100], 10)
test_prm = prm_problem(test_map, (1.5 * pi, 0, 1.5 * pi, 0),
(0, 0, 0.5 * pi, 0))
after = datetime.now()
# use astar_search to look for path between start and goal with the roadmap
result = astar_search(test_prm, test_prm.heuristics)
print("Time spent:")
print(after - before)
print(result)
# The following parts are for visualization! No need to read through!
# changing the above sections will directly affect the visuals
# so please dont modify the below part
########################################################################################
poss = []
for i in range(0, len(result.path)):
poss.append(
test_map.calc_pos((0, 0), [100, 100, 100, 100], result.path[i]))
xx = 0
# You write this:
from SensorlessProblem import SensorlessProblem
from MazeworldWithTimeFactorProblem import MazeworldProblemWithTimeFactor
from Maze import Maze
from astar_search import astar_search
# null heuristic, useful for testing astar search without heuristic (uniform cost search).
def null_heuristic(state):
return 0
# Test problems
test_maze3 = Maze("maze3.maz")
test_mp = SensorlessProblem(test_maze3, (1, 4))
# print(test_mp.get_successors(test_mp.start_state))
# this should explore a lot of nodes; it's just uniform-cost search
result = astar_search(test_mp, null_heuristic)
print(result)
# this should do a bit better:
result = astar_search(test_mp, test_mp.cummulative_heuristic)
print(result)
test_mp.animate_path(result.path)
# Your additional tests here:
# print test_mp.robot1_table
# result = astar_search(test_mp, test_mp.manhattan_heuristic)
# print result
test_maze3 = Maze("maze3.maz")
print test_maze3
test_mp = MazeworldProblem(test_maze3, (1, 4, 1, 3, 1, 2))
# print(test_mp.get_successors_all(test_mp.start_state))
#this should explore a lot of nodes; it's just uniform-cost search
# result = astar_search(test_mp, null_heuristic)
# print(result)
# this should do a bit better:
result = astar_search(test_mp, test_mp.wavefront_heuristic)
print(result)
# test_mp.animate_path(result.path)
# Your additional tests here:
# test_maze01 = Maze("maze01.maz")
# print test_maze01
# test_mp = MazeworldProblem(test_maze01, (1, 2, 1, 1, 1, 0))
# result = astar_search(test_mp, test_mp.manhattan_heuristic)
# print(result)
# test_maze02 = Maze("maze02.maz")
# print test_maze02
# test_mp = MazeworldProblem(test_maze02, (4, 0, 1, 0))
# result = astar_search(test_mp, test_mp.manhattan_heuristic)
def run_all_tests(search_problem):
print(search_problem.maze)
print(bfs_search(search_problem))
print(astar_search(search_problem, null_heuristic))
print(astar_search(search_problem, search_problem.manhattan_heuristic))
print(astar_search(search_problem, search_problem.straight_line_heuristic))
import astar_search as search
import graph_node as gn
import graph_heuristik
rootnode = gn.GraphNode(None, "Frankfurt")
node = search.astar_search(rootnode, graph_heuristik.h1)
path = []
while True:
if node:
path.append(node)
node = node.parent
if node == None:
break
path.reverse()
for node in path:
print(node.unique, node.path_cost)
# null heuristic, useful for testing astar search without heuristic (uniform cost search).
def null_heuristic(state):
return 0
# Test problems
test_maze3 = Maze("maze3.maz")
test_mp = MazeworldProblem(test_maze3, (1, 4, 1, 3, 1, 2))
# print(test_mp.get_successors(test_mp.start_state))
# this should explore a lot of nodes; it's just uniform-cost search
result = astar_search(test_mp, null_heuristic)
print(result)
# this should do a bit better:
result = astar_search(test_mp, test_mp.manhattan_heuristic)
print(result)
# test_mp.animate_path(result.path)
# Your additional tests here:
maze_four = Maze("maze4corridor.maz")
test_corridor = MazeworldProblem(maze_four, (2, 4, 2, 5, 2, 6))
result = astar_search(test_corridor, test_corridor.manhattan_heuristic)
print("\n maze 4 \n")
print(result)
# test_corridor.animate_path(result.path)
