-
Notifications
You must be signed in to change notification settings - Fork 0
/
RRT_star.py
172 lines (152 loc) · 6.86 KB
/
RRT_star.py
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
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
import random
import math
ROW = 0
COL = 0
class Node:
def __init__(self, position):
self.position = position
self.parent = None
self.cost = 0.0
class RRT_star:
def __init__(self, max_iterations=10000, step_size=3, search_radius=10,exploration_constant=0.9):
self.max_iterations = max_iterations
self.step_size = step_size
self.search_radius = search_radius
self.exploration_constant=exploration_constant
self.path = []
def distance(self, node1, node2):
return math.sqrt((node1.position[0] - node2.position[0]) ** 2 + (node1.position[1] - node2.position[1]) ** 2)
def is_valid(self,row, col):
return (row >= 0) and (row < ROW) and (col >= 0) and (col < COL)
def is_collision_free(self, node1, node2,robot_radius):
x1, y1 = node1.position
x2, y2 = node2.position
line_points = self.bresenham(x1, y1, x2, y2)
for x, y in line_points:
# if self.grid[int(x)][int(y)] == 1:
if(not self._is_collision_free(self.grid,x,y,robot_radius)):
return False
return True
def _is_collision_free(self,grid, row, col,robot_radius):
for i in range(-robot_radius, robot_radius + 1):
for j in range(-robot_radius, robot_radius + 1):
check_row = row + i
check_col = col + j
# Check if the cell is within grid bounds
if (self.is_valid(check_row,check_col)):
# If any cell within the robot's radius is an obstacle, return False
if grid[check_row][check_col] == 1:
return False
else:
# If the cell is out of grid bounds, consider it as an obstacle
return False
return True
def calculate_adaptive_radius(self, num_nodes):
dimension = 2 # For 2D space
volume = self.grid.shape[0] * self.grid.shape[1] # Total area of the grid
gamma = 2 * (1 + 1/dimension) * (volume / math.pi) ** (1/dimension)
return min(self.search_radius, gamma * (math.log(num_nodes) / num_nodes) ** (1/dimension))
def bresenham(self, x1, y1, x2, y2):
points = []
dx = abs(x2 - x1)
dy = abs(y2 - y1)
sx = 1 if x1 < x2 else -1
sy = 1 if y1 < y2 else -1
err = dx - dy
while True:
points.append((x1, y1))
if x1 == x2 and y1 == y2:
break
e2 = err * 2
if e2 > -dy:
err -= dy
x1 += sx
if e2 < dx:
err += dx
y1 += sy
return points
def get_random_node(self):
while True:
if(random.randint(0, self.grid.shape[1] - 1)>(self.grid.shape[1]*self.exploration_constant)):
return self.goal
x = random.randint(0, self.grid.shape[1] - 1)
y = random.randint(0, self.grid.shape[0] - 1)
if self.grid[y, x] == 0:
return Node((y, x))
def get_nearest_node(self, random_node):
nearest_node = self.nodes[0]
min_distance = self.distance(nearest_node, random_node)
for node in self.nodes:
dist = self.distance(node, random_node)
if dist < min_distance:
nearest_node = node
min_distance = dist
return nearest_node
def get_nearby_nodes(self, new_node,adaptive_radius):
nearby_nodes = []
for node in self.nodes:
if self.distance(node, new_node) <= adaptive_radius:
nearby_nodes.append(node)
return nearby_nodes
def rewire(self, new_node, nearby_nodes,robot_radius):
for nearby_node in nearby_nodes:
if (self.is_collision_free(new_node, nearby_node,robot_radius)) and (new_node.cost + self.distance(new_node, nearby_node) < (1.0 * nearby_node.cost)):
nearby_node.parent = new_node
nearby_node.cost = new_node.cost + self.distance(new_node, nearby_node)
def make_plan(self,grid, start, goal,is_generator,robot_radius):
global ROW,COL
ROW=len(grid)
COL=len(grid[0])
print("rrt plannrer invoked")
self.grid = grid
self.start = Node(tuple(start))
self.start.cost=-1000
self.goal = Node(tuple(goal))
self.nodes = [self.start]
node_list=[]
fronterior_points=[]
node_graph=[]
for i in range(self.max_iterations):
random_node = self.get_random_node()
nearest_node = self.get_nearest_node(random_node)
theta = math.atan2(random_node.position[0] - nearest_node.position[0], random_node.position[1] - nearest_node.position[1])
new_position = (int(nearest_node.position[0] + self.step_size * math.sin(theta)),
int(nearest_node.position[1] + self.step_size * math.cos(theta)))
if (new_position[0] < 0) or (new_position[0] >= self.grid.shape[0]) or (new_position[1] < 0) or (new_position[1] >= self.grid.shape[1]):
continue
new_node = Node(new_position)
print(self.distance(new_node, nearest_node))
if not self.is_collision_free(nearest_node, new_node,robot_radius):
continue
new_node.parent = nearest_node
new_node.cost = nearest_node.cost + self.distance(nearest_node, new_node) + (2*grid[new_position[0]][[new_position[1]]])
adaptive_radius = self.calculate_adaptive_radius(len(self.nodes))
nearby_nodes = self.get_nearby_nodes(new_node,adaptive_radius)
self.rewire(new_node, nearby_nodes,robot_radius)
self.nodes.append(new_node)
node_list.append(new_node.position)
fronterior_points.append(new_node.position)
node_graph.append([new_node.position,new_node.parent.position])
if (self.distance(new_node, self.goal) <= self.step_size) and (self.is_collision_free(new_node, self.goal,robot_radius)):
self.goal.parent = new_node
self.goal.cost = new_node.cost + self.distance(new_node, self.goal)
self.nodes.append(self.goal)
self.path = self.extract_path()
yield self.path, node_graph, fronterior_points
node_graph=[]
for i in self.nodes:
if(i.parent!=None):
node_graph.append([i.position,i.parent.position])
if(is_generator):
yield None, node_graph, fronterior_points
if(is_generator):
yield None, None, None
def extract_path(self):
path = []
node = self.goal
while node.parent is not None:
path.append(node.position)
node = node.parent
path.append(self.start.position)
path.reverse()
return path