はじめに

PyBullet (Python上で動く物理シミュレータ) を使用して，ロボットアームを可視化して，動かしたい．
前記事では，PyBullet (Python上で動く物理シミュレータ) で2軸ロボットアームにグリッパーを追加して，立方体を把持するシミュレーションを実装した．
(https://qiita.com/haruhiro1020/items/01f661a571c6fe76c551)

本記事では，前記事の続きで，グリッパー付きの2軸ロボットアームを使用して，干渉物が存在する環境下にて，物体を把持する(下図はPyBullet上のグリッパー付き2軸ロボットアームである)．黒枠が干渉物，赤枠がグリッパー，緑枠が把持したい物体である．

本記事では，ロボットが自動で把持物体の位置まで干渉しないように移動し，物体を把持するまでを実装する．そのため，把持した物体を他の場所へ移動させることは実装していない．実際に動かしている動画は下図の通りである．

本記事で実装すること

・干渉物が存在する環境下での経路生成 + 物体把持

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

・3軸，6軸ロボットアームへの対応
・カメラによる物体の位置把握 + 干渉物が存在する環境下での経路生成 + 物体把持

動作環境

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

PyBullet のインストール方法

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

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

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

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

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

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

グリッパーの定義

本記事では，下図のようなパラレルグリッパーをロボットアームに追加する．

上図はグリッパーが開いた時の画像である．下図はグリッパーを閉じた時の画像である．2本の指の間に把持物体を置いた状態で，グリッパーを閉じることで，物体把持を実施する．

パラレルグリッパーを採用した理由は下記の通りである．
・URDFの作成および制御が容易
・立方体および直方体の場合，接触する面積が大きくなるため，摩擦力が大きくなり滑りにくい

人の手である "多指グリッパー" も存在するが，URDFを作成するのが困難であるため，採用しない．

把持対象物に関して

本記事では，把持対象物を立方体とする．
また，把持までの流れは以下の通りとなる．

１：始点から終点まで，干渉しない経路を探索する
２：干渉しない始点から終点までの経路を移動する
３：物体把持

ここで，重要なことは上記の「１」の時に，ロボットと把持対象物がぶつかってしまう可能性があること．ぶつかってしまうと，把持対象物が指定した位置から大きくずれた位置に動いてしまう．
上記を考慮して，把持対象物は経路探索時に，ぶつかっても動かないように拘束させる．PyBulletでは，拘束させる(動かさないようにする)ことが可能である．

グリッパー付き2軸ロボットアームのURDF作成

グリッパー付き2軸ロボットアームのURDFを作成する．URDFに関しては，前記事で説明したため，本記事では割愛する．
(前記事 https://qiita.com/haruhiro1020/items/01f661a571c6fe76c551)

経路生成手法である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 について説明する．

constant.py

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


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

# 重力
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 内のクラスを実装するのがメインな処理である．

main.py

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


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


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



CONST_SEED = 1  # シード値 (常に同じ結果としたいから)
HAND_FLG   = True   # ハンドの装着有無


def main():
    """
    メイン処理
    """
    # ロボットが保存されている URDF ファイル名
    if HAND_FLG:
        # ハンド付きのURDF
        robot_urdf = "robot_2dof_hand.urdf"
    else:
        # ハンドなしのURDF
        robot_urdf = "robot_2dof.urdf"

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

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

    # 把持対象物が保存されている URDF ファイル名
    grasp_urdf = "grasp_object.urdf"

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

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

    # 初期位置
    start_pos = np.array([1.0, -1.0])

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

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

    print(f"result = {result}")


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

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

PyBulletでロボットを動かす処理を定義する pybullet_robot.py について説明する．

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 BaseGripper:
    """
    グリッパーのベースクラス (抽象クラス)

    プロパティ
        _robot_id(): ロボットアームのID番号
        _n_joints(int): ロボットアームの関節数

    メソッド
        public
            run(): 実行 現在角度を関節に与える (オープン・クローズ以外で常に呼ぶこと)
            open(): グリッパーのオープン
            close(): グリッパーのクローズ
    """
    # 定数の定義
    _JOINT_CURRENT_VALUE_IDX = 0    # 関節の現在値の要素番号
    
    
    def __init__(self, robot_id, n_joint):
        """
        コンストラクタ

        パラメータ
            robot_id(p.loadURDFの戻り値): ロボットURDFを読み込んだ際のID
            n_joint(int): ロボットの関節数 (グリッパーは含むが，グリッパー先端は含まない)
        """
        # プロパティの初期化
        self._robot_id = robot_id
        self._n_joint  = n_joint
        
        
    def run(self):
        """
        実行 (毎時刻，本関数を呼ぶこと)
        """
        raise InterruptedError("run() is necessary override.")


class ParallelGripper(BaseGripper):
    """
    パラレルグリッパー (2本の指が並行に動く)

    メソッド
        public
            実行 (毎時刻，本関数を呼ぶこと)

        protected
            _get_move_direction(): グリッパーの移動方向を取得
            _get_gripper_right_left_idx(): グリッパーの右・左の関節番号
            _set_joint_values(): 関節角度[m]を設定
            _get_joint_values(): 関節角度[m]を取得
    """
    # 定数の定義
    _GRIPPER_RIGHT_IDX =  -2    # 右グリッパーの関節番号
    _GRIPPER_LEFT_IDX  =  -1    # 左グリッパーの関節番号
    
    _GRIPPER_MOVE_VAL  = 0.01   # グリッパーの1回あたりの移動量 [m]
    _GRIPPER_LATERAL_FRIC = 1.0 # グリッパーの摩擦係数
    
    _GRIPPER_CLOSE_VAL = 0.03   # グリッパーのクローズ時のフィンガー角度 [m]
    _GRIPPER_OPEN_VAL  = 0      # グリッパーのクローズ時のフィンガー角度 [m]
    
    
    def __init__(self, robot_id, n_joint):
        """
        コンストラクタ

        パラメータ
            robot_id(p.loadURDFの戻り値): ロボットURDFを読み込んだ際のID
            n_joint(int): ロボットの関節数 (グリッパーは含むが，グリッパー先端は含まない)
        """
        # 親クラスのコンストラクタ
        super().__init__(robot_id, n_joint)

        # ダイナミクスの変更
        self._chg_dynamics()
    
    
    def _chg_dynamics(self):
        """
        ダイナミクスの変更
        """
        # グリッパーの右・左の関節番号を取得
        gripper_right_left_idx = self._get_gripper_right_left_idx()

        for idx in gripper_right_left_idx:
            p.changeDynamics(self._robot_id,        # 把持対象物ID
                            idx,                    # 関節番号
                            lateralFriction=1.0)    # 床との摩擦係数
    
    
    def run(self, open=False, close=False):
        """
        実行 (毎時刻，本関数を呼ぶこと)

        パラメータ
            open(bool): グリッパーのオープンフラグ
            close(bool): グリッパーのクローズフラグ
        """
        # グリッパーの現在の関節角度[m]を取得
        joint_values = self._get_joint_values()

        # オープンとクローズが同時実行の時，安全の観点よりクローズよりもオープンを優先
        if open:
            # グリッパーのオープン時
            # 移動方向を取得して，設定したい関節角度[m]を計算
            direction = self._get_move_direction(open=open)
            joint_values = direction * self._GRIPPER_OPEN_VAL
        elif close:
            # グリッパーのクローズ時
            # グリッパーのクローズに関するキーボードが押下された時
            direction = self._get_move_direction(open=open)
            joint_values = direction * self._GRIPPER_CLOSE_VAL
        else:
            # 押下されていないため，何もしない
            pass

        # 関節角度の設定
        self._set_joint_values(joint_values)

    def _get_move_direction(self, open):
        """
        グリッパーの移動方向を取得

        パラメータ
            open(bool): True/False = オープン/クローズ

        戻り値
            numpy.ndarray: グリッパーの移動方向 (右関節・左関節の順番)
        """
        # グリッパーの右関節・左関節の移動方向
        move_direction = np.array([-1.0, 1.0])
        if not open:        # クローズ
            move_direction *= -1

        return move_direction

    def _get_gripper_right_left_idx(self):
        """
        グリッパーの右・左の関節番号

        戻り値
            list: グリッパーの右・左の関節番号 (右・左の順番にデータ保存)
        """
        right_left_idx = [self._n_joint + self._GRIPPER_RIGHT_IDX, self._n_joint + self._GRIPPER_LEFT_IDX]

        return right_left_idx

    def _set_joint_values(self, values):
        """
        関節角度[m]を設定

        パラメータ
            values(numpy.ndarray): 設定値
        """
        # パラメータのサイズを確認
        if values.shape[0] != DIMENTION_2D:
            # 異常
            raise ValueError(f"values'shape[0] is abnormal. values'shape[0] is {values.shape[0]}")

        # グリッパーの右・左の関節番号
        gripper_right_left_idx = self._get_gripper_right_left_idx()

        for idx, gripper_idx in enumerate(gripper_right_left_idx):
            p.setJointMotorControl2(
                bodyIndex=self._robot_id,
                jointIndex=gripper_idx,
                controlMode=p.POSITION_CONTROL,
                targetPosition=values[idx],
                positionGain=0.5,      # デフォルトよりやや高め
                velocityGain=1.0       # 高速応答（必要に応じて調整）
            )

    def _get_joint_values(self):
        """
        関節角度[m]を取得

        戻り値
            numpy.ndarray: 関節角度 (グリッパーの右関節，グリッパーの左関節)
        """
        # グリッパーの右・左の関節番号
        gripper_right_left_idx = self._get_gripper_right_left_idx()
        joint_values = []

        for gripper_idx in gripper_right_left_idx:
            # グリッパー関節の状態を取得
            joint_state = p.getJointState(bodyUniqueId=self._robot_id, jointIndex=gripper_idx)
            # 関節の値を保存
            joint_values.append(joint_state[self._JOINT_CURRENT_VALUE_IDX])

        return np.array(joint_values)


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

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

    メソッド
        public

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

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


        protected

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

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

            運動学関連
                _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             # 関節の最大値が保存されている要素番号
    
    _SLIDER_MAKE_WAIT_TIME = 0.2    # スライダー作成の待機時間 [sec]
    _SIMULATION_SLEEP_TIME = 1. / 200.   # シミュレーションの待機時間 [sec]
    
    _DEBUG_TEXT_LIFE_TIME  = 0      # テキストの生存時間 [sec] (0は無限時間)
    _DEBUG_TEXT_SIZE       = 0.5    # テキストの大きさ
    
    _ZERO_NEAR = 1e-4               # 0近傍の値
    
    _INTERFERENCE_MARGIN   = 0.1    # 干渉判定のマージン [m]
    
    _PATH_PLAN_TIME = 1000      # 経路生成の最大時間 [sec]
    _N_MARGIN_MOVE  = 50        # 経路生成終了後の余白時間 [回]
    
    _N_HAND_JOINT   = 4         # ハンド用の関節数
    _N_INVERSE_HAND_JOINT = -2  # 逆運動学計算時のハンド関節数
    
    _KEY_DOWN_MOVE_POS = 0.1    # キーボード押下時のロボット手先位置の移動量 [m]
    _KEY_DOWN_MOVE_ORI = 0.05   # キーボード押下時のロボット手先姿勢の移動量 [rad]
    
    _GRASP_OBJECT_POS    = [ 1.8, 1.0, 0.05]    # 把持対象物の位置
    _GRASP_OBJECT_OFFSET = [-0.4, 0,   0]       # 把持対象物の位置のオフセット
    
    
    def __init__(self, interpolation, robot_urdf, environment_urdf=None, grasp_urdf=None, hand=False):
        """
        コンストラクタ

        パラメータ
            interpolation(str): 補間方法 (関節空間/位置空間)
            robot_urdf(str): ロボットアームのファイル名 (urdf)
            environment_urdf(str): 環境のファイル名 (urdf)
            grasp_object(str): 把持対象物のファイル名 (urdf)
            hand(bool): ハンドの装着有無 True/False = 装着/未装着
        """
        # 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, hand)

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

        self._rrt = None


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

        パラメータ
            robot_urdf(str): ロボットアームのファイル名 (urdf)
            interpolation(str): 探索方法 (関節空間/位置空間)
            hand(bool): ハンドの装着有無 True/False = 装着/未装着
        """
        # 引数の確認
        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

        # ハンド装着有無
        if hand:
            # ハンド装着
            if self._n_joints != (DIMENTION_2D + self._N_HAND_JOINT):
                # 2軸ロボットアーム以外
                raise ValueError(f"self._n_joints is abnormal. {self._n_joints} is abnormal.")

            self._hand = ParallelGripper(self._robot_id, self._n_joints)

        else:
            # ハンド非装着
            if self._n_joints != DIMENTION_2D:
                # 2軸ロボットアーム以外
                raise ValueError(f"self._n_joints is abnormal. {self._n_joints} is abnormal.")

            self._hand = None

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

        # 環境を読み込む
        self._environment_id = None
        if environment_urdf is not None:
            self._environment_id = p.loadURDF(environment_urdf, basePosition=[0, 0, 0], useFixedBase=True)

        # 把持対象物を読み込む
        self._grasp_id = None
        self._grasp_constraint_id = None
        if grasp_urdf is not None:
            self._grasp_id = p.loadURDF(grasp_urdf, basePosition=[pos + offset for pos, offset in zip(self._GRASP_OBJECT_POS, self._GRASP_OBJECT_OFFSET)])

            # 把持対象物に摩擦を付与する
            p.changeDynamics(self._grasp_id,        # 把持対象物ID
                            -1,                     # ベースに対して
                            lateralFriction=1.0,    # 床との摩擦係数
                            spinningFriction=1.0,   # 回転摩擦係数
                            rollingFriction=1)      # 転がり摩擦

            # 把持対象物の位置・姿勢を取得
            grasp_pos, grasp_ori = self._get_grasp_pos()
            # 把持対象物に拘束条件を付与
            self._grasp_constraint_id = self._set_constraint(self._grasp_id, grasp_pos, grasp_ori)
    # 事前準備メソッド ↑


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

        パラメータ
            start_pos(numpy.ndarray): 経路生成の始点
            path_plan(str): 経路生成手法

        戻り値
            bool: True/False = 経路生成に成功/失敗
        """
        # リアルタイムでのシミュレーション
        p.setRealTimeSimulation(1)

        # 把持対象物の位置を取得
        end_pos, _ = self._get_grasp_pos(dim2=True)
        end_pos = np.array(end_pos)

        # 始点と終点で干渉していないかの確認
        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()

        # 経路生成後の余白時間
        self._exec_margin_time(end_pos)

        return result

    def _exec_margin_time(self, end_pos):
        """
        経路生成後の余白時間の処理

        パラメータ
            end_pos(numpy.ndarray): 経路生成の終点
        """
        # 長めの待機時間
        time.sleep(self._SIMULATION_SLEEP_TIME * 10)

        # PyBulletで関節角度を与えても，与えた関節角度ピッタリにはならず，数値誤差が発生するから，終点へ移動する処理を実施する
        end_theta = self._convert_pos_to_theta(end_pos)

        # 経路生成後の余白時間 (即座にPyBulletが終了するのを防ぐための余白時間)
        for _ in range(self._N_MARGIN_MOVE):
            # 終点に移動
            self._set_joint(end_theta)
            # グリッパーの実行
            self._run_gripper(open=True)
            # 待機時間
            time.sleep(self._SIMULATION_SLEEP_TIME)

        # 把持物体の拘束を解除
        if self._grasp_constraint_id is not None:
            self._release_constraint(self._grasp_constraint_id)
            time.sleep(self._SIMULATION_SLEEP_TIME)
            self._grasp_constraint_id = None

        # 経路生成後の余白時間 (即座にPyBulletが終了するのを防ぐための余白時間)
        for _ in range(self._N_MARGIN_MOVE):
            # 終点に移動
            self._set_joint(end_theta)
            # グリッパーの実行
            self._run_gripper(close=True)
            # 待機時間
            time.sleep(self._SIMULATION_SLEEP_TIME)

    def _set_constraint(self, object_id, pos, ori):
        """
        拘束条件の設定

        パラメータ
            object_id(int): 拘束したい対象物ID
            pos(list): 拘束したい位置
            ori(list): 拘束したい姿勢

        戻り値
            int: 拘束条件ID
        """
        constraint_id = p.createConstraint(
                            object_id,      # 親番号(拘束したい対象物ID)
                            -1,             # 親リンクの要素番号("-1"はベース)
                            -1,             # 子番号("-1"はなし)
                            -1,             # 子リンクの要素番号("-1"はベース)
                            p.JOINT_FIXED,  # 関節タイプ(今回は固定"JOINT_FIXED")
                            [0, 0, 0],      # 関節軸
                            [0, 0, 0],      # 親の中心からの位置 
                            pos,            # 子の中心からの位置 (今回は子を設定していないから，ワールド座標系から見た関節位置)
                            parentFrameOrientation=ori,         # 親の中心からの姿勢
                            childFrameOrientation=[0, 0, 0, 1]) # 子の中心からの姿勢 (今回は，子を設定していないから，ワールド座標系から見た姿勢)

        return constraint_id

    def _release_constraint(self, constraint_id):
        """
        拘束条件の解除
        """
        if constraint_id is not None:
            # 拘束条件を解除
            p.removeConstraint(constraint_id)

    def _get_robot_joint_from_grasp_pos(self):
        """
        把持対象物の位置となるロボットの関節角度を取得
        
        戻り値
            numpy.ndarray: ロボットの関節角度
        """
        # 把持対象物の位置を取得
        grasp_pos, grasp_ori = self._get_grasp_pos()
        # 逆運動学 (手先位置から関節角度へ変換) の実施
        thetas = self._convert_pos_to_theta(grasp_pos)

        return thetas

    def _get_grasp_pos(self, dim2=False):
        """
        把持対象物の位置を取得

        パラメータ
            dim2(bool): 2次元位置として取得するかどうか

        戻り値
            list: 把持対象物の位置 ([rad] or [m])
            list: 把持対象物の姿勢
        """
        if self._grasp_id is None:  # 把持対象物が存在しない
            raise ValueError("self._grasp_id is abnorma. please set grasp_urdf.")

        # 把持対象物の位置[m]を取得
        grasp_pos, grasp_ori = p.getBasePositionAndOrientation(self._grasp_id)

        if dim2:
            grasp_pos = grasp_pos[:DIMENTION_2D]

        return grasp_pos, grasp_ori

    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)
            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)
        # グリッパーの実行
        self._run_gripper(open=True)

        # 始点から終点までの経路を移動
        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)
            # グリッパーの実行
            self._run_gripper(open=True)
            # 待機時間
            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 _run_gripper(self, open=False, close=False):
        """
        グリッパーの実行
        
        パラメータ
            open(bool): グリッパーのオープンフラグ
            close(bool): グリッパーのクローズフラグ
        """
        if self._hand is None:
            # ハンド非装着のため，処理終了
            return

        self._hand.run(open, close)
    # メイン処理 ↑


    # 干渉判定処理 ↓
    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)

        # グリッパーの実行
        self._run_gripper(open=True)

        # 待機時間
        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)

        # グリッパーの実行
        self._run_gripper(open=True)

        # 待機時間
        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)[:-self._N_INVERSE_HAND_JOINT]

        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/01f661a571c6fe76c551)

拘束条件の設定 (_set_constraint)

拘束条件の設定の中身を説明する．

    def _set_constraint(self, object_id, pos, ori):
        """
        拘束条件の設定

        パラメータ
            object_id(int): 拘束したい対象物ID
            pos(list): 拘束したい位置
            ori(list): 拘束したい姿勢

        戻り値
            int: 拘束条件ID
        """
        constraint_id = p.createConstraint(
                            object_id,      # 親番号(拘束したい対象物ID)
                            -1,             # 親リンクの要素番号("-1"はベース)
                            -1,             # 子番号("-1"はなし)
                            -1,             # 子リンクの要素番号("-1"はベース)
                            p.JOINT_FIXED,  # 関節タイプ(今回は固定"JOINT_FIXED")
                            [0, 0, 0],      # 関節軸
                            [0, 0, 0],      # 親の中心からの位置 
                            pos,            # 子の中心からの位置 (今回は子を設定していないから，ワールド座標系から見た関節位置)
                            parentFrameOrientation=ori,         # 親の中心からの姿勢
                            childFrameOrientation=[0, 0, 0, 1]) # 子の中心からの姿勢 (今回は，子を設定していないから，ワールド座標系から見た姿勢)

        return constraint_id

経路探索中にロボットと把持対象物がぶつかっても，把持対象物が移動しないように拘束条件を追加した．拘束条件はPyBulletのcreateConstraint()関数を使用する．createConstraint()関数の引数および戻り値は公式ドキュメントを抜粋すると，下図の通りである．

上図の青枠が引数，赤枠が戻り値である．今回は位置と姿勢を変更しないような拘束条件を追加した．

把持対象物の位置を取得 (_get_grasp_pos)

把持対象物の位置を取得の中身を説明する．

    def _get_grasp_pos(self, dim2=False):
        """
        把持対象物の位置を取得

        パラメータ
            dim2(bool): 2次元位置として取得するかどうか

        戻り値
            list: 把持対象物の位置 ([rad] or [m])
            list: 把持対象物の姿勢
        """
        ...
        # 把持対象物の位置[m]を取得
        grasp_pos, grasp_ori = p.getBasePositionAndOrientation(self._grasp_id)
        ...

今回定義した把持対象物のURDFファイルには，関節(joint)が定義されていない．リンクだけを定義している．リンクだけを定義したURDFファイルから，位置・姿勢を取得するには，PyBulletのgetBasePositionAndOrientation()を使用する必要がある．

経路生成後の余白時間の処理 (_exec_margin_time)

経路生成後の余白時間の処理の中身を説明する．

    def _exec_margin_time(self, end_pos):
        """
        経路生成後の余白時間の処理

        パラメータ
            end_pos(numpy.ndarray): 経路生成の終点
        """
        # 長めの待機時間
        time.sleep(self._SIMULATION_SLEEP_TIME * 10)

        # PyBulletで関節角度を与えても，与えた関節角度ピッタリにはならず，数値誤差が発生するから，終点へ移動する処理を実施する
        end_theta = self._convert_pos_to_theta(end_pos)

        # 経路生成後の余白時間 (即座にPyBulletが終了するのを防ぐための余白時間)
        for _ in range(self._N_MARGIN_MOVE):
            # 終点に移動
            self._set_joint(end_theta)
            # グリッパーの実行
            self._run_gripper(open=True)
            # 待機時間
            time.sleep(self._SIMULATION_SLEEP_TIME)

        # 把持物体の拘束を解除
        if self._grasp_constraint_id is not None:
            self._release_constraint(self._grasp_constraint_id)
            time.sleep(self._SIMULATION_SLEEP_TIME)
            self._grasp_constraint_id = None

        # 経路生成後の余白時間 (即座にPyBulletが終了するのを防ぐための余白時間)
        for _ in range(self._N_MARGIN_MOVE):
            # 終点に移動
            self._set_joint(end_theta)
            # グリッパーの実行
            self._run_gripper(close=True)
            # 待機時間
            time.sleep(self._SIMULATION_SLEEP_TIME)

把持(グリッパーのクローズ)する時に，把持対象物の位置へロボットが正確に移動していないといけない．正確に移動させるために，終点に移動させる余白時間を用意した．
終点へ正確に移動させたら，把持物体が動けるように，拘束条件(把持対象物の位置・姿勢の固定化)を解除した．
把持物体の拘束条件を解除してから，把持を実行した．

経路生成手法である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でロボットを動かす

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

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

動画２：探索後の始点から終点までの経路 + 物体把持

把持したい物体まで，干渉しない経路を作成して，物体把持ができることを確認した．物体把持まで自動化できたため，次なる目標は下記の通りとなる．
目標１：カメラを付けて，把持したい物体の位置を取得
目標２：物体把持後に他の場所へ物体を移動 (複数本の経路を生成)
目標３：強化学習の実装

おわりに

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

次記事では，下記内容を実装していきます．
・PyBullet で干渉物が存在する環境下で，3軸ロボットアームの経路生成と把持 (経路生成手法としてRRTを採用)
(https://qiita.com/haruhiro1020/items/d26009d50be190d97368#%E3%81%8A%E3%82%8F%E3%82%8A%E3%81%AB)