はじめに

私がロボットに関して興味を持っており，ロボットの経路を作成したいと思ったところ，RRT* (Rapidly-exploring Random Tree) と呼ばれる経路生成の手法に辿り着いた．
本記事では，経路生成の古典的手法である RRT* (Rapidly-exploring Random Tree) をPythonより，実装してみる．
前記事では，球の干渉物が存在する3次元空間を探索する RRT* を実装した．
前記事の内容を下記に記します．
・https://qiita.com/haruhiro1020/items/de9fb322dce87588d48d
本記事では，円形の干渉物が存在する2次元空間を探索する RRT* にアニメーションを実装する．

RRT* はRRTに最適性を付与したアルゴリズムである．RRTとRRT* の違いは前記事で説明したため，本記事では割愛する．
また，前記事では，RRT* のグラフを作成したが，アニメーション未実装のため，本記事でアニメーションを追加する．

本記事で実装すること

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

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

・球の干渉物が存在する3次元空間でのRRT* による経路生成のアニメーション化

動作環境

・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* に関しては前記事(https://qiita.com/haruhiro1020/items/7c5bdbe530711c9d331b)にて説明したため，割愛する．

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

前記事とソースコードは同じため，本記事では割愛する．
前記事 (https://qiita.com/haruhiro1020/items/7c5bdbe530711c9d331b) の "constant.py"，"rrt.py" に記載．

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

前記事とソースコードは同じため，本記事では割愛する．
前記事 (https://qiita.com/haruhiro1020/items/7c5bdbe530711c9d331b) の "interference.py" に記載．

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

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

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(): グラフ作成
        plot_Animation(): アニメーション作成
    
    protectedメソッド (自クラスまたは子クラスが参照可能)
        _plot_2D(): 2D(2次元)グラフ作成
        _plot_2DAnimation(): 2D(2次元)アニメーション作成
        _reset2D(): 2次元データのリセット
        _plot_2Dinterference_obj(): 2次元の干渉物(円形)をプロット
        _plot_3D(): 3D(3次元)グラフ作成
        _plot_3DAnimation(): 3D(3次元)アニメーション作成
        _reset3D(): 3次元データのリセット
        _set_3DAxis(): 3次元データのラベルや範囲を設定
        _update_3Ddata(): 3次元各データの更新
        _plot_3Dinterference_obj(): 3次元の干渉物(球)のプロット
    """
    # 定数の定義
    _ONE_CUT_NUM = 6        # 1回
    _ANIMATION_NAME = "rrt_animation.gif"
    _PLOT_NAME      = "rrt_plot.gif"
    _NODE_NEAR_NODE_IDX = RRT_NEAR_NODE_IDX    # ノード内の最短ノード要素

    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_2Dinterference_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_2D_all_node_parent(self, pathes, positions, parent_nodes, line_color="red", pathes_plot=True):
        """
        全ノードを親ノード(最短ノード)と一緒にプロット(2次元データ)

        パラメータ
            pathes(numpy.ndarray): 始点から終点までの経路
            positions(numpy.ndarray): 全ノード位置
            parent_nodes(numpy.ndarray): 全ノードの親ノード
            line_color(str): ノードと親ノードの直線の色
            pathes_plot(bool): 始点から終点までの経路をプロットするか否か
        
        戻り値
            path_images(list): 全ノードをプロットしたリストデータ
        """
        path_images = []

        # 引数の確認
        if positions.shape[0] != parent_nodes.shape[0]:
            # データ数不一致で異常
            return

        # ノードを全部プロット
        image = self._axis.scatter(positions[ :, 0], positions[ :, 1], color="gray", marker="*")
        path_images.extend([image])

        # 始点をプロット
        image = self._axis.scatter(pathes[ 0, 0], pathes[ 0, 1], color="cyan", marker="*")
        path_images.extend([image])
        # 終点をプロット
        image = self._axis.scatter(pathes[-1, 0], pathes[-1, 1], color="lime", marker="*")
        path_images.extend([image])

        for i in reversed(range(1, positions.shape[0])):
            # 追加の遅いノードから順番にプロットしていく
            now_position  = positions[i]
            now_node      = i
            near_node_idx = int(parent_nodes[i])
            while True:         # 始点をプロットするまでループ
                parent_position = positions[near_node_idx]
                plot_pos = np.append(now_position.reshape(1, -1), parent_position.reshape(1, -1), axis=0)
                # 親ノードとの線をプロット
                image = self._axis.plot(plot_pos[:, 0], plot_pos[:, 1], color=line_color)
                path_images.extend(image)
                # ノードおよび親ノードの更新
                now_position = parent_position
                near_node_idx = int(parent_nodes[near_node_idx])
                if near_node_idx == INITIAL_NODE_NEAR_NODE:
                    # 始点には，親ノードが存在しないから，while処理を抜ける
                    break

        # 最終的なパスをプロット
        if pathes_plot:
            image = self._axis.plot(pathes[:, 0], pathes[:, 1], color="green", alpha=0.7)
            path_images.extend(image)

        return path_images

    def plot(self, pathes, debug_pathes, interference_obj, dimention):
        """
        グラフ作成
        
        パラメータ
            pathes(numpy.ndarray): 経路
            debug_pathes(numpy.ndarray): デバッグ用経路
            interference_obj(list): 円形の干渉物
            dimention(int): 次元数
        
        戻り値
            plot_result(bool): True / False = 成功 / 失敗
        """
        plot_result = False
        
        # 引数の確認
        if pathes.size == 0 or debug_pathes.size == 0:
            return plot_result
        
        # デバッグ用経路から位置・親ノードを取得
        debug_pathes_position  = debug_pathes[:, :self._NODE_NEAR_NODE_IDX]
        debug_pathes_near_node = debug_pathes[:,  self._NODE_NEAR_NODE_IDX]
        
        if dimention == DIMENTION_2D:   # 2次元データ
            plot_result = self._plot_2D(pathes, debug_pathes_position, debug_pathes_near_node, interference_obj)
        elif dimention == DIMENTION_3D: # 3次元データ
            plot_result = self._plot_3D(pathes, debug_pathes_position, debug_pathes_near_node, interference_obj)

        return plot_result

    def _plot_2D(self, pathes, debug_pathes_position, debug_pathes_near_node, interference_obj):
        """
        2D(2次元)グラフ作成
        
        パラメータ
            pathes(numpy.ndarray): 経路
            debug_pathes_position(numpy.ndarray): デバッグ用経路のノード位置
            debug_pathes_near_node(numpy.ndarray): デバッグ用経路の親ノード
            interference_obj(list): 円形の干渉物
        
        戻り値
            plot_result(bool): True / False = 成功 / 失敗
        """
        plot_result = False

        # データをリセットする
        self._reset2D()

        # 干渉物をプロット
        self._plot_2Dinterference_obj(interference_obj)

        imgs = []

        # 全ノードを親ノード(最短ノード)と一緒にプロット
        path_images = self._plot_2D_all_node_parent(pathes, debug_pathes_position, debug_pathes_near_node)
        if path_images:
            for _ in range(self._ONE_CUT_NUM):
                imgs.append(path_images)

            # アニメーション
            animation = ani.ArtistAnimation(self._figure, imgs)
            animation.save(self._PLOT_NAME, writer='imagemagick')
            plt.show()

            plot_result = True

        return plot_result

    def plot_Animation(self, pathes, debug_pathes, debug_random_pathes, interference_obj, dimention):
        """
        アニメーション作成
        
        パラメータ
            pathes(numpy.ndarray): 経路
            debug_pathes(numpy.ndarray): デバッグ用経路
            debug_random_pathes(numpy.ndarray): デバッグ用ランダム経路
            interference_obj(list): 円形の干渉物
            dimention(int): 次元数
        
        戻り値
            plot_result(bool): True / False = 成功 / 失敗
        """
        plot_result = False
        
        # 引数の確認
        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
        
        # デバッグ用経路から位置・親ノードを取得
        debug_pathes_position  = debug_pathes[:, :self._NODE_NEAR_NODE_IDX]
        debug_pathes_near_node = debug_pathes[:,  self._NODE_NEAR_NODE_IDX]
        # デバッグ用ランダム経路から位置・親ノードを取得
        debug_random_pathes_position  = debug_random_pathes[:, :self._NODE_NEAR_NODE_IDX]
        debug_random_pathes_near_node = debug_random_pathes[:,  self._NODE_NEAR_NODE_IDX]
        
        if dimention == DIMENTION_2D:   # 2次元データ
            plot_result = self._plot_2DAnimation(pathes, debug_pathes_position, debug_pathes_near_node, debug_random_pathes_position, interference_obj)
        elif dimention == DIMENTION_3D: # 3次元データ
            plot_result = self._plot_3DAnimation(pathes, debug_pathes_position, debug_pathes_near_node, debug_random_pathes_position, interference_obj)

        return plot_result

    def _plot_2DAnimation(self, pathes, debug_pathes_pos, debug_pathes_near_node, debug_random_pathes_pos, interference_obj):
        """
        2D(2次元)アニメーション作成
        
        パラメータ
            pathes(numpy.ndarray): 経路
            debug_pathes_pos(numpy.ndarray): デバッグ用経路の位置
            debug_pathes_near_node(numpy.ndarray): デバッグ用経路の親ノード
            debug_random_pathes_pos(numpy.ndarray): デバッグ用ランダム経路の位置
            interference_obj(list): 円形の干渉物
        
        戻り値
            plot_result(bool): True / False = 成功 / 失敗
        """
        plot_result = False

        # データをリセットする
        self._reset2D()

        # 円形の干渉物をプロット
        self._plot_2Dinterference_obj(interference_obj)

        imgs = []

        # アニメーション
        for i in range(debug_pathes_pos.shape[0]):
            for j in range(self._ONE_CUT_NUM * 2):
                # デバッグ用経路の点数分ループ
                path_images = []
                # ランダムな経路をプロット
                image = self._axis.plot(debug_random_pathes_pos[i, 0], debug_random_pathes_pos[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_pos[:, 0], debug_pathes_pos[:, 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_pos, debug_pathes_near_node, debug_random_pathes, interference_obj):
        """
        3D(3次元)各データの更新

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

        # 干渉物のプロット
        self._plot_3Dinterference_obj(interference_obj)

        if i < debug_pathes_pos.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_pos[:, 0], debug_pathes_pos[:, 1], debug_pathes_pos[:, 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_3D_all_node_parent(self, pathes, positions, parent_nodes, line_color="red"):
        """
        全ノードを親ノード(最短ノード)と一緒にプロット(3次元データ)

        パラメータ
            pathes(numpy.ndarray): 始点から終点までの経路
            positions(numpy.ndarray): 全ノード位置
            parent_nodes(numpy.ndarray): 全ノードの親ノード
        
        戻り値
            path_images(list): 全ノードをプロットしたリストデータ
        """
        path_images = []

        # 引数の確認
        if positions.shape[0] != parent_nodes.shape[0]:
            # データ数不一致で異常
            return

        # ノードを全部プロット
        image = self._axis.scatter(positions[ :, 0], positions[ :, 1], positions[ :, 2], color="gray", marker="*")
        path_images.extend([image])

        # 始点をプロット
        image = self._axis.scatter(pathes[ 0, 0], pathes[ 0, 1], pathes[ 0, 2], color="cyan", marker="*")
        path_images.extend([image])
        # 終点をプロット
        image = self._axis.scatter(pathes[-1, 0], pathes[-1, 1], pathes[-1, 2], color="lime", marker="*")
        path_images.extend([image])

        for i in reversed(range(1, positions.shape[0])):
            # 追加の遅いノードから順番にプロットしていく
            now_position  = positions[i]
            now_node      = i
            near_node_idx = int(parent_nodes[i])
            while True:         # 始点をプロットするまでループ
                parent_position = positions[near_node_idx]
                plot_pos = np.append(now_position.reshape(1, -1), parent_position.reshape(1, -1), axis=0)
                # 親ノードとの線をプロット
                image = self._axis.plot(plot_pos[:, 0], plot_pos[:, 1],  plot_pos[:, 2], color=line_color)
                path_images.extend(image)
                # ノードおよび親ノードの更新
                now_position = parent_position
                near_node_idx = int(parent_nodes[near_node_idx])
                if near_node_idx == INITIAL_NODE_NEAR_NODE:
                    # 始点には，親ノードが存在しないから，while処理を抜ける
                    break

        # 最終的なパスをプロット
        image = self._axis.plot(pathes[:, 0], pathes[:, 1],  pathes[:, 2], color="green", alpha=0.7)
        path_images.extend(image)

        return path_images

    def _plot_3D(self, pathes, debug_pathes_position, debug_pathes_near_node, interference_obj):
        """
        3D(3次元)グラフ作成
        
        パラメータ
            pathes(numpy.ndarray): 経路
            debug_pathes_position(numpy.ndarray): デバッグ用経路のノード位置
            debug_pathes_near_node(numpy.ndarray): デバッグ用経路の親ノード
            interference_obj(list): 円形の干渉物
        
        戻り値
            plot_result(bool): True / False = 成功 / 失敗
        """
        plot_result = False

        # データをリセットする
        self._reset3D()

        # 干渉物をプロット
        self._plot_3Dinterference_obj(interference_obj)

        imgs = []

        # 全ノードを親ノード(最短ノード)と一緒にプロット
        path_images = self._plot_3D_all_node_parent(pathes, debug_pathes_position, debug_pathes_near_node)
        if path_images:
            for _ in range(self._ONE_CUT_NUM):
                imgs.append(path_images)

            # アニメーション
            animation = ani.ArtistAnimation(self._figure, imgs)
            animation.save(self._PLOT_NAME, writer='imagemagick')
            plt.show()

            plot_result = True

        return plot_result

    def _plot_3DAnimation(self, pathes, debug_pathes_pos, debug_pathes_near_node, debug_random_pathes_pos, interference_obj):
        """
        3D(3次元)アニメーション作成
        
        パラメータ
            pathes(numpy.ndarray): 経路
            debug_pathes_pos(numpy.ndarray): デバッグ用経路の位置
            debug_pathes_near_node(numpy.ndarray): デバッグ用経路の親ノード
            debug_random_pathes_pos(numpy.ndarray): デバッグ用ランダム経路の位置
            interference_obj(list): 球の干渉物
        
        戻り値
            plot_result(bool): True / False = 成功 / 失敗
        """
        plot_result = False

        # データをリセットする
        self._reset3D()

        # アニメーションのフレーム数
        n_frame = debug_pathes_pos.shape[0] + self._ONE_CUT_NUM * 4
        animation = ani.FuncAnimation(self._figure, self._update_3Ddata, fargs=(pathes, debug_pathes_pos, debug_pathes_near_node, debug_random_pathes_pos, 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のアニメーション作成
    
    プロパティ
        _figure: 描画枠
        _axis: 描画内容
        _start_tree_idx(int): 始点ツリーのノード番号
        _end_tree_idx(int): 終点ツリーのノード番号
    
    publicメソッド (全てのクラスから参照可能)
        plot(): グラフ作成
        plot_Animation(): アニメーション作成
    
    protectedメソッド (自クラスまたは子クラスが参照可能)
        _plot_2D_data(): 2次元データのプロット
        _plot_2D(): 2D(2次元)グラフ作成
        _plot_2DAnimation(): 2D(2次元)アニメーション作成
        _update_3Ddata(): 3D(3次元)各データの更新
        _plot_3DAnimation(): 3D(3次元)アニメーション作成
        _plot_3D_data(): 3D(3次元)各データのプロット
        _reset3D(): 3次元データのリセット
    """
    # 定数の定義
    _ONE_CUT_NUM = 6        # 1回
    _ANIMATION_NAME = "rrt_connect_animation.gif"   # アニメーション保存ファイル名
    _PLOT_NAME      = "rrt_connect_plot.gif"        # グラフ保存ファイル名

    def __init__(self, min_pos, max_pos):
        """
        コンストラクタ
        """
        # 親クラスの初期化処理
        super().__init__(min_pos, max_pos)

    def plot(self, pathes, start_tree, end_tree, interference_obj, dimention):
        """
        グラフ作成
        
        パラメータ
            pathes(numpy.ndarray): 経路
            start_tree(numpy.ndarray): 始点ツリー
            end_tree(numpy.ndarray): 終点ツリー
            interference_obj(list): 円形の干渉物
            dimention(int): 次元数
        
        戻り値
            plot_result(bool): True / False = 成功 / 失敗
        """
        plot_result = False

        # 引数の確認
        if pathes.size == 0 or start_tree.size == 0 or end_tree.size == 0:
            return plot_result

        # 始点ツリーを位置と親ノードに分割
        start_tree_pos = start_tree[:, :RRT_CONNECT_NEAR_NODE_IDX]
        start_tree_near_node = start_tree[:, RRT_CONNECT_NEAR_NODE_IDX]
        # 終点ツリーを位置と親ノードに分割
        end_tree_pos = end_tree[:, :RRT_CONNECT_NEAR_NODE_IDX]
        end_tree_near_node = end_tree[:, RRT_CONNECT_NEAR_NODE_IDX]

        if dimention == DIMENTION_2D:
            plot_result = self._plot_2D(pathes, start_tree_pos, start_tree_near_node, end_tree_pos, end_tree_near_node, interference_obj)
        elif dimention == DIMENTION_3D:
            plot_result = self._plot_3D(pathes, start_tree_pos, start_tree_near_node, end_tree_pos, end_tree_near_node, interference_obj)

        return plot_result

    def _plot_2D(self, pathes, start_tree_pos, start_tree_near_node, end_tree_pos, end_tree_near_node, interference_obj):
        """
        2D(2次元)グラフ作成
        
        パラメータ
            pathes(numpy.ndarray): 経路
            start_tree_pos(numpy.ndarray): 始点ツリーのノード位置
            start_tree_near_node(numpy.ndarray): 始点ツリーの親ノード
            end_tree_pos(numpy.ndarray): 始点ツリーのノード位置
            end_tree_near_node(numpy.ndarray): 始点ツリーの親ノード
            interference_obj(list): 円形の干渉物
        
        戻り値
            plot_result(bool): True / False = 成功 / 失敗
        """
        plot_result = False

        # データをリセットする
        self._reset2D()

        # 円形の干渉物をプロット
        self._plot_2Dinterference_obj(interference_obj)

        imgs = []

        # 始点ツリー内の全ノードを親ノード(最短ノード)と一緒にプロット
        path_images_start_tree = self._plot_2D_all_node_parent(pathes, start_tree_pos, start_tree_near_node, "red")
        # 終点ツリー内の全ノードを親ノードと一緒にプロット
        path_images_end_tree   = self._plot_2D_all_node_parent(pathes, end_tree_pos,   end_tree_near_node,   "blue")
        if path_images_start_tree and path_images_end_tree:
            path_images_start_end_tree = []
            path_images_start_end_tree.extend(path_images_start_tree)
            path_images_start_end_tree.extend(path_images_end_tree)
            for _ in range(self._ONE_CUT_NUM):
                imgs.append(path_images_start_end_tree)

            # アニメーション
            animation = ani.ArtistAnimation(self._figure, imgs)
            animation.save(self._PLOT_NAME, writer='imagemagick')
            plt.show()

            plot_result = True

        return plot_result

    def _plot_2DAnimation(self, pathes, start_tree_pos, start_tree_near_node, end_tree_pos, end_tree_near_node, debug_states, interference_obj):
        """
        2D(2次元)アニメーション作成
        
        パラメータ
            pathes(numpy.ndarray): 経路
            start_tree_pos(numpy.ndarray): 始点ツリーのノード位置
            start_tree_near_node(numpy.ndarray): 始点ツリーの親ノード
            end_tree_pos(numpy.ndarray): 始点ツリーのノード位置
            end_tree_near_node(numpy.ndarray): 始点ツリーの親ノード
            debug_states(list): デバッグ用の状態
            interference_obj(list): 円形の干渉物
        
        戻り値
            plot_result(bool): True / False = 成功 / 失敗
        """
        plot_result = False

        # データをリセットする
        self._reset2D()

        # 円形の干渉物をプロット
        self._plot_2Dinterference_obj(interference_obj)

        self._plot_2D_data(pathes, start_tree_pos, start_tree_near_node, end_tree_pos, end_tree_near_node, debug_states)

        plot_result = True

        return plot_result

    def _plot_2D_data(self, pathes, start_tree_pos, start_tree_near_node, end_tree_pos, end_tree_near_node, debug_states):
        """
        2D(2次元)データのプロット

        パラメータ
            pathes(numpy.ndarray): 経路
            start_tree_pos(numpy.ndarray): 始点ツリーのノード位置
            start_tree_near_node(numpy.ndarray): 始点ツリーの親ノード
            end_tree_pos(numpy.ndarray): 始点ツリーのノード位置
            end_tree_near_node(numpy.ndarray): 始点ツリーの親ノード
            debug_states(list): デバッグ用の状態
        """
        # 各ツリーの要素番号を定義
        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()

    def _update_3Ddata(self, i, pathes, start_tree_pos, start_tree_near_node, end_tree_pos, end_tree_near_node, debug_states, interference_obj):
        """
        3D(3次元)各データの更新

        パラメータ
            i(int): フレーム番号
            pathes(numpy.ndarray): 経路
            start_tree_pos(numpy.ndarray): 始点ツリーのノード位置
            start_tree_near_node(numpy.ndarray): 始点ツリーの親ノード
            end_tree_pos(numpy.ndarray): 始点ツリーのノード位置
            end_tree_near_node(numpy.ndarray): 始点ツリーの親ノード
            debug_states(list): デバッグ用の状態
            interference_obj(list): 干渉物
        """
        # 以前のプロットをクリアする
        self._axis.clear()
        self._set_3DAxis()
        
        # 干渉物のプロット
        self._plot_3Dinterference_obj(interference_obj)
        
        if i == 0:
            # フレーム数が初期値の場合は，ツリーのインデックス番号を初期化する
            self._start_tree_idx = 0
            self._end_tree_idx = 0
        
        self._plot_3D_data(i, pathes, start_tree_pos, start_tree_near_node, end_tree_pos, end_tree_near_node, debug_states)

    def _plot_3D_data(self, i, pathes, start_tree_pos, start_tree_near_node, end_tree_pos, end_tree_near_node, debug_states):
        """
        3D(3次元)各データのプロット

        パラメータ
            i(int): フレーム番号
            pathes(numpy.ndarray): 経路
            start_tree_pos(numpy.ndarray): 始点ツリーのノード位置
            start_tree_near_node(numpy.ndarray): 始点ツリーの親ノード
            end_tree_pos(numpy.ndarray): 始点ツリーのノード位置
            end_tree_near_node(numpy.ndarray): 始点ツリーの親ノード
            debug_states(list): デバッグ用の状態
        """
        if i < len(debug_states):
            # 状態に応じた処理を実施する
            state = debug_states[i]

            if state == STATE_START_TREE_RANDOM or state == STATE_START_TREE_END_TREE:        
                # 始点ツリーにランダム点を追加状態または始点ツリーから終点ツリーへノードを伸ばす状態
                self._start_tree_idx += 1
                # 追加した点の位置を取得
                now_pos   = start_tree_pos[self._start_tree_idx]
                # 追加した点の最短ノードを取得
                near_node = int(start_tree_near_node[self._start_tree_idx])
                # 最短ノードの位置を取得
                near_pos  = start_tree_pos[near_node]
            else:
                # 終点ツリーにランダム点を追加状態または終点ツリーから始点ツリーへノードを伸ばす状態
                self._end_tree_idx += 1
                # 追加した点の位置を取得
                now_pos   = end_tree_pos[self._end_tree_idx]
                # 追加した点の最短ノードを取得
                near_node = int(end_tree_near_node[self._end_tree_idx])
                # 最短ノードの位置を取得
                near_pos  = end_tree_pos[near_node]

            # 生成できている始点ツリーをプロット
            self._axis.scatter(start_tree_pos[:self._start_tree_idx + 1, 0], start_tree_pos[:self._start_tree_idx + 1, 1], start_tree_pos[:self._start_tree_idx + 1, 2],color="red", marker="*")
            # 生成できている終点ツリーをプロット
            self._axis.scatter(end_tree_pos[:self._end_tree_idx + 1, 0], end_tree_pos[:self._end_tree_idx + 1, 1], end_tree_pos[:self._end_tree_idx + 1, 2], color="orange", marker="*")

            # 始点をプロット
            self._axis.scatter(start_tree_pos[0, 0], start_tree_pos[0, 1], start_tree_pos[0, 2], color="cyan", marker="*", s=24)
            # 終点をプロット
            self._axis.scatter(end_tree_pos[0, 0], end_tree_pos[0, 1], end_tree_pos[0, 2], color="lime", marker="*", s=24)

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

            # 最短ノードもプロット
            plot_pos = np.append(now_pos.reshape(1, -1), near_pos.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)
            
            # 相手ツリーのランダムノードをプロット
            if state == STATE_START_TREE_END_TREE:
                # 終点ツリーのランダムノードをプロット
                self._axis.scatter(end_tree_pos[self._end_tree_idx, 0], end_tree_pos[self._end_tree_idx, 1], end_tree_pos[self._end_tree_idx, 2], color="green")
            elif state == STATE_END_TREE_START_TREE:
                self._axis.scatter(start_tree_pos[self._start_tree_idx, 0], start_tree_pos[self._start_tree_idx, 1], start_tree_pos[self._start_tree_idx, 2], color="green")

        else:
            # 生成した経路をプロットする
            self._axis.plot(pathes[:, 0], pathes[:, 1], pathes[:, 2], color="green")
            self._axis.scatter(start_tree_pos[:, 0], start_tree_pos[:, 1], start_tree_pos[:, 2], color="gray")
            self._axis.scatter(end_tree_pos[:, 0], end_tree_pos[:, 1], end_tree_pos[:, 2], color="gray")

    def _reset3D(self):
        """
        3次元データのリセット
        """
        # 親クラスのリセット処理
        super()._reset3D()
        # ツリーの要素番号をリセット
        self._start_tree_idx = 0
        self._end_tree_idx = 0

    def _plot_3D(self, pathes, start_tree_pos, start_tree_near_node, end_tree_pos, end_tree_near_node, interference_obj):
        """
        3D(3次元)グラフ作成
        
        パラメータ
            pathes(numpy.ndarray): 経路
            start_tree_pos(numpy.ndarray): 始点ツリーのノード位置
            start_tree_near_node(numpy.ndarray): 始点ツリーの親ノード
            end_tree_pos(numpy.ndarray): 始点ツリーのノード位置
            end_tree_near_node(numpy.ndarray): 始点ツリーの親ノード
            interference_obj(list): 円形の干渉物
        
        戻り値
            plot_result(bool): True / False = 成功 / 失敗
        """
        plot_result = False

        # データをリセットする
        self._reset3D()

        # 円形の干渉物をプロット
        self._plot_3Dinterference_obj(interference_obj)

        imgs = []

        # 始点ツリー内の全ノードを親ノード(最短ノード)と一緒にプロット (始点ツリーを赤色とする)
        path_images_start_tree = self._plot_3D_all_node_parent(pathes, start_tree_pos, start_tree_near_node, "red")
        # 終点ツリー内の全ノードを親ノードと一緒にプロット (終点ツリーを青色とする)
        path_images_end_tree   = self._plot_3D_all_node_parent(pathes, end_tree_pos,   end_tree_near_node,   "blue")
        if path_images_start_tree and path_images_end_tree:
            path_images_start_end_tree = []
            path_images_start_end_tree.extend(path_images_start_tree)
            path_images_start_end_tree.extend(path_images_end_tree)
            for _ in range(self._ONE_CUT_NUM):
                imgs.append(path_images_start_end_tree)

            # アニメーション
            animation = ani.ArtistAnimation(self._figure, imgs)
            animation.save(self._PLOT_NAME, writer='imagemagick')
            plt.show()

            plot_result = True

        return plot_result

    def _plot_3DAnimation(self, pathes, start_tree_pos, start_tree_near_node, end_tree_pos, end_tree_near_node, debug_states, interference_obj):
        """
        3D(3次元)アニメーション作成
        
        パラメータ
            pathes(numpy.ndarray): 経路
            start_tree_pos(numpy.ndarray): 始点ツリーのノード位置
            start_tree_near_node(numpy.ndarray): 始点ツリーの親ノード
            end_tree_pos(numpy.ndarray): 始点ツリーのノード位置
            end_tree_near_node(numpy.ndarray): 始点ツリーの親ノード
            debug_states(list): デバッグ用の状態
            interference_obj(list): 球の干渉物
        
        戻り値
            plot_result(bool): True / False = 成功 / 失敗
        """
        plot_result = False

        # データをリセットする
        self._reset3D()

        # アニメーションのフレーム数
        n_frame = len(debug_states) + self._ONE_CUT_NUM
        animation = ani.FuncAnimation(self._figure, self._update_3Ddata, fargs=(pathes, start_tree_pos, start_tree_near_node, end_tree_pos, end_tree_near_node, debug_states, interference_obj), interval=500, frames=n_frame)

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

        plot_result = True

        return plot_result

    def plot_Animation(self, pathes, start_tree, end_tree, debug_states, interference_obj, dimention):
        """
        アニメーション作成
        
        パラメータ
            pathes(numpy.ndarray): 経路
            start_tree(numpy.ndarray): 始点ツリー
            end_tree(numpy.ndarray): 終点ツリー
            debug_states(list): デバッグ用の状態
            interference_obj(list): 干渉物
            dimention(int): 次元数
        
        戻り値
            plot_result(bool): True / False = 成功 / 失敗
        """
        plot_result = False

        # 引数の確認
        if pathes.size == 0 or start_tree.size == 0 or end_tree.size == 0 or len(debug_states) == 0:
            return plot_result

        # 始点ツリーを位置と親ノードに分割
        start_tree_pos = start_tree[:, :RRT_CONNECT_NEAR_NODE_IDX]
        start_tree_near_node = start_tree[:, RRT_CONNECT_NEAR_NODE_IDX]
        # 終点ツリーを位置と親ノードに分割
        end_tree_pos = end_tree[:, :RRT_CONNECT_NEAR_NODE_IDX]
        end_tree_near_node = end_tree[:, RRT_CONNECT_NEAR_NODE_IDX]

        if dimention == DIMENTION_2D:   # 2次元データ
            plot_result = self._plot_2DAnimation(pathes, start_tree_pos, start_tree_near_node, end_tree_pos, end_tree_near_node, debug_states, interference_obj)
        elif dimention == DIMENTION_3D: # 3次元データ
            plot_result = self._plot_3DAnimation(pathes, start_tree_pos, start_tree_near_node, end_tree_pos, end_tree_near_node, debug_states, interference_obj)

        return plot_result


class RRTStarAnimation(RRTAnimation):
    """
    RRT*のアニメーション作成
    
    プロパティ
        _min_pos(numpy.ndarray): RRT*の最小探索範囲
        _max_pos(numpy.ndarray): RRT*の最大探索範囲
    
    publicメソッド (全てのクラスから参照可能)
        plot(): グラフ作成
        plot_Animation(): アニメーション作成
    
    protectedメソッド (自クラスまたは子クラスが参照可能)
        _plot_2D(): 2D(2次元)グラフ作成
        _plot_2DAnimation(): 2D(2次元)アニメーション作成
        _plot_3D(): 3D(3次元)グラフ作成
        _plot_3DAnimation(): 3D(3次元)アニメーション作成
        _update_3Ddata(): 3次元各データの更新
    """
    # 定数の定義
    _ONE_CUT_NUM = 6        # 1回
    _ANIMATION_NAME = "rrt_star_animation.gif"      # アニメーション保存ファイル名
    _PLOT_NAME      = "rrt_star_plot.gif"           # グラフ保存ファイル名

    def __init__(self, min_pos, max_pos):
        """
        コンストラクタ
        """
        # 親クラスの初期化処理
        super().__init__(min_pos, max_pos)

    def _plot_2D(self, pathes, debug_pathes_pos, debug_pathes_near_node, interference_obj):
        """
        2D(2次元)グラフ作成
        
        パラメータ
            pathes(numpy.ndarray): 経路
            debug_pathes_pos(numpy.ndarray): デバッグ用経路のノード位置
            debug_pathes_near_node(numpy.ndarray): デバッグ用経路の親ノード
            interference_obj(list): 円形の干渉物
        
        戻り値
            plot_result(bool): True / False = 成功 / 失敗
        """
        plot_result = False

        # データをリセットする
        self._reset2D()

        # 円形の干渉物をプロット
        self._plot_2Dinterference_obj(interference_obj)

        imgs = []

        # 全ノードを親ノード(最短ノード)と一緒にプロット
        path_images = self._plot_2D_all_node_parent(pathes, debug_pathes_pos, debug_pathes_near_node)
        if path_images:
            for _ in range(self._ONE_CUT_NUM):
                imgs.append(path_images)

            # アニメーション
            animation = ani.ArtistAnimation(self._figure, imgs)
            animation.save(self._PLOT_NAME, writer='imagemagick')
            plt.show()

            plot_result = True

        return plot_result

    def plot(self, pathes, folder_name, file_name, n_file, extension, interference_obj, dimention):
        """
        グラフ作成
        
        パラメータ
            pathes(numpy.ndarray): 経路
            folder_name(str): フォルダー名
            file_name(str): ファイル名
            n_file(int): ファイル数
            extension(str): ファイルの拡張子
            interference_obj(list): 円形の干渉物
            dimention(int): 次元数
        
        戻り値
            plot_result(bool): True / False = 成功 / 失敗
        """
        plot_result = False

        # 引数の確認
        if pathes.size == 0 or not folder_name or not file_name or n_file <= 0 or not extension:
            # 引数が異常
            return plot_result

        # プロットするデータが保存されているファイル名
        f_name = f"{folder_name}/{file_name}_{n_file}.{extension}"

        try:
            file_data  = np.loadtxt(f_name)
        except Exception as e:      # ファイル読み込みで例外エラー発生
            print(f"Error is {e}")
            return plot_result

        # ファイルから，位置・最短ノード・コスト・近傍判定の半径に分割
        debug_pathes_pos       = file_data[:, :RRT_STAR_NEAR_NODE_IDX]
        debug_pathes_near_node = file_data[:,  RRT_STAR_NEAR_NODE_IDX]
        debug_pathes_cost      = file_data[:,  RRT_STAR_COST_IDX]
        debug_pathes_radius    = file_data[:,  RRT_STAR_RADIUS_IDX]

        if dimention == DIMENTION_2D:
            plot_result = self._plot_2D(pathes, debug_pathes_pos, debug_pathes_near_node, interference_obj)
        # 新規追加 ↓
        elif dimention == DIMENTION_3D:
            plot_result = self._plot_3D(pathes, debug_pathes_pos, debug_pathes_near_node, interference_obj)
        # 新規追加 ↑

        return plot_result

    def _plot_3D(self, pathes, debug_pathes_pos, debug_pathes_near_node, interference_obj):
        """
        3D(3次元)グラフ作成
        
        パラメータ
            pathes(numpy.ndarray): 経路
            debug_pathes_pos(numpy.ndarray): デバッグ用経路のノード位置
            debug_pathes_near_node(numpy.ndarray): デバッグ用経路の親ノード
            interference_obj(list): 球の干渉物
        
        戻り値
            plot_result(bool): True / False = 成功 / 失敗
        """
        plot_result = False

        # データをリセットする
        self._reset3D()

        # 球の干渉物をプロット
        self._plot_3Dinterference_obj(interference_obj)

        imgs = []

        # 始点ツリー内の全ノードを親ノード(最短ノード)と一緒にプロット (始点ツリーを赤色とする)
        path_images = self._plot_3D_all_node_parent(pathes, debug_pathes_pos, debug_pathes_near_node)
        if path_images:
            for _ in range(self._ONE_CUT_NUM):
                imgs.append(path_images)

            # アニメーション
            animation = ani.ArtistAnimation(self._figure, imgs)
            animation.save(self._PLOT_NAME, writer='imagemagick')
            plt.show()

            plot_result = True

        return plot_result

    def _plot_2D_near_node_radius(self, center_pos, radius, color="gray", alpha=0.5):
        """
        新規ノードと近傍ノードの範囲を一緒にプロット
        
        パラメータ
            center_pos(numpy.ndarray): 中心位置
            radius(float): 半径
            color(str): 近傍ノードの範囲の色
            alpha(float): 近傍ノードの色の濃さ (0 ~ 1の範囲)
        
        戻り値
            image(list): 近傍ノードの範囲
        """
        if alpha < 0 or alpha > 1:
            # 範囲外のため，修正
            alpha = 0.5

        # 近傍ノードを円形でプロット
        circle = patches.Circle(center_pos, radius, color=color, alpha=alpha)
        image = [self._axis.add_patch(circle)]

        return image

    def _plot_2DAnimation(self, pathes, folder_name, file_name, n_file, extension, interference_obj):
        """
        2D(2次元)アニメーション作成
        
        パラメータ
            pathes(numpy.ndarray): 経路
            folder_name(str): フォルダー名
            file_name(str): ファイル名
            n_file(int): ファイル数
            extension(str): ファイルの拡張子
            interference_obj(list): 干渉物
        
        戻り値
            plot_result(bool): True / False = 成功 / 失敗

        Args:
            pathes (_type_): _description_
            folder_name (_type_): _description_
            file_name (_type_): _description_
            n_file (_type_): _description_
            extension (_type_): _description_
            interference_obj (_type_): _description_
        """
        # データをリセットする
        self._reset2D()

        # 円形の干渉物をプロット
        self._plot_2Dinterference_obj(interference_obj)

        imgs = []       # プロットしたいデータを全部保存する

        for i in range(1, n_file + 1):
            print(f"file_num = {i}")
            path_images = []
            # 全ファイルを読み込む (ファイル名は1から開始するから，range()に1を追加)
            f_name = f"{folder_name}/{file_name}_{i}.{extension}"
            file_data  = np.loadtxt(f_name)
            # ファイルから，位置・最短ノード・コスト・近傍ノードの半径に分割
            position   = file_data[:, :RRT_STAR_NEAR_NODE_IDX]
            near_node  = file_data[:,  RRT_STAR_NEAR_NODE_IDX]
            cost       = file_data[:,  RRT_STAR_COST_IDX]
            radius     = file_data[:,  RRT_STAR_RADIUS_IDX]

            # ファイル内のデータを親ノードと一緒にプロットする
            image = self._plot_2D_all_node_parent(pathes, position, near_node, pathes_plot=False)
            path_images.extend(image)
            # 新規ノードと近傍ノードの範囲を一緒にプロットする
            image = self._plot_2D_near_node_radius(position[-1], radius[-1], color="blue", alpha=0.3)
            path_images.extend(image)
            imgs.append(path_images)

        # 最終的なパスもプロット
        for _ in range(self._ONE_CUT_NUM):
            path_images = self._plot_2D_all_node_parent(pathes, position, near_node, pathes_plot=True)
            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 plot_Animation(self, pathes, folder_name, file_name, n_file, extension, interference_obj, dimention):
        """
        アニメーション作成
        
        パラメータ
            pathes(numpy.ndarray): 経路
            folder_name(str): フォルダー名
            file_name(str): ファイル名
            n_file(int): ファイル数
            extension(str): ファイルの拡張子
            interference_obj(list): 干渉物
            dimention(int): 次元数
        
        戻り値
            plot_result(bool): True / False = 成功 / 失敗
        """
        plot_result = False

        # 引数の確認
        if pathes.size == 0 or not folder_name or not file_name or n_file <= 0 or not extension:
            # 引数が異常
            return plot_result

        if dimention == DIMENTION_2D:
            plot_result = self._plot_2DAnimation(pathes, folder_name, file_name, n_file, extension, interference_obj)

        return plot_result

main.py

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

# 自作モジュールの読み込み
from rrt import RRT, RRTConnect, RRTStar        # 経路生成アルゴリズム
from rrt_animation import RRTAnimation, RRTConnectAnimation, RRTStarAnimation   # 経路生成のアニメーション
from interference import Circle, Ball   # 干渉物
from constant import *              # 定数


START_POS = 0                       # 始点位置
END_POS   = 2                       # 終点位置


def rrt_planning(dimention, plot_mode, start_pos, end_pos, interference_obj):
    """
    RRTによる経路生成
    
    パラメータ
        dimention(int): 次元数
        plot_mode(int): プロット状態
        start_pos(numpy.ndarray): 始点
        end_pos(numpy.ndarray): 終点
        interference_obj(list): 干渉物
    
    戻り値
        planning_result(bool): True / False = 経路生成に成功 / 失敗
        elapsed_time(float): 経路生成の処理時間 [sec]
        n_waypoint(int): 経由点数(始点と終点を含む)
    """
    # 戻り値の初期化
    planning_result = False
    elapsed_time = 0.0
    n_waypoint = 0

    # RRTクラスのインスタンスを作成
    rrt = RRT()
    # 始点から終点までの経路を生成
    start_time = time.time()
    planning_result = rrt.planning(start_pos, end_pos, interference_obj)
    if planning_result:
        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 plot_mode == PLOT_GRAPH:         # グラフの描画
            result = rrtAni.plot(rrt.pathes, rrt.debug_pathes, interference_obj, dimention)
        elif plot_mode == PLOT_ANIMATION:   # アニメーションの映画
            result = rrtAni.plot_Animation(rrt.pathes, rrt.debug_pathes, rrt.debug_random_pathes, interference_obj, dimention)

    return planning_result, elapsed_time, n_waypoint


def rrt_connect_planning(dimention, plot_mode, start_pos, end_pos, interference_obj):
    """
    RRT-Connectによる経路生成
    
    パラメータ
        dimention(int): 次元数
        plot_mode(int): プロット状態
        start_pos(numpy.ndarray): 始点
        end_pos(numpy.ndarray): 終点
        interference_obj(list): 干渉物
    
    戻り値
        planning_result(bool): True / False = 経路生成に成功 / 失敗
        elapsed_time(float): 経路生成の処理時間 [sec]
        n_waypoint(int): 経由点数(始点と終点を含む)
    """
    # 戻り値の初期化
    planning_result = False
    elapsed_time = 0.0
    n_waypoint = 0

    # RRT-Connectクラスのインスタンスを作成
    rrt_connect = RRTConnect()
    # 始点から終点までの経路を生成
    start_time = time.time()
    planning_result = rrt_connect.planning(start_pos, end_pos, interference_obj)
    if planning_result:
        # 経路生成に成功
        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 plot_mode == PLOT_GRAPH:     # グラフの描画
            result = rrt_connect_animation.plot(rrt_connect.pathes, rrt_connect.start_tree, rrt_connect.end_tree, interference_obj, dimention)
        elif plot_mode == PLOT_ANIMATION:   # アニメーションの映画
            result = rrt_connect_animation.plot_Animation(rrt_connect.pathes, rrt_connect.start_tree, rrt_connect.end_tree, rrt_connect.debug_states, interference_obj, dimention)

    return planning_result, elapsed_time, n_waypoint


def rrt_star_planning(dimention, plot_mode, start_pos, end_pos, interference_obj):
    """
    RRT*による経路生成
    
    パラメータ
        dimention(int): 次元数
        plot_mode(int): プロット状態
        start_pos(numpy.ndarray): 始点
        end_pos(numpy.ndarray): 終点
        interference_obj(list): 干渉物
    
    戻り値
        planning_result(bool): True / False = 経路生成に成功 / 失敗
        elapsed_time(float): 経路生成の処理時間 [sec]
        n_waypoint(int): 経由点数(始点と終点を含む)
    """
    # 戻り値の初期化
    planning_result = False
    elapsed_time = 0.0
    n_waypoint = 0

    # RRT*クラスのインスタンスを作成
    rrt_star = RRTStar()
    rrt_star.max_iterate = 300    # アニメーションの容量を考慮したら，300が良かった
    # 始点から終点までの経路を生成
    start_time = time.time()
    planning_result = rrt_star.planning(start_pos, end_pos, interference_obj)
    if planning_result:
        # 経路生成に成功
        end_time = time.time()
        # 処理時間を計算
        elapsed_time = end_time - start_time
        # 経由点数を計算
        n_waypoint = rrt_star.pathes.shape[0]
        # 生成した経路をファイルに保存
        rrt_star.save()

        # RRT*アニメーションのインスタンスを作成
        rrtStarAni = RRTStarAnimation(rrt_star.strict_min_pos, rrt_star.strict_max_pos)
        # グラフ描画とアニメーション描画で同じ引数としたい
        if plot_mode == PLOT_GRAPH:         # グラフの描画
            result = rrtStarAni.plot(rrt_star.pathes, rrt_star.foler_name_current_tree, rrt_star.file_name_current_tree, rrt_star.count_success, "csv", interference_obj, dimention)
        elif plot_mode == PLOT_ANIMATION:   # アニメーションの描画
            result = rrtStarAni.plot_Animation(rrt_star.pathes, rrt_star.foler_name_current_tree, rrt_star.file_name_current_tree, rrt_star.count_success, "csv", interference_obj, dimention)

    return planning_result, elapsed_time, n_waypoint


def path_planning(path_plan, dimention, plot_mode):
    """
    経路生成
        path_plan(int): 経路生成アルゴリズム番号
        dimention(int): 次元数
        plot_mode(int): プロット状態
    
    戻り値
        planning_result(bool): True / False = 経路生成に成功 / 失敗
        elapsed_time(float): 経路生成の処理時間 [sec]
        n_waypoint(int): 経由点数(始点と終点を含む)
    """
    # 戻り値の初期化
    planning_result = False
    elapsed_time = 0
    n_waypoint = 0

    if not (dimention == DIMENTION_2D or dimention == DIMENTION_3D):
        # 次元数が2 or 3以外は処理終了
        return planning_result, elapsed_time, n_waypoint

    # 始点と終点を作成
    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 == DIMENTION_2D:
        # 円形の干渉物を作成
        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 == DIMENTION_3D:
        # 球の干渉物を作成
        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 path_plan == PATH_PLANNING_RRT:                      # RRTによる経路生成
        planning_result, elapsed_time, n_waypoint = rrt_planning(dimention, plot_mode, start_pos, end_pos, interference_obj)

    elif path_plan == PATH_PLANNING_RRT_CONNECT:            # RRT-Connectによる経路生成
        planning_result, elapsed_time, n_waypoint = rrt_connect_planning(dimention, plot_mode, start_pos, end_pos, interference_obj)

    elif path_plan == PATH_PLANNING_RRT_STAR:               # RRT*による経路生成
        planning_result, elapsed_time, n_waypoint = rrt_star_planning(dimention, plot_mode, start_pos, end_pos, interference_obj)

    print(f"planning_result = {planning_result}")
    return planning_result, elapsed_time, n_waypoint


def main():
    """
    メイン処理
    """
    # プロットできる状態
    plot_modes = [PLOT_GRAPH, PLOT_ANIMATION, PLOT_NONE]
    # plot_modes = [PLOT_NONE, ]
    # 経路生成アルゴリズム
    path_plans = [PATH_PLANNING_RRT, PATH_PLANNING_RRT_CONNECT, PATH_PLANNING_RRT_STAR]
    # path_plans = [PATH_PLANNING_RRT, ]  # Informed RRT*だけを実施する場合
    # 次元数
    # dimentions = [DIMENTION_2D, DIMENTION_3D]
    dimentions = [DIMENTION_2D, ]

    # 全アルゴリズム，全次元，全プロット状態の全組み合わせを実施する
    for path_plan in path_plans:
        for dimention in dimentions:
            for plot_mode in plot_modes:
                if path_plan == PATH_PLANNING_RRT:
                    print("RRT")
                elif path_plan == PATH_PLANNING_RRT_CONNECT:
                    print("RRT-Connect")
                elif path_plan == PATH_PLANNING_RRT_STAR:
                    print("RRT*")

                if plot_mode == PLOT_GRAPH or plot_mode == PLOT_ANIMATION:
                    # グラフまたはアニメーションを1つ作成
                    path_planning(path_plan, dimention, plot_mode)
                else:
                    # 経路生成の成功割合や処理時間，経由点数の統計量を計算
                    planning_results = 0                # 経路生成の成功回数
                    elapsed_times = []                  # 処理時間
                    n_waypoints   = []                  # 経由点数
                    n_path_plan   = 30                  # 経路生成の回数

                    for i in range(n_path_plan):
                        np.random.seed(i)               # RRT, RRT-Connect, RRT*で比較するためにシード値を設定する
                        result, elapsed_time, waypoint = path_planning(path_plan, dimention, plot_mode)
                        if result:
                            planning_results += 1
                            elapsed_times.append(elapsed_time)
                            n_waypoints.append(waypoint)

                    # 経路生成の成功割合
                    success_rate = planning_results / n_path_plan
                    print(f"success_rate = {success_rate}")

                    if elapsed_times:
                        # 処理時間の平均，最大，最小，標準偏差を出力
                        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)}")
                    if n_waypoints:
                        # 経由点の平均，最大，最小，標準偏差を出力
                        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)}")


if __name__ == "__main__":
    # 本ファイルがメインで呼ばれた時の処理
    main()

RRT* による経路生成アニメーション

RRT* による経路生成のアニメーションを展開する．
アニメーションを下図に添付した．
アニメーションの見方は以下の通りである．
・水色の星形(左下)データ：経路生成の始点
・緑色の星形(右上)データ：経路生成の終点
・灰色の星形データ：作成されたノード
・赤色の線：新規データと最短コスト(距離)の親ノードを結んだ線
・緑色の線：最終的な始点から終点までの経路
・青色の円：近傍ノードを選択する範囲
・灰色の円：干渉物

アニメーションの結果より，ノード数が増えるほど，近傍ノードの範囲(青色の円)が小さくなり，想定通りの結果となることが確認できた．青色の円が大きいと処理時間も伸びるため，パラメータ調整がむz
また，ノード数を増やすことで，ノードの親ノードも更新されていることも確認できた．
RRT* は最適な経路となるが，処理時間の長さがネックである．

おわりに

本記事では，Pythonを使用して，下記内容を実装しました．
・2次元空間で，円形の干渉物を回避する RRT* による経路生成 + アニメーション化

次記事では，下記内容を実装していきます．
・3次元空間で，球の干渉物を回避する RRT* による経路生成 + アニメーション化