def main():
point_list = np.load('final_rrt_prune_path_coords.npy')
print("original list:", len(point_list))
(BC_x, BC_y) = bezierCurveTesting1(point_list, animate=False, write_path=None)
plt.ion()
itr = 0
visualizeNewPath(BC_x, BC_y, point_list, animate=True, write_path='./rrt_smooth_no_obs_frames', itr=itr)
itr += 1
# for bcx, bcy in zip(BC_x, BC_y):
while True:
idx = 0
while idx < len(BC_x):
bcx, bcy = BC_x[idx], BC_y[idx]
# if the current waypoint is within the obstacle space => make a new waypoint
if obs.withinObstacleSpace(point=(bcx, bcy), radius=mn.DRONE_RADIUS, clearance=(mn.DRONE_RADIUS / 2)):
print("within obstacle")
break
idx += 1
if idx == len(BC_x):
np.save(file='final_rrt_prune_smooth_path_coords.npy', arr=point_list)
break
point_list = addMidPointsToPath(point_list)
(BC_x, BC_y) = bezierCurveTesting1(point_list, animate=False, write_path=None)
visualizeNewPath(BC_x, BC_y, point_list, animate=True, write_path='./rrt_smooth_no_obs_frames', itr=itr)
itr += 1
plt.ioff()
plt.show()
def actionMove(current_node, next_action, goal_position, clearance, plotter=plt, viz_please=False): Xi, Yi = current_node.getXYCoords() Thetai = current_node.orientation UL, UR = next_action mc = current_node.movement_cost t = 0 r = 0.038 L = 0.354 dt = 0.1 Xn = Xi Yn = Yi Thetan = math.radians(Thetai) # Xi, Yi,Thetai: Input point's coordinates # Xs, Ys: Start point coordinates for plot function # Xn, Yn, Thetan: End point coordintes while t < 1: t = t + dt Xs = Xn Ys = Yn Xn += 0.5 * r * (UL + UR) * math.cos(Thetan) * dt Yn += 0.5 * r * (UL + UR) * math.sin(Thetan) * dt Thetan += (r / L) * (UR - UL) * dt if viz_please: plotter.plot([Xs, Xn], [Ys, Yn], color="blue") mc += utils.euclideanDistance((Xs, Ys), (Xn, Yn)) # if the intermediate step hits a boundary if (Yn < MIN_COORDS[1]) or (Yn >= MAX_COORDS[1]) or (Xn < MIN_COORDS[0]) or (Xn >= MAX_COORDS[0]): return None # if the intermediate step hits an obstacle if (obstacles.withinObstacleSpace((Xn, Yn), radius=a_star.ROBOT_RADIUS, clearance=clearance)): return None cc = (Yn, Xn) pc = current_node.current_coords ori = math.degrees(Thetan) pori = current_node.orientation gc = utils.euclideanDistance(cc, goal_position) ret_val = node.Node(current_coords=cc, parent_coords=pc, orientation=ori, parent_orientation=pori, action=next_action, movement_cost=mc, goal_cost=gc) return ret_val
def hitsObstacle(start_node, goal_node, step_size=0.1): sn_x, sn_y = start_node.getXYCoords() gn_x, gn_y = goal_node.getXYCoords() theta = np.arctan2((gn_y - sn_y), (gn_x - sn_x)) nn_x, nn_y = sn_x, sn_y while (utils.euclideanDistance(point_1=(nn_x, nn_y), point_2=(gn_x, gn_y)) > 0.001): # next node nn_x = nn_x + (step_size * np.cos(theta)) nn_y = nn_y + (step_size * np.sin(theta)) if obs.withinObstacleSpace(point=(nn_x, nn_y), radius=main.DRONE_RADIUS, clearance=main.DRONE_RADIUS / 2): return True return False
def isObstacleLine(start, end):
pts = np.linspace(0, 1, num=100)
obs = []
for t in pts:
eqn = t * start + (1 - t) * end
# print(eqn)
obs.append(
obstacles.withinObstacleSpace(point=eqn, radius=0, clearance=0))
obs = np.array(obs)
if np.any(obs):
return True
else:
return False
def main():
# User input for the start state
start_c, start_r, start_orient = map(
float,
raw_input(
"Enter starting coordinates (x y) and orientation: ").split())
# User input for goal state
goal_c, goal_r = map(float,
raw_input("Enter goal coordinates (x y): ").split())
radius = float(input("Enter the robot radius: "))
clearance = float(input("Enter the clearance: "))
step_size = float(input("Enter the robot step_size: "))
# check if the start node lies withing the map and not on obstacles
if (start_r < actions.MIN_COORDS[1]) or (
start_r >= actions.MAX_COORDS[1]
) or (start_c < actions.MIN_COORDS[0]) or (
start_c >= actions.MAX_COORDS[0]) or obstacles.withinObstacleSpace(
(start_c, start_r), radius, clearance):
print(
"ERROR: Invalid start node. It either lies outside the map boundary or within the obstacle region."
)
sys.exit(0)
# check if the goal node lies withing the map and not on obstacles
if (goal_r < actions.MIN_COORDS[1]) or (
goal_r >= actions.MAX_COORDS[1]
) or (goal_c < actions.MIN_COORDS[0]) or (
goal_c >= actions.MAX_COORDS[0]) or obstacles.withinObstacleSpace(
(goal_c, goal_r), radius, clearance):
print(
"ERROR: Invalid goal node. It either lies outside the map boundary or within the obstacle region."
)
sys.exit(0)
# check is step size lies between 0 and 10
if step_size < 1 or step_size > 10:
print("ERROR: Invalid step_size. It must lie within 1 and 10.")
sys.exit(0)
# write code to find the actual path using a star
start_time = time.clock()
path, viz_nodes = a_star.aStar(start_pos=(start_r, start_c),
goal_pos=(goal_r, goal_c),
robot_radius=radius,
clearance=clearance,
step_size=step_size,
theta=30,
duplicate_step_thresh=0.5,
duplicate_orientation_thresh=30)
print "Time to run A*:", time.clock() - start_time, "seconds"
# visualize path
goal_node = node.Node(current_coords=(goal_r, goal_c),
parent_coords=None,
orientation=None,
parent_orientation=None,
movement_cost=None,
goal_cost=0)
univ.function(viz_nodes, path, goal_node, step_size)
def main():
"""
Main function that takes user input and
computes shortest path using the A* algorithm
complying with the given constraints.
"""
# User input for the start state
start_c, start_r, theta = map(
float,
raw_input(
"Enter starting coordinates (x y) and orientation: ").split())
start_rc = (start_r, start_c)
# User input for goal state
goal_c, goal_r = map(float,
raw_input("Enter goal coordinates (x y): ").split())
goal_rc = (goal_r, goal_c)
rpm1, rpm2 = map(float, raw_input("Enter rpm1 and rpm2: ").split())
clearance = float(input("Enter the clearance: "))
clearance = 0.2 if clearance < 0.2 else clearance # Having a reasonable clearance
# check if the start node lies withing the map and not on obstacles
if ((start_r < actions_new.MIN_COORDS[1])
or (start_r >= actions_new.MAX_COORDS[1])
or (start_c < actions_new.MIN_COORDS[0])
or (start_c >= actions_new.MAX_COORDS[0])
or obstacles.withinObstacleSpace(
(start_c, start_r), a_star.ROBOT_RADIUS, clearance)):
print(
"ERROR: Invalid start node. It either lies outside the map boundary or within the obstacle region."
)
return
# check if the goal node lies withing the map and not on obstacles
if ((goal_r < actions_new.MIN_COORDS[1])
or (goal_r >= actions_new.MAX_COORDS[1])
or (goal_c < actions_new.MIN_COORDS[0])
or (goal_c >= actions_new.MAX_COORDS[0])
or obstacles.withinObstacleSpace(
(goal_c, goal_r), a_star.ROBOT_RADIUS, clearance)):
print(
"ERROR: Invalid goal node. It either lies outside the map boundary or within the obstacle region."
)
return
# write code to find the actual path using a star
start_time = time.clock()
path, visited_viz_nodes = a_star.a_star(start_rc=start_rc,
goal_rc=goal_rc,
orientation=theta,
rpm1=rpm1,
rpm2=rpm2,
clearance=clearance,
viz_please=False)
print "Time to run A*:", time.clock() - start_time, "seconds"
print("Number of visited nodes:", len(visited_viz_nodes))
print("Number of nodes in path:", len(path))
np.save("./path_dumps/path_final.npy", path)
np.save("./path_dumps/visited_viz_nodes_final.npy", visited_viz_nodes)
plotter = viz.initPlot(start_rc[::-1],
goal_rc[::-1],
title="Final Plotting")
# plt.savefig(os.path.join(a_star.OUTPUT_DIR, "1.png"))
plt.ion()
i = 2
# i = viz.plotPath(path=visited_viz_nodes, rev=False, pause_time=0.001, plotter=plotter, color="blue", linewidth=1, write_path_prefix=-1, show=False, skip_frames=25)
i = viz.plotPath(path=path,
rev=True,
pause_time=0.001,
plotter=plotter,
color="lime",
linewidth=3,
write_path_prefix=-1,
show=True,
skip_frames=5)
plt.ioff()
print("Done with plots.")
plt.show()
def findShortestPath(start_x, start_y, start_theta, goal_x, goal_y, rpm1, rpm2,
clearance):
start_r = start_y
start_c = start_x
start_rc = (start_r, start_c)
goal_r = goal_y
goal_c = goal_x
goal_rc = (goal_r, goal_c)
# check if the start node lies withing the map and not on obstacles
if ((start_r < actions_new.MIN_COORDS[1])
or (start_r >= actions_new.MAX_COORDS[1])
or (start_c < actions_new.MIN_COORDS[0])
or (start_c >= actions_new.MAX_COORDS[0])
or obstacles.withinObstacleSpace(
(start_c, start_r), a_star.ROBOT_RADIUS, clearance)):
print(
"ERROR: Invalid start node. It either lies outside the map boundary or within the obstacle region."
)
return
# check if the goal node lies withing the map and not on obstacles
if ((goal_r < actions_new.MIN_COORDS[1])
or (goal_r >= actions_new.MAX_COORDS[1])
or (goal_c < actions_new.MIN_COORDS[0])
or (goal_c >= actions_new.MAX_COORDS[0])
or obstacles.withinObstacleSpace(
(goal_c, goal_r), a_star.ROBOT_RADIUS, clearance)):
print(
"ERROR: Invalid goal node. It either lies outside the map boundary or within the obstacle region."
)
return
# write code to find the actual path using a star
start_time = time.clock()
path, visited_viz_nodes = a_star.a_star(start_rc=start_rc,
goal_rc=goal_rc,
orientation=start_theta,
rpm1=rpm1,
rpm2=rpm2,
clearance=clearance,
viz_please=False)
print "Time to run A*:", time.clock() - start_time, "seconds"
print("Number of visited nodes:", len(visited_viz_nodes))
print("Number of nodes in path:", len(path))
plotter = viz.initPlot(start_rc[::-1],
goal_rc[::-1],
title="Final Plotting")
# plt.savefig(os.path.join(a_star.OUTPUT_DIR, "1.png"))
plt.ion()
i = 2
# i = viz.plotPath(path=visited_viz_nodes, rev=False, pause_time=0.001, plotter=plotter, color="blue", linewidth=1, write_path_prefix=-1, show=False, skip_frames=25)
i = viz.plotPath(path=path,
rev=True,
pause_time=0.001,
plotter=plotter,
color="lime",
linewidth=3,
write_path_prefix=-1,
show=True,
skip_frames=5)
plt.ioff()
print("Done with plots.")
plt.show()
return path
def rrt(start_coords, goal_coords, radius, clearance):
x_start, y_start = start_coords
x_goal, y_goal = goal_coords
start_node = node.Node(current_coords=start_coords,
parent_coords=None,
distance=None)
goal_node = node.Node(current_coords=goal_coords,
parent_coords=None,
distance=None)
print(start_node.printNode())
print(goal_node.printNode())
tree = []
path = []
# tree.append(start_node)
tree.append([0, start_node.current_coords, (0, 0)])
N = 0
while (N < MAX_ITER):
print("N:", N)
x_random = (np.random.uniform(X_LIM[0], X_LIM[1]),
np.random.uniform(Y_LIM[0], Y_LIM[1]))
# print("x_random", x_random)
##########################################
#put a check if x_random is repeated
##########################################
# print("<<<<<<<<<<<<Before updating the distance<<<<")
# print("tree", tree)
# print("<<<<<<<<<<<<After updating the distance<<<<<")
for i in tree:
# while True:
distance = utils.euclideanDistance(point_1=i[1], point_2=x_random)
i[0] = distance
# print("tree", i)
heapq.heapify(tree)
# Finding the node in the tree which is the nearest to the x_random
min_node = heapq.heappop(tree)
# print("len(tree)", len(tree))
heapq.heappush(tree, [min_node[0], min_node[1], min_node[2]])
# sys.exit(0)
# print("min node", min_node)
# print("next_node", next_node)
# print("distance", distance)
# path.append(add_node)
# tree.append([min_node[0], x_random, min_node[1]])
# print("<<<<<<<<<<<After apending the latest x random<<<<<")
# print("tree", tree)
# if the random node is within the step size
if min_node[0] <= STEP_SIZE:
# create a node with this value and add it to the path
obstacle_status = obstacles.withinObstacleSpace(
point=min_node[1], radius=radius, clearance=clearance)
if (obstacle_status == False):
add_node = node.Node(current_coords=x_random,
parent_coords=min_node[1],
distance=min_node[0])
path.append(add_node)
# print("<<<<<<<<<<<<<_before<<<<<<<<<,")
# print("len(tree)", len(tree))
heapq.heappush(tree, [
add_node.distance, add_node.current_coords,
add_node.parent_coords
])
# print("<<<<<<<<<<<<after<<<<<<<<<<,")
# print("len(tree)", len(tree))
# tree.append([add_node.distance, add_node.current_coords, add_node.parent_coords])
else:
# Find an intermediate point in between a and b
x1, y1 = min_node[1]
x2, y2 = x_random
slope = (y2 - y1) / (x2 - x1)
theta = math.atan(slope)
x_near = (x1 + (STEP_SIZE * math.cos(theta)),
y1 + (STEP_SIZE * math.sin(theta)))
obstacle_status = obstacles.withinObstacleSpace(
point=min_node[1], radius=radius, clearance=clearance)
if (obstacle_status == False):
add_node = node.Node(current_coords=x_near,
parent_coords=min_node[1],
distance=STEP_SIZE)
# print("<<<<<<<<<<<<<_before<<<<<<<<<,")
# print("len(tree)", len(tree))
path.append(add_node)
heapq.heappush(tree, [
add_node.distance, add_node.current_coords,
add_node.parent_coords
])
# print("<<<<<<<<<<<<<_before<<<<<<<<<,")
# print("len(tree)", len(tree))
# tree.append([add_node.distance, add_node.current_coords, add_node.parent_coords])
goal_status = utils.goal_reached(
current_node=add_node,
goal_node=goal_node,
goal_reach_threshold=GOAL_REACH_THRESH)
if (goal_status):
print("Reached Goal!")
return path, goal_node
# break
N += 1
# print("-------------------")
# print(path)
return path, goal_node
def main():
# point_list = np.random.randint(0, 100, (10, 2))
# print(point_list)
# point_list.sort(axis=0)
# print(point_list)
point_list = np.load('rrt_prune_smooth_path_coords.npy')
(BC_x, BC_y) = bezierCurveTesting1(point_list,
animate=True,
write_path='./rrt_prune_smooth_frames')
# for bcx, bcy in zip(BC_x, BC_y):
idx = 0
while idx < len(BC_x):
bcx, bcy = BC_x[idx], BC_y[idx]
if obs.withinObstacleSpace(point=(bcx, bcy),
radius=mn.DRONE_RADIUS,
clearance=(mn.DRONE_RADIUS / 2)):
_, closest_wp_idx = findClosestWayPoint(ref_point=(bcx, bcy),
point_list=point_list)
if closest_wp_idx > 0:
prev_cwp = point_list[closest_wp_idx - 1]
else:
prev_cwp = None
if closest_wp_idx < len(point_list) - 1:
next_cwp = point_list[closest_wp_idx + 1]
else:
next_cwp = None
if prev_cwp is None and next_cwp is None:
continue
elif prev_cwp is not None and next_cwp is None:
other_wp_idx = closest_wp_idx - 1
other_cwp = prev_cwp
elif next_cwp is not None and prev_cwp is None:
other_wp_idx = closest_wp_idx + 1
other_cwp = next_cwp
else:
_, other_wp_idx = findClosestWayPoint(
ref_point=(bcx, bcy), point_list=[prev_cwp, next_cwp])
if other_wp_idx == 0:
other_wp_idx = closest_wp_idx - 1
other_cwp = prev_cwp
else:
other_wp_idx = closest_wp_idx + 1
other_cwp = next_cwp
closest_wp = point_list[closest_wp_idx]
if other_wp_idx < closest_wp_idx:
mid_wp = (closest_wp + other_cwp) / 2
point_list = np.insert(point_list,
other_wp_idx,
mid_wp,
axis=0)
else:
mid_wp = (closest_wp + other_cwp) / 2
point_list = np.insert(point_list,
closest_wp_idx,
mid_wp,
axis=0)
print("intermediate list:", len(point_list))
(BC_x, BC_y) = bezierCurveTesting1(point_list,
animate=False,
write_path=None)
idx = -1
idx += 1
def main_no():
# point_list = np.random.randint(0, 100, (10, 2))
# print(point_list)
# point_list.sort(axis=0)
# print(point_list)
point_list = np.load('rrt_prune_smooth_path_coords.npy')
print("original list:", len(point_list))
(BC_x, BC_y) = bezierCurveTesting1(point_list, animate=True, write_path='./rrt_prune_smooth_frames')
# for bcx, bcy in zip(BC_x, BC_y):
idx = 0
while idx < len(BC_x):
bcx, bcy = BC_x[idx], BC_y[idx]
# if the current waypoint is within the obstacle space => make a new waypoint
if obs.withinObstacleSpace(point=(bcx, bcy), radius=mn.DRONE_RADIUS, clearance=(mn.DRONE_RADIUS / 2)):
# find the 2 closest original waypoints
_, closest_wp_idx = findClosestWayPoint(ref_point=(bcx, bcy), point_list=point_list)
if closest_wp_idx > 0:
prev_cwp = point_list[closest_wp_idx - 1]
else:
prev_cwp = None
if closest_wp_idx < len(point_list) - 1:
next_cwp = point_list[closest_wp_idx + 1]
else:
next_cwp = None
if prev_cwp is None and next_cwp is None:
continue
elif prev_cwp is not None and next_cwp is None:
other_wp_idx = closest_wp_idx - 1
other_cwp = prev_cwp
elif next_cwp is not None and prev_cwp is None:
other_wp_idx = closest_wp_idx + 1
other_cwp = next_cwp
else:
_, other_wp_idx = findClosestWayPoint(ref_point=(bcx, bcy), point_list=[prev_cwp, next_cwp])
if other_wp_idx == 0:
other_wp_idx = closest_wp_idx - 1
other_cwp = prev_cwp
else:
other_wp_idx = closest_wp_idx + 1
other_cwp = next_cwp
# create a push the new wawypoint in the original list
closest_wp = point_list[closest_wp_idx]
if other_wp_idx < closest_wp_idx:
mid_wp = (closest_wp + other_cwp) / 2
point_list = np.insert(point_list, other_wp_idx, mid_wp, axis=0)
else:
mid_wp = (closest_wp + other_cwp) / 2
point_list = np.insert(point_list, closest_wp_idx, mid_wp, axis=0)
print("intermediate list:", len(point_list))
(BC_x, BC_y) = bezierCurveTesting1(point_list, animate=False, write_path=None)
if len(point_list) == 30:
break
idx = -1
idx += 1
print("new list:", len(point_list))
visualizeNewPath(BC_x, BC_y, point_list)
def aStar(start_pos,
goal_pos,
robot_radius,
clearance,
step_size,
theta=30,
duplicate_step_thresh=0.5,
duplicate_orientation_thresh=30):
start_r, start_c = start_pos
goal_r, goal_c = goal_pos
start_node = node.Node(current_coords=(start_r, start_c),
parent_coords=None,
orientation=0,
parent_orientation=None,
movement_cost=0,
goal_cost=utils.euclideanDistance(
start_pos, goal_pos))
goal_node = node.Node(current_coords=(goal_r, goal_c),
parent_coords=None,
orientation=None,
parent_orientation=None,
movement_cost=None,
goal_cost=0)
# Saving a tuple with total cost and the state node
minheap = [((start_node.movement_cost + start_node.goal_cost), start_node)]
heapq.heapify(minheap)
# defining the visited node like this avoids checking if two nodes are duplicate. because there is only 1 position to store the visited information for all the nodes that lie within this area.
visited = {}
visited[(utils.valRound(start_r), utils.valRound(start_c),
0)] = start_node # marking the start node as visited
viz_visited_coords = [start_node]
while len(minheap) > 0:
_, curr_node = heapq.heappop(minheap)
# if curr_node.isDuplicate(goal_node):
if curr_node.goal_cost < (1.5 * step_size):
print("Reached Goal!")
print("Current node:---")
curr_node.printNode()
print("Goal node:---")
goal_node.printNode()
# backtrack to get the path
path = actions.backtrack(curr_node, visited)
# path = None # FOR NOW FOR DEBUGGING PURPOSES
return (path, viz_visited_coords)
# for row_step, col_step in movement_steps:
for angle in range(-60, 61, theta):
# Action Move
# next_node = actions.actionMove(curr_node, row_step, col_step)
next_node = actions.actionMove(
current_node=curr_node,
theta_step=angle,
linear_step=step_size,
goal_position=goal_node.current_coords)
if next_node is not None:
# if hit an obstacle, ignore this movement
if obstacles.withinObstacleSpace(
(next_node.current_coords[1], next_node.current_coords[0]),
robot_radius, clearance):
continue
# Check if the current node has already been visited.
# If it has, then see if the current path is better than the previous one
# based on the total cost = movement cost + goal cost
node_state = (utils.valRound(next_node.current_coords[0]),
utils.valRound(next_node.current_coords[1]),
utils.orientationBin(next_node.orientation,
theta))
if node_state in visited:
# if current cost is a better cost
# if (next_node.movement_cost + next_node.goal_cost) < (visited[node_state].movement_cost + visited[node_state].goal_cost):
if (next_node < visited[node_state]):
visited[
node_state].current_coords = next_node.current_coords
visited[
node_state].parent_coords = next_node.parent_coords
visited[node_state].orientation = next_node.orientation
visited[
node_state].parent_orientation = next_node.parent_orientation
visited[
node_state].movement_cost = next_node.movement_cost
visited[node_state].goal_cost = next_node.goal_cost
h_idx = utils.findInHeap(next_node, minheap)
if (h_idx > -1):
minheap[h_idx] = ((next_node.movement_cost +
next_node.goal_cost), next_node)
else:
# visited.append(next_node)
visited[node_state] = next_node
heapq.heappush(minheap, ((next_node.movement_cost +
next_node.goal_cost), next_node))
viz_visited_coords.append(next_node)
heapq.heapify(minheap)