def test_astar_heuristic():
graph = Graph()
node1 = RobotState(position=Point(0, 0))
node2 = RobotState(position=Point(1, 1))
node3 = RobotState(position=Point(3, 1))
node4 = RobotState(position=Point(2, 2))
node5 = RobotState(position=Point(3, 2))
node6 = RobotState(position=Point(2, 3))
graph.add_node(node1)
graph.add_node(node2)
graph.add_node(node3)
graph.add_node(node4)
graph.add_node(node5)
graph.add_node(node6)
graph.add_edge(node1, node2)
graph.add_edge(node2, node3)
graph.add_edge(node2, node4)
graph.add_edge(node4, node5)
graph.add_edge(node4, node6)
graph.add_edge(node5, node6)
heuristic = {
node1: 5,
node2: 3,
node3: 1,
node4: 1,
node5: 0,
node6: 2,
}
start = node1
end = node5
_ = astar(graph, start, end, heuristic)
print(_.path)
def test_astar():
graph = Graph()
node1 = RobotState(position=Point(0, 0))
node2 = RobotState(position=Point(1, 1))
node3 = RobotState(position=Point(3, 1))
node4 = RobotState(position=Point(2, 2))
node5 = RobotState(position=Point(3, 2))
node6 = RobotState(position=Point(2, 3))
graph.add_node(node1)
graph.add_node(node2)
graph.add_node(node3)
graph.add_node(node4)
graph.add_node(node5)
graph.add_node(node6)
graph.add_edge(node1, node2)
graph.add_edge(node2, node3)
graph.add_edge(node2, node4)
graph.add_edge(node4, node5)
graph.add_edge(node4, node6)
graph.add_edge(node5, node6)
start = node1
end = node5
_ = astar(graph, start, end)
print(_.path)
def test_astar():
graph = {
Point(0, 0): [Point(1, 1)],
Point(1, 1): [Point(0, 0), Point(3, 1), Point(2, 2)],
Point(3, 1): [Point(1, 1)],
Point(2, 2): [Point(1, 1), Point(3, 2), Point(2, 3)],
Point(3, 2): [Point(2, 2), Point(2, 3)],
Point(2, 3): [Point(2, 2), Point(3, 2)],
}
start = Point(0, 0)
end = Point(3, 2)
_ = astar(graph, start, end)
def plan(self, start, goal, env):
"""Constructs a graph avoiding obstacles and then plans path from start to goal within the graph.
Args:
start (gennav.utils.RobotState): tuple with start point coordinates.
goal (gennav.utils.RobotState): tuple with end point coordinates.
env (gennav.envs.Environment): Base class for an envrionment.
Returns:
gennav.utils.Trajectory: The planned path as trajectory
"""
# construct graph
graph = self.construct(env)
# find collision free point in graph closest to start_point
min_dist = float("inf")
for node in graph.keys():
dist = math.sqrt((node.x - start.position.x)**2 +
(node.y - start.position.y)**2)
traj = Trajectory([
RobotState(position=node),
RobotState(position=start.position)
])
if dist < min_dist and (env.get_traj_status(traj)):
min_dist = dist
s = node
# find collision free point in graph closest to end_point
min_dist = float("inf")
for node in graph.keys():
dist = math.sqrt((node.x - goal.position.x)**2 +
(node.y - goal.position.y)**2)
traj = Trajectory([
RobotState(position=node),
RobotState(position=goal.position)
])
if dist < min_dist and (env.get_traj_status(traj)):
min_dist = dist
e = node
# add start_point to path
path = [start]
traj = Trajectory(path)
# perform astar search
p = astar(graph, s, e)
if len(p.path) == 1:
return traj
else:
traj.path.extend(p.path)
# add end_point to path
traj.path.append(goal)
return traj
def plan(self, start, goal, env):
"""Constructs a graph avoiding obstacles and then plans path from start to goal within the graph.
Args:
start (gennav.utils.RobotState): tuple with start point coordinates.
goal (gennav.utils.RobotState): tuple with end point coordinates.
env (gennav.envs.Environment): Base class for an envrionment.
Returns:
gennav.utils.Trajectory: The planned path as trajectory
dict: Dictionary containing additional information
"""
# Check if start and goal states are obstacle free
if not env.get_status(start):
raise InvalidStartState(start,
message="Start state is in obstacle.")
if not env.get_status(goal):
raise InvalidGoalState(goal, message="Goal state is in obstacle.")
# construct graph
graph = self.construct(env)
# find collision free point in graph closest to start_point
min_dist = float("inf")
for node in graph.nodes:
dist = compute_distance(node.position, start.position)
traj = Trajectory([node, start])
if dist < min_dist and (env.get_traj_status(traj)):
min_dist = dist
s = node
# find collision free point in graph closest to end_point
min_dist = float("inf")
for node in graph.nodes:
dist = compute_distance(node.position, goal.position)
traj = Trajectory([node, goal])
if dist < min_dist and (env.get_traj_status(traj)):
min_dist = dist
e = node
# add start_point to path
path = [start]
traj = Trajectory(path)
# perform astar search
p = astar(graph, s, e)
if len(p.path) == 1:
raise PathNotFound(p, message="Path contains only one state")
else:
traj.path.extend(p.path)
# add end_point to path
traj.path.append(goal)
return traj, {}
def test_astar_heuristic():
graph = {
Point(0, 0): [Point(1, 1)],
Point(1, 1): [Point(0, 0), Point(3, 1), Point(2, 2)],
Point(3, 1): [Point(1, 1)],
Point(2, 2): [Point(1, 1), Point(3, 2), Point(2, 3)],
Point(3, 2): [Point(2, 2), Point(2, 3)],
Point(2, 3): [Point(2, 2), Point(3, 2)],
}
heuristic = {
Point(0, 0): 5,
Point(1, 1): 3,
Point(3, 1): 1,
Point(2, 2): 1,
Point(3, 2): 0,
Point(2, 3): 2,
}
start = Point(0, 0)
end = Point(3, 2)
_ = astar(graph, start, end, heuristic)
def plan(self, start, end, env):
"""Plans path from start to goal avoiding obstacles.
Args:
start (gennav.utils.RobotState): Starting state
end (gennav.utils.RobotState): Goal state
env: (gennav.envs.Environment) Base class for an envrionment.
Returns:
gennav.utils.Trajectory: The planned path as trajectory
dict: Dictionary containing additional information
"""
# Check if start and end states are obstacle free
if not env.get_status(start):
raise InvalidStartState(start,
message="Start state is in obstacle.")
if not env.get_status(end):
raise InvalidGoalState(end, message="Goal state is in obstacle.")
# Initialize graph
graph = Graph()
graph.add_node(start)
# Initialize node list with Starting Position
node_list = [start]
# Loop for maximum iterations to get the best possible path
for iter in range(self.max_iter):
rnd_node = self.sampler()
# Find nearest node to the sampled point
distance_list = [
compute_distance(node.position, rnd_node.position)
for node in node_list
]
try:
# for python2
nearest_node_index = min(xrange(len(distance_list)),
key=distance_list.__getitem__)
except NameError:
# for python 3
nearest_node_index = distance_list.index(min(distance_list))
nearest_node = node_list[nearest_node_index]
# Create new point in the direction of sampled point
theta = math.atan2(
rnd_node.position.y - nearest_node.position.y,
rnd_node.position.x - nearest_node.position.x,
)
new_node = RobotState(position=Point(
nearest_node.position.x + self.exp_dis * math.cos(theta),
nearest_node.position.y + self.exp_dis * math.sin(theta),
))
# Check whether new point is inside an obstacles
trj = Trajectory([nearest_node, new_node])
if not env.get_status(new_node) or not env.get_traj_status(trj):
continue
else:
node_list.append(new_node)
# FINDING NEAREST INDICES
nnode = len(node_list) + 1
# The circle in which to check parent node and rewiring
r = self.circle * math.sqrt((math.log(nnode) / nnode))
dist_list = [
compute_distance(node.position, new_node.position)
for node in node_list
]
# Getting all the indexes within r units of new_node
near_inds = [dist_list.index(i) for i in dist_list if i <= r**2]
graph.add_node(new_node)
graph.add_edge(nearest_node, new_node)
# Add all the collision free edges to the graph.
for ind in near_inds:
node_check = node_list[ind]
trj = Trajectory([new_node, node_check])
# If the above condition is met append the edge.
if env.get_traj_status(trj):
graph.add_edge(new_node, node_check)
if end not in graph.nodes:
min_cost = 2.0
closest_node = RobotState()
for node in graph.nodes:
temp = compute_distance(node.position, end.position)
if env.get_traj_status(Trajectory([node, end
])) and temp < min_cost:
min_cost = temp
closest_node = node
graph.add_edge(closest_node, end)
graph.add_node(end)
if start in graph.edges[end]:
graph.del_edge(start, end)
path = []
p = astar(graph, start, end)
path.extend(p.path)
if len(path) != 1:
print("Goal Reached!")
path = Trajectory(path)
if len(path.path) == 1:
raise PathNotFound(path, message="Path contains only one state")
return path, {}
