github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-02-15 17:05:12 -05:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source 
* 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>
This commit is contained in:
Chris Mower
2022-04-19 04:53:24 +01:00
committed by GitHub
parent af2061a1e7
commit b53fdf75f6
10 changed files with 88 additions and 42 deletions
Download Patch File Download Diff File Expand all files Collapse all files

20
PathPlanning/RRT/rrt_with_sobol_sampler.py
View File

@@ -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: