import random
import math
import matplotlib.pyplot as plt
import time
class Map2D:
def __init__( self , width=10 , height=10 ):
self.width = width
self.height = height
self.obstacles = [ (2, 10, 3, 2) , (6,0,7,8,) ]
def is_in_obstacle( self , x , y ):
for ( x_min , y_min , x_max , y_max ) in self.obstacles:
if( x_min <= x_max) and ( y_min <= y_max ):
if ( x_min <= x <= x_max ) and ( y_min <= y <= y_max ):
return True
elif ( x_max <= x_min ) and ( y_min <= y_max ):
if ( x_max <= x <= x_min ) and ( y_min <= y <= y_max ):
return True
elif ( x_max <= x_min ) and ( y_max <= y_min ):
if ( x_max <= x <= x_min ) and ( y_max <= y <= y_min ):
return True
elif ( x_min <= x_max ) and ( y_max <= y_min ):
if ( x_min <= x <= x_max ) and ( y_max <= y <= y_min ):
return True
return False
def plot_map( self ):
fig , ax = plt.subplots()
ax.set_xlim( 0 , self.width )
ax.set_ylim( 0 , self.height )
ax.set_aspect("equal" , adjustable="box" )
ax.set_title("2d map")
for ( x_min , y_min , x_max , y_max ) in self.obstacles:
rect_width = x_max - x_min
rect_height = y_max - y_min
obstacle_rect = plt.Rectangle( (x_min,y_min) , rect_width , rect_height , fill=True , alpha=0.4 ) # alpha는 불투명도
ax.add_patch( obstacle_rect )
return fig , ax
class Node:
def __init__( self , x , y , parent=None , cost=None ):
self.x , self.y , self.parent , self.cost = x , y, parent , cost
self.children = []
class RRTSmart:
def __init__( self , map_2d , start , goal , step_size=0.5 , max_iter=100 , goal_sample_rate=0.05 , dist_threshold=0.3 , near_neighbor_dist=0.5 ):
self.map = map_2d
self.start , self.goal = Node( start[0] , start[1] , cost=0 ) , Node( goal[0] , goal[1] )
self.step_size , self.max_iter = step_size , max_iter
self.goal_sample_rate , self.dist_threshold = goal_sample_rate , dist_threshold
self.tree = [ self.start ]
self.fig , self.ax = self.map.plot_map()
self.near_neighbor_dist = near_neighbor_dist
def distance( self , from_node , to_node ):
return math.hypot( to_node.x - from_node.x , to_node.y - from_node.y )
def get_random_node( self ):
if random.random() < self.goal_sample_rate:
return Node( self.goal.x , self.goal.y )
else:
x , y = random.uniform( 0, self.map.width ) , random.uniform( 0 , self.map.height )
return Node( x , y )
def nearest_node( self , tree , node ):
return min( tree , key=lambda nd: self.distance( nd , node ) )
def collision_free( self , node1 , node2 ):
steps = max( int( self.distance( node1 , node2 ) / (self.step_size * 0.5 ) ) , 1 )
for i in range( steps + 1):
t = i/steps
x = node1.x + t*( node2.x - node1.x )
y = node1.y + t*( node2.y - node1.y )
# print(f'{x} , {y}')
if self.map.is_in_obstacle( x , y ):
return False
return True
def steer( self , from_node , to_node ):
dist = self.distance( from_node , to_node )
if dist < self.step_size:
new_node = Node( to_node.x , to_node.y )
return new_node , dist
else:
theta = math.atan2( to_node.y - from_node.y , to_node.x - from_node.x )
new_x = from_node.x + self.step_size * math.cos( theta )
new_y = from_node.y + self.step_size * math.sin( theta )
new_node = Node( new_x , new_y )
return new_node , dist
def get_near_neighbors( self , node ):
rst = []
for nd in self.tree:
if self.distance(node,nd) <= self.near_neighbor_dist:
if self.collision_free( node , nd ):
rst.append( nd )
return rst
def get_nearest_neighbor( self , node , near_nodes , tmp_dist ):
rtn_node = node
rtn_dist = tmp_dist
for nd in near_nodes:
dist_neighbor = self.distance( node , nd )
if dist_neighbor < rtn_dist:
rtn_dist = dist_neighbor
rtn_node = nd
return rtn_node , rtn_dist
def rewire( self , new_node , near_nodes ):
for nd in near_nodes:
potential_cost = new_node.cost + self.distance( new_node , nd )
if potential_cost < nd.cost:
if nd.parent:
nd.parent.children.remove(nd)
nd.parent = new_node
new_node.children.append(nd)
nd.cost = potential_cost
# self.ax.plot([new_node.x, nd.x], [new_node.y, nd.y],color='purple', linewidth=3, alpha=0.3)
self._update_subtree_cost(nd)
def _update_subtree_cost( self , node ):
for c in node.children:
new_cost = node.cost + self.distance( node , c )
if new_cost < c.cost:
c.cost = new_cost
self._update_subtree_cost(c)
def build(self):
print('rrt smart알고리즘 시작\n우선 rrt star를 이용하기 초기 경로를 찾습니다.' )
start_time = time.time()
for i in range( self.max_iter ):
q_rand = self.get_random_node() # 랜덤하게 뽑고
q_nearest = self.nearest_node( self.tree , q_rand )# 가장 가까운 지점 찾고( q_new의 부모가 될 후보... )
q_new , _ = self.steer( q_nearest , q_rand ) # 곧 추가시킬 q_new를 찾고,
if not self.collision_free( q_new , q_nearest ): # q_new와 q_nearest가 부딪히는지 확인
continue
near_nodes = self.get_near_neighbors( q_new ) # q_new에서 일정한 반경내의 부모가 될 후보들을 찾는다.
# q_nearest , dist_with_qNew_qNearest = get_nearest_neighbor( q_new , near_nodes , dist_with_qNew_qNearest ) # q_new가 가장 가까운 부모를 찾는다.
# q_new에서 가장 가까운 부모로 이어준다.
best_parent = q_nearest
best_cost = q_nearest.cost + self.distance( q_nearest , q_new )
for nd in near_nodes:
cost_candiate = nd.cost + self.distance( nd , q_new )
if cost_candiate < best_cost:
best_parent = nd
best_cost = cost_candiate
q_new.parent = best_parent
q_new.cost = best_cost
best_parent.children.append(q_new)
self.tree.append(q_new)
self.rewire( q_new , near_nodes )
self.ax.plot( [q_nearest.x , q_new.x] , [q_nearest.y , q_new.y] , '-g' , linewidth=2.5 , alpha=0.5)
if self.distance( q_new , self.goal ) < self.dist_threshold:
end_time = time.time()
print(f'경로 찾음! 걸린 시간 : {end_time - start_time: .4f}초')
self.goal.parent = self.tree[-1]
self.goal.cost = self.goal.parent.cost + self.distance( self.goal.parent , self.goal )
path=self.reconstruct_path()
return path
return None
def reconstruct_path( self ):
path = [ ]
cur = self.goal
# print(cur.parent)
while cur is not None:
path.append( ( cur.x , cur.y ) )
cur = cur.parent
path.reverse()
print(f'최종적인 경로의 길이 {len(path)} , {self.goal.cost}')
return path
def visualize_path(self, path):
if path is None:
print("error")
return
# 1. 트리의 모든 노드 위치를 검정색 점으로 표시
node_x = [node.x for node in self.tree]
node_y = [node.y for node in self.tree]
self.ax.scatter(node_x, node_y, c='k', s=5, label='Nodes') # 'k'는 검정색, s는 점 크기
# 2. 경로가 있으면 경로 시각화
px, py = zip(*path)
self.ax.plot(px, py, '-r', linewidth=0.5, label='Final Path') # 최종 경로(빨간색 선)
self.ax.plot(self.start.x, self.start.y, 'bo', label='Start') # 시작점(파란 원)
self.ax.plot(self.goal.x, self.goal.y, 'bx', label='Goal') # 목표점(파란 X)
# self.ax.legend() # 필요하면 범례 활성화
plt.show()
import random
import math
import matplotlib.pyplot as plt
import time
class Map2D:
def __init__( self , width=100 , height=100 ):
self.width = width
self.height = height
self.obstacles = [ (20, 100, 30, 20) , (60,0,70,80,) , (40,40,50,50) , (80,80,90,90)]
def is_in_obstacle( self , x , y ):
for ( x_min , y_min , x_max , y_max ) in self.obstacles:
if( x_min <= x_max) and ( y_min <= y_max ):
if ( x_min <= x <= x_max ) and ( y_min <= y <= y_max ):
return True
elif ( x_max <= x_min ) and ( y_min <= y_max ):
if ( x_max <= x <= x_min ) and ( y_min <= y <= y_max ):
return True
elif ( x_max <= x_min ) and ( y_max <= y_min ):
if ( x_max <= x <= x_min ) and ( y_max <= y <= y_min ):
return True
elif ( x_min <= x_max ) and ( y_max <= y_min ):
if ( x_min <= x <= x_max ) and ( y_max <= y <= y_min ):
return True
return False
def plot_map( self ):
fig , ax = plt.subplots()
ax.set_xlim( 0 , self.width )
ax.set_ylim( 0 , self.height )
ax.set_aspect("equal" , adjustable="box" )
ax.set_title("2d map")
for ( x_min , y_min , x_max , y_max ) in self.obstacles:
rect_width = x_max - x_min
rect_height = y_max - y_min
obstacle_rect = plt.Rectangle( (x_min,y_min) , rect_width , rect_height , fill=True , alpha=0.4 ) # alpha는 불투명도
ax.add_patch( obstacle_rect )
return fig , ax
if __name__ == "__main__":
# 맵 생성
my_map = Map2D()
# 시작점, 목표점
start_pos = (10, 10)
goal_pos = (90, 10)
# RRT-Connect 객체 생성
rrt_connect = RRTSmart(
map_2d=my_map,
start=start_pos,
goal=goal_pos,
step_size=0.3, # 확장 시 한 번에 이동할 거리
max_iter=15000, # 최대 반복 횟수
goal_sample_rate=0.1 # goal을 직접 샘플링할 확률(10%)
)
# 알고리즘 실행
final_path = rrt_connect.build()
# print(final_path)
# 결과 시각화
rrt_connect.visualize_path(final_path)
class RRTInformed:
def __init__( self , map_2d , start , goal , step_size=0.5 , max_iter=100 , goal_sample_rate=0.05 , dist_threshold=0.3 , near_neighbor_dist=1 ):
self.map = map_2d
self.start , self.goal = Node( start[0] , start[1] , cost=0 ) , Node( goal[0] , goal[1] )
self.step_size , self.max_iter = step_size , max_iter
self.goal_sample_rate , self.dist_threshold = goal_sample_rate , dist_threshold
self.tree = [ self.start ]
self.fig , self.ax = self.map.plot_map()
self.near_neighbor_dist = near_neighbor_dist
# ============================= 유틸리티 =====================================
def distance( self , from_node , to_node ):
return math.hypot( to_node.x - from_node.x , to_node.y - from_node.y )
def collision_free( self , node1 , node2 ):
steps = max( int( self.distance( node1 , node2 ) / (self.step_size * 0.5 ) ) , 1 )
for i in range( steps + 1):
t = i/steps
x = node1.x + t*( node2.x - node1.x )
y = node1.y + t*( node2.y - node1.y )
if self.map.is_in_obstacle( x , y ):
return False
return True
def _update_subtree_cost( self , node ):
for child in node.children:
new_cost = node.cost + self.distance( node , child )
child.cost = new_cost
self._update_subtree_cost(child)
# ============================= RRT*로직 =====================================
# RRT*-1
def random_sampling( self ):
if random.random() < self.goal_sample_rate:
return Node( self.goal.x , self.goal.y )
else:
x , y = random.uniform( 0 , self.map.width ) , random.uniform( 0 , self.map.height )
return Node( x , y )
# RRT*-2
def get_near_node_WhereInTree( self , tree , q_rand ):
return min( tree , key=lambda nd: self.distance( nd , q_rand ) )
# RRT*-3
def steer( self , from_node , to_node ):
dist = self.distance( from_node , to_node )
if dist < self.step_size:
q_new = Node( to_node.x , to_node.y , parent=from_node )
return q_new
else:
theta = math.atan2( to_node.y - from_node.y , to_node.x - from_node.x )
new_x = from_node.x + self.step_size * math.cos( theta )
new_y = from_node.y + self.step_size * math.sin( theta )
q_new = Node( new_x , new_y , parent=from_node )
return q_new
# RRT*-5
def qNew_choose_bestParent( self , q_new ):
rst = []
for nd in self.tree:
if self.distance( q_new ,nd ) <= self.near_neighbor_dist:
if self.collision_free( q_new , nd ):
rst.append( nd )
if len(rst)==0:
q_new.cost = q_new.parent.cost + self.distance( q_new.parent , q_new )
q_new.parent.children.apprend(q_new)
self.tree.append(q_new)
return rst
else:
best_parent = q_new.parent
best_cost = q_new.parent.cost + self.distance( q_new.parent , q_new )
for parent_candidate in rst:
candidate_cost = parent_candidate.cost + self.distance( parent_candidate , q_new )
if candidate_cost < best_cost:
best_parent , best_cost = parent_candidate , candidate_cost
q_new.parent= best_parent
q_new.cost = best_cost
q_new.parent.children.append(q_new)
self.tree.append(q_new)
return rst
# RRT*-6
def rewire( self , q_new , nodes ):
for child_candidate in nodes:
candidate_cost = q_new.cost + self.distance( q_new , child_candidate )
if candidate_cost < child_candidate.cost:
if child_candidate.parent:
child_candidate.parent.children.remove( child_candidate )
child_candidate.parent = q_new
q_new.children.append( child_candidate )
child_candidate.cost = candidate_cost
self._update_subtree_cost( child_candidate)
# =============================== build =====================================
def build(self):
start_time = time.time()
for i in range( self.max_iter ):
# 1. 우선 RRT-star로 경로를 찾는다
if self.goal.cost==None:
q_rand = self.random_sampling() # RRT*-1 : 랜덤샘플링
q_near = self.get_near_node_WhereInTree( self.tree , q_rand ) # RRT*-2 : tree에서 q_rand와 가장 가까운 노드 찾기
q_new = self.steer( q_near , q_rand )# RRT*-3 : tree의 q_near에서 q_rand방향으로 뻗어가는 q_new를 찾기
if not self.collision_free( q_new , q_near ): # RRT*-4 : q_new와 q_near사이에 장애물 존재시 ==> 처음부터 다시 random sampling
continue
qNew_bestParent_candidate = self.qNew_choose_bestParent( q_new ) # RRT*-5 : q_new의 현재 부모보다 더 가까운 부모가 있는지 찾아본다.
self.rewire( q_new , qNew_bestParent_candidate ) # RRT*-6 : 트리에 존재하는 q_new근처 노드에서 q_new가 부모가 되었을때 최적인 경우를 찾아본다.
self.ax.plot( [q_new.parent.x , q_new.x] , [q_new.parent.y , q_new.y] , '-g' , linewidth=2.5 , alpha=0.5)
if self.distance( q_new , self.goal ) < self.dist_threshold:
end_time = time.time()
print(f'informed RRT에서 최초 경로를 찾음! 걸린 시간 : {end_time - start_time: .4f}초')
self.goal.parent = self.tree[-1]
self.goal.cost = self.goal.parent.cost + self.distance( self.goal.parent , self.goal )
path=self.reconstruct_path()
# else:
return path
def reconstruct_path( self ):
path = [ ]
cur = self.goal
# print(cur.parent)
while cur is not None:
path.append( ( cur.x , cur.y ) )
cur = cur.parent
path.reverse()
print(f'최종적인 경로의 길이 {len(path)} , {self.goal.cost}')
return path
def visualize_path(self, path):
if path is None:
print("error")
return
# 1. 트리의 모든 노드 위치를 검정색 점으로 표시
node_x = [node.x for node in self.tree]
node_y = [node.y for node in self.tree]
self.ax.scatter(node_x, node_y, c='k', s=5, label='Nodes') # 'k'는 검정색, s는 점 크기
# 2. 경로가 있으면 경로 시각화
px, py = zip(*path)
self.ax.plot(px, py, '-r', linewidth=1, label='Final Path') # 최종 경로(빨간색 선)
self.ax.plot(self.start.x, self.start.y, 'bo', label='Start') # 시작점(파란 원)
self.ax.plot(self.goal.x, self.goal.y, 'bx', label='Goal') # 목표점(파란 X)
# self.ax.legend() # 필요하면 범례 활성화
plt.show()
그냥 RRT*임