clean up informed_rrt_star.py (AtsushiSakai#785)

* clean up informed_rrt_star.py

* clean up informed_rrt_star.py

Author: AtsushiSakai

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,129 +28,127 @@
 
 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):
 
         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,59 +178,58 @@ 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):
         return math.hypot(node1.x - node2.x, node1.y - node2.y)
 
     @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)
         if d < self.expand_dis:
             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!!")