mirror of
https://github.com/AtsushiSakai/PythonRobotics.git
synced 2026-01-13 05:38:11 -05:00
290 lines
9.3 KiB
Python
290 lines
9.3 KiB
Python
"""
|
|
|
|
Path planning Sample Code with RRT*
|
|
|
|
author: Atsushi Sakai(@Atsushi_twi)
|
|
|
|
"""
|
|
|
|
import math
|
|
import sys
|
|
import matplotlib.pyplot as plt
|
|
import pathlib
|
|
sys.path.append(str(pathlib.Path(__file__).parent.parent))
|
|
|
|
from RRT.rrt import RRT
|
|
|
|
show_animation = True
|
|
|
|
|
|
class RRTStar(RRT):
|
|
"""
|
|
Class for RRT Star planning
|
|
"""
|
|
|
|
class Node(RRT.Node):
|
|
def __init__(self, x, y):
|
|
super().__init__(x, y)
|
|
self.cost = 0.0
|
|
|
|
def __init__(self,
|
|
start,
|
|
goal,
|
|
obstacle_list,
|
|
rand_area,
|
|
expand_dis=30.0,
|
|
path_resolution=1.0,
|
|
goal_sample_rate=20,
|
|
max_iter=300,
|
|
connect_circle_dist=50.0,
|
|
search_until_max_iter=False,
|
|
robot_radius=0.0):
|
|
"""
|
|
Setting Parameter
|
|
|
|
start:Start Position [x,y]
|
|
goal:Goal Position [x,y]
|
|
obstacleList:obstacle Positions [[x,y,size],...]
|
|
randArea:Random Sampling Area [min,max]
|
|
|
|
"""
|
|
super().__init__(start, goal, obstacle_list, rand_area, expand_dis,
|
|
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
|
|
self.node_list = []
|
|
|
|
def planning(self, animation=True):
|
|
"""
|
|
rrt star path planning
|
|
|
|
animation: flag for animation on or off .
|
|
"""
|
|
|
|
self.node_list = [self.start]
|
|
for i in range(self.max_iter):
|
|
print("Iter:", i, ", number of nodes:", len(self.node_list))
|
|
rnd = self.get_random_node()
|
|
nearest_ind = self.get_nearest_node_index(self.node_list, rnd)
|
|
new_node = self.steer(self.node_list[nearest_ind], rnd,
|
|
self.expand_dis)
|
|
near_node = self.node_list[nearest_ind]
|
|
new_node.cost = near_node.cost + \
|
|
math.hypot(new_node.x-near_node.x,
|
|
new_node.y-near_node.y)
|
|
|
|
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)
|
|
if node_with_updated_parent:
|
|
self.rewire(node_with_updated_parent, near_inds)
|
|
self.node_list.append(node_with_updated_parent)
|
|
else:
|
|
self.node_list.append(new_node)
|
|
|
|
if animation:
|
|
self.draw_graph(rnd)
|
|
|
|
if ((not self.search_until_max_iter)
|
|
and new_node): # if reaches goal
|
|
last_index = self.search_best_goal_node()
|
|
if last_index is not None:
|
|
return self.generate_final_course(last_index)
|
|
|
|
print("reached max iteration")
|
|
|
|
last_index = self.search_best_goal_node()
|
|
if last_index is not None:
|
|
return self.generate_final_course(last_index)
|
|
|
|
return None
|
|
|
|
def choose_parent(self, new_node, near_inds):
|
|
"""
|
|
Computes the cheapest point to new_node contained in the list
|
|
near_inds and set such a node as the parent of new_node.
|
|
Arguments:
|
|
--------
|
|
new_node, Node
|
|
randomly generated node with a path from its neared point
|
|
There are not coalitions between this node and th tree.
|
|
near_inds: list
|
|
Indices of indices of the nodes what are near to new_node
|
|
|
|
Returns.
|
|
------
|
|
Node, a copy of new_node
|
|
"""
|
|
if not near_inds:
|
|
return None
|
|
|
|
# search nearest cost in near_inds
|
|
costs = []
|
|
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, self.robot_radius):
|
|
costs.append(self.calc_new_cost(near_node, new_node))
|
|
else:
|
|
costs.append(float("inf")) # the cost of collision node
|
|
min_cost = min(costs)
|
|
|
|
if min_cost == float("inf"):
|
|
print("There is no good path.(min_cost is inf)")
|
|
return None
|
|
|
|
min_ind = near_inds[costs.index(min_cost)]
|
|
new_node = self.steer(self.node_list[min_ind], new_node)
|
|
new_node.cost = min_cost
|
|
|
|
return new_node
|
|
|
|
def search_best_goal_node(self):
|
|
dist_to_goal_list = [
|
|
self.calc_dist_to_goal(n.x, n.y) for n in self.node_list
|
|
]
|
|
goal_inds = [
|
|
dist_to_goal_list.index(i) for i in dist_to_goal_list
|
|
if i <= self.expand_dis
|
|
]
|
|
|
|
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, self.robot_radius):
|
|
safe_goal_inds.append(goal_ind)
|
|
|
|
if not safe_goal_inds:
|
|
return None
|
|
|
|
safe_goal_costs = [self.node_list[i].cost +
|
|
self.calc_dist_to_goal(self.node_list[i].x, self.node_list[i].y)
|
|
for i in safe_goal_inds]
|
|
|
|
min_cost = min(safe_goal_costs)
|
|
for i, cost in zip(safe_goal_inds, safe_goal_costs):
|
|
if cost == min_cost:
|
|
return i
|
|
|
|
return None
|
|
|
|
def find_near_nodes(self, new_node):
|
|
"""
|
|
1) defines a ball centered on new_node
|
|
2) Returns all nodes of the three that are inside this ball
|
|
Arguments:
|
|
---------
|
|
new_node: Node
|
|
new randomly generated node, without collisions between
|
|
its nearest node
|
|
Returns:
|
|
-------
|
|
list
|
|
List with the indices of the nodes inside the ball of
|
|
radius r
|
|
"""
|
|
nnode = len(self.node_list) + 1
|
|
r = self.connect_circle_dist * math.sqrt(math.log(nnode) / nnode)
|
|
# if expand_dist exists, search vertices in a range no more than
|
|
# expand_dist
|
|
if hasattr(self, 'expand_dis'):
|
|
r = min(r, self.expand_dis)
|
|
dist_list = [(node.x - new_node.x)**2 + (node.y - new_node.y)**2
|
|
for node in self.node_list]
|
|
near_inds = [dist_list.index(i) for i in dist_list if i <= r**2]
|
|
return near_inds
|
|
|
|
def rewire(self, new_node, near_inds):
|
|
"""
|
|
For each node in near_inds, this will check if it is cheaper to
|
|
arrive to them from new_node.
|
|
In such a case, this will re-assign the parent of the nodes in
|
|
near_inds to new_node.
|
|
Parameters:
|
|
----------
|
|
new_node, Node
|
|
Node randomly added which can be joined to the tree
|
|
|
|
near_inds, list of uints
|
|
A list of indices of the self.new_node which contains
|
|
nodes within a circle of a given radius.
|
|
Remark: parent is designated in choose_parent.
|
|
|
|
"""
|
|
for i in near_inds:
|
|
near_node = self.node_list[i]
|
|
edge_node = self.steer(new_node, near_node)
|
|
if not edge_node:
|
|
continue
|
|
edge_node.cost = self.calc_new_cost(new_node, near_node)
|
|
|
|
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:
|
|
for node in self.node_list:
|
|
if node.parent == self.node_list[i]:
|
|
node.parent = edge_node
|
|
self.node_list[i] = edge_node
|
|
self.propagate_cost_to_leaves(self.node_list[i])
|
|
|
|
def calc_new_cost(self, from_node, to_node):
|
|
d, _ = self.calc_distance_and_angle(from_node, to_node)
|
|
return from_node.cost + d
|
|
|
|
def propagate_cost_to_leaves(self, parent_node):
|
|
|
|
for node in self.node_list:
|
|
if node.parent == parent_node:
|
|
node.cost = self.calc_new_cost(parent_node, node)
|
|
self.propagate_cost_to_leaves(node)
|
|
|
|
|
|
def main():
|
|
print("Start " + __file__)
|
|
|
|
# ====Search Path with RRT====
|
|
obstacle_list = [
|
|
(5, 5, 1),
|
|
(3, 6, 2),
|
|
(3, 8, 2),
|
|
(3, 10, 2),
|
|
(7, 5, 2),
|
|
(9, 5, 2),
|
|
(8, 10, 1),
|
|
(6, 12, 1),
|
|
] # [x,y,size(radius)]
|
|
|
|
# Set Initial parameters
|
|
rrt_star = RRTStar(
|
|
start=[0, 0],
|
|
goal=[6, 10],
|
|
rand_area=[-2, 15],
|
|
obstacle_list=obstacle_list,
|
|
expand_dis=1,
|
|
robot_radius=0.8)
|
|
path = rrt_star.planning(animation=show_animation)
|
|
|
|
if path is None:
|
|
print("Cannot find path")
|
|
else:
|
|
print("found path!!")
|
|
|
|
# Draw final path
|
|
if show_animation:
|
|
rrt_star.draw_graph()
|
|
plt.plot([x for (x, y) in path], [y for (x, y) in path], 'r--')
|
|
plt.grid(True)
|
|
plt.show()
|
|
|
|
|
|
if __name__ == '__main__':
|
|
main()
|