RRT-Connect

1. RRT-Connect

• RRT 기반의 변형 알고리즘 중 하나
• 시작 지점과 도착 지점에서 각각의 트리를 시작하여, 두 트리가 만날 때까지 확장하는 경로 계획 방법

1. RRT-Connect 의사코드

2. RRT-Connect 알고리즘

1. 초기화

• 시작 지점 $q_{init}$과 도착 지점 $q_{goal}$를 기점으로 각각의 트리 $T_a$와 $T_b$ 를 초기화 하고, 정의된 반복 횟수 내에서 반복하며 확장

2. 연결

• 랜덤 샘플 $q_{rand}$을 선택하고, 도착 지점 $q_{goal}$을 기점으로 구성된 트리에서 가장 가까운 점 사이에 장애물이 없다면 해당 트리에 연결
• 해당 노드를 목표 노드 $q_{target}$이라 정의

3. 선택

• 목표 노드 $q_{target}$와 시작 지점 $q_{init}$를 기점으로 생성된 트리에서 가까운 지점 $q_{near}$를 찾는다.

4. 연결

• $q_{near}$노드에서 $q_{target}$ 노드 방향으로 정의된 길이만큼 떨어진 공간에 $q_{new}$ 를 생성
• $q_{new}$ 이 장애물 등에 충돌하지 않는다면, 반복해서 $q_{target}$ 노드 방향으로 확장

5. 경로 계획 결과

• 만약 $q_{new}$ 이 장애물 등에 충돌한다면, 확장을 멈추고 두 개의 트리를 스왑(SWAP)
  • 랜덤 샘플을 선택하여, 하나의 트리에 연결하고, 해당 노드와 다른 트리를 연결하는 연산을 반복

• 확장을 반복하여 두 개의 트리가 연결되면, 시작 지점에서 도착 지점 사이의 노드들을 연결하여 경로를 생성

3. RRT-Connect 코드

import numpy as np
import random
import math
import matplotlib.pyplot as plt
# from matplotlib.patches import Circle, Rectangle
# from matplotlib.animation import FuncAnimation
# from matplotlib.lines import Line2D


class Node:     # 트리의 각 노드를 나타내는 클래스를 정의
    def __init__(self, x, y):
        self.x = x  # 노드의 x 좌표
        self.y = y  # 노드의 y 좌표
        self.parent = None  # 트리에서의 부모 노드

def distance(node1, node2): # 두 노드 사이의 유클리드 거리를 계산하는 함수
    return math.sqrt((node1.x-node2.x)**2 + (node1.y-node2.y)**2)  # 점과 점 사이 거리 공식(루트((x1-x2)제곱+(y1-y2)제곱))

def find_nearest_node(tree, node):  # 주어진 노드에 가장 가까운 트리 내의 노드를 찾는 함수
    nearest_node = tree[0]      # 가장 가까운 노드를 일단 트리의 첫 번째 노드로 설정

    for t_node in tree:  # 트리 내의 모든 노드에 대해 반복하며 가장 가까운 노드를 찾음
        dist = distance(t_node, node)  # 샘플링된 노드와 트리 의 노드 사이의 거리 계산(가장 짧은 거리가 나오는 노드가 선택됨)
        if dist < distance(nearest_node, node):
            nearest_node = t_node    # 더 가까운 노드를 nearest_node로 설정
    
    return nearest_node

def extend(tree, node, step_size):  # 주어진 노드를 기준으로 트리를 확장하는 함수
    nearest_node = find_nearest_node(tree, node)    # 주어진 노드에 가장 가까운 트리 내의 노드를 찾음
    new_node = Node(0,0)    # 새로운 노드를 생성
    theta = math.atan2(node.y-nearest_node.y, node.x-nearest_node.x)   # 두 노드 사이의 각도를 계산

    # 새로운 노드의 좌표를 계산
    new_node.x = nearest_node.x + step_size + math.cos(theta)
    new_node.y = nearest_node.y + step_size + math.sin(theta)
    new_node.parent = nearest_node   # 새로운 노드의 부모를 가장 가까운 노드로 설정

    return new_node

def rrt_connect(start, goal, xmax, ymax, step_size, max_iter):  # RRT 연결 알고리즘을 통해 경로를 찾는 함수
    tree_a = [start]    # 시작 노드를 가지고 있는 트리를 생성
    tree_b = [goal]     # 목표 노드를 가지고 있는 트리를 생성

    # 주어진 반복 횟수만큼 RRT 알고리즘을 실행
    for i in range(max_iter):
        if i % 2 == 0:  # 짝수번째 반복에서는 tree_a를 확장
            random_node = Node(random.random() * xmax, random.random() * ymax)  # 임의의 노드를 생성
            new_node = extend(tree_a, random_node, step_size)    # 생성한 노드로부터 확장한 새로운 노드를 얻음

            if new_node:    # 새로운 노드가 생성되었다면,
                tree_a.append(new_node) # 트리에 새로운 노드를 추가
                nearest_node_b = find_nearest_node(tree_b, new_node)  # tree_b에서 새로운 노드에 가장 가까운 노드를 찾음
                if distance(new_node, nearest_node_b) < step_size:   # 두 노드 사이의 거리가 step_size보다 작다면,
                    new_node_b = extend(tree_b, new_node, step_size) # tree_b를 확장하고 새로운 노드를 얻음
                    if new_node_b: # tree_b의 새로운 노드가 생성되었다면,
                        tree_b.append(new_node_b)   # 새로운 노드를 tree_b에 추가
                        return tree_a, tree_b, new_node, new_node_b # 경로를 찾았으므로 결과를 반환
        else:   # 홀수번째 반복에서는 tree_b를 확장
            random_node = Node(random.random() * xmax, random.random() * ymax)  # 임의의 노드를 생성
            new_node = extend(tree_b, random_node, step_size) # 생성한 노드로부터 확장한 새로운 노드를 얻음

            # tree_a 노드 확장 메커니즘과 동일하게 진행
            if new_node: 
                tree_b.append(new_node)
                nearest_node_a = find_nearest_node(tree_a, new_node)
                if distance(new_node, nearest_node_a) < step_size:
                    new_node_a = extend(tree_a, new_node, step_size)
                    if new_node_a:
                        tree_a.append(new_node_a)
                        return tree_a, tree_b, new_node, new_node_a
    
    return None, None, None, None # 경로를 찾지 못한 경우 None을 반환

# 트리 및 연결된 경로를 시각화하는 함수
def plot_trees(tree_a, tree_b, connnection, xmax, ymax):
    if connnection: # 연결된 경로가 있다면,
        node_a, node_b = connnection  # 연결된 두 노드를 가져옴

    fig, ax = plt.subplots()

    ax.plot(tree_a[0].x, tree_a[0].y, 'go', markersize=10, label="Start")  # 시작 노드를 초록색으로 표시
    ax.plot(tree_b[0].x, tree_b[0].y, 'mo', markersize=10, label="Target")  # 목표 노드를 자주색으로 표시

    # 트리 A의 각 노드와 부모 노드를 이어서 그림
    for node in tree_a:
        if node.parent:
            ax.plot([node.x, node.parent.x],[node.y, node.parent.y], 'ro-', markersize=5)
    
    # 트리 B의 각 노드와 부모 노드를 이어서 그림
    for node in tree_b:
        if node.parent:
            ax.plot([node.x, node.parent.x], [node.y, node.parent.y], 'bo-', markersize=5)
    
    if connnection: # 연결된 경로가 있다면, 연결된 두 노드를 노란색으로 표시
        ax.plot([node_a.x, node_b.x], [node_a.y, node_b.y], 'yo-', markersize=5, label="Connection")

    # 그래프의 범위를 설정
    ax.set_xlim(0, xmax)
    ax.set_ylim(0, ymax)
    plt.xlabel("X"); plt.ylabel("Y")
    plt.title("RRT-Connection Path Exploration")
    plt.legend()
    plt.grid(True)
    plt.show()

def main():
    start = Node(1,1) # 시작 노드 생성
    goal = Node(9,9) # 목표 노드 생성
    xmax, ymax = 10, 10 # 그래프의 최대 x 및 y 좌표값 설정
    step_size = 0.5 # 확장하는 스탭 크기 설정
    max_iter = 1000 # 반복 횟수 설정

    # RRT-Connect 알고리즘을 통해 경로를 찾음
    tree_a, tree_b, connection_a, connection_b = rrt_connect(start, goal, xmax, ymax, step_size, max_iter)
    connection = (connection_a, connection_b) if connection_a and connection_b else None  # 연결된 노드가 있는지 확인
    plot_trees(tree_a, tree_b, connection, xmax, ymax) # 트리와 연결된 경로를 시각화

    if tree_a and tree_b: # 트리 A와 B가 모두 생성되었다면,
        print("Path Found") # 경로가 찾아졌음을 표시
    else:
        print("Path Not Found") 

if __name__ == "__main__":
    main()