From 76977ac2564f1bae8d6dddae2c6fe454fddf232a Mon Sep 17 00:00:00 2001 From: Atsushi Sakai Date: Sun, 28 Jul 2019 19:50:02 +0900 Subject: [PATCH] cleanup closed loop RRT star. --- .../closed_loop_rrt_star_car.py | 325 +++--------------- PathPlanning/LQRRRTStar/lqr_rrt_star.py | 2 +- PathPlanning/RRT/rrt.py | 4 +- PathPlanning/RRTDubins/rrt_dubins.py | 2 +- PathPlanning/RRTStar/rrt_star.py | 2 +- PathPlanning/RRTStarDubins/rrt_star_dubins.py | 2 +- .../RRTStarReedsShepp/rrt_star_reeds_shepp.py | 50 +-- 7 files changed, 87 insertions(+), 300 deletions(-) diff --git a/PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py b/PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py index 2aa0ba61..a07dc48a 100644 --- a/PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py +++ b/PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py @@ -1,14 +1,13 @@ """ + Path planning Sample Code with Closed loop RRT for car like robot. author: AtsushiSakai(@Atsushi_twi) """ -import copy import math import os -import random import sys import matplotlib.pyplot as plt @@ -18,92 +17,49 @@ import pure_pursuit sys.path.append(os.path.dirname( os.path.abspath(__file__)) + "/../ReedsSheppPath/") +sys.path.append(os.path.dirname( + os.path.abspath(__file__)) + "/../RRTStarReedsShepp/") try: import reeds_shepp_path_planning import unicycle_model -except: + from rrt_star_reeds_shepp import RRTStarReedsShepp +except ImportError: raise show_animation = True -target_speed = 10.0 / 3.6 -STEP_SIZE = 0.1 - - -class RRT(): +class ClosedLoopRRTStar(RRTStarReedsShepp): """ - Class for RRT planning + Class for Closed loop RRT star planning """ - def __init__(self, start, goal, obstacleList, randArea, - maxIter=50): + def __init__(self, start, goal, obstacle_list, rand_area, + max_iter=200, + connect_circle_dist=50.0 + ): + super().__init__(start, goal, obstacle_list, rand_area, + max_iter=max_iter, + connect_circle_dist=connect_circle_dist, + ) + + self.target_speed = 10.0 / 3.6 + self.yaw_th = np.deg2rad(3.0) + self.xy_th = 0.5 + self.invalid_travel_ratio = 5.0 + + def planning(self, animation=True): """ - Setting Parameter - - start:Start Position [x,y] - goal:Goal Position [x,y] - obstacleList:obstacle Positions [[x,y,size],...] - randArea:Ramdom Samping Area [min,max] - - """ - self.start = Node(start[0], start[1], start[2]) - self.end = Node(goal[0], goal[1], goal[2]) - self.minrand = randArea[0] - self.maxrand = randArea[1] - self.obstacleList = obstacleList - self.maxIter = maxIter - - def try_goal_path(self): - - goal = Node(self.end.x, self.end.y, self.end.yaw) - - newNode = self.steer(goal, len(self.nodeList) - 1) - if newNode is None: - return - - if self.CollisionCheck(newNode, self.obstacleList): - # print("goal path is OK") - self.nodeList.append(newNode) - - def Planning(self, animation=True): - """ - Pathplanning + do planning animation: flag for animation on or off """ - - self.nodeList = [self.start] - - self.try_goal_path() - - for i in range(self.maxIter): - print("loop:", i) - rnd = self.get_random_point() - nind = self.GetNearestListIndex(self.nodeList, rnd) - - newNode = self.steer(rnd, nind) - # print(newNode.cost) - if newNode is None: - continue - - if self.CollisionCheck(newNode, self.obstacleList): - nearinds = self.find_near_nodes(newNode) - newNode = self.choose_parent(newNode, nearinds) - if newNode is None: - continue - - self.nodeList.append(newNode) - self.rewire(newNode, nearinds) - - self.try_goal_path() - - if animation and i % 5 == 0: - self.DrawGraph(rnd=rnd) + # planning with RRTStarReedsShepp + super().planning(animation=animation) # generate coruse - path_indexs = self.get_best_last_indexs() + path_indexs = self.get_goal_indexes() flag, x, y, yaw, v, t, a, d = self.search_best_feasible_path( path_indexs) @@ -120,10 +76,9 @@ class RRT(): # pure pursuit tracking for ind in path_indexs: - path = self.gen_final_course(ind) + path = self.generate_final_course(ind) - flag, x, y, yaw, v, t, a, d = self.check_tracking_path_is_feasible( - path) + flag, x, y, yaw, v, t, a, d = self.check_tracking_path_is_feasible(path) if flag and best_time >= t[-1]: print("feasible path is found") @@ -142,121 +97,46 @@ class RRT(): return False, None, None, None, None, None, None, None def check_tracking_path_is_feasible(self, path): - # print("check_tracking_path_is_feasible") - cx = np.array(path[:, 0]) - cy = np.array(path[:, 1]) - cyaw = np.array(path[:, 2]) + cx = np.array([state[0] for state in path])[::-1] + cy = np.array([state[1] for state in path])[::-1] + cyaw = np.array([state[2] for state in path])[::-1] goal = [cx[-1], cy[-1], cyaw[-1]] cx, cy, cyaw = pure_pursuit.extend_path(cx, cy, cyaw) speed_profile = pure_pursuit.calc_speed_profile( - cx, cy, cyaw, target_speed) + cx, cy, cyaw, self.target_speed) t, x, y, yaw, v, a, d, find_goal = pure_pursuit.closed_loop_prediction( cx, cy, cyaw, speed_profile, goal) - yaw = [self.pi_2_pi(iyaw) for iyaw in yaw] + yaw = [reeds_shepp_path_planning.pi_2_pi(iyaw) for iyaw in yaw] if not find_goal: print("cannot reach goal") - if abs(yaw[-1] - goal[2]) >= math.pi / 4.0: + if abs(yaw[-1] - goal[2]) >= self.yaw_th * 10.0: print("final angle is bad") find_goal = False travel = sum([abs(iv) * unicycle_model.dt for iv in v]) - # print(travel) origin_travel = sum([math.sqrt(dx ** 2 + dy ** 2) for (dx, dy) in zip(np.diff(cx), np.diff(cy))]) - # print(origin_travel) - if (travel / origin_travel) >= 5.0: + if (travel / origin_travel) >= self.invalid_travel_ratio: print("path is too long") find_goal = False - if not self.CollisionCheckWithXY(x, y, self.obstacleList): + if not self.collision_check_with_xy(x, y, self.obstacle_list): print("This path is collision") find_goal = False return find_goal, x, y, yaw, v, t, a, d - def choose_parent(self, newNode, nearinds): - if not nearinds: - return newNode - - dlist = [] - for i in nearinds: - tNode = self.steer(newNode, i) - if tNode is None: - continue - - if self.CollisionCheck(tNode, self.obstacleList): - dlist.append(tNode.cost) - else: - dlist.append(float("inf")) - - mincost = min(dlist) - minind = nearinds[dlist.index(mincost)] - - if mincost == float("inf"): - print("mincost is inf") - return newNode - - newNode = self.steer(newNode, minind) - if newNode is None: - return None - - return newNode - - def pi_2_pi(self, angle): - return (angle + math.pi) % (2 * math.pi) - math.pi - - def steer(self, rnd, nind): - # print(rnd) - - nearestNode = self.nodeList[nind] - - px, py, pyaw, mode, clen = reeds_shepp_path_planning.reeds_shepp_path_planning( - nearestNode.x, nearestNode.y, nearestNode.yaw, - rnd.x, rnd.y, rnd.yaw, unicycle_model.curvature_max, STEP_SIZE) - - if px is None: - return None - - newNode = copy.deepcopy(nearestNode) - newNode.x = px[-1] - newNode.y = py[-1] - newNode.yaw = pyaw[-1] - - newNode.path_x = px - newNode.path_y = py - newNode.path_yaw = pyaw - newNode.cost += sum([abs(c) for c in clen]) - newNode.parent = nind - - return newNode - - def get_random_point(self): - - rnd = [random.uniform(self.minrand, self.maxrand), - random.uniform(self.minrand, self.maxrand), - random.uniform(-math.pi, math.pi) - ] - - node = Node(rnd[0], rnd[1], rnd[2]) - - return node - - def get_best_last_indexs(self): - # print("search_finish_node") - - YAWTH = np.deg2rad(1.0) - XYTH = 0.5 - + def get_goal_indexes(self): goalinds = [] - for (i, node) in enumerate(self.nodeList): - if self.calc_dist_to_goal(node.x, node.y) <= XYTH: + for (i, node) in enumerate(self.node_list): + if self.calc_dist_to_goal(node.x, node.y) <= self.xy_th: goalinds.append(i) print("OK XY TH num is") print(len(goalinds)) @@ -264,106 +144,17 @@ class RRT(): # angle check fgoalinds = [] for i in goalinds: - if abs(self.nodeList[i].yaw - self.end.yaw) <= YAWTH: + if abs(self.node_list[i].yaw - self.end.yaw) <= self.yaw_th: fgoalinds.append(i) print("OK YAW TH num is") print(len(fgoalinds)) return fgoalinds - def gen_final_course(self, goalind): - path = [[self.end.x, self.end.y, self.end.yaw]] - while self.nodeList[goalind].parent is not None: - node = self.nodeList[goalind] - path_x = reversed(node.path_x) - path_y = reversed(node.path_y) - path_yaw = reversed(node.path_yaw) - for (ix, iy, iyaw) in zip(path_x, path_y, path_yaw): - path.append([ix, iy, iyaw]) - # path.append([node.x, node.y]) - goalind = node.parent - path.append([self.start.x, self.start.y, self.start.yaw]) + @staticmethod + def collision_check_with_xy(x, y, obstacle_list): - path = np.array(path[::-1]) - return path - - def calc_dist_to_goal(self, x, y): - return np.linalg.norm([x - self.end.x, y - self.end.y]) - - def find_near_nodes(self, newNode): - nnode = len(self.nodeList) - r = 50.0 * math.sqrt((math.log(nnode) / nnode)) - # r = self.expandDis * 5.0 - dlist = [(node.x - newNode.x) ** 2 - + (node.y - newNode.y) ** 2 - + (node.yaw - newNode.yaw) ** 2 - for node in self.nodeList] - nearinds = [dlist.index(i) for i in dlist if i <= r ** 2] - return nearinds - - def rewire(self, newNode, nearinds): - - nnode = len(self.nodeList) - - for i in nearinds: - nearNode = self.nodeList[i] - tNode = self.steer(nearNode, nnode - 1) - - if tNode is None: - continue - - obstacleOK = self.CollisionCheck(tNode, self.obstacleList) - imporveCost = nearNode.cost > tNode.cost - - if obstacleOK and imporveCost: - # print("rewire") - self.nodeList[i] = tNode - - def DrawGraph(self, rnd=None): # pragma: no cover - """ - Draw Graph - """ - if rnd is not None: - plt.plot(rnd.x, rnd.y, "^k") - for node in self.nodeList: - if node.parent is not None: - plt.plot(node.path_x, node.path_y, "-g") - - for (ox, oy, size) in self.obstacleList: - plt.plot(ox, oy, "ok", ms=30 * size) - - reeds_shepp_path_planning.plot_arrow( - self.start.x, self.start.y, self.start.yaw) - reeds_shepp_path_planning.plot_arrow( - self.end.x, self.end.y, self.end.yaw) - - plt.axis([-2, 15, -2, 15]) - plt.grid(True) - plt.pause(0.01) - - def GetNearestListIndex(self, nodeList, rnd): - dlist = [(node.x - rnd.x) ** 2 - + (node.y - rnd.y) ** 2 - + (node.yaw - rnd.yaw) ** 2 for node in nodeList] - minind = dlist.index(min(dlist)) - - return minind - - def CollisionCheck(self, node, obstacleList): - - for (ox, oy, size) in obstacleList: - for (ix, iy) in zip(node.path_x, node.path_y): - dx = ox - ix - dy = oy - iy - d = dx * dx + dy * dy - if d <= size ** 2: - return False # collision - - return True # safe - - def CollisionCheckWithXY(self, x, y, obstacleList): - - for (ox, oy, size) in obstacleList: + for (ox, oy, size) in obstacle_list: for (ix, iy) in zip(x, y): dx = ox - ix dy = oy - iy @@ -374,26 +165,10 @@ class RRT(): return True # safe -class Node(): - """ - RRT Node - """ - - def __init__(self, x, y, yaw): - self.x = x - self.y = y - self.yaw = yaw - self.path_x = [] - self.path_y = [] - self.path_yaw = [] - self.cost = 0.0 - self.parent = None - - -def main(gx=6.0, gy=7.0, gyaw=np.deg2rad(90.0), maxIter=200): +def main(gx=6.0, gy=7.0, gyaw=np.deg2rad(90.0), max_iter=100): print("Start" + __file__) # ====Search Path with RRT==== - obstacleList = [ + obstacle_list = [ (5, 5, 1), (4, 6, 1), (4, 8, 1), @@ -409,16 +184,18 @@ def main(gx=6.0, gy=7.0, gyaw=np.deg2rad(90.0), maxIter=200): start = [0.0, 0.0, np.deg2rad(0.0)] goal = [gx, gy, gyaw] - rrt = RRT(start, goal, randArea=[-2.0, 20.0], - obstacleList=obstacleList, maxIter=maxIter) - flag, x, y, yaw, v, t, a, d = rrt.Planning(animation=show_animation) + closed_loop_rrt_star = ClosedLoopRRTStar(start, goal, + obstacle_list, + [-2.0, 20.0], + max_iter=max_iter) + flag, x, y, yaw, v, t, a, d = closed_loop_rrt_star.planning(animation=show_animation) if not flag: print("cannot find feasible path") # Draw final path if show_animation: - rrt.DrawGraph() + closed_loop_rrt_star.draw_graph() plt.plot(x, y, '-r') plt.grid(True) plt.pause(0.001) diff --git a/PathPlanning/LQRRRTStar/lqr_rrt_star.py b/PathPlanning/LQRRRTStar/lqr_rrt_star.py index c6cd760e..2e276b9f 100644 --- a/PathPlanning/LQRRRTStar/lqr_rrt_star.py +++ b/PathPlanning/LQRRRTStar/lqr_rrt_star.py @@ -72,7 +72,7 @@ class LQRRRTStar(RRTStar): for i in range(self.max_iter): print("Iter:", i, ", number of nodes:", len(self.node_list)) rnd = self.get_random_node() - nearest_ind = self.get_nearest_list_index(self.node_list, rnd) + nearest_ind = self.get_nearest_node_index(self.node_list, rnd) new_node = self.steer(self.node_list[nearest_ind], rnd) if self.check_collision(new_node, self.obstacle_list): diff --git a/PathPlanning/RRT/rrt.py b/PathPlanning/RRT/rrt.py index 541c2a96..887cb154 100644 --- a/PathPlanning/RRT/rrt.py +++ b/PathPlanning/RRT/rrt.py @@ -63,7 +63,7 @@ class RRT: self.node_list = [self.start] for i in range(self.max_iter): rnd_node = self.get_random_node() - nearest_ind = self.get_nearest_list_index(self.node_list, rnd_node) + nearest_ind = self.get_nearest_node_index(self.node_list, rnd_node) nearest_node = self.node_list[nearest_ind] new_node = self.steer(nearest_node, rnd_node, self.expand_dis) @@ -154,7 +154,7 @@ class RRT: plt.pause(0.01) @staticmethod - def get_nearest_list_index(node_list, rnd_node): + def get_nearest_node_index(node_list, rnd_node): dlist = [(node.x - rnd_node.x) ** 2 + (node.y - rnd_node.y) ** 2 for node in node_list] minind = dlist.index(min(dlist)) diff --git a/PathPlanning/RRTDubins/rrt_dubins.py b/PathPlanning/RRTDubins/rrt_dubins.py index 78ce9611..ed2b27ca 100644 --- a/PathPlanning/RRTDubins/rrt_dubins.py +++ b/PathPlanning/RRTDubins/rrt_dubins.py @@ -80,7 +80,7 @@ class RRTDubins(RRT): for i in range(self.max_iter): print("Iter:", i, ", number of nodes:", len(self.node_list)) rnd = self.get_random_node() - nearest_ind = self.get_nearest_list_index(self.node_list, rnd) + nearest_ind = self.get_nearest_node_index(self.node_list, rnd) new_node = self.steer(self.node_list[nearest_ind], rnd) if self.check_collision(new_node, self.obstacle_list): diff --git a/PathPlanning/RRTStar/rrt_star.py b/PathPlanning/RRTStar/rrt_star.py index c92f4350..eb37964d 100644 --- a/PathPlanning/RRTStar/rrt_star.py +++ b/PathPlanning/RRTStar/rrt_star.py @@ -65,7 +65,7 @@ class RRTStar(RRT): for i in range(self.max_iter): print("Iter:", i, ", number of nodes:", len(self.node_list)) rnd = self.get_random_node() - nearest_ind = self.get_nearest_list_index(self.node_list, rnd) + nearest_ind = self.get_nearest_node_index(self.node_list, rnd) new_node = self.steer(self.node_list[nearest_ind], rnd, self.expand_dis) if self.check_collision(new_node, self.obstacle_list): diff --git a/PathPlanning/RRTStarDubins/rrt_star_dubins.py b/PathPlanning/RRTStarDubins/rrt_star_dubins.py index b98c7dca..4d731293 100644 --- a/PathPlanning/RRTStarDubins/rrt_star_dubins.py +++ b/PathPlanning/RRTStarDubins/rrt_star_dubins.py @@ -81,7 +81,7 @@ class RRTStarDubins(RRTStar): for i in range(self.max_iter): print("Iter:", i, ", number of nodes:", len(self.node_list)) rnd = self.get_random_node() - nearest_ind = self.get_nearest_list_index(self.node_list, rnd) + nearest_ind = self.get_nearest_node_index(self.node_list, rnd) new_node = self.steer(self.node_list[nearest_ind], rnd) if self.check_collision(new_node, self.obstacle_list): diff --git a/PathPlanning/RRTStarReedsShepp/rrt_star_reeds_shepp.py b/PathPlanning/RRTStarReedsShepp/rrt_star_reeds_shepp.py index ea31337c..5d78f98f 100644 --- a/PathPlanning/RRTStarReedsShepp/rrt_star_reeds_shepp.py +++ b/PathPlanning/RRTStarReedsShepp/rrt_star_reeds_shepp.py @@ -44,7 +44,6 @@ class RRTStarReedsShepp(RRTStar): self.path_yaw = [] def __init__(self, start, goal, obstacle_list, rand_area, - goal_sample_rate=10, max_iter=200, connect_circle_dist=50.0 ): @@ -61,7 +60,6 @@ class RRTStarReedsShepp(RRTStar): self.end = self.Node(goal[0], goal[1], goal[2]) self.min_rand = rand_area[0] self.max_rand = rand_area[1] - self.goal_sample_rate = goal_sample_rate self.max_iter = max_iter self.obstacle_list = obstacle_list self.connect_circle_dist = connect_circle_dist @@ -72,7 +70,7 @@ class RRTStarReedsShepp(RRTStar): def planning(self, animation=True, search_until_max_iter=True): """ - RRT Star planning + planning animation: flag for animation on or off """ @@ -81,7 +79,7 @@ class RRTStarReedsShepp(RRTStar): for i in range(self.max_iter): print("Iter:", i, ", number of nodes:", len(self.node_list)) rnd = self.get_random_node() - nearest_ind = self.get_nearest_list_index(self.node_list, rnd) + nearest_ind = self.get_nearest_node_index(self.node_list, rnd) new_node = self.steer(self.node_list[nearest_ind], rnd) if self.check_collision(new_node, self.obstacle_list): @@ -90,6 +88,7 @@ class RRTStarReedsShepp(RRTStar): if new_node: self.node_list.append(new_node) self.rewire(new_node, near_indexes) + self.try_goal_path(new_node) if animation and i % 5 == 0: self.plot_start_goal_arrow() @@ -110,6 +109,17 @@ class RRTStarReedsShepp(RRTStar): return None + def try_goal_path(self, node): + + goal = self.Node(self.end.x, self.end.y, self.end.yaw) + + new_node = self.steer(node, goal) + if new_node is None: + return + + if self.check_collision(new_node, self.obstacle_list): + self.node_list.append(new_node) + def draw_graph(self, rnd=None): plt.clf() if rnd is not None: @@ -151,7 +161,7 @@ class RRTStarReedsShepp(RRTStar): new_node.path_x = px new_node.path_y = py new_node.path_yaw = pyaw - new_node.cost += sum(course_lengths) + new_node.cost += sum([abs(l) for l in course_lengths]) new_node.parent = from_node return new_node @@ -164,17 +174,14 @@ class RRTStarReedsShepp(RRTStar): if not course_lengths: return float("inf") - return from_node.cost + sum(course_lengths) + return from_node.cost + sum([abs(l) for l in course_lengths]) def get_random_node(self): - if random.randint(0, 100) > self.goal_sample_rate: - rnd = self.Node(random.uniform(self.min_rand, self.max_rand), - random.uniform(self.min_rand, self.max_rand), - random.uniform(-math.pi, math.pi) - ) - else: # goal point sampling - rnd = self.Node(self.end.x, self.end.y, self.end.yaw) + rnd = self.Node(random.uniform(self.min_rand, self.max_rand), + random.uniform(self.min_rand, self.max_rand), + random.uniform(-math.pi, math.pi) + ) return rnd @@ -184,6 +191,7 @@ class RRTStarReedsShepp(RRTStar): for (i, node) in enumerate(self.node_list): if self.calc_dist_to_goal(node.x, node.y) <= self.goal_xy_th: goal_indexes.append(i) + print("goal_indexes:", len(goal_indexes)) # angle check final_goal_indexes = [] @@ -191,10 +199,13 @@ class RRTStarReedsShepp(RRTStar): if abs(self.node_list[i].yaw - self.end.yaw) <= self.goal_yaw_th: final_goal_indexes.append(i) + print("final_goal_indexes:", len(final_goal_indexes)) + if not final_goal_indexes: return None min_cost = min([self.node_list[i].cost for i in final_goal_indexes]) + print("min_cost:", min_cost) for i in final_goal_indexes: if self.node_list[i].cost == min_cost: return i @@ -202,18 +213,17 @@ class RRTStarReedsShepp(RRTStar): return None def generate_final_course(self, goal_index): - print("final") - path = [[self.end.x, self.end.y]] + path = [[self.end.x, self.end.y, self.end.yaw]] node = self.node_list[goal_index] while node.parent: - for (ix, iy) in zip(reversed(node.path_x), reversed(node.path_y)): - path.append([ix, iy]) + for (ix, iy, iyaw) in zip(reversed(node.path_x), reversed(node.path_y), reversed(node.path_yaw)): + path.append([ix, iy, iyaw]) node = node.parent - path.append([self.start.x, self.start.y]) + path.append([self.start.x, self.start.y, self.start.yaw]) return path -def main(max_iter=200): +def main(max_iter=100): print("Start " + __file__) # ====Search Path with RRT==== @@ -241,7 +251,7 @@ def main(max_iter=200): # Draw final path if path and show_animation: # pragma: no cover rrt_star_reeds_shepp.draw_graph() - plt.plot([x for (x, y) in path], [y for (x, y) in path], '-r') + plt.plot([x for (x, y, yaw) in path], [y for (x, y, yaw) in path], '-r') plt.grid(True) plt.pause(0.001) plt.show()