Newer
Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
"""
RRT*
"""
import sys
sys.path.append("c:\\Users\\rmoan2\\guided_mrmp_24")
from Utils import Node, Env, Plotting
from SingleAgentPlanners import RRT
class RRTStar(RRT):
def __init__(self, s_start, s_goal, step_len, goal_sample_rate, iter_max, r):
super().__init__(s_start, s_goal, step_len, goal_sample_rate, iter_max)
self.r = r
self.name="RRT*"
def nearest_neighbor(self, node_list, node):
# print("using correct nn funct")
node_new = super().nearest_neighbor(node_list, node)
# node_new = self.new_state(node_nearest, node)
if node_new:
# rewire optimization
for node_n in node_list:
# inside the optimization circle
new_dist = self.dist(node_n, node_new)
if new_dist < self.r:
cost = node_n.g + new_dist
# update new sample node's cost and parent
if node_new.g > cost and not self.is_collision(node_n, node_new):
node_new.parent = node_n.current
node_new.g = cost
else:
# update nodes' cost inside the radius
cost = node_new.g + new_dist
if node_n.g > cost and not self.is_collision(node_n, node_new):
node_n.parent = node_new.current
node_n.g = cost
else:
continue
return node_new
else:
return None
if __name__ == "__main__":
x_start = (18, 8) # Starting node
x_goal = (37, 18) # Goal node
# rrt = RRT(x_start, x_goal, 0.5, 0.05, 10000)
# path = rrt.run()
rrtstar = RRTStar(x_start, x_goal, 0.5, 0.05, 10000, r=10.0)
path = rrtstar.run()