mirror of
https://github.com/AtsushiSakai/PythonRobotics.git
synced 2026-04-22 03:00:41 -04:00
0e93ecb669
* Fix: Include robot_radius in path_smoothing collision check * Test: Add unit test to verify smoothed path respects robot_radius
211 lines
6.1 KiB
Python
211 lines
6.1 KiB
Python
"""
|
|
|
|
Path planning Sample Code with RRT with path smoothing
|
|
|
|
@author: AtsushiSakai(@Atsushi_twi)
|
|
|
|
"""
|
|
|
|
import math
|
|
import random
|
|
import matplotlib.pyplot as plt
|
|
import sys
|
|
import pathlib
|
|
sys.path.append(str(pathlib.Path(__file__).parent))
|
|
|
|
from rrt import RRT
|
|
|
|
show_animation = True
|
|
|
|
|
|
def get_path_length(path):
|
|
le = 0
|
|
for i in range(len(path) - 1):
|
|
dx = path[i + 1][0] - path[i][0]
|
|
dy = path[i + 1][1] - path[i][1]
|
|
d = math.hypot(dx, dy)
|
|
le += d
|
|
|
|
return le
|
|
|
|
|
|
def get_target_point(path, targetL):
|
|
le = 0
|
|
ti = 0
|
|
lastPairLen = 0
|
|
for i in range(len(path) - 1):
|
|
dx = path[i + 1][0] - path[i][0]
|
|
dy = path[i + 1][1] - path[i][1]
|
|
d = math.hypot(dx, dy)
|
|
le += d
|
|
if le >= targetL:
|
|
ti = i - 1
|
|
lastPairLen = d
|
|
break
|
|
|
|
partRatio = (le - targetL) / lastPairLen
|
|
|
|
x = path[ti][0] + (path[ti + 1][0] - path[ti][0]) * partRatio
|
|
y = path[ti][1] + (path[ti + 1][1] - path[ti][1]) * partRatio
|
|
|
|
return [x, y, ti]
|
|
|
|
|
|
def is_point_collision(x, y, obstacle_list, robot_radius):
|
|
"""
|
|
Check whether a single point collides with any obstacle.
|
|
|
|
This function calculates the Euclidean distance between the given point (x, y)
|
|
and each obstacle center. If the distance is less than or equal to the sum of
|
|
the obstacle's radius and the robot's radius, a collision is detected.
|
|
|
|
Args:
|
|
x (float): X-coordinate of the point to check.
|
|
y (float): Y-coordinate of the point to check.
|
|
obstacle_list (List[Tuple[float, float, float]]): List of obstacles defined as (ox, oy, radius).
|
|
robot_radius (float): Radius of the robot, used to inflate the obstacles.
|
|
|
|
Returns:
|
|
bool: True if the point is in collision with any obstacle, False otherwise.
|
|
"""
|
|
for (ox, oy, obstacle_radius) in obstacle_list:
|
|
d = math.hypot(ox - x, oy - y)
|
|
if d <= obstacle_radius + robot_radius:
|
|
return True # Collided
|
|
return False
|
|
|
|
|
|
def line_collision_check(first, second, obstacle_list, robot_radius=0.0, sample_step=0.2):
|
|
"""
|
|
Check if the line segment between `first` and `second` collides with any obstacle.
|
|
Considers the robot_radius by inflating the obstacle size.
|
|
|
|
Args:
|
|
first (List[float]): Start point of the line [x, y]
|
|
second (List[float]): End point of the line [x, y]
|
|
obstacle_list (List[Tuple[float, float, float]]): Obstacles as (x, y, radius)
|
|
robot_radius (float): Radius of robot
|
|
sample_step (float): Distance between sampling points along the segment
|
|
|
|
Returns:
|
|
bool: True if collision-free, False otherwise
|
|
"""
|
|
x1, y1 = first[0], first[1]
|
|
x2, y2 = second[0], second[1]
|
|
|
|
dx = x2 - x1
|
|
dy = y2 - y1
|
|
length = math.hypot(dx, dy)
|
|
|
|
if length == 0:
|
|
# Degenerate case: point collision check
|
|
return not is_point_collision(x1, y1, obstacle_list, robot_radius)
|
|
|
|
steps = int(length / sample_step) + 1 # Sampling every sample_step along the segment
|
|
|
|
for i in range(steps + 1):
|
|
t = i / steps
|
|
x = x1 + t * dx
|
|
y = y1 + t * dy
|
|
|
|
if is_point_collision(x, y, obstacle_list, robot_radius):
|
|
return False # Collision found
|
|
|
|
return True # Safe
|
|
|
|
|
|
def path_smoothing(path, max_iter, obstacle_list, robot_radius=0.0):
|
|
"""
|
|
Smooths a given path by iteratively replacing segments with shortcut connections,
|
|
while ensuring the new segments are collision-free.
|
|
|
|
The algorithm randomly picks two points along the original path and attempts to
|
|
connect them with a straight line. If the line does not collide with any obstacles
|
|
(considering the robot's radius), the intermediate path points between them are
|
|
replaced with the direct connection.
|
|
|
|
Args:
|
|
path (List[List[float]]): The original path as a list of [x, y] coordinates.
|
|
max_iter (int): Number of iterations for smoothing attempts.
|
|
obstacle_list (List[Tuple[float, float, float]]): List of obstacles represented as
|
|
(x, y, radius).
|
|
robot_radius (float, optional): Radius of the robot, used to inflate obstacle size
|
|
during collision checking. Defaults to 0.0.
|
|
|
|
Returns:
|
|
List[List[float]]: The smoothed path as a list of [x, y] coordinates.
|
|
|
|
Example:
|
|
>>> smoothed = path_smoothing(path, 1000, obstacle_list, robot_radius=0.5)
|
|
"""
|
|
le = get_path_length(path)
|
|
|
|
for i in range(max_iter):
|
|
# Sample two points
|
|
pickPoints = [random.uniform(0, le), random.uniform(0, le)]
|
|
pickPoints.sort()
|
|
first = get_target_point(path, pickPoints[0])
|
|
second = get_target_point(path, pickPoints[1])
|
|
|
|
if first[2] <= 0 or second[2] <= 0:
|
|
continue
|
|
|
|
if (second[2] + 1) > len(path):
|
|
continue
|
|
|
|
if second[2] == first[2]:
|
|
continue
|
|
|
|
# collision check
|
|
if not line_collision_check(first, second, obstacle_list, robot_radius):
|
|
continue
|
|
|
|
# Create New path
|
|
newPath = []
|
|
newPath.extend(path[:first[2] + 1])
|
|
newPath.append([first[0], first[1]])
|
|
newPath.append([second[0], second[1]])
|
|
newPath.extend(path[second[2] + 1:])
|
|
path = newPath
|
|
le = get_path_length(path)
|
|
|
|
return path
|
|
|
|
|
|
def main():
|
|
# ====Search Path with RRT====
|
|
# Parameter
|
|
obstacleList = [
|
|
(5, 5, 1),
|
|
(3, 6, 2),
|
|
(3, 8, 2),
|
|
(3, 10, 2),
|
|
(7, 5, 2),
|
|
(9, 5, 2)
|
|
] # [x,y,radius]
|
|
rrt = RRT(start=[0, 0], goal=[6, 10],
|
|
rand_area=[-2, 15], obstacle_list=obstacleList,
|
|
robot_radius=0.3)
|
|
path = rrt.planning(animation=show_animation)
|
|
|
|
# Path smoothing
|
|
maxIter = 1000
|
|
smoothedPath = path_smoothing(path, maxIter, obstacleList,
|
|
robot_radius=rrt.robot_radius)
|
|
|
|
# Draw final path
|
|
if show_animation:
|
|
rrt.draw_graph()
|
|
plt.plot([x for (x, y) in path], [y for (x, y) in path], '-r')
|
|
|
|
plt.plot([x for (x, y) in smoothedPath], [
|
|
y for (x, y) in smoothedPath], '-c')
|
|
|
|
plt.grid(True)
|
|
plt.pause(0.01) # Need for Mac
|
|
plt.show()
|
|
|
|
|
|
if __name__ == '__main__':
|
|
main()
|