Lab 5: Discrete Planning
Overview
In Lab5, you will plan paths for a Turtlebot operating on the 2D plane. We will assume that a grid map is given with some grid cells marked as obstacles. The obstacles on the grid have been inflated by the radius of the robot. The task is to apply A* algorithm to find an optimal path from a given start configuration to a given goal configuration without colliding with obstacles. You can use Manhattan distance or Euclidean distance as the heuristic function (other admissible heuristic functions should also work). Please refer to lectures 16 and 17 for more details. The pseudocode is provided at the end of this page.
We will implement two different versions of A* algorithm to search for discrete paths:
Point A*. Consider the robot as a point (with only x and y coordinates). In this case, the robot is only capable of translation and the discretized C-space is the same as the provided grid map. Hence, the task is to find a path from a given start cell to a given goal cell. During A* search, you can treat each 2D cell as a node. When expanding a node, i.e., generate its successors/children, we will assume a 4-connected grid (no diagonal movements).
Oriented A*. Include orientation θ as an additional kinematic constraint, so thst the C-space is now three dimensional (x, y, θ), often denoted as SE(2) for planar robots. The discretized version of the C-space consists of the 2D grid cells enhanced by an orientation dimension. We assume a coarse resolution for the orientation θ consisting of 0, 90, 180, and 270 degrees. During A* search, when expanding a node, the robot has three discrete high-level actions that can lead to corresponding neighboring 3D cells :
Forward: Keep the current orientation and move forward.
Turn Left and Move Forward: Rotate 90° to the left, and then move forward along the new direction.
Turn Right and Move Forward: Rotate 90° to the right, and then move forward along the new direction.
A* Algorithm
A* is a pathfinding algorithm that finds the shortest path from a start point to a goal in a graph or grid. It does this by combining two key ideas:
Path Cost (g): The actual cost from the start to the current node.
Heuristic Estimate (h): The estimated cost-to-go from the current node to the goal.
By considering the sum f=g+h, A* prioritizes exploring nodes that appear to be closer to the goal both in terms of what has been achieved and what still remains. This makes A* both complete (it will find a solution if one exists) and optimal (it finds the best solution) when the heuristic is consistent (consistent heuristics are admissible, i.e., never overestimate the true cost to the goal).
Sample Code
Open a new terminal and go to your ee144w25 package. We will create a new python script.
roscd ee144w25/src
touch point_a_star.py
gedit point_a_star.py
Please copy and paste the following code in point_a_star.py.
def neighbors(current):
# define the list of 4 neighbors
neighbors = []
return [ (current[0]+nbr[0], current[1]+nbr[1]) for nbr in neighbors ]
def heuristic_distance(candidate, goal):
pass
def get_path_from_A_star(start, goal, obstacles):
# input start: integer 2-tuple of the current grid, e.g., (0, 0)
# goal: integer 2-tuple of the goal grid, e.g., (5, 1)
# obstacles: a list of grids marked as obstacles, e.g., [(2, -1), (2, 0), ...]
# output path: a list of grids connecting start to goal, e.g., [(1, 0), (1, 1), ...]
# note that the path should contain the goal but not the start
# e.g., the path from (0, 0) to (2, 2) should be [(1, 0), (1, 1), (2, 1), (2, 2)]
return path
Now, create a new python script for A star with orientation .
roscd ee144w25/src
touch orient_a_star.py
gedit orient_a_star.py
Please copy and paste the following code in orient_a_star.py.
actions = [-1, 0, 1] # right turn and then move forward, move forward, left turn and then move forward
directions = {
0: (-1, 0), # Go west
1: (0, 1), # Go south
2: (1, 0), # Go east
3: (0, -1), # Go north
}
def neighbors_orient(current, cost):
# define the list of 3 neighbors according the actions
neighbors = []
x, y, theta =
# fill in the blanks
for i in range(len(actions)):
new_theta =
new_x =
new_y =
cell_cost =
neighbors.append(((new_x, new_y, new_theta), cell_cost))
return neighbors
def heuristic_distance(candidate, goal):
pass
def get_path_from_A_star_orient(start, goal, obstacles, cost):
# input start: integer 3-tuple of the current grid, e.g., (0, 0, 0)
# goal: integer 3-tuple of the goal grid, e.g., (5, 1, 0)
# obstacles: a list of grids marked as obstacles, e.g., [(2, -1), (2, 0), ...]
# output path: a list of grids connecting start to goal, e.g., [(1, 0, 0), (1, 1, 1), ...]
# note that the path should contain the goal but not the start
return path
To test the algorithm, you can use the following visualization script and change test cases as desired. For the visualization in your get_path_from_A_star or get_path_from_A_star_orient you have to also return the closed list. For the autograder remove that.
import matplotlib.pyplot as plt
import matplotlib.patches as patches
from point_a_star import get_path_from_A_star
from orient_a_star import get_path_from_A_star_orient
def visualize_grid(bounds, obstacles, closed, path, start, goal):
(min_row, max_row), (min_col, max_col) = bounds
fig, ax = plt.subplots(figsize=(max_col - min_col + 1, max_row - min_row + 1))
# Check if path states include orientation (i.e. are 3-tuples)
show_actions = False
if path:
sample = next(iter(path))
if isinstance(sample, tuple) and len(sample) == 3:
show_actions = True
action_at = {}
for i in range(len(path) - 1):
curr = path[i]
nxt = path[i + 1]
# Compute the difference in orientation from current to next.
diff = (nxt[2] - curr[2]) % 4
if diff == 0:
act = 'F' # Forward (no orientation change)
elif diff == 1:
act = 'L' # Left turn
elif diff == 3:
act = 'R' # Right turn (since -1 mod 4 equals 3)
else:
act = '?' # Any unexpected difference
action_at[(curr[0], curr[1])] = act
final_path_positions = {(s[0], s[1]) for s in path}
else:
final_path_positions = set(path)
else:
final_path_positions = set()
# Draw the grid.
for r in range(min_row, max_row + 1):
for c in range(min_col, max_col + 1):
cell_color = 'white'
if (r, c) in obstacles:
cell_color = 'gray'
elif (r, c) in closed:
cell_color = 'lightcyan'
if (r, c) in final_path_positions:
cell_color = 'mediumspringgreen'
if (r, c) == (start[0], start[1]):
cell_color = 'cyan'
if (r, c) == goal:
cell_color = 'yellow'
rect = patches.Rectangle((c, r), 1, 1, edgecolor='black', facecolor=cell_color)
ax.add_patch(rect)
if (r, c) == (start[0], start[1]):
ax.text(c + 0.5, r + 0.5, "Start", ha='center', va='center', fontsize=10, color='black')
elif (r, c) == goal:
ax.text(c + 0.5, r + 0.5, "Goal", ha='center', va='center', fontsize=10, color='black')
elif show_actions and (r, c) in action_at:
ax.text(c + 0.5, r + 0.5, action_at[(r, c)], ha='center', va='center', fontsize=10, color='black')
elif (r, c) in closed and (r, c) not in final_path_positions:
ax.text(c + 0.5, r + 0.5, "x", ha='center', va='center', fontsize=12, color='red')
ax.set_xlim(min_col, max_col + 1)
ax.set_ylim(min_row, max_row + 1)
ax.set_aspect('equal')
xticks = [c + 0.5 for c in range(min_col, max_col + 1)]
yticks = [r + 0.5 for r in range(min_row, max_row + 1)]
ax.set_xticks(xticks)
ax.set_yticks(yticks)
ax.set_xticklabels(range(min_col, max_col + 1))
ax.set_yticklabels(range(min_row, max_row + 1))
plt.show()
if __name__ == "__main__":
# Define start state (row, col, theta) – for example, start at (0, 0)
start = (0, 0) #(0, 0, 0)
goal = (-5, -2) #(-5, -2, 1)
obstacles = {(-2, 1), (-2, 0), (-2, -1), (-2, -2), (-4, -2), (-4, -3)}
costs = [1, 1, 1]
# return close_list for visualization (point or orient)
final_path, closed_list = get_path_from_A_star(start, goal, obstacles)
#final_path, closed_list = get_path_from_A_star_orient(start, goal, obstacles, costs)
print("Final path:")
for s in final_path:
print(s)
closed_positions = {(s[0], s[1]) for s in closed_list}
# Determine grid boundaries using all original coordinates.
all_rows = [p[0] for p in obstacles] + [s[0] for s in closed_positions] + \
[start[0], goal[0]] + [s[0] for s in final_path]
all_cols = [p[1] for p in obstacles] + [s[1] for s in closed_positions] + \
[start[1], goal[1]] + [s[1] for s in final_path]
min_r, max_r = min(all_rows), max(all_rows)
min_c, max_c = min(all_cols), max(all_cols)
bounds = ((min_r, max_r), (min_c, max_c))
# Visualize the grid using the original coordinates.
visualize_grid(bounds, obstacles, closed_positions, final_path, start, goal)
# visualize_grid(bounds, obstacles, closed_positions, final_path, start, goal[:-1])