diff --git a/PathPlanning/InformedRRTStar/informed_rrt_star.py b/PathPlanning/InformedRRTStar/informed_rrt_star.py
index 289e33fe4b1..4f0ec31988b 100644
--- a/PathPlanning/InformedRRTStar/informed_rrt_star.py
+++ b/PathPlanning/InformedRRTStar/informed_rrt_star.py
@@ -11,6 +11,7 @@
"""
import sys
import pathlib
+
sys.path.append(str(pathlib.Path(__file__).parent.parent.parent))
import copy
@@ -27,18 +28,17 @@
class InformedRRTStar:
- def __init__(self, start, goal,
- obstacleList, randArea,
- expandDis=0.5, goalSampleRate=10, maxIter=200):
+ def __init__(self, start, goal, obstacle_list, rand_area, expand_dis=0.5,
+ goal_sample_rate=10, max_iter=200):
self.start = Node(start[0], start[1])
self.goal = Node(goal[0], goal[1])
- self.min_rand = randArea[0]
- self.max_rand = randArea[1]
- self.expand_dis = expandDis
- self.goal_sample_rate = goalSampleRate
- self.max_iter = maxIter
- self.obstacle_list = obstacleList
+ self.min_rand = rand_area[0]
+ self.max_rand = rand_area[1]
+ self.expand_dis = expand_dis
+ self.goal_sample_rate = goal_sample_rate
+ self.max_iter = max_iter
+ self.obstacle_list = obstacle_list
self.node_list = None
def informed_rrt_star_search(self, animation=True):
@@ -46,110 +46,109 @@ def informed_rrt_star_search(self, animation=True):
self.node_list = [self.start]
# max length we expect to find in our 'informed' sample space,
# starts as infinite
- cBest = float('inf')
- solutionSet = set()
+ c_best = float('inf')
+ solution_set = set()
path = None
# Computing the sampling space
- cMin = math.sqrt(pow(self.start.x - self.goal.x, 2)
- + pow(self.start.y - self.goal.y, 2))
- xCenter = np.array([[(self.start.x + self.goal.x) / 2.0],
- [(self.start.y + self.goal.y) / 2.0], [0]])
- a1 = np.array([[(self.goal.x - self.start.x) / cMin],
- [(self.goal.y - self.start.y) / cMin], [0]])
+ c_min = math.hypot(self.start.x - self.goal.x,
+ self.start.y - self.goal.y)
+ x_center = np.array([[(self.start.x + self.goal.x) / 2.0],
+ [(self.start.y + self.goal.y) / 2.0], [0]])
+ a1 = np.array([[(self.goal.x - self.start.x) / c_min],
+ [(self.goal.y - self.start.y) / c_min], [0]])
e_theta = math.atan2(a1[1], a1[0])
# first column of identity matrix transposed
id1_t = np.array([1.0, 0.0, 0.0]).reshape(1, 3)
- M = a1 @ id1_t
- U, S, Vh = np.linalg.svd(M, True, True)
- C = np.dot(np.dot(U, np.diag(
- [1.0, 1.0, np.linalg.det(U) * np.linalg.det(np.transpose(Vh))])),
- Vh)
+ m = a1 @ id1_t
+ u, s, vh = np.linalg.svd(m, True, True)
+ c = u @ np.diag(
+ [1.0, 1.0,
+ np.linalg.det(u) * np.linalg.det(np.transpose(vh))]) @ vh
for i in range(self.max_iter):
- # Sample space is defined by cBest
- # cMin is the minimum distance between the start point and the goal
- # xCenter is the midpoint between the start and the goal
- # cBest changes when a new path is found
+ # Sample space is defined by c_best
+ # c_min is the minimum distance between the start point and
+ # the goal x_center is the midpoint between the start and the
+ # goal c_best changes when a new path is found
- rnd = self.informed_sample(cBest, cMin, xCenter, C)
+ rnd = self.informed_sample(c_best, c_min, x_center, c)
n_ind = self.get_nearest_list_index(self.node_list, rnd)
- nearestNode = self.node_list[n_ind]
+ nearest_node = self.node_list[n_ind]
# steer
- theta = math.atan2(rnd[1] - nearestNode.y, rnd[0] - nearestNode.x)
- newNode = self.get_new_node(theta, n_ind, nearestNode)
- d = self.line_cost(nearestNode, newNode)
+ theta = math.atan2(rnd[1] - nearest_node.y,
+ rnd[0] - nearest_node.x)
+ new_node = self.get_new_node(theta, n_ind, nearest_node)
+ d = self.line_cost(nearest_node, new_node)
- noCollision = self.check_collision(nearestNode, theta, d)
+ no_collision = self.check_collision(nearest_node, theta, d)
- if noCollision:
- nearInds = self.find_near_nodes(newNode)
- newNode = self.choose_parent(newNode, nearInds)
+ if no_collision:
+ near_inds = self.find_near_nodes(new_node)
+ new_node = self.choose_parent(new_node, near_inds)
- self.node_list.append(newNode)
- self.rewire(newNode, nearInds)
+ self.node_list.append(new_node)
+ self.rewire(new_node, near_inds)
- if self.is_near_goal(newNode):
- if self.check_segment_collision(newNode.x, newNode.y,
+ if self.is_near_goal(new_node):
+ if self.check_segment_collision(new_node.x, new_node.y,
self.goal.x, self.goal.y):
- solutionSet.add(newNode)
- lastIndex = len(self.node_list) - 1
- tempPath = self.get_final_course(lastIndex)
- tempPathLen = self.get_path_len(tempPath)
- if tempPathLen < cBest:
- path = tempPath
- cBest = tempPathLen
+ solution_set.add(new_node)
+ last_index = len(self.node_list) - 1
+ temp_path = self.get_final_course(last_index)
+ temp_path_len = self.get_path_len(temp_path)
+ if temp_path_len < c_best:
+ path = temp_path
+ c_best = temp_path_len
if animation:
- self.draw_graph(xCenter=xCenter,
- cBest=cBest, cMin=cMin,
+ self.draw_graph(x_center=x_center, c_best=c_best, c_min=c_min,
e_theta=e_theta, rnd=rnd)
return path
- def choose_parent(self, newNode, nearInds):
- if len(nearInds) == 0:
- return newNode
+ def choose_parent(self, new_node, near_inds):
+ if len(near_inds) == 0:
+ return new_node
- dList = []
- for i in nearInds:
- dx = newNode.x - self.node_list[i].x
- dy = newNode.y - self.node_list[i].y
+ d_list = []
+ for i in near_inds:
+ dx = new_node.x - self.node_list[i].x
+ dy = new_node.y - self.node_list[i].y
d = math.hypot(dx, dy)
theta = math.atan2(dy, dx)
if self.check_collision(self.node_list[i], theta, d):
- dList.append(self.node_list[i].cost + d)
+ d_list.append(self.node_list[i].cost + d)
else:
- dList.append(float('inf'))
+ d_list.append(float('inf'))
- minCost = min(dList)
- minInd = nearInds[dList.index(minCost)]
+ min_cost = min(d_list)
+ min_ind = near_inds[d_list.index(min_cost)]
- if minCost == float('inf'):
+ if min_cost == float('inf'):
print("min cost is inf")
- return newNode
+ return new_node
- newNode.cost = minCost
- newNode.parent = minInd
+ new_node.cost = min_cost
+ new_node.parent = min_ind
- return newNode
+ return new_node
- def find_near_nodes(self, newNode):
+ def find_near_nodes(self, new_node):
n_node = len(self.node_list)
r = 50.0 * math.sqrt((math.log(n_node) / n_node))
- d_list = [(node.x - newNode.x) ** 2 + (node.y - newNode.y) ** 2
- for node in self.node_list]
+ d_list = [(node.x - new_node.x) ** 2 + (node.y - new_node.y) ** 2 for
+ node in self.node_list]
near_inds = [d_list.index(i) for i in d_list if i <= r ** 2]
return near_inds
- def informed_sample(self, cMax, cMin, xCenter, C):
- if cMax < float('inf'):
- r = [cMax / 2.0,
- math.sqrt(cMax ** 2 - cMin ** 2) / 2.0,
- math.sqrt(cMax ** 2 - cMin ** 2) / 2.0]
- L = np.diag(r)
- xBall = self.sample_unit_ball()
- rnd = np.dot(np.dot(C, L), xBall) + xCenter
+ def informed_sample(self, c_max, c_min, x_center, c):
+ if c_max < float('inf'):
+ r = [c_max / 2.0, math.sqrt(c_max ** 2 - c_min ** 2) / 2.0,
+ math.sqrt(c_max ** 2 - c_min ** 2) / 2.0]
+ rl = np.diag(r)
+ x_ball = self.sample_unit_ball()
+ rnd = np.dot(np.dot(c, rl), x_ball) + x_center
rnd = [rnd[(0, 0)], rnd[(1, 0)]]
else:
rnd = self.sample_free_space()
@@ -179,16 +178,15 @@ def sample_free_space(self):
@staticmethod
def get_path_len(path):
- pathLen = 0
+ path_len = 0
for i in range(1, len(path)):
node1_x = path[i][0]
node1_y = path[i][1]
node2_x = path[i - 1][0]
node2_y = path[i - 1][1]
- pathLen += math.sqrt((node1_x - node2_x)
- ** 2 + (node1_y - node2_y) ** 2)
+ path_len += math.hypot(node1_x - node2_x, node1_y - node2_y)
- return pathLen
+ return path_len
@staticmethod
def line_cost(node1, node2):
@@ -196,20 +194,20 @@ def line_cost(node1, node2):
@staticmethod
def get_nearest_list_index(nodes, rnd):
- dList = [(node.x - rnd[0]) ** 2
- + (node.y - rnd[1]) ** 2 for node in nodes]
- minIndex = dList.index(min(dList))
- return minIndex
+ d_list = [(node.x - rnd[0]) ** 2 + (node.y - rnd[1]) ** 2 for node in
+ nodes]
+ min_index = d_list.index(min(d_list))
+ return min_index
- def get_new_node(self, theta, n_ind, nearestNode):
- newNode = copy.deepcopy(nearestNode)
+ def get_new_node(self, theta, n_ind, nearest_node):
+ new_node = copy.deepcopy(nearest_node)
- newNode.x += self.expand_dis * math.cos(theta)
- newNode.y += self.expand_dis * math.sin(theta)
+ new_node.x += self.expand_dis * math.cos(theta)
+ new_node.y += self.expand_dis * math.sin(theta)
- newNode.cost += self.expand_dis
- newNode.parent = n_ind
- return newNode
+ new_node.cost += self.expand_dis
+ new_node.parent = n_ind
+ return new_node
def is_near_goal(self, node):
d = self.line_cost(node, self.goal)
@@ -217,21 +215,21 @@ def is_near_goal(self, node):
return True
return False
- def rewire(self, newNode, nearInds):
+ def rewire(self, new_node, near_inds):
n_node = len(self.node_list)
- for i in nearInds:
- nearNode = self.node_list[i]
+ for i in near_inds:
+ near_node = self.node_list[i]
- d = math.hypot(nearNode.x - newNode.x, nearNode.y - newNode.y)
+ d = math.hypot(near_node.x - new_node.x, near_node.y - new_node.y)
- s_cost = newNode.cost + d
+ s_cost = new_node.cost + d
- if nearNode.cost > s_cost:
- theta = math.atan2(newNode.y - nearNode.y,
- newNode.x - nearNode.x)
- if self.check_collision(nearNode, theta, d):
- nearNode.parent = n_node - 1
- nearNode.cost = s_cost
+ if near_node.cost > s_cost:
+ theta = math.atan2(new_node.y - near_node.y,
+ new_node.x - near_node.x)
+ if self.check_collision(near_node, theta, d):
+ near_node.parent = n_node - 1
+ near_node.cost = s_cost
@staticmethod
def distance_squared_point_to_segment(v, w, p):
@@ -251,45 +249,44 @@ def distance_squared_point_to_segment(v, w, p):
def check_segment_collision(self, x1, y1, x2, y2):
for (ox, oy, size) in self.obstacle_list:
dd = self.distance_squared_point_to_segment(
- np.array([x1, y1]),
- np.array([x2, y2]),
- np.array([ox, oy]))
+ np.array([x1, y1]), np.array([x2, y2]), np.array([ox, oy]))
if dd <= size ** 2:
return False # collision
return True
- def check_collision(self, nearNode, theta, d):
- tmpNode = copy.deepcopy(nearNode)
- end_x = tmpNode.x + math.cos(theta) * d
- end_y = tmpNode.y + math.sin(theta) * d
- return self.check_segment_collision(tmpNode.x, tmpNode.y, end_x, end_y)
+ def check_collision(self, near_node, theta, d):
+ tmp_node = copy.deepcopy(near_node)
+ end_x = tmp_node.x + math.cos(theta) * d
+ end_y = tmp_node.y + math.sin(theta) * d
+ return self.check_segment_collision(tmp_node.x, tmp_node.y,
+ end_x, end_y)
- def get_final_course(self, lastIndex):
+ def get_final_course(self, last_index):
path = [[self.goal.x, self.goal.y]]
- while self.node_list[lastIndex].parent is not None:
- node = self.node_list[lastIndex]
+ while self.node_list[last_index].parent is not None:
+ node = self.node_list[last_index]
path.append([node.x, node.y])
- lastIndex = node.parent
+ last_index = node.parent
path.append([self.start.x, self.start.y])
return path
- def draw_graph(self, xCenter=None, cBest=None, cMin=None, e_theta=None,
+ def draw_graph(self, x_center=None, c_best=None, c_min=None, e_theta=None,
rnd=None):
plt.clf()
# for stopping simulation with the esc key.
plt.gcf().canvas.mpl_connect(
- 'key_release_event',
- lambda event: [exit(0) if event.key == 'escape' else None])
+ 'key_release_event', lambda event:
+ [exit(0) if event.key == 'escape' else None])
if rnd is not None:
plt.plot(rnd[0], rnd[1], "^k")
- if cBest != float('inf'):
- self.plot_ellipse(xCenter, cBest, cMin, e_theta)
+ if c_best != float('inf'):
+ self.plot_ellipse(x_center, c_best, c_min, e_theta)
for node in self.node_list:
if node.parent is not None:
if node.x or node.y is not None:
- plt.plot([node.x, self.node_list[node.parent].x], [
- node.y, self.node_list[node.parent].y], "-g")
+ plt.plot([node.x, self.node_list[node.parent].x],
+ [node.y, self.node_list[node.parent].y], "-g")
for (ox, oy, size) in self.obstacle_list:
plt.plot(ox, oy, "ok", ms=30 * size)
@@ -301,13 +298,13 @@ def draw_graph(self, xCenter=None, cBest=None, cMin=None, e_theta=None,
plt.pause(0.01)
@staticmethod
- def plot_ellipse(xCenter, cBest, cMin, e_theta): # pragma: no cover
+ def plot_ellipse(x_center, c_best, c_min, e_theta): # pragma: no cover
- a = math.sqrt(cBest ** 2 - cMin ** 2) / 2.0
- b = cBest / 2.0
+ a = math.sqrt(c_best ** 2 - c_min ** 2) / 2.0
+ b = c_best / 2.0
angle = math.pi / 2.0 - e_theta
- cx = xCenter[0]
- cy = xCenter[1]
+ cx = x_center[0]
+ cy = x_center[1]
t = np.arange(0, 2 * math.pi + 0.1, 0.1)
x = [a * math.cos(it) for it in t]
y = [b * math.sin(it) for it in t]
@@ -331,18 +328,12 @@ def main():
print("Start informed rrt star planning")
# create obstacles
- obstacleList = [
- (5, 5, 0.5),
- (9, 6, 1),
- (7, 5, 1),
- (1, 5, 1),
- (3, 6, 1),
- (7, 9, 1)
- ]
+ obstacle_list = [(5, 5, 0.5), (9, 6, 1), (7, 5, 1), (1, 5, 1), (3, 6, 1),
+ (7, 9, 1)]
# Set params
- rrt = InformedRRTStar(start=[0, 0], goal=[5, 10],
- randArea=[-2, 15], obstacleList=obstacleList)
+ rrt = InformedRRTStar(start=[0, 0], goal=[5, 10], rand_area=[-2, 15],
+ obstacle_list=obstacle_list)
path = rrt.informed_rrt_star_search(animation=show_animation)
print("Done!!")