経路生成(RRT)に干渉判定ライブラリ(python-fcl)を組み込んでみた

はじめに

前記事で，干渉判定用ライブラリであるpython-fclの使用方法を簡単に説明した．
以前の記事では，経路生成RRTおよび処理時間を計測するline_profilerを実装した．
本記事では，経路生成RRTにpython-fclを組み込み，処理時間の短縮を図る．

本記事で実装すること

・経路生成手法であるRRTに干渉判定用ライブラリであるpython-fclを組み込む
・処理時間を計測する

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

・RRT以外の経路生成手法にpython-fclを組み込む

動作環境

・macOS Sequoia (バージョン15.5)
・Python3 (3.10.9)
・Numpy (1.23.5) (数値計算用ライブラリ)
・Matplotlib (3.7.0) (アニメーション作成用ライブラリ)
・line_profiler (4.1.2) (関数内の各処理時間を計測するライブラリ)
・python-fcl (0.7.0.8) (干渉判定ライブラリ)

経路生成手法のまとめ

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

RRTに関して

RRTに関して説明は，以前に説明したため，割愛する．
(Webサイトを記載)

経路生成を実施する環境に関して

下図に示した環境を使用して，経路生成を実装する．
下図環境下で，干渉判定を実施する．

また，干渉物を AABB(Axis-Aligned Bounding Box(座標軸に沿ったバウンディングボックス)) として考えることで，高速な干渉判定が実装できる．

2点間をカプセルと見立てて干渉判定

2点間の干渉判定を実施するときは，2点間をカプセルと見立てて干渉判定を実施する．
処理内容を下図に記す．

2点間をカプセルとして，干渉物と干渉判定を実施する．
カプセルと見立てることで，2点間の干渉判定漏れを無くすことができる．

ソースコード

定数を定義するファイル (main.py)

定数を定義しているソースコードを下記に記す．

constant.py

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


# 経路生成アルゴリズムを定義
PATH_PLANNING_RRT               = 10    # RRTによる経路生成

# ノードの最短ノード要素とコスト要素を定義
RRT_NEAR_NODE_IDX               = -1    # RRTでの最短ノード要素

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

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

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

# 経路生成の平均処理時間などを算出するためのサンプル数
PATH_PLANNING_NUM         = 100          # 経路生成のサンプル数

# 干渉物の名称
class INTERFERENCE(Enum):
    CIRCLE      = "circle"      # 円
    RECTANGLE   = "rectangle"   # 長方形
    BALL        = "ball"        # 球
    CUBOID      = "cuboid"      # 直方体

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

干渉物を定義した環境ファイル (environment.py)

干渉物を定義した環境のソースコードを下記に記す．

# python-fclによる干渉判定チェック

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

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

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


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

        # 円形の干渉物を定義 (円の位置 (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] = circles

        # # 長方形の干渉物を定義
        # 長方形の長さ (x, y) と中心点の位置 (x, y)
        # rectangles = [[0.2, 0.3, 0.6, 0.6], [0.4, 0.5, 0.8, 1.0], [1.0, 1.0, 0.8, 0.8]]
        # for x, y, center_x, center_y in rectangles:
        #     # 直方体の長さ (x, y, z) を設定
        #     rectangle = fcl.Box(x, y, self._Z_VALUE)
        #     # 長方形の中心点を設定．無回転とする
        #     translation = fcl.Transform(np.array([center_x, center_y, self._Z_VALUE]))
        #     obj = fcl.CollisionObject(rectangle, translation)
        #     # モデルを追加
        #     objects.append(obj)
        # # 長方形の干渉物を保存
        # self._interferences[INTERFERENCE.RECTANGLE] = rectangles

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

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

    @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):
        """
        干渉判定 + 最短距離

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

        戻り値
            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 > 0:
            # 干渉なし
            collision = False
        else:
            # 干渉あり
            collision = True

        return collision


class Base3DEnv:
    """
    経路生成時の3次元環境
    
    プロパティ
        _distance(float): 干渉物との最短距離
        _inteferences(dist): 干渉物の情報 (名称(球や直方体など) + 形状(半径，中心点など))
    
    メソッド
        public
            inteferences(): _inteferencesプロパティのゲッター
            distance(): _distanceプロパティのゲッター
            is_collision(): 干渉判定
            is_collision_dist(): 干渉判定 + 最短距離
    """
    # 定数の定義
    
    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] = balls

        # # 直方体の干渉物を定義
        # # 直方体の長さ (x, y, z) と中心点の位置 (x, y, z)
        # cuboids = [[0.2, 0.3, 0.3, 0.6, 0.6, 0.6], [0.4, 0.5, 0.6, 0.8, 1.0, 1.0], [1.0, 1.0, 1.0, 0.8, 0.8, 0.8]]
        # for x, y, z, center_x, center_y, center_z in cuboids:
        #     # 直方体の各辺の長さを設定
        #     cuboid = fcl.Box(x, y, z)
        #     # 長方形の中心点を設定．無回転とする
        #     translation = fcl.Transform(np.array([center_x, center_y, center_z]))
        #     obj = fcl.CollisionObject(cuboid, translation)
        #     # モデルを追加
        #     objects.append(obj)
        # # 直方体の干渉物を保存
        # self._interferences[INTERFERENCE.CUBOID] = cuboids

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

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

    @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):
        """
        干渉判定 + 最短距離

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

        戻り値
            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 > 0:
            # 干渉なし
            collision = False
        else:
            # 干渉あり
            collision = True

        return collision

干渉物を定義した環境ファイル (environment.py)

干渉物を定義した環境のソースコードを下記に記す．

rrt.py

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

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

# 自作モジュールの読み込み
from constant import *              # 定数
from environment import Base2DEnv, Base3DEnv    # 環境


class Tree:
    """
    ツリークラス
    
    プロパティ
        _nodes(numpy.ndarray): ノード
        _near_node_idx(int): _nodes内の最短ノードを保存している列番号
    
    メソッド
        public
            nodes(): _nodesプロパティのゲッター
            add_node(): ノードの追加
            get_near_node(): 最短距離のノードを取得
    """
    # 定数の定義
    
    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
        # 距離を計算
        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 RRT:
    """
    RRTクラス
    
    プロパティ
        _pathes(numpy.ndarray): 始点から終点までの経路
        _start_tree(Tree): 始点ツリー
        _start_random_tree(Tree): ランダムな位置を保存した始点ツリー
        _strict_min_pos(numpy.ndarray): 探索の最小範囲
        _strict_max_pos(numpy.ndarray): 探索の最大範囲
        _environment(Base2DEnv): 経路生成したい環境
        _debug(bool): デバッグフラグ (グラフやアニメーションでデータを確認したいかどうか)
    
    メソッド
        public
            planning(): 経路生成の実装
            pathes(): _pathesプロパティのゲッター
            start_tree(): _treeプロパティのゲッター
            start_random_tree(): _random_treeプロパティのゲッター
            strict_min_pos(): _strict_min_posプロパティのゲッター
            strict_max_pos(): _strict_max_posプロパティのゲッター
            environment(): _environmentプロパティのゲッター
        
        protected
            _reset(): データの初期化
            _add_node(): ツリーにノードを追加する
            _chk_end_pos_dist(): 終点との距離が一定範囲内であるかの確認
            _fin_planning(): 経路生成の終了処理
            _calc_new_pos(): 最短ノードからランダムな値方向へ新しいノード(位置)を作成
            _strict_planning_pos(): 探索範囲を制限する
            _get_random_pos(): ランダムな位置を取得
            _line_interference(): 2点間の干渉チェック
            _is_interference_pos(): 干渉判定処理
            _save(): データの保存
    """
    # 定数の定義
    # ファイル名の定義
    _FILE_NAME_PATHES = "rrt_pathes.csv"                        # _pathesプロパティを保存するファイル名
    _FILE_NAME_START_TREE = "rrt_start_tree.csv"                # _start_treeプロパティを保存するファイル名
    _FILE_NAME_START_RANDOM_TREE = "rrt_start_random_tree.csv"  # _start_random_treeプロパティを保存するファイル名

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

    # 干渉判定の定義
    _DEVIDED_DISTANCE = 0.01    # 2点間を分割する際の基準距離
    _NODE_RADIUS = 0.05         # ノードの半径

    # 探索に関する定義
    _MOVING_VALUE = 0.25        # 1回の移動量
    _STRICT_PLANNING_POS = 0.2  # 探索範囲の制限
    _TIMEOUT_VALUE = 10         # タイムアウト時間 [second]
    _GOAL_SAMPLE_RATE = 0.1     # ランダムな値を取るときに，終点を選択する確率

    _Z_VALUE_2D         = 0.0   # 2次元の時のz値
    _INITIAL_NODE_NEAR = -1     # 初期値の最短距離ノード番号

    def __init__(self):
        """
        コンストラクタ
        """
        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

    @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

    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 _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 None:
            del self._environment
        self._environment = None

    def _set_environment(self, environment):
        """
        経路生成した環境を設定

        パラメータ
            environment(list): 経路生成したい環境
        """
        if environment is not None:
            self._environment = environment

    def planning(self, start_pos, end_pos, environment, debug=False):
        """
        経路生成

        パラメータ
            start_pos(list): 経路生成の始点
            end_pos(list): 経路生成の終点
            environment(Base2DEnv): 環境
            debug(bool): デバッグフラグ
        
        戻り値
            result(bool): True / False = 成功 / 失敗
        """
        # 処理結果
        result = False

        # デバッグフラグの更新
        self._debug = debug
        # データの初期化
        self._reset()

        # 始点と終点の次元数が一致しているかの確認
        start_pos_dim = np.size(start_pos)
        end_pos_dim   = np.size(end_pos)
        if start_pos_dim != end_pos_dim:
            # 次元数が異なるので異常
            return result

        self._dim = start_pos_dim

        # 環境を設定
        self._set_environment(environment)

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

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

        # 終点までの経路生成が終わるまでループ
        while True:
            # ランダムな値を取得
            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)
            # ファイルに保存する
            self._save()

        return result

    def _line_interference(self, pos1, pos2):
        """
        2点間の干渉チェック

        パラメータ
            pos1(numpy.ndarray): 位置1
            pos2(numpy.ndarray): 位置2
        
        戻り値
            is_interference(bool): True / False = 干渉あり / 干渉なし
        """
        is_interference = False

        if self._environment is None:
            # 干渉物ないため，常に干渉なし
            return is_interference

        # 2点間をカプセルとして干渉物を定義
        obj = self._make_capsule_obj(pos1, pos2)
        # 環境とカプセルの干渉判定 (最短距離はself._environmentのdistanceに保存される)
        # is_interference = self._environment.is_collision_dist(obj)
        is_interference = self._environment.is_collision(obj)
        # print(f"dist = {self._environment.distance}")

        return is_interference

    def _make_capsule_obj(self, pos1, pos2):
        """
        2点間をカプセルとして干渉物を作成

        パラメータ
            pos1(numpy.ndarray): 点1
            pos2(numpy.ndarray): 点2

        戻り値
            obj(fcl.CollisionObject): 干渉物
        """
        # 2点間の差分を計算
        difference = pos2 - pos1
        # 2点間の距離(ユークリッド距離)を計算
        distance = np.linalg.norm(difference)
        # 2点間の回転行列を計算
        rotation = self._rotation_to_world_frame(pos1, pos2)
        # 中心点を算出
        center_pos = (pos1 + pos2) / 2

        # RRTで生成した点を円/球と考えて，2点をカプセルとして定義する
        capsule = fcl.Capsule(self._NODE_RADIUS, distance)                  # カプセルの球の半径と円柱の高さを定義

        if self._dim == DIMENTION_2D:
            # fclでは中心点を(x, y, z)としないといけないから，z軸の値を追加
            pos = np.zeros(3)
            pos[:2] = center_pos
            # fclでは回転行列を3 * 3としないといけないから，z方向も追加
            rot = np.eye(3)         # 無回転の回転行列
            rot[:2, :2] = rotation  # z軸方向に回転させる
            # カプセルの中心点が2点間の中点，回転行列は2点間の回転行列
            translation = fcl.Transform(rot, pos)
        else:
            # カプセルの中心点は2点間の中点
            translation = fcl.Transform(rotation, center_pos)

        obj = fcl.CollisionObject(capsule, translation)

        return obj

    def _rotation_to_world_frame(self, pos1, pos2):
        """
        点1から点2方向の回転行列を作成 (Informed RRT* を参考にした)

        パラメータ
            pos1(numpy.ndarray): 点1
            pos2(numpy.ndarray): 点2
        
        戻り値
            Rotation(numpy.ndarray): 回転行列
        """
        # 点1から点2までの単位方向ベクトルを作成
        difference  = pos2 - pos1
        # 0割回避のために微小な値を分母に追加
        norm_vector = difference / (np.linalg.norm(difference) + EPSILON)
        eye = np.eye(self._dim)         # 単位行列

        # 単位方向ベクトルと単位行列の1行目の内積を計算して，行列とする
        cross_matrix = np.dot(norm_vector.reshape(-1, 1), eye[:, 0].reshape(1, -1))

        # 行列の特異値分解
        U, S, V_T = np.linalg.svd(cross_matrix)

        # 特異値の対角要素の最終要素を U, V_T に置き換える
        diagonal = np.diagflat(np.ones(self._dim))
        diagonal[-1, -1] = np.linalg.det(U) * np.linalg.det(V_T)

        # 修正した特異値を使用して，行列に戻す
        Rotation = np.dot(U, diagonal)
        Rotation = np.dot(Rotation, V_T)

        return Rotation

    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
        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 == self._INITIAL_NODE_NEAR:
                # 始点ノードまで取得できたため，処理終了
                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 _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)
        self._strict_min_pos = min_pos - self._STRICT_PLANNING_POS
        self._strict_max_pos = max_pos + self._STRICT_PLANNING_POS

    def _get_random_pos(self, 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 _save(self):
        """
        生成した経路をファイル保存
        """
        if self._debug:
            np.savetxt(self._FILE_NAME_PATHES,              self._pathes)
            np.savetxt(self._FILE_NAME_START_TREE,          self._start_tree.nodes)
            np.savetxt(self._FILE_NAME_START_RANDOM_TREE,   self._start_random_tree.nodes)

メイン処理 (main.py)

メイン処理のソースコードを下記に記す．

main.py

# メイン処理を実施

# ライブラリの読み込み
import numpy as np                  # 数値計算
import time                         # 時間計測
from line_profiler import LineProfiler  # 関数の処理時間を計測

# 自作モジュールの読み込み
from rrt import RRT             # 経路生成アルゴリズム
from environment import Base2DEnv, Base3DEnv      # 干渉物の環境
from constant import *              # 定数


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


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

    # RRTクラスのインスタンスを作成
    rrt = RRT()
    # 始点から終点までの経路を生成
    start_time = time.time()
    # 視点から終点までの経路生成
    planning_result = rrt.planning(start_pos, end_pos, interference_obj)
    if planning_result:
        end_time = time.time()
        # 処理時間を計算
        elapsed_time = end_time - start_time
        # 経由点数を計算
        n_waypoint = rrt.pathes.shape[0]

    return planning_result, elapsed_time, n_waypoint


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

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

    # 始点と終点を作成
    start_pos = START_POS * np.ones(dimention).reshape(1, -1)
    end_pos   = END_POS   * np.ones(dimention).reshape(1, -1)

    if dimention == DIMENTION_2D:
        # 円形の干渉物を作成
        interference_obj = Base2DEnv()
    elif dimention == DIMENTION_3D:
        # 球の干渉物を作成
        interference_obj = Base3DEnv()
    else:
        # 干渉物はなし
        interference_obj = []

    if path_plan == PATH_PLANNING_RRT:                      # RRTによる経路生成
        planning_result, elapsed_time, n_waypoint = rrt_planning(dimention, plot_mode, start_pos, end_pos, interference_obj)

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


def main():
    """
    メイン処理
    """
    # プロットできる状態
    plot_modes = [PLOT_NONE, ]          # 処理時間を計測する
    # 経路生成アルゴリズム
    path_plans = [PATH_PLANNING_RRT, ]  # RRT だけを実施する
    # 次元数
    dimentions = [DIMENTION_2D, ]       # 2次元

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

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

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

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

                    if elapsed_times:
                        # 処理時間の平均，最大，最小，標準偏差を出力
                        print(f"time ave = {np.mean(elapsed_times)}")
                        print(f"time max = {np.max( elapsed_times)}")
                        print(f"time min = {np.min( elapsed_times)}")
                        print(f"time std = {np.std( elapsed_times)}")
                    if n_waypoints:
                        # 経由点の平均，最大，最小，標準偏差を出力
                        print(f"waypoint ave = {np.mean(n_waypoints)}")
                        print(f"waypoint max = {np.max( n_waypoints)}")
                        print(f"waypoint min = {np.min( n_waypoints)}")
                        print(f"waypoint std = {np.std( n_waypoints)}")

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

処理結果

上記のmain.pyを実行して，100回経路を生成した．
その時の結果を下表にまとめる．

平均処理時間 [sec]	最小処理時間 [sec]	最大処理時間 [sec]	処理時間の標準偏差 [sec]	平均経由点数	最小経由点数	最大経由点数	経由点数の標準偏差
0.00547	0.00222	0.02911	0.00332	20.8	25.0	18.0	1.6

現状は2点間をカプセルとして考えているが，将来的にはロボットも組み込みたい．python-fclでは，干渉物との最短距離を算出できるため，ロボットにも応用できる．

おわりに

本記事では，Pythonを使用して，下記内容を実装しました．
・経路生成のRRT + python-fcl(干渉判定ライブラリ)

次記事では，下記内容を実装していきます．
・2軸ロボットアームの順運動学習の説明