Add optional robot radius to RRT/RRTStar path planners (AtsushiSakai#655)

* Add optional robot radius to RRT/RRTStar path planners.
* update __init__ and check_collision to include radius
* during animation, if a robot radius is given then it is drawn

* Add test for robot radius

* Correct import error

* Correct missing robot_radius errors

* Address "expected 2 blank lines, found 1" error

* Address "line too long" errors

* Add missing argument description.

* Remove collision_check_with_xy and replace with check_collision

* Fix "missing whitespace after ','" error

* Update PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py

Co-authored-by: Atsushi Sakai <[email protected]>

Co-authored-by: Atsushi Sakai <[email protected]>

Author: cmower

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 @@ def check_tracking_path_is_feasible(self, path):
             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 @@ def get_goal_indexes(self):
 
         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__)

PathPlanning/LQRRRTStar/lqr_rrt_star.py

@@ -35,7 +35,8 @@ def __init__(self, start, goal, obstacle_list, rand_area,
                  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 @@ def __init__(self, start, goal, obstacle_list, rand_area,
         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 @@ def __init__(self, start, goal, obstacle_list, rand_area,
         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 @@ def planning(self, animation=True, search_until_max_iter=True):
             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:

PathPlanning/RRT/rrt.py

@@ -50,7 +50,8 @@ def __init__(self,
                  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 @@ def __init__(self,
         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 @@ def __init__(self,
         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 @@ def planning(self, animation=True):
             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 @@ def planning(self, animation=True):
                                       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 @@ def draw_graph(self, rnd=None):
             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 @@ def check_if_outside_play_area(node, play_area):
             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 @@ def check_collision(node, obstacleList):
             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)
 

PathPlanning/RRT/rrt_with_sobol_sampler.py

@@ -62,14 +62,16 @@ def __init__(self,
                  expand_dis=3.0,
                  path_resolution=0.5,
                  goal_sample_rate=5,
-                 max_iter=500):
+                 max_iter=500,
+                 robot_radius=0.0):
         """
         Setting Parameter
 
         start:Start Position [x,y]
         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 @@ def __init__(self,
         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 @@ def planning(self, animation=True):
 
             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 @@ def planning(self, animation=True):
                                       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 @@ def draw_graph(self, rnd=None):
             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 @@ def get_nearest_node_index(node_list, rnd_node):
         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 @@ def check_collision(node, obstacle_list):
             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:

PathPlanning/RRTDubins/rrt_dubins.py

@@ -46,6 +46,7 @@ def __init__(self, x, y, yaw):
     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 @@ def __init__(self, start, goal, obstacle_list, rand_area,
         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 @@ def __init__(self, start, goal, obstacle_list, rand_area,
         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 @@ def planning(self, animation=True, search_until_max_iter=True):
             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:

PathPlanning/RRTStar/rrt_star.py

@@ -42,7 +42,8 @@ def __init__(self,
                  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 @@ def __init__(self,
 
         """
         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 @@ def planning(self, animation=True):
                 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 @@ def choose_parent(self, new_node, near_inds):
         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 @@ def search_best_goal_node(self):
         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 @@ def rewire(self, new_node, near_inds):
                 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:

PathPlanning/RRTStarDubins/rrt_star_dubins.py

@@ -46,7 +46,8 @@ def __init__(self, x, y, yaw):
     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 @@ def __init__(self, start, goal, obstacle_list, rand_area,
         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 @@ def __init__(self, start, goal, obstacle_list, rand_area,
         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 @@ def planning(self, animation=True, search_until_max_iter=True):
             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: