経路生成 (RRT-Connect) を実装してみた Part1

はじめに

私がロボットに関して興味を持っており，ロボットの経路を作成したいと思ったところ，RRT-Connect (Rapidly-exploring Random Tree) と呼ばれる経路生成の手法に辿り着いた．
本記事では，経路生成の古典的手法である RRT-Connect (Rapidly-exploring Random Tree) をPythonより，実装してみる．
前記事では，球の干渉物が存在する3次元空間を探索するRRTを実装した．
前記事の内容を下記に記します．
・https://qiita.com/haruhiro1020/items/61e477b156daa6b0cc40
本記事では，円形の干渉物が存在する2次元空間を探索する RRT-Connect を実装する．
RRT-Connectは下記論文を参考にした．
参考文献:
J.Kuffner "RRT-Connect: An Efficient Approach to Single-Query Path Planning.", ICRA 2000.
RRTとRRT-Connectの違いは後ほど説明する．

本記事で実装すること

・円形の干渉物が存在する2次元空間でのRRT-Connectによる経路生成
・経路生成のアニメーションを作成

本記事では実装できないこと(将来実装したい内容)

・球の干渉物が存在する3次元空間でのRRT-Connectによる経路生成

動作環境

・macOS Sequoia (バージョン15.5)
・Python3 (3.10.9)
・Numpy (1.23.5) (数値計算用ライブラリ)
・Matplotlib (3.7.0) (アニメーション作成用ライブラリ)

経路生成手法のまとめ

経路生成手法をまとめた記事を展開いたします．RRT以外の手法も記載してますので，参照いただけますと幸いです．
(https://qiita.com/haruhiro1020/items/000b3287fce082ae4283)

RRTに関して

RRTに関して説明する．
RRTは始点から，終点に向かってどんどんノードを増やしていき，終点との距離がある値以下になったら，経路生成を完了する．
RRTは下図のようなアルゴリズムである．

RRT-Connectに関して

RRT-Connectに関して説明する．
RRT-Connectは始点と終点の両方からノードを増やしていき，始点から終点までノードが繋がれば，経路生成を完了する．
RRT-Connectは下図のようなアルゴリズムである．

上図のRRT-Connectアルゴリズムより，状態遷移図を作成した．状態としては，下図の通りになる．

始点から終点までの経路生成(円形の干渉物が存在する2次元空間)

PythonによるRRT/RRT-Connectの実装は下記の通りです．

constant.py

# 複数ファイルで使用する定数の定義

# RRT-Connectの状態を定義
STATE_START_TREE_RANDOM   = 0  # 始点ツリーにランダム点を追加
STATE_START_TREE_END_TREE = 1  # 始点ツリーから終点ツリーへノードを伸ばす
STATE_END_TREE_RANDOM     = 2  # 終点ツリーにランダム点を追加
STATE_END_TREE_START_TREE = 3  # 終点ツリーから始点ツリーへノードを伸ばす

rrt.py

# 経路生成手法であるRRT (Rapidly-exploring Random Tree) の実装

# ライブラリの読み込み
import numpy as np
import time

# 自作モジュールの読み込み
from constant import *


class RRT:
    """
    RRTクラス
    
    プロパティ
        _pathes(numpy.ndarray): 始点から終点までの経路
        _debug_pathes(numpy.ndarray): デバッグ用経路
        _debug_random_pathes(numpy.ndarray): デバッグ用ランダム位置
        _strict_min_pos(numpy.ndarray): 探索の最小範囲
        _strict_max_pos(numpy.ndarray): 探索の最大範囲
        _interference_obj(list): 干渉物
    
    publicメソッド (全てのクラスから参照可能)
        planning(): 経路生成の実装
        pathes(): _pathesプロパティのゲッター
        debug_pathes(): _debug_pathesプロパティのゲッター
        debug_random_pathes(): _debug_random_pathesプロパティのゲッター
        strict_min_pos(): _strict_min_posプロパティのゲッター
        strict_max_pos(): _strict_max_posプロパティのゲッター
        interference_obj(): _interference_objプロパティのゲッター
    
    protectedメソッド (自クラスまたは子クラスが参照可能)
        _reset(): データの初期化
        _set_node(): データにノードを設定する
        _chk_end_pos_dist(): 終点との距離が一定範囲内であるかの確認
        _fin_planning(): 経路生成の終了処理
        _calc_new_pos(): 最短ノードからランダムな値方向へ新しいノード(位置)を作成
        _get_near_node(): ランダムな位置と最短距離であるノードを取得
        _strict_planning_pos(): 探索範囲を制限する
        _get_random_pos(): ランダムな位置を取得
        _set_interference_obj(): 干渉物を設定
        _is_interference_pos(): 干渉判定処理
    """
    # 定数の定義
    _INITIAL_NODE_NEAR = -1     # 初期値の最短距離ノード番号
    _MOVING_VALUE = 0.25        # 1回の移動量
    _STRICT_PLANNING_POS = 0.2  # 探索範囲の制限
    _TIMEOUT_VALUE = 10         # タイムアウト時間 [second]
    _FILE_NAME_PATHES = "pathes.csv"                # _pathesプロパティを保存するファイル名
    _FILE_NAME_DEBUG_PATHES = "debug_pathes.csv"    # _debug_pathesプロパティを保存するファイル名
    _FILE_NAME_DEBUG_RANDOM_PATHES = "debug_random_pathes.csv"  # _debug_random_pathesプロパティを保存するファイル名
    _NODE_RADIUS = 0.05         # ノードの半径
    _DIMENTION_2D = 2           # 2次元データの経路生成
    _DIMENTION_3D = 3           # 3次元データの経路生成
    
    def __init__(self):
        """
        コンストラクタ
        """
        self._debug_pathes = []
        self._debug_random_pathes = []
        self._pathes = []
        self._interference_obj = []
    
    @property
    def pathes(self):
        """
        _pathesプロパティのゲッター
        """
        return self._pathes
    
    @property
    def debug_pathes(self):
        """
        _debug_pathesプロパティのゲッター
        """
        return self._debug_pathes
    
    @property
    def debug_random_pathes(self):
        """
        _debug_random_pathesプロパティのゲッター
        """
        return self._debug_random_pathes
    
    @property
    def strict_min_pos(self):
        """
        _strict_min_posプロパティのゲッター
        """
        return self._strict_min_pos

    @property
    def strict_max_pos(self):
        """
        _strict_max_posプロパティのゲッター
        """
        return self._strict_max_pos
    
    @property
    def interference_obj(self):
        """
        _interference_objプロパティのゲッター
        """
        return self._interference_obj

    def _set_interference_obj(self, interference_obj, dim):
        """
        干渉物を設定

        パラメータ
            interference_obj(list): 干渉物
            dim(int): 始点/終点の次元数
        
        戻り値
            set_result(bool): True / False = 設定の成功 / 失敗
        """
        set_result = True
        if interference_obj:
            # 干渉物が存在する
            # 干渉物と始点/終点の次元数が一致しているかの確認 (# 変更 ↓)
            for interference in interference_obj:
                if dim != interference.dimention:
                    # 不一致
                    set_result = False
                    break

            if set_result:
                self._interference_obj = interference_obj
            # 変更 ↑
        else:
            self._interference_obj = []

        return set_result

    def _set_node(self, node):
        """
        データにノードを設定する

        パラメータ
            node(numpy.ndarray): ノード位置と最短距離ノード番号 (最終要素)
        """
        if len(self._debug_pathes) == 0:
            self._debug_pathes = node
        else:
            self._debug_pathes = np.append(self._debug_pathes, node, axis=0)
    
    def _set_random_node(self, node):
        """
        データにノードを設定する

        パラメータ
            node(numpy.ndarray): ノード位置と最短距離ノード番号 (最終要素)
        """
        if len(self._debug_random_pathes) == 0:
            self._debug_random_pathes = node
        else:
            self._debug_random_pathes = np.append(self._debug_random_pathes, node, axis=0)
    
    def planning(self, start_pos, end_pos, interference_obj):
        """
        経路生成

        パラメータ
            start_pos(numpy.ndarray): 経路生成の始点
            end_pos(numpy.ndarray): 経路生成の終点
            interference_obj(list): 干渉物
        
        戻り値
            result(bool): True / False = 成功 / 失敗
        """
        # 処理結果
        result = False
        
        # 始点と終点の次元数が一致しているかの確認
        start_pos_dim = np.size(start_pos)
        end_pos_dim   = np.size(end_pos)
        if start_pos_dim != end_pos_dim:
            # 次元数が異なるので異常
            return result

        # 干渉物を設定 (# 変更)
        if not self._set_interference_obj(interference_obj, start_pos_dim):
            # 干渉物と始点/終点の次元数が不一致のため，処理失敗
            return result

        # 始点ノードを設定
        self._set_node(np.append(start_pos, self._INITIAL_NODE_NEAR).reshape(1, -1))
        self._set_random_node(np.append(start_pos, self._INITIAL_NODE_NEAR).reshape(1, -1))
        # 探索する範囲を設定
        self._strict_planning_pos(start_pos, end_pos)
        
        # 経路生成は一定時間内に終了させる
        start_time = time.time()

        # 終点までの経路生成が終わるまでループ
        while True:
            # ランダムな値を取得
            random_pos = self._get_random_pos()
            # ランダムな値と最短ノードを計算
            near_node = self._get_near_node(random_pos)
            # 最短ノード位置を取得
            near_pos = self._debug_pathes[near_node, :-1]
            # 最短ノードからランダムな値方向へ新しいノード(位置)を作成
            new_pos = self._calc_new_pos(random_pos, near_pos)
            # 新しい点と干渉物との干渉判定
            is_interference = self._is_interference_pos(new_pos, start_pos_dim)
            if not is_interference:
                # 干渉なしのため，データを設定する
                # ランダムノードを設定
                self._set_random_node(np.append(random_pos, near_node).reshape(1, -1))
                # ノードを設定
                self._set_node(np.append(new_pos, near_node).reshape(1, -1))
                # 終点との距離が一定範囲内であるかの確認
                if self._chk_end_pos_dist(new_pos, end_pos):
                    # 一定範囲内のため，経路生成の完了
                    result = True
                    break
            
            # 干渉の有無に関わらずにタイムアウトの確認はする
            now_time = time.time()
            if now_time - start_time >= self._TIMEOUT_VALUE:
                # タイムアウト
                break
        
        if result:
            # 経路生成に成功のため，経路生成の終了処理
            self._fin_planning(start_pos, end_pos)

        return result

    def _is_interference_pos(self, pos, dim):
        """
        干渉判定処理

        パラメータ
            pos(numpy.ndarray): 干渉判定したい点
            dim(int): 次元数
        
        戻り値
            is_interference(bool): True / False = 干渉あり / なし
        """
        is_interference = False
        
        for interference in self._interference_obj:
            if dim == self._DIMENTION_2D:
                # RRTで生成した点は円形と仮定して，干渉判定
                if interference.is_interference_circle(pos, self._NODE_RADIUS):
                    # 干渉あり
                    is_interference = True
                    break
            else:
                # RRTで生成した点は球と仮定して，干渉判定
                if interference.is_interference_ball(pos, self._NODE_RADIUS):
                    # 干渉あり
                    is_interference = True
                    break

        return is_interference

    def _chk_end_pos_dist(self, pos, end_pos):
        """
        終点との距離が一定範囲内であるかの確認

        パラメータ
            pos(numpy.ndarray): ノード位置
            end_pos(numpy.ndarray): 経路生成の終点
        
        戻り値
            is_near(bool): True / False = 一定範囲内である / でない
        """
        is_near = False
        # 距離を計算
        dist = np.linalg.norm(end_pos - pos)
        # 一定範囲内であるかの確認
        if dist <= self._MOVING_VALUE:
            is_near = True
        
        return is_near

    def _fin_planning(self, start_pos, end_pos):
        """
        経路生成の終了処理

        パラメータ
            start_pos(list): 経路生成の始点
            end_pos(list): 経路生成の終点
        """
        # 始点から終点までの経路に関係するノードを選択
        revers_path = end_pos
        near_node = -1
        while True:
            # 終点から始点方向へノードを取得
            node = self._debug_pathes[near_node, :]
            pos = node[:-1].reshape(1, -1)
            # 浮動小数型になっているので，整数型に型変換
            near_node = int(node[-1])
            revers_path = np.append(revers_path, pos, axis=0)
            if near_node == self._INITIAL_NODE_NEAR:
                # 始点ノードまで取得できたため，処理終了
                break
        
        # 経路が終点からの順番になっているため，始点から終点とする
        self._pathes = revers_path[::-1]
        # デバッグ用の経路に終点を追加
        self._set_random_node(np.append(end_pos, self._debug_pathes.shape[0] - 1).reshape(1, -1))
        self._set_node(np.append(end_pos, self._debug_pathes.shape[0] - 1).reshape(1, -1))

    def _calc_new_pos(self, random_pos, near_pos):
        """
        最短ノード(位置)からランダムな値方向へ新しいノード(位置)を作成

        パラメータ
            random_pos(numpy.ndarray): ランダムな位置
            near_pos(numpy.ndarray): 最短ノード位置
        
        戻り値
            new_pos(numpy.ndarray): 新しいノード
        """
        # 方向を計算
        direction = random_pos - near_pos
        # 方向の大きさを1にする
        norm_direction = direction / np.linalg.norm(direction)
        # 新しいノードを作成
        new_pos = near_pos + norm_direction * self._MOVING_VALUE

        return new_pos

    def _get_near_node(self, random_pos):
        """
        ランダムな位置との最短距離ノードを取得

        パラメータ
            random_pos(numpy.ndarray): ランダムな位置
        
        戻り値
            min_dist_node(int): 最短距離ノード
        """
        # 最後以外に位置が保存されている
        node_pos = self._debug_pathes[:, :-1]
        # 差分を計算
        difference = node_pos - random_pos
        # 差分から距離を計算
        distance = np.linalg.norm(difference, axis=1)
        # 最小距離のノードを取得
        min_dist_node = np.argmin(distance)

        return min_dist_node

    def _strict_planning_pos(self, start_pos, end_pos):
        """
        探索範囲を制限する

        パラメータ
            start_pos(numpy.ndarray): 始点
            end_pos(numpy.ndarray): 終点
        """
        all_pos = np.append(start_pos, end_pos, axis=0)
        # 各列の最大/最小値を取得
        min_pos = np.min(all_pos, axis=0)
        max_pos = np.max(all_pos, axis=0)
        self._strict_min_pos = min_pos - self._STRICT_PLANNING_POS
        self._strict_max_pos = max_pos + self._STRICT_PLANNING_POS

    def _get_random_pos(self):
        """
        ランダムな位置を取得

        戻り値
            random_pos(numpy.ndarray): ランダムな位置
        """
        random_pos = np.random.uniform(self._strict_min_pos, self._strict_max_pos)

        return random_pos

    def save(self):
        """
        生成した経路をファイル保存
        """
        np.savetxt(self._FILE_NAME_PATHES, self._pathes)
        np.savetxt(self._FILE_NAME_DEBUG_PATHES, self._debug_pathes)
        np.savetxt(self._FILE_NAME_DEBUG_RANDOM_PATHES, self._debug_random_pathes)


class RRTConnect(RRT):
    """
    RRT-Connectクラス
    
    プロパティ
        _start_tree(numpy.ndarray): 始点ツリー
        _end_tree(numpy.ndarray): 終点ツリー
        _state(int): ツリーの状態
        _debug_states(list): ツリー状態のデバッグ
    
    publicメソッド (全てのクラスから参照可能)
        planning(): 経路生成の実装
        start_tree(): _start_treeプロパティのゲッター
        end_tree(): _end_treeプロパティのゲッター
        debug_states(): _debug_statesプロパティのゲッター
    
    protectedメソッド (自クラスまたは子クラスが参照可能)
        _set_node_start_tree(): 始点ツリーにノードを設定する
        _set_node_end_tree(): 終点ツリーにノードを設定する
        _fin_planning(): 経路生成の終了処理
        _get_near_node(): ランダムな位置と最短距離であるノードを取得
        _save_state(): 状態を保存
        _state_transition(): 状態を遷移させる
        _exec_state(): 状態に応じた処理を実施
    """
    # 定数の定義
    _FILE_NAME_START_TREE   = "start_tree.csv"    # _start_treeプロパティを保存するファイル名
    _FILE_NAME_END_TREE     = "end_tree.csv"      # _end_treeプロパティを保存するファイル名
    _FILE_NAME_DEBUG_STATES = "debug_states.csv"  # _debug_statesプロパティを保存するファイル名

    def __init__(self):
        """
        コンストラクタ
        """
        # 親クラスの初期化
        super().__init__()
        # プロパティの初期化
        self._start_tree = []
        self._end_tree = []
        self._state = STATE_START_TREE_RANDOM
        self._debug_states = []

    @property
    def start_tree(self):
        """
        _start_treeプロパティのゲッター
        """
        return self._start_tree

    @property
    def end_tree(self):
        """
        _end_treeプロパティのゲッター
        """
        return self._end_tree

    @property
    def debug_states(self):
        """
        _debug_statesプロパティのゲッター
        """
        return self._debug_states

    def _set_node_start_tree(self, node):
        """
        始点ツリーにノードを設定する

        パラメータ
            node(numpy.ndarray): ノード位置
        """
        if len(self._start_tree) == 0:
            self._start_tree = node
        else:
            self._start_tree = np.append(self._start_tree, node, axis=0)
    
    def _set_node_end_tree(self, node):
        """
        終点ツリーにノードを設定する

        パラメータ
            node(numpy.ndarray): ノード位置
        """
        if len(self._end_tree) == 0:
            self._end_tree = node
        else:
            self._end_tree = np.append(self._end_tree, node, axis=0)

    def planning(self, start_pos, end_pos, interference_obj):
        """
        経路生成

        パラメータ
            start_pos(numpy.ndarray): 経路生成の始点
            end_pos(numpy.ndarray): 経路生成の終点
            interference_obj(list): 干渉物
        
        戻り値
            result(bool): True / False = 成功 / 失敗
        """
        # 処理結果
        result = False
        
        # 始点と終点の次元数が一致しているかの確認
        start_pos_dim = np.size(start_pos)
        end_pos_dim   = np.size(end_pos)
        if start_pos_dim != end_pos_dim:
            # 次元数が異なるので異常
            return result

        # 干渉物を設定
        if not self._set_interference_obj(interference_obj, start_pos_dim):
            # 干渉物と始点/終点の次元数が不一致のため，処理失敗
            return result

        # 始点ツリーに始点ノードを設定
        self._set_node_start_tree(np.append(start_pos, self._INITIAL_NODE_NEAR).reshape(1, -1))
        # 終点ツリーに終点のーどを設定
        self._set_node_end_tree(np.append(end_pos, self._INITIAL_NODE_NEAR).reshape(1, -1))
        # 探索する範囲を設定
        self._strict_planning_pos(start_pos, end_pos)
        
        # 経路生成は一定時間内に終了させる
        start_time = time.time()

        # 終点までの経路生成が終わるまでループ
        while True:
            # 1処理実施して，経路生成が完了したか確認する
            if self._exec_state(start_pos_dim):
                # 経路生成の完了
                result = True
                break
            
            # 干渉の有無に関わらずにタイムアウトの確認はする
            now_time = time.time()
            if now_time - start_time >= self._TIMEOUT_VALUE:
                # タイムアウト
                break
        
        if result:
            # 経路生成に成功のため，経路生成の終了処理
            self._fin_planning(start_pos, end_pos)

        return result

    def _state_transition(self):
        """
        状態を遷移させる
        """
        if self._state == STATE_START_TREE_RANDOM:        # 始点ツリーにランダム点を追加する状態
            self._state = STATE_END_TREE_START_TREE
        elif self._state == STATE_START_TREE_END_TREE:    # 始点ツリーに終点ツリーで作成したノードへ伸ばす状態
            self._state = STATE_START_TREE_RANDOM
        elif self._state == STATE_END_TREE_RANDOM:        # 終点ツリーにランダム点を追加する状態
            self._state = STATE_START_TREE_END_TREE
        else:                                                   # 終点ツリーに始点ツリーで作成したノードへ伸ばす状態
            self._state = STATE_END_TREE_RANDOM

    def _save_state(self):
        """
        状態を保存
        """
        self._debug_states.append(self._state)

    def _exec_state(self, start_pos_dim):
        """
        状態(_state)に応じた処理を実施
        
        パラメータ
            start_pos_dim(int): 始点の次元数
        
        戻り値
            complete_path(bool): True / False = 経路生成の完了 / 未完了
        """
        complete_path = False
        
        # 最短ノードを算出したいツリーを取得
        if  self._state == STATE_START_TREE_RANDOM or self._state == STATE_START_TREE_END_TREE:
            # 始点ツリーにランダム点を追加する状態または始点ツリーに終点ツリーで作成したノードへ伸ばす状態
            my_tree = self._start_tree
            your_tree = self._end_tree
            set_tree_func = self._set_node_start_tree
        else:
            my_tree = self._end_tree
            your_tree = self._start_tree
            set_tree_func = self._set_node_end_tree

        # ツリーにランダム点を追加
        if  self._state == STATE_START_TREE_RANDOM or self._state == STATE_END_TREE_RANDOM:
            # 始点ツリーにランダム点を追加する状態または終点ツリーにランダム点を追加する状態
            # ランダムな値を取得する
            random_pos = self._get_random_pos()
            # ランダムな値とツリーの最短ノードを計算
            near_node = self._get_near_node(random_pos, my_tree)
            # 位置と最短ノードが保存されているから，位置だけを取得
            near_pos  = my_tree[near_node][:-1]
            # 最短ノードからランダムな値方向へ新しいノードを作成
            new_pos = self._calc_new_pos(random_pos, near_pos)
            # 干渉物との干渉判定
            is_interference = self._is_interference_pos(new_pos, start_pos_dim)
            if not is_interference:
                # ツリーにノードを追加
                set_tree_func(np.append(new_pos, near_node).reshape(1, -1))
                # 状態を保存
                self._save_state()
                # 干渉なしのため，状態を遷移させる
                self._state_transition()
        else:
            # ツリーで作成したノードを取得
            end_pos = your_tree[-1, :-1]
            # ツリー内の最短ノードを計算
            near_node = self._get_near_node(end_pos, my_tree)
            # 位置と最短ノードが保存されているから，位置だけを取得
            near_pos  = my_tree[near_node, :-1]
            # 最短ノードから終点ツリーのノード方向へ新しいノードを作成
            new_pos   = self._calc_new_pos(end_pos, near_pos)
            # 干渉物との干渉判定
            is_interference = self._is_interference_pos(new_pos, start_pos_dim)
            if is_interference:
                # 干渉ありのため，状態を遷移させる
                self._state_transition()
            else:
                # ツリーにノードを追加
                set_tree_func(np.append(new_pos, near_node).reshape(1, -1))
                # 状態を保存
                self._save_state()
                # 始点から終点までの経路が生成できたかの確認
                if self._chk_end_pos_dist(new_pos, end_pos):
                    # 経路生成の完了
                    complete_path = True

        return complete_path

    def _fin_planning(self, start_pos, end_pos):
        """
        経路生成の終了処理

        パラメータ
            start_pos(list): 経路生成の始点
            end_pos(list): 経路生成の終点
        """
        # 始点ツリーから，終点ツリーと繋がったパスを取得
        start_path = []
        near_node  = -1
        while True:
            # 終点ツリーと繋がったノードから始点へノードを取得
            node = self._start_tree[near_node, :]
            pos  = node[:-1].reshape(1, -1)
            # 浮動小数型になっているので，整数型に型変換
            near_node = int(node[-1])
            if len(start_path) == 0:
                # 初回の場合
                start_path = pos
            else:
                start_path = np.append(start_path, pos, axis=0)

            if near_node == self._INITIAL_NODE_NEAR:
                # 始点ノードまで取得できたため，ループ終了
                break

        # 始点ツリーのパスは，逆順(終点ツリーと繋がったノードから始点ノードの順番)に保存されているから，正順に変更
        self._pathes = start_path[::-1]

        # 終点ツリーから，始点ツリーと繋がったパスを取得
        end_path = []
        near_node = -1
        while True:
            # 始点ツリーと繋がったノードから終点へノードを取得
            node = self._end_tree[near_node, :]
            pos  = node[:-1].reshape(1, -1)
            # 浮動小数型になっているので，整数型に型変換
            near_node = int(node[-1])
            if len(end_path) == 0:
                # 初回の場合
                end_path = pos
            else:
                end_path = np.append(end_path, pos, axis=0)

            if near_node == self._INITIAL_NODE_NEAR:
                # 終点ノードまで取得できたため，ループ終了
                break

        # 始点から終点までの経路を保存
        self._pathes = np.append(self._pathes, end_path, axis=0)

    def _get_near_node(self, random_pos, tree):
        """
        ランダムな位置とツリーの最短距離ノードを取得
        
        パラメータ
            random_pos(numpy.ndarray): ランダムな位置
            tree(numpy.ndarray): ツリー
        
        戻り値
            min_dist_node(int): 最短距離ノード
        """
        # 最後以外に位置が保存されている
        node_pos = tree[:, :-1]
        # 差分を計算
        difference = node_pos - random_pos
        # 差分から距離を計算
        distance = np.linalg.norm(difference, axis=1)
        # 最小距離のノードを取得
        min_dist_node = np.argmin(distance)

        return min_dist_node

    def save(self):
        """
        生成した経路をファイル保存
        """
        np.savetxt(self._FILE_NAME_PATHES, self._pathes)
        np.savetxt(self._FILE_NAME_START_TREE, self._start_tree)
        np.savetxt(self._FILE_NAME_END_TREE, self._end_tree)
        np.savetxt(self._FILE_NAME_DEBUG_STATES, self._debug_states)

円形の干渉物との干渉判定

干渉物に関するソースコードを下記に記す．

interference.py

# 干渉物を定義する

# ライブラリの読み込み
import numpy as np


class Interference2D:
    """
    2次元の干渉物クラス(抽象クラス)
    
    プロパティ
        _center(numpy.ndarray): 中心位置(x, y)
        _radius(float): 半径
    
    publicメソッド (全てのクラスから参照可能)
        is_interference_circle(): 円形との干渉判定
        dimention(): _DIMENTION定数のゲッター
    """
    # 定数の定義
    _DIMENTION = 2      # 次元数
    
    def is_interference_circle(self, center, radius):
        """
        円形との干渉判定
        
        パラメータ
            center(numpy.ndarray): 中心位置(x, y)
            radius(float): 半径
        
        戻り値
            is_interference(bool): True / False = 干渉あり / なし
        """
        pass

    @property
    def dimention(self):
        """
        _DIMENTION定数のゲッター
        """
        return self._DIMENTION


class Circle(Interference2D):
    """
    円形の干渉物クラス
    
    プロパティ
        _center(numpy.ndarray): 中心位置(x, y)
        _radius(float): 半径
    
    publicメソッド (全てのクラスから参照可能)
        is_interference_circle(): 円形との干渉判定
        center(): _centerプロパティのゲッター
        radius(): _radiusプロパティのゲッター
    """
    # 定数の定義
    
    def __init__(self, center, radius):
        """
        コンストラクタ
        
        パラメータ
            center(numpy.ndarray): 中心位置(x, y)
            radisu(float): 半径
        """
        # プロパティの初期化
        self._center = center
        self._radius = radius
    
    @property
    def center(self):
        """
        _centerプロパティのゲッター
        """
        return self._center
    
    @property
    def radius(self):
        """
        _radiusプロパティのゲッター
        """
        return self._radius
    
    def is_interference_circle(self, center, radius):
        """
        円形との干渉判定
        
        パラメータ
            center(numpy.ndarray): 中心位置(x, y)
            radius(float): 半径
        
        戻り値
            is_interference(bool): True / False = 干渉あり / なし
        """
        is_interference = False
        # 2点間の差分を計算
        difference = self._center - center
        # 2点間の距離(ユークリッド距離)を算出
        distance = np.linalg.norm(difference)
        if distance <= self._radius + radius:
            # 干渉した
            is_interference = True
        
        return is_interference


class Interference3D:
    """
    3次元の干渉物クラス(抽象クラス)
    
    プロパティ
        _center(numpy.ndarray): 中心位置(x, y)
        _radius(float): 半径
    
    publicメソッド (全てのクラスから参照可能)
        is_interference_ball(): 球との干渉判定
        dimention(): _DIMENTION定数のゲッター
    """
    # 定数の定義
    _DIMENTION = 3      # 次元数
    
    def is_interference_ball(self, center, radius):
        """
        球との干渉判定
        
        パラメータ
            center(numpy.ndarray): 中心位置(x, y, z)
            radius(float): 半径
        
        戻り値
            is_interference(bool): True / False = 干渉あり / なし
        """
        pass

    @property
    def dimention(self):
        """
        _DIMENTION定数のゲッター
        """
        return self._DIMENTION

class Ball(Interference3D):
    """
    球の干渉物クラス
    
    プロパティ
        _center(numpy.ndarray): 中心位置(x, y, z)
        _radius(float): 半径
    
    publicメソッド (全てのクラスから参照可能)
        is_interference_ball(): 球との干渉判定
    """
    # 定数の定義
    
    def __init__(self, center, radius):
        """
        コンストラクタ
        
        パラメータ
            center(numpy.ndarray): 中心位置(x, y, z)
            radisu(float): 半径
        """
        # プロパティの初期化
        self._center = center
        self._radius = radius
    
    @property
    def center(self):
        """
        _centerプロパティのゲッター
        """
        return self._center
    
    @property
    def radius(self):
        """
        _radiusプロパティのゲッター
        """
        return self._radius
    
    def is_interference_ball(self, center, radius):
        """
        球との干渉判定
        
        パラメータ
            center(numpy.ndarray): 中心位置(x, y, z)
            radius(float): 半径
        
        戻り値
            is_interference(bool): True / False = 干渉あり / なし
        """
        is_interference = False
        # 2点間の差分を計算
        difference = self._center - center
        # 2点間の距離(ユークリッド距離)を算出
        distance = np.linalg.norm(difference)
        if distance <= self._radius + radius:
            # 干渉した
            is_interference = True
        
        return is_interference

経路生成のアニメーション

上記でRRT/RRT-Connectの実装をしたが，想定通りに動いているかの確認が必要なため，アニメーションを作成して確認する．
PythonによるRRT/RRT-Connectのアニメーション実装は下記の通りです．

rrt_animation.py

# RRTで作成した経路のアニメーションを作成する

# ライブラリの読み込み
import numpy as np
import matplotlib.pyplot as plt
import matplotlib.animation as ani
import matplotlib.patches as patches

# 自作モジュールの読み込み
from constant import *


class RRTAnimation:
    """
    RRTのアニメーション作成
    
    プロパティ
        _min_pos(numpy.ndarray): RRTの最小探索範囲
        _max_pos(numpy.ndarray): RRTの最大探索範囲
        _figure: 描画枠
        _axis: 描画内容
    
    publicメソッド (全てのクラスから参照可能)
        plot_2DAnimation(): 2D(2次元)アニメーション作成
        plot_3DAnimation(): 3D(3次元)アニメーション作成
    
    protectedメソッド (自クラスまたは子クラスが参照可能)
        _reset2D(): 2次元データのリセット
        _reset3D(): 3次元データのリセット
        _update_3Ddata(): 3次元各データの更新
    """
    # 定数の定義
    _ONE_CUT_NUM = 6        # 1回
    _ANIMATION_NAME = "rrt_animation.gif"
    
    def __init__(self, min_pos, max_pos):
        """
        コンストラクタ
        """
        self._min_pos = min_pos
        self._max_pos = max_pos
        
    def _reset2D(self):
        """
        2次元データのリセット
        """
        self._figure = plt.Figure()
        self._axis = self._figure.add_subplot(111)
        
        # X/Y軸に文字を記載
        self._axis.set_xlabel("X")
        self._axis.set_ylabel("Y")
        # プロットするX/Yの範囲を設定
        self._axis.set_xlim(self._min_pos[0], self._max_pos[0])
        self._axis.set_ylim(self._min_pos[1], self._max_pos[1])
        self._axis.grid()
        self._axis.set_aspect("equal")
    
    def _plot_interference_obj(self, interference_obj):
        """
        円形の干渉物のプロット
        
        パラメータ
            interference_obj(list): 円形の干渉物
        
        戻り値
            interference_images(list): 円形の干渉物
        """
        for interference in interference_obj:
            # 円形データをプロット
            circle = patches.Circle(interference.center, interference.radius, color='gray', alpha=0.5)
            self._axis.add_patch(circle)
    
    def plot_2DAnimation(self, pathes, debug_pathes, debug_random_pathes, interference_obj):
        """
        2D(2次元)アニメーション作成
        
        パラメータ
            pathes(numpy.ndarray): 経路
            debug_pathes(numpy.ndarray): デバッグ用経路
            debug_random_pathes(numpy.ndarray): デバッグ用ランダム経路
            interference_obj(list): 円形の干渉物
        
        戻り値
            plot_result(bbol): True / False = 成功 / 失敗
        """
        plot_result = False

        # データをリセットする
        self._reset2D()
        
        # 引数の確認
        if pathes.size == 0 or debug_pathes.size == 0 or debug_random_pathes.size == 0:
            return plot_result
        
        # デバッグ用経路とデバッグ用ランダム経路の行数が一致していることを確認
        if debug_pathes.shape[0] != debug_random_pathes.shape[0]:
            # 異常なデータ
            return plot_result
        
        # 円形の干渉物をプロット
        self._plot_interference_obj(interference_obj)

        # ノード位置を取得
        debug_pathes_pos = debug_pathes[:, :-1]
        debug_pathes_near_node = debug_pathes[:, -1]
        imgs = []
        
        # アニメーション
        for i in range(debug_pathes.shape[0]):
            for j in range(self._ONE_CUT_NUM * 2):
                # デバッグ用経路の点数分ループ
                path_images = []
                # ランダムな経路をプロット
                image = self._axis.plot(debug_random_pathes[i, 0], debug_random_pathes[i, 1], color="red", marker="*")
                path_images.extend(image)
                # ノードをプロット
                image = self._axis.scatter(debug_pathes_pos[:i, 0], debug_pathes_pos[:i, 1], color="black", marker="*")
                path_images.extend([image])
                # 始点をプロット
                image = self._axis.scatter(debug_pathes_pos[ 0, 0], debug_pathes_pos[ 0, 1], color="cyan", marker="*", s=24)
                path_images.extend([image])
                # 終点をプロット
                image = self._axis.scatter(debug_pathes_pos[-1, 0], debug_pathes_pos[-1, 1], color="lime", marker="*", s=24)
                path_images.extend([image])
                # 現ノードをプロット
                image = self._axis.scatter(debug_pathes_pos[ i, 0], debug_pathes_pos[ i, 1], color="blue", s=24)
                path_images.extend([image])
                # 最短ノードもプロット
                if i != 0:
                    # 始点には最短ノードが存在しないから，始点以外でプロットする
                    near_node = int(debug_pathes_near_node[i])
                    plot_pos = np.append(debug_pathes_pos[i].reshape(1, -1), debug_pathes_pos[near_node].reshape(1, -1), axis=0)
                    image = self._axis.plot(plot_pos[:, 0], plot_pos[:, 1], color="red")
                    path_images.extend(image)
                    image = self._axis.scatter(plot_pos[1, 0], plot_pos[1, 1], color="green", marker="*", s=24)
                    path_images.extend([image])
                imgs.append(path_images)
        
        # 最終的なパスをプロット
        for _ in range(self._ONE_CUT_NUM * 4):
            path_images = []
            image = self._axis.plot(pathes[:, 0], pathes[:, 1], color="green")
            path_images.extend(image)
            image = self._axis.scatter(debug_pathes[:, 0], debug_pathes[:, 1], color="gray")
            path_images.extend([image])
            imgs.append(path_images)

        # アニメーション
        animation = ani.ArtistAnimation(self._figure, imgs)
        animation.save(self._ANIMATION_NAME, writer='imagemagick')
        plt.show()
        
        plot_result = True
        
        return plot_result
    
    def _reset3D(self):
        """
        3次元データのリセット
        """
        self._figure = plt.figure()
        self._axis = self._figure.add_subplot(111, projection="3d")
        
        # 0 ~ 2piまでの範囲とする
        theta_1_0 = np.linspace(0, np.pi * 2, 100) 
        theta_2_0 = np.linspace(0, np.pi * 2, 100)
        theta_1, theta_2 = np.meshgrid(theta_1_0, theta_2_0)
        
        # x, y, zの曲座標表示 (中心点が原点である半径1の球)
        self._x = np.cos(theta_2) * np.sin(theta_1)
        self._y = np.sin(theta_2) * np.sin(theta_1)
        self._z = np.cos(theta_1)
        
    def _set_3DAxis(self):
        """
        3次元データのラベルや範囲を設定
        """
        # X/Y/Z軸に文字を記載
        self._axis.set_xlabel("X")
        self._axis.set_ylabel("Y")
        self._axis.set_zlabel("Z")
        # プロットするX/Y/Zの範囲を設定
        self._axis.set_xlim(self._min_pos[0], self._max_pos[0])
        self._axis.set_ylim(self._min_pos[1], self._max_pos[1])
        self._axis.set_zlim(self._min_pos[2], self._max_pos[2])
        self._axis.grid()
        self._axis.set_aspect("equal")
        
    def _update_3Ddata(self, i, pathes, debug_pathes, debug_random_pathes, interference_obj):
        """
        3D(3次元)各データの更新

        パラメータ
            i(int): フレーム番号
            pathes(numpy.ndarray): 経路
            debug_pathes(numpy.ndarray): デバッグ用経路
            debug_random_pathes(numpy.ndarray): デバッグ用ランダム経路
            interference_obj(list): 干渉物
        """
        # 以前のプロットをクリアする
        self._axis.clear()
        self._set_3DAxis()

        # ノード位置を取得
        debug_pathes_pos = debug_pathes[:, :-1]
        debug_pathes_near_node = debug_pathes[:, -1]
        
        # 干渉物のプロット
        self._plot_3Dinterference_obj(interference_obj)
        
        if i < debug_pathes.shape[0]:
            # 各ノードをプロットする
            # ランダムな経路をプロット
            self._axis.plot(debug_random_pathes[i, 0], debug_random_pathes[i, 1], debug_random_pathes[i, 2], color="red", marker="*")
            # ノードをプロット
            self._axis.scatter(debug_pathes_pos[:i, 0], debug_pathes_pos[:i, 1], debug_pathes_pos[:i, 2], color="black", marker="*")
            # 始点をプロット
            self._axis.scatter(debug_pathes_pos[ 0, 0], debug_pathes_pos[ 0, 1], debug_pathes_pos[ 0, 2], color="cyan", marker="*", s=24)
            # 終点をプロット
            self._axis.scatter(debug_pathes_pos[-1, 0], debug_pathes_pos[-1, 1], debug_pathes_pos[-1, 2], color="lime", marker="*", s=24)
            # 現ノードをプロット
            self._axis.scatter(debug_pathes_pos[ i, 0], debug_pathes_pos[ i, 1], debug_pathes_pos[ i, 2], color="blue", s=24)
            # 最短ノードもプロット
            if i != 0:
                # 始点には最短ノードが存在しないから，始点以外でプロットする
                near_node = int(debug_pathes_near_node[i])
                plot_pos = np.append(debug_pathes_pos[i].reshape(1, -1), debug_pathes_pos[near_node].reshape(1, -1), axis=0)
                self._axis.plot(plot_pos[:, 0], plot_pos[:, 1], plot_pos[:, 2], color="red")
                self._axis.scatter(plot_pos[1, 0], plot_pos[1, 1], plot_pos[1, 2], color="green", marker="*", s=24)
        else:
            # 生成した経路をプロットする
            self._axis.plot(pathes[:, 0], pathes[:, 1], pathes[:, 2], color="green")
            self._axis.scatter(debug_pathes[:, 0], debug_pathes[:, 1], debug_pathes[:, 2], color="gray")
    
    def _plot_3Dinterference_obj(self, interference_obj):
        """
        球の干渉物のプロット
        
        パラメータ
            interference_obj(list): 球の干渉物
        
        戻り値
            interference_images(list): 干渉物データ
        """
        for interference in interference_obj:
            # 球データをプロット
            center = interference.center
            self._axis.plot_wireframe(self._x * interference.radius + center[0], self._y * interference.radius + center[1], self._z * interference.radius + center[2], color="gray", alpha=0.1)
    
    def plot_3DAnimation(self, pathes, debug_pathes, debug_random_pathes, interference_obj):
        """
        3D(3次元)アニメーション作成
        
        パラメータ
            pathes(numpy.ndarray): 経路
            debug_pathes(numpy.ndarray): デバッグ用経路
            debug_random_pathes(numpy.ndarray): デバッグ用ランダム経路
        
        戻り値
            plot_result(bbol): True / False = 成功 / 失敗
        """
        plot_result = False

        # データをリセットする
        self._reset3D()
        
        # 引数の確認
        if pathes.size == 0 or debug_pathes.size == 0 or debug_random_pathes.size == 0:
            return plot_result
        
        # デバッグ用経路とデバッグ用ランダム経路の行数が一致していることを確認
        if debug_pathes.shape[0] != debug_random_pathes.shape[0]:
            # 異常なデータ
            return plot_result

        # アニメーションのフレーム数
        n_frame = debug_pathes.shape[0] + self._ONE_CUT_NUM * 4
        animation = ani.FuncAnimation(self._figure, self._update_3Ddata, fargs=(pathes, debug_pathes, debug_random_pathes, interference_obj), interval=500, frames=n_frame)

        # アニメーション
        animation.save(self._ANIMATION_NAME, writer="imagemagick")
        plt.show()
        
        plot_result = True
        
        return plot_result

# 新規追加 ↓
class RRTConnectAnimation(RRTAnimation):
    """
    RRT-Connectのアニメーション作成
    
    プロパティ
        _min_pos(numpy.ndarray): RRT-Connectの最小探索範囲
        _max_pos(numpy.ndarray): RRT-Connectの最大探索範囲
        _figure: 描画枠
        _axis: 描画内容
    
    publicメソッド (全てのクラスから参照可能)
        plot_2DAnimation(): 2D(2次元)アニメーション作成
        plot_3DAnimation(): 3D(3次元)アニメーション作成
    
    protectedメソッド (自クラスまたは子クラスが参照可能)
        _reset2D(): 2次元データのリセット
        _reset3D(): 3次元データのリセット
        _plot_2D_data(): 2次元データのプロット
    """
    # 定数の定義
    _ONE_CUT_NUM = 6        # 1回
    _ANIMATION_NAME = "rrt_connect_animation.gif"
    
    def __init__(self, min_pos, max_pos):
        """
        コンストラクタ
        """
        # 親クラスの初期化処理
        super().__init__(min_pos, max_pos)
        
    def plot_2DAnimation(self, pathes, start_tree, end_tree, debug_states, interference_obj):
        """
        2D(2次元)アニメーション作成
        
        パラメータ
            pathes(numpy.ndarray): 経路
            start_tree(numpy.ndarray): 始点ツリー
            end_tree(numpy.ndarray): 終点ツリー
            debug_states(list): デバッグ用の状態
            interference_obj(list): 円形の干渉物
        
        戻り値
            plot_result(bool): True / False = 成功 / 失敗
        """
        plot_result = False

        # データをリセットする
        self._reset2D()
        
        # 引数の確認
        if pathes.size == 0 or start_tree.size == 0 or end_tree.size == 0 or len(debug_states) == 0:
            return plot_result
        
        # 円形の干渉物をプロット
        self._plot_interference_obj(interference_obj)
        
        self._plot_2D_data(pathes, start_tree, end_tree, debug_states)
        
        plot_result = True
        
        return plot_result

    def _plot_2D_data(self, pathes, start_tree, end_tree, debug_states):
        """
        2D(2次元)データのプロット

        パラメータ
            pathes(numpy.ndarray): 経路
            start_tree(numpy.ndarray): 始点ツリー
            end_tree(numpy.ndarray): 終点ツリー
            debug_states(list): デバッグ用の状態
        """
        # 始点ツリーを位置と最短距離ノードに分割
        start_tree_pos = start_tree[:, :-1]
        start_tree_near_node = start_tree[:, -1]
        # 終点ツリーを位置と最短距離ノードに分割
        end_tree_pos = end_tree[:, :-1]
        end_tree_near_node = end_tree[:, -1]

        # 各ツリーの要素番号を定義
        start_tree_idx = 0
        end_tree_idx   = 0

        # ノードを追加したツリーを保存
        now_tree_pos = start_tree_pos
        now_tree_near_node = start_tree_near_node
        
        imgs = []
        # アニメーション
        for idx, state in enumerate(debug_states):
            # 状態の数分ループ
            path_images = []
            if state == STATE_START_TREE_RANDOM or state == STATE_START_TREE_END_TREE:        
                # 始点ツリーにランダム点を追加状態または始点ツリーから終点ツリーへノードを伸ばす状態
                start_tree_idx += 1
                # 追加した点の位置を取得
                now_pos   = start_tree_pos[start_tree_idx]
                # 追加した点の最短ノードを取得
                near_node = int(start_tree_near_node[start_tree_idx])
                # 最短ノードの位置を取得
                near_pos  = start_tree_pos[near_node]
            else:
                # 終点ツリーにランダム点を追加状態または終点ツリーから始点ツリーへノードを伸ばす状態
                end_tree_idx += 1
                # 追加した点の位置を取得
                now_pos   = end_tree_pos[end_tree_idx]
                # 追加した点の最短ノードを取得
                near_node = int(end_tree_near_node[end_tree_idx])
                # 最短ノードの位置を取得
                near_pos  = end_tree_pos[near_node]

            # 生成できている始点ツリーをプロット
            image = self._axis.scatter(start_tree_pos[:start_tree_idx + 1, 0], start_tree_pos[:start_tree_idx + 1, 1], color="red", marker="*")
            path_images.extend([image])
            # 生成できている終点ツリーをプロット
            image = self._axis.scatter(end_tree_pos[:end_tree_idx + 1, 0], end_tree_pos[:end_tree_idx + 1, 1], color="orange", marker="*")
            path_images.extend([image])
            
            # 始点をプロット
            image = self._axis.scatter(start_tree_pos[0, 0], start_tree_pos[0, 1], color="cyan", marker="*", s=24)
            path_images.extend([image])
            # 終点をプロット
            image = self._axis.scatter(end_tree_pos[0, 0], end_tree_pos[0, 1], color="lime", marker="*", s=24)
            path_images.extend([image])

            # 現ノードをプロット
            image = self._axis.scatter(now_pos[0], now_pos[1], color="blue", s=24)
            path_images.extend([image])

            # 最短ノードもプロット
            plot_pos = np.append(now_pos.reshape(1, -1), near_pos.reshape(1, -1), axis=0)
            image = self._axis.plot(plot_pos[:, 0], plot_pos[:, 1], color="red")
            path_images.extend(image)
            image = self._axis.scatter(plot_pos[1, 0], plot_pos[1, 1], color="green", marker="*", s=24)
            path_images.extend([image])
            
            # 相手ツリーのランダムノードをプロット
            if state == STATE_START_TREE_END_TREE:
                # 終点ツリーのランダムノードをプロット
                image = self._axis.scatter(end_tree_pos[end_tree_idx, 0], end_tree_pos[end_tree_idx, 1], color="green")
                path_images.extend([image])
            elif state == STATE_END_TREE_START_TREE:
                image = self._axis.scatter(start_tree_pos[start_tree_idx, 0], start_tree_pos[start_tree_idx, 1], color="green")
                path_images.extend([image])
            imgs.append(path_images)
        
        for _ in range(self._ONE_CUT_NUM):
            path_images = []
            # 最終的なパスをプロット
            image = self._axis.plot(pathes[:, 0], pathes[:, 1], color="green")
            path_images.extend(image)
            # 始点ツリーをプロット
            image = self._axis.scatter(start_tree_pos[:, 0], start_tree_pos[:, 1], color="gray")
            path_images.extend([image])
            # 終点ツリーをプロット
            image = self._axis.scatter(end_tree_pos[:, 0], end_tree_pos[:, 1], color="gray")
            path_images.extend([image])
            imgs.append(path_images)

        # アニメーション
        animation = ani.ArtistAnimation(self._figure, imgs, interval=500)
        animation.save(self._ANIMATION_NAME, writer='imagemagick')
        plt.show()
# 新規追加 ↑

性能評価 (RRT/RRT-Connect)

RRTとRRT-Connectによる性能評価を実施する．
性能評価を実施するために，干渉物の位置と大きさを一定として，下記内容を比較することによって，性能を評価していく．各アルゴリズムを30回実施して，性能を評価する．
・始点から終点までの経路生成の処理時間 [sec]
・生成した経路の経由点の数 (始点と終点を含む)

性能評価する時のメイン処理は下記の通りである．

main.py

# ライブラリの読み込み
import numpy as np
import matplotlib.pyplot as plt
import time

# 自作モジュールの読み込み
from rrt import RRT, RRTConnect
from rrt_animation import RRTAnimation, RRTConnectAnimation
from interference import Circle, Ball

RRT_CONNECT_FLG = True      # True / False = RRTConnect / RRT
DIMENTION = 2               # 次元数
START_POS = 0               # 始点位置
END_POS   = 2               # 終点位置

def path_planning():
    """
    経路生成
    """
    # 始点と終点を作成
    start_pos = np.ones(DIMENTION).reshape(1, -1) * START_POS
    end_pos   = np.ones(DIMENTION).reshape(1, -1) * END_POS
    
    # 干渉物の半径を定義
    interference_radius = 0.3
    if DIMENTION == 2:
        # 円形の干渉物を作成
        interference_pos = [[0.8, 0.8], [1.2, 0.8], [1.2, 1.2], [0.8, 1.2]]
        interference_obj = [Circle(np.array(pos), interference_radius) for pos in interference_pos]
    elif DIMENTION == 3:
        # 球の干渉物を作成
        interference_pos = [[0.8, 0.8, 0.8], [1.2, 0.8, 0.8], [1.2, 1.2, 1.2], [0.8, 1.2, 1.2]]
        interference_obj = [Ball(np.array(pos), interference_radius) for pos in interference_pos]
    else:
        # 干渉物はない
        interference_obj = []

    if RRT_CONNECT_FLG:
        # RRT-Connectクラスのインスタンスを作成
        rrt_connect = RRTConnect()
        # 始点から終点までの経路を生成
        start_time = time.time()
        planning_result = rrt_connect.planning(start_pos, end_pos, interference_obj)
        end_time = time.time()
        # 処理時間を計算
        elapsed_time = end_time - start_time
        # 経由点数を計算
        n_waypoint = rrt_connect.pathes.shape[0]
        # 生成した経路をファイルに保存
        rrt_connect.save()

        # RRT-Connectアニメーションのインスタンスを作成
        rrt_connect_animation = RRTConnectAnimation(rrt_connect.strict_min_pos, rrt_connect.strict_max_pos)
        # アニメーションを描画
        if DIMENTION == 2:
            result = rrt_connect_animation.plot_2DAnimation(rrt_connect.pathes, rrt_connect.start_tree, rrt_connect.end_tree, rrt_connect.debug_states, interference_obj)
        # elif DIMENTION == 3: # RRT-Connectの3次元は未作成のため，コメントアウト
        #     result = rrt_connect_animation.plot_3DAnimation(rrt_connect.pathes, rrt_connect.start_tree, rrt_connect.end_tree, rrt_connect.debug_states, interference_obj)
    else:
        # RRTクラスのインスタンスを作成
        rrt = RRT()
        # 始点から終点までの経路を生成
        start_time = time.time()
        planning_result = rrt.planning(start_pos, end_pos, interference_obj)
        end_time = time.time()
        # 処理時間を計算
        elapsed_time = end_time - start_time
        # 経由点数を計算
        n_waypoint = rrt.pathes.shape[0]
        # 生成した経路をファイルに保存
        rrt.save()

        # # RRTアニメーションのインスタンスを作成
        # rrtAni = RRTAnimation(rrt.strict_min_pos, rrt.strict_max_pos)
        # # アニメーションを描画
        # if DIMENTION == 2:
        #     result = rrtAni.plot_2DAnimation(rrt.pathes, rrt.debug_pathes, rrt.debug_random_pathes, interference_obj)
        # elif DIMENTION == 3:
        #     result = rrtAni.plot_3DAnimation(rrt.pathes, rrt.debug_pathes, rrt.debug_random_pathes, interference_obj)

    return elapsed_time, n_waypoint

if __name__ == "__main__":
    # 本ファイルがメインで呼ばれた時の処理
    elapsed_times = []
    n_waypoints   = []
    for _ in range(30):
        elapsed_time, waypoint = path_planning()
        elapsed_times.append(elapsed_time)
        n_waypoints.append(waypoint)
    
    # 処理時間の平均，最小，最大，標準偏差を出力
    print(f"time ave = {np.mean(elapsed_times)}")
    print(f"time max = {np.max(elapsed_times)}")
    print(f"time min = {np.min(elapsed_times)}")
    print(f"time std = {np.std(elapsed_times)}")
    # 経由点の平均，最小，最大，標準偏差を出力
    print(f"waypoint ave = {np.mean(n_waypoints)}")
    print(f"waypoint max = {np.max(n_waypoints)}")
    print(f"waypoint min = {np.min(n_waypoints)}")
    print(f"waypoint std = {np.std(n_waypoints)}")

main.pyを実行すると，"rrt_connect_animation.gif"が作成される．
RRT-Connectによる経路生成は下図のようになった．
アニメーションの見方は以下の通りである．
・水色の星形(左下)データ：経路生成の始点
・緑色の星形(右上)データ：経路生成の終点
・黄色の星形データ：終点側のツリー
・赤色の星形データ：始点側のツリー
・青色の円データ：新規データ
・緑色の円データ：目標ノード
・赤色の線：新規データと最短距離のデータを結んだ線
・緑色の線：最終的な始点から終点までの経路
・灰色の円：干渉物

各アルゴリムの性能は下表の通りとなった．

アルゴリズム	平均処理時間 [sec]	最小処理時間 [sec]	最大処理時間 [sec]	処理時間の標準偏差 [sec]	平均経由点数	最小経由点数	最大経由点数	経由点数の標準偏差
RRT	0.002614	0.001427	0.004697	0.000859	17.4	16	21	1.33
RRT-Connect	0.000956	0.000538	0.001469	0.000250	16.8	15	20	1.07

上表から本結果に関して，以下のことが言える．
・RRT-Connectの方がRRTよりも処理時間が短い (約1/2倍の処理時間となった)
・RRTとRRT-Connectでは，経由点数に差異が生じなかった

おわりに

本記事では，Pythonを使用して，下記内容を実装しました．
・2次元空間で，円形の干渉物を回避するRRT-Connectによる経路生成

次記事では，下記内容を実装していきます．
・3次元空間で，球の干渉物を回避するRRT-Connectによる経路生成
(https://qiita.com/haruhiro1020/items/74ed385c068d4268f4d5)