はじめに

PyBullet (Python上で動く物理シミュレータ) を使用して，ロボットアームを可視化して，動かしたい．
前記事では，PyBullet (Python上で動く物理シミュレータ) で3軸ロボットアームを可視化し，スライダーを用いて手先位置を簡単に変更できるようにした．
(https://qiita.com/haruhiro1020/items/89ed03b9a63e2cb9b76b)
本記事では，下図のような干渉物の存在する環境下で，3軸ロボットアームの経路生成を実装する．黒色の直方体が干渉物である．青色の球が初期位置，赤色の球が目標位置である．

下記アニメーションは本記事で実装する内容である．RRTで干渉回避する経路を探索して，干渉しない始点から終点まで動くアニメーションである．

干渉判定に関しては，PyBulletで提供している関数を使用する．
以前までの記事では，干渉判定用ライブラリである python-fcl を使用していたが，PyBullet内で干渉判定の関数があるため，PyBulletに任せる．

本記事で実装すること

・PyBullet で干渉物が存在する環境下での経路生成(経路生成手法としてRRTを採用)

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

・PyBullet と6軸ロボットアームを動かす
・PyBullet にカメラを搭載し，カメラより干渉物の位置・姿勢を把握して，干渉回避する経路生成
・PyBullet にハンドを搭載し，把持対象物を把持する

動作環境

・macOS Sequoia (バージョン15.5)
・Python3 (3.13.3)
・PyBullet (3.2.5) (物理シミュレータ)
・Numpy (2.3.0) (数値計算用ライブラリ)

PyBullet のインストール方法

PyBullet のインストール方法については前記事にて，説明したため，割愛する．
(前記事 https://qiita.com/haruhiro1020/items/ecc5215234350b663770)

3軸ロボットアームの定義

本記事で動かしたい，3軸ロボットアームについて説明する．
本記事では，下図のような3軸ロボットアームを考えている．
$\theta_{1}$はZ軸方向， $\theta_{2}, \theta_{3}$ はY軸方向へ回転する．

上図のパラメータ $l_{1}, l_{2}, l_{3}, m_{1}, m_{2}, m_{3}$ の値は下表として，考える(値は適当である)．

パラメータ名	概要	値
$l_{1}$	リンク$1$の長さ	1.0 [m]
$l_{2}$	リンク$2$の長さ	1.0 [m]
$l_{3}$	リンク$3$の長さ	1.0 [m]
$m_{1}$	リンク$1$の質量	1.0 [kg]
$m_{2}$	リンク$2$の質量	1.0 [kg]
$m_{3}$	リンク$3$の質量	1.0 [kg]

PyBulletで，上図のロボットを使用するために，URDF (Unified Robot Description Format)を作成する必要がある．

3軸ロボットアームのURDF作成

PyBulletで，3軸ロボットアームを使用するためのURDF作成に関しては，前記事にて説明したため，割愛する．また，URDFの中身も前記事と同じであるため，割愛する．
(前記事 https://qiita.com/haruhiro1020/items/2958018f76ae2b9251e7)

干渉物が存在する環境のURDF作成

本記事では，干渉物が存在する環境をURDFにて，作成する．
環境はロボットではないが，ロボットと見立ててURDFを作成する．
本記事では，URDFのソースコードを記載するが，URDF内のタグについての説明はしない．URDF内のタグについては前記事を参照ください．
(前記事 https://qiita.com/haruhiro1020/items/ecc5215234350b663770)

environment.urdf

<?xml version="1.0" ?>
<robot name="two_boxes_obstacle">


    <link name="base">
        <!-- 見た目，干渉判定，慣性はなし -->
    </link>


    <!-- 直方体を動かさないようのジョイント -->
    <joint name="joint_box1" type="fixed">

        <parent link="base"/>
        <child link="box1"/>
        <origin xyz="0 0 0" rpy="0 0 0"/>

    </joint>


    <!-- 直方体1 -->
    <link name="box1">

        <!-- 見た目 -->
        <visual>
            <origin xyz="1.8 0.5 0.8"/>
            <geometry>
                <box size="0.5 0.5 0.5"/>
            </geometry>
            <material name="black">
                <color rgba="0 0 0 0.5"/>
            </material>
        </visual>

        <!-- 干渉判定 -->
        <collision>
            <origin xyz="1.8 0.5 0.8"/>
            <geometry>
                <box size="0.5 0.5 0.5"/>
            </geometry>
        </collision>

        <!-- 慣性行列 -->
        <inertial>
            <mass value="1.0" />
            <inertia ixx="0.0017" iyy="0.0842" izz="0.0842" ixy="0.0" ixz="0.0" iyz="0.0"/>
        </inertial>

    </link>


    <!-- 直方体を動かさないようのジョイント -->
    <joint name="joint_box2" type="fixed">

        <parent link="base"/>
        <child link="box2"/>
        <origin xyz="0 0 0" rpy="0 0 0"/>

    </joint>


    <!-- 直方体2 -->
    <link name="box2">

        <!-- 見た目 -->
        <visual>
            <origin xyz="1.3 -0.2 0.8"/>
            <geometry>
                <box size="0.5 0.5 0.5"/>
            </geometry>
            <material name="black">
                <color rgba="0 0 0 0.5"/>
            </material>
        </visual>

        <!-- 干渉判定 -->
        <collision>
            <origin xyz="1.3 -0.2 0.8"/>
            <geometry>
                <box size="0.5 0.5 0.5"/>
            </geometry>
        </collision>

        <!-- 慣性行列 -->
        <inertial>
            <mass value="1.0" />
            <inertia ixx="0.0017" iyy="0.0842" izz="0.0842" ixy="0.0" ixz="0.0" iyz="0.0"/>
        </inertial>

    </link>


    <!-- 球を動かさないようのジョイント -->
    <joint name="joint_sphere1" type="fixed">

        <parent link="base"/>
        <child link="sphere1"/>
        <origin xyz="0 0 0" rpy="0 0 0"/>

    </joint>


    <!-- 球1 -->
    <link name="sphere1">

        <!-- 見た目 -->
        <visual>
            <origin xyz="1.0 -1.0 1.0"/>
            <geometry>
                <sphere radius="0.2"/>
            </geometry>
            <material name="blue">
                <color rgba="0 0 1.0 0.5"/>
            </material>
        </visual>

        <!-- 干渉判定，慣性はなし -->

    </link>


    <!-- 球を動かさないようのジョイント -->
    <joint name="joint_sphere2" type="fixed">

        <parent link="base"/>
        <child link="sphere2"/>
        <origin xyz="0 0 0" rpy="0 0 0"/>

    </joint>


    <!-- 球2 -->
    <link name="sphere2">

        <!-- 見た目 -->
        <visual>
            <origin xyz="1.0 1.0 1.0"/>
            <geometry>
                <sphere radius="0.2"/>
            </geometry>
            <material name="red">
                <color rgba="1.0 0 0 0.5"/>
            </material>
        </visual>

        <!-- 干渉判定，慣性はなし -->

    </link>


</robot>

始点や終点を表示している球は，干渉判定の対象とはせずに，見た目だけを存在させている．球を表示しないと，始点と終点の位置がわからなくなるので，デバッグ用として表示させている．

経路生成手法であるRRT

本記事で使用する経路生成手法として，RRTを使用する．
RRTは以前に記事を作成したため，内容は割愛する．
(RRTに関する記事 https://qiita.com/haruhiro1020/items/6eaae645bd638c2b8897)
本記事では，RRTを使用して，干渉回避できる経路を作成する．

PyBulletの使用方法

Pythonの物理シミュレータであるPyBulletの使用方法について説明する．
下記リンクのPyBullet公式で調べながら，PyBulletを使用している．
・https://pybullet.org/wordpress/
上記リンクを開くと，下図のようなサイトに飛ぶ．使用方法を調べるときは，下図の赤枠内の「PYBULLET QUICKSTART GUIDE」タグをクリックする．

「PYBULLET QUICKSTART GUIDE」タグをクリックすると，下図のようなドキュメントを見ることができる．基本的には，ドキュメントに記載されている関数の使用方法を見て，ソースコードを作成している．

PyBulletの関数の引数や戻り値をもっと知りたいのでしたら，上記ドキュメントを見た方がわかりやすいです．

全体のソースコード

はじめに，本記事で使用する全体のソースコードについて説明する．
その後，重要な箇所を抜粋して別途解説をしていく．
ソースコードとして，下記の4ファイルを作成する．
・定数の定義 (constant.py)
・全体的なメイン処理 (main.py)
・PyBulletでロボットを動かす (pybullet_robot.py)
・経路生成手法であるRRT (pybullet_rrt.py)

各ファイルの中身を説明する．

定数の定義 (constant.py)

定数を定義する constant.py について説明する．
本記事では，3軸ロボットアームの説明だが，6軸ロボットアームにも拡張していく予定である．また，経路生成手法として，RRTを採用しているが，他手法にも拡張していく予定である．

constant.py

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


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

# 重力
GRABITY_VALUE   = 9.81

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

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

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

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

# 補間方法の定義
class INTERPOLATION(Enum):
    """
    補間方法
    """
    NONE      = "none"      # 未定義
    JOINT     = "joint"     # 関節補間
    POSITION  = "pos"       # 位置補間

# RRTによる経路生成手法の定義
class PATHPLAN(Enum):
    """
    経路生成手法
    """
    RRT     = "rrt"     # RRTによる経路生成

全体的なメイン処理 (main.py)

全体的なメイン処理を定義する main.py について説明する．
後ほど説明する pybullet_robot.py 内のクラスを実装するのがメインな処理である．
RRTでの探索空間を関節空間や直交空間を選択できるようにしている．

main.py

# Pybullet (Pythonでの3次元物理シミュレータ) による2軸ロボットアームの可視化


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


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



N_ROBOT_AXIS = DIMENTION_3D     # ロボットの関節数
CONST_SEED = 1  # シード値 (常に同じ結果としたいから)


def main():
    """
    メイン処理
    """
    # ロボットが保存されている URDF ファイル名
    if N_ROBOT_AXIS == DIMENTION_2D:    # 2軸ロボットアーム
        robot_urdf = "robot_2dof.urdf"
    elif N_ROBOT_AXIS == DIMENTION_3D:  # 3軸ロボットアーム
        robot_urdf = "robot_3dof.urdf"
    else:
        raise ValueError(f"N_ROBOT_AXIS is abnormal. N_ROBOT_AXIS is {N_ROBOT_AXIS}")

    # 寛容が保存されている URDF ファイル名
    environment_urdf = "environment.urdf"

    # 経路生成手法
    path_plan = PATHPLAN.RRT.value

    # 探索空間を指定
    interpolation = INTERPOLATION.JOINT.value       # 関節空間の探索
    # interpolation = INTERPOLATION.POSITION.value    # 直交空間の探索

    # Pybulletを使用するインスタンス作成
    my_robot = MainPyBulletRobot(robot_urdf, environment_urdf, interpolation)

    # シード値の設定
    np.random.seed(CONST_SEED)

    # 初期位置・終点位置
    if N_ROBOT_AXIS == DIMENTION_2D:    # 2軸ロボットアーム
        start_pos = np.array([1.0, -1.0])
        end_pos   = np.array([1.0,  1.0])
    else:
        start_pos = np.array([1.0, -1.0, 1.0])
        end_pos   = np.array([1.0,  1.0, 1.0])

    if interpolation == INTERPOLATION.JOINT.value:
        # 関節空間の探索時は位置を関節角度に変換
        start_theta = my_robot.convert_pos_to_theta(start_pos)
        end_theta   = my_robot.convert_pos_to_theta(end_pos)
        print(f"start_theta = {start_theta}")
        print(f"end_theta = {end_theta}")

        result = my_robot.run(start_theta, end_theta, path_plan)
    else:
        result = my_robot.run(start_pos,   end_pos,   path_plan)

    print(f"result = {result}")


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

PyBulletでロボットを動かす (pybullet_robot.py)

PyBulletでロボットを動かす処理を定義する pybullet_robot.py について説明する．
経路生成手法である RRT を使用して，シミュレータ上のロボットを動かして，干渉判定を実装している．

pybullet_robot.py

# PyBulletで使用するロボットを記載


# ライブラリの読み込み
import pybullet as p    # PyBullet
import pybullet_data    # PyBulletで使用するデータ
import time             # 時間
import numpy as np      # 数値計算ライブラリ


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


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



class MainPyBulletRobot:
    """
    PyBulletのメインクラス

    プロパティ
        _robot_id(): ロボットアームのID番号
        _n_joints(int): ロボットアームの関節数
        _environment_id(): 県境のID番号
        _interpolation(str): 探索空間 (直交空間/関節空間)
        _rrt(rrt.py内のクラス): 経路生成

    メソッド
        public

            メイン処理関連
                run(): 実行 (始点から終点まで，干渉しない経路を生成)

            運動学関連
                convert_pos_to_theta(): 位置から関節角度に変換 (クラス外で使う用)


        protected

            事前準備関連
                _init_robot(): ロボットの初期化
                _init_environment(): 環境の初期化

            メイン処理関連
                _set_path_plan(): 経路生成手法の設定
                _path_planning(): 始点から終点までの経路生成
                _post_path_planning(): 経路生成の後処理 (経路生成成功時だけ実装)
                _set_jump_joint(): 関節角度にジャンプ
                _set_joint(): 関節角度の設定 (現在位置から移動)

            運動学関連
                _convert_pos_to_theta(): 位置から関節角度に変換 (クラス内で使う用)

            干渉判定処理
                _is_line_interference(): 2点間の干渉判定
                _is_interference_start_end_pos(): 始点と終点が干渉判定していないかの確認
                _is_interference_pos(): 位置にジャンプして干渉判定
    """
    # 定数の定義
    _PLANE_URDF     = "plane.urdf"  # 地面に関する urdf ファイル
    
    _IDX_MIN_JOINT  = 8             # 関節の最小値が保存されている要素番号
    _IDX_MAX_JOINT  = 9             # 関節の最大値が保存されている要素番号
    
    _SIMULATION_SLEEP_TIME = 0.05   # シミュレーションの待機時間 [sec]
    
    _INTERFERENCE_MARGIN   = 0.1    # 干渉判定のマージン [m]
    
    _PATH_PLAN_TIME = 100   # 経路生成の最大時間 [sec]
    _N_MARGIN_MOVE  = 100   # 経路生成終了後の余白時間 [回]
    
    
    def __init__(self, robot_urdf, environment_urdf, interpolation):
        """
        コンストラクタ

        パラメータ
            robot_urdf(str): ロボットアームのファイル名 (urdf)
            environment_urdf(str): 環境のファイル名 (urdf)
            interpolation(str): 補間方法 (関節空間/位置空間)
        """
        # PyBulletの初期化
        p.connect(p.GUI)
        # パスの追加
        p.setAdditionalSearchPath(pybullet_data.getDataPath())
        # シミュレーションの初期化
        p.resetSimulation()
        # 重力の設定 (下(-z軸)方向の加速度)
        p.setGravity(0, 0, -GRABITY_VALUE)

        # ロボットの初期化
        self._init_robot(robot_urdf, interpolation)

        # 環境の初期化
        self._init_environment(environment_urdf)

        self._rrt = None


    # 事前準備メソッド ↓
    def _init_robot(self, robot_urdf, interpolation):
        """
        ロボットの初期化

        パラメータ
            robot_urdf(str): ロボットアームのファイル名 (urdf)
            interpolation(str): 探索方法 (関節空間/位置空間)
        """
        # 引数の確認
        if not (interpolation == INTERPOLATION.JOINT.value or interpolation == INTERPOLATION.POSITION.value):
            # 異常
            raise ValueError(f"interpolation is abnormal. interpolation is {interpolation}")

        # プロパティの更新
        self._interpolation = interpolation

        # ロボットを読み込む．ベースリンクの原点は (x, y, z) = (0, 0, 0) として，ベースリンクは地面に固定
        self._robot_id = p.loadURDF(robot_urdf, basePosition=[0, 0, 0], useFixedBase=True)

        # urdf よりロボットの関節数を取得 (エンドエフェクタ用の仮想関節は不要なため -1)
        self._n_joints = p.getNumJoints(self._robot_id) - 1

        # ロボットの関節数に応じて，robot.py内のクラスを決定
        if not (self._n_joints == DIMENTION_2D or self._n_joints == DIMENTION_3D):
            # 2軸，3軸ロボットアーム以外
            raise ValueError(f"self._n_joints is abnormal. {self._n_joints} is abnormal.")

    def _init_environment(self, environment_urdf):
        """
        環境の初期化
        
        パラメータ
            environment_urdf(str): 環境が保存されているファイル名
        """
        # 地面を読み込む (pybulletが提供している "plane.urdf" を読み込む)
        p.loadURDF(self._PLANE_URDF)

        # 環境を読み込む
        self._environment_id = p.loadURDF(environment_urdf, basePosition=[0, 0, 0], useFixedBase=True)
    # 事前準備メソッド ↑


    # メイン処理 ↓
    def run(self, start_pos, end_pos, path_plan):
        """
        実行
            始点から終点まで，干渉しない経路を生成

        パラメータ
            start_pos(numpy.ndarray): 経路生成の始点
            end_pos(numpy.ndarray): 経路生成の終点
            path_plan(str): 経路生成手法
        
        戻り値
            result(bool): True/False = 経路生成に成功/失敗
        """
        # リアルタイムでのシミュレーション
        p.setRealTimeSimulation(1)

        # 始点と終点で干渉していないかの確認
        self._is_interference_start_end_pos(start_pos, end_pos)

        # 経路生成手法の設定
        self._set_path_plan(path_plan)

        # 経路生成の実装
        result = self._path_planning(start_pos, end_pos)

        if result:
            # 経路生成に成功
            self._post_path_planning(start_pos, end_pos)

        # ファイル保存
        self._rrt.save()

        # 経路生成後の余白時間 (即座にPyBulletが終了するのを防ぐための余白時間)
        for _ in range(self._N_MARGIN_MOVE):
            # 終点に移動
            # PyBulletで関節角度を与えても，与えた関節角度ピッタリにはならず，数値誤差が発生するから，終点へ移動する処理を実施する
            end_theta = self._convert_pos_to_theta(end_pos)
            self._set_joint(end_theta)
            # 待機時間
            time.sleep(self._SIMULATION_SLEEP_TIME)

        return result

    def _set_path_plan(self, path_plan):
        """
        経路生成手法の設定

        パラメータ
            path_plan(str): 経路生成手法名
        """
        if path_plan == PATHPLAN.RRT.value:
            # RRT
            self._rrt = RRTPyBullet()
        else:
            # 異常
            raise ValueError(f"path_plan is abnormal. path_plan is {path_plan}")

    def _path_planning(self, start_pos, end_pos):
        """
        始点から終点までの経路生成

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

        戻り値
            result(bool): True/False = 経路生成に成功/失敗
        """
        result = False

        # 経路生成の準備
        self._rrt.preparation(start_pos, end_pos, self._interpolation)

        start_time = time.time()

        # 始点から終点までの経路が生成するまでループ
        while True:
            now_time = time.time()
            if (now_time - start_time) >= self._PATH_PLAN_TIME:
                # タイムアウト
                break

            # 経路生成を1度実行
            new_node_pos, near_node_pos, near_node = self._rrt.expand_once(end_pos)

            # print(f"new_node_pos = {new_node_pos}")

            if self._is_line_interference(new_node_pos, near_node_pos):
                # 干渉あり
                continue

            # 干渉なしだから，ノード追加 + 経路生成の完了確認
            if not self._rrt.add_node_and_chk_goal(end_pos, new_node_pos, near_node):
                # 終点までの近傍ではない
                continue

            # 新規ノードと始点までの干渉確認
            if not self._is_line_interference(new_node_pos, end_pos):
                # 始点から終点までの経路生成に成功
                result = True
                break

        return result

    def _post_path_planning(self, start_pos, end_pos):
        """
        経路生成の後処理 (経路生成成功時だけ実装)

        パラメータ
            start_pos(numpy.ndarray): 経路生成の始点
            end_pos(numpy.ndarray): 経路生成の終点
        """
        # 経路生成の終了処理
        self._rrt.fin_planning(start_pos, end_pos)

        # 始点に移動
        theta = self._convert_pos_to_theta(start_pos)
        self._set_jump_joint(theta)

        # print(f"self._rrt.pathes = {self._rrt.pathes}")

        # 始点から終点までの経路を移動
        for row_idx in range(self._rrt.pathes.shape[0]):
            next_theta = self._convert_pos_to_theta(self._rrt.pathes[row_idx])
            self._set_joint(next_theta)
            # 待機時間
            time.sleep(self._SIMULATION_SLEEP_TIME)

    def _set_jump_joint(self, thetas):
        """
        関節角度をジャンプ

        パラメータ
            thetas(numpy.ndarray): 関節角度
        """
        for i in range(len(thetas)):
            # 関節角度を設定
            p.resetJointState(
                bodyUniqueId=self._robot_id,    # IDの設定
                jointIndex=i,                   # 関節番号の設定
                targetValue=thetas[i]           # 関節角度
            )

    def _set_joint(self, thetas):
        """
        関節角度の設定 (現在位置から移動)

        パラメータ
            thetas(numpy.ndarray): 設定したい関節角度
        """
        for i in range(len(thetas)):
            # 関節角度を設定
            p.setJointMotorControl2(
                bodyUniqueId=self._robot_id,    # IDの設定
                jointIndex=i,                   # 関節番号の設定
                controlMode=p.POSITION_CONTROL, # 位置制御
                targetPosition=thetas[i]        # 関節角度
            )
    # メイン処理 ↑


    # 干渉判定処理 ↓
    def _is_line_interference(self, pos1, pos2):
        """
        2点間の干渉判定

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

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

        # 2点の干渉判定
        if self._is_interference_pos(pos2):
            return is_interference
        if self._is_interference_pos(pos1):
            return is_interference

        # pos1からpos2へ移動
        theta = self._convert_pos_to_theta(pos2)
        self._set_joint(theta)

        # 待機時間
        time.sleep(self._SIMULATION_SLEEP_TIME)

        # ロボットと干渉物との干渉判定
        close_points = p.getClosestPoints(self._robot_id, self._environment_id, self._INTERFERENCE_MARGIN)
        if len(close_points) == 0:  # 干渉なし
            is_interference = False

        return is_interference

    def _is_interference_start_end_pos(self, start_pos, end_pos):
        """
        始点と終点が干渉判定していないかの確認

        パラメータ
            start_pos(numpy.ndarray): 経路生成の始点 (直交空間/関節空間)
            end_pos(numpy.ndarray): 経路生成の終点 (直交空間/関節空間)
        """
        # 始点の干渉判定
        if self._is_interference_pos(start_pos):
            raise ValueError("start_pos is interference. change start_pos.")

        # 終点の干渉判定
        if self._is_interference_pos(end_pos):
            raise ValueError("end_pos is interference. change end_pos.")

    def _is_interference_pos(self, pos):
        """
        位置にジャンプして干渉判定

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

        # 位置から関節角度に変換
        theta = self._convert_pos_to_theta(pos)
        # print(f"theta = {theta}")
        # 位置にジャンプ
        self._set_jump_joint(theta)

        # 待機時間
        time.sleep(self._SIMULATION_SLEEP_TIME)

        # ロボットと干渉物との干渉判定
        close_points = p.getClosestPoints(self._robot_id, self._environment_id, self._INTERFERENCE_MARGIN)
        if len(close_points) == 0:  # 干渉なし
            is_interference = False

        return is_interference
    # 干渉判定処理 ↑


    # 運動学関連 ↓
    def convert_pos_to_theta(self, pos):
        """
        位置から関節角度に変換 (クラス外で使う用)

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

        戻り値
            thetas(numpy.ndarray): 関節角度
        """
        if pos.shape[0] == DIMENTION_2D:
            pos = np.append(pos, 0.0)
        # エンドエフェクタのリンク要素はベースリンクを除いた要素番号となる
        thetas = p.calculateInverseKinematics(self._robot_id, self._n_joints, pos)
        thetas = np.array(thetas)

        return thetas

    def _convert_pos_to_theta(self, pos):
        """
        位置から関節角度に変換 (クラス内で使う用)

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

        戻り値
            thetas(numpy.ndarray): 関節角度
        """
        if self._interpolation == INTERPOLATION.POSITION.value:
            # 逆運動学により，関節角度を返す
            # 逆運動学には，(x, y, z)の3次元データが必須なため，2次元の場合はzのデータも増やす
            if pos.shape[0] == DIMENTION_2D:
                pos = np.append(pos, 0.0)
            # エンドエフェクタのリンク要素はベースリンクを除いた要素番号となる
            thetas = p.calculateInverseKinematics(self._robot_id, self._n_joints, pos)
            thetas = np.array(thetas)
        else:
            # pos が関節角度のため，そのまま返す
            thetas = np.copy(pos)

        return thetas
    # 運動学関連 ↑

ソースコードの中身は前記事と大方同じであるため，本記事では割愛する．
(前記事 https://qiita.com/haruhiro1020/items/cc29d72d291158fdc1ce)

経路生成手法であるRRT (pybullet_rrt.py)

経路生成手法であるRRTの処理を定義する pybullet_rrt.py について説明する．
RRT内ではツリーの作成を実施する．干渉判定はPyBulletで実施するため，RRT内には干渉判定を実装しない．

pybullet_rrt.py

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


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


# 自作モジュールの読み込み
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 RRTPyBullet:
    """
    RRTにロボットを追加したクラス

    プロパティ
        _name(str): 経路生成手法名
        _pathes(numpy.ndarray): 始点から終点までの経路 (PruningやShortcut済みの経路)
        _start_tree(Tree): 始点ツリー
        _strict_min_pos(numpy.ndarray): 探索の最小範囲
        _strict_max_pos(numpy.ndarray): 探索の最大範囲
        _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プロパティのゲッター
                pathes(): _pathesプロパティのゲッター

            準備処理
                preparation(): 経路生成の準備

            メイン処理関連
                expand_once(): ランダム探索による新規ノードおよび最短ノードの取得 (干渉判定は実装しない)
                add_node_and_check_goal(): ノード追加 + 経路生成の完了確認
                fin_planning(): 経路生成の終了処理

            ファイル関連
                save(): 生成した経路をファイル保存

        protected

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

            準備処理関連
                _reset(): データの初期化
                _set_interpolation(): 経路生成したい探索空間の設定
                _strict_planning_pos(): 探索範囲を制限する

            ファイル関連
                _make_folder(): フォルダーの作成
                _reset_folder(): フォルダー内のファイルを全削除
                _get_path_plan_folders(): 経路生成結果を保存する複数のフォルダー名を取得
                _make_path_plan_folder(): 経路生成結果を保存するフォルダー作成
                _save_numpy_data_to_txt(): Numpyデータをテキストファイルに保存
    """
    # 定数の定義
    # ファイル名の定義
    # 各経路生成手法で絶対に定義するべきファイル名 ↓
    # _pathesプロパティを保存するファイル名
    _FILE_NAME_PATHES = f"{PATHPLAN.RRT.value}_pathes.csv"
    # _start_treeプロパティを保存するファイル名
    _FILE_NAME_START_TREE = f"{PATHPLAN.RRT.value}_start_tree.csv"
    # 各経路生成手法で絶対に定義するべきファイル名 ↑

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

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

    _GOAL_SAMPLE_RATE = 0.1     # ランダムな値を取るときに，終点を選択する確率


    def __init__(self):
        """
        コンストラクタ
        """
        self._name   = PATHPLAN.RRT.value
        self._pathes = []
        self._start_tree    = Tree(self._NODE_NEAR_NODE_IDX)
        self._interpolation = INTERPOLATION.NONE.value
        self._moving_value  = self._MOVING_VALUE_JOINT
        self._dim = DIMENTION_NONE


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

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


    # メイン処理メソッド ↓
    def expand_once(self, end_pos):
        """
        ランダム探索による新規ノードおよび最短ノードの取得 (干渉判定は実装しない)

        パラメータ
            end_pos(list): 経路生成の終点

        戻り値
            new_node_pos(numpy.ndarray): 新規ノード
            near_node_pos(numpy.ndarray): 最短ノード位置
            near_node(int): 最短ノード番号
        """
        # ランダムな値を取得
        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_node_pos  = self._calc_new_pos(random_pos, near_node_pos)

        return new_node_pos, near_node_pos, near_node

    def add_node_and_chk_goal(self, end_pos, node_pos, near_node):
        """
        ノード追加 + 経路生成の完了確認

        パラメータ
            end_pos(numpy.ndarray): 経路生成の終点
            node_pos(numpy.ndarray): ノード位置
            near_node(int): 親ノード

        戻り値
            is_successful(bool): True/False = 経路生成の完了/未完了
        """
        # 処理結果
        is_successful = False

        # 始点ツリーにノードを追加
        self._add_node_start_tree(node_pos, near_node)

        # 終点との距離が一定範囲内であるかの確認
        if self._chk_end_pos_dist(node_pos, end_pos):
            # 一定範囲内のため，経路生成の完了
            is_successful = True

        return is_successful

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

        パラメータ
            pos(numpy.ndarray): 位置
            near_node(int): 最短ノード
        """
        # _start_treeにノードを追加
        node = np.append(pos, near_node).reshape(1, -1)
        self._start_tree.add_node(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:
            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, 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(self, start_pos, end_pos, interpolation):
        """
        経路生成の準備

        パラメータ
            start_pos(list): 経路生成の始点
            end_pos(list): 経路生成の終点
            interpolation(int): 補間方法 (関節空間/位置空間)
        """
        # データの初期化
        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._set_interpolation(interpolation)

        # 始点ノードをツリーに追加
        self._add_node_start_tree(start_pos, INITIAL_NODE_NEAR_NODE)

        # 探索範囲を設定
        self._strict_planning_pos(start_pos, end_pos)

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

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

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

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

        self._interpolation = INTERPOLATION.NONE.value
        self._dim = DIMENTION_NONE

    def _set_interpolation(self, interpolation):
        """
        経路生成したい探索空間の設定

        パラメータ
            interpolation(int): 補間の種類 (関節補間/位置補間)
        """
        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
        print(f"self._strict_min_pos = {self._strict_min_pos}")
        print(f"self._strict_max_pos = {self._strict_max_pos}")
    # 準備処理関連メソッド ↑


    # ファイル関連メソッド ↓
    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 = [os.path.join(self._name, interpolation), ]

        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):
        """
        生成した経路をファイル保存
        """
        # 始点から終点までの修正済みの経路をファイル保存
        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)

    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)
    # ファイル関連メソッド ↑

RRT内で干渉判定を実施できないため，RRTクラスの実装側で干渉判定の結果に応じて，処理を実装できるようにpubic関数を用意した．
RRTクラスの実装側で，呼んでほしくない処理はprotected関数として，public関数を最小限とした．
今回は，RRTによる経路生成だが将来的にはRRTの拡張アルゴリズムを使用する予定である．
RRTに関しては，私の他記事内やコメントを記載したため，各関数についての説明は割愛する．

PyBulletでロボットを動かす

上記にて，ロボットのURDF，ソースコードを説明した．
main.py ファイルを実施することによって，PyBullet上のロボットを動かしていく．
探索空間を直交空間と関節空間に分けてロボットを動かした動画を載せる．

直交空間で探索した時の動画

直交空間で探索した時のロボットの動画を下図に2つ載せる．

動画１：始点から終点まで干渉しない経路を生成するための探索

動画２：探索後の始点から終点までの経路

ロボットが色々と動いて，干渉が生じないような経路を探索しているのがわかる．
また，最終的な始点から終点までの経路では，干渉回避できていることが見て取れる．干渉物ギリギリを動いている経路が作成できた．
なかなかZ軸方向への探索ができていなかったため，探索に時間がかかってしまった．環境に合わせてパラメータを調整すると良い結果が得られそうだ．

関節空間で探索した時の動画

関節空間で探索した時のロボットの動画を下図に2つ載せる．

動画３：始点から終点まで干渉しない経路を生成するための探索

動画４：探索後の始点から終点までの経路

関節空間で作成した経路は，直交空間で作成した経路と比べて，干渉物を遠回りする経路となった．関節空間内では始点から終点まで直線的に移動させても，直交空間上では少し遠回りをするような経路となることが，要因として挙げられる．
環境に応じて，得意な探索空間がありそうだ．
始点から終点までの経路が遠回りとなったため，経路生成手法を RRT から RRT* に変更したら，最適な経路が作成できると考えている．

おわりに

本記事では，Pythonを使用して，下記内容を実装しました
・PyBullet で干渉物が存在する環境下で，3軸ロボットアームの経路生成 (経路生成手法としてRRTを採用)

次記事では，下記内容を実装していきます．
・PyBullet で6軸ロボットアームを動かす (関節角度を自由に設定)
(https://qiita.com/haruhiro1020/items/75d95e1628fdea0743f4)