def plot_visited(env, visited):
plt.ion()
fig = e.plot_environment(env)
axes = fig.gca()
for i in range(len(visited)):
e.plot_environment(env, fig=fig)
for obstacle in visited[i].obstacles:
axes.add_artist(obstacle.get_circle('b'))
plt.draw()
input()
plt.ioff()
def plot_rrt(env, bounds, path, vertices, parents, radius, start_pose,
end_region, time):
ax = plot_environment(env, bounds=bounds)
plot_poly(ax, Point(start_pose).buffer(radius, resolution=3), 'magenta')
plot_poly(ax, end_region, 'brown', alpha=0.2)
# plot the full tree
for v in vertices:
if v in parents:
x, y = LineString([parents[v], v]).xy
ax.plot(x,
y,
color='blue',
linewidth=1,
solid_capstyle='round',
zorder=1)
# plot the final path
for i in range(1, len(path)):
x, y = LineString([path[i - 1], path[i]]).xy
ax.plot(x,
y,
color='green',
linewidth=3,
solid_capstyle='round',
zorder=1)
# tree nodes, time, num obs, path length (nodes)
pathLength = getPathLength(path)
plt.title(
'Nodes: %d Time: %.3f sec Obstacles: %d Path Length: %.3f (%d nodes)' %
(len(vertices), time, len(env.obstacles), pathLength, len(path)))
plt.show()
def debug_plot(env, bounds, vertices, parents, start_pose, end_region):
ax = plot_environment(env, bounds=bounds)
plot_poly(ax, Point(start_pose).buffer(radius, resolution=3), 'magenta')
plot_poly(ax, end_region, 'brown', alpha=0.2)
for v in vertices:
if v in parents:
plot_line(ax, LineString([parents[v], v]))
plt.show()
def plot_env(self):
""" Plot the environment, including static obstacles but excluding
agents and RRT trees.
Returns:
ax: the plot object
"""
ax = plot_environment(self.env)
ax.set_title("Multiagent Planner")
return ax
def draw_results(algo_name, path, V, E, env, bounds, object_radius, resolution, start_pose, goal_region, elapsed_time):
"""
Plots the path from start node to goal region as well as the graph (or tree) searched with the Sampling Based Algorithms.
Args:
algo_name (str): The name of the algorithm used (used as title of the plot)
path (list<(float,float), (float,float)>): The sequence of coordinates traveled to reach goal from start node
V (set<(float, float)>): All nodes in the explored graph/tree
E (set<(float,float), (float, float)>): The set of all edges considered in the graph/tree
env (yaml environment): 2D yaml environment for the path planning to take place
bounds (int, int int int): min x, min y, max x, max y of the coordinates in the environment.
object_radius (float): radius of our object.
resolution (int): Number of segments used to approximate a quarter circle around a point.
start_pose(float,float): Coordinates of initial point of the path.
goal_region (Polygon): A polygon object representing the end goal.
elapsed_time (float): Time it took for the algorithm to run
Return:
None
Action:
Plots a path using the environment module.
"""
originalPath,pruningPath=path
graph_size = len(V)
path_size = len(originalPath)
# Calculate path length
path_length1 = 0.0
path_length2 = 0.0
for i in range(len(originalPath)-1):
path_length1 += euclidian_dist(originalPath[i], originalPath[i+1])
for i in range(len(pruningPath)-1):
path_length2 += euclidian_dist(pruningPath[i], pruningPath[i+1])
# Create title with descriptive information based on environment, path length, and elapsed_time
title = algo_name + "\n" + str(graph_size) + " Nodes. " + str(len(env.obstacles)) + " Obstacles. Path Size: " + str(path_size) + "\n Path Length: " + str([path_length1,path_length2]) + "\n Runtime(s)= " + str(elapsed_time)
# Plot environment
env_plot = plot_environment(env, bounds)
# Add title
env_plot.set_title(title)
# Plot goal
plot_poly(env_plot, goal_region, 'green')
# Plot start
buffered_start_vertex = Point(start_pose).buffer(object_radius, resolution)
plot_poly(env_plot, buffered_start_vertex, 'red')
# Plot Edges explored by ploting lines between each edge
for edge in E:
line = LineString([edge[0], edge[1]])
plot_line(env_plot, line)
# Plot path
plot_path(env_plot, originalPath, object_radius,'black')
plot_path(env_plot, pruningPath, object_radius,'red')
def plot_one_step(env, x_ref, x_bar, x_opt, x_next=None, nominals=None):
''' Plots a single step of trajectory optimization in environment '''
fig, ax = environment.plot_environment(env, figsize=(16, 10))
ax.plot(x_ref[0, :], x_ref[1, :], '-o', alpha=0.8,
color='blue', markersize=3, label='reference trajectory')
if nominals is not None:
for nom in nominals:
ax.plot(nom[0, :], nom[1, :], 'r-', label='nominal trajectories')
else:
ax.plot(x_bar[0, :], x_bar[1, :], 'r-', label='nominal trajectory')
ax.plot(x_opt[0, :], x_opt[1, :], '-o',
color='lightseagreen', label='optimal trajectory')
if x_next is not None:
ax.plot(x_next[0], x_next[1], 'o', color='blueviolet', label='next x0')
handles, labels = plt.gca().get_legend_handles_labels()
labels, ids = np.unique(labels, return_index=True)
handles = [handles[i] for i in ids]
ax.legend(handles, labels, loc='best')
return fig, ax
def plot_all_steps(env, x_ref_full, history):
''' Plots optimization paths in environment at each step over time
course of the full MPC run
'''
fig, ax = environment.plot_environment(env, figsize=(16, 10))
ax.plot(x_ref_full[0, :], x_ref_full[1, :],
'-o', color='blue', label='reference trajectory', markersize=3)
for i in range(len(history.keys())):
xi = history[i]['x_opt']
ax.plot(xi[0, :], xi[1, :], color='lightseagreen',
linewidth=1, label='N-step optimal traj')
x0x = [history[i]['x0'][0] for i in history.keys()]
x0y = [history[i]['x0'][1] for i in history.keys()]
ax.plot(x0x, x0y, '-o', color='blueviolet', label='actual trajectory')
xf_bar = history[len(history.keys())-1]['x_bar']
# ax.plot(xf_bar[0, :], xf_bar[1, :], 'r', label='xf_bar')
ax.axis('equal')
handles, labels = plt.gca().get_legend_handles_labels()
labels, ids = np.unique(labels, return_index=True)
handles = [handles[i] for i in ids]
ax.legend(handles, labels, loc='best')
return fig, ax
def draw_results(algo_name, path, V, E, env, bounds, object_radius, resolution,
start_pose, goal_region, elapsed_time):
graph_size = len(V)
path_size = len(path)
path_length = 0.0
for i in xrange(len(path) - 1):
path_length += euclidian_dist(path[i], path[i + 1])
title = algo_name + "\n" + str(graph_size) + " Nodes. " + str(
len(env.obstacles)) + " Obstacles. Path Size: " + str(
path_size) + "\n Path Length: " + str(
path_length) + "\n Runtime(s)= " + str(elapsed_time)
env_plot = plot_environment(env, bounds)
env_plot.set_title(title)
plot_poly(env_plot, goal_region, 'green')
buffered_start_vertex = Point(start_pose).buffer(object_radius, resolution)
plot_poly(env_plot, buffered_start_vertex, 'red')
for edge in E:
line = LineString([edge[0], edge[1]])
plot_line(env_plot, line)
plot_path(env_plot, path, object_radius)
def animate(env, ctrl_pts, bc_headings, v, dt, x_ref, history, rect=False):
fig, ax = environment.plot_environment(env, figsize=(16, 10))
xs = x_ref[0, :]
ys = x_ref[1, :]
x0s = [history[i]['x0'][0] for i in history.keys()]
y0s = [history[i]['x0'][1] for i in history.keys()]
# plot reference trajectory
ax.plot(xs, ys, '-o', alpha=0.8, markersize=3,
color='blue', label='reference trajectory')
# optimized trajectory
opt_line, = ax.plot([], [], '-o', lw=3, color='lightseagreen',
label='N-step optimal traj')
nom_line, = ax.plot([], [], color='red', label='nominal trajectory')
act_line, = ax.plot([], [], '-o', lw=3, markersize=6,
color='blueviolet', label='actual trajectory')
if rect:
ld, wd = 0.5, 0.2
a2 = np.arctan2(wd, ld)
diag = np.sqrt(ld**2 + wd**2)
heading = np.rad2deg(np.arctan2((y0s[1]-y0s[0]), (x0s[1]-x0s[0])))
car = patches.Rectangle(
(x0s[0]-ld, y0s[0]-wd), 2*ld, 2*wd, angle=-heading, fc='none', lw=1, ec='k')
def init():
opt_line.set_data([], [])
act_line.set_data([], [])
nom_line.set_data([], [])
if rect:
ax.add_patch(car)
return opt_line,
def step(i):
x = history[i]['x_opt'][0]
y = history[i]['x_opt'][1]
xbar = history[i]['x_bar'][0]
ybar = history[i]['x_bar'][1]
opt_line.set_data(x, y)
act_line.set_data(x0s[:i], y0s[:i])
nom_line.set_data(xbar, ybar)
if rect:
if (len(x) == 1):
heading = bc_headings[1]
else:
heading = np.arctan2((y[1]-y[0]), (x[1]-x[0]))
xoff = diag*np.cos(heading + a2)
yoff = diag*np.sin(heading + a2)
car.set_xy((x[0] - xoff, y[0] - yoff))
car.angle = np.rad2deg(heading)
return opt_line,
anim = animation.FuncAnimation(fig, step, init_func=init,
frames=len(history.keys()), interval=1000*dt*2, blit=True)
ax.axis('equal')
ax.legend()
plt.close()
return anim
def rrt(bounds, environment, start_pose, radius, end_region):
'''
- bounds: (minx, miny, maxx, maxy) tuple over region
- environment: instance of the Environment class that describes the placement of the obstacles in the scene
- start_pose: start_pose = (x,y) is a tuple indicating the starting position of the robot
- radius: radius is the radius of the robot (used for collision checking)
- end_region: end_region is a shapely Polygon that describes the region that the robot needs to reach
'''
# Adding tuples to nodes -> represent all nodes expanded by tree
nodes = [start_pose]
# Creating graph of SearchNodes from the tuples to represent the tree with parents. Used to order and
graph = Graph()
graph.add_node(SearchNode(start_pose))
goalPath = Path(
SearchNode(start_pose)) # for initialization in case of no path found
# Draw the environment (with its obstacles) and with the start node + end region
if plotting:
ax = plot_environment(environment, bounds)
plot_poly(ax, Point(start_pose).buffer(radius, resolution=3), 'blue')
plot_poly(ax, end_region, 'red', alpha=0.2)
for i in range(10000): # random high number
# this range must be evaluated according to size of problem. RRT do not ensure any solution paths
# sample a random node inside bounds (random number between xmin/max and ymin/max). all bound-"checking" is donw here: makes sure we don't have to do ant checks later
rand_xval = random.uniform(bounds[0], bounds[2])
rand_yval = random.uniform(bounds[1], bounds[3])
node_rand = (rand_xval, rand_yval)
'''
--- For every x'th iteration - aim towards goal. A good choice varies, and depends on the step length that I decided in steerpath(). Not having this makes total number of nodes blow up. I have kept choices of when to goal bias and the travel distance in steering function to perform well in large environments. TODO: This number depends on complexity of space. Easy space: lower number
--- Then, find out which node in our list that is the nearest to the random node. Returns a searchNode and a float distance.
--- Steer towards the new node -> correct parents and add cost
--- Check if the new steered node is in bounds.
--- If not visited before (avoid cycle) and no obstacles are in the path: add to tree
'''
sampling_rate = 12
if not (i % sampling_rate):
node_rand = end_region.centroid.coords[0]
node_nearest, node_dist = nearestSNode(graph, node_rand)
## TODO: this steering function should somehow consider dynamics
steered_node = steerPath(node_nearest.state, node_rand)
if not (bounds[0] < steered_node[0] <
bounds[2]) or not (bounds[1] < steered_node[1] < bounds[3]):
continue # sometimes not checking for this made the path go out of bounds
node_steered = SearchNode(steered_node, node_nearest,
node_nearest.cost + node_dist)
if node_steered.state not in nodes:
if not obstacleIsInPath(node_nearest.state, node_steered.state,
environment, radius):
# Keep track of total number of nodes in tree
# Add edge (also adds new nodes to graph) and weight
# Plot non-colliding edge to show all searched nodes
nodes.append(node_steered.state)
graph.add_edge(node_nearest, node_steered, node_dist)
if plotting:
line = LineString([node_nearest.state, node_steered.state])
plot_line_mine(ax, line)
else:
continue # Avoid goal check if collision is found
# Check last addition for goal state
# TODO: ADDED EXTRA SAFETY IN GOAL CHECK
if goalReached(node_steered.state, 1.5 * radius, end_region):
goalPath = Path(node_steered)
break # break the while loop when solution is found!
# No. of nodes in total - No. of nodes in sol path - Sol path length
noOfTotalNodes = len(nodes)
noOfNodesInSol = len(goalPath.path)
pathLength = goalPath.cost
if plotting:
for i in range(noOfNodesInSol - 1):
# Draw goal path
line = LineString([goalPath.path[i], goalPath.path[i + 1]])
plot_line_mine(ax, line)
# Draw the expanded line
expanded_line = line.buffer(radius, resolution=3)
plot_poly(ax, expanded_line, 'green', alpha=0.2)
# plotting last node in goalPath and setting title to format in task
plot_poly(ax,
Point(goalPath.path[-1]).buffer(radius, resolution=3),
'blue')
titleString = "Nodes total / in solution path: %s/ %s \nPath length: %0.3f"
ax.set_title(titleString %
(noOfTotalNodes, noOfNodesInSol, pathLength))
return goalPath.path
def path_to_wp(waypoints,
wp_index,
init_triplet,
env,
wp_radii,
fig=None,
neighbour_func=lambda c, n, s: c.quadruplet_distance(n, s),
scope_range=50,
deadends=None):
"""
This implements a simple A*-search, using the cost_to_move()
and heuristic_cost() functions to find g and h values, respectively.
Returns a list of triplets that the snake will move through.
TODO:
Catch when it is unable to complete and physically backtrack, marking the current path unfeasible
Investigate why some triplets have no parents
tests
"""
plt.ion()
deadends = deadends if deadends else {}
previous_wp = waypoints[wp_index - 1]
wp = waypoints[wp_index]
max_dist = wp_radii[wp_index]
visited = [] # Closed set
seen = []
queue = [] # Open set
heappush(queue, init_triplet)
current = None
found = False
active_triplet = init_triplet
init_triplet.is_first_in_path = True
print(active_triplet)
active = init_triplet.obstacles[2]
iteration = 1
wps_left = 0
while True:
while queue:
current = heappop(queue)
visited.append(current)
if fig is not None:
ax = fig.gca()
ax.clear()
fig = e.plot_environment(env, fig=fig)
fig = plot_desired_path(waypoints, 'g', wp_radii, fig=fig)
fig = plot_active_segment(previous_wp,
wp,
'm',
wp_radii,
path=create_path(current),
fig=fig)
circle = plt.Circle((active.x, active.y), 4, color='black')
circle2 = plt.Circle((active.x, active.y),
scope_range,
fill=False,
color='yellow')
ax.add_artist(circle)
ax.add_artist(circle2)
plt.draw()
plt.pause(.001)
# Check if we have reached our goal, and end if we have
if wp.distance_to(
Point(current.obstacles[2].x,
current.obstacles[2].y)) <= max_dist:
found = True
while wps_left > 0:
current = path_to_wp(waypoints, wp_index + 1, current, env,
wp_radii, fig, neighbour_func,
scope_range, deadends)
wps_left -= 1
break
if active.distance_to(current.obstacles[2]) > scope_range:
previous_active = active
active_triplet = new_active(current, active)
active = active_triplet.obstacles[2]
queue = filter_queue(queue, active, previous_active)
add_neighbors_to_queue(init_triplet, current, queue, visited, seen,
previous_wp, wp, neighbour_func, deadends)
if not found:
print("No path found, backtracking")
if active_triplet.parent is None:
return None
if active.id not in active_triplet.parent.deadend:
deadends[active_triplet.parent] = deadends.get(
active_triplet.parent, []) + [active.id]
print("Active id: ", active.id)
print("Current parent deadend = ",
deadends.get(active_triplet.parent, []))
if active_triplet.is_first_in_path:
active_triplet.is_first_in_path = False
wp_index -= 1
previous_wp = waypoints[wp_index - 1]
wp = waypoints[wp_index]
wps_left += 1
active_triplet = current.parent
active = active_triplet.obstacles[2]
current = active_triplet
add_neighbors_to_queue(init_triplet, current, queue, visited, seen,
previous_wp, wp, neighbour_func, deadends)
continue
break
plt.ioff()
return current