はじめに

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

RRT*-Connect は最適性を保証する RRT* ，始点と終点の双方向から経路生成する RRT-Connect を組み合わせた手法である．
RRT-Connect は以下記事で説明したため，本記事では割愛する．
・https://qiita.com/haruhiro1020/items/2b3985495b864a4a7e16
RRT* は以下記事で説明したため，本記事では割愛する．
・https://qiita.com/haruhiro1020/items/7c5bdbe530711c9d331b

RRT*-Connect は下記を参考にした．
参考文献：
S.Klemm, J.Oberlander et al. "RRT*-Connect: Faster, Asymptotically Optimal Motion Planning", IEEE 2015
本記事では，RRT* とRRT*-Connect の違いを説明したのち，ソースコードおよび性能評価を実施する．

本記事で経路生成する環境は下図の通りである．
青色の点が3軸ロボットアームの初期位置，赤色の点が3軸ロボットアームの目標位置である．
下図環境下で，干渉しない経路を生成するのが，本記事の目標である．

3軸ロボットアームに関しては，下記リンクに記載．
・https://qiita.com/haruhiro1020/items/6994a94efaff98d8dae1
python-fclに関しては，下記リンクに記載．
・https://qiita.com/haruhiro1020/items/d364e4c56b1570e1423a

本記事で実装すること

・球と直方体の干渉物が存在する3次元空間で，3軸ロボットアームを使用してRRT*-Connect による経路生成
・RRT* とRRT*-Connect の経路生成結果をアニメーションで表現

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

・球と直方体の干渉物が存在する3次元空間で，6軸ロボットアームを使用してRRT*-Conect による経路生成

動作環境

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

経路生成手法のまとめ

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

ソースコードに関して

投稿しているソースコードは綺麗でないため，随時更新しようと考えております．
また，処理時間を考慮した作りになっていないため，処理時間が高速となる作り方を勉強した後にソースコードを更新しようと考えております．

経路生成アルゴリズムに関して

経路生成のアルゴリズムを説明する．

RRTに関して

RRTアルゴリズムは以下記事に記載したため，本記事では割愛する．
(https://qiita.com/haruhiro1020/items/7c5bdbe530711c9d331b)

RRT-Connect に関して

RRT-Connect アルゴリズムは以下記事に記載したため，本記事では割愛する．
(https://qiita.com/haruhiro1020/items/7c5bdbe530711c9d331b)

RRT* に関して

RRT* アルゴリズムは以下記事に記載したため，本記事では割愛する．
(https://qiita.com/haruhiro1020/items/7c5bdbe530711c9d331b)

RRT*-Connect に関して

RRT*-Connect アルゴリズムは前記事に記載したため，本記事では割愛する．
(https://qiita.com/haruhiro1020/items/253764ced728003dba75)

3軸ロボットアームに関して

3軸ロボットアームを説明する．
ロボットアームの順運動学(関節角度から手先位置を算出)は以下記事にて説明した．
・https://qiita.com/haruhiro1020/items/27c0cf098056dc792ab9
ロボットアームの逆運動学(手先位置から関節角度を算出)は以下記事にて説明した．
・https://qiita.com/haruhiro1020/items/3d72eddc3f623eee963a

ソースコードの説明

Pythonにて，RRT*-Connect のソースコードを作成する．

定数の定義 (constant.py)

複数ファイルで使用する定数を定義したファイルは下記の通りです．

constant.py

# 複数ファイルで使用する定数の定義
from enum import Enum


# 次元数を定義
DIMENTION_NONE  = -1    # 未定義
DIMENTION_2D    =  2    # 2次元
DIMENTION_3D    =  3    # 3次元


# シード値の最小値と最大値
MIN_SEED    = 0
MAX_SEED    = 2 ** 32 - 1  # 4バイト (uint) の最大値


# ノードの最短ノード要素とコスト要素を定義
# RRT
RRT_NEAR_NODE_IDX               = -1    # RRTでの最短ノード要素
# RRT-Connect
RRT_CONNECT_NEAR_NODE_IDX       = -1    # RRT-Connectでの最短ノード要素
# RRT*
RRT_STAR_NEAR_NODE_IDX          = -3    # RRT*での最短ノード勝訴
RRT_STAR_COST_IDX               = -2    # RRT*でのコスト要素
RRT_STAR_RADIUS_IDX             = -1    # RRT*での半径要素
# Informed RRT*
INFORMED_RRT_STAR_NEAR_NODE_IDX = -3    # Informed RRT*での最短ノード勝訴
INFORMED_RRT_STAR_COST_IDX      = -2    # Informed RRT*でのコスト要素
INFORMED_RRT_STAR_RADIUS_IDX    = -1    # Informed RRT*での半径要素
# RRT*-Smart
RRT_STAR_SMART_NEAR_NODE_IDX    = -3    # RRT*-Smartでの最短ノード勝訴
RRT_STAR_SMART_COST_IDX         = -2    # RRT*-Smartでのコスト要素
RRT_STAR_SMART_RADIUS_IDX       = -1    # RRT*-Smartでの半径要素
# RRT*-Connect
RRT_STAR_CONNECT_NEAR_NODE_IDX  = -3    # RRT*-Smartでの最短ノード勝訴
RRT_STAR_CONNECT_COST_IDX       = -2    # RRT*-Smartでのコスト要素
RRT_STAR_CONNECT_RADIUS_IDX     = -1    # RRT*-Smartでの半径要素


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


# 回転軸
ROTATION_X_AXIS = "rot_x"   # x軸周りに回転
ROTATION_Y_AXIS = "rot_y"   # y軸周りに回転
ROTATION_Z_AXIS = "rot_z"   # z軸周りに回転
ROTATION_X_NEGATIVE_AXIS = "rot_neg_x"  # x軸周りに逆回転
ROTATION_Y_NEGATIVE_AXIS = "rot_neg_y"  # y軸周りに逆回転
ROTATION_Z_NEGATIVE_AXIS = "rot_neg_z"  # z軸周りに逆回転


# ツリーの初期ノードの親ノード
INITIAL_NODE_NEAR_NODE      = -1        # 初期ノードに親ノードが存在しないから-1


# 補間時の分割する時の距離
DEVIDED_DISTANCE_JOINT      = 0.1       # 関節補間時の距離 [rad]
DEVIDED_DISTANCE_POSIION    = 0.1       # 位置補間時の距離 [m]


# 干渉物の名称を定義
class INTERFERENCE(Enum):
    """
    干渉物の名称を定義
    """
    NONE         = ""          # 未定義
    # 2次元の干渉物 ↓
    CIRCLE       = "circle"    # 円形の干渉物
    RECTANGLE    = "rectangle" # 長方形の干渉物
    LINE2D       = "line2D"    # 2次元の直線
    # 2次元の干渉物 ↑
    # 3次元の干渉物 ↓
    BALL         = "ball"      # 球の干渉物
    CUBOID       = "cuboid"    # 直方体の干渉物
    LINE3D       = "line3D"    # 3次元の直線
    # 3次元の干渉物 ↑


# 0割を防ぐための定数
EPSILON                   = 1e-6


# プロットする時にグラフ(静止画)とするかアニメーションを定義
PLOT_NONE       = 20        # プロットしない
PLOT_GRAPH      = "graph"   # グラフ
PLOT_ANIMATION  = "anime"   # アニメーション


# 補間方法の定義
class INTERPOLATION(Enum):
    """
    補間方法
    """
    JOINT     = "joint"     # 関節補間
    POSITION  = "pos"       # 位置補間
    LINEAR    = "linear"    # 直線補間
    CUBIC     = "cubic"     # 3次補間
    QUINTIC   = "quintic"   # 5次補間


# RRTで作成した経路の修正方法を定義
class MODIFICATION(Enum):
    """
    修正方法
    """
    # 修正方法が単一
    PRUNING_START       = "pruning_start"       # 始点からのPruning
    PRUNING_END         = "pruning_end"         # 終点からのPruning
    PRUNING_RANDOM      = "pruning_random"      # ランダム点のPruning
    SHORTCUT_NORMAL     = "shortcut"            # Shortcut
    SHORTCUT_PARTIAL    = "partial_shortcut"    # Partial Shortcut
    # 修正方法の組み合わせ
    PARTIAL_START       = "partial_start"       # Partial Shortcut + 始点からのPruning


# RRTによる経路生成手法の定義
class PATHPLAN(Enum):
    """
    経路生成手法
    """
    RRT                 = "rrt"                 # RRTによる経路生成
    # 双方向RRT
    RRT_CONNECT         = "rrt_connect"         # RRT-Connectによる経路生成
    # RRT*系列
    RRT_STAR            = "rrt_star"            # RRT*による経路生成
    RRT_STAR_SMART      = "rrt_star_smart"      # RRT*-Smartによる経路生成
    RRT_STAR_CONNECT    = "rrt_star_connect"    # RRT*-Connectによる経路生成
    INFORMED_RRT_STAR   = "informed_rrt_star"   # Informed RRT*による経路生成

RRT*-Connect による経路生成 (rrt.py)

上で説明した，RRT*-Connect ソースコードは下記の通りです．
下記の RRTStarConnectRobot クラスを新規作成した．
RRT* クラスと RRT-Connect クラスを多重継承している．

rrt.py

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

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

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


class Tree:
    """
    ツリークラス

    プロパティ
        _nodes(numpy.ndarray): ノード
        _near_node_idx(int): _nodes内の最短ノードを保存している列番号

    メソッド
        public
            nodes(): _nodesプロパティのゲッター
            reset(): データの初期化
            add_node(): ノードの追加
            get_near_node(): 最短距離のノードを取得
            get_near_node_list(): ノードと近傍ノードをリストで取得

        protected
            _chk_node_exist(): ノードが存在するかの確認
    """
    # 定数の定義


    def __init__(self, near_node_idx):
        """
        コンストラクタ
        """
        # プロパティの初期化
        self._nodes = []
        self._near_node_idx = near_node_idx

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

    def reset(self):
        """
        データの初期化
        """
        if len(self._nodes) != 0:
            # 何かしらのデータが保存
            del self._nodes
        self._nodes = []

    def add_node(self, node):
        """
        ノードの追加

        パラメータ
            node(numpy.ndarray): ノード
        """
        if len(self._nodes) == 0:       # 初回だけ実行
            self._nodes = node
        else:
            self._nodes = np.append(self._nodes, node, axis=0)

    def _chk_node_exist(self):
        """
        ノードが存在するかの確認
        """
        if len(self._nodes) == 0:
            # 存在しない
            raise ValueError("self._nodes is not exist")

    def get_near_node(self, pos):
        """
        最短距離のノードを取得

        パラメータ
            pos(numpy.ndarray): 位置

        戻り値
            min_dist_idx(int): 最短距離のノード番号
        """
        # ノードの存在確認
        self._chk_node_exist()

        # ノードから位置を取得
        nodes_pos = self._nodes[:, :self._near_node_idx]
        # 差分を計算
        difference = nodes_pos - pos
        # 距離を計算 (各ノードとの距離を算出するため，引数にaxis=1を与えた)
        distance = np.linalg.norm(difference, axis=1)
        # 最短距離ノードを取得
        min_dist_idx = np.argmin(distance)

        return min_dist_idx

    def get_near_node_list(self, pos, radius):
        """
        ノードと近傍ノードをリストで取得

        パラメータ
            pos(numpy.ndarray): ノード位置
            radius(float): 半径

        戻り値
            near_node_list(list): 近傍ノードリスト
        """
        # ノードの存在確認
        self._chk_node_exist()

        near_node_list = []
        # ツリー内全ノード位置を取得
        all_node_pos = self._nodes[:, :self._near_node_idx]
        # ノードとツリー内全ノードの差分を計算
        difference = all_node_pos - pos
        # 差分から距離(ユークリッド距離)を計算
        distance = np.linalg.norm(difference, axis=1)

        # 距離が一定以内のノードだけを保存
        near_node_list = [idx for idx, dist in enumerate(distance) if dist <= radius]

        return near_node_list


class RRTRobot:
    """
    RRTにロボットを追加したクラス

    プロパティ
        _name(str): 経路生成手法名
        _pathes(numpy.ndarray): 始点から終点までの経路 (PruningやShortcut済みの経路)
        _start_tree(Tree): 始点ツリー
        _start_random_tree(Tree): ランダムな位置を保存した始点ツリー
        _strict_min_pos(numpy.ndarray): 探索の最小範囲
        _strict_max_pos(numpy.ndarray): 探索の最大範囲
        _environment(environment.py内のクラス): 経路生成したい環境
        _debug(bool): デバッグフラグ (グラフやアニメーションでデータを確認したいかどうか)
        _robot(robot.py内のクラス): ロボットクラス
        _moving_value(float): 1回あたりの移動量 [rad] or [m]
        _before_modify_pathes(numpy.ndarray): PruningやShortcutなどの修正前の経路
        _pruning_pathes(numpy.ndarray): Pruning後の経路
        _shortcut_pathes(numpy.ndarray): Shortcut後の経路
        _partial_shortcut_pathes(numpy.ndarray): Partial Shortcut後の経路

    メソッド
        public

            プロパティのゲッター関連
                name(): _nameプロパティのゲッター
                start_tree(): _treeプロパティのゲッター
                start_random_tree(): _random_treeプロパティのゲッター
                pathes(): _pathesプロパティのゲッター
                strict_min_pos(): _strict_min_posプロパティのゲッター
                strict_max_pos(): _strict_max_posプロパティのゲッター
                debug(): _debugプロパティのゲッター
                robot(): _robotプロパティのゲッター
                environment(): _environmentプロパティのゲッター
                interpolation(): _interpolationプロパティのゲッター
                dim(): _dimプロパティのゲッター

            メイン処理関連
                planning(): 経路生成の実装


        protected

            メイン処理関連
                _add_node_start_tree(): 始点ツリーにノードを追加
                _chk_end_pos_dist(): 終点との距離が一定範囲内であるかの確認
                _fin_planning(): 経路生成の終了処理
                _calc_new_pos(): 最短ノードからランダムな値方向へ新しいノード(位置)を作成
                _get_random_pos(): ランダムな位置を取得

            準備処理関連
                _preparation_planning(): 経路生成の準備
                _reset(): データの初期化
                _set_robot(): 経路生成したいロボット情報を設定
                _strict_planning_pos(): 探索範囲を制限する

            干渉判定の処理関連
                _line_interference(): 2点間の干渉チェック
                _interference_pos(): 干渉判定

            ファイル関連
                _make_folder(): フォルダーの作成
                _reset_folder(): フォルダー内のファイルを全削除
                _get_path_plan_folders(): 経路生成結果を保存する複数のフォルダー名を取得
                _make_path_plan_folder(): 経路生成結果を保存するフォルダー作成
                _save(): 生成した経路をファイル保存
                _save_numpy_data_to_txt(): Numpyデータをテキストファイルに保存

            経路生成後の後処理関連
                _modify_path(): 始点から終点までの経路を修正
                _get_shortcut_two_nodes(): Shortcutで使用する2つのノードを取得
                _path_partial_shortcut(): Partial Shortcut (経路上の2点をランダムで選択して, 2点内の全ノード内の1パラメータを直線補間して, 干渉なければ2点内のノード内の1パラメータを修正)
                _path_shortcut(): Shortcut (経路上の2点をランダムで選択して, 干渉なければ2点内のノードを全削除)
                _path_pruning_random(): Pruning (経路の経由点をランダム点の削除)
                _path_pruning_from_end(): Pruning (経路の経由点を終点方向から順番に削除)
                _path_pruning_from_start(): Pruning (経路の経由点を始点方向から順番に削除)
    """
    # 定数の定義
    # ファイル名の定義
    # 各経路生成手法で絶対に定義するべきファイル名 ↓
    # _pathesプロパティを保存するファイル名
    _FILE_NAME_PATHES = f"{PATHPLAN.RRT.value}_pathes.csv"
    # _start_treeプロパティを保存するファイル名
    _FILE_NAME_START_TREE = f"{PATHPLAN.RRT.value}_start_tree.csv"
    # _start_random_treeプロパティを保存するファイル名
    _FILE_NAME_START_RANDOM_TREE = f"{PATHPLAN.RRT.value}_start_random_tree.csv"
    # _before_modify_pathesプロパティを保存するファイル名
    _FILE_NAME_BEFORE_MODIFY_PATHES = f"{PATHPLAN.RRT.value}_before_modify_pathes.csv"
    # _pruning_pathesプロパティを保存するファイル名
    _FILE_NAME_PRUNING_PATHES = f"{PATHPLAN.RRT.value}_pruning_pathes.csv"
    # _shortcut_pathesプロパティを保存するファイル名
    _FILE_NAME_SHORTCUT_PATHES = f"{PATHPLAN.RRT.value}_shortcut_pathes.csv"
    # _partial_shortcut_pathesプロパティを保存するファイル名
    _FILE_NAME_PARTIAL_SHORTCUT_PATHES = f"{PATHPLAN.RRT.value}_partial_shortcut_pathes.csv"

    # 毎処理保存するファイル名の定義
    # Pruning実施毎の経路を保存
    _FILE_NAME_PRUING_EACH_PATHES = f"{PATHPLAN.RRT.value}_pruning_each_pathes"
    # Shortcut実施毎の経路を保存
    _FILE_NAME_SHORTCUT_EACH_PATHES = f"{PATHPLAN.RRT.value}_shortcut_each_pathes"
    # Partial Shortcut実施毎の経路を保存
    _FILE_NAME_PARTIAL_SHORTCUT_EACH_PATHES = f"{PATHPLAN.RRT.value}_partial_shortcut_each_pathes"
    # 各経路生成手法で絶対に定義するべきファイル名 ↑

    # フォルダー名の定義
    _FOLODER_NAME_PRUNING = "Pruning"                             # Pruning結果を保存するフォルダ
    # Shortcut結果を保存するフォルダ
    _FOLODER_NAME_SHORTCUT = "Shortcut"
    # PartialShortcut結果を保存するフォルダ
    _FOLODER_NAME_PARTIAL_SHORTCUT = "PartialShortcut"

    # ツリーの要素番号を定義
    _NODE_NEAR_NODE_IDX = RRT_NEAR_NODE_IDX     # ノード内の最短ノード要素

    # 干渉判定の定義
    _DEVIDED_DISTANCE_POS = 0.01     # 2点間を分割する際の基準距離 [m]
    _DEVIDED_DISTANCE_JOINT = 0.005     # 2点間を分割する際の基準距離 [rad]
    # 干渉物とのマージン (最短距離がマージンよりも小さかったら干渉ありとする) [m]
    _INTERFERENCE_MARGIN = 0.05

    # 探索に関する定義
    _MOVING_VALUE_JOINT = 0.1         # 1回の移動量 [rad] (ロボットの関節空間)
    _MOVING_VALUE_POS = 0.2         # 1回の移動量 [m] (ロボットの位置空間)
    _STRICT_PLANNING_ROB_JOINT = np.pi / 4  # 探索範囲の制限 [rad] (ロボットの関節空間)
    _STRICT_PLANNING_ROB_POS = 1.0  # 探索範囲の制限 [m] (ロボットの位置空間)

    _TIMEOUT_VALUE = 10         # タイムアウト時間 [second]
    _GOAL_SAMPLE_RATE = 0.1     # ランダムな値を取るときに，終点を選択する確率


    def __init__(self):
        """
        コンストラクタ
        """
        self._name = PATHPLAN.RRT.value
        self._start_tree = Tree(self._NODE_NEAR_NODE_IDX)
        self._start_random_tree = Tree(self._NODE_NEAR_NODE_IDX)
        self._pathes = []
        self._environment = None
        self._debug = False
        self._robot = None
        self._interpolation = INTERPOLATION.POSITION.value
        self._moving_value = self._MOVING_VALUE_JOINT

        # PruningやShortcut関連のプロパティの初期化
        self._before_modify_pathes = []
        self._pruning_pathes = []
        self._shortcut_pathes = []
        self._partial_shortcut_pathes = []


    # プロパティのゲッターメソッド ↓
    @property
    def name(self):
        """
        _nameプロパティのゲッター
        """
        return self._name

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

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

    @property
    def pathes(self):
        """
        _pathesプロパティのゲッター
        """
        return self._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 debug(self):
        """
        _debugプロパティのゲッター
        """
        return self._debug

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

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

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

    @property
    def dim(self):
        """
        _dimプロパティのゲッター
        """
        return self._dim
    # プロパティのゲッターメソッド ↑


    # メイン処理メソッド ↓
    def planning(self, start_pos, end_pos, environment, robot, interpolation, modification=MODIFICATION.PRUNING_START, debug=False):
        """
        経路生成の実装

        パラメータ
            start_pos(list): 経路生成の始点
            end_pos(list): 経路生成の終点
            environment(Base2DEnv): 環境
            robot(Robot): ロボットクラス
            interpolation(int): 補間方法 (関節空間/位置空間)
            modification(str): 経路生成後の修正方法 (Pruning/Shortcutなど)
            debug(bool): デバッグフラグ

        戻り値
            result(bool): True / False = 成功 / 失敗
        """
        # 処理結果
        result = False

        # 経路生成の準備
        self._preparation_planning(start_pos, end_pos, environment, robot, interpolation, modification, debug)

        # 始点ノードを設定
        self._add_node_start_tree(start_pos, start_pos, INITIAL_NODE_NEAR_NODE)
        # 探索する範囲を設定
        self._strict_planning_pos(start_pos, end_pos)

        # 経路生成は一定時間内に終了させる
        start_time = time.time()

        counter = 0

        # 終点までの経路生成が終わるまでループ
        while True:
            # for _ in range(1000):
            counter += 1
            # ランダムな値を取得
            random_pos = self._get_random_pos(end_pos)
            # ランダムな値と最短ノードを計算
            near_node = self._start_tree.get_near_node(random_pos)
            # 最短ノードの位置
            near_node_pos = self._start_tree.nodes[near_node, :self._NODE_NEAR_NODE_IDX]
            # 最短ノードからランダムな値方向へ新しいノード(位置)を作成
            new_pos = self._calc_new_pos(random_pos, near_node_pos)

            # 新規ノードと最短ノード間での干渉判定
            is_interference = self._line_interference(new_pos, near_node_pos)
            if not is_interference:
                # 干渉なしのため，ノードを設定する
                self._add_node_start_tree(new_pos, random_pos, near_node)

                # 終点との距離が一定範囲内であるかの確認
                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)
            # PathPruningの実施
            self._modify_path()
            # ファイルに保存する
            self._save()

        return result

    def _add_node_start_tree(self, pos, random_pos, near_node):
        """
        始点ツリーにノードを追加

        パラメータ
            pos(numpy.ndarray): 位置
            random_pos(numpy.ndarray): ランダムな位置
            near_node(int): 最短ノード
        """
        # _start_treeにノードを追加
        node = np.append(pos, near_node).reshape(1, -1)
        self._start_tree.add_node(node)

        # デバッグ有効の時しか，ランダムなツリーにデータを保存したくない (処理時間の短縮)
        if self._debug:
            # _start_random_treeにノードを追加
            random_node = np.append(random_pos, near_node).reshape(1, -1)
            self._start_random_tree.add_node(random_node)

    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:
            if not self._line_interference(pos, end_pos):
                # 干渉しない
                is_near = True

        return is_near

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

        パラメータ
            start_pos(list): 経路生成の始点
            end_pos(list): 経路生成の終点
        """
        # 始点から終点までの経路に関係するノードを選択
        revers_path = end_pos.reshape(1, -1)
        near_node   = -1

        while True:
            # 終点から始点方向へノードを取得
            node = self._start_tree.nodes[near_node]
            pos  = node[:self._NODE_NEAR_NODE_IDX].reshape(1, -1)
            # 浮動小数型になっているので，整数型に型変換
            near_node = int(node[self._NODE_NEAR_NODE_IDX])
            revers_path = np.append(revers_path, pos, axis=0)
            if near_node == INITIAL_NODE_NEAR_NODE:
                # 始点ノードまで取得できたため，処理終了
                break

        # 経路が終点からの順番になっているため，始点から終点とする
        self._pathes = revers_path[::-1]
        # ツリーに終点を追加 (要素番号を指定するため -1)
        self._add_node_start_tree(end_pos, end_pos, self._start_tree.nodes.shape[0] - 1)

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

        パラメータ
            random_pos(numpy.ndarray): ランダムな位置
            near_node_pos(numpy.ndarray): 最短ノード位置

        戻り値
            new_pos(numpy.ndarray): 新しいノード
        """
        # 方向を計算
        direction = random_pos - near_node_pos
        # 方向の大きさを1にする
        norm_direction = direction / (np.linalg.norm(direction) + EPSILON)
        # 新しいノードを作成
        new_pos = near_node_pos + norm_direction * self._moving_value

        return new_pos

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

        パラメータ
            target_pos(numpy.ndarray): 目標点

        戻り値
            random_pos(numpy.ndarray): ランダムな位置
        """
        # 乱数を取って，目標点を選択するかランダムを選択するか
        select_goal = np.random.rand()
        if select_goal < self._GOAL_SAMPLE_RATE:
            # 目標点を選択s
            random_pos = target_pos
        else:
            random_pos = np.random.uniform(self._strict_min_pos, self._strict_max_pos)

        return random_pos
    # メイン処理メソッド ↑


    # 準備処理関連メソッド ↓
    def _preparation_planning(self, start_pos, end_pos, environment, robot, interpolation, modification, debug):
        """
        経路生成の準備

        パラメータ
            start_pos(list): 経路生成の始点
            end_pos(list): 経路生成の終点
            environment(Base2DEnv): 環境
            robot(Robot): ロボットクラス
            interpolation(int): 補間方法 (関節空間/位置空間)
            modification(str): 経路生成後の修正方法 (Pruning/Shortcutなど)
            debug(bool): デバッグフラグ
        """
        # デバッグフラグの更新
        self._debug = debug
        # 修正方法の更新
        self._modification = modification
        # データの初期化
        self._reset()

        # 始点と終点の次元数が一致しているかの確認
        start_pos_dim = np.size(start_pos)
        end_pos_dim   = np.size(end_pos)
        if start_pos_dim != end_pos_dim:
            # 次元数が異なるので異常
            raise ValueError(f"start_pos_dim and end_pos_dim are not matched. start_pos_dim is {start_pos_dim}, end_pos_dim is {end_pos_dim}")

        self._dim = start_pos_dim

        # 環境を設定
        self._environment = environment
        # ロボットの設定
        self._set_robot(robot, interpolation)

        # 始点と終点の干渉判定
        if self._interference_pos(start_pos):
            # 干渉ありまたは逆運動学の解が求まらない
            raise ValueError(f"start_pos is interference. please change start_pos")

        if self._interference_pos(end_pos):
            # 干渉ありまたは逆運動学の解が求まらない
            raise ValueError(f"end_pos is interference. please change end_pos")

        # 結果を保存するフォルダ作成
        self._make_path_plan_folder(interpolation)

        # フォルダー内のファイルを全部削除
        self._reset_folder(interpolation)

    def _reset(self):
        """
        データの初期化
        """
        self._start_tree.reset()
        self._start_random_tree.reset()

        if len(self._pathes) != 0:
            # 何かしらのデータが保存
            del self._pathes
        self._pathes = []

        if self._environment is not None:
            del self._environment
        self._environment = None

        if self._robot is not None:
            del self._robot
        self._robot = None

        self._interpolation = INTERPOLATION.POSITION.value

    def _set_robot(self, robot, interpolation):
        """
        経路生成したいロボット情報を設定

        パラメータ
            robot(Robot): ロボットクラス
            interpolation(int): 補間の種類 (関節補間/位置補間)
        """
        self._robot = robot
        if interpolation == INTERPOLATION.POSITION.value:
            # 位置空間
            self._moving_value = self._MOVING_VALUE_POS
        elif interpolation == INTERPOLATION.JOINT.value:
            # 関節空間
            self._moving_value = self._MOVING_VALUE_JOINT
        else:
            # 異常値
            raise ValueError(f"interpolation is abnormal. interpolation is {interpolation}")

        # 補間種類の更新
        self._interpolation = interpolation

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

        パラメータ
            start_pos(numpy.ndarray): 始点
            end_pos(numpy.ndarray): 終点
        """
        all_pos = np.array([start_pos, end_pos])
        # 各列の最大/最小値を取得
        min_pos = np.min(all_pos, axis=0)
        max_pos = np.max(all_pos, axis=0)

        if self._interpolation == INTERPOLATION.POSITION:
            # 位置空間の探索
            strict_planning_pos = self._STRICT_PLANNING_ROB_POS
        else:
            # 関節空間の探索
            strict_planning_pos = self._STRICT_PLANNING_ROB_JOINT

        self._strict_min_pos = min_pos - strict_planning_pos
        self._strict_max_pos = max_pos + strict_planning_pos
    # 準備処理関連メソッド ↑


    # 干渉判定の処理メソッド ↓
    def _line_interference(self, pos1, pos2):
        """
        2点間の干渉チェック

        パラメータ
            pos1(numpy.ndarray): 位置1/関節1
            pos2(numpy.ndarray): 位置2/関節2

        戻り値
            is_interference(bool): True / False = 干渉あり / 干渉なし
        """
        is_interference = False

        # 差分を計算
        difference = pos2 - pos1
        # 距離を計算
        distance = np.linalg.norm(difference)

        if self._interpolation == INTERPOLATION.POSITION:
            # 位置空間 (分割時の距離を保存)
            devided_dist = self._DEVIDED_DISTANCE_POS
        else:
            # 関節空間 (分割時の距離を保存)
            devided_dist = self._DEVIDED_DISTANCE_JOINT

        # 2点間の分割数を算出
        n_devided = max(int(distance / devided_dist), 1)
        for i in range(n_devided + 1):
            direct_pos  = i / n_devided * difference
            devided_pos = pos1 + direct_pos
            if self._interference_pos(devided_pos):
                # 干渉あり
                is_interference = True
                break

        return is_interference

    def _interference_pos(self, pos):
        """
        干渉判定

        パラメータ
            pos(numpy.ndarray): 位置/関節

        戻り値
            is_interference(bool): True / False = 干渉あり / 干渉なし
        """
        is_interference = False

        if self._interpolation == INTERPOLATION.POSITION.value:
            # 位置空間の場合は，逆運動学を計算
            try:
                thetas = self._robot.inverse_kinematics(pos)
                # ロボットの位置を更新
                self._robot.update(thetas)
                # 干渉判定
                if self._environment.is_collision_dist(self._robot.manager, margin=self._INTERFERENCE_MARGIN):
                    # 干渉あり
                    is_interference = True

            except Exception as e:
                # 逆運動学の解が得られないから，干渉ありとする
                is_interference = True

        else:
            # 関節空間
            # ロボットの位置を更新
            self._robot.update(pos)
            # 干渉判定
            if self._environment.is_collision_dist(self._robot.manager, margin=self._INTERFERENCE_MARGIN):
                # 干渉あり
                is_interference = True

        return is_interference
    # 干渉判定の処理メソッド ↑


    # ファイル関連メソッド ↓
    def _make_folder(self, folder_name):
        """
        フォルダーの作成

        パラメータ
            folder_name(str): 作成したいフォルダー名
        """
        # フォルダーが作成済みでもエラーを出力しないよう，exist_ok=Trueとした．
        os.makedirs(folder_name, exist_ok=True)

    def _reset_folder(self, interpolation):
        """
        フォルダー内のファイルを全削除

        パラメータ
            interpolation(str): 探索方法 (位置空間/関節空間)
        """
        # フォルダー名を取得
        folder_names = self._get_path_plan_folders(interpolation)
        for folder_name in folder_names:
            for entry in os.listdir(folder_name):
                full_path = os.path.join(folder_name, entry)
                if os.path.isfile(full_path) or os.path.islink(full_path):
                    os.remove(full_path)         # 通常ファイル・シンボリックリンクを削除

    def _get_path_plan_folders(self, interpolation):
        """
        経路生成結果を保存する複数のフォルダー名を取得

        パラメータ
            interpolation(str): 探索方法 (位置空間/関節空間)

        戻り値
            folder_names(list): 複数のフォルダー名
        """
        folder_names = []

        folder_name = self._name
        # サブフォルダー
        sub_folder_names = [self._FOLODER_NAME_PRUNING, self._FOLODER_NAME_SHORTCUT, self._FOLODER_NAME_PARTIAL_SHORTCUT]
        for sub_folder_name in sub_folder_names:
            folder_names.append(os.path.join(folder_name, interpolation, sub_folder_name))

        return folder_names

    def _make_path_plan_folder(self, interpolation):
        """
        経路生成結果を保存するフォルダー作成

        パラメータ
            interpolation(str): 探索方法 (位置空間/関節空間)
        """
        # フォルダー名を取得
        folder_names = self._get_path_plan_folders(interpolation)
        for folder_name in folder_names:
            self._make_folder(folder_name)

    def _save(self):
        """
        生成した経路をファイル保存
        """
        if self._debug:
            # 始点から終点までの修正済みの経路をファイル保存
            self._save_numpy_data_to_txt(self._pathes, self._FILE_NAME_PATHES)
            # 始点のツリーを保存
            self._save_numpy_data_to_txt(self._start_tree.nodes, self._FILE_NAME_START_TREE)
            # 始点のランダムツリーを保存
            self._save_numpy_data_to_txt(self._start_random_tree.nodes, self._FILE_NAME_START_RANDOM_TREE)
            if self._modification is not None:
                # 修正前の始点から終点までの経路を保存
                self._save_numpy_data_to_txt(self._before_modify_pathes, self._FILE_NAME_BEFORE_MODIFY_PATHES)
                # Pruning後の経路を保存
                self._save_numpy_data_to_txt(self._pruning_pathes, self._FILE_NAME_PRUNING_PATHES)
                # Shortcut後の経路を保存
                self._save_numpy_data_to_txt(self._shortcut_pathes, self._FILE_NAME_SHORTCUT_PATHES)
                # Partial Shortcut後の経路を保存
                self._save_numpy_data_to_txt(self._partial_shortcut_pathes, self._FILE_NAME_SHORTCUT_PATHES)

    def _save_numpy_data_to_txt(self, data, file_name):
        """
        Numpyデータをテキストファイルに保存

        パラメータ
            data(numpy.ndarray): ファイル保存したいデータ
            file_name(str): 保存したいファイル名
        """
        # 引数の確認
        if len(data) == 0:
            # データが存在しないため，処理終了
            return

        if not file_name:
            # ファイル名が存在しないため，処理終了
            return

        # ファイル名にフォルダ名を追加 (各経路生成手法で異なるフォルダにデータを保存)
        full_path = f"{self._name}/{self._interpolation}/{file_name}"

        np.savetxt(full_path, data)
    # ファイル関連メソッド ↑


    # 経路生成後の後処理メソッド ↓
    def _modify_path(self):
        """
        始点から終点までの経路を修正

        パラメータ
            modification(str): 経路生成後の修正方法 (Pruning/Shortcutなど)
        """
        if self._modification is None:
            # 経路を修正しない
            return

        # 修正前の経路を保存
        self._before_modify_pathes = np.copy(self._pathes)

        if self._modification == MODIFICATION.PRUNING_START:
            # 始点からPruning
            self._path_pruning_from_start()
        elif self._modification == MODIFICATION.PRUNING_END:
            # 終点からPruning
            self._path_pruning_from_end()
        elif self._modification == MODIFICATION.PRUNING_RANDOM:
            # ランダム点のPruning
            self._path_pruning_random()
        elif self._modification == MODIFICATION.SHORTCUT_NORMAL:
            # Shortcut
            self._path_shortcut()
        elif self._modification == MODIFICATION.SHORTCUT_PARTIAL:
            # Partial Shortcut
            self._path_partial_shortcut()
        elif self._modification == MODIFICATION.PARTIAL_START:
            # Partial Shortcut + 始点からPruning
            self._path_partial_shortcut()
            self._path_pruning_from_start()
        else:
            # 異常
            raise ValueError(f"modification is abnormal. modification is {self._modification}")

    def _get_shortcut_two_nodes(self):
        """
        Shortcutで使用する2つのノードを取得

        戻り値
            two_nodes(list): 2つのノード
        """
        two_nodes = []

        # 削除したいノードを2つ選択する
        node1 = np.random.randint(0, self._pathes.shape[0] - 1)
        node2 = np.random.randint(0, self._pathes.shape[0] - 1)
        # 同じノードであれば，ノード選択に戻る
        if node1 == node2:
            return two_nodes

        # node1に小さいノード番号を保存
        if node1 > node2:
            node1, node2 = node2, node1

        # 隣接するノードであれば，ノード選択に戻る (隣接していれば，Shortcutする理由がない)
        if node1 + 1 == node2:
            return two_nodes

        # 2つのノードを保存
        two_nodes = [node1, node2]

        return two_nodes

    def _path_partial_shortcut(self):
        """
        Partial Shortcut (経路上の2点をランダムで選択して, 2点内の全ノード内の1パラメータを直線補間して, 干渉なければ2点内のノード内の1パラメータを修正)
            self._pathesプロパティにPruning後の経路が保存される
        """
        # 要素番号を保存
        n_success = 0
        continueous_fail = 0        # 連続失敗回数

        # 位置を構成する要素数を取得
        n_element = self._pathes.shape[1]

        for _ in range(500):
            # ループ回数
            # if continueous_fail > 20:
            #     # 連続で失敗したため，これ以上はPartial Shortcutできないと判断
            #     break

            # 位置の中から，1要素をランダムに取得する (全要素を選択する確率は同じ)
            element = np.random.randint(0, n_element - 1)

            # 削除したいノードを2つ選択する
            two_nodes = self._get_shortcut_two_nodes()
            if len(two_nodes) != 2:
                # ノード選択に失敗したため，ループ処理の先頭に戻る
                continueous_fail += 1
                continue

            # 2点間のデータを全部取得
            partial_pathes = self._pathes[two_nodes[0]: two_nodes[1] + 1].copy()
            # 2点で変更したい要素データの差分を計算
            diff_data = partial_pathes[-1, element] - partial_pathes[0, element]
            for i in range(1, partial_pathes.shape[0] - 1):
                partial_pathes[i + 1, element] = partial_pathes[0, element] + diff_data * i / partial_pathes.shape[0]

            # 変更した経路間での干渉判定
            is_collision = False
            for i in range(0, partial_pathes.shape[0] - 1):
                if self._line_interference(partial_pathes[i], partial_pathes[i + 1]):
                    # 干渉ありのため，連続失敗回数の更新
                    is_collision = True
                    break

            if is_collision:
                # 干渉あり
                continueous_fail += 1
            else:
                # 干渉なし
                self._pathes[two_nodes[0]: two_nodes[1] + 1] = partial_pathes
                continueous_fail = 0
                # Shortcutの結果を随時ファイル保存
                n_success += 1
                if self._debug:
                    # ファイル名の作成
                    file_name = f"{self._FOLODER_NAME_PARTIAL_SHORTCUT}/{self._FILE_NAME_PARTIAL_SHORTCUT_EACH_PATHES}_{n_success}.csv"
                    self._save_numpy_data_to_txt(self._pathes, file_name)

        # Shortcut後の経路を保存
        self._partial_shortcut_pathes = np.copy(self._pathes)

    def _path_shortcut(self):
        """
        Shortcut (経路上に2点を作成して, 干渉なければ2点内のノードを全削除)
            self._pathesプロパティにPruning後の経路が保存される
        """
        # 要素番号を保存
        n_success = 0
        continueous_fail = 0        # 連続失敗回数

        for _ in range(100):
            # ループ回数
            if continueous_fail > 10:
                # 連続で失敗したため，これ以上はShortcutできないと判断
                break

            # 削除したいノードを2つ選択する
            two_nodes = self._get_shortcut_two_nodes()
            if len(two_nodes) != 2:
                # ノード選択に失敗したため，ループ処理の先頭に戻る
                continueous_fail += 1
                continue

            if not self._line_interference(self._pathes[two_nodes[0]], self._pathes[two_nodes[1]]):
                # 干渉なしのため，(idx+1)要素を削除 (行列の行を削除するため，axis=0を引数に与える)
                self._pathes = np.delete(self._pathes, range(two_nodes[0] + 1, two_nodes[1] - 1), axis=0)
                # 連続失敗回数を初期化
                continueous_fail = 0
                # Shortcutの結果を随時ファイル保存
                n_success += 1
                if self._debug:
                    # ファイル名の作成
                    file_name = f"{self._FOLODER_NAME_SHORTCUT}/{self._FILE_NAME_SHORTCUT_EACH_PATHES}_{n_success}.csv"
                    self._save_numpy_data_to_txt(self._pathes, file_name)
            else:
                # 干渉ありのため，連続失敗回数の更新
                continueous_fail += 1

        # Shortcut後の経路を保存
        self._shortcut_pathes = np.copy(self._pathes)

    def _path_pruning_random(self):
        """
        Pruning (経路の経由点をランダム点の削除)
            self._pathesプロパティにPruning後の経路が保存される
        """
        # 要素番号を保存
        idx = -1
        n_success = 0
        continueous_fail = 0        # 連続失敗回数

        for i in range(100):
            # ループ回数
            # 削除したいノード番号をランダムで選択 (始点と終点を含まないように-2)
            idx = np.random.randint(0, self._pathes.shape[0] - 2)
            if not self._line_interference(self._pathes[idx], self._pathes[idx - 2]):
                # 干渉なしのため，(idx+1)要素を削除 (行列の行を削除するため，axis=0を引数に与える)
                self._pathes = np.delete(self._pathes, idx - 1, axis=0)
                # 連続失敗回数を初期化
                continueous_fail = 0
                # Pruningの結果を随時ファイル保存
                n_success += 1
                if self._debug:
                    # ファイル名の作成
                    file_name = f"{self._FOLODER_NAME_PRUNING}/{self._FILE_NAME_PRUING_EACH_PATHES}_{n_success}.csv"
                    self._save_numpy_data_to_txt(self._pathes, file_name)
            else:
                # 干渉ありのため，連続失敗回数の更新
                continueous_fail += 1

            if continueous_fail > 5:
                # 連続で失敗したら，削除できるノードは存在しないと考えられる
                break

        # Pruning後の経路を保存
        self._pruning_pathes = np.copy(self._pathes)

    def _path_pruning_from_end(self):
        """
        Pruning (経路の経由点を終点方向から順番に削除)
            self._pathesプロパティにPruning後の経路が保存される
        """
        # 要素番号を保存
        idx = -1
        count = 0

        # 経由点数分ループする (始点と終点を含まないために，-2をする)
        while abs(idx) < self._pathes.shape[0] - 2:
            # (idx+1)要素前後のノード間で干渉がないかの確認
            if not self._line_interference(self._pathes[idx], self._pathes[idx - 2]):
                # 干渉なしのため，(idx+1)要素を削除 (行列の行を削除するため，axis=0を引数に与える)
                self._pathes = np.delete(self._pathes, idx - 1, axis=0)
            else:
                # 干渉ありのため，次要素を選択
                idx -= 1

            # Pruningの結果を随時ファイル保存
            count += 1
            if self._debug:
                # ファイル名の作成
                file_name = f"{self._FOLODER_NAME_PRUNING}/{self._FILE_NAME_PRUING_EACH_PATHES}_{count}.csv"
                self._save_numpy_data_to_txt(self._pathes, file_name)

        # Pruning後の経路を保存
        self._pruning_pathes = np.copy(self._pathes)

    def _path_pruning_from_start(self):
        """
        Pruning (経路の経由点を始点方向から順番に削除)
            self._pathesプロパティにPruning後の経路が保存される
        """
        # 要素番号を保存
        idx   = 0
        count = 0

        # 経由点数分ループする (始点と終点を含まないために，-2をする)
        while idx < self._pathes.shape[0] - 2:
            # (idx+1)要素前後のノード間で干渉がないかの確認
            if not self._line_interference(self._pathes[idx], self._pathes[idx + 2]):
                # 干渉なしのため，(idx+1)要素を削除 (行列の行を削除するため，axis=0を引数に与える)
                self._pathes = np.delete(self._pathes, idx + 1, axis=0)
            else:
                # 干渉ありのため，次要素を選択
                idx += 1

            # Pruningの結果を随時ファイル保存
            count += 1
            if self._debug:
                # ファイル名の作成
                file_name = f"{self._FOLODER_NAME_PRUNING}/{self._FILE_NAME_PRUING_EACH_PATHES}_{count}.csv"
                self._save_numpy_data_to_txt(self._pathes, file_name)

        # Pruning後の経路を保存
        self._pruning_pathes = np.copy(self._pathes)
    # 経路生成後の後処理メソッド ↑


class RRTConnectRobot(RRTRobot):
    """
    RRT-Connectクラス

    プロパティ
        _name(str): 経路生成手法名
        _end_tree(numpy.ndarray): 終点ツリー
        _end_random_tree(numpy.ndarray): 終点ランダムツリー
        _state(int): 現在のツリー状態
        _states(list): 全時刻のツリー状態

    メソッド
        public

            プロパティのゲッター関連
                states(): _statesプロパティのゲッター
                end_tree(): _end_treeプロパティのゲッター
                end_random_tree(): _end_random_treeプロパティのゲッター

            メイン処理関連
                planning(): 経路生成の実装


        protected

            メイン処理関連
                _add_node_end_tree(): 終点ツリーにノードを設定する
                _fin_planning(): 経路生成の終了処理
                _state_transition(): 状態を遷移させる
                _exec_state(): 状態に応じた処理を実施
                _save_state(): 状態を保存

            準備処理関連
                _reset(): データの初期化

            ファイル関連
                _save(): 生成した経路をファイル保存
    """
    # 定数の定義
    # ファイル名の定義
    # 各経路生成手法で絶対に定義するべきファイル名 ↓
    # _pathesプロパティを保存するファイル名
    _FILE_NAME_PATHES = f"{PATHPLAN.RRT_CONNECT.value}_pathes.csv"
    # _start_treeプロパティを保存するファイル名
    _FILE_NAME_START_TREE = f"{PATHPLAN.RRT_CONNECT.value}_start_tree.csv"
    # _start_random_treeプロパティを保存するファイル名
    _FILE_NAME_START_RANDOM_TREE = f"{PATHPLAN.RRT_CONNECT.value}_start_random_tree.csv"
    # _before_modify_pathesプロパティを保存するファイル名
    _FILE_NAME_BEFORE_MODIFY_PATHES = f"{PATHPLAN.RRT_CONNECT.value}_before_modify_pathes.csv"
    # _pruning_pathesプロパティを保存するファイル名
    _FILE_NAME_PRUNING_PATHES = f"{PATHPLAN.RRT_CONNECT.value}_pruning_pathes.csv"
    # _shortcut_pathesプロパティを保存するファイル名
    _FILE_NAME_SHORTCUT_PATHES = f"{PATHPLAN.RRT_CONNECT.value}_shortcut_pathes.csv"
    # _partial_shortcut_pathesプロパティを保存するファイル名
    _FILE_NAME_PARTIAL_SHORTCUT_PATHES = f"{PATHPLAN.RRT_CONNECT.value}_partial_shortcut_pathes.csv"

    # 毎処理保存するファイル名の定義
    # Pruning実施毎の経路を保存
    _FILE_NAME_PRUING_EACH_PATHES = f"{PATHPLAN.RRT_CONNECT.value}_pruning_each_pathes"
    # Shortcut実施毎の経路を保存
    _FILE_NAME_SHORTCUT_EACH_PATHES = f"{PATHPLAN.RRT_CONNECT.value}_shortcut_each_pathes"
    # Partial Shortcut実施毎の経路を保存
    _FILE_NAME_PARTIAL_SHORTCUT_EACH_PATHES = f"{PATHPLAN.RRT_CONNECT.value}_partial_shortcut_each_pathes"
    # 各経路生成手法で絶対に定義するべきファイル名 ↑

    # 終点ツリー用のファイル名
    # _end_treeプロパティを保存するファイル名
    _FILE_NAME_END_TREE = f"{PATHPLAN.RRT_CONNECT.value}_end_tree.csv"
    # _end_random_treeプロパティを保存するファイル名
    _FILE_NAME_END_RANDOM_TREE = f"{PATHPLAN.RRT_CONNECT.value}_end_random_tree.csv"
    # _statesプロパティを保存するファイル名
    _FILE_NAME_STATES = f"{PATHPLAN.RRT_CONNECT.value}_states.csv"

    # ツリーの要素番号を定義
    _NODE_NEAR_NODE_IDX = RRT_CONNECT_NEAR_NODE_IDX     # ノード内の最短ノード要素


    def __init__(self):
        """
        コンストラクタ
        """
        # 親クラスの初期化
        super().__init__()
        # プロパティの初期化 (始点ツリーは親クラスで初期化されている)
        self._name = PATHPLAN.RRT_CONNECT.value
        self._end_tree = Tree(self._NODE_NEAR_NODE_IDX)
        self._end_random_tree = Tree(self._NODE_NEAR_NODE_IDX)
        self._state = STATE_START_TREE_RANDOM  # 現在の状態
        self._states = []                       # 全時刻の状態を保存


    # プロパティのゲッターメソッド ↓
    @property
    def states(self):
        """
        _statesプロパティのゲッター
        """
        return self._states

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

    @property
    def end_random_tree(self):
        """
        _end_random_treeプロパティのゲッター
        """
        return self._end_random_tree
    # プロパティのゲッターメソッド ↑


    # メイン処理メソッド ↓
    def planning(self, start_pos, end_pos, environment, robot, interpolation, modification=MODIFICATION.PRUNING_START, debug=False):
        """
        経路生成の実装

        パラメータ
            start_pos(list): 経路生成の始点
            end_pos(list): 経路生成の終点
            environment(Base2DEnv): 環境
            robot(Robot): ロボットクラス
            interpolation(int): 補間方法 (関節空間/位置空間)
            modification(str): 経路生成後の修正方法 (Pruning/Shortcutなど)
            debug(bool): デバッグフラグ

        戻り値
            result(bool): True / False = 成功 / 失敗
        """
        # 処理結果
        result = False

        # 経路生成の準備
        self._preparation_planning(start_pos, end_pos, environment, robot, interpolation, modification, debug)

        # 始点ツリーに始点ノードを設定
        self._add_node_start_tree(start_pos, start_pos, INITIAL_NODE_NEAR_NODE)
        # 終点ツリーに終点ノードを設定
        self._add_node_end_tree(end_pos, end_pos, INITIAL_NODE_NEAR_NODE)
        # 探索する範囲を設定
        self._strict_planning_pos(start_pos, end_pos)

        # 経路生成は一定時間内に終了させる
        start_time = time.time()

        counter = 0

        # 終点までの経路生成が終わるまでループ
        while True:
            counter += 1
            # 1処理実施して，経路生成が完了したか確認する
            if self._exec_state(start_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)
            # PathPruningの実施
            self._modify_path()
            # ファイルに保存する
            self._save()

        return result

    def _add_node_end_tree(self, pos, random_pos, near_node):
        """
        終点ツリーにノードを設定する

        パラメータ
            pos(numpy.ndarray): 位置
            random_pos(numpy.ndarray): ランダムな位置
            near_node(int): 最短ノード
        """
        # _end_treeにノードを追加
        node = np.append(pos, near_node).reshape(1, -1)
        self._end_tree.add_node(node)

        # デバッグ有効の時しか，ランダムなツリーにデータを保存したくない (処理時間の短縮)
        if self._debug:
            # _end_random_treeにノードを追加
            random_node = np.append(random_pos, near_node).reshape(1, -1)
            self._end_random_tree.add_node(random_node)

    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._states.append(self._state)

    def _exec_state(self, start_pos, end_pos):
        """
        状態(_state)に応じた処理を実施

        パラメータ
            start_pos(numpy.ndarray): 始点
            end_pos(numpy.ndarray): 終点

        戻り値
            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
            target_pos = end_pos
            set_tree_func = self._add_node_start_tree
        else:
            my_tree = self._end_tree
            your_tree = self._start_tree
            target_pos = start_pos
            set_tree_func = self._add_node_end_tree

        # ツリーにランダム点を追加
        if self._state == STATE_START_TREE_RANDOM or self._state == STATE_END_TREE_RANDOM:
            # 始点ツリーにランダム点を追加する状態または終点ツリーにランダム点を追加する状態
            # ランダムな値を取得する
            random_pos = self._get_random_pos(target_pos)
            # ランダムな値とツリーの最短ノードを計算
            near_node = my_tree.get_near_node(random_pos)
            # 位置と最短ノードが保存されているから，位置だけを取得
            near_node_pos = my_tree.nodes[near_node, :self._NODE_NEAR_NODE_IDX]
            # 最短ノードからランダムな値方向へ新しいノードを作成
            new_pos = self._calc_new_pos(random_pos, near_node_pos)

            # 干渉物との干渉判定
            is_interference = self._line_interference(new_pos, near_node_pos)
            if not is_interference:  # 干渉なし
                # ツリーにノードを追加
                set_tree_func(new_pos, random_pos, near_node)
                # 状態を保存
                self._save_state()
                # 状態を遷移させる
                self._state_transition()
        else:
            # ツリーで作成したノードを取得
            end_pos = your_tree.nodes[-1, :self._NODE_NEAR_NODE_IDX]
            # ツリー内の最短ノードを計算
            near_node = my_tree.get_near_node(end_pos)
            # 位置と最短ノードが保存されているから，位置だけを取得
            near_node_pos = my_tree.nodes[near_node, :self._NODE_NEAR_NODE_IDX]
            # 最短ノードから終点ツリーのノード方向へ新しいノードを作成
            new_pos = self._calc_new_pos(end_pos, near_node_pos)

            # 干渉物との干渉判定
            is_interference = self._line_interference(new_pos, near_node_pos)
            if is_interference:  # 干渉あり
                # 状態を遷移させる
                self._state_transition()
            else:   # 干渉なし
                # ツリーにノードを追加
                set_tree_func(new_pos, end_pos, near_node)
                # 状態を保存
                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.nodes[near_node]
            pos = node[:self._NODE_NEAR_NODE_IDX].reshape(1, -1)
            # 浮動小数型になっているので，整数型に型変換
            near_node = int(node[self._NODE_NEAR_NODE_IDX])
            if len(start_path) == 0:
                # 初回の場合
                start_path = pos
            else:
                start_path = np.append(start_path, pos, axis=0)

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

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

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

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

        # 始点から終点までの経路を保存
        self._pathes = np.append(self._pathes, end_path, axis=0)
    # メイン処理メソッド ↑


    # 準備処理メソッド ↓
    def _reset(self):
        """
        データの初期化
        """
        # 親クラスの初期化処理
        super()._reset()

        # 終点側のツリーを初期化
        self._end_tree.reset()
        self._end_random_tree.reset()

        # 状態の初期化
        self._state = STATE_START_TREE_RANDOM
        if len(self._states) != 0:
            # 何かしらのデータが保存
            del self._states
        self._states = []
    # 準備処理メソッド ↑


    # ファイル関連メソッド ↓
    def _save(self):
        """
        生成した経路をファイル保存
        """
        if self._debug:
            # 親クラスのファイル保存を実施
            super()._save()
            # 終点ツリーを保存
            self._save_numpy_data_to_txt(self._end_tree.nodes, self._FILE_NAME_END_TREE)
            # 終点のランダムツリーを保存
            self._save_numpy_data_to_txt(self._end_random_tree.nodes, self._FILE_NAME_END_RANDOM_TREE)
            # 状態の保存
            self._save_numpy_data_to_txt(np.array(self._states), self._FILE_NAME_STATES)
    # ファイル関連メソッド ↑


class CostedTree(Tree):
    """
    コスト付きツリークラス

    プロパティ
        _nodes(numpy.ndarray): ノード
        _near_node_idx(int): 最短ノード列番号 (_nodesの中に最短ノードも保存している)
        _cost_idx(int): コスト列番号 (_nodesの中にコストも保存している)
        _radius_idx(int): 探索半径の列番号 (_nodesの中に探索半径も保存している)

    メソッド
        public
            calc_cost(): コストの計算
            chg_node_info(): ノード情報を変更
    """
    # 定数の定義


    def __init__(self, near_node_idx, cost_idx, radius_idx):
        """
        コンストラクタ
        """
        # プロパティの初期化
        self._nodes = []
        self._near_node_idx = near_node_idx
        self._cost_idx = cost_idx
        self._radius_idx = radius_idx

    def calc_cost(self, node_idx, pos):
        """
        コストの計算

        パラメータ
            node_idx(int): ノード番号
            pos(numpy.ndarray): 位置

        戻り値
            cost(float): コスト
        """
        # ノードの存在確認
        self._chk_node_exist()

        # ノードから位置・コストを取得
        node = self._nodes[node_idx]
        node_pos  = node[:self._near_node_idx]
        node_cost = node[self._cost_idx]

        # 距離を計算
        distance = np.linalg.norm(node_pos - pos)
        # コストを計算
        cost = node_cost + distance

        return cost

    def chg_node_info(self, node_idx, near_node, cost):
        """
        ノード情報を変更

        パラメータ
            node_idx(int): 変更したいノード番号
            near_node(int): 変更後の最短ノード
            cost(float): 変更後のコスト
        """
        self._nodes[node_idx, self._near_node_idx] = near_node
        self._nodes[node_idx, self._cost_idx] = cost


class RRTStarRobot(RRTRobot):
    """
    RRT*クラス

    プロパティ
        _name(str): 経路生成手法名
        _max_iterate(int): 最大探索回数
        _count_success(int): ノード追加の成功回数
        _near_node_radius(float): 近傍ノードの探索半径

    メソッド
        public

            プロパティの ゲッター/セッター 関連
                max_iterate(): _max_iterateプロパティの ゲッター/セッター
                count_success(): _count_successプロパティのゲッター
                file_name_current_tree(): _FILE_NAME_CURRENT_TREE定数のゲッター
                file_name_random_tree(): _FILE_NAME_RANDOM_TREE定数のゲッター
                folder_name_current_tree(): _FOLDER_NAME_CURRENT_TREE定数のゲッター

            メイン処理関連
                planning(): 経路生成の実装


        protected

            メイン処理関連
                _add_node_start_tree(): 始点ツリーにノードを追加
                _calc_cost(): ノードのコストを計算
                _get_near_node_list(): ノードと近傍ノードをリストで取得
                _get_min_cost_node(): コストが最小となる近傍ノードを取得
                _update_near_node(): 近傍ノード内の親ノードを更新
                _fin_planning(): 経路生成の終了処理
                _get_unit_ball(): 単位球内の位置を取得

            ファイル関連
                _get_path_plan_folders(): 経路生成結果を保存する複数のフォルダー名を取得
                _save_current_tree(): 現在のツリーをファイル化
    """
    # 定数の定義
    # ファイル名の定義
    # 各経路生成手法で絶対に定義するべきファイル名 ↓
    # _pathesプロパティを保存するファイル名
    _FILE_NAME_PATHES = f"{PATHPLAN.RRT_STAR.value}_pathes.csv"
    # _start_treeプロパティを保存するファイル名
    _FILE_NAME_START_TREE = f"{PATHPLAN.RRT_STAR.value}_start_tree.csv"
    # _start_random_treeプロパティを保存するファイル名
    _FILE_NAME_START_RANDOM_TREE = f"{PATHPLAN.RRT_STAR.value}_start_random_tree.csv"
    # _before_modify_pathesプロパティを保存するファイル名
    _FILE_NAME_BEFORE_MODIFY_PATHES = f"{PATHPLAN.RRT_STAR.value}_before_modify_pathes.csv"
    # _pruning_pathesプロパティを保存するファイル名
    _FILE_NAME_PRUNING_PATHES = f"{PATHPLAN.RRT_STAR.value}_pruning_pathes.csv"
    # _shortcut_pathesプロパティを保存するファイル名
    _FILE_NAME_SHORTCUT_PATHES = f"{PATHPLAN.RRT_STAR.value}_shortcut_pathes.csv"
    # _partial_shortcut_pathesプロパティを保存するファイル名
    _FILE_NAME_PARTIAL_SHORTCUT_PATHES = f"{PATHPLAN.RRT_STAR.value}_partial_shortcut_pathes.csv"

    # 毎処理保存するファイル名の定義
    # Pruning実施毎の経路を保存
    _FILE_NAME_PRUING_EACH_PATHES = f"{PATHPLAN.RRT_STAR.value}_pruning_each_pathes"
    # Shortcut実施毎の経路を保存
    _FILE_NAME_SHORTCUT_EACH_PATHES = f"{PATHPLAN.RRT_STAR.value}_shortcut_each_pathes"
    # Partial Shortcut実施毎の経路を保存
    _FILE_NAME_PARTIAL_SHORTCUT_EACH_PATHES = f"{PATHPLAN.RRT_STAR.value}_partial_shortcut_each_pathes"
    # 各経路生成手法で絶対に定義するべきファイル名 ↑

    # フォルダー名の定義
    _FOLDER_NAME_CURRENT_TREE = "result"                    # ノード追加時の現在のツリーを保存するフォルダ名

    # 毎処理保存するファイル名の定義
    # ノード追加時の現在のツリーを保存するファイル名
    _FILE_NAME_CURRENT_TREE = f"{PATHPLAN.RRT_STAR.value}_current_tree"
    # ノード追加時の現在のランダムツリーを保存するファイル名
    _FILE_NAME_RANDOM_TREE = f"{PATHPLAN.RRT_STAR.value}_random_tree"

    # ツリーの要素番号を定義
    _NODE_NEAR_NODE_IDX = RRT_STAR_NEAR_NODE_IDX        # ノード内の最短ノード要素
    _NODE_COST_IDX = RRT_STAR_COST_IDX             # ノード内のコスト要素
    _NODE_RADIUS_IDX = RRT_STAR_RADIUS_IDX           # ノード内の半径要素

    # その他
    _MAX_ITERATE = 300               # 最大探索回数
    _TIMEOUT_VALUE = 1000              # タイムアウト時間 [second]
    _NEAR_NODE_RADIUS_COEF = 20         # 近傍ノードとする探索球の半径の係数

    def __init__(self):
        """
        コンストラクタ
        """
        self._name = PATHPLAN.RRT_STAR.value
        self._start_tree = CostedTree(self._NODE_NEAR_NODE_IDX, self._NODE_COST_IDX, self._NODE_RADIUS_IDX)
        self._start_random_tree = CostedTree(self._NODE_NEAR_NODE_IDX, self._NODE_COST_IDX, self._NODE_RADIUS_IDX)
        self._pathes = []
        self._debug = False
        self._max_iterate = self._MAX_ITERATE
        self._environment = None
        self._robot = None
        self._near_node_radius = 0


    # プロパティの ゲッター/セッター メソッド ↓
    @property
    def max_iterate(self):
        """
        _max_iterateプロパティのゲッター
        """
        return self._max_iterate

    @max_iterate.setter
    def max_iterate(self, value):
        """
        _max_iterateプロパティのセッター

        パラメータ
            value(int): 設定したい値
        """
        # 0以下では何もしない
        if value > 0:
            self._max_iterate = value

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

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

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

    @property
    def folder_name_current_tree(self):
        """
        _FOLDER_NAME_CURRENT_TREE定数のゲッター
        """
        return self._FOLDER_NAME_CURRENT_TREE
    # プロパティの ゲッター/セッター メソッド ↑


    # メイン処理メソッド ↓
    def planning(self, start_pos, end_pos, environment, robot, interpolation, modification=MODIFICATION.PRUNING_START, debug=False):
        """
        経路生成の実装

        パラメータ
            start_pos(list): 経路生成の始点
            end_pos(list): 経路生成の終点
            environment(Base2DEnv): 環境
            robot(Robot): ロボットクラス
            interpolation(int): 補間方法 (関節空間/位置空間)
            modification(str): 経路生成後の修正方法 (Pruning/Shortcutなど)
            debug(bool): デバッグフラグ

        戻り値
            result(bool): True / False = 成功 / 失敗
        """
        # 処理結果
        result = False

        # 経路生成の準備
        self._preparation_planning(start_pos, end_pos, environment, robot, interpolation, modification, debug)

        # 始点ノードを設定
        self._add_node_start_tree(start_pos, start_pos, INITIAL_NODE_NEAR_NODE, 0.0, 0.0)
        # 探索する範囲を設定
        self._strict_planning_pos(start_pos, end_pos)
        # 近傍ノードを定義する半径
        self._near_node_radius = self._moving_value * self._NEAR_NODE_RADIUS_COEF

        # 経路生成は一定時間内に終了させる
        start_time = time.time()

        self._count_success = 0       # ノード追加に成功した回数

        # 全探索回数ループ
        for _ in range(self._max_iterate):
            # ランダムな値を取得
            random_pos = self._get_random_pos(end_pos)
            # ランダムな値と最短ノードを計算
            near_node  = self._start_tree.get_near_node(random_pos)
            # 最短ノード位置を取得
            near_node_pos = self._start_tree.nodes[near_node, :self._NODE_NEAR_NODE_IDX]
            # 最短ノードからランダムな値方向へ新しいノード(位置)を作成
            new_pos = self._calc_new_pos(random_pos, near_node_pos)

            # 新規ノードと最短ノード間での干渉判定
            is_interference = self._line_interference(new_pos, near_node_pos)
            if not is_interference:
                # 干渉なしのため，データを設定する

                # 新規箇所 ↓
                # 新規ノードのコストを計算
                new_cost = self._start_tree.calc_cost(near_node, new_pos)
                # 近傍ノードを全部取得
                near_node_list, threshold = self._get_near_node_list(new_pos)
                # 近傍ノードからコストが最小となるノードを取得
                min_cost_node, min_cost = self._get_min_cost_node(new_pos, near_node_list, near_node, new_cost)
                # 近傍ノード内でコストが小さくなれば，最短ノードを更新
                self._update_near_node(new_pos, near_node_list, min_cost)
                # 新規箇所 ↑

                # ノードを設定
                self._add_node_start_tree(new_pos, random_pos, min_cost_node, min_cost, threshold)
                # 終点との距離が一定範囲内であるかの確認
                if self._chk_end_pos_dist(new_pos, end_pos):
                    # 一定範囲内のため，戻り値を成功に更新
                    result = True

                self._count_success += 1
                # 現在のツリーをファイル化する
                self._save_current_tree()

            # 干渉の有無に関わらずにタイムアウトの確認はする
            now_time = time.time()
            if now_time - start_time >= self._TIMEOUT_VALUE:
                # タイムアウト
                break

        if result:
            # 経路生成に成功のため，経路生成の終了処理
            self._fin_planning(start_pos, end_pos)
            # PathPruningの実施
            self._modify_path()
            # ファイル保存
            self._save()

        return result

    def _add_node_start_tree(self, pos, random_pos, near_node, cost, radius):
        """
        始点ツリーにノードを追加

        パラメータ
            pos(numpy.ndarray): 位置
            random_pos(numpy.ndarray): ランダムな位置
            near_node(int): 最短ノード
        """
        array_data = np.array([near_node, cost, radius])
        # _start_treeにノードを追加
        node = np.append(pos, array_data).reshape(1, -1)
        self._start_tree.add_node(node)

        # デバッグフラグが有効の時しか，ランダムなツリーにデータを保存したくない
        if self._debug:
            # _start_random_treeにノードを追加
            random_node = np.append(random_pos, array_data).reshape(1, -1)
            self._start_random_tree.add_node(random_node)

    def _calc_cost(self, pos, near_node):
        """
        ノードのコストを計算

        パラメータ
            pos(numpy.ndarray): 新規ノード位置
            near_node(int): 新規ノードとの最短ノード

        戻り値
            cost(float): コスト
        """
        # 最短ノードの情報を保存
        node = self._start_tree.nodes[near_node]
        node_pos  = node[:self._NODE_NEAR_NODE_IDX]
        node_cost = node[self._NODE_COST_IDX]

        # 最短ノードと新規ノードのコスト(距離)を算出
        cost  = np.linalg.norm(node_pos - pos)
        cost += node_cost

        return cost

    def _get_near_node_list(self, pos):
        """
        ノードと近傍ノードをリストで取得

        パラメータ
            pos(numpy.ndarray): ノード位置
            fin_flg(bool): 経路生成の終了フラグ

        戻り値
            near_node_list(list): 近傍ノードリスト
            threshold(float): 近傍判定の閾値
        """
        # 閾値はノード数に依存する
        n_node = self._start_tree.nodes.shape[0]
        threshold = self._near_node_radius * np.power(np.log(n_node) / n_node, 1 / self._dim)

        # 閾値以内の距離であるノードを全部取得
        near_node_list = self._start_tree.get_near_node_list(pos, threshold)

        return near_node_list, threshold

    def _get_min_cost_node(self, pos, near_node_list, near_node, near_node_cost):
        """
        コストが最小となる近傍ノードを取得

        パラメータ
            pos(numpy.ndarray): ノード位置
            near_node_list(list): 近傍ノードリスト
            near_node(int): 最短ノード
            near_node_cost(float): 最短ノード追加時のコスト

        戻り値
            min_cost_node(int): コスト最小ノード
            min_cost(float): コスト最小値
        """
        # 戻り値の初期化
        min_cost = near_node_cost
        min_cost_node = near_node

        if not near_node_list:
            # 近傍ノードがないため，処理終了
            return min_cost_node, min_cost

        for node_idx in near_node_list:
            # 近傍ノードに関する情報を保存
            node = self._start_tree.nodes[node_idx]
            node_pos  = node[:self._NODE_NEAR_NODE_IDX]
            node_cost = node[self._NODE_COST_IDX]

            # コスト(距離)を計算
            distance = np.linalg.norm(node_pos - pos)

            # 2点間の干渉チェック
            is_interference = self._line_interference(node_pos, pos)
            if not is_interference:
                # 干渉なし
                new_cost = distance + node_cost
                if new_cost < min_cost:
                    # 小さくなるため，最短ノードを修正
                    min_cost_node = node_idx
                    min_cost = new_cost

        return min_cost_node, min_cost

    def _update_near_node(self, pos, near_node_list, cost):
        """
        近傍ノード内の親ノードを更新

        パラメータ
            pos(numpy.ndarray): ノード位置
            near_node_list(list): 近傍ノードリスト
            cost(float): コスト
        """
        if not near_node_list:
            # 近傍ノードがないため，処理終了
            return

        # 新規ノードのインデックス番号を保存
        new_node_idx = self._start_tree.nodes.shape[0]

        for node_idx in near_node_list:
            # 近傍ノードに関する情報を保存
            node = self._start_tree.nodes[node_idx]
            node_pos  = node[:self._NODE_NEAR_NODE_IDX]
            node_cost = node[ self._NODE_COST_IDX]

            # コスト(距離)を計算
            distance = np.linalg.norm(node_pos - pos)

            # 2点間の干渉チェック
            is_interference = self._line_interference(node_pos, pos)
            if not is_interference:
                # 干渉なし
                new_cost = distance + cost
                if new_cost < node_cost:
                    # 小さくなるため，最短ノードを修正
                    self._start_tree.chg_node_info(
                        node_idx, new_node_idx, new_cost)

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

        パラメータ
            start_pos(list): 経路生成の始点
            end_pos(list): 経路生成の終点
        """
        # 始点から終点までの経路に関係するノードを選択
        revers_path = end_pos.reshape(1, -1)
        # 終点との最短ノードを取得
        near_node = self._start_tree.get_near_node(end_pos)
        # 最短ノードとのコストを計算
        near_node_cost = self._calc_cost(end_pos, near_node)
        # 近傍ノードを全部取得
        near_node_list, threshold = self._get_near_node_list(end_pos)
        # 近傍ノードからコストが最小となるノードを取得
        min_cost_node, min_cost = self._get_min_cost_node(end_pos, near_node_list, near_node, near_node_cost)
        near_node = min_cost_node

        while True:
            # 終点から始点方向へノードを取得
            node = self._start_tree.nodes[near_node]
            pos  = node[:self._NODE_NEAR_NODE_IDX].reshape(1, -1)
            # 浮動小数型になっているので，整数型に型変換
            near_node = int(node[self._NODE_NEAR_NODE_IDX])
            revers_path = np.append(revers_path, pos, axis=0)
            if near_node == INITIAL_NODE_NEAR_NODE:
                # 始点ノードまで取得できたため，処理終了
                break

        # 経路が終点からの順番になっているため，始点から終点とする
        self._pathes = revers_path[::-1]
        # デバッグ用の経路に終点を追加 (終点 + 終点の親ノード)
        self._add_node_start_tree(end_pos, end_pos, min_cost_node, 0.0, 0.0)

    def _get_unit_ball(self):
        """
        単位球内の位置を取得

        戻り値
            position(numpy.ndarray): 単位球内の位置
        """
        lenth_of_one_side = 1.0
        while True:
            # -1から1の範囲で，適当な座標を取得
            position = np.random.uniform(np.ones(self._dim) * lenth_of_one_side * -1, np.ones(self._dim) * lenth_of_one_side)
            # 距離を計算
            distance = np.linalg.norm(position)
            if distance <= 1:
                # 単位球の中をプロット
                # 単位球の中をプロットする確率は 単位球の体積 / 一辺が2の立方体の体積
                break
            else:
                # 単位球の外をプロット
                # プロットできなかった時は，辺の長さを小さくして，単位球の中をプロットしやすくする (whileループから早く抜けたいから)
                lenth_of_one_side -= 0.0001

        return position
    # メイン処理メソッド ↑


    # ファイル関連メソッド ↓
    def _get_path_plan_folders(self, interpolation):
        """
        経路生成結果を保存する複数のフォルダー名を取得

        パラメータ
            interpolation(str): 探索方法 (位置空間/関節空間)

        戻り値
            folder_names(list): 複数のフォルダー名
        """
        folder_names = []

        folder_name = self._name
        # サブフォルダー
        sub_folder_names = [self._FOLODER_NAME_PRUNING, self._FOLODER_NAME_SHORTCUT,
                            self._FOLODER_NAME_PARTIAL_SHORTCUT, self._FOLDER_NAME_CURRENT_TREE]

        for sub_folder_name in sub_folder_names:
            folder_names.append(os.path.join(
                folder_name, interpolation, sub_folder_name))

        return folder_names

    def _save_current_tree(self):
        """
        現在のツリーをファイル化
        """
        if self._debug:
            # 新規ノードが追加できたら，self._count_successはインクリメントされていく
            # 始点のツリーを保存
            file_name = os.path.join(self._FOLDER_NAME_CURRENT_TREE, f"{self._FILE_NAME_CURRENT_TREE}_{self._count_success}.csv")
            self._save_numpy_data_to_txt(self._start_tree.nodes, file_name)

            # 始点のランダムツリーを保存
            file_name = os.path.join(self._FOLDER_NAME_CURRENT_TREE, f"{self._FILE_NAME_RANDOM_TREE}_{self._count_success}.csv")
            self._save_numpy_data_to_txt(self._start_random_tree.nodes, file_name)
    # ファイル関連メソッド ↑


class RRTStarConnectRobot(RRTConnectRobot, RRTStarRobot):
    """
    RRT*-Connect (RRT* + RRT-Connect)
        RRT*は最適性がある
        RRT-Connectは双方向探索のため，処理時間が短い
        RRT*-ConnectはRRT*とRRT-Connectの良い点を合わせたアルゴリズムである

    プロパティ
        _max_iterate(int): 最大探索回数
        _count_success(int): ノード追加の成功回数
        _near_node_radius(float): 近傍ノードの探索半径
        _end_tree(CostedTree): 終点ツリー
        _end_random_tree(CostedTree): 終点ランダムツリー
        _connected_nodes(list): 始点から終点まで繋がったノード

    メソッド
        public

            プロパティのゲッター関連
                file_name_current_start_tree(): _FILE_NAME_CURRENT_START_TREE定数のゲッター
                file_name_current_start_random_tree(): _FILE_NAME_CURRENT_START_RANDOM_TREE定数のゲッター
                file_name_current_end_tree(): _FILE_NAME_CURRENT_END_TREE定数のゲッター
                file_name_current_end_random_tree(): _FILE_NAME_CURRENT_END_RANDOM_TREE定数のゲッター

            メイン処理関連
                planning(): 経路生成の実装

        protected

            メイン処理関連
                _add_node_end_tree(): 終点ツリーにノードを設定する
                _exec_state(): 状態(_state)に応じた処理を実施
                _add_connected_nodes(): 始点から終点まで繋がった際のノードを追加
                _get_min_cost_connected_nodes(): 始点から終点まで繋がった際のノードで，最小コストとなるノード番号
                _fin_planning(): 経路生成の終了処理
                _get_near_node_list(): ノードと近傍ノード(終点ツリー内の)をリストで取得
                _get_min_cost_node(): コストが最小となる近傍ノードを取得
                _update_near_node(): 近傍ノード内の親ノードを更新

            準備処理関連
                _reset(): データの初期化

            ファイル関連
                _save(); 生成した経路をファイル保存
                _save_current_tree(): 現在のツリーをファイル化
    """
    # 定数の定義
    # ファイル名の定義
    # 各経路生成手法で絶対に定義するべきファイル名 ↓
    # _pathesプロパティを保存するファイル名
    _FILE_NAME_PATHES = f"{PATHPLAN.RRT_STAR_CONNECT.value}_pathes.csv"
    # _start_treeプロパティを保存するファイル名
    _FILE_NAME_START_TREE = f"{PATHPLAN.RRT_STAR_CONNECT.value}_start_tree.csv"
    # _start_random_treeプロパティを保存するファイル名
    _FILE_NAME_START_RANDOM_TREE = f"{PATHPLAN.RRT_STAR_CONNECT.value}_start_random_tree.csv"
    # _before_modify_pathesプロパティを保存するファイル名
    _FILE_NAME_BEFORE_MODIFY_PATHES = f"{PATHPLAN.RRT_STAR_CONNECT.value}_before_modify_pathes.csv"
    # _pruning_pathesプロパティを保存するファイル名
    _FILE_NAME_PRUNING_PATHES = f"{PATHPLAN.RRT_STAR_CONNECT.value}_pruning_pathes.csv"
    # _shortcut_pathesプロパティを保存するファイル名
    _FILE_NAME_SHORTCUT_PATHES = f"{PATHPLAN.RRT_STAR_CONNECT.value}_shortcut_pathes.csv"
    # _partial_shortcut_pathesプロパティを保存するファイル名
    _FILE_NAME_PARTIAL_SHORTCUT_PATHES = f"{PATHPLAN.RRT_STAR_CONNECT.value}_partial_shortcut_pathes.csv"

    # 毎処理保存するファイル名の定義
    # Pruning実施毎の経路を保存
    _FILE_NAME_PRUING_EACH_PATHES = f"{PATHPLAN.RRT_STAR_CONNECT.value}_pruning_each_pathes"
    # Shortcut実施毎の経路を保存
    _FILE_NAME_SHORTCUT_EACH_PATHES = f"{PATHPLAN.RRT_STAR_CONNECT.value}_shortcut_each_pathes"
    # Partial Shortcut実施毎の経路を保存
    _FILE_NAME_PARTIAL_SHORTCUT_EACH_PATHES = f"{PATHPLAN.RRT_STAR_CONNECT.value}_partial_shortcut_each_pathes"
    # 各経路生成手法で絶対に定義するべきファイル名 ↑

    # 終点ツリー用のファイル名
    # _end_treeプロパティを保存するファイル名
    _FILE_NAME_END_TREE = f"{PATHPLAN.RRT_STAR_CONNECT.value}_end_tree.csv"
    # _end_random_treeプロパティを保存するファイル名
    _FILE_NAME_END_RANDOM_TREE = f"{PATHPLAN.RRT_STAR_CONNECT.value}_end_random_tree.csv"
    # _statesプロパティを保存するファイル名
    _FILE_NAME_STATES = f"{PATHPLAN.RRT_STAR_CONNECT.value}_states.csv"

    # 毎処理保存するファイル名の定義
    # ノード追加時の現在のツリーを保存するファイル名
    _FILE_NAME_CURRENT_START_TREE = f"{PATHPLAN.RRT_STAR_CONNECT.value}_start_current_tree"
    # ノード追加時の現在のランダムツリーを保存するファイル名
    _FILE_NAME_CURRENT_START_RANDOM_TREE = f"{PATHPLAN.RRT_STAR_CONNECT.value}_start_random_tree"
    # ノード追加時の現在のツリーを保存するファイル名
    _FILE_NAME_CURRENT_END_TREE = f"{PATHPLAN.RRT_STAR_CONNECT.value}_end_current_tree"
    # ノード追加時の現在のランダムツリーを保存するファイル名
    _FILE_NAME_CURRENT_END_RANDOM_TREE = f"{PATHPLAN.RRT_STAR_CONNECT.value}_end_random_tree"

    # ツリーの要素番号を定義
    _NODE_NEAR_NODE_IDX = RRT_STAR_CONNECT_NEAR_NODE_IDX    # ノード内の最短ノード要素
    _NODE_COST_IDX = RRT_STAR_CONNECT_COST_IDX         # ノード内のコスト要素
    _NODE_RADIUS_IDX = RRT_STAR_CONNECT_RADIUS_IDX       # ノード内の半径要素

    # その他
    _MAX_ITERATE = 300               # 最大探索回数
    _TIMEOUT_VALUE = 1000              # タイムアウト時間 [second]
    _MAX_COST = 1e10              # 最大コスト


    def __init__(self):
        """
        コンストラクタ
        """
        # 親クラスの初期化処理
        super().__init__()
        # プロパティの初期化 (始点ツリーは親クラスで初期化されている)
        self._name = PATHPLAN.RRT_STAR_CONNECT.value
        self._end_tree = CostedTree(self._NODE_NEAR_NODE_IDX, self._NODE_COST_IDX, self._NODE_RADIUS_IDX)
        self._end_random_tree = CostedTree(self._NODE_NEAR_NODE_IDX, self._NODE_COST_IDX, self._NODE_RADIUS_IDX)
        self._connected_nodes = []


    # プロパティのゲッター ↓
    @property
    def file_name_current_start_tree(self):
        """
        _FILE_NAME_CURRENT_START_TREE定数のゲッター
        """
        return self._FILE_NAME_CURRENT_START_TREE

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

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

    @property
    def file_name_current_end_random_tree(self):
        """
        _FILE_NAME_CURRENT_END_RANDOM_TREE定数のゲッター
        """
        return self._FILE_NAME_CURRENT_END_RANDOM_TREE
    # プロパティのゲッター ↑


    # メイン処理メソッド ↓
    def planning(self, start_pos, end_pos, environment, robot, interpolation, modification=MODIFICATION.PRUNING_START, debug=False):
        """
        経路生成

        パラメータ
            start_pos(list): 経路生成の始点
            end_pos(list): 経路生成の終点
            environment(Base2DEnv): 環境
            robot(Robot): ロボットクラス
            interpolation(int): 補間方法 (関節空間/位置空間)
            modification(str): 経路生成後の修正方法 (Pruning/Shortcutなど)
            debug(bool): デバッグフラグ

        戻り値
            result(bool): True / False = 成功 / 失敗
        """
        # 処理結果
        result = False

        # 経路生成の準備
        self._preparation_planning(start_pos, end_pos, environment, robot, interpolation, modification, debug)

        # 始点ノードを設定
        self._add_node_start_tree(start_pos, start_pos, INITIAL_NODE_NEAR_NODE, 0.0, 0.0)
        # 終点ノードを設定
        self._add_node_end_tree(end_pos, end_pos, INITIAL_NODE_NEAR_NODE, 0.0, 0.0)
        # 探索する範囲を設定
        self._strict_planning_pos(start_pos, end_pos)
        # 近傍ノードを定義する半径
        self._near_node_radius = self._moving_value * self._NEAR_NODE_RADIUS_COEF

        # 経路生成は一定時間内に終了させる
        start_time = time.time()

        self._count_success = 0         # ノード追加に成功した回数

        # 全探索回数ループ
        for i in range(self._max_iterate):

            # RRT-Connect ↓
            # 1処理実施して，経路生成が完了したか確認する
            if self._exec_state(start_pos, end_pos):
                # 経路生成の完了
                result = True
            # RRT-Connect ↑

            # 干渉の有無に関わらずにタイムアウトの確認はする
            now_time = time.time()
            if now_time - start_time >= self._TIMEOUT_VALUE:
                # タイムアウト
                break

        if result:
            # 経路生成に成功のため，経路生成の終了処理
            self._fin_planning(start_pos, end_pos)
            # PathPruningの実施
            self._modify_path()
            # ファイルに保存する
            self._save()

        return result

    def _add_node_end_tree(self, pos, random_pos, near_node, cost, radius):
        """
        終点ツリーにノードを設定する

        パラメータ
            pos(numpy.ndarray): 位置
            random_pos(numpy.ndarray): ランダムな位置
            near_node(int): 最短ノード
            cost(float): 始点からposまでの総コスト
            radius(float): RRT*の探索半径
        """
        array_data = np.array([near_node, cost, radius])
        # _end_treeにノードを追加
        node = np.append(pos, array_data).reshape(1, -1)
        self._end_tree.add_node(node)

        # デバッグ有効の時しか，ランダムなツリーにデータを保存したくない (処理時間の短縮)
        if self._debug:
            # _end_random_treeにノードを追加
            random_node = np.append(random_pos, array_data).reshape(1, -1)
            self._end_random_tree.add_node(random_node)

    def _exec_state(self, start_pos, end_pos):
        """
        状態(_state)に応じた処理を実施

        パラメータ
            start_pos(numpy.ndarray): 始点
            end_pos(numpy.ndarray): 終点

        戻り値
            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
            target_pos = end_pos
            set_tree_func = self._add_node_start_tree
        else:
            my_tree    = self._end_tree
            your_tree  = self._start_tree
            target_pos = start_pos
            set_tree_func = self._add_node_end_tree

        # ツリーにランダム点を追加
        if self._state == STATE_START_TREE_RANDOM or self._state == STATE_END_TREE_RANDOM:
            # 始点ツリーにランダム点を追加する状態または終点ツリーにランダム点を追加する状態
            # ランダムな値を取得する
            random_pos = self._get_random_pos(target_pos)
            # ランダムな値とツリーの最短ノードを計算
            near_node  = my_tree.get_near_node(random_pos)
            # 位置と最短ノードが保存されているから，位置だけを取得
            near_node_pos = my_tree.nodes[near_node, :self._NODE_NEAR_NODE_IDX]
            # 最短ノードからランダムな値方向へ新しいノードを作成
            new_pos = self._calc_new_pos(random_pos, near_node_pos)

            # 干渉物との干渉判定
            is_interference = self._line_interference(new_pos, near_node_pos)
            if not is_interference:  # 干渉なし
                # RRT*の処理を追加 ↓
                self._count_success += 1
                # 新規ノードのコストを計算
                new_cost = my_tree.calc_cost(near_node, new_pos)
                # 近傍ノードを全部取得
                near_node_list, threshold = self._get_near_node_list(new_pos, my_tree)
                # 近傍ノードからコストが最小となるノードを取得
                min_cost_node, min_cost = self._get_min_cost_node(new_pos, near_node_list, near_node, new_cost, my_tree)
                # 近傍ノード内でコストが小さくなれば，最短ノードを更新
                self._update_near_node(new_pos, near_node_list, min_cost, my_tree)
                # RRT*の処理を追加 ↑

                # ツリーにノードを追加
                set_tree_func(new_pos, random_pos, min_cost_node, min_cost, threshold)
                # 状態を保存
                self._save_state()
                # 状態を遷移させる
                self._state_transition()
                # 現在のツリーをファイル化する
                self._save_current_tree()
        else:
            # ツリーで作成したノードを取得
            end_pos   = your_tree.nodes[-1, :self._NODE_NEAR_NODE_IDX]
            # ツリー内の最短ノードを計算
            near_node = my_tree.get_near_node(end_pos)
            # 位置と最短ノードが保存されているから，位置だけを取得
            near_node_pos = my_tree.nodes[near_node, :self._NODE_NEAR_NODE_IDX]
            # 最短ノードから終点ツリーのノード方向へ新しいノードを作成
            new_pos = self._calc_new_pos(end_pos, near_node_pos)

            # 干渉物との干渉判定
            is_interference = self._line_interference(new_pos, near_node_pos)
            if is_interference:  # 干渉あり
                # 状態を遷移させる
                self._state_transition()
            else:   # 干渉なし
                # RRT*の処理を追加 ↓
                self._count_success += 1
                # 新規ノードのコストを計算
                new_cost = my_tree.calc_cost(near_node, new_pos)
                # 近傍ノードを全部取得
                near_node_list, threshold = self._get_near_node_list(new_pos, my_tree)
                # 近傍ノードからコストが最小となるノードを取得
                min_cost_node, min_cost = self._get_min_cost_node(new_pos, near_node_list, near_node, new_cost, my_tree)
                # 近傍ノード内でコストが小さくなれば，最短ノードを更新
                self._update_near_node(new_pos, near_node_list, min_cost, my_tree)
                # RRT*の処理を追加 ↑

                # ツリーにノードを追加
                set_tree_func(new_pos, end_pos, min_cost_node, min_cost, threshold)
                # 状態を保存
                self._save_state()
                # 現在のツリーをファイル化する
                self._save_current_tree()
                # 始点から終点までの経路が生成できたかの確認
                if self._chk_end_pos_dist(new_pos, end_pos):
                    # 経路生成の完了

                    # RRT*-Connect特有の処理 ↓
                    # ペアを覚えておく
                    if self._state == STATE_START_TREE_END_TREE:
                        # 始点ツリーがmy_tree，終点ツリーがyour_tree
                        start_node_idx =   my_tree.nodes.shape[0] - 1
                        end_node_idx   = your_tree.nodes.shape[0] - 1
                    else:
                        # 終点ツリーがmy_tree, 始点ツリーがyour_tree
                        start_node_idx = your_tree.nodes.shape[0] - 1
                        end_node_idx   =   my_tree.nodes.shape[0] - 1
                    self._add_connected_nodes(start_node_idx, end_node_idx)
                    # RRT*-Connect特有の処理 ↑

                    # 経路生成に成功したため，状態を遷移させて，異なる経路を作成する
                    self._state_transition()

                    complete_path = True

        return complete_path

    def _add_connected_nodes(self, start_node_idx, end_node_idx):
        """
        始点から終点まで繋がった際のノードを追加

        パラメータ
            start_node_idx(int): 始点ノード番号
            end_node_idx(int): 終点ノード番号
        """
        add_node = [start_node_idx, end_node_idx]
        self._connected_nodes.append(add_node)

    def _get_min_cost_connected_nodes(self):
        """
        始点から終点まで繋がった際のノードで，最小コストとなるノード番号

        戻り値
            start_node_idx(int): 始点ノード番号
            end_node_idx(int): 終点ノード番号
        """
        # 注意点：始点から終点までの経路が作成されてから呼ばれる前提である
        if len(self._connected_nodes) == 0:
            raise ValueError("self._connected_nodes is abnormal.")

        min_cost = self._MAX_COST
        start_node_idx = 0
        end_node_idx = 0

        for node_idxs in self._connected_nodes:
            # ノード番号を始点と終点に分割
            start_node = self._start_tree.nodes[node_idxs[0]]
            end_node   =   self._end_tree.nodes[node_idxs[1]]
            # 各ノードのコストを取得
            start_node_cost = start_node[self._NODE_COST_IDX]
            end_node_cost   =   end_node[self._NODE_COST_IDX]
            # 各ノードの位置を取得
            start_node_pos = start_node[:self._NODE_NEAR_NODE_IDX]
            end_node_pos   =   end_node[:self._NODE_NEAR_NODE_IDX]
            # 2点間の距離を計算
            distance = np.linalg.norm(end_node_pos - start_node_pos)
            now_cost = start_node_cost + end_node_cost + distance
            if now_cost < min_cost:
                # 最短コストの更新
                start_node_idx = node_idxs[0]
                end_node_idx = node_idxs[1]
                min_cost = now_cost

        return start_node_idx, end_node_idx

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

        パラメータ
            start_pos(list): 経路生成の始点
            end_pos(list): 経路生成の終点
        """
        # 最短コストとなるノード番号を取得
        start_node_idx, end_node_idx = self._get_min_cost_connected_nodes()

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

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

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

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

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

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

    def _get_near_node_list(self, pos, tree):
        """
        ノードと近傍ノード(終点ツリー内の)をリストで取得

        パラメータ
            pos(numpy.ndarray): ノード位置
            tree(CostedTree): コスト付きツリー (始点ツリー or 終点ツリー)

        戻り値
            near_node_list(list): 近傍ノードリスト
            threshold(float): 近傍判定の閾値
        """
        # 閾値はノード数に依存する
        n_node = tree.nodes.shape[0]
        threshold = self._near_node_radius * np.power(np.log(n_node) / n_node, 1 / self._dim)

        # 閾値以内の距離であるノードを全部取得
        near_node_list = tree.get_near_node_list(pos, threshold)

        return near_node_list, threshold

    def _get_min_cost_node(self, pos, near_node_list, near_node, near_node_cost, tree):
        """
        コストが最小となる近傍ノードを取得

        パラメータ
            pos(numpy.ndarray): ノード位置
            near_node_list(list): 近傍ノードリスト
            near_node(int): 最短ノード
            near_node_cost(float): 最短ノード追加時のコスト
            tree(CostedTree): コスト付きツリー (始点ツリー or 終点ツリー)

        戻り値
            min_cost_node(int): コスト最小ノード
            min_cost(float): コスト最小値
        """
        # 戻り値の初期化
        min_cost = near_node_cost
        min_cost_node = near_node

        if not near_node_list:
            # 近傍ノードがないため，処理終了
            return min_cost_node, min_cost

        for node_idx in near_node_list:
            # 近傍ノードに関する情報を保存
            node      = tree.nodes[node_idx]
            node_pos  = node[:self._NODE_NEAR_NODE_IDX]
            node_cost = node[self._NODE_COST_IDX]

            # コスト(距離)を計算
            distance = np.linalg.norm(node_pos - pos)

            # 2点間の干渉チェック
            is_interference = self._line_interference(node_pos, pos)
            if not is_interference:
                # 干渉なし
                new_cost = distance + node_cost
                if new_cost < min_cost:
                    # 小さくなるため，最短ノードを修正
                    min_cost_node = node_idx
                    min_cost = new_cost

        return min_cost_node, min_cost

    def _update_near_node(self, pos, near_node_list, cost, tree):
        """
        近傍ノード内の親ノードを更新

        パラメータ
            pos(numpy.ndarray): ノード位置
            near_node_list(list): 近傍ノードリスト
            cost(float): コスト
            tree(CostedTree): コスト付きツリー (始点ツリー or 終点ツリー)
        """
        if not near_node_list:
            # 近傍ノードがないため，処理終了
            return

        # 新規ノードのインデックス番号を保存
        new_node_idx = tree.nodes.shape[0]

        for node_idx in near_node_list:
            # 近傍ノードに関する情報を保存
            node      = tree.nodes[node_idx]
            node_pos  = node[:self._NODE_NEAR_NODE_IDX]
            node_cost = node[self._NODE_COST_IDX]

            # コスト(距離)を計算
            distance = np.linalg.norm(node_pos - pos)

            # 2点間の干渉チェック
            is_interference = self._line_interference(node_pos, pos)
            if not is_interference:
                # 干渉なし
                new_cost = distance + cost
                if new_cost < node_cost:
                    # 小さくなるため，最短ノードを修正
                    tree.chg_node_info(node_idx, new_node_idx, new_cost)
    # メイン処理メソッド ↑


    # 準備処理関連メソッド ↓
    def _reset(self):
        """
        データの初期化
        """
        # 親クラスの初期化
        super()._reset()

        if len(self._connected_nodes) != 0:
            # 何かしらのデータがある
            del self._connected_nodes
        self._connected_nodes = []
    # 準備処理関連メソッド ↑


    # ファイル関連メソッド ↓
    def _save(self):
        """
        生成した経路をファイル保存
        """
        if self._debug:
            # RRT-Connectクラスのファイル保存を実施
            RRTConnectRobot._save(self)

    def _save_current_tree(self):
        """
        現在のツリーをファイル化
        """
        if self._debug:
            # 新規ノードが追加できたら，self._count_successはインクリメントされていく
            # 始点に関するツリーを保存
            file_name = os.path.join(self._FOLDER_NAME_CURRENT_TREE, f"{self._FILE_NAME_CURRENT_START_TREE}_{self._count_success}.csv")
            self._save_numpy_data_to_txt(self._start_tree.nodes, file_name)
            file_name = os.path.join(self._FOLDER_NAME_CURRENT_TREE, f"{self._FILE_NAME_CURRENT_START_RANDOM_TREE}_{self._count_success}.csv")
            self._save_numpy_data_to_txt(self._start_random_tree.nodes, file_name)

            # 終点に関するツリーを保存
            file_name = os.path.join(self._FOLDER_NAME_CURRENT_TREE, f"{self._FILE_NAME_CURRENT_END_TREE}_{self._count_success}.csv")
            self._save_numpy_data_to_txt(self._end_tree.nodes, file_name)
            file_name = os.path.join(self._FOLDER_NAME_CURRENT_TREE, f"{self._FILE_NAME_CURRENT_END_RANDOM_TREE}_{self._count_success}.csv")
            self._save_numpy_data_to_txt(self._end_random_tree.nodes, file_name)
    # ファイル関連メソッド ↑


class MainRRTRobot:
    """
    ロボットの経路生成のメインクラス

    プロパティ
        _rrt(rrt.py内のクラス): 経路生成手法クラス

    メソッド
        public
            rrt(): _rrtプロパティのゲッター
            planning(): 経路生成

        protected
            _reset(): データの初期化
            _get_each_path_plan_instance(): 経路生成手法に応じたクラスのインスタンスを取得
    """
    # 定数の定義

    def __init__(self):
        """
        コンストラクタ
        """
        # プロパティの初期化
        self._rrt = None


    # プロパティのゲッターメソッド ↓
    @property
    def rrt(self):
        """
        _rrtプロパティのゲッター
        """
        return self._rrt
    # プロパティのゲッターメソッド ↑


    def _reset(self):
        """
        データの初期化
        """
        if self._rrt is not None:
            # インスタンス作成済みの場合
            del self._rrt
        self._rrt = None

    def _get_each_path_plan_instance(self, path_plan):
        """
        経路生成手法に応じたクラスのインスタンスを取得

        パラメータ
            path_plan(str): 経路生成手法名

        戻り値
            rrt: 各経路生成手法のクラス
        """
        # 経路生成手法によって，アニメーションの分岐を分ける
        if path_plan == PATHPLAN.RRT.value:
            # RRT
            rrt = RRTRobot()
        elif path_plan == PATHPLAN.RRT_CONNECT.value:
            # RRT-Connect
            rrt = RRTConnectRobot()
        elif path_plan == PATHPLAN.RRT_STAR.value:
            # RRT*
            rrt = RRTStarRobot()
        elif path_plan == PATHPLAN.RRT_STAR_CONNECT.value:
            # RRT*-Connect
            rrt = RRTStarConnectRobot()
        else:
            # 異常
            raise ValueError(f"path_plan is abnormal. path_plan is {path_plan}")

        return rrt

    def planning(self, path_plan, start_pos, end_pos, environment, robot, interpolation, modification=MODIFICATION.PRUNING_START, debug=False):
        """
        経路生成

        パラメータ
            path_plan(str): 経路生成手法名
            start_pos(list): 経路生成の始点
            end_pos(list): 経路生成の終点
            environment(Base2DEnv): 環境
            robot(Robot): ロボットクラス
            interpolation(int): 補間方法 (関節空間/位置空間)
            modification(str): 経路生成後の修正方法 (Pruning/Shortcutなど)
            debug(bool): デバッグフラグ

        戻り値
            result(bool): True / False = 成功 / 失敗
        """
        # データのリセット
        self._reset()

        # 経路生成手法クラスのインスタンスを取得
        self._rrt = self._get_each_path_plan_instance(path_plan)

        # 経路生成
        result = self._rrt.planning(start_pos, end_pos, environment, robot, interpolation, modification, debug)

        return result

経路生成の環境 (environment.py)

本記事で使用する環境に関するクラスを作成した．
Robot3DEnv()クラスを使用する．

environment.py

# 環境の作成

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

# サードパーティーの読み込み
import fcl

# 自作モジュールの読み込み
from constant import *              # 定数
from rotation import MyRotation     # 回転行列


class Base2DEnv:
    """
    経路生成時の2次元環境
    
    プロパティ
        _distance(float): 干渉物との最短距離
        _inteferences(dist): 干渉物の情報 (名称(円や長方形など) + 形状(半径，中心点など))
    
    メソッド
        public
            inteferences(): _inteferencesプロパティのゲッター
            distance(): _distanceプロパティのゲッター
            is_collision(): 干渉判定
            is_collision_dist(): 干渉判定 + 最短距離
    """
    # 定数の定義
    _Z_VALUE = 0.0          # z軸方向の値
    _PLOT_MIN_X = -0.5      # 描画時のX座標の最小値 (アニメーションを見て，手動で設定した)
    _PLOT_MAX_X =  2.0      # 描画時のX座標の最大値 (アニメーションを見て，手動で設定した)
    _PLOT_MIN_Y = -1.5      # 描画時のY座標の最小値 (アニメーションを見て，手動で設定した)
    _PLOT_MAX_Y =  2.0      # 描画時のY座標の最大値 (アニメーションを見て，手動で設定した)
    
    
    def __init__(self):
        """
        コンストラクタ
        """
        objects = []
        self._interferences = {}
        self._rot = MyRotation()

        # 円形の干渉物を定義 (円の位置 (x, y) と半径を定義)
        circles = [[0.8, 0.8, 0.3], [1.2, 0.8, 0.3], [1.2, 1.2, 0.3], [0.8, 1.2, 0.3]]
        for x, y, radius in circles:
            # fclに円が定義されていないため，楕円のz軸を0として円とする
            circle = fcl.Ellipsoid(radius, radius, self._Z_VALUE)
            # 円の中心点が原点であるため，中心点を平行移動させる
            translation = fcl.Transform(np.array([x, y, self._Z_VALUE]))
            obj = fcl.CollisionObject(circle, translation)
            # モデルを追加
            objects.append(obj)
        # 円形の干渉物を保存
        self._interferences[INTERFERENCE.CIRCLE.value] = circles

        # DynamicAABBTreeCollisionManager に登録
        self._manager = fcl.DynamicAABBTreeCollisionManager()
        self._manager.registerObjects(objects)
        self._manager.setup()

        # 最短距離を更新
        self._distance = 0.0

    @property
    def plot_min_pos(self):
        """
        描画時の最小値(x, y)を取得
        """
        min_pos = np.array([self._PLOT_MIN_X, self._PLOT_MIN_Y])
        return min_pos

    @property
    def plot_max_pos(self):
        """
        描画時の最大値(x, y)を取得
        """
        max_pos = np.array([self._PLOT_MAX_X, self._PLOT_MAX_Y])
        return max_pos

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

    @property
    def interferences(self):
        """
        _interferencesプロパティのゲッター (描画で使用する)
        """
        return self._interferences

    def is_collision(self, obj):
        """
        干渉判定

        パラメータ
            obj(fcl.CollisionObject): 干渉物

        戻り値
            is_collision(bool): True / False = 干渉あり / 干渉なし
        """
        # 衝突リクエスト
        # num_max_contacts ... 最大の衝突点数
        # enable_contact ... 衝突点情報の有無
        request  = fcl.CollisionRequest(num_max_contacts=100, enable_contact=True)
        col_data = fcl.CollisionData(request=request)

        # 本環境と干渉物の干渉判定
        self._manager.collide(obj, col_data, fcl.defaultCollisionCallback)

        return col_data.result.is_collision

    def is_collision_dist(self, obj, margin=0.0):
        """
        干渉判定 + 最短距離

        パラメータ
            obj(fcl.CollisionObject): 干渉物
            margin(float): 干渉判定のマージン

        戻り値
            collision(bool): True / False = 干渉あり / 干渉なし
        """
        dist_data   = fcl.DistanceData()

        # 最短距離の結果を保存する
        self._manager.distance(obj, dist_data, fcl.defaultDistanceCallback)
        min_dist = dist_data.result.min_distance
        # 最短距離の更新
        self._distance = min_dist

        if min_dist > margin:
            # 干渉なし
            collision = False
        else:
            # 干渉あり
            collision = True

        return collision


class Robot2DEnv(Base2DEnv):
    """
    経路生成時の2次元環境 (ロボット用)
    
    プロパティ
        _distance(float): 干渉物との最短距離
        _inteferences(dist): 干渉物の情報 (名称(円や長方形など) + 形状(半径，中心点など))
    
    メソッド
        public
            inteferences(): _inteferencesプロパティのゲッター
            distance(): _distanceプロパティのゲッター
            is_collision(): 干渉判定
            is_collision_dist(): 干渉判定 + 最短距離
    """
    # 定数の定義
    
    
    def __init__(self):
        """
        コンストラクタ
        """
        objects = []
        self._interferences = {}
        self._rot = MyRotation()

        # 円形の干渉物を定義 (円の位置 (x, y) と半径を定義)
        circles = [[1.5, 0.3, 0.3], [0.5, -0.5, 0.3]]
        for x, y, radius in circles:
            # fclに円が定義されていないため，球とする
            circle = fcl.Sphere(radius)
            # 円の中心点が原点であるため，中心点を平行移動させる
            translation = fcl.Transform(np.array([x, y, self._Z_VALUE]))
            obj = fcl.CollisionObject(circle, translation)
            # モデルを追加
            objects.append(obj)
        # 円形の干渉物を保存
        self._interferences[INTERFERENCE.CIRCLE.value] = circles

        # 長方形の干渉物を定義
        # 長方形の長さ (x, y) と中心点の位置 (x, y)，角度 [deg]
        rectangles = [[0.4, 0.6, 1.4, -0.6, 0], [0.5, 0.2, 1.2, 0.7, 0]]
        for x, y, center_x, center_y, angle in rectangles:
            # 直方体の長さ (x, y, z) を設定
            rectangle = fcl.Box(x, y, self._Z_VALUE)
            # 回転行列の作成
            rotation = self._rot.rot(np.deg2rad(angle), ROTATION_Z_AXIS)
            # 長方形の中心点，回転行列を設定
            translation = fcl.Transform(rotation, np.array([center_x, center_y, self._Z_VALUE]))
            obj = fcl.CollisionObject(rectangle, translation)
            # モデルを追加
            objects.append(obj)
        # 長方形の干渉物を保存
        self._interferences[INTERFERENCE.RECTANGLE.value] = rectangles

        # DynamicAABBTreeCollisionManager に登録
        self._manager = fcl.DynamicAABBTreeCollisionManager()
        self._manager.registerObjects(objects)
        self._manager.setup()

        # 最短距離を更新
        self._distance = 0.0



class Base3DEnv:
    """
    経路生成時の3次元環境
    
    プロパティ
        _distance(float): 干渉物との最短距離
        _inteferences(dist): 干渉物の情報 (名称(球や直方体など) + 形状(半径，中心点など))
    
    メソッド
        public
            inteferences(): _inteferencesプロパティのゲッター
            distance(): _distanceプロパティのゲッター
            is_collision(): 干渉判定
            is_collision_dist(): 干渉判定 + 最短距離
    """
    # 定数の定義
    _INITIAL_MARGIN = 0.0   # 干渉物とのマージン [m]
    _PLOT_MIN_X = -1        # 描画時のX座標の最小値
    _PLOT_MAX_X =  2        # 描画時のX座標の最大値
    _PLOT_MIN_Y = -2        # 描画時のY座標の最小値
    _PLOT_MAX_Y =  2        # 描画時のY座標の最大値
    _PLOT_MIN_Z = -1        # 描画時のZ座標の最小値
    _PLOT_MAX_Z =  4        # 描画時のZ座標の最大値
    
    
    def __init__(self):
        """
        コンストラクタ
        """
        objects = []
        self._interferences = {}

        # 球の干渉物を定義 (球の位置 (x, y, z) と半径を定義)
        balls = [[0.8, 0.8, 0.8, 0.3], [1.2, 0.8, 0.8, 0.3], [1.2, 1.2, 1.2, 0.3], [0.8, 1.2, 1.2, 0.3]]
        for x, y, z, radius in balls:
            # 球の半径を設定
            ball = fcl.Sphere(radius)
            # 球の中心点を平行移動
            translation = fcl.Transform(np.array([x, y, z]))
            obj = fcl.CollisionObject(ball, translation)
            # モデルを追加
            objects.append(obj)
        # 球の干渉物を保存
        self._interferences[INTERFERENCE.BALL.value] = balls

        # DynamicAABBTreeCollisionManager に登録
        self._manager = fcl.DynamicAABBTreeCollisionManager()
        self._manager.registerObjects(objects)
        self._manager.setup()

        # 最短距離を更新
        self._distance = 0.0

    @property
    def plot_min_pos(self):
        """
        描画時の最小値(x, y, z)を取得
        """
        min_pos = np.array([self._PLOT_MIN_X, self._PLOT_MIN_Y, self._PLOT_MIN_Z])
        return min_pos

    @property
    def plot_max_pos(self):
        """
        描画時の最大値(x, y, z)を取得
        """
        max_pos = np.array([self._PLOT_MAX_X, self._PLOT_MAX_Y, self._PLOT_MAX_Z])
        return max_pos

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

    @property
    def interferences(self):
        """
        _interferencesプロパティのゲッター (描画で使用する)
        """
        return self._interferences

    def is_collision(self, obj):
        """
        干渉判定

        パラメータ
            obj(fcl.CollisionObject): 干渉物

        戻り値
            is_collision(bool): True / False = 干渉あり / 干渉なし
        """
        # 衝突リクエスト
        # num_max_contacts ... 最大の衝突点数
        # enable_contact ... 衝突点情報の有無
        request  = fcl.CollisionRequest(num_max_contacts=100, enable_contact=True)
        col_data = fcl.CollisionData(request=request)

        # 本環境と干渉物の干渉判定
        self._manager.collide(obj, col_data, fcl.defaultCollisionCallback)

        return col_data.result.is_collision

    def is_collision_dist(self, obj, margin=_INITIAL_MARGIN):
        """
        干渉判定 + 最短距離

        パラメータ
            obj(fcl.CollisionObject): 干渉物
            margin(float): 干渉判定のマージン [m]

        戻り値
            collision(bool): True / False = 干渉あり / 干渉なし
        """
        # 引数確認
        if margin < 0:
            margin = self._INITIAL_MARGIN

        dist_data   = fcl.DistanceData()

        # 最短距離の結果を保存する
        self._manager.distance(obj, dist_data, fcl.defaultDistanceCallback)
        min_dist = dist_data.result.min_distance
        # 最短距離の更新
        self._distance = min_dist
        # print(f"min_dist = {min_dist}")

        if min_dist > margin:
            # 干渉なし
            collision = False
        else:
            # 干渉あり
            collision = True

        return collision


class Robot3DEnv(Base3DEnv):
    """
    経路生成時の3次元環境 (ロボット用)
    
    プロパティ
        _distance(float): 干渉物との最短距離
        _inteferences(dist): 干渉物の情報 (名称(円や長方形など) + 形状(半径，中心点など))
    
    メソッド
        public
            inteferences(): _inteferencesプロパティのゲッター
            distance(): _distanceプロパティのゲッター
            is_collision(): 干渉判定
            is_collision_dist(): 干渉判定 + 最短距離
    """
    # 定数の定義
    
    def __init__(self):
        """
        コンストラクタ
        """
        objects = []
        self._interferences = {}
        self._rot = MyRotation()

        # 円形の干渉物を定義 (円の位置 (x, y, z) と半径を定義)
        balls = [[0.5, 1.5, 1.5, 0.5], [1.5, 0.3, 1.5, 0.3], [1.5, 0.5, 1.0, 0.5]]
        for x, y, z, radius in balls:
            # fclに円が定義されていないため，球とする
            ball = fcl.Sphere(radius)
            # 円の中心点が原点であるため，中心点を平行移動させる
            translation = fcl.Transform(np.array([x, y, z]))
            obj = fcl.CollisionObject(ball, translation)
            # モデルを追加
            objects.append(obj)
        # 円形の干渉物を保存
        self._interferences[INTERFERENCE.BALL.value] = balls

        # 直方体の干渉物を定義
        # 直方体の長さ (x, y, z) と中心点の位置 (x, y, z)，x,y,z軸の回転角度 [deg]
        cuboids = [[0.4, 0.3, 0.8, 1.4, -0.6, 1.5, 0, 0, 0], [0.6, 0.8, 1.0, 1.2, 0.7, 2.0, 0, 0, 0]]
        for x, y, z, center_x, center_y, center_z, angle_x, angle_y, angle_z in cuboids:
            # 直方体の長さ (x, y, z) を設定
            cuboid = fcl.Box(x, y, z)
            # 回転行列の作成
            rotation_x = self._rot.rot(np.deg2rad(angle_x), ROTATION_X_AXIS)
            rotation_y = self._rot.rot(np.deg2rad(angle_y), ROTATION_Y_AXIS)
            rotation_z = self._rot.rot(np.deg2rad(angle_z), ROTATION_Z_AXIS)
            rotation = np.dot(rotation_z, np.dot(rotation_y, rotation_x))
            # 長方形の中心点，回転行列を設定
            translation = fcl.Transform(rotation, np.array([center_x, center_y, center_z]))
            obj = fcl.CollisionObject(cuboid, translation)
            # モデルを追加
            objects.append(obj)
        # 長方形の干渉物を保存
        self._interferences[INTERFERENCE.CUBOID.value] = cuboids

        # DynamicAABBTreeCollisionManager に登録
        self._manager = fcl.DynamicAABBTreeCollisionManager()
        self._manager.registerObjects(objects)
        self._manager.setup()

        # 最短距離を更新
        self._distance = 0.0

回転行列 (rotation.py)

回転行列の定義を説明する．

rotation.py

# 回転行列の定義

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


# サードパーティーの読み込み


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



class MyRotation:
    """
    回転行列クラス
    """
    _PITCH_THRESHOLD = 1e-4             # ピッチ角の閾値
    _ZERO_NEAR = 1e-4                   # 0近傍の閾値
    _EPSILON   = 1e-5                   # 微小値
    
    _ROT_MAX_VALUE =  1.0               # 回転行列の最大値
    _ROT_MIN_VALUE = -1.0               # 回転行列の最小値
    
    
    def _rot_x(self, theta):
        """
        x軸方向にtheta[rad]回転させる回転行列

        パラメータ
            theta(float): 回転角度 [rad]
        
        戻り値
            rotation(numpy.ndarray): 回転行列
        """
        rotation = np.array([[1, 0,              0            ],
                             [0, np.cos(theta), -np.sin(theta)],
                             [0, np.sin(theta),  np.cos(theta)]])

        return rotation

    def _rot_y(self, theta):
        """
        y軸方向にtheta[rad]回転させる回転行列

        パラメータ
            theta(float): 回転角度 [rad]
        
        戻り値
            rotation(numpy.ndarray): 回転行列
        """
        rotation = np.array([[ np.cos(theta), 0, np.sin(theta)],
                             [ 0,             1, 0            ],
                             [-np.sin(theta), 0, np.cos(theta)]])

        return rotation

    def _rot_z(self, theta):
        """
        z軸方向にtheta[rad]回転させる回転行列

        パラメータ
            theta(float): 回転角度 [rad]
        
        戻り値
            rotation(numpy.ndarray): 回転行列
        """
        rotation = np.array([[np.cos(theta), -np.sin(theta), 0],
                             [np.sin(theta),  np.cos(theta), 0],
                             [0,              0,             1]])

        return rotation

    def rot(self, theta, axis):
        """
        回転軸に応じた回転行列の取得

        パラメータ
            theta(float): 回転角度 [rad]
            axis(str): 回転軸
        
        戻り値
            rotation(numpy.ndarray): 回転行列
        """
        if axis == ROTATION_X_AXIS:
            # 回転軸がx軸
            rotation = self._rot_x(theta)
        elif axis == ROTATION_Y_AXIS:
            # 回転軸がy軸
            rotation = self._rot_y(theta)
        elif axis == ROTATION_Z_AXIS:
            # 回転軸がz軸
            rotation = self._rot_z(theta)
        elif axis == ROTATION_X_NEGATIVE_AXIS:
            # 回転軸がx軸だが，逆回転
            rotation = self._rot_x(-theta)
        elif axis == ROTATION_Y_NEGATIVE_AXIS:
            # 回転軸がy軸だが，逆回転
            rotation = self._rot_y(-theta)
        elif axis == ROTATION_Z_NEGATIVE_AXIS:
            # 回転軸がz軸だが，逆回転
            rotation = self._rot_z(-theta)
        else:
            # 異常
            raise ValueError(f"axis is abnormal. now is {axis}")

        return rotation

3軸ロボットアーム (robot.py)

3軸ロボットアームの処理を説明する．

robot.py

# ロボットアームの運動学を記載

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

# サードパーティーの読み込み
import fcl

# 自作モジュールの読み込み
from constant import *              # 定数
from rotation import MyRotation     # 回転行列


class Robot:
    """
    ロボットのベースクラス(抽象クラス)
    
    プロパティ
        _links(numpy.ndarray): ロボットのリンク長 [m]
        _rot(Rotation): 回転行列クラス
        _objects(list): 干渉物オブジェクト
        _manager(fcl.DynamicAABBTreeCollisionManager): 干渉判定クラス
        _jacov_thetas(list): 微分逆行列で取得した角度を保存
    
    メソッド
        public
            forward_kinematics(): 順運動学 (ロボットの関節角度からロボットの手先位置を算出)
            inverse_kinematics(): 逆運動学 (ロボットの手先位置からロボットの関節角度を算出)
            forward_kinematics_all_pos(): 順運動学で全リンクの位置を取得
            update(): 角度を与えて，各リンクの直方体を更新する
            differential_inverse_kinematics(): 微分逆運動学
            links(): _linksプロパティのゲッター
            manager(): _managerプロパティのゲッター
            jacov_thetas(): _jacov_thetasプロパティのゲッター
        
        protected
            _calc_homogeneou_matrix(): 同時変換行列の計算
            _jacovian(): ヤコビ行列
    """
    # 定数の定義
    _DIMENTION_POSE  = DIMENTION_NONE       # 手先位置の次元数
    _DIMENTION_THETA = DIMENTION_NONE       # 関節角度の次元数
    _DIMENTION_LINK  = DIMENTION_NONE       # リンク数
    _DIMENTION_AXIS  = DIMENTION_NONE       # 回転軸数
    
    _INITIAL_THETA   = 0.0                  # 初期回転角度 [rad]
    _HOMOGENEOU_MAT_ELEMENT = 4             # 同時変換行列の次元数
    _ROTATION_MAT_ELEMENT   = 3             # 回転行列の次元数
    
    _DETERMINANT_THRESHOLD = 1e-4           # 行列式の閾値
    _BOX_WIDTH = 1e-2                       # 各リンクの幅を定義
    _JACOV_DELTA_TIME = 0.10                # ヤコビの1時刻
    _JACOV_NEAR_POS   = 1e-6                # 目標位置との近傍距離 [m]
    _JACOV_MAX_COUNT  = 100                 # ヤコビの最大回数
    
    _LEVENBERG_MARUQUARDT_LAMBDA = 0.1      # Levenberg-Marquardt法で使用するパラメータ
    
    
    def __init__(self, links):
        """
        コンストラクタ

        パラメータ
            links(numpy.ndarray): ロボットのリンク長 [m]
        """
        if np.size(links) != self._DIMENTION_LINK:
            # 異常
            raise ValueError(f"links's size is abnormal. correct is {self._DIMENTION_Link}")

        # プロパティの初期化
        self._links = links
        self._rot = MyRotation()
        self._objects = []
        self._manager = None
        self._jacov_thetas = []
        self._lambda = self._LEVENBERG_MARUQUARDT_LAMBDA

    def _reset_jacov_thetas(self):
        """
        _jacov_thetasプロパティのリセット
        """
        if len(self._jacov_thetas) != 0:
            self._jacov_thetas.clear()

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

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

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

    def forward_kinematics(self, thetas):
        """
        順運動学 (ロボットの関節角度からロボットの手先位置を算出)

        パラメータ
            thetas(numpy.ndarray): ロボットの関節角度 [rad]
        
        戻り値
            pose(numpy.ndarray): ロボットの手先位置 (位置 + 姿勢) [m] + [rad]
        """
        raise NotImplementedError("forward_kinematics() is necessary override.")

    def inverse_kinematics(self, pose):
        """
        逆運動学 (ロボットの手先位置からロボットの関節角度を算出)

        パラメータ
            pose(numpy.ndarray): ロボットの手先位置 (位置 + 姿勢) [m] + [rad]
        
        戻り値
            thetas(numpy.ndarray): ロボットの関節角度 [rad]
        """
        raise NotImplementedError("inverse_kinematics() is necessary override.")

    def forward_kinematics_all_pos(self, thetas):
        """
        順運動学で全リンクの位置を取得

        パラメータ
            thetas(numpy.ndarray): ロボットの関節角度 [rad]
        
        戻り値
            all_pose(numpy.ndarray): ロボットの全リンク位置 (位置 + 姿勢) [m] + [rad]
        """
        raise NotImplementedError("forward_kinematics() is necessary override.")

    def update(self, thetas):
        """
        角度を与えて，各リンクの直方体を更新する

        パラメータ
            thetas(numpy.ndarray): ロボットの関節角度 [rad]
        """
        raise NotImplementedError("update() is necessary override.")

    def differential_inverse_kinematics(self, thetas, target_pos):
        """
        2点間の微分逆運動学

        パラメータ
            thetas(numpy.ndarray): ロボットの関節角度 [rad]
            target_pos(numpy.ndarray): 目標位置 (位置[m]・姿勢[rad])
        
        戻り値
            target_thetas(numpy.ndarray): 目標位置の関節角度 [rad]
        """
        raise NotImplementedError("differential_inverse_kinematics() is necessary override.")

    def _jacovian(self, thetas):
        """
        ヤコビ行列の計算

        パラメータ
            thetas(numpy.ndarray): ロボットの関節角度 [rad]
        
        戻り値
            jacovian(numpy.ndarray): ヤコビ行列
        """
        raise NotImplementedError("_jacovian() is necessary override.")

    def _calc_homogeneou_matrix(self, thetas):
        """
        同時変換行列の計算

        パラメータ
            thetas(numpy.ndarray): 関節角度 [rad]
        
        戻り値
            homogeneou_matix(numpy.ndarray): 全リンクの同時変換行列
        """
        # パラメータの次元数を確認
        if np.size(thetas) != self._DIMENTION_THETA:
            raise ValueError(f"thetas's size is abnormal. thetas's size is {np.size(thetas)}")

        homogeneou_matix = np.zeros((self._DIMENTION_LINK, self._HOMOGENEOU_MAT_ELEMENT, self._HOMOGENEOU_MAT_ELEMENT))

        # 1リンク前の同時変換行列
        prev_homogeneou_matrix = np.eye(self._HOMOGENEOU_MAT_ELEMENT)
        for i in range(self._DIMENTION_THETA):
            # 4行4列の要素を1に更新
            homogeneou_matix[i, -1, -1] = 1
            # 回転行列の計算
            rotation_matrix   = self._rot.rot(thetas[i], self._axiss[i])
            # リンク間の相対位置を取得
            relative_position = self._relative_positions[i].reshape(1, -1)
            # 同時変換行列に回転行列を保存
            homogeneou_matix[i, :self._ROTATION_MAT_ELEMENT, :self._ROTATION_MAT_ELEMENT] = rotation_matrix
            # 同時変換行列に相対位置を保存
            homogeneou_matix[i, :self._ROTATION_MAT_ELEMENT,  self._ROTATION_MAT_ELEMENT] = relative_position
            # 1リンク前の同時変換行列と組み合わせる
            homogeneou_matix[i] = np.dot(prev_homogeneou_matrix, homogeneou_matix[i])
            # 1リンク前の同時変換行列の更新
            prev_homogeneou_matrix = homogeneou_matix[i]

        return homogeneou_matix


class Robot2DoF(Robot):
    """
    2軸ロボットクラス
    
    プロパティ
        _links(numpy.ndarray): ロボットのリンク長
        _rot(Rotation): 回転行列クラス
        _axiss(list): 関節の回転軸
    
    メソッド
        public
            forward_kinematics(): 順運動学 (ロボットの関節角度からロボットの手先位置を算出)
    """
    # 定数の定義
    _DIMENTION_POSE  = DIMENTION_2D         # 手先位置の次元数
    _DIMENTION_THETA = DIMENTION_2D         # 関節角度の次元数
    _DIMENTION_LINK  = DIMENTION_2D         # リンク数
    
    
    def __init__(self, links):
        """
        コンストラクタ

        パラメータ
            links(numpy.ndarray): ロボットのリンク長 [m]
        """
        # 親クラスの初期化
        super().__init__(links)
        # ロボットの各リンクを直方体として定義する
        self._objects = []
        # (全リンクの角度を0とした時の) 各リンク間の相対位置
        self._relative_positions = np.zeros((self._DIMENTION_POSE + 1, 3))
        self._relative_positions[0] = np.array([0, 0, 0])
        self._relative_positions[1] = np.array([self._links[0], 0, 0])
        self._relative_positions[2] = np.array([self._links[1], 0, 0])

        # リンク1とリンク2は回転軸がz軸である
        self._axiss = [ROTATION_Z_AXIS, ROTATION_Z_AXIS]
        # 初期角度
        initial_thetas = np.zeros(self._DIMENTION_THETA)
        # 順運動学により，全リンク(ベースリンク，リンク1，リンク2)の位置を計算
        all_link_pose = self.forward_kinematics_all_link_pos(initial_thetas)

        # 1つ前のリンクの回転行列を更新
        prev_rotation = np.eye(self._ROTATION_MAT_ELEMENT)

        # ロボットの各リンクを直方体として定義する
        for i in range(self._DIMENTION_THETA):
            # 各リンクの回転行列を定義
            rotation = self._rot.rot(initial_thetas[i], self._axiss[i])
            rotation = np.dot(prev_rotation, rotation)
            # 各リンクの中心位置 (x, y, z) を定義
            center = np.zeros(DIMENTION_3D)
            center[:self._DIMENTION_POSE] = all_link_pose[i + 1] / 2 + all_link_pose[i]
            # 直方体の定義 (x, y, zの長さを保存)
            box = fcl.Box(self._relative_positions[i + 1, 0], 2 * self._BOX_WIDTH, 2 * self._BOX_WIDTH)
            # 直方体の中心を定義 (位置・姿勢)
            translation = fcl.Transform(rotation, center)
            obj = fcl.CollisionObject(box, translation)
            # モデルを追加
            self._objects.append(obj)
            # 1つ前のリンクの回転行列を更新
            prev_rotation = rotation

        # 直方体をAABBとして，定義
        # DynamicAABBTreeCollisionManager に登録
        self._manager = fcl.DynamicAABBTreeCollisionManager()
        self._manager.registerObjects(self._objects)
        self._manager.setup()

        self._jacov_thetas = []

    def forward_kinematics(self, thetas):
        """
        順運動学 (ロボットの関節角度からロボットの手先位置を算出)

        パラメータ
            thetas(numpy.ndarray): ロボットの関節角度 [rad]
        
        戻り値
            pose(numpy.ndarray): ロボットの手先位置 (位置) [m]
        """
        # パラメータの次元数を確認
        if np.size(thetas) != self._DIMENTION_THETA:
            raise ValueError(f"thetas's size is abnormal. thetas's size is {np.size(thetas)}")

        # 同時変換行列(2 * 4 * 4)を計算する
        homogeneou_matrix = self._calc_homogeneou_matrix(thetas)

        # 最終リンクの同時変換行列(最終リンク座標の位置・姿勢)より，手先位置を計算する
        final_link_matrix = homogeneou_matrix[self._DIMENTION_LINK - 1]
        # 最終リンクから手先位置までの早退位置(4ベクトル)を定義
        relative_pos = np.ones(self._HOMOGENEOU_MAT_ELEMENT)
        relative_pos[:self._HOMOGENEOU_MAT_ELEMENT - 1] = self._relative_positions[-1]
        pose = np.dot(final_link_matrix, relative_pos)
        # 手先位置(x, y)を取得
        pose = pose[:self._DIMENTION_POSE]

        return pose

    def differential_inverse_kinematics(self, thetas, target_pos):
        """
        2点間の微分逆運動学

        パラメータ
            thetas(numpy.ndarray): ロボットの関節角度 [rad]
            target_pos(numpy.ndarray): 目標位置 (位置[m]・姿勢[rad])
        
        戻り値
            target_thetas(numpy.ndarray): 目標位置の関節角度 [rad]
        """
        # 引数の確認
        if np.size(thetas) != self._DIMENTION_THETA:
            # 異常
            raise ValueError(f"theta's size is abnormal. theta's size is {np.size(thetas)}")

        # 現在の関節角度を保存 (異なるアドレスにデータを保存するためにnp.copy()を採用)
        current_thetas = np.copy(thetas)

        # 目標値の関節角度が見つかったどうか
        success_flg = False

        # 微分逆行列で取得したデータを初期化
        self._reset_jacov_thetas()
        # 計算した角度を保存する
        self._jacov_thetas.append(current_thetas)

        # 目標位置に近づくまでループ
        for i in range(self._JACOV_MAX_COUNT):
            # ヤコビ行列の取得
            jacovian = self._jacovian(current_thetas)

            # 現在の手先位置を計算
            current_pos = self.forward_kinematics(current_thetas)
            # 位置の差分を計算
            dP = target_pos - current_pos

            # # <修正内容> 旧内容 (特異点非対応) ↓
            # # dP(位置の差分) = J(ヤコビ行列) * dTheta(角度の差分)
            # # dTheta = J^(-1)(ヤコビ行列の逆行列) * dP
            # # 行列式が0近傍であるかの確認(ヤコビ行列の逆行列の存在確認)
            # det = np.linalg.det(jacovian)
            # if abs(det) <= self._DETERMINANT_THRESHOLD:
            #     # 0近傍であるため，逆行列が存在しない
            #     raise ValueError(f"abs(det) is near 0. abs(det) is {abs(det)}")

            # # dTheta = J^(-1)(ヤコビ行列の逆行列) * dP
            # dTheta = np.dot(np.linalg.inv(jacovian), dP)
            # # <修正内容> 旧内容 (特異点非対応) ↑

            # # <修正内容> 新内容 (レーベンバーグマルカート法) ↓
            # # 逆行列の中身を作成
            # new_jacovian = np.dot(jacovian, jacovian.T) + self._lambda * np.eye(jacovian.shape[0])
            # dTheta = np.dot(np.dot(jacovian.T, np.linalg.inv(new_jacovian)), dP)
            # # <修正内容> 新内容 (レーベンバーグマルカート法) ↑
            
            # # <修正内容> 新内容 (特異値分解) ↓
            # # ヤコビ行列を特異値分解する
            # U, S, V_T = np.linalg.svd(jacovian)
            # # 特異値の閾値を作成 (特異値の最大値を使用する)
            # threshold = S[0] * 0.05
            # # 特異値を安定化する
            # S_inv = np.array([1/s if s > threshold else 0 for s in S])
            # # 擬似逆行列を構成する
            # J_pinv = np.dot(np.dot(V_T.T, np.diag(S_inv)), U.T)
            # dTheta = np.dot(J_pinv, dP)
            # # <修正内容> 新内容 (特異値分解) ↑

            # <修正内容> 新内容 (レーベンバーグマルカート法 + 可操作度) ↓
            # 可操作度を計算
            manipulabitity = np.sqrt(np.linalg.det(np.dot(jacovian, jacovian.T)))
            if manipulabitity <= 0.1:
                inv_data = np.dot(jacovian, jacovian.T) + self._lambda * np.eye(jacovian.shape[0])
                dTheta = np.dot(np.dot(jacovian.T, np.linalg.inv(inv_data)), dP)
            else:
                # 特異点近傍ではないため，逆行列を普通に計算
                dTheta = np.dot(np.linalg.inv(jacovian), dP)
            # <修正内容> 新内容 (レーベンバーグマルカート法 + 可操作度) ↑



            # 関節角度の更新 (+=とするとcurrent_thetasが常に同じアドレスになるため，current_thetasを異なるアドレスとしたい)
            # print(f"id(current_thetas) = {id(current_thetas)}")   # アドレス確認
            # current_thetas = current_thetas + dTheta * self._JACOV_DELTA_TIME
            # 位置の差分が大きいほど，角度の更新量を小さくしたい
            coefficient = 1 / max(0.5, np.linalg.norm(dP))
            # coefficient = 1
            current_thetas = current_thetas + dTheta * coefficient * self._JACOV_DELTA_TIME
            # print(f"id(current_thetas) = {id(current_thetas)}")   # アドレス確認

            # 計算した角度を保存する
            self._jacov_thetas.append(current_thetas)

            # 更新後の手先位置が目標位置の近傍であれば，処理終了とする
            current_pos = self.forward_kinematics(current_thetas)
            distance = np.linalg.norm(target_pos - current_pos)
            # print(f"distance = {distance}")
            if distance <= self._JACOV_NEAR_POS:
                # 近傍のため，処理終了
                success_flg = True
                break

        # if not success_flg:
        #     # 目標位置の関節角度が見つからない
        #     raise ValueError(f"target_pos's theta is not found. please change target_pos")

        target_thetas = current_thetas

        return target_thetas

    def _jacovian(self, thetas):
        """
        ヤコビ行列の計算

        パラメータ
            thetas(numpy.ndarray): ロボットの関節角度 [rad]
        
        戻り値
            jacovian(numpy.ndarray): ヤコビ行列
        """
        # 引数の確認
        if np.size(thetas) != self._DIMENTION_THETA:
            # 異常
            raise ValueError(f"theta's size is abnormal. theta's size is {np.size(thetas)}")

        # jacovian = [[-l1 * sin(theta1) - l2 * sin(theta12), -l2 * sin(theta12)], [l1 * cos(theta1) + l2 * cos(theta12), l2 * cos(theta12)]]
        # 各リンクの長さをローカル変数に保存
        l1 = self._links[0]
        l2 = self._links[1]

        # 三角関数の計算
        sin1  = np.sin(thetas[0])
        cos1  = np.cos(thetas[0])
        sin12 = np.sin(thetas[0] + thetas[1])
        cos12 = np.cos(thetas[0] + thetas[1])

        # ヤコビ行列
        jacovian =    np.array([[-l1 * sin1 - l2 * sin12, -l2 * sin12],
                                [ l1 * cos1 + l2 * cos12,  l2 * cos12]])

        return jacovian

    def inverse_kinematics(self, pose, upper=False):
        """
        逆運動学 (ロボットの手先位置からロボットの関節角度を算出)

        パラメータ
            pose(numpy.ndarray): ロボットの手先位置 (位置) [m]
            upper(bool): 腕が上向かどうか

        戻り値
            thetas(numpy.ndarray): ロボットの関節角度 [rad]
        """
        # パラメータの次元数を確認
        if np.size(pose) != self._DIMENTION_POSE:
            raise ValueError(f"parameter pose's size is abnormal. pose's size is {np.size(pose)}")

        # c2 = {(px ** 2 + py ** 2) - (l1 ** 2 + l2 ** 2)} / (2 * l1 * l2)
        px = pose[0]
        py = pose[1]
        l1 = self._links[0]
        l2 = self._links[1]
        cos2  = ((px ** 2 + py ** 2) - (l1 ** 2 + l2 ** 2)) / (2 * l1 * l2)
        # cosの範囲は-1以上1以下である
        if cos2 < -1 or cos2 > 1:
            # 異常
            raise ValueError(f"cos2 is abnormal. cos2 is {cos2}")

        # sinも求めて，theta2をatan2()より算出する
        sin2 = np.sqrt(1 - cos2 ** 2)
        theta2 = np.arctan2(sin2,  cos2)
        if not upper:
            # 下向きの角度のため，三角関数も更新
            theta2 = -theta2
            sin2 = np.sin(theta2)
            cos2 = np.cos(theta2)

        # 行列計算
        # [c1, s1] = [[l1 + l2 * c2, -l2 * s2], [l2 * s2, l1 + l2 * c2]] ** -1 * [px, py]
        element1 =  l1 + l2 * cos2
        element2 = -l2 * sin2
        matrix = np.array([[ element1, element2],
                           [-element2, element1]])
        # 行列式を計算
        det = np.linalg.det(matrix)
        # 0近傍の確認
        if det <= self._DETERMINANT_THRESHOLD and det >= -self._DETERMINANT_THRESHOLD:
            # 0近傍 (異常)
            raise ValueError(f"det is abnormal. det is {det}")

        # [c1, s1]の計算
        cos1_sin1 = np.dot(np.linalg.inv(matrix), pose)
        # theta1をatan2()より算出する
        theta1 = np.arctan2(cos1_sin1[1], cos1_sin1[0])

        thetas = np.array([theta1, theta2])

        return thetas

    def forward_kinematics_all_link_pos(self, thetas):
        """
        順運動学で全リンクの位置を取得 (グラフの描画で使用する)

        パラメータ
            thetas(numpy.ndarray): ロボットの関節角度 [rad]
        
        戻り値
            all_link_pose(numpy.ndarray): ロボットの全リンク位置 (位置 + 姿勢) [m] + [rad]
        """
        # パラメータの次元数を確認
        if np.size(thetas) != self._DIMENTION_THETA:
            raise ValueError(f"thetas's size is abnormal. thetas's size is {np.size(thetas)}")

        # 同時変換行列(2 * 4 * 4)を計算する
        homogeneou_matix = self._calc_homogeneou_matrix(thetas)

        # 全リンクの座標系の原点を取得
        all_link_pose = np.zeros((self._DIMENTION_LINK + 1, self._DIMENTION_POSE))
        for i, matrix in enumerate(homogeneou_matix):
            # 同時変換行列から位置を取得
            pos = matrix[:self._DIMENTION_POSE, self._ROTATION_MAT_ELEMENT].reshape(1, -1)
            all_link_pose[i] = pos

        # 最後のリンクの座標系の原点から，手先の位置を計算する
        pos = np.ones(self._HOMOGENEOU_MAT_ELEMENT)
        pos[:DIMENTION_3D] = self._relative_positions[-1]
        all_link_pose[-1]  = np.dot(homogeneou_matix[-1], pos)[:self._DIMENTION_POSE].reshape(1, -1)

        return all_link_pose

    def update(self, thetas):
        """
        角度を与えて，各リンクの直方体を更新する

        パラメータ
            thetas(numpy.ndarray): ロボットの関節角度 [rad]
        """
        # パラメータの次元数を確認
        if np.size(thetas) != self._DIMENTION_THETA:
            raise ValueError(f"thetas's size is abnormal. thetas's size is {np.size(thetas)}")

        # 順運動学により，全リンク(ベースリンク，リンク1，リンク2)の位置を計算
        all_link_pose = self.forward_kinematics_all_link_pos(thetas)
        # 1つ前のリンクの回転行列を定義
        prev_rotation = np.eye(self._ROTATION_MAT_ELEMENT)

        # ロボットの各リンクを直方体として定義する
        for i in range(self._DIMENTION_THETA):
            # 各リンクの回転行列を定義
            rotation = self._rot.rot(thetas[i], self._axiss[i])
            # 1つ前のリンクの回転も考慮する
            rotation = np.dot(prev_rotation, rotation)
            # 各リンクの中心位置 (x, y, z) を定義
            center = np.zeros(DIMENTION_3D)
            center[:self._DIMENTION_POSE] = (all_link_pose[i + 1] - all_link_pose[i]) / 2 + all_link_pose[i]
            # 直方体の中心を定義 (位置・姿勢)
            translation = fcl.Transform(rotation, center)
            # モデルの位置を更新
            self._objects[i].setTransform(translation)
            # 1つ前のリンクの回転行列を更新
            prev_rotation = rotation

        # AABBを更新
        self._manager.update()



class Robot3DoF(Robot):
    """
    3軸ロボットクラス
    
    プロパティ
        _links(numpy.ndarray): ロボットのリンク長
        _rot(Rotation): 回転行列クラス
        _axiss(list): 関節の回転軸
    
    メソッド
        public
            forward_kinematics(): 順運動学 (ロボットの関節角度からロボットの手先位置を算出)
            inverse_kinematics(): 逆運動学 (ロボットの手先位置からロボットの関節角度を算出)
            forward_kinematics_all_pos(): 順運動学で全リンクの位置を取得
            update(): 角度を与えて，各リンクの直方体を更新する
    """
    # 定数の定義
    _DIMENTION_POSE  = DIMENTION_3D         # 手先位置の次元数
    _DIMENTION_THETA = DIMENTION_3D         # 関節角度の次元数
    _DIMENTION_LINK  = DIMENTION_3D         # リンク数
    
    _THETA1_XY_THRESHOLD   = 1e-4           # theta1算出時のx, y閾値
    
    
    def __init__(self, links):
        """
        コンストラクタ

        パラメータ
            links(numpy.ndarray): ロボットのリンク長 [m]
        """
        # 親クラスの初期化
        super().__init__(links)
        # ロボットの各リンクを直方体として定義する
        self._objects = []
        # (全リンクの角度を0とした時の) 各リンク間の相対位置
        self._relative_positions = np.zeros((self._DIMENTION_POSE + 1, 3))
        self._relative_positions[0] = np.array([0, 0, 0])
        self._relative_positions[1] = np.array([0,              0, self._links[0]])
        self._relative_positions[2] = np.array([self._links[1], 0, 0             ])
        self._relative_positions[3] = np.array([self._links[2], 0, 0             ])

        # リンク1は回転軸がz軸，リンク2とリンク3は回転軸がy軸である
        self._axiss = [ROTATION_Z_AXIS, ROTATION_Y_NEGATIVE_AXIS, ROTATION_Y_NEGATIVE_AXIS]
        # 初期角度
        initial_thetas = np.zeros(self._DIMENTION_THETA)
        # 順運動学により，全リンク(ベースリンク，リンク1，リンク2，リンク3)の位置を計算
        all_link_pose = self.forward_kinematics_all_link_pos(initial_thetas)

        # 1つ前のリンクの回転行列を更新
        prev_rotation = np.eye(self._ROTATION_MAT_ELEMENT)

        # 各リンクの角度が全部0の時のx, y, zを定義
        box_xyz = [[self._BOX_WIDTH * 2, self._BOX_WIDTH * 2, self._links[0]     ],
                   [self._links[1],      self._BOX_WIDTH * 2, self._BOX_WIDTH * 2],
                   [self._links[2],      self._BOX_WIDTH * 2, self._BOX_WIDTH * 2]]

        # ロボットの各リンクを直方体として定義する
        for i in range(self._DIMENTION_THETA):
            # 各リンクの回転行列を定義
            rotation = self._rot.rot(initial_thetas[i], self._axiss[i])
            rotation = np.dot(prev_rotation, rotation)
            # 各リンクの中心位置 (x, y, z) を定義
            center = (all_link_pose[i + 1] - all_link_pose[i]) / 2 + all_link_pose[i]
            # 直方体の定義 (x, y, zの長さを保存)
            x, y, z = box_xyz[i]
            box = fcl.Box(x, y, z)
            # 直方体の中心を定義 (位置・姿勢)
            translation = fcl.Transform(rotation, center)
            obj = fcl.CollisionObject(box, translation)
            # モデルを追加
            self._objects.append(obj)
            # 1つ前のリンクの回転行列を更新
            prev_rotation = rotation

        # 直方体をAABBとして，定義
        # DynamicAABBTreeCollisionManager に登録
        self._manager = fcl.DynamicAABBTreeCollisionManager()
        self._manager.registerObjects(self._objects)
        self._manager.setup()

    def forward_kinematics(self, thetas):
        """
        順運動学 (ロボットの関節角度からロボットの手先位置を算出)

        パラメータ
            thetas(numpy.ndarray): ロボットの関節角度 [rad]
        
        戻り値
            pose(numpy.ndarray): ロボットの手先位置 (位置) [m]
        """
        # パラメータの次元数を確認
        if np.size(thetas) != self._DIMENTION_THETA:
            raise ValueError(f"thetas's size is abnormal. thetas's size is {np.size(thetas)}")

        # 同時変換行列(3 * 4 * 4)を計算する
        homogeneou_matrix = self._calc_homogeneou_matrix(thetas)

        # 最終リンクの同時変換行列(最終リンク座標の位置・姿勢)より，手先位置を計算する
        final_link_matrix = homogeneou_matrix[self._DIMENTION_LINK - 1]
        # 最終リンクから手先位置までの早退位置(4ベクトル)を定義
        relative_pos = np.ones(self._HOMOGENEOU_MAT_ELEMENT)
        relative_pos[:self._HOMOGENEOU_MAT_ELEMENT - 1] = self._relative_positions[-1]
        pose = np.dot(final_link_matrix, relative_pos)
        # 手先位置(x, y)を取得
        pose = pose[:self._DIMENTION_POSE]

        return pose

    def differential_inverse_kinematics(self, thetas, target_pos):
        """
        2点間の微分逆運動学

        パラメータ
            thetas(numpy.ndarray): ロボットの関節角度 [rad]
            target_pos(numpy.ndarray): 目標位置 (位置[m]・姿勢[rad])
        
        戻り値
            target_thetas(numpy.ndarray): 目標位置の関節角度 [rad]
        """
        # 引数の確認
        if np.size(thetas) != self._DIMENTION_THETA:
            # 異常
            raise ValueError(f"theta's size is abnormal. theta's size is {np.size(thetas)}")

        # 現在の関節角度を保存 (異なるアドレスにデータを保存するためにnp.copy()を採用)
        current_thetas = np.copy(thetas)

        # 目標値の関節角度が見つかったどうか
        success_flg = False

        # 微分逆行列で取得したデータを初期化
        self._reset_jacov_thetas()
        # 計算した角度を保存する
        self._jacov_thetas.append(current_thetas)

        # 目標位置に近づくまでループ
        for i in range(self._JACOV_MAX_COUNT):
            # ヤコビ行列の取得
            jacovian = self._jacovian(current_thetas)

            # dP(位置の差分) = J(ヤコビ行列) * dTheta(角度の差分)
            # dTheta = J^(-1)(ヤコビ行列の逆行列) * dP
            # 行列式が0近傍であるかの確認(ヤコビ行列の逆行列の存在確認)
            det = np.linalg.det(jacovian)
            if abs(det) <= self._DETERMINANT_THRESHOLD:
                # 0近傍であるため，逆行列が存在しない
                raise ValueError(f"abs(det) is near 0. abs(det) is {abs(det)}")

            # 現在の手先位置を計算
            current_pos = self.forward_kinematics(current_thetas)
            # 位置の差分を計算
            dP = target_pos - current_pos
            # dTheta = J^(-1)(ヤコビ行列の逆行列) * dP
            dTheta = np.dot(np.linalg.inv(jacovian), dP)

            # 関節角度の更新 (+=とするとcurrent_thetasが常に同じアドレスになるため，current_thetasを異なるアドレスとしたい)
            # print(f"id(current_thetas) = {id(current_thetas)}")   # アドレス確認
            # current_thetas = current_thetas + dTheta * self._JACOV_DELTA_TIME
            # 位置の差分が大きいほど，角度の更新量を小さくしたい
            coefficient = 1 / max(0.1, np.linalg.norm(dP))
            current_thetas = current_thetas + dTheta * coefficient * self._JACOV_DELTA_TIME
            # print(f"id(current_thetas) = {id(current_thetas)}")   # アドレス確認

            # 計算した角度を保存する
            self._jacov_thetas.append(current_thetas)

            # 更新後の手先位置が目標位置の近傍であれば，処理終了とする
            current_pos = self.forward_kinematics(current_thetas)
            distance = np.linalg.norm(target_pos - current_pos)
            # print(f"distance = {distance}")
            if distance <= self._JACOV_NEAR_POS:
                # 近傍のため，処理終了
                success_flg = True
                break

        if not success_flg:
            # 目標位置の関節角度が見つからない
            raise ValueError(f"target_pos's theta is not found. please change target_pos")

        target_thetas = current_thetas

        return target_thetas

    def _jacovian(self, thetas):
        """
        ヤコビ行列の計算

        パラメータ
            thetas(numpy.ndarray): ロボットの関節角度 [rad]
        
        戻り値
            jacovian(numpy.ndarray): ヤコビ行列
        """
        # 引数の確認
        if np.size(thetas) != self._DIMENTION_THETA:
            # 異常
            raise ValueError(f"theta's size is abnormal. theta's size is {np.size(thetas)}")

        # jacovian = [[-l1 * sin(theta1) - l2 * sin(theta12), -l2 * sin(theta12)], [l1 * cos(theta1) + l2 * cos(theta12), l2 * cos(theta12)]]
        # 各リンクの長さをローカル変数に保存
        l1 = self._links[0]
        l2 = self._links[1]
        l3 = self._links[2]

        # 三角関数の計算
        sin1  = np.sin(thetas[0])
        cos1  = np.cos(thetas[0])
        sin2  = np.sin(thetas[1])
        cos2  = np.cos(thetas[1])
        sin23 = np.sin(thetas[1] + thetas[2])
        cos23 = np.cos(thetas[1] + thetas[2])

        # ヤコビ行列
        jacovian =    np.array([[-sin1 * (l2 * cos2 + l3 * cos23), -cos1 * (l2 * sin2 + l3 * sin23), -cos1 * l3 * sin23],
                                [ cos1 * (l2 * cos2 + l3 * cos23), -sin1 * (l2 * sin2 + l3 * sin23), -sin1 * l3 * sin23],
                                [ 0                              ,          l2 * cos2 + l3 * cos23 ,         l3 * cos23]])

        return jacovian

    def inverse_kinematics(self, pose, upper=False, front=True):
        """
        逆運動学 (ロボットの手先位置からロボットの関節角度を算出)

        パラメータ
            pose(numpy.ndarray): ロボットの手先位置 (位置) [m]
            upper(bool): 腕が上向かどうか
            front(bool): 正面かどうか
        
        戻り値
            thetas(numpy.ndarray): ロボットの関節角度 [rad]
        """
        # パラメータの次元数を確認
        if np.size(pose) != self._DIMENTION_POSE:
            raise ValueError(f"parameter pose's size is abnormal. pose's size is {np.size(pose)}")

        # はじめにtheta3を算出する
        # c3 = {(px ** 2 + py ** 2 + (pz - l1) ** 2) - (l2 ** 2 + l3 ** 2)} / (2 * l2 * l3)
        px = pose[0]
        py = pose[1]
        pz = pose[2]
        l1 = self._links[0]
        l2 = self._links[1]
        l3 = self._links[2]
        cos3  = ((px ** 2 + py ** 2 + (pz - l1) ** 2) - (l2 ** 2 + l3 ** 2)) / (2 * l2 * l3)
        # cosの範囲は-1以上1以下である
        if abs(cos3) > 1:
            # 異常
            raise ValueError(f"cos3 is abnormal. cos3 is {cos3}")

        # sinも求めて，theta3をatan2(sin2, cos2)より算出する
        sin3 = np.sqrt(1 - cos3 ** 2)
        theta3 = np.arctan2(sin3,  cos3)
        if not upper:
            # 下向きの角度のため，三角関数も更新
            theta3 = -theta3
            sin3 = np.sin(theta3)
            cos3 = np.cos(theta3)

        # 次にtheta1を算出する
        # theta1 = atan2(py, px)
        # py, pxが0近傍なら失敗
        if abs(py) <= self._THETA1_XY_THRESHOLD and abs(px) <= self._THETA1_XY_THRESHOLD:
            # 近傍のため，エラー
            raise ValueError(f"abs(py) and abs(px) is abnormal. py is {py}, px is {px}")
        if front:
            # 位置(pose)に対して正面向き
            theta1 = np.arctan2( py,  px)
        else:
            # 位置(pose)に対して後ろ向き
            theta1 = np.arctan2(-py, -px)

        # 最後にtheta2を算出する
        # 行列計算
        # [c2, s2] = [[l2 + l3 * c3, -l3 * s3], [l3 * s3, l2 + l3 * c3]] ** -1 * [root(px ** 2 + py ** 2), pz - l1]
        element1 =  l2 + l3 * cos3
        element2 = -l3 * sin3
        matrix = np.array([[ element1, element2],
                           [-element2, element1]])
        # 行列式を計算
        det = np.linalg.det(matrix)
        # 0近傍の確認
        if abs(det) <= self._DETERMINANT_THRESHOLD:
            # 0近傍 (異常)
            raise ValueError(f"det is abnormal. det is {det}")

        # 位置を保存 ([root(px ** 2 + py ** 2), pz - l1])
        position = np.array([np.sqrt(px ** 2 + py ** 2), pz - l1])

        # [c2, s2]の計算
        cos2_sin2 = np.dot(np.linalg.inv(matrix), position)
        # theta2をatan2()より算出する
        theta2 = np.arctan2(cos2_sin2[1], cos2_sin2[0])

        thetas = np.array([theta1, theta2, theta3])

        return thetas

    def forward_kinematics_all_link_pos(self, thetas):
        """
        順運動学で全リンクの位置を取得 (グラフの描画で使用する)

        パラメータ
            thetas(numpy.ndarray): ロボットの関節角度 [rad]
        
        戻り値
            all_link_pose(numpy.ndarray): ロボットの全リンク位置 (位置 + 姿勢) [m] + [rad]
        """
        # パラメータの次元数を確認
        if np.size(thetas) != self._DIMENTION_THETA:
            raise ValueError(f"thetas's size is abnormal. thetas's size is {np.size(thetas)}")

        # 同時変換行列(3 * 4 * 4)を計算する
        homogeneou_matix = self._calc_homogeneou_matrix(thetas)

        # 全リンクの座標系の原点を取得
        all_link_pose = np.zeros((self._DIMENTION_LINK + 1, self._DIMENTION_POSE))
        for i, matrix in enumerate(homogeneou_matix):
            # 同時変換行列から位置を取得
            pos = matrix[:self._DIMENTION_POSE, self._ROTATION_MAT_ELEMENT].reshape(1, -1)
            all_link_pose[i] = pos

        # 最後のリンクの座標
        # 最後のリンクの座標系の原点から，手先の位置を計算する
        pos = np.ones(self._HOMOGENEOU_MAT_ELEMENT)
        pos[:DIMENTION_3D] = self._relative_positions[-1]
        all_link_pose[-1]  = np.dot(homogeneou_matix[-1], pos)[:self._DIMENTION_POSE].reshape(1, -1)

        return all_link_pose

    def update(self, thetas):
        """
        角度を与えて，各リンクの直方体を更新する

        パラメータ
            thetas(numpy.ndarray): ロボットの関節角度 [rad]
        """
        # パラメータの次元数を確認
        if np.size(thetas) != self._DIMENTION_THETA:
            raise ValueError(f"thetas's size is abnormal. thetas's size is {np.size(thetas)}")

        # 順運動学により，全リンク(ベースリンク，リンク1，リンク2，リンク3)の位置を計算
        all_link_pose = self.forward_kinematics_all_link_pos(thetas)
        # 1つ前のリンクの回転行列を定義
        prev_rotation = np.eye(self._ROTATION_MAT_ELEMENT)

        # ロボットの各リンクを直方体として定義する
        for i in range(self._DIMENTION_THETA):
            # 各リンクの回転行列を定義
            rotation = self._rot.rot(thetas[i], self._axiss[i])
            # 1つ前のリンクの回転も考慮する
            rotation = np.dot(prev_rotation, rotation)
            # 各リンクの中心位置 (x, y, z) を定義
            center = (all_link_pose[i + 1] - all_link_pose[i]) / 2 + all_link_pose[i]
            # 直方体の中心を定義 (位置・姿勢)
            translation = fcl.Transform(rotation, center)
            # モデルの位置を更新
            self._objects[i].setTransform(translation)
            # 1つ前のリンクの回転行列を更新
            prev_rotation = rotation

        # AABBを更新
        self._manager.update()

RRT*-Connect のアニメーション (animation.py)

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

animation.py

# ロボットのアニメーションを実施

# ライブラリの読み込み
import numpy as np      # 数値計算
import matplotlib.pyplot as plt     # 描画用
import matplotlib.animation as ani  # アニメーション用
import matplotlib.patches as patches    # 2次元形状の描画
from mpl_toolkits.mplot3d.art3d import Poly3DCollection     # 3次元形状の描画
import logging          # ログ用
import os               # フォルダ作成用


# 自作モジュールの読み込み
from constant import *              # 定数
from rotation import MyRotation     # 回転行列


LOG_FLG = True       # ログ出力フラグ
logging.basicConfig(level=logging.INFO)    # デバッグ情報をログ出力できるように修正


class RRTAnimation:
    """
    RRTのアニメーション作成

    プロパティ
        _figure: 描画枠
        _axis: 描画内容
        _path_plan: アニメーションできる経路生成手法名
        _rot: 回転行列
        _log_flg: ログ出力フラグ


    メソッド
        public
            plot(): グラフの描画
            plot_Animation(): アニメーションの描画


        protected
            ログ出力関連
                _log(): ログ出力

            設定関連
                _reset_2d(): 2次元データの描画内容をリセット
                _set_2d_axis(): 2次元データのラベルなどを設定
                _reset_3d(): 3次元データの描画内容をリセット
                _set_3d_axis(): 3次元データのラベルなどを設定

            環境関連
                _plot_environment(): 環境の描画
                _plot_circle(): 円の描画
                _plot_rectangle(): 長方形の描画
                _plot_ball(): 球の描画
                _plot_cuboid(): 直方体の描画
                _plot_2d_ellipse(): 2次元の楕円をプロット
                _plot_3d_ellipse(): 3次元の楕円をプロット

            ロボット関連
                _plot_robot(): ロボットの描画 (グラフ用)
                _plot_robot_animation(): ロボットの描画 (アニメーション用)
                _plot_robot_1data(): ロボットを1データ分描画
                _convert_pos_to_theta(): データを位置空間から関節空間に変換
                _convert_theta_to_pos(): データを関節空間から位置空間に変換
                _convert_multi_theta_to_pos(): 複数データを位置空間から関節空間に変換
                _convert_multi_pos_to_theta(): 複数データを位置空間から関節空間に変換

            グラフ描画関連
                _get_file_name(): ファイル名の取得
                _plot_2d(): グラフの描画 (2次元)
                _plot_3d(): グラフの描画 (3次元)
                _plot_all_node_parent(): ツリー内の全ノードを親ノードと一緒に描画
                _plot_start_end_node(): 始点ノードと終点ノードをプロット
                _plot_2d_tree(): ツリーの描画 (2次元)
                _plot_3d_tree(): ツリーの描画 (3次元)
                _plot_2d_pathes(): 始点から終点までの最終的な経路をプロット (2次元)
                _plot_3d_pathes(): 始点から終点までの最終的な経路をプロット (3次元)

            アニメーション描画関連
                _plot_2d_animation(): アニメーションの描画 (2次元)
                _plot_3d_animation(): アニメーションの描画 (3次元)
                _update_2d_data(): 1フレーム分のデータを描画 (2次元)
                _update_3d_data(): 1フレーム分のデータを描画 (2次元)
                _plot_2d_path_animation(): 始点から終点までの経路を描画 (2次元アニメーション用)
                _plot_3d_path_animation(): 始点から終点までの経路を描画 (3次元アニメーション用)
                _plot_2d_tree_animation(): ツリーの描画 (2次元アニメーション用)
                _plot_3d_tree_animation(): ツリーの描画 (3次元アニメーション用)
    """
    # 定数の定義
    _NEAR_NODE_IDX = RRT_NEAR_NODE_IDX    # ノード内の最短ノード要素

    # 干渉物に関する定数
    _INTERFERENCE_COLOR = "gray"    # 干渉物の色
    _INTERFERENCE_ALPHA = 0.5       # 干渉物の色の濃さ (0 ~ 1)
    _RADIUS_TO_DIAMETER = 2         # 半径から直径への変換

    # 描画時の色
    _START_POS_COLOR = "cyan"     # 始点の色
    _END_POS_COLOR = "lime"     # 終点の色

    # ロボットに関する定数
    _ROBOT_LINK_COLOR = "black"    # ロボットのリンクに関する色
    _ROBOT_JOINT_COLOR = "black"    # ロボットの関節に関する色

    # ファイルの拡張子に関する定数
    _FILE_NAME_GRAPH_EXTENTION = "png"  # グラフファイルの拡張子
    _FILE_NAME_ANIME_EXTENSION = "gif"  # アニメーションファイルの拡張子

    # アニメーション
    _ANIMATION_MARGIN = 5   # アニメーションのマージン
    _ANIMATION_INTERBAL = 500   # アニメーションのインターバル [msec]


    def __init__(self):
        """
        コンストラクタ
        """
        # プロパティの初期化
        self._path_plan = PATHPLAN.RRT.value
        self._rot = MyRotation()
        self._log_flg = LOG_FLG


    # ログ出力関連のメソッド ↓
    def _log(self, data):
        """
        ログ出力

        パラメータ
            data(): ログ出力したいデータ
        """
        if self._log_flg:
            # ログ出力する
            logging.info(data)
        else:
            # ログ出力しない
            pass
    # ログ出力関連のメソッド ↑


    # 設定関連のメソッド ↓
    def _reset_2d(self):
        """
        2次元データの描画内容をリセット
        """
        self._figure = plt.Figure()
        self._axis   = self._figure.add_subplot(111)

        # X/Y軸に文字を記載
        self._axis.set_xlabel("X [m]")
        self._axis.set_ylabel("Y [m]")

    def _set_2d_axis(self, min_pos, max_pos):
        """
        2次元データのラベルなどを設定

        パラメータ
            min_pos(numpy.ndarray): 描画範囲の最小値 (x, y)
            max_pos(numpy.ndarray): 描画範囲の最大値 (x, y)
        """
        # X/Y軸に文字を記載
        self._axis.set_xlabel("X [m]")
        self._axis.set_ylabel("Y [m]")

        # X/Y軸の最小値・最大値を設定
        self._axis.set_xlim(min_pos[0], max_pos[0])
        self._axis.set_ylim(min_pos[1], max_pos[1])

        self._axis.grid()
        self._axis.set_aspect("equal")

    def _reset_3d(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, 20)
        theta_2_0 = np.linspace(0, np.pi * 2, 20)
        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_3d_axis(self, min_pos, max_pos):
        """
        3次元データのラベルなどを設定

        パラメータ
            min_pos(numpy.ndarray): 描画範囲の最小値 (x, y, z)
            max_pos(numpy.ndarray): 描画範囲の最大値 (x, y, z)
        """
        # X/Y/Z軸に文字を記載
        self._axis.set_xlabel("X [m]")
        self._axis.set_ylabel("Y [m]")
        self._axis.set_zlabel("Z [m]")

        # X/Y/Z軸の最小値・最大値を設定
        self._axis.set_xlim(min_pos[0], max_pos[0])
        self._axis.set_ylim(min_pos[1], max_pos[1])
        self._axis.set_zlim(min_pos[2], max_pos[2])

        self._axis.grid()
        self._axis.set_aspect("equal")
    # 設定関連のメソッド ↑


    # 環境関連のメソッド ↓
    def _plot_environment(self, environment):
        """
        環境の描画

        パラメータ
            environment(envirionment.py内のクラス): 環境
        """
        if environment is None:
            # 環境がないため，何もしない
            return

        for name, datas in environment.interferences.items():
            # 干渉物の名称
            if name == INTERFERENCE.CIRCLE.value:
                # 円
                for x, y, radius in datas:
                    self._plot_circle(x, y, radius)
            elif name == INTERFERENCE.RECTANGLE.value:
                # 長方形
                for x, y, center_x, center_y, angle in datas:
                    # 中心点から左隅の点に移動させるために，-x/2，-y/2を実施
                    self._plot_rectangle(center_x - x / 2, center_y - y / 2, x, y, angle)
            elif name == INTERFERENCE.BALL.value:
                # 球
                for x, y, z, radius in datas:
                    self._plot_ball(np.array([x, y, z]), radius)
            elif name == INTERFERENCE.CUBOID.value:
                # 直方体
                for x, y, z, center_x, center_y, center_z, angle_x, angle_y, angle_z in datas:
                    # 回転行列の作成
                    rotation_x = self._rot.rot(np.deg2rad(angle_x), ROTATION_X_AXIS)
                    rotation_y = self._rot.rot(np.deg2rad(angle_y), ROTATION_Y_AXIS)
                    rotation_z = self._rot.rot(np.deg2rad(angle_z), ROTATION_Z_AXIS)
                    rotation   = np.dot(rotation_z, np.dot(rotation_y, rotation_x))

                    # 描画
                    self._plot_cuboid(np.array([center_x, center_y, center_z]), x, y, z)
            else:
                # 何もしない
                pass

    def _plot_circle(self, x, y, radius, color=_INTERFERENCE_COLOR, alpha=_INTERFERENCE_ALPHA, fill=True):
        """
        円の描画

        パラメータ
            x(float): 中心点 (x)
            y(float): 中心点 (y)
            radius(float): 半径
            fill(bool): 円の内部を塗りつぶすかどうか
            color(str): 色
            alpha(float): 色の濃さ (0 ~ 1)
        """
        # 半径が0以下では描画しない
        if radius <= 0:
            return

        circle = patches.Circle((x, y), radius, color=color, alpha=alpha, fill=fill)
        self._axis.add_patch(circle)

    def _plot_rectangle(self, x, y, width, height, angle, color=_INTERFERENCE_COLOR, alpha=_INTERFERENCE_ALPHA):
        """
        長方形の描画

        パラメータ
            x(float): 左下隅のx座標
            y(float): 左下隅のy座標
            width(float): 幅
            height(float): 高さ
            angle(float): 角度 [deg]
            color(str): 色
            alpha(float): 色の濃さ (0 ~ 1)
        """
        # 幅と高さが0以下では描画しない
        if width <= 0 or height <= 0:
            return

        rect = patches.Rectangle((x, y), width, height, angle=angle, color=color, alpha=alpha)
        self._axis.add_patch(rect)

    def _plot_ball(self, center, radius, color=_INTERFERENCE_COLOR, alpha=_INTERFERENCE_ALPHA):
        """
        球の描画

        パラメータ
            center(np.ndarray): 中心位置 (x, y, z)
            radius(float): 半径
            color(str): 色
            alpha(float): 色の濃さ (0 ~ 1)
        """
        # 半径が0以下では描画しない
        if radius <= 0:
            return

        self._axis.plot_wireframe(self._x * radius + center[0], self._y * radius + center[1], self._z * radius + center[2], color=color, alpha=alpha)

    def _plot_cuboid(self, center, x, y, z, color=_INTERFERENCE_COLOR, alpha=_INTERFERENCE_ALPHA):
        """
        直方体の描画

        パラメータ
            center(numpy.ndarray): 中心位置 (x, y, z)
            x(float): 直方体のx軸の長さ
            y(float): 直方体のy軸の長さ
            z(float): 直方体のz軸の長さ
            rotation(numpy.ndarray): 回転行列
        """
        # 直方体の長さが0以下では描画しない
        if x <= 0 or y <= 0 or z <= 0:
            return

        # 直方体の頂点を算出する
        points  = np.array([[center[0] - x / 2, center[1] - y / 2, center[2] - z / 2],
                            [center[0] + x / 2, center[1] - y / 2, center[2] - z / 2],
                            [center[0] + x / 2, center[1] + y / 2, center[2] - z / 2],
                            [center[0] - x / 2, center[1] + y / 2, center[2] - z / 2],
                            [center[0] - x / 2, center[1] - y / 2, center[2] + z / 2],
                            [center[0] + x / 2, center[1] - y / 2, center[2] + z / 2],
                            [center[0] + x / 2, center[1] + y / 2, center[2] + z / 2],
                            [center[0] - x / 2, center[1] + y / 2, center[2] + z / 2]])

        # 頂点の4点から面を算出する
        verts =    [[points[0], points[1], points[2], points[3]],
                    [points[4], points[5], points[6], points[7]],
                    [points[0], points[1], points[5], points[4]],
                    [points[2], points[3], points[7], points[6]],
                    [points[1], points[2], points[6], points[5]],
                    [points[4], points[7], points[3], points[0]]]

        # 直方体の描画
        self._axis.add_collection3d(Poly3DCollection(verts, facecolors=color, edgecolors=color, alpha=alpha))

    def _plot_2d_ellipse(self, center, radius, theta, color=_INTERFERENCE_COLOR, alpha=_INTERFERENCE_ALPHA, fill=True):
        """
        2次元の楕円をプロット

        パラメータ
            center(numpy.ndarray): 楕円の中心点
            radius(numpy.ndarray): 楕円の半径
            theta(float): 楕円の回転角度 [rad]
            color(str): 色
            alpha(float): 色の濃さ (0.0 ~ 1.0)
        """
        # 半径が0以下では描画しない
        if radius.any() <= 0:
            return

        # 楕円の中身は塗りつぶさない (描画時に半径から直径にすると整合性が取れる)
        ellipse = patches.Ellipse(center, radius[0] * self._RADIUS_TO_DIAMETER, radius[1] * self._RADIUS_TO_DIAMETER, theta, color=color, alpha=alpha, fill=fill)
        self._axis.add_patch(ellipse)

    def _plot_3d_ellipse(self, center, radius, rotation, color=_INTERFERENCE_COLOR, alpha=_INTERFERENCE_ALPHA):
        """
        3次元の楕円をプロット

        パラメータ
            center(numpy.ndarray): 楕円の中心点 (x, y, z)
            radius(numpy.ndarray): 楕円の半径 (x, y, z)
            rotation(numpy.ndarray): 楕円の回転行列 (3 * 3行列)
            color(str): 色
            alpha(float): 色の濃さ (0.0 ~ 1.0)
        """
        # 半径が0以下では描画しない
        if radius.any() <= 0:
            return

        x = self._x * radius[0]
        y = self._y * radius[1]
        z = self._z * radius[2]

        # 座標をベクトル形式に変換
        # shape: (3, N)
        points = np.stack([x.flatten(), y.flatten(), z.flatten()], axis=0)
        rotated_points = np.dot(rotation, points)

        # 元の形に戻す（メッシュグリッドと同じ形状）
        x_rot = rotated_points[0, :].reshape(x.shape)
        y_rot = rotated_points[1, :].reshape(y.shape)
        z_rot = rotated_points[2, :].reshape(z.shape)

        self._axis.plot_wireframe(x_rot + center[0], y_rot + center[1], z_rot + center[2], color=color, alpha=alpha)
    # 環境関連のメソッド ↑


    # ロボット関連のメソッド ↓
    def _plot_robot(self, rrt):
        """
        ロボットの描画 (グラフ用)

        パラメータ
            rrt(rrt.py内のクラス): 経路生成手法クラス
        """
        # パラメータrrtから，ロボットクラスをローカル変数に保存
        robot = rrt.robot
        if robot is None:
            # 処理終了
            return

        # 始点から終点までの経路を位置空間から関節空間に変換
        pathes = self._convert_multi_pos_to_theta(robot, rrt.pathes, rrt.interpolation)

        # 始点から終点までの経路を全部プロット
        for path in pathes:
            # ロボットのプロット
            self._plot_robot_1data(robot, path, rrt.dim)

    def _plot_robot_animation(self, i, rrt):
        """
        ロボットの描画 (アニメーション用)

        パラメータ
            i(int): フレーム番号 (ツリーのノード数 + 始点から終点までの経由点数)
            rrt(rrt.py内のクラス): 経路生成手法クラス
        """
        # パラメータrrtから，ロボットクラスをローカル変数に保存
        robot = rrt.robot
        if robot is None:
            # 処理終了
            return

        if i < 0:
            # 異常
            return

        if rrt.debug:
            # デバッグありのため，始点から終点に関係しないノードも使用
            if i < rrt.start_tree.nodes.shape[0]:
                # ツリー内のノードをプロット
                theta = self._convert_pos_to_theta(robot, rrt.start_tree.nodes[i, :self._NEAR_NODE_IDX], rrt.interpolation)
            else:
                # 始点から終点までの経由点を使用
                n_waypoint = i - rrt.start_tree.nodes.shape[0]
                if n_waypoint < rrt.pathes.shape[0]:
                    # n_waypoint番目の経由点をプロット
                    theta = self._convert_pos_to_theta(robot, rrt.pathes[n_waypoint], rrt.interpolation)
                else:
                    # 終点をプロット
                    theta = self._convert_pos_to_theta(robot, rrt.pathes[-1], rrt.interpolation)
        else:
            # デバッグなしのため，始点から終点までの経由点を使用
            if i < rrt.pathes.shape[0]:
                # i番目の経由点をプロット
                theta = self._convert_pos_to_theta(robot, rrt.pathes[i], rrt.interpolation)
            else:
                # 終点をプロット
                theta = self._convert_pos_to_theta(robot, rrt.pathes[-1], rrt.interpolation)

        # ロボットのプロット
        self._plot_robot_1data(robot, theta, rrt.dim)

    def _plot_robot_1data(self, robot, theta, dim, color="blue", alpha=0.2):
        """
        ロボットを1データ分描画

        パラメータ
            robot(robot.py内のクラス): ロボットクラス
            theta(numpy.ndarray): 1点分のデータ
        """
        # 関節角度から，全リンク位置を計算
        all_link_pos = robot.forward_kinematics_all_link_pos(theta)
        if dim == DIMENTION_2D:     # 2次元
            # 線をプロット
            self._axis.plot(   all_link_pos[:, 0], all_link_pos[:, 1], color=color, alpha=alpha)
            # 点をプロット
            self._axis.scatter(all_link_pos[:, 0], all_link_pos[:, 1], color=color, alpha=alpha)
        else:       # 3次元
            # 線をプロット
            self._axis.plot(   all_link_pos[:, 0], all_link_pos[:, 1], all_link_pos[:, 2], color=color, alpha=alpha)
            # 点をプロット
            self._axis.scatter(all_link_pos[:, 0], all_link_pos[:, 1], all_link_pos[:, 2], color=color, alpha=alpha)

    def _convert_pos_to_theta(self, robot, path, interpolation):
        """
        データを位置空間から関節空間に変換

        パラメータ
            robot(robot.py内のクラス): ロボットクラス
            path(numpy.ndarray): 経路上の経由点
            interpolation(int): 補間方法

        戻り値
            theta(numpy.ndarray): 関節
        """
        if robot is None:
            # ロボットクラスが存在しないため，そのままpathを返す
            theta = np.copy(path)
        else:
            if interpolation == INTERPOLATION.POSITION.value:
                # 位置空間のため，逆運動学より関節空間に変換
                theta = robot.inverse_kinematics(path)
            else:
                # 関節空間のため，成形せずそのままpathを返す
                theta = np.copy(path)

        return theta

    def _convert_theta_to_pos(self, robot, path, interpolation):
        """
        データを関節空間から位置空間に変換

        パラメータ
            robot(robot.py内のクラス): ロボットクラス
            path(numpy.ndarray): 経路上の経由点
            interpolation(int): 補間方法

        戻り値
            pos(numpy.ndarray): 位置
        """
        if robot is None:
            # ロボットクラスが存在しないため，そのままpathを返す
            pos = np.copy(path)
        else:
            if interpolation == INTERPOLATION.JOINT.value:
                # 関節空間のため，順運動学より位置空間に変換
                pos = robot.forward_kinematics(path)
            else:
                # 位置空間のため，成形せずそのままpathを返す
                pos = np.copy(path)

        return pos

    def _convert_multi_theta_to_pos(self, robot, pathes, interpolation):
        """
        複数データを関節空間から位置空間に変換

        パラメータ
            robot(robot.py内のクラス): ロボットクラス
            pathes(numpy.ndarray): 変換したい複数データ
            interpolation(int): 補間手法 (位置空間/関節空間)

        戻り値
            pos_pathes(numpy.ndarray): 位置空間
        """
        if robot is None:
            # ロボットが存在しないため，変換せずそのまま返す
            pos_pathes = np.copy(pathes)
        else:
            if interpolation == INTERPOLATION.JOINT.value:
                # リスト内包表記より，複数データを位置空間へ変換
                pos_pathes = [self._convert_theta_to_pos(robot, path, interpolation) for path in pathes]
                # リスト型からnumpy型に型変換
                pos_pathes = np.array(pos_pathes)
            else:
                # 位置空間であるから，変換せずそのまま返す
                pos_pathes = np.copy(pathes)

        return pos_pathes

    def _convert_multi_pos_to_theta(self, robot, pathes, interpolation):
        """
        複数データを位置空間から関節空間に変換

        パラメータ
            robot(robot.py内のクラス): ロボットクラス
            pathes(numpy.ndarray): 変換したい複数データ
            interpolation(int): 補間手法 (位置空間/関節空間)

        戻り値
            joint_pathes(numpy.ndarray): 関節空間
        """
        if robot is None:
            # ロボットが存在しないため，変換せずそのまま返す
            joint_pathes = np.copy(pathes)
        else:
            if interpolation == INTERPOLATION.POSITION.value:
                # リスト内包表記より，複数データを関節空間へ変換
                joint_pathes = [self._convert_pos_to_theta(robot, path, interpolation) for path in pathes]
                # リスト型からnumpy型に型変換
                joint_pathes = np.array(joint_pathes)
            else:
                # 関節空間であるから，変換せずそのまま返す
                joint_pathes = np.copy(pathes)

        return joint_pathes
    # ロボット関連のメソッド ↑


    # グラフ描画関連のメソッド ↓
    def plot(self, rrt):
        """
        グラフの描画

        パラメータ
            rrt(rrt.py内のクラス): 経路生成手法クラス
        """
        # 念の為，経路生成手法が正しいかの確認
        if rrt.name != self._path_plan:
            # 想定外の経路生成手法であるため，異常
            raise ValueError(f"rrt.name is not {self._path_plan}. rrt.name is {rrt.name}")

        if rrt.dim == DIMENTION_2D:
            # 2次元データ
            self._plot_2d(rrt)
        elif rrt.dim == DIMENTION_3D:
            # 3次元データ
            self._plot_3d(rrt)
        else:
            # 異常
            raise ValueError(f"rrt.dim is abnormal. rrt.dim is {rrt.dim}")

    def _get_file_name(self, rrt, animation=True):
        """
        ファイル名の取得

        パラメータ
            rrt(rrt.py内のクラス): 経路生成手法クラス
            animation(bool): True / False = アニメーション / グラフ

        戻り値
            file_name(str): ファイル名
        """
        # ファイル名を作成するための要素をローカル変数に保存
        if rrt.dim == DIMENTION_2D:
            # 2次元空間の探索
            dim = "2d"
        else:
            # 3次元空間の探索
            dim = "3d"

        if animation:
            # アニメーションの描画
            extension = self._FILE_NAME_ANIME_EXTENSION
            plot = PLOT_ANIMATION
        else:
            # グラフの描画
            extension = self._FILE_NAME_GRAPH_EXTENTION
            plot = PLOT_GRAPH

        # 上記の要素を使用して，ファイル名を作成
        if rrt.debug:
            # デバッグあり
            file_name = f"{self._path_plan}/{self._path_plan}_{dim}_{rrt.interpolation}_debug_{plot}.{extension}"
        else:
            # デバッグなし
            file_name = f"{self._path_plan}/{self._path_plan}_{dim}_{rrt.interpolation}_{plot}.{extension}"

        self._log(f"file_name = {file_name}")

        return file_name

    def _plot_2d(self, rrt):
        """
        グラフの描画 (2次元)

        パラメータ
            rrt(rrt.py内のクラス): 経路生成手法クラス
        """
        # データをリセットする
        self._reset_2d()
        self._set_2d_axis(rrt.environment.plot_min_pos, rrt.environment.plot_max_pos)

        # 環境をプロット
        self._plot_environment(rrt.environment)

        # ロボットをプロット
        self._plot_robot(rrt)

        # 全ノードを親ノード(最短ノード)と一緒にプロット
        if rrt.debug:
            # デバッグ中であれば，全ノード + 最終的な経路をプロット
            self._plot_2d_tree(rrt)
            self._plot_2d_pathes(rrt)
        else:
            # デバッグ中でなければ，最終的な経路だけをプロットする
            self._plot_2d_pathes(rrt)

        # ファイル名の取得
        file_name = self._get_file_name(rrt, animation=False)

        # ファイル保存
        self._figure.savefig(file_name)

        # 描画
        plt.show()

    def _plot_3d(self, rrt):
        """
        グラフの描画 (3次元)

        パラメータ
            rrt(rrt.py内のクラス): 経路生成手法クラス
        """
        # データをリセットする
        self._reset_3d()
        self._set_3d_axis(rrt.environment.plot_min_pos, rrt.environment.plot_max_pos)

        # 環境をプロット
        self._plot_environment(rrt.environment)

        # ロボットをプロット
        self._plot_robot(rrt)

        # 全ノードを親ノード(最短ノード)と一緒にプロット
        if rrt.debug:
            # デバッグ中であれば，全ノード + 最終的な経路をプロット
            self._plot_3d_tree(rrt)
            self._plot_3d_pathes(rrt)
        else:
            # デバッグ中でなければ，最終的な経路だけをプロットする
            self._plot_3d_pathes(rrt)

        # ファイル名の取得
        file_name = self._get_file_name(rrt, animation=False)

        # ファイル保存
        self._figure.savefig(file_name)

        # 描画
        plt.show()

    def _plot_all_node_parent(self, tree_pos, tree_parent, robot, interpolation, dim, color="red", alpha=0.5):
        """
        ツリー内の全ノードを親ノードと一緒に描画

        パラメータ
            tree_pos(numpy.ndarray): ツリー内のノード
            tree_parent(numpy.ndarray): ツリー内の親ノード
            robot(robot.py内のクラス): ロボットクラス
            interpolation(int): 補間方法 (位置空間/関節空間)
            dim(int): 次元数
            color(str): 色
            alpha(float): 色の濃さ (0 ~ 1)
        """
        # パラメータの確認
        if tree_pos.shape[0] != tree_parent.shape[0]:
            # データが異常
            raise ValueError(f"tree_pos.shape[0] and tree_parent.shape[0] are not match. tree_pos.shape[0] is {tree_pos.shape[0]}. tree_parent.shape[0] is {tree_parent.shape[0]}")

        for i in reversed(range(1, tree_pos.shape[0])):
            # 追加の遅いノードから順番にプロットしていく
            now_position  = tree_pos[i]
            near_node_idx = int(tree_parent[i])
            while True:     # 始点をプロットするまでループ
                parent_position = tree_pos[near_node_idx]
                plot_pos = np.append(now_position.reshape(1, -1), parent_position.reshape(1, -1), axis=0)
                # 描画データを関節空間から位置空間へ変換する
                plot_pos = self._convert_multi_theta_to_pos(robot, plot_pos, interpolation)

                # 親ノードとの線をプロット
                if dim == DIMENTION_2D:
                    # 2次元データ (plot()メソッドで値は確認済み)
                    self._axis.plot(plot_pos[:, 0], plot_pos[:, 1], color=color, alpha=alpha)
                else:
                    # 3次元データ
                    self._axis.plot(plot_pos[:, 0], plot_pos[:, 1], plot_pos[:, 2], color=color, alpha=alpha)

                # ノードおよび親ノードの更新
                now_position  = parent_position
                near_node_idx = int(tree_parent[near_node_idx])
                if near_node_idx == INITIAL_NODE_NEAR_NODE:
                    # 始点には，親ノードが存在しないから，while処理を抜ける
                    break

    def _plot_start_end_node(self, rrt):
        """
        始点ノードと終点ノードをプロット

        パラメータ
            rrt(rrt.py内のクラス): 経路生成手法クラス
        """
        # 始点から終点までの最終的な経路をローカル変数に保存
        pathes = rrt.pathes

        # 始点/終点を関節空間から位置空間に変換
        start_pos = self._convert_theta_to_pos(rrt.robot, pathes[0] , rrt.interpolation)
        end_pos   = self._convert_theta_to_pos(rrt.robot, pathes[-1], rrt.interpolation)
        # 始点/終点をプロット
        if rrt.dim == DIMENTION_2D:
            # 2次元データ
            self._axis.scatter(start_pos[0], start_pos[1], color=self._START_POS_COLOR, marker="*")
            self._axis.scatter(  end_pos[0],   end_pos[1], color=self._END_POS_COLOR,   marker="*")
        else:
            # 3次元データ
            self._axis.scatter(start_pos[0], start_pos[1], start_pos[2], color=self._START_POS_COLOR, marker="*")
            self._axis.scatter(  end_pos[0],   end_pos[1],   end_pos[2], color=self._END_POS_COLOR,   marker="*")

    def _plot_2d_tree(self, rrt):
        """
        ツリーの描画 (2次元)

        パラメータ
            rrt(rrt.py内のクラス): 経路生成手法クラス
        """
        # パラメータrrtから使用するデータをローカル変数に保存
        start_tree_nodes  = rrt.start_tree.nodes                 # 始点ツリー
        # ツリーから位置と親ノードを取得
        start_tree_pos    = start_tree_nodes[:, :self._NEAR_NODE_IDX]
        start_tree_parent = start_tree_nodes[:,  self._NEAR_NODE_IDX]

        # 全ノードを親ノード一緒にプロット
        self._plot_all_node_parent(start_tree_pos, start_tree_parent, rrt.robot, rrt.interpolation, rrt.dim)
        # 始点ノードと終点ノードをプロット
        self._plot_start_end_node(rrt)

    def _plot_3d_tree(self, rrt):
        """
        ツリーの描画 (3次元)

        パラメータ
            rrt(rrt.py内のクラス): 経路生成手法クラス
        """
        # パラメータrrtから使用するデータをローカル変数に保存
        start_tree_nodes  = rrt.start_tree.nodes                 # 始点ツリー
        # ツリーから位置と親ノードを取得
        start_tree_pos    = start_tree_nodes[:, :self._NEAR_NODE_IDX]
        start_tree_parent = start_tree_nodes[:,  self._NEAR_NODE_IDX]

        # 全ノードを親ノード一緒にプロット
        self._plot_all_node_parent(start_tree_pos, start_tree_parent, rrt.robot, rrt.interpolation, rrt.dim)
        # 始点ノードと終点ノードをプロット
        self._plot_start_end_node(rrt)

    def _plot_2d_pathes(self, rrt, color="green", alpha=0.7):
        """
        始点から終点までの最終的な経路をプロット (2次元)

        パラメータ
            rrt(rrt.py内のクラス): 経路生成手法クラス
        """
        # 始点から終点までの経路を位置空間で取得
        pathes = self._convert_multi_theta_to_pos(rrt.robot, rrt.pathes, rrt.interpolation)
        self._axis.plot(pathes[:, 0], pathes[:, 1], color=color, alpha=alpha)

    def _plot_3d_pathes(self, rrt, color="green", alpha=0.7):
        """
        始点から終点までの最終的な経路をプロット (3次元)

        パラメータ
            rrt(rrt.py内のクラス): 経路生成手法クラス
        """
        # 始点から終点までの経路を位置空間で取得
        pathes = self._convert_multi_theta_to_pos(rrt.robot, rrt.pathes, rrt.interpolation)
        self._axis.plot(pathes[:, 0], pathes[:, 1], pathes[:, 2], color=color, alpha=alpha)
    # グラフ描画関連のメソッド ↑


    # アニメーション描画関連のメソッド ↓
    def plot_Animation(self, rrt):
        """
        アニメーションの描画

        パラメータ
            rrt(rrt.py内のクラス): 経路生成手法
        """
        # 念の為，経路生成手法が正しいかの確認
        if rrt.name != self._path_plan:
            # 想定外の経路生成手法であるため，異常
            raise ValueError(f"rrt.name is not {self._path_plan}. rrt.name is {rrt.name}")

        if rrt.dim == DIMENTION_2D:
            # 2次元データ
            self._plot_2d_animation(rrt)
        elif rrt.dim == DIMENTION_3D:
            # 3次元データ
            self._plot_3d_animation(rrt)
        else:
            # 異常
            raise ValueError(f"rrt.dim is abnormal. rrt.dim is {rrt.dim}")

    def _plot_2d_animation(self, rrt):
        """
        アニメーションの描画 (2次元)

        パラメータ
            rrt(rrt.py内のクラス): 経路生成手法クラス
        """
        # データをリセット
        self._reset_2d()

        # アニメーションのフレーム数
        if rrt.debug:
            # デバッグあり (フレーム数 = ツリーのノード数 + 始点から終点までの経由点数 + マージン)
            n_frame = rrt.start_tree.nodes.shape[0] + rrt.pathes.shape[0] + self._ANIMATION_MARGIN
        else:
            # デバッグなし (フレーム数 = 始点から終点までの経由点数 + マージン)
            n_frame = rrt.pathes.shape[0] + self._ANIMATION_MARGIN

        self._log(f"n_frame = {n_frame}")

        # アニメーション作成
        animation = ani.FuncAnimation(self._figure, self._update_2d_data, fargs=(rrt, ), interval=self._ANIMATION_INTERBAL, frames=n_frame)

        # ファイル名の取得
        file_name = self._get_file_name(rrt, animation=True)

        # アニメーションの保存
        animation.save(file_name, writer="imagemagick")
        plt.show()

    def _plot_3d_animation(self, rrt):
        """
        アニメーションの描画 (3次元)

        パラメータ
            rrt(rrt.py内のクラス): 経路生成手法クラス
        """
        # データをリセット
        self._reset_3d()

        # アニメーションのフレーム数
        if rrt.debug:
            # デバッグあり (フレーム数 = ツリーのノード数 + 始点から終点までの経由点数 + マージン)
            n_frame = rrt.start_tree.nodes.shape[0] + rrt.pathes.shape[0] + self._ANIMATION_MARGIN
        else:
            # デバッグなし (フレーム数 = 始点から終点までの経由点数)
            n_frame = rrt.pathes.shape[0] + self._ANIMATION_MARGIN

        self._log(f"n_frame = {n_frame}")

        # アニメーション作成
        animation = ani.FuncAnimation(self._figure, self._update_3d_data, fargs=(rrt, ), interval=self._ANIMATION_INTERBAL, frames=n_frame)

        # ファイル名の取得
        file_name = self._get_file_name(rrt, animation=True)

        # アニメーションの保存
        animation.save(file_name, writer="imagemagick")
        plt.show()

    def _update_2d_data(self, i, rrt):
        """
        1フレーム分のデータを描画 (2次元)

        パラメータ
            i(int): フレーム番号 (ツリーのノード数 + 始点から終点までの経由点数)
            rrt(rrt.py内のクラス): 経路生成手法クラス
        """
        # 以前のプロットをクリアする
        self._axis.clear()
        self._set_2d_axis(rrt.environment.plot_min_pos, rrt.environment.plot_max_pos)

        # 環境のプロット
        self._plot_environment(rrt.environment)

        # ロボットのプロット
        self._plot_robot_animation(i, rrt)

        if rrt.debug:
            # デバッグありのため，親ノードも全プロット
            self._plot_2d_tree_animation(i, rrt)
            # 始点から終点までの経由点数を計算
            n_waypoint = i - rrt.start_tree.nodes.shape[0]
            self._plot_2d_path_animation(n_waypoint, rrt)
        else:
            # デバッグなしのため，始点から終点までの経路をプロット
            self._plot_2d_path_animation(i, rrt)

        # 始点と終点をプロット
        self._plot_start_end_node(rrt)

    def _update_3d_data(self, i, rrt):
        """
        1フレーム分のデータを描画 (3次元)

        パラメータ
            i(int): フレーム番号 (ツリーのノード数 + 始点から終点までの経由点数)
            rrt(rrt.py内のクラス): 経路生成手法クラス
        """
        # 以前のプロットをクリアする
        self._axis.clear()
        self._set_3d_axis(rrt.environment.plot_min_pos, rrt.environment.plot_max_pos)

        # 環境のプロット
        self._plot_environment(rrt.environment)

        # ロボットのプロット
        self._plot_robot_animation(i, rrt)

        if rrt.debug:
            # デバッグありのため，親ノードも全プロット
            self._plot_3d_tree_animation(i, rrt)
            # 始点から終点までの経由点数を計算
            n_waypoint = i - rrt.start_tree.nodes.shape[0]
            self._plot_3d_path_animation(n_waypoint, rrt)
        else:
            # デバッグなしのため，始点から終点までの経路をプロット
            self._plot_3d_path_animation(i, rrt)

        # 始点と終点をプロット
        self._plot_start_end_node(rrt)

    def _plot_2d_path_animation(self, i, rrt, color="green", alpha=0.7):
        """
        始点から終点までの経路を描画 (2次元アニメーション用)

        パラメータ
            i(int): プロットしたい経路番号
            rrt(rrt.py内のクラス): 経路生成手法クラス
        """
        if i < 0:
            # 異常値のため，処理終了
            return

        if i < rrt.pathes.shape[0]:
            # iまでの経由点を全部，位置空間で取得
            if i == 0:
                pathes = self._convert_theta_to_pos(rrt.robot, rrt.pathes[0], rrt.interpolation)
                # 1次元データだから，2次元データに変換
                pathes = pathes.reshape(1, -1)
            else:
                pathes = self._convert_multi_theta_to_pos(rrt.robot, rrt.pathes[:i + 1], rrt.interpolation)
        else:
            # 始点から終点までの全経由点を位置空間で取得
            pathes = self._convert_multi_theta_to_pos(rrt.robot, rrt.pathes, rrt.interpolation)

        # プロット
        self._axis.plot(pathes[:i + 1, 0], pathes[:i + 1, 1], color=color, alpha=alpha)

    def _plot_3d_path_animation(self, i, rrt, color="green", alpha=0.7):
        """
        始点から終点までの経路を描画 (3次元アニメーション用)

        パラメータ
            i(int): プロットしたい経路番号
            rrt(rrt.py内のクラス): 経路生成手法クラス
        """
        if i < 0:
            # 異常値のため，処理終了
            return

        if i < rrt.pathes.shape[0]:
            # iまでの経由点を全部，位置空間で取得
            if i == 0:
                pathes = self._convert_theta_to_pos(rrt.robot, rrt.pathes[0], rrt.interpolation)
                # 1次元データだから，2次元データに変換
                pathes = pathes.reshape(1, -1)
            else:
                pathes = self._convert_multi_theta_to_pos(rrt.robot, rrt.pathes[:i + 1], rrt.interpolation)
        else:
            # 始点から終点までの全経由点を位置空間で取得
            pathes = self._convert_multi_theta_to_pos(rrt.robot, rrt.pathes, rrt.interpolation)

        # プロット
        self._axis.plot(pathes[:i + 1, 0], pathes[:i + 1, 1], pathes[:i + 1, 2], color=color, alpha=alpha)

    def _plot_2d_tree_animation(self, i, rrt):
        """
        ツリーの描画 (2次元アニメーション用)

        パラメータ
            i(int): プロットしたい経由点番号
            rrt(rrt.py内のクラス): 経路生成手法クラス
        """
        if i < 0:
            # 異常値のため，処理終了
            return

        # パラメータrrtから使用するデータをローカル変数に保存
        start_tree_nodes = rrt.start_tree.nodes                 # 始点ツリー

        # ツリーから位置と親ノードを取得
        if i < start_tree_nodes.shape[0]:
            # i番目の経由点までを全取得
            start_tree_pos    = start_tree_nodes[:i + 1, :self._NEAR_NODE_IDX]
            start_tree_parent = start_tree_nodes[:i + 1,  self._NEAR_NODE_IDX]
        else:
            # 経由点を全取得
            start_tree_pos    = start_tree_nodes[:, :self._NEAR_NODE_IDX]
            start_tree_parent = start_tree_nodes[:,  self._NEAR_NODE_IDX]

        # 親ノード一緒にプロット
        self._plot_all_node_parent(start_tree_pos, start_tree_parent, rrt.robot, rrt.interpolation, rrt.dim)
        # 始点ノードと終点ノードをプロット
        self._plot_start_end_node(rrt)

    def _plot_3d_tree_animation(self, i, rrt):
        """
        ツリーの描画 (3次元アニメーション用)

        パラメータ
            i(int): プロットしたい経由点番号
            rrt(rrt.py内のクラス): 経路生成手法クラス
        """
        if i < 0:
            # 異常値のため，処理終了
            return

        # パラメータrrtから使用するデータをローカル変数に保存
        start_tree_nodes = rrt.start_tree.nodes                 # 始点ツリー

        # ツリーから位置と親ノードを取得
        if i < start_tree_nodes.shape[0]:
            # i番目の経由点までを全取得
            start_tree_pos    = start_tree_nodes[:i + 1, :self._NEAR_NODE_IDX]
            start_tree_parent = start_tree_nodes[:i + 1,  self._NEAR_NODE_IDX]
        else:
            # 経由点を全取得
            start_tree_pos    = start_tree_nodes[:, :self._NEAR_NODE_IDX]
            start_tree_parent = start_tree_nodes[:,  self._NEAR_NODE_IDX]

        # 親ノード一緒にプロット
        self._plot_all_node_parent(start_tree_pos, start_tree_parent, rrt.robot, rrt.interpolation, rrt.dim)
        # 始点ノードと終点ノードをプロット
        self._plot_start_end_node(rrt)
    # アニメーション描画関連のメソッド ↑


class RRTConnectAnimation(RRTAnimation):
    """
    RRT-Connectのアニメーション作成

    プロパティ
        _path_plan(str): 経路生成手法名

    メソッド
        protected

            ロボット関連
                _plot_robot_animation(): ロボットの描画 (アニメーション用)
                _get_new_node_theta(): 新規ノードの関節角度を取得

            グラフ描画関連
                _plot_2d_tree(): ツリーの描画 (2次元)
                _plot_3d_tree(): ツリーの描画 (3次元)

            アニメーション描画関連
                _plot_2d_animation(): アニメーションの描画 (2次元)
                _plot_3d_animation(): アニメーションの描画 (3次元)
                _get_start_end_tree_nodes(): iフレーム目までの始点ツリーと終点ツリーのノード数を取得
                _update_2d_data(): 1フレーム分のデータを描画 (2次元)
                _update_3d_data(): 1フレーム分のデータを描画 (3次元)
                _plot_2d_tree_animation(): ツリーの描画 (2次元アニメーション用)
                _plot_3d_tree_animation(): ツリーの描画 (3次元アニメーション用)
    """
    # 定数の定義
    # 描画時の色
    _START_TREE_COLOR = "red"
    _END_TREE_COLOR = "orange"

    _NEAR_NODE_IDX = RRT_CONNECT_NEAR_NODE_IDX


    def __init__(self):
        """
        コンストラクタ
        """
        # 親クラスの初期化処理
        super().__init__()
        # 経路生成手法名をRRT-Connectとする
        self._path_plan = PATHPLAN.RRT_CONNECT.value


    # ロボット関連のメソッド ↓
    def _plot_robot_animation(self, i, rrt):
        """
        ロボットの描画 (アニメーション用)

        パラメータ
            i(int): フレーム番号 (ツリーのノード数 + 始点から終点までの経由点数)
            rrt(rrt.py内のクラス): 経路生成手法クラス
        """
        # パラメータrrtから，ロボットクラスをローカル変数に保存
        robot = rrt.robot
        if robot is None:
            # 処理終了
            return

        if i < 0:
            # 異常
            return

        # 新規ノードの関節角度を取得
        theta = self._get_new_node_theta(i, rrt)

        # ロボットのプロット
        self._plot_robot_1data(robot, theta, rrt.dim)

    def _get_new_node_theta(self, i, rrt):
        """
        新規ノードの関節角度を取得

        パラメータ
            i(int): フレーム番号 (ツリーのノード数 + 始点から終点までの経由点数)
            rrt(rrt.py内のクラス): 経路生成手法クラス

        戻り値
            theta(numpy.ndarray): 新規ノードの関節角度 (1 * 次元数)
        """
        # パラメータiが正常かつパラメータrrt内にrobotが存在する前提で実装
        # 本関数を呼ぶ前に上記内容を確認しておくこと
        robot = rrt.robot

        if rrt.debug:
            # デバッグありのため，始点から終点に関係しないノードも使用
            if i < rrt.start_tree.nodes.shape[0] + rrt.end_tree.nodes.shape[0]:
                if i == 0:
                    # 始点プロット
                    theta = self._convert_pos_to_theta(robot, rrt.start_tree.nodes[0, :self._NEAR_NODE_IDX], rrt.interpolation)
                elif i >= (rrt.start_tree.nodes.shape[0] + rrt.end_tree.nodes.shape[0]) - 2:
                    # 終点プロット
                    theta = self._convert_pos_to_theta(robot, rrt.end_tree.nodes[0, :self._NEAR_NODE_IDX], rrt.interpolation)
                else:
                    # iフレーム目までの始点ツリーと終点ツリーを取得
                    n_start_tree, n_end_tree = self._get_start_end_tree_nodes(i, rrt)
                    # iフレーム目がどの状態であるかを確認
                    state = rrt.states[i]
                    if state == STATE_START_TREE_RANDOM or state == STATE_START_TREE_END_TREE:
                        # 始点ツリーにノード追加中
                        theta = self._convert_pos_to_theta(robot, rrt.start_tree.nodes[n_start_tree, :self._NEAR_NODE_IDX], rrt.interpolation)
                    else:
                        # 終点ツリーにノード追加中
                        theta = self._convert_pos_to_theta(robot, rrt.end_tree.nodes[n_end_tree, :self._NEAR_NODE_IDX], rrt.interpolation)
            else:
                # 始点から終点までの経由点を使用
                n_waypoint = i - (rrt.start_tree.nodes.shape[0] + rrt.end_tree.nodes.shape[0])
                if n_waypoint < rrt.pathes.shape[0]:
                    # n_waypoint番目の経由点をプロット
                    theta = self._convert_pos_to_theta(robot, rrt.pathes[n_waypoint], rrt.interpolation)
                else:
                    # 終点をプロット
                    theta = self._convert_pos_to_theta(robot, rrt.pathes[-1], rrt.interpolation)
        else:
            # デバッグなしのため，始点から終点までの経由点を使用
            if i < rrt.pathes.shape[0]:
                # i番目の経由点をプロット
                theta = self._convert_pos_to_theta(robot, rrt.pathes[i], rrt.interpolation)
            else:
                # 終点をプロット
                theta = self._convert_pos_to_theta(robot, rrt.pathes[-1], rrt.interpolation)

        return theta
    # ロボット関連のメソッド ↑


    # グラフ描画関連のメソッド ↓
    def _plot_2d_tree(self, rrt):
        """
        ツリーの描画 (2次元)

        パラメータ
            rrt(rrt.py内のクラス): 経路生成手法クラス
        """
        # パラメータrrtから使用するデータをローカル変数に保存
        start_tree_nodes  = rrt.start_tree.nodes                 # 始点ツリー
        end_tree_nodes    = rrt.end_tree.nodes
        # ツリーから位置と親ノードを取得
        start_tree_pos    = start_tree_nodes[:, :self._NEAR_NODE_IDX]
        start_tree_parent = start_tree_nodes[:,  self._NEAR_NODE_IDX]
        end_tree_pos      =   end_tree_nodes[:, :self._NEAR_NODE_IDX]
        end_tree_parent   =   end_tree_nodes[:,  self._NEAR_NODE_IDX]

        # 始点ツリー内の全ノードを親ノード一緒にプロット
        self._plot_all_node_parent(start_tree_pos, start_tree_parent, rrt.robot, rrt.interpolation, rrt.dim, color=self._START_TREE_COLOR)
        # 終点ツリー内の全ノードを親ノード一緒にプロット
        self._plot_all_node_parent(end_tree_pos,   end_tree_parent,   rrt.robot, rrt.interpolation, rrt.dim, color=self._END_TREE_COLOR)

        # 始点ノードと終点ノードをプロット
        self._plot_start_end_node(rrt)

    def _plot_3d_tree(self, rrt):
        """
        ツリーの描画 (3次元)

        パラメータ
            rrt(rrt.py内のクラス): 経路生成手法クラス
        """
        # パラメータrrtから使用するデータをローカル変数に保存
        start_tree_nodes  = rrt.start_tree.nodes                 # 始点ツリー
        end_tree_nodes    = rrt.end_tree.nodes
        # ツリーから位置と親ノードを取得
        start_tree_pos    = start_tree_nodes[:, :self._NEAR_NODE_IDX]
        start_tree_parent = start_tree_nodes[:,  self._NEAR_NODE_IDX]
        end_tree_pos      =   end_tree_nodes[:, :self._NEAR_NODE_IDX]
        end_tree_parent   =   end_tree_nodes[:,  self._NEAR_NODE_IDX]

        # 始点ツリー内の全ノードを親ノード一緒にプロット
        self._plot_all_node_parent(start_tree_pos, start_tree_parent, rrt.robot, rrt.interpolation, rrt.dim, color=self._START_TREE_COLOR)
        # 終点ツリー内の全ノードを親ノード一緒にプロット
        self._plot_all_node_parent(end_tree_pos,   end_tree_parent,   rrt.robot, rrt.interpolation, rrt.dim, color=self._END_TREE_COLOR)

        # 始点ノードと終点ノードをプロット
        self._plot_start_end_node(rrt)
    # グラフ描画関連のメソッド ↑


    # アニメーション描画関連のメソッド ↓
    def _plot_2d_animation(self, rrt):
        """
        アニメーションの描画 (2次元)

        パラメータ
            rrt(rrt.py内のクラス): 経路生成手法クラス
        """
        # データをリセット
        self._reset_2d()

        # アニメーションのフレーム数
        if rrt.debug:
            # デバッグあり (フレーム数 = 始点ツリーのノード数 + 終点ツリーのノード数 + 始点から終点までの経由点数 + マージン)
            n_frame = rrt.start_tree.nodes.shape[0] + rrt.end_tree.nodes.shape[0] + rrt.pathes.shape[0] + self._ANIMATION_MARGIN
        else:
            # デバッグなし (フレーム数 = 始点から終点までの経由点数 + マージン)
            n_frame = rrt.pathes.shape[0] + self._ANIMATION_MARGIN

        self._log(f"n_frame = {n_frame}")

        # アニメーション作成
        animation = ani.FuncAnimation(self._figure, self._update_2d_data, fargs=(rrt, ), interval=self._ANIMATION_INTERBAL, frames=n_frame)

        # ファイル名の取得
        file_name = self._get_file_name(rrt, animation=True)

        # アニメーションの保存
        animation.save(file_name, writer="imagemagick")
        plt.show()

    def _plot_3d_animation(self, rrt):
        """
        アニメーションの描画 (3次元)

        パラメータ
            rrt(rrt.py内のクラス): 経路生成手法クラス
        """
        # データをリセット
        self._reset_3d()

        # アニメーションのフレーム数
        if rrt.debug:
            # デバッグあり (フレーム数 = ツリーのノード数 + 始点から終点までの経由点数 + マージン)
            n_frame = rrt.start_tree.nodes.shape[0] + rrt.end_tree.nodes.shape[0] + rrt.pathes.shape[0] + self._ANIMATION_MARGIN
        else:
            # デバッグなし (フレーム数 = 始点から終点までの経由点数 + マージン)
            n_frame = rrt.pathes.shape[0] + self._ANIMATION_MARGIN

        self._log(f"n_frame = {n_frame}")

        # アニメーション作成
        animation = ani.FuncAnimation(self._figure, self._update_3d_data, fargs=(rrt, ), interval=self._ANIMATION_INTERBAL, frames=n_frame)

        # ファイル名の取得
        file_name = self._get_file_name(rrt, animation=True)

        # アニメーションの保存
        animation.save(file_name, writer="imagemagick")
        plt.show()

    def _get_start_end_tree_nodes(self, i, rrt):
        """
        iフレーム目までの始点ツリーと終点ツリーのノード数を取得

        パラメータ
            i(int): フレーム番号 (ツリーのノード数 + 始点から終点までの経由点数)
            rrt(rrt.py内のクラス): 経路生成手法クラス

        戻り値
            n_start_tree(int): iフレーム目までの始点ツリーのノード数
            n_end_tree(int): iフレーム目までの終点ツリーのノード数
        """
        n_start_tree = 0
        n_end_tree   = 0

        if i < 0:
            # 異常
            return n_start_tree, n_end_tree

        # 状態から始点ツリーへの追加回数を取得
        n_start_tree = np.sum(np.isin(rrt.states[:i + 1], [STATE_START_TREE_RANDOM, STATE_START_TREE_END_TREE]))
        # 状態から終点ツリーへの追加回数を取得
        n_end_tree   = np.sum(np.isin(rrt.states[:i + 1], [STATE_END_TREE_RANDOM,   STATE_END_TREE_START_TREE]))

        return n_start_tree, n_end_tree

    def _update_2d_data(self, i, rrt):
        """
        1フレーム分のデータを描画 (2次元)

        パラメータ
            i(int): フレーム番号 (ツリーのノード数 + 始点から終点までの経由点数)
            rrt(rrt.py内のクラス): 経路生成手法クラス
        """
        # 以前のプロットをクリアする
        self._axis.clear()
        self._set_2d_axis(rrt.environment.plot_min_pos, rrt.environment.plot_max_pos)

        # 環境のプロット
        self._plot_environment(rrt.environment)

        # ロボットのプロット
        self._plot_robot_animation(i, rrt)

        if rrt.debug:
            # 状態から始点ツリー，終点ツリーのノード数を取得
            n_start_tree, n_end_tree = self._get_start_end_tree_nodes(i, rrt)
            # デバッグありのため，始点ツリーと終点ツリーの親ノードも全プロット
            self._plot_2d_tree_animation(n_start_tree, rrt, start_tree=True,  color=self._START_TREE_COLOR)
            self._plot_2d_tree_animation(n_end_tree,   rrt, start_tree=False, color=self._END_TREE_COLOR)
            # 始点から終点までの経由点数を計算
            n_waypoint = i - (rrt.start_tree.nodes.shape[0] + rrt.end_tree.nodes.shape[0])
            self._plot_2d_path_animation(n_waypoint, rrt)
        else:
            # デバッグなしのため，始点から終点までの経路をプロット
            self._plot_2d_path_animation(i, rrt)

        # 始点と終点をプロット
        self._plot_start_end_node(rrt)

    def _update_3d_data(self, i, rrt):
        """
        1フレーム分のデータを描画 (3次元)

        パラメータ
            i(int): フレーム番号 (ツリーのノード数 + 始点から終点までの経由点数)
            rrt(rrt.py内のクラス): 経路生成手法クラス
        """
        # 以前のプロットをクリアする
        self._axis.clear()
        self._set_3d_axis(rrt.environment.plot_min_pos, rrt.environment.plot_max_pos)

        # 環境のプロット
        self._plot_environment(rrt.environment)

        # ロボットのプロット
        self._plot_robot_animation(i, rrt)

        if rrt.debug:
            # 状態から始点ツリー，終点ツリーのノード数を取得
            n_start_tree, n_end_tree = self._get_start_end_tree_nodes(i, rrt)
            # デバッグありのため，始点ツリーと終点ツリーの親ノードも全プロット
            self._plot_3d_tree_animation(n_start_tree, rrt, start_tree=True,  color=self._START_TREE_COLOR)
            self._plot_3d_tree_animation(n_end_tree,   rrt, start_tree=False, color=self._END_TREE_COLOR)
            # 始点から終点までの経由点数を計算
            n_waypoint = i - (rrt.start_tree.nodes.shape[0] + rrt.end_tree.nodes.shape[0])
            self._plot_3d_path_animation(n_waypoint, rrt)
        else:
            # デバッグなしのため，始点から終点までの経路をプロット
            self._plot_3d_path_animation(i, rrt)

        # 始点と終点をプロット
        self._plot_start_end_node(rrt)

    def _plot_2d_tree_animation(self, i, rrt, start_tree=True, color="red"):
        """
        ツリーの描画 (2次元アニメーション用)

        パラメータ
            i(int): プロットしたい経由点番号
            rrt(rrt.py内のクラス): 経路生成手法クラス
            start_tree(bool): True / False = 始点ツリー / 終点ツリー
            color(str): 描画時の色
        """
        if i < 0:
            # 異常値のため，処理終了
            return

        # パラメータrrtから使用するデータをローカル変数に保存
        if start_tree:
            # 始点ツリー
            tree_nodes = rrt.start_tree.nodes
        else:
            # 終点ツリー
            tree_nodes = rrt.end_tree.nodes

        # ツリーから位置と親ノードを取得
        if i < tree_nodes.shape[0]:
            # i番目の経由点までを全取得
            tree_pos    = tree_nodes[:i + 1, :self._NEAR_NODE_IDX]
            tree_parent = tree_nodes[:i + 1,  self._NEAR_NODE_IDX]
        else:
            # 経由点を全取得
            tree_pos    = tree_nodes[:, :self._NEAR_NODE_IDX]
            tree_parent = tree_nodes[:,  self._NEAR_NODE_IDX]

        # 親ノード一緒にプロット
        self._plot_all_node_parent(tree_pos, tree_parent, rrt.robot, rrt.interpolation, rrt.dim, color=color)
        # 始点ノードと終点ノードをプロット
        self._plot_start_end_node(rrt)

    def _plot_3d_tree_animation(self, i, rrt, start_tree=True, color="red"):
        """
        ツリーの描画 (3次元アニメーション用)

        パラメータ
            i(int): プロットしたい経由点番号
            rrt(rrt.py内のクラス): 経路生成手法クラス
            start_tree(bool): True / False = 始点ツリー / 終点ツリー
        """
        if i < 0:
            # 異常値のため，処理終了
            return

        # パラメータrrtから使用するデータをローカル変数に保存
        if start_tree:
            # 始点ツリー
            tree_nodes = rrt.start_tree.nodes
        else:
            # 終点ツリー
            tree_nodes = rrt.end_tree.nodes

        # ツリーから位置と親ノードを取得
        if i < tree_nodes.shape[0]:
            # i番目の経由点までを全取得
            tree_pos    = tree_nodes[:i + 1, :self._NEAR_NODE_IDX]
            tree_parent = tree_nodes[:i + 1,  self._NEAR_NODE_IDX]
        else:
            # 経由点を全取得
            tree_pos    = tree_nodes[:, :self._NEAR_NODE_IDX]
            tree_parent = tree_nodes[:,  self._NEAR_NODE_IDX]

        # 親ノード一緒にプロット
        self._plot_all_node_parent(tree_pos, tree_parent, rrt.robot, rrt.interpolation, rrt.dim, color=color)
        # 始点ノードと終点ノードをプロット
        self._plot_start_end_node(rrt)
    # アニメーション描画関連のメソッド ↑


class RRTStarAnimation(RRTAnimation):
    """
    RRT*のアニメーション作成

    プロパティ
        _path_plan(str): 経路生成手法名

    メソッド
        protected

            アニメーション描画関連
                _plot_2d_tree_animation(): ツリーの描画 (2次元アニメーション用)
                _plot_3d_tree_animation(): ツリーの描画 (3次元アニメーション用)
    """
    # 定数の定義
    _NEAR_NODE_IDX = RRT_STAR_NEAR_NODE_IDX
    _RADIUS_NODE_IDX = RRT_STAR_RADIUS_IDX

    _EXPLOTION_CIRCLE_COLOR = "blue"
    _EXPLOTION_CIRCLE_ALPHA = 0.2


    def __init__(self):
        """
        コンストラクタ
        """
        # 親クラスの初期化処理
        super().__init__()
        # 経路生成手法名をRRT*とする
        self._path_plan = PATHPLAN.RRT_STAR.value


    # アニメーション描画関連のメソッド ↓
    def _plot_2d_tree_animation(self, i, rrt):
        """
        ツリーの描画 (2次元アニメーション用)

        パラメータ
            i(int): プロットしたい経由点番号
            rrt(rrt.py内のクラス): 経路生成手法クラス
        """
        if i < 0:
            # 異常値のため，処理終了
            return

        # iとRRT*の成功回数を比較
        if i == 0:
            # 始点だけを取得
            start_tree_nodes  = rrt.start_tree.nodes
            start_tree_pos    = start_tree_nodes[0, :self._NEAR_NODE_IDX].reshape(1, -1)
            start_tree_parent = start_tree_nodes[0,  self._NEAR_NODE_IDX].reshape(1, -1)
        elif i < rrt.count_success:
            # i回成功目のツリーをファイルから読み込む
            # RRT*はノード数を増やしていくと，親ノードも変わるため，ノード追加ごとにツリーを毎回ファイル保存している
            # ファイルからツリーを読み込む (ファイル名 = 経路生成手法名 / 探索空間名 / result / ツリー名_ノード追加番号.csv)
            file_name = f"{rrt.file_name_current_tree}_{i}.csv"
            full_path = os.path.join(rrt.name, rrt.interpolation, rrt.folder_name_current_tree, file_name)
            start_tree_nodes  = np.loadtxt(full_path)
            # ノードを位置と親ノードに分割
            start_tree_pos    = start_tree_nodes[:, :self._NEAR_NODE_IDX]
            start_tree_parent = start_tree_nodes[:,  self._NEAR_NODE_IDX]
        else:
            # 最終的な経由点を全部取得
            # パラメータrrtから使用するデータをローカル変数に保存
            start_tree_nodes  = rrt.start_tree.nodes
            # ノードを位置と親ノードに分割
            start_tree_pos    = start_tree_nodes[:, :self._NEAR_NODE_IDX]
            start_tree_parent = start_tree_nodes[:,  self._NEAR_NODE_IDX]

        # 親ノード一緒にプロット
        self._plot_all_node_parent(start_tree_pos, start_tree_parent, rrt.robot, rrt.interpolation, rrt.dim)
        # 始点ノードと終点ノードをプロット
        self._plot_start_end_node(rrt)

    def _plot_3d_tree_animation(self, i, rrt):
        """
        ツリーの描画 (3次元アニメーション用)

        パラメータ
            i(int): プロットしたい経由点番号
            rrt(rrt.py内のクラス): 経路生成手法クラス
        """
        if i < 0:
            # 異常値のため，処理終了
            return

        # iとRRT*の成功回数を比較
        if i == 0:
            # 始点だけをプロット
            start_tree_nodes  = rrt.start_tree.nodes
            start_tree_pos    = start_tree_nodes[0, :self._NEAR_NODE_IDX].reshape(1, -1)
            start_tree_parent = start_tree_nodes[0,  self._NEAR_NODE_IDX].reshape(1, -1)
        elif i <= rrt.count_success:
            # i回成功目のツリーをファイルから読み込む
            # RRT*はノード数を増やしていくと，親ノードも変わるため，ノード追加ごとにツリーを毎回ファイル保存している
            # ファイルからツリーを読み込む (ファイル名 = 経路生成手法名 / 探索空間名 / result / ツリー名_ノード追加番号.csv)
            file_name = f"{rrt.file_name_current_tree}_{i}.csv"
            full_path = os.path.join(rrt.name, rrt.interpolation, rrt.folder_name_current_tree, file_name)
            start_tree_nodes  = np.loadtxt(full_path)
            # ノードを位置と親ノードに分割
            start_tree_pos    = start_tree_nodes[:, :self._NEAR_NODE_IDX]
            start_tree_parent = start_tree_nodes[:,  self._NEAR_NODE_IDX]
        else:
            # 最終的な経由点を全部取得
            # パラメータrrtから使用するデータをローカル変数に保存
            start_tree_nodes  = rrt.start_tree.nodes
            # ノードを位置と親ノードに分割
            start_tree_pos    = start_tree_nodes[:, :self._NEAR_NODE_IDX]
            start_tree_parent = start_tree_nodes[:,  self._NEAR_NODE_IDX]

        # 親ノード一緒にプロット
        self._plot_all_node_parent(start_tree_pos, start_tree_parent, rrt.robot, rrt.interpolation, rrt.dim)
        # 始点ノードと終点ノードをプロット
        self._plot_start_end_node(rrt)
    # アニメーション描画関連のメソッド ↑


class RRTStarConnectAnimation(RRTStarAnimation, RRTConnectAnimation):
    """
    RRT*-Connect (RRT-Connect + RRT*) のアニメーション描画

    プロパティ
        _path_plan(str): 経路生成手法名

    メソッド
        public
            plot(): グラフ描画
            plot_Animation(): アニメーション描画

        protected

            アニメーション描画関連
                _update_2d_data(): 1フレーム分のデータを描画 (2次元)
                _plot_2d_tree_animation(): ツリーの描画 (2次元アニメーション用)
                _plot_3d_tree_animation(): ツリーの描画 (3次元アニメーション用)
    """
    # 定数の定義
    _NEAR_NODE_IDX = RRT_STAR_NEAR_NODE_IDX
    _RADIUS_NODE_IDX = RRT_STAR_RADIUS_IDX

    _EXPLOTION_CIRCLE_COLOR = "blue"
    _EXPLOTION_CIRCLE_ALPHA = 0.2


    def __init__(self):
        """
        コンストラクタ
        """
        # 親クラスの初期化処理
        super().__init__()
        # 経路生成手法名をRRT*-Connectとする
        self._path_plan = PATHPLAN.RRT_STAR_CONNECT.value


    # グラフ描画関連のメソッド ↓
    def plot(self, rrt):
        """
        グラフの描画

        パラメータ
            rrt(rrt.py内のクラス): 経路生成手法クラス
        """
        # 念の為，経路生成手法が正しいかの確認
        if rrt.name != self._path_plan:
            # 想定外の経路生成手法であるため，異常
            raise ValueError(f"rrt.name is not {self._path_plan}. rrt.name is {rrt.name}")

        # RRT-Connectのメソッドを使用する
        if rrt.dim == DIMENTION_2D:
            # 2次元データ
            RRTConnectAnimation._plot_2d(self, rrt)
        elif rrt.dim == DIMENTION_3D:
            # 3次元データ
            RRTConnectAnimation._plot_3d(self, rrt)
        else:
            # 異常
            raise ValueError(f"rrt.dim is abnormal. rrt.dim is {rrt.dim}")
    # グラフ描画関連のメソッド ↑


    # アニメーション描画関連のメソッド ↓
    def _update_2d_data(self, i, rrt):
        """
        1フレーム分のデータを描画 (2次元)

        パラメータ
            i(int): フレーム番号 (ツリーのノード数 + 始点から終点までの経由点数)
            rrt(rrt.py内のクラス): 経路生成手法クラス
        """
        # 以前のプロットをクリアする
        self._axis.clear()
        self._set_2d_axis(rrt.environment.plot_min_pos, rrt.environment.plot_max_pos)

        # 環境のプロット
        self._plot_environment(rrt.environment)

        # ロボットのプロット
        RRTConnectAnimation._plot_robot_animation(self, i, rrt)

        if rrt.debug:
            # i番目までの始点ツリーと終点ツリーの数を取得
            n_start_tree, n_end_tree = self._get_start_end_tree_nodes(i, rrt)
            # デバッグありのため，始点ツリーと終点ツリーの親ノードも全プロット
            self._plot_2d_tree_animation(i, n_start_tree, rrt, start_tree=True,  color=self._START_TREE_COLOR)
            self._plot_2d_tree_animation(i, n_end_tree,   rrt, start_tree=False, color=self._END_TREE_COLOR)
            # 始点から終点までの経由点数を計算
            n_waypoint = i - (rrt.start_tree.nodes.shape[0] + rrt.end_tree.nodes.shape[0])
            self._plot_2d_path_animation(n_waypoint, rrt)
        else:
            # デバッグなしのため，始点から終点までの経路をプロット
            self._plot_2d_path_animation(i, rrt)

        # 始点と終点をプロット
        self._plot_start_end_node(rrt)

    def _update_3d_data(self, i, rrt):
        """
        1フレーム分のデータを描画 (3次元)

        パラメータ
            i(int): フレーム番号 (ツリーのノード数 + 始点から終点までの経由点数)
            rrt(rrt.py内のクラス): 経路生成手法クラス
        """
        # 以前のプロットをクリアする
        self._axis.clear()
        self._set_3d_axis(rrt.environment.plot_min_pos, rrt.environment.plot_max_pos)

        # 環境のプロット
        self._plot_environment(rrt.environment)

        # ロボットのプロット
        RRTConnectAnimation._plot_robot_animation(self, i, rrt)

        if rrt.debug:
            # i番目までの始点ツリーと終点ツリーの数を取得
            n_start_tree, n_end_tree = self._get_start_end_tree_nodes(i, rrt)
            # デバッグありのため，始点ツリーと終点ツリーの親ノードも全プロット
            self._plot_3d_tree_animation(i, n_start_tree, rrt, start_tree=True,  color=self._START_TREE_COLOR)
            self._plot_3d_tree_animation(i, n_end_tree,   rrt, start_tree=False, color=self._END_TREE_COLOR)
            # 始点から終点までの経由点数を計算
            n_waypoint = i - (rrt.start_tree.nodes.shape[0] + rrt.end_tree.nodes.shape[0])
            self._plot_3d_path_animation(n_waypoint, rrt)
        else:
            # デバッグなしのため，始点から終点までの経路をプロット
            self._plot_3d_path_animation(i, rrt)

        # 始点と終点をプロット
        self._plot_start_end_node(rrt)

    def _plot_2d_tree_animation(self, i, n_node, rrt, start_tree=True, color="red"):
        """
        ツリーの描画 (2次元アニメーション用)

        パラメータ
            i(int): プロットしたい経由点番号
            n_node(int): i番目の経由点追加時のツリー内のノード数
            rrt(rrt.py内のクラス): 経路生成手法クラス
            start_tree(bool): True / False = 始点ツリー / 終点ツリー
            color(str): 描画時の色
        """
        if i < 0:
            # 異常値のため，処理終了
            return

        # パラメータrrtから使用するデータをローカル変数に保存
        if start_tree:
            # 始点ツリー
            tree_nodes = rrt.start_tree.nodes
            file_name  = f"{rrt.file_name_current_start_tree}_{i + 1}.csv"
        else:
            # 終点ツリー
            tree_nodes = rrt.end_tree.nodes
            file_name  = f"{rrt.file_name_current_end_tree}_{i + 1}.csv"

        if i == 0 or n_node <= 1:
            # ツリー内の初期点をプロット
            tree_pos    = tree_nodes[0, :self._NEAR_NODE_IDX].reshape(1, -1)
            tree_parent = tree_nodes[0,  self._NEAR_NODE_IDX].reshape(1, -1)
        elif i < rrt.count_success:
            # i回成功目のツリーをファイルから読み込む
            # RRT*-Connectはノード数を増やしていくと，親ノードも変わるため，ノード追加ごとにツリーを毎回ファイル保存している
            # ファイルからツリーを読み込む (ファイル名 = 経路生成手法名 / 探索空間名 / result / ツリー名_ノード追加番号.csv)
            full_path   = os.path.join(rrt.name, rrt.interpolation, rrt.folder_name_current_tree, file_name)
            tree_nodes  = np.loadtxt(full_path)
            # ノードを位置と親ノードに分割
            tree_pos    = tree_nodes[:, :self._NEAR_NODE_IDX]
            tree_parent = tree_nodes[:,  self._NEAR_NODE_IDX]
        else:
            # 最終的な経由点を全部取得
            # ノードを位置と親ノードに分割
            tree_pos    = tree_nodes[:, :self._NEAR_NODE_IDX]
            tree_parent = tree_nodes[:,  self._NEAR_NODE_IDX]

        # 親ノード一緒にプロット
        self._plot_all_node_parent(tree_pos, tree_parent, rrt.robot, rrt.interpolation, rrt.dim, color=color)
        # 始点ノードと終点ノードをプロット
        self._plot_start_end_node(rrt)

    def _plot_3d_tree_animation(self, i, n_node, rrt, start_tree=True, color="red"):
        """
        ツリーの描画 (3次元アニメーション用)

        パラメータ
            i(int): プロットしたい経由点番号
            n_node(int): i番目の経由点追加時のツリー内のノード数
            rrt(rrt.py内のクラス): 経路生成手法クラス
            start_tree(bool): True / False = 始点ツリー / 終点ツリー
            color(str): 描画時の色
        """
        if i < 0:
            # 異常値のため，処理終了
            return

        # パラメータrrtから使用するデータをローカル変数に保存
        if start_tree:
            # 始点ツリー
            tree_nodes = rrt.start_tree.nodes
            file_name  = f"{rrt.file_name_current_start_tree}_{i + 1}.csv"
        else:
            # 終点ツリー
            tree_nodes = rrt.end_tree.nodes
            file_name  = f"{rrt.file_name_current_end_tree}_{i + 1}.csv"

        if i == 0 or n_node <= 1:
            # ツリー内の初期点をプロット
            tree_pos    = tree_nodes[0, :self._NEAR_NODE_IDX].reshape(1, -1)
            tree_parent = tree_nodes[0,  self._NEAR_NODE_IDX].reshape(1, -1)
        elif i < rrt.count_success:
            # i回成功目のツリーをファイルから読み込む
            # RRT*-Connectはノード数を増やしていくと，親ノードも変わるため，ノード追加ごとにツリーを毎回ファイル保存している
            # ファイルからツリーを読み込む (ファイル名 = 経路生成手法名 / 探索空間名 / result / ツリー名_ノード追加番号.csv)
            full_path   = os.path.join(rrt.name, rrt.interpolation, rrt.folder_name_current_tree, file_name)
            tree_nodes  = np.loadtxt(full_path)
            # ノードを位置と親ノードに分割
            tree_pos    = tree_nodes[:, :self._NEAR_NODE_IDX]
            tree_parent = tree_nodes[:,  self._NEAR_NODE_IDX]
        else:
            # 最終的な経由点を全部取得
            # ノードを位置と親ノードに分割
            tree_pos    = tree_nodes[:, :self._NEAR_NODE_IDX]
            tree_parent = tree_nodes[:,  self._NEAR_NODE_IDX]

        # 親ノード一緒にプロット
        self._plot_all_node_parent(tree_pos, tree_parent, rrt.robot, rrt.interpolation, rrt.dim, color=color)
        # 始点ノードと終点ノードをプロット
        self._plot_start_end_node(rrt)
    # アニメーション描画関連のメソッド ↑


class MainRRTRobotAnimation:
    """
    ロボットの経路生成アニメーションのメインクラス

    プロパティ
        なし

    メソッド
        public
            plot(): グラフの描画
            plot_Animation(): アニメーションの描画

        protected
            _get_each_anime_instance(): 経路生成手法に応じたアニメーションクラスのインスタンスを取得
    """
    # 定数の定義


    def __init__(self):
        """
        コンストラクタ
        """
        # フォルダ作成
        for enum_data in PATHPLAN:
            # ファルダーが作成済みでもエラーを出力しないよう，exist_ok=Trueとした．
            os.makedirs(enum_data.value, exist_ok=True)


    def _get_each_anime_instance(self, rrt):
        """
        経路生成手法に応じたアニメーションクラスのインスタンスを取得

        パラメータ
            rrt(rrt.py内のクラス): 経路生成手法クラス

        戻り値
            rrt_anime: 各経路生成手法のアニメーションクラス
        """
        # 経路生成手法によって，アニメーションの分岐を分ける
        if rrt.name == PATHPLAN.RRT.value:
            # RRTアニメーション
            rrt_anime = RRTAnimation()
        elif rrt.name == PATHPLAN.RRT_CONNECT.value:
            # RRT-Connectアニメーション
            rrt_anime = RRTConnectAnimation()
        elif rrt.name == PATHPLAN.RRT_STAR.value:
            # RRT*アニメーション
            rrt_anime = RRTStarAnimation()
        elif rrt.name == PATHPLAN.RRT_STAR_CONNECT.value:
            # RRT*-Connectアニメーション
            rrt_anime = RRTStarConnectAnimation()
        else:
            # 異常
            raise ValueError(f"rrt.name is abnormal. rrt.name is {rrt.name}")

        return rrt_anime

    def plot(self, rrt):
        """
        グラフの描画

        パラメータ
            rrt(rrt.py内のクラス): 経路生成手法
        """
        # アニメーションクラスのインスタンスを取得
        rrt_anime = self._get_each_anime_instance(rrt)

        # アニメーション描画
        rrt_anime.plot(rrt)

    def plot_Animation(self, rrt):
        """
        アニメーションの描画

        パラメータ
            rrt(rrt.py内のクラス): 経路生成手法
        """
        # アニメーションクラスのインスタンスを取得
        rrt_anime = self._get_each_anime_instance(rrt)

        # アニメーション描画
        rrt_anime.plot_Animation(rrt)

メイン処理 (main.py)

経路生成およびアニメーション作成のメイン処理を説明する．

main.py

# メイン処理

# ライブラリの読み込み
import numpy as np                  # 数値計算


# 自作モジュールの読み込み
from constant import *              # 定数
from robot import Robot2DoF, Robot3DoF          # ロボットクラス
from animation import MainRRTRobotAnimation     # 経路生成手法のアニメーション
from rrt import MainRRTRobot        # 経路生成
from environment import Robot2DEnv, Robot3DEnv  # 経路生成の環境


CONST_SEED      = 1             # 固定したいシード値
# CONST_SEED      = np.random.randint(MIN_SEED, MAX_SEED)   # 固定したいシード値
# print(f"CONST_SEED = {CONST_SEED}")
N_ROBOT_AXIS    = DIMENTION_2D  # ロボットの軸数
N_ROBOT_AXIS    = DIMENTION_3D  # ロボットの軸数



def main():
    """
    メイン処理
    """
    if N_ROBOT_AXIS == DIMENTION_2D:    # 2軸ロボットアームの場合
        # 2軸ロボットのリンク長
        links = np.array([1.0, 1.0])
        # 2軸ロボットのインスタンスを作成
        robot = Robot2DoF(links)

        # 始点
        start_pos = np.array([1.0, -1.0])
        # 終点
        end_pos   = np.array([1.0,  1.0])

        # 環境
        environment = Robot2DEnv()

    elif N_ROBOT_AXIS == DIMENTION_3D:  # 3軸ロボットアームの場合
        # 3軸ロボットのリンク長
        links = np.array([1.0, 1.0, 1.0])
        # 3軸ロボットのインスタンスを作成
        robot = Robot3DoF(links)

        # 始点
        start_pos = np.array([1.0, -1.0, 1.0])
        # 終点
        end_pos = np.array([-1.0, 1.0, 2.0])

        # 環境
        environment = Robot3DEnv()

    else:
        # 異常
        raise ValueError(f"N_ROBOT_AXIS is abnormal. N_ROBOT_AXIS is {N_ROBOT_AXIS}")


    try:
        # 始点と終点の逆運動学
        start_theta = robot.inverse_kinematics(start_pos)
        end_theta   = robot.inverse_kinematics(end_pos)
    except Exception as e:
        # 逆運動学の解が存在しない (始点または終点が異常な値)
        raise ValueError(f"please start_pos or end_pos is change. pos is singularity")

    # RRTのデバッグフラグ
    debugs = [True, False]
    # 経路の修正方法
    # modification = MODIFICATION.PARTIAL_START
    modification = None

    # 補間の種類 (位置空間/関節空間)
    interpolations = [INTERPOLATION.POSITION.value, INTERPOLATION.JOINT.value]
    interpolations = [INTERPOLATION.POSITION.value, ]

    # 経路生成手法
    path_plans = [PATHPLAN.RRT_CONNECT.value, PATHPLAN.RRT_STAR.value, PATHPLAN.RRT_STAR_CONNECT.value]
    path_plans = [PATHPLAN.RRT_STAR_CONNECT.value, ]

    # 描画方法
    plot_modes = [PLOT_GRAPH, PLOT_ANIMATION]
    plot_modes = [PLOT_ANIMATION, ]

    # アニメーション作成クラスのインスタンス生成
    robot_anime = MainRRTRobotAnimation()

    # 経路生成手法クラスのインスタンスを作成
    rrt_robot   = MainRRTRobot()

    for path_plan in path_plans:
        for debug in debugs:
            for interpolation in interpolations:
                # 各々のアルゴリズムで比較したいからシード値は固定
                np.random.seed(CONST_SEED)
                if interpolation == INTERPOLATION.POSITION.value:
                    # 位置空間での経路生成の実行
                    planning_result = rrt_robot.planning(path_plan, start_pos,   end_pos,   environment, robot, interpolation, modification=modification, debug=debug)
                else:
                    # 関節空間での経路生成の実行
                    planning_result = rrt_robot.planning(path_plan, start_theta, end_theta, environment, robot, interpolation, modification=modification, debug=debug)

                print(f"planning_result = {planning_result}")
                if not planning_result:
                    # 経路生成の失敗
                    continue

                # 描画
                for plot_mode in plot_modes:
                    if plot_mode == PLOT_GRAPH:
                        # グラフ描画
                        robot_anime.plot(rrt_robot.rrt)
                    elif plot_mode == PLOT_ANIMATION:
                        # アニメーション描画
                        robot_anime.plot_Animation(rrt_robot.rrt)
                    else:
                        # 何もしない
                        pass


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

性能評価およびアニメーション

RRT*-Connect の性能評価およびアニメーションを展開する．

RRT-Connect と RRT* と RRT*-Connect の経路比較

RRT*-Connect は RRT-Connect の双方向探索と RRT* の最適性を組み合わせた手法であるため，3手法で作成した経路の比較をする．
アニメーションの見方は以下の通りである．
・水色の星形(中下)データ：経路生成の始点
・緑色の星形(中上)データ：経路生成の終点
・赤色の線：新規データと最短コスト(距離)の親ノードを結んだ線 (始点ツリー)
・オレンジ色の線：新規データと最短コスト(距離)の親ノードを結んだ線 (終点ツリー)
・緑色の線：最終的な始点から終点までの経路
・灰色の円：干渉物

初めに，RRT-Connect でロボットの位置空間上で作成した経路を下図に添付した．

次に，RRT* でロボットの位置空間上で作成した経路を下図に添付した．

最後に，RRT*-Connect でロボットの位置空間上で作成した経路を下図に添付した．

経路生成したい環境の干渉物を散乱させたら，RRT*-Connect のメリットが理解できると考えられる．
他の環境でも試してみようと思った．

RRT-Connect と RRT* と RRT*-Connect のアルゴリズム比較

上記では，各アルゴリズムでの始点から終点までの経路を比較した．
本節では，各アルゴリズムで作成した経路ができるまでの過程を比較する．
アニメーションの見方は以下の通りである．
・水色の星形(中下)データ：経路生成の始点
・緑色の星形(中上)データ：経路生成の終点
・赤色の線：新規データと最短コスト(距離)の親ノードを結んだ線 (始点ツリー)
・オレンジ色の線：新規データと最短コスト(距離)の親ノードを結んだ線 (終点ツリー)
・緑色の線：最終的な始点から終点までの経路
・灰色の球/直方体：干渉物

初めに，RRT-Connect でロボットの位置空間上で作成した経路ができるまでの過程を下図に添付した．

次に，RRT* でロボットの位置空間上で作成した経路ができるまでの過程を下図に添付した．

最後に，RRT*-Connect でロボットの位置空間上で作成した経路ができるまでの過程を下図に添付した．

過程をアニメーション化することによって，各アルゴリズムの強みや弱みを理解しやすくなる．
経路生成したい環境に応じて，探索回数などのパラメータを自動で決定できるような機械学習を組めたら良いなと思った．
他の環境でも試してみたいと思った．

おわりに

本記事では，Pythonを使用して，下記内容を実装しました．
・干渉物の存在する2次元空間で，3軸ロボットアームを用いて RRT*-Connect による経路生成 + アニメーション

次記事では，下記内容を実装していきます．
・干渉物の存在する3次元空間で，6軸ロボットアームを用いて RRT*-Smart による経路生成 + アニメーション
・干渉物の存在する2次元空間で，2軸ロボットアームを用いて，RRT, RRT-Connect, RRT*, RRT*-Connect, RRT*-Smart, Informed RRT* の比較