diff --git a/PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py b/PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py index c0c8c356..dc9a5756 100644 --- a/PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py +++ b/PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py @@ -37,11 +37,13 @@ class ClosedLoopRRTStar(RRTStarReedsShepp): def __init__(self, start, goal, obstacle_list, rand_area, max_iter=200, - connect_circle_dist=50.0 + connect_circle_dist=50.0, + robot_radius=0.0 ): super().__init__(start, goal, obstacle_list, rand_area, max_iter=max_iter, connect_circle_dist=connect_circle_dist, + robot_radius=robot_radius ) self.target_speed = 10.0 / 3.6 @@ -127,7 +129,11 @@ class ClosedLoopRRTStar(RRTStarReedsShepp): print("path is too long") find_goal = False - if not self.collision_check_with_xy(x, y, self.obstacle_list): + tmp_node = self.Node(x, y, 0) + tmp_node.path_x = x + tmp_node.path_y = y + if not self.check_collision( + tmp_node, self.obstacle_list, self.robot_radius): print("This path is collision") find_goal = False @@ -151,19 +157,6 @@ class ClosedLoopRRTStar(RRTStarReedsShepp): return fgoalinds - @staticmethod - def collision_check_with_xy(x, y, obstacle_list): - - for (ox, oy, size) in obstacle_list: - for (ix, iy) in zip(x, y): - dx = ox - ix - dy = oy - iy - d = dx * dx + dy * dy - if d <= size ** 2: - return False # collision - - return True # safe - def main(gx=6.0, gy=7.0, gyaw=np.deg2rad(90.0), max_iter=100): print("Start" + __file__) diff --git a/PathPlanning/LQRRRTStar/lqr_rrt_star.py b/PathPlanning/LQRRRTStar/lqr_rrt_star.py index 177131ac..28d332aa 100644 --- a/PathPlanning/LQRRRTStar/lqr_rrt_star.py +++ b/PathPlanning/LQRRRTStar/lqr_rrt_star.py @@ -35,7 +35,8 @@ class LQRRRTStar(RRTStar): goal_sample_rate=10, max_iter=200, connect_circle_dist=50.0, - step_size=0.2 + step_size=0.2, + robot_radius=0.0, ): """ Setting Parameter @@ -44,6 +45,7 @@ class LQRRRTStar(RRTStar): goal:Goal Position [x,y] obstacleList:obstacle Positions [[x,y,size],...] randArea:Random Sampling Area [min,max] + robot_radius: robot body modeled as circle with given radius """ self.start = self.Node(start[0], start[1]) @@ -58,6 +60,7 @@ class LQRRRTStar(RRTStar): self.curvature = 1.0 self.goal_xy_th = 0.5 self.step_size = step_size + self.robot_radius = robot_radius self.lqr_planner = LQRPlanner() @@ -75,7 +78,8 @@ class LQRRRTStar(RRTStar): 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): + if self.check_collision( + new_node, self.obstacle_list, self.robot_radius): near_indexes = self.find_near_nodes(new_node) new_node = self.choose_parent(new_node, near_indexes) if new_node: diff --git a/PathPlanning/RRT/rrt.py b/PathPlanning/RRT/rrt.py index dfbd4b1b..55092e6e 100644 --- a/PathPlanning/RRT/rrt.py +++ b/PathPlanning/RRT/rrt.py @@ -50,7 +50,8 @@ class RRT: path_resolution=0.5, goal_sample_rate=5, max_iter=500, - play_area=None + play_area=None, + robot_radius=0.0, ): """ Setting Parameter @@ -60,6 +61,7 @@ class RRT: obstacleList:obstacle Positions [[x,y,size],...] randArea:Random Sampling Area [min,max] play_area:stay inside this area [xmin,xmax,ymin,ymax] + robot_radius: robot body modeled as circle with given radius """ self.start = self.Node(start[0], start[1]) @@ -76,6 +78,7 @@ class RRT: self.max_iter = max_iter self.obstacle_list = obstacle_list self.node_list = [] + self.robot_radius = robot_radius def planning(self, animation=True): """ @@ -93,7 +96,8 @@ class RRT: new_node = self.steer(nearest_node, rnd_node, self.expand_dis) if self.check_if_outside_play_area(new_node, self.play_area) and \ - self.check_collision(new_node, self.obstacle_list): + self.check_collision( + new_node, self.obstacle_list, self.robot_radius): self.node_list.append(new_node) if animation and i % 5 == 0: @@ -103,7 +107,8 @@ class RRT: self.node_list[-1].y) <= self.expand_dis: final_node = self.steer(self.node_list[-1], self.end, self.expand_dis) - if self.check_collision(final_node, self.obstacle_list): + if self.check_collision( + final_node, self.obstacle_list, self.robot_radius): return self.generate_final_course(len(self.node_list) - 1) if animation and i % 5: @@ -173,6 +178,8 @@ class RRT: lambda event: [exit(0) if event.key == 'escape' else None]) if rnd is not None: plt.plot(rnd.x, rnd.y, "^k") + if self.robot_radius > 0.0: + self.plot_circle(rnd.x, rnd.y, self.robot_radius, '-r') for node in self.node_list: if node.parent: plt.plot(node.path_x, node.path_y, "-g") @@ -225,7 +232,7 @@ class RRT: return True # inside - ok @staticmethod - def check_collision(node, obstacleList): + def check_collision(node, obstacleList, robot_radius): if node is None: return False @@ -235,7 +242,7 @@ class RRT: dy_list = [oy - y for y in node.path_y] d_list = [dx * dx + dy * dy for (dx, dy) in zip(dx_list, dy_list)] - if min(d_list) <= size**2: + if min(d_list) <= (size+robot_radius)**2: return False # collision return True # safe @@ -262,6 +269,7 @@ def main(gx=6.0, gy=10.0): rand_area=[-2, 15], obstacle_list=obstacleList, # play_area=[0, 10, 0, 14] + robot_radius=0.8 ) path = rrt.planning(animation=show_animation) diff --git a/PathPlanning/RRT/rrt_with_sobol_sampler.py b/PathPlanning/RRT/rrt_with_sobol_sampler.py index b88e51c6..8045d8f7 100644 --- a/PathPlanning/RRT/rrt_with_sobol_sampler.py +++ b/PathPlanning/RRT/rrt_with_sobol_sampler.py @@ -62,7 +62,8 @@ class RRTSobol: expand_dis=3.0, path_resolution=0.5, goal_sample_rate=5, - max_iter=500): + max_iter=500, + robot_radius=0.0): """ Setting Parameter @@ -70,6 +71,7 @@ class RRTSobol: goal:Goal Position [x,y] obstacle_list:obstacle Positions [[x,y,size],...] randArea:Random Sampling Area [min,max] + robot_radius: robot body modeled as circle with given radius """ self.start = self.Node(start[0], start[1]) @@ -83,6 +85,7 @@ class RRTSobol: self.obstacle_list = obstacle_list self.node_list = [] self.sobol_inter_ = 0 + self.robot_radius = robot_radius def planning(self, animation=True): """ @@ -99,7 +102,8 @@ class RRTSobol: new_node = self.steer(nearest_node, rnd_node, self.expand_dis) - if self.check_collision(new_node, self.obstacle_list): + if self.check_collision( + new_node, self.obstacle_list, self.robot_radius): self.node_list.append(new_node) if animation and i % 5 == 0: @@ -109,7 +113,8 @@ class RRTSobol: self.node_list[-1].y) <= self.expand_dis: final_node = self.steer(self.node_list[-1], self.end, self.expand_dis) - if self.check_collision(final_node, self.obstacle_list): + if self.check_collision( + final_node, self.obstacle_list, self.robot_radius): return self.generate_final_course(len(self.node_list) - 1) if animation and i % 5: @@ -183,6 +188,8 @@ class RRTSobol: lambda event: [sys.exit(0) if event.key == 'escape' else None]) if rnd is not None: plt.plot(rnd.x, rnd.y, "^k") + if self.robot_radius >= 0.0: + self.plot_circle(rnd.x, rnd.y, self.robot_radius, '-r') for node in self.node_list: if node.parent: plt.plot(node.path_x, node.path_y, "-g") @@ -214,7 +221,7 @@ class RRTSobol: return minind @staticmethod - def check_collision(node, obstacle_list): + def check_collision(node, obstacle_list, robot_radius): if node is None: return False @@ -224,7 +231,7 @@ class RRTSobol: dy_list = [oy - y for y in node.path_y] d_list = [dx * dx + dy * dy for (dx, dy) in zip(dx_list, dy_list)] - if min(d_list) <= size**2: + if min(d_list) <= (size+robot_radius)**2: return False # collision return True # safe @@ -249,7 +256,8 @@ def main(gx=6.0, gy=10.0): start=[0, 0], goal=[gx, gy], rand_area=[-2, 15], - obstacle_list=obstacle_list) + obstacle_list=obstacle_list, + robot_radius=0.8) path = rrt.planning(animation=show_animation) if path is None: diff --git a/PathPlanning/RRTDubins/rrt_dubins.py b/PathPlanning/RRTDubins/rrt_dubins.py index 8ab06c73..a43362b3 100644 --- a/PathPlanning/RRTDubins/rrt_dubins.py +++ b/PathPlanning/RRTDubins/rrt_dubins.py @@ -46,6 +46,7 @@ class RRTDubins(RRT): def __init__(self, start, goal, obstacle_list, rand_area, goal_sample_rate=10, max_iter=200, + robot_radius=0.0 ): """ Setting Parameter @@ -54,6 +55,7 @@ class RRTDubins(RRT): goal:Goal Position [x,y] obstacleList:obstacle Positions [[x,y,size],...] randArea:Random Sampling Area [min,max] + robot_radius: robot body modeled as circle with given radius """ self.start = self.Node(start[0], start[1], start[2]) @@ -67,6 +69,7 @@ class RRTDubins(RRT): self.curvature = 1.0 # for dubins path self.goal_yaw_th = np.deg2rad(1.0) self.goal_xy_th = 0.5 + self.robot_radius = robot_radius def planning(self, animation=True, search_until_max_iter=True): """ @@ -82,7 +85,8 @@ class RRTDubins(RRT): 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): + if self.check_collision( + new_node, self.obstacle_list, self.robot_radius): self.node_list.append(new_node) if animation and i % 5 == 0: diff --git a/PathPlanning/RRTStar/rrt_star.py b/PathPlanning/RRTStar/rrt_star.py index bef00860..e2c05456 100644 --- a/PathPlanning/RRTStar/rrt_star.py +++ b/PathPlanning/RRTStar/rrt_star.py @@ -42,7 +42,8 @@ class RRTStar(RRT): goal_sample_rate=20, max_iter=300, connect_circle_dist=50.0, - search_until_max_iter=False): + search_until_max_iter=False, + robot_radius=0.0): """ Setting Parameter @@ -53,7 +54,8 @@ class RRTStar(RRT): """ super().__init__(start, goal, obstacle_list, rand_area, expand_dis, - path_resolution, goal_sample_rate, max_iter) + path_resolution, goal_sample_rate, max_iter, + robot_radius=robot_radius) self.connect_circle_dist = connect_circle_dist self.goal_node = self.Node(goal[0], goal[1]) self.search_until_max_iter = search_until_max_iter @@ -77,7 +79,8 @@ class RRTStar(RRT): math.hypot(new_node.x-near_node.x, new_node.y-near_node.y) - if self.check_collision(new_node, self.obstacle_list): + if self.check_collision( + new_node, self.obstacle_list, self.robot_radius): near_inds = self.find_near_nodes(new_node) node_with_updated_parent = self.choose_parent( new_node, near_inds) @@ -128,7 +131,8 @@ class RRTStar(RRT): for i in near_inds: near_node = self.node_list[i] t_node = self.steer(near_node, new_node) - if t_node and self.check_collision(t_node, self.obstacle_list): + if t_node and self.check_collision( + t_node, self.obstacle_list, self.robot_radius): costs.append(self.calc_new_cost(near_node, new_node)) else: costs.append(float("inf")) # the cost of collision node @@ -156,7 +160,8 @@ class RRTStar(RRT): safe_goal_inds = [] for goal_ind in goal_inds: t_node = self.steer(self.node_list[goal_ind], self.goal_node) - if self.check_collision(t_node, self.obstacle_list): + if self.check_collision( + t_node, self.obstacle_list, self.robot_radius): safe_goal_inds.append(goal_ind) if not safe_goal_inds: @@ -219,7 +224,8 @@ class RRTStar(RRT): continue edge_node.cost = self.calc_new_cost(new_node, near_node) - no_collision = self.check_collision(edge_node, self.obstacle_list) + no_collision = self.check_collision( + edge_node, self.obstacle_list, self.robot_radius) improved_cost = near_node.cost > edge_node.cost if no_collision and improved_cost: @@ -264,7 +270,8 @@ def main(): goal=[6, 10], rand_area=[-2, 15], obstacle_list=obstacle_list, - expand_dis=1) + expand_dis=1, + robot_radius=0.8) path = rrt_star.planning(animation=show_animation) if path is None: diff --git a/PathPlanning/RRTStarDubins/rrt_star_dubins.py b/PathPlanning/RRTStarDubins/rrt_star_dubins.py index 26367eec..21cc9b2b 100644 --- a/PathPlanning/RRTStarDubins/rrt_star_dubins.py +++ b/PathPlanning/RRTStarDubins/rrt_star_dubins.py @@ -46,7 +46,8 @@ class RRTStarDubins(RRTStar): def __init__(self, start, goal, obstacle_list, rand_area, goal_sample_rate=10, max_iter=200, - connect_circle_dist=50.0 + connect_circle_dist=50.0, + robot_radius=0.0, ): """ Setting Parameter @@ -55,6 +56,7 @@ class RRTStarDubins(RRTStar): goal:Goal Position [x,y] obstacleList:obstacle Positions [[x,y,size],...] randArea:Random Sampling Area [min,max] + robot_radius: robot body modeled as circle with given radius """ self.start = self.Node(start[0], start[1], start[2]) @@ -69,6 +71,7 @@ class RRTStarDubins(RRTStar): self.curvature = 1.0 # for dubins path self.goal_yaw_th = np.deg2rad(1.0) self.goal_xy_th = 0.5 + self.robot_radius = robot_radius def planning(self, animation=True, search_until_max_iter=True): """ @@ -84,7 +87,8 @@ class RRTStarDubins(RRTStar): 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): + if self.check_collision( + new_node, self.obstacle_list, self.robot_radius): near_indexes = self.find_near_nodes(new_node) new_node = self.choose_parent(new_node, near_indexes) if new_node: diff --git a/PathPlanning/RRTStarReedsShepp/rrt_star_reeds_shepp.py b/PathPlanning/RRTStarReedsShepp/rrt_star_reeds_shepp.py index 6ea66dc7..323bc76b 100644 --- a/PathPlanning/RRTStarReedsShepp/rrt_star_reeds_shepp.py +++ b/PathPlanning/RRTStarReedsShepp/rrt_star_reeds_shepp.py @@ -45,7 +45,8 @@ class RRTStarReedsShepp(RRTStar): def __init__(self, start, goal, obstacle_list, rand_area, max_iter=200, - connect_circle_dist=50.0 + connect_circle_dist=50.0, + robot_radius=0.0 ): """ Setting Parameter @@ -54,6 +55,7 @@ class RRTStarReedsShepp(RRTStar): goal:Goal Position [x,y] obstacleList:obstacle Positions [[x,y,size],...] randArea:Random Sampling Area [min,max] + robot_radius: robot body modeled as circle with given radius """ self.start = self.Node(start[0], start[1], start[2]) @@ -63,6 +65,7 @@ class RRTStarReedsShepp(RRTStar): self.max_iter = max_iter self.obstacle_list = obstacle_list self.connect_circle_dist = connect_circle_dist + self.robot_radius = robot_radius self.curvature = 1.0 self.goal_yaw_th = np.deg2rad(1.0) @@ -82,7 +85,8 @@ class RRTStarReedsShepp(RRTStar): 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): + if self.check_collision( + new_node, self.obstacle_list, self.robot_radius): near_indexes = self.find_near_nodes(new_node) new_node = self.choose_parent(new_node, near_indexes) if new_node: @@ -117,7 +121,8 @@ class RRTStarReedsShepp(RRTStar): if new_node is None: return - if self.check_collision(new_node, self.obstacle_list): + if self.check_collision( + new_node, self.obstacle_list, self.robot_radius): self.node_list.append(new_node) def draw_graph(self, rnd=None): diff --git a/tests/test_rrt_star.py b/tests/test_rrt_star.py index 5327f4ca..232995ec 100644 --- a/tests/test_rrt_star.py +++ b/tests/test_rrt_star.py @@ -19,5 +19,18 @@ def test_no_obstacle(): assert path is not None +def test_no_obstacle_and_robot_radius(): + obstacle_list = [] + + # Set Initial parameters + rrt_star = m.RRTStar(start=[0, 0], + goal=[6, 10], + rand_area=[-2, 15], + obstacle_list=obstacle_list, + robot_radius=0.8) + path = rrt_star.planning(animation=False) + assert path is not None + + if __name__ == '__main__': conftest.run_this_test(__file__) diff --git a/tests/test_rrt_star_reeds_shepp.py b/tests/test_rrt_star_reeds_shepp.py index 0a466305..cb6b586b 100644 --- a/tests/test_rrt_star_reeds_shepp.py +++ b/tests/test_rrt_star_reeds_shepp.py @@ -1,5 +1,5 @@ import conftest # Add root path to sys.path -import rrt_star_reeds_shepp as m +from PathPlanning.RRTStarReedsShepp import rrt_star_reeds_shepp as m def test1():