Skip to content

Commit b53fdf7

Browse files
cmowerAtsushiSakai
andauthored
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 <asakai.amsl+github@gmail.com> Co-authored-by: Atsushi Sakai <asakai.amsl+github@gmail.com>
1 parent af2061a commit b53fdf7

File tree

10 files changed

+88
-42
lines changed

10 files changed

+88
-42
lines changed

PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py

Lines changed: 8 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -37,11 +37,13 @@ class ClosedLoopRRTStar(RRTStarReedsShepp):
3737

3838
def __init__(self, start, goal, obstacle_list, rand_area,
3939
max_iter=200,
40-
connect_circle_dist=50.0
40+
connect_circle_dist=50.0,
41+
robot_radius=0.0
4142
):
4243
super().__init__(start, goal, obstacle_list, rand_area,
4344
max_iter=max_iter,
4445
connect_circle_dist=connect_circle_dist,
46+
robot_radius=robot_radius
4547
)
4648

4749
self.target_speed = 10.0 / 3.6
@@ -127,7 +129,11 @@ def check_tracking_path_is_feasible(self, path):
127129
print("path is too long")
128130
find_goal = False
129131

130-
if not self.collision_check_with_xy(x, y, self.obstacle_list):
132+
tmp_node = self.Node(x, y, 0)
133+
tmp_node.path_x = x
134+
tmp_node.path_y = y
135+
if not self.check_collision(
136+
tmp_node, self.obstacle_list, self.robot_radius):
131137
print("This path is collision")
132138
find_goal = False
133139

@@ -151,19 +157,6 @@ def get_goal_indexes(self):
151157

152158
return fgoalinds
153159

154-
@staticmethod
155-
def collision_check_with_xy(x, y, obstacle_list):
156-
157-
for (ox, oy, size) in obstacle_list:
158-
for (ix, iy) in zip(x, y):
159-
dx = ox - ix
160-
dy = oy - iy
161-
d = dx * dx + dy * dy
162-
if d <= size ** 2:
163-
return False # collision
164-
165-
return True # safe
166-
167160

168161
def main(gx=6.0, gy=7.0, gyaw=np.deg2rad(90.0), max_iter=100):
169162
print("Start" + __file__)

PathPlanning/LQRRRTStar/lqr_rrt_star.py

Lines changed: 6 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -35,7 +35,8 @@ def __init__(self, start, goal, obstacle_list, rand_area,
3535
goal_sample_rate=10,
3636
max_iter=200,
3737
connect_circle_dist=50.0,
38-
step_size=0.2
38+
step_size=0.2,
39+
robot_radius=0.0,
3940
):
4041
"""
4142
Setting Parameter
@@ -44,6 +45,7 @@ def __init__(self, start, goal, obstacle_list, rand_area,
4445
goal:Goal Position [x,y]
4546
obstacleList:obstacle Positions [[x,y,size],...]
4647
randArea:Random Sampling Area [min,max]
48+
robot_radius: robot body modeled as circle with given radius
4749
4850
"""
4951
self.start = self.Node(start[0], start[1])
@@ -58,6 +60,7 @@ def __init__(self, start, goal, obstacle_list, rand_area,
5860
self.curvature = 1.0
5961
self.goal_xy_th = 0.5
6062
self.step_size = step_size
63+
self.robot_radius = robot_radius
6164

6265
self.lqr_planner = LQRPlanner()
6366

@@ -75,7 +78,8 @@ def planning(self, animation=True, search_until_max_iter=True):
7578
nearest_ind = self.get_nearest_node_index(self.node_list, rnd)
7679
new_node = self.steer(self.node_list[nearest_ind], rnd)
7780

78-
if self.check_collision(new_node, self.obstacle_list):
81+
if self.check_collision(
82+
new_node, self.obstacle_list, self.robot_radius):
7983
near_indexes = self.find_near_nodes(new_node)
8084
new_node = self.choose_parent(new_node, near_indexes)
8185
if new_node:

PathPlanning/RRT/rrt.py

Lines changed: 13 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -50,7 +50,8 @@ def __init__(self,
5050
path_resolution=0.5,
5151
goal_sample_rate=5,
5252
max_iter=500,
53-
play_area=None
53+
play_area=None,
54+
robot_radius=0.0,
5455
):
5556
"""
5657
Setting Parameter
@@ -60,6 +61,7 @@ def __init__(self,
6061
obstacleList:obstacle Positions [[x,y,size],...]
6162
randArea:Random Sampling Area [min,max]
6263
play_area:stay inside this area [xmin,xmax,ymin,ymax]
64+
robot_radius: robot body modeled as circle with given radius
6365
6466
"""
6567
self.start = self.Node(start[0], start[1])
@@ -76,6 +78,7 @@ def __init__(self,
7678
self.max_iter = max_iter
7779
self.obstacle_list = obstacle_list
7880
self.node_list = []
81+
self.robot_radius = robot_radius
7982

8083
def planning(self, animation=True):
8184
"""
@@ -93,7 +96,8 @@ def planning(self, animation=True):
9396
new_node = self.steer(nearest_node, rnd_node, self.expand_dis)
9497

9598
if self.check_if_outside_play_area(new_node, self.play_area) and \
96-
self.check_collision(new_node, self.obstacle_list):
99+
self.check_collision(
100+
new_node, self.obstacle_list, self.robot_radius):
97101
self.node_list.append(new_node)
98102

99103
if animation and i % 5 == 0:
@@ -103,7 +107,8 @@ def planning(self, animation=True):
103107
self.node_list[-1].y) <= self.expand_dis:
104108
final_node = self.steer(self.node_list[-1], self.end,
105109
self.expand_dis)
106-
if self.check_collision(final_node, self.obstacle_list):
110+
if self.check_collision(
111+
final_node, self.obstacle_list, self.robot_radius):
107112
return self.generate_final_course(len(self.node_list) - 1)
108113

109114
if animation and i % 5:
@@ -173,6 +178,8 @@ def draw_graph(self, rnd=None):
173178
lambda event: [exit(0) if event.key == 'escape' else None])
174179
if rnd is not None:
175180
plt.plot(rnd.x, rnd.y, "^k")
181+
if self.robot_radius > 0.0:
182+
self.plot_circle(rnd.x, rnd.y, self.robot_radius, '-r')
176183
for node in self.node_list:
177184
if node.parent:
178185
plt.plot(node.path_x, node.path_y, "-g")
@@ -225,7 +232,7 @@ def check_if_outside_play_area(node, play_area):
225232
return True # inside - ok
226233

227234
@staticmethod
228-
def check_collision(node, obstacleList):
235+
def check_collision(node, obstacleList, robot_radius):
229236

230237
if node is None:
231238
return False
@@ -235,7 +242,7 @@ def check_collision(node, obstacleList):
235242
dy_list = [oy - y for y in node.path_y]
236243
d_list = [dx * dx + dy * dy for (dx, dy) in zip(dx_list, dy_list)]
237244

238-
if min(d_list) <= size**2:
245+
if min(d_list) <= (size+robot_radius)**2:
239246
return False # collision
240247

241248
return True # safe
@@ -262,6 +269,7 @@ def main(gx=6.0, gy=10.0):
262269
rand_area=[-2, 15],
263270
obstacle_list=obstacleList,
264271
# play_area=[0, 10, 0, 14]
272+
robot_radius=0.8
265273
)
266274
path = rrt.planning(animation=show_animation)
267275

PathPlanning/RRT/rrt_with_sobol_sampler.py

Lines changed: 14 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -62,14 +62,16 @@ def __init__(self,
6262
expand_dis=3.0,
6363
path_resolution=0.5,
6464
goal_sample_rate=5,
65-
max_iter=500):
65+
max_iter=500,
66+
robot_radius=0.0):
6667
"""
6768
Setting Parameter
6869
6970
start:Start Position [x,y]
7071
goal:Goal Position [x,y]
7172
obstacle_list:obstacle Positions [[x,y,size],...]
7273
randArea:Random Sampling Area [min,max]
74+
robot_radius: robot body modeled as circle with given radius
7375
7476
"""
7577
self.start = self.Node(start[0], start[1])
@@ -83,6 +85,7 @@ def __init__(self,
8385
self.obstacle_list = obstacle_list
8486
self.node_list = []
8587
self.sobol_inter_ = 0
88+
self.robot_radius = robot_radius
8689

8790
def planning(self, animation=True):
8891
"""
@@ -99,7 +102,8 @@ def planning(self, animation=True):
99102

100103
new_node = self.steer(nearest_node, rnd_node, self.expand_dis)
101104

102-
if self.check_collision(new_node, self.obstacle_list):
105+
if self.check_collision(
106+
new_node, self.obstacle_list, self.robot_radius):
103107
self.node_list.append(new_node)
104108

105109
if animation and i % 5 == 0:
@@ -109,7 +113,8 @@ def planning(self, animation=True):
109113
self.node_list[-1].y) <= self.expand_dis:
110114
final_node = self.steer(self.node_list[-1], self.end,
111115
self.expand_dis)
112-
if self.check_collision(final_node, self.obstacle_list):
116+
if self.check_collision(
117+
final_node, self.obstacle_list, self.robot_radius):
113118
return self.generate_final_course(len(self.node_list) - 1)
114119

115120
if animation and i % 5:
@@ -183,6 +188,8 @@ def draw_graph(self, rnd=None):
183188
lambda event: [sys.exit(0) if event.key == 'escape' else None])
184189
if rnd is not None:
185190
plt.plot(rnd.x, rnd.y, "^k")
191+
if self.robot_radius >= 0.0:
192+
self.plot_circle(rnd.x, rnd.y, self.robot_radius, '-r')
186193
for node in self.node_list:
187194
if node.parent:
188195
plt.plot(node.path_x, node.path_y, "-g")
@@ -214,7 +221,7 @@ def get_nearest_node_index(node_list, rnd_node):
214221
return minind
215222

216223
@staticmethod
217-
def check_collision(node, obstacle_list):
224+
def check_collision(node, obstacle_list, robot_radius):
218225

219226
if node is None:
220227
return False
@@ -224,7 +231,7 @@ def check_collision(node, obstacle_list):
224231
dy_list = [oy - y for y in node.path_y]
225232
d_list = [dx * dx + dy * dy for (dx, dy) in zip(dx_list, dy_list)]
226233

227-
if min(d_list) <= size**2:
234+
if min(d_list) <= (size+robot_radius)**2:
228235
return False # collision
229236

230237
return True # safe
@@ -249,7 +256,8 @@ def main(gx=6.0, gy=10.0):
249256
start=[0, 0],
250257
goal=[gx, gy],
251258
rand_area=[-2, 15],
252-
obstacle_list=obstacle_list)
259+
obstacle_list=obstacle_list,
260+
robot_radius=0.8)
253261
path = rrt.planning(animation=show_animation)
254262

255263
if path is None:

PathPlanning/RRTDubins/rrt_dubins.py

Lines changed: 5 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -46,6 +46,7 @@ def __init__(self, x, y, yaw):
4646
def __init__(self, start, goal, obstacle_list, rand_area,
4747
goal_sample_rate=10,
4848
max_iter=200,
49+
robot_radius=0.0
4950
):
5051
"""
5152
Setting Parameter
@@ -54,6 +55,7 @@ def __init__(self, start, goal, obstacle_list, rand_area,
5455
goal:Goal Position [x,y]
5556
obstacleList:obstacle Positions [[x,y,size],...]
5657
randArea:Random Sampling Area [min,max]
58+
robot_radius: robot body modeled as circle with given radius
5759
5860
"""
5961
self.start = self.Node(start[0], start[1], start[2])
@@ -67,6 +69,7 @@ def __init__(self, start, goal, obstacle_list, rand_area,
6769
self.curvature = 1.0 # for dubins path
6870
self.goal_yaw_th = np.deg2rad(1.0)
6971
self.goal_xy_th = 0.5
72+
self.robot_radius = robot_radius
7073

7174
def planning(self, animation=True, search_until_max_iter=True):
7275
"""
@@ -82,7 +85,8 @@ def planning(self, animation=True, search_until_max_iter=True):
8285
nearest_ind = self.get_nearest_node_index(self.node_list, rnd)
8386
new_node = self.steer(self.node_list[nearest_ind], rnd)
8487

85-
if self.check_collision(new_node, self.obstacle_list):
88+
if self.check_collision(
89+
new_node, self.obstacle_list, self.robot_radius):
8690
self.node_list.append(new_node)
8791

8892
if animation and i % 5 == 0:

PathPlanning/RRTStar/rrt_star.py

Lines changed: 14 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -42,7 +42,8 @@ def __init__(self,
4242
goal_sample_rate=20,
4343
max_iter=300,
4444
connect_circle_dist=50.0,
45-
search_until_max_iter=False):
45+
search_until_max_iter=False,
46+
robot_radius=0.0):
4647
"""
4748
Setting Parameter
4849
@@ -53,7 +54,8 @@ def __init__(self,
5354
5455
"""
5556
super().__init__(start, goal, obstacle_list, rand_area, expand_dis,
56-
path_resolution, goal_sample_rate, max_iter)
57+
path_resolution, goal_sample_rate, max_iter,
58+
robot_radius=robot_radius)
5759
self.connect_circle_dist = connect_circle_dist
5860
self.goal_node = self.Node(goal[0], goal[1])
5961
self.search_until_max_iter = search_until_max_iter
@@ -77,7 +79,8 @@ def planning(self, animation=True):
7779
math.hypot(new_node.x-near_node.x,
7880
new_node.y-near_node.y)
7981

80-
if self.check_collision(new_node, self.obstacle_list):
82+
if self.check_collision(
83+
new_node, self.obstacle_list, self.robot_radius):
8184
near_inds = self.find_near_nodes(new_node)
8285
node_with_updated_parent = self.choose_parent(
8386
new_node, near_inds)
@@ -128,7 +131,8 @@ def choose_parent(self, new_node, near_inds):
128131
for i in near_inds:
129132
near_node = self.node_list[i]
130133
t_node = self.steer(near_node, new_node)
131-
if t_node and self.check_collision(t_node, self.obstacle_list):
134+
if t_node and self.check_collision(
135+
t_node, self.obstacle_list, self.robot_radius):
132136
costs.append(self.calc_new_cost(near_node, new_node))
133137
else:
134138
costs.append(float("inf")) # the cost of collision node
@@ -156,7 +160,8 @@ def search_best_goal_node(self):
156160
safe_goal_inds = []
157161
for goal_ind in goal_inds:
158162
t_node = self.steer(self.node_list[goal_ind], self.goal_node)
159-
if self.check_collision(t_node, self.obstacle_list):
163+
if self.check_collision(
164+
t_node, self.obstacle_list, self.robot_radius):
160165
safe_goal_inds.append(goal_ind)
161166

162167
if not safe_goal_inds:
@@ -219,7 +224,8 @@ def rewire(self, new_node, near_inds):
219224
continue
220225
edge_node.cost = self.calc_new_cost(new_node, near_node)
221226

222-
no_collision = self.check_collision(edge_node, self.obstacle_list)
227+
no_collision = self.check_collision(
228+
edge_node, self.obstacle_list, self.robot_radius)
223229
improved_cost = near_node.cost > edge_node.cost
224230

225231
if no_collision and improved_cost:
@@ -264,7 +270,8 @@ def main():
264270
goal=[6, 10],
265271
rand_area=[-2, 15],
266272
obstacle_list=obstacle_list,
267-
expand_dis=1)
273+
expand_dis=1,
274+
robot_radius=0.8)
268275
path = rrt_star.planning(animation=show_animation)
269276

270277
if path is None:

PathPlanning/RRTStarDubins/rrt_star_dubins.py

Lines changed: 6 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -46,7 +46,8 @@ def __init__(self, x, y, yaw):
4646
def __init__(self, start, goal, obstacle_list, rand_area,
4747
goal_sample_rate=10,
4848
max_iter=200,
49-
connect_circle_dist=50.0
49+
connect_circle_dist=50.0,
50+
robot_radius=0.0,
5051
):
5152
"""
5253
Setting Parameter
@@ -55,6 +56,7 @@ def __init__(self, start, goal, obstacle_list, rand_area,
5556
goal:Goal Position [x,y]
5657
obstacleList:obstacle Positions [[x,y,size],...]
5758
randArea:Random Sampling Area [min,max]
59+
robot_radius: robot body modeled as circle with given radius
5860
5961
"""
6062
self.start = self.Node(start[0], start[1], start[2])
@@ -69,6 +71,7 @@ def __init__(self, start, goal, obstacle_list, rand_area,
6971
self.curvature = 1.0 # for dubins path
7072
self.goal_yaw_th = np.deg2rad(1.0)
7173
self.goal_xy_th = 0.5
74+
self.robot_radius = robot_radius
7275

7376
def planning(self, animation=True, search_until_max_iter=True):
7477
"""
@@ -84,7 +87,8 @@ def planning(self, animation=True, search_until_max_iter=True):
8487
nearest_ind = self.get_nearest_node_index(self.node_list, rnd)
8588
new_node = self.steer(self.node_list[nearest_ind], rnd)
8689

87-
if self.check_collision(new_node, self.obstacle_list):
90+
if self.check_collision(
91+
new_node, self.obstacle_list, self.robot_radius):
8892
near_indexes = self.find_near_nodes(new_node)
8993
new_node = self.choose_parent(new_node, near_indexes)
9094
if new_node:

0 commit comments

Comments
 (0)