はじめに

PyBullet (Python上で動く物理シミュレータ) を使用して，ロボットアームを可視化して，動かしたい．
前記事では，PyBullet (Python上で動く物理シミュレータ) で干渉物が存在する環境において，グリッパー付き2軸ロボットアームの経路生成および物体把持を実装した．
(https://qiita.com/haruhiro1020/items/43ddb8b3c4e379ab7983)

本記事では，前記事の続きで，3軸ロボットアームにグリッパーを追加して，干渉物が存在する環境下にて，物体を把持する(下図はPyBullet上のグリッパー付き3軸ロボットアームである)．赤枠がグリッパー，緑枠が把持したい物体である．把持したい物体は，机の上に置いた．今回は机を干渉物として考えて，机に干渉しないように，物体把持を実装する．

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

本記事で実装すること

・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)

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)を作成する必要がある．

グリッパーの定義

本記事では，下図のようなパラレルグリッパーをロボットアームに追加する．グリッパーは2軸ロボットアームに追加したのと大方同じである．座標系が異なるだけである．

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

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

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

把持対象物に関して

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

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

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

把持対象物のURDF作成

把持対象物(立方体)のUDEFを作成する．

grasp_object.urdf

<?xml version="1.0"?>
<robot name="grasp_cube">
    <link name="cube_link">

        <!-- 見た目 -->
        <visual>
            <origin xyz="0 0 0" rpy="0 0 0"/>
            <geometry>
                <box size="0.1 0.1 0.1"/>
            </geometry>
        </visual>

        <!-- 干渉判定 -->
        <collision>
            <origin xyz="0 0 0" rpy="0 0 0"/>
            <geometry>
                <box size="0.1 0.1 0.1"/>
            </geometry>
        </collision>

        <!-- 慣性 -->
        <inertial>
            <origin xyz="0 0 0" rpy="0 0 0"/>
            <mass value="0.1"/>
            <inertia ixx="0.01" iyy="0.01" izz="0.01" ixy="0.0" ixz="0.0" iyz="0.0"/>
        </inertial>

    </link>
</robot>

経路生成の環境に関して

本記事では，先頭で説明した環境より干渉しない経路を生成する．
机を配置して，机の上に把持対象物が存在する環境としている．今回は，机と把持対象物は別URDFファイルを作成して，対応している．

環境(机)のURDF作成

環境(机)のURDFを作成する．机の色は，ネットで「木の色カラーコード」と検索した値を使用している．

environment_3dof.urdf

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


    <!-- 色を先に定義 (共通化したいから) ↓ -->
    <material name="wood">
        <color rgba="0.871 0.722 0.529 1"/>
    </material>
    <!-- 色を先に定義 (共通化したいから) ↑ -->


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


    <!-- テーブルの作成 ↓ -->
    <!-- テーブルの天板を動かさないようのジョイント -->
    <joint name="joint_table_top" type="fixed">

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

    </joint>


    <!-- テーブルの天板 -->
    <link name="table_top">

        <!-- 見た目 -->
        <visual>
            <origin xyz="0 0 0"/>
            <geometry>
                <box size="0.5 1 0.01"/>
            </geometry>
            <material name="wood"/>
        </visual>

        <!-- 干渉判定 -->
        <collision>
            <origin xyz="0 0 0"/>
            <geometry>
                <box size="0.5 1 0.01"/>
            </geometry>
        </collision>

        <!-- 慣性行列 -->
        <inertial>
            <origin xyz="0 0 0"/>
            <mass value="1.0"/>
            <inertia ixx="0.083" iyy="0.021" izz="0.104" ixy="0.0" ixz="0.0" iyz="0.0"/>
        </inertial>

    </link>


    <!-- テーブルの天板とテーブルの足を固定化する関節 -->
    <joint name="joint_table_leg1" type="fixed">

        <parent link="table_top"/>
        <child link="table_leg1"/>
        <origin xyz="0.2 0.45 0" rpy="0 0 0"/>

    </joint>


    <!-- テーブルの足1 (+x, +y方向) -->
    <link name="table_leg1">

        <!-- 見た目 -->
        <visual>
            <origin xyz="0 0 -0.5"/>
            <geometry>
                <box size="0.1 0.1 1"/>
            </geometry>
            <material name="wood"/>
        </visual>

        <!-- 干渉判定 -->
        <collision>
            <origin xyz="0 0 -0.5"/>
            <geometry>
                <box size="0.1 0.1 1"/>
            </geometry>
        </collision>

        <!-- 慣性行列 -->
        <inertial>
            <origin xyz="0 0 -0.5"/>
            <mass value="2.0"/>
            <inertia ixx="0.1683" iyy="0.1683" izz="0.0033" ixy="0.0" ixz="0.0" iyz="0.0"/>
        </inertial>

    </link>


    <!-- テーブルの天板とテーブルの足を固定化する関節 -->
    <joint name="joint_table_leg2" type="fixed">

        <parent link="table_top"/>
        <child link="table_leg2"/>
        <origin xyz="-0.2 0.45 0" rpy="0 0 0"/>

    </joint>


    <!-- テーブルの足2 (-x, +y方向) -->
    <link name="table_leg2">

        <!-- 見た目 -->
        <visual>
            <origin xyz="0 0 -0.5"/>
            <geometry>
                <box size="0.1 0.1 1"/>
            </geometry>
            <material name="wood"/>
        </visual>

        <!-- 干渉判定 -->
        <collision>
            <origin xyz="0 0 -0.5"/>
            <geometry>
                <box size="0.1 0.1 1"/>
            </geometry>
        </collision>

        <!-- 慣性行列 -->
        <inertial>
            <origin xyz="0 0 -0.5"/>
            <mass value="2.0"/>
            <inertia ixx="0.1683" iyy="0.1683" izz="0.0033" ixy="0.0" ixz="0.0" iyz="0.0"/>
        </inertial>

    </link>


    <!-- テーブルの天板とテーブルの足を固定化する関節 -->
    <joint name="joint_table_leg3" type="fixed">

        <parent link="table_top"/>
        <child link="table_leg3"/>
        <origin xyz="-0.2 -0.45 0" rpy="0 0 0"/>

    </joint>


    <!-- テーブルの足3 (-x, -y方向) -->
    <link name="table_leg3">

        <!-- 見た目 -->
        <visual>
            <origin xyz="0 0 -0.5"/>
            <geometry>
                <box size="0.1 0.1 1"/>
            </geometry>
            <material name="wood"/>
        </visual>

        <!-- 干渉判定 -->
        <collision>
            <origin xyz="0 0 -0.5"/>
            <geometry>
                <box size="0.1 0.1 1"/>
            </geometry>
        </collision>

        <!-- 慣性行列 -->
        <inertial>
            <origin xyz="0 0 -0.5"/>
            <mass value="2.0"/>
            <inertia ixx="0.1683" iyy="0.1683" izz="0.0033" ixy="0.0" ixz="0.0" iyz="0.0"/>
        </inertial>

    </link>


    <!-- テーブルの天板とテーブルの足を固定化する関節 -->
    <joint name="joint_table_leg4" type="fixed">

        <parent link="table_top"/>
        <child link="table_leg4"/>
        <origin xyz="0.2 -0.45 0" rpy="0 0 0"/>

    </joint>


    <!-- テーブルの足4 (+x, -y方向) -->
    <link name="table_leg4">

        <!-- 見た目 -->
        <visual>
            <origin xyz="0 0 -0.5"/>
            <geometry>
                <box size="0.1 0.1 1"/>
            </geometry>
            <material name="wood"/>
        </visual>

        <!-- 干渉判定 -->
        <collision>
            <origin xyz="0 0 -0.5"/>
            <geometry>
                <box size="0.1 0.1 1"/>
            </geometry>
        </collision>

        <!-- 慣性行列 -->
        <inertial>
            <origin xyz="0 0 -0.5"/>
            <mass value="2.0"/>
            <inertia ixx="0.1683" iyy="0.1683" izz="0.0033" ixy="0.0" ixz="0.0" iyz="0.0"/>
        </inertial>

    </link>
    <!-- テーブルの作成 ↑ -->


</robot>

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

グリッパー付き3軸ロボットアームのURDFを作成する．グリッパーなし3軸ロボットアームのURDFは前記事と同じであるため，割愛する．(前記事 https://qiita.com/haruhiro1020/items/2da9fe2687e0849570ea)

robot_3dof_hand.urdf

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

    <!-- 色を先に定義 (共通化したいから) ↓ -->
    <material name="black">
        <color rgba="0 0 0 1"/>
    </material>
    <!-- 色を先に定義 (共通化したいから) ↑ -->


    <!-- ベースリンク -->
    <link name="base_link">
        <!-- 見た目，干渉判定，慣性なし -->
    </link>


    <!-- ベースリンクとリンク1間の関節 -->
    <joint name="joint1" type="revolute">

        <parent link="base_link"/>
        <child link="link1"/>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <axis xyz="0 0 1"/>
        <limit lower="-3.14" upper="3.14" effort="100" velocity="5.0"/>

    </joint>


    <!-- リンク1 -->
    <link name="link1">

        <!-- 見た目 -->
        <visual>
            <origin xyz="0.0 0.0 0.5"/>
            <geometry>
                <box size="0.2 0.2 1.0"/>
            </geometry>
        </visual>

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

        <!-- 慣性行列 -->
        <inertial>
            <origin xyz="0.0 0.0 0.5"/>
            <!-- 質量は適当 -->
            <mass value="1.0"/>
            <inertia ixx="0.08667" iyy="0.08667" izz="0.00667" ixy="0.0" ixz="0.0" iyz="0.0"/>
        </inertial>

    </link>


    <!-- リンク1とリンク2間の関節 -->
    <joint name="joint2" type="revolute">

        <parent link="link1"/>
        <child link="link2"/>
        <origin xyz="0 0 1.0" rpy="0 0 0"/>
        <axis xyz="0 1 0"/>
        <limit lower="-3.14" upper="3.14" effort="100" velocity="5.0"/>

    </joint>


    <!-- リンク2 -->
    <link name="link2">

        <!-- 見た目 -->
        <visual>
            <origin xyz="0.0 0.0 0.5"/>
            <geometry>
                <box size="0.2 0.2 1.0"/>
            </geometry>
        </visual>

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

        <!-- 慣性行列 -->
        <inertial>
            <origin xyz="0.0 0.0 0.5"/>
            <!-- 質量は適当 -->
            <mass value="1.0"/>
            <inertia ixx="0.08667" iyy="0.08667" izz="0.00667" ixy="0.0" ixz="0.0" iyz="0.0"/>
        </inertial>

    </link>


    <!-- リンク2とリンク3間の関節 -->
    <joint name="joint3" type="revolute">

        <parent link="link2"/>
        <child link="link3"/>
        <origin xyz="0 0 1.0" rpy="0 0 0"/>
        <axis xyz="0 1 0"/>
        <limit lower="-3.14" upper="3.14" effort="100" velocity="5.0"/>

    </joint>


    <!-- リンク3 -->
    <link name="link3">

        <!-- 見た目 -->
        <visual>
            <origin xyz="0.0 0.0 0.5"/>
            <geometry>
                <box size="0.2 0.2 1.0"/>
            </geometry>
        </visual>

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

        <!-- 慣性行列 -->
        <inertial>
            <origin xyz="0.0 0.0 0.5"/>
            <!-- 質量は適当 -->
            <mass value="1.0"/>
            <inertia ixx="0.08667" iyy="0.08667" izz="0.00667" ixy="0.0" ixz="0.0" iyz="0.0"/>
        </inertial>

    </link>


    <!-- リンク3とグリッパー間の関節 -->
    <joint name="gripper_joint" type="fixed">

        <parent link="link3"/>
        <child link="gripper_base_link"/>
        <origin xyz="0 0 1.0" rpy="0 0 0"/>

    </joint>


    <!-- グリッパーのベース部分 -->
    <!-- グリッパーはT字型とする -->
    <!-- T字型の縦棒作成 -->
    <link name="gripper_base_link">

        <!-- 見た目 -->
        <visual>
            <origin xyz="0 0 0.1" rpy="0 0 0"/>
            <geometry>
                <box size="0.1 0.1 0.2"/>
            </geometry>
            <material name="black"/>
        </visual>

        <!-- 干渉判定 -->
        <visual>
            <origin xyz="0 0 0.1" rpy="0 0 0"/>
            <geometry>
                <box size="0.1 0.1 0.2"/>
            </geometry>
            <material name="black"/>
        </visual>

        <!-- 慣性 -->
        <inertial>
            <mass value="0.05"/>
            <origin xyz="0 0 0.1" rpy="0 0 0"/>
            <inertia ixx="0.000208" iyy="0.000208" izz="0.000083" ixy="0.0" ixz="0.0" iyz="0.0"/>
        </inertial>

    </link>


    <!-- グリッパーとフィンガー間の関節 -->
    <joint name="finger_joint" type="fixed">

        <parent link="gripper_base_link"/>
        <child link="gripper_finger_link"/>
        <origin xyz="0 0 0.2" rpy="0 0 0"/>

    </joint>


    <!-- T字型の横棒作成 -->
    <link name="gripper_finger_link">

        <!-- 見た目 -->
        <visual>
            <origin xyz="0 0 0" rpy="0 0 0"/>
            <geometry>
                <box size="0.2 0.2 0.1"/>
            </geometry>
            <material name="black"/>
        </visual>

        <!-- 干渉判定 -->
        <collision>
            <origin xyz="0 0 0" rpy="0 0 0"/>
            <geometry>
                <box size="0.2 0.2 0.1"/>
            </geometry>
        </collision>

        <!-- 慣性 -->
        <inertial>
            <mass value="0.1"/>
            <origin xyz="0 0 0" rpy="0 0 0"/>
            <inertia ixx="0.000417" iyy="0.000417" izz="0.000667" ixy="0.0" ixz="0.0" iyz="0.0"/>
        </inertial>

    </link>


    <!-- フィンガーと右フィンガー間の関節 -->
    <joint name="right_finger_joint" type="prismatic">

        <parent link="gripper_finger_link"/>
        <child link="right_finger_link"/>
        <origin xyz="-0.095 0 0.05" rpy="0 0 0"/>
        <axis xyz="1 0 0"/>
        <limit lower="0.0" upper="0.090" effort="100.0" velocity="1.0"/>

    </joint>


    <!-- 右フィンガー -->
    <link name="right_finger_link">

        <!-- 見た目 -->
        <visual>
            <origin xyz="0.0 0.0 0.15"/>
            <geometry>
                <box size="0.01 0.2 0.3"/>
            </geometry>
            <material name="black"/>
        </visual>

        <!-- 干渉判定 -->
        <collision>
            <origin xyz="0.0 0.0 0.15"/>
            <geometry>
                <box size="0.01 0.2 0.3"/>
            </geometry>
        </collision>

        <!-- 慣性行列 -->
        <inertial>
            <origin xyz="0.0 0.0 0.15"/>
            <mass value="0.015"/>
            <inertia ixx="0.000163" iyy="0.000113" izz="0.000051" ixy="0.0" ixz="0.0" iyz="0.0"/>
        </inertial>

    </link>


    <!-- ハンドベースと左フィンガー間の関節 -->
    <joint name="left_finger_joint" type="prismatic">

        <parent link="gripper_finger_link"/>
        <child link="left_finger_link"/>
        <origin xyz="0.095 0 0.05" rpy="0 0 0"/>
        <axis xyz="1 0 0"/>
        <limit lower="-0.090" upper="0.0" effort="100.0" velocity="1.0"/>

    </joint>


    <!-- 左グリッパー -->
    <link name="left_finger_link">

        <!-- 見た目 -->
        <visual>
            <origin xyz="0.0 0.0 0.15"/>
            <geometry>
                <box size="0.01 0.2 0.3"/>
            </geometry>
            <material name="black"/>
        </visual>

        <!-- 干渉判定 -->
        <collision>
            <origin xyz="0.0 0.0 0.15"/>
            <geometry>
                <box size="0.01 0.2 0.3"/>
            </geometry>
        </collision>

        <!-- 慣性行列 -->
        <inertial>
            <origin xyz="0.0 0.0 0.15"/>
            <mass value="0.015"/>
            <inertia ixx="0.000163" iyy="0.000113" izz="0.000051" ixy="0.0" ixz="0.0" iyz="0.0"/>
        </inertial>

    </link>


    <!-- フィンガーと先端 (エンドエフェクター) 間の関節 -->
    <joint name="ee_joint" type="fixed">

        <parent link="gripper_finger_link"/>
        <child link="ee_link"/>
        <origin xyz="0 0 0.3" rpy="0 0 0"/>

    </joint>


    <!-- グリッパー先端 (エンドエフェクタ) -->
    <link name="ee_link">
        <!-- 見た目，干渉判定，慣性はなし -->
    </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 について説明する．

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による経路生成

# ロボットアームが保存されている URDF ファイル名
class ROBOTURDF(Enum):
    """
    URDFファイル名
    """
    # 2軸ロボットアーム
    DOF2 = "robot_2dof.urdf"            # ハンド(グリッパ)なし
    DOF2_HAND = "robot_2dof_hand.urdf"  # ハンド(グリッパ)付き

    # 3軸ロボットアーム
    DOF3 = "robot_3dof.urdf"            # ハンド(グリッパ)なし
    DOF3_HAND = "robot_3dof_hand.urdf"  # ハンド(グリッパ)付き

全体的なメイン処理 (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 *



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


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

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

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

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

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

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

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

    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 PyBulletRobot:
    """
    PyBulletのロボットのベースクラス (抽象クラス)

    プロパティ
        _robot_id(): ロボットアームのID番号
        _n_joint(int): URFDファイル内のロボットの間接数
        _hand(RobotHand): ハンドクラス
        _interpolation(str): 探索空間 (直交空間/関節空間)

    メソッド
        public
            set_joint(): 関節角度の設定
            set_jump_joint(): 関節角度をジャンプ
            convert_pos_to_theta(): 位置から関節角度に変換
            run_gripper(): グリッパーの実行

        protected
            _chk_pos_dim(): 位置(姿勢も含む)の次元数確認
            _chk_thetas_dim(): 関節の次元数確認
    """
    # 定数の定義
    # 子クラスで必ず定義する必要がある ↓
    _DIM_JOINT = DIMENTION_NONE     # 関節の次元数
    _DIM_POSE  = DIMENTION_NONE     # 位置(姿勢も含む)の次元数
    # 子クラスで必ず定義する必要がある ↑
    
    _N_HAND_JOINT = 4               # ハンド用の関節数
    _N_HAND_JOINT_NOT_FIXED = 2     # ハンド用の関節で固定関節以外
    
    
    def __init__(self, robot_id, interpolation, hand):
        """
        コンストラクタ

        パラメータ
            robot_id(int): ロボットID (loadURDF()の戻り値)
            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._robot_id = robot_id
        self._interpolation = interpolation

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

        # ハンド装着有無
        if hand:        # ハンド装着
            if self._n_joints != (self._DIM_JOINT + self._N_HAND_JOINT):
                # 関節数が異常
                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 != self._DIM_JOINT:
                # 関節数が異常
                raise ValueError(f"self._n_joints is abnormal. {self._n_joints} is abnormal.")

            self._hand = None


    def _chk_pos_dim(self, pos):
        """
        位置(姿勢も含む)の次元数確認

        パラメータ
            pos(numpy.ndarray): 位置 [m]，姿勢 [rad]
        """
        # 引数の確認
        if len(pos) != self._DIM_POSE:
            # 異常
            raise ValueError(f"pos's shape is abnormal. pos'size is {len(pos)}")

    def _chk_thetas_dim(self, thetas):
        """
        関節の次元数確認

        パラメータ
            thetas(numpy.ndarray): 関節角度 [rad]
        """
        if self._hand is None:      # ハンドなし
            # 引数の確認
            if len(thetas) != self._DIM_JOINT:
                # 異常
                raise ValueError(f"theta's shape is abnormal. thetas'size is {len(thetas)}")

        else:                       # ハンドあり
            # 引数の確認
            if len(thetas) != (self._DIM_JOINT + self._N_HAND_JOINT_NOT_FIXED):
                # 異常
                raise ValueError(f"theta's shape is abnormal. thetas'size is {len(thetas)}")

    def set_joint(self, thetas):
        """
        関節角度の設定

        パラメータ
            thetas(numpy.ndarray): 関節角度 [rad]
        """
        # 引数の確認
        self._chk_thetas_dim(thetas)

        for i in range(len(thetas)):
            # 関節角度を設定
            p.setJointMotorControl2(
                bodyUniqueId=self._robot_id,    # IDの設定
                jointIndex=i,                   # 関節番号の設定
                controlMode=p.POSITION_CONTROL, # 位置制御
                targetPosition=thetas[i]        # 関節角度
            )

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

        パラメータ
            thetas(numpy.ndarray): 関節角度 [rad]
        """
        # 引数の確認
        self._chk_thetas_dim(thetas)

        for i in range(thetas.shape[0]):
            # 関節角度を設定
            p.resetJointState(
                bodyUniqueId=self._robot_id,    # IDの設定
                jointIndex=i,                   # 関節番号の設定
                targetValue=thetas[i]           # 関節角度
            )

    def convert_pos_to_theta(self, pos, force=False):
        """
        位置から関節角度に変換

        パラメータ
            pos(numpy.ndarray): 位置 / 関節角度
            force(bool): パラメータposを絶対に位置とみなす

        戻り値
            numpy.ndarray: 関節角度 [rad]
        """
        raise InterruptedError("convert_pos_to_theta() is necessary override.")

    def run_gripper(self, open=False, close=False):
        """
        グリッパーの実行
        
        パラメータ
            open(bool): グリッパーのオープンフラグ
            close(bool): グリッパーのクローズフラグ
        """
        if self._hand is None:
            # ハンド非装着のため，処理終了
            return

        self._hand.run(open, close)


class PyBullet2DoFRobot(PyBulletRobot):
    """
    PyBulletの2軸ロボットクラス

    メソッド
        public
            convert_pos_to_theta(): 位置から関節角度に変換

        private
            __inverse_kinematics(): 逆運動学(位置から関節角度に変換)
    """
    # 定数の定義
    _Z_VALUE = 0.0      # 位置を3次元変換する時のZ値
    
    _DIM_JOINT = DIMENTION_2D     # 関節の次元数
    _DIM_POSE  = DIMENTION_2D     # 位置(姿勢も含む)の次元数
    
    
    def __init__(self, robot_id, interpolation, hand):
        """
        コンストラクタ

        パラメータ
            robot_id(int): ロボットID (loadURDF()の戻り値)
            interpolation(str): 探索方法 (関節空間/位置空間)
            hand(bool): ハンド装着の有無 True/False = あり/なし
        """
        # 親クラスのコンストラクを実行
        super().__init__(robot_id, interpolation, hand)


    def __inverse_kinematics(self, pos):
        """
        逆運動学(位置から関節角度に変換)

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

        戻り値
            numpy.ndarray: 関節角度 [rad]
        """
        # 引数の確認
        self._chk_pos_dim(pos)

        # posは2次元データであるため，3次元データへ変換する
        # (PyBulletの逆運動学を実装するため)
        pos = np.append(pos, self._Z_VALUE)

        # エンドエフェクタのリンク要素はベースリンクを除いた要素番号となる
        thetas = p.calculateInverseKinematics(self._robot_id, self._n_joints, pos)
        thetas = np.array(thetas)

        return thetas


    def convert_pos_to_theta(self, pos, force=False):
        """
        位置から関節角度に変換

        パラメータ
            pos(numpy.ndarray): 位置 [m] / 関節角度 [rad]
            force(bool): パラメータposを絶対に位置とみなす

        戻り値
            numpy.ndarray: 関節角度 [rad]
        """
        if force:       # posを位置とみなす
            # 逆運動学
            thetas = self.__inverse_kinematics(pos)

        else:           # プロパティ "_interpolation" より決定
            if self._interpolation == INTERPOLATION.POSITION.value:
                # 逆運動学
                thetas = self.__inverse_kinematics(pos)

            else:
                # pos が関節角度のため，そのまま返す
                thetas = np.copy(pos)

        return thetas


class PyBullet3DoFRobot(PyBulletRobot):
    """
    PyBulletの3軸ロボットクラス

    メソッド
        public
            convert_pos_to_theta(): 位置から関節角度に変換

        private
            __inverse_kinematics(): 逆運動学(位置から関節角度に変換)
    """
    # 定数の定義
    _DIM_JOINT = DIMENTION_3D     # 関節の次元数
    _DIM_POSE  = DIMENTION_3D     # 位置(姿勢も含む)の次元数
    
    
    def __init__(self, robot_id, interpolation, hand):
        """
        コンストラクタ

        パラメータ
            robot_id(int): ロボットID (loadURDF()の戻り値)
            interpolation(str): 探索方法 (関節空間/位置空間)
            hand(bool): ハンド装着の有無 True/False = あり/なし
        """
        # 親クラスのコンストラクを実行
        super().__init__(robot_id, interpolation, hand)


    def __inverse_kinematics(self, pos):
        """
        逆運動学(位置から関節角度に変換)

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

        戻り値
            numpy.ndarray: 関節角度 [rad]
        """
        # 引数の確認
        self._chk_pos_dim(pos)

        # エンドエフェクタのリンク要素はベースリンクを除いた要素番号となる
        thetas = p.calculateInverseKinematics(self._robot_id, self._n_joints, pos)
        thetas = np.array(thetas)

        return thetas


    def convert_pos_to_theta(self, pos, force=False):
        """
        位置から関節角度に変換

        パラメータ
            pos(numpy.ndarray): 位置 [m] / 関節角度 [rad]
            force(bool): パラメータposを絶対に位置とみなす

        戻り値
            numpy.ndarray: 関節角度 [rad]
        """
        if force:       # posを位置とみなす
            # 逆運動学
            thetas = self.__inverse_kinematics(pos)

        else:           # プロパティ "_interpolation" より決定
            if self._interpolation == INTERPOLATION.POSITION.value:
                # 逆運動学
                thetas = self.__inverse_kinematics(pos)

            else:
                # pos が関節角度のため，そのまま返す
                thetas = np.copy(pos)

        return thetas


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

    プロパティ
        _robot_id(): ロボットアームのID番号
        _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(): 経路生成の後処理 (経路生成成功時だけ実装)

            干渉判定処理
                _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.15   # 干渉判定のマージン [m]
    
    _PATH_PLAN_TIME = 100   # 経路生成の最大時間 [sec]
    _N_MARGIN_MOVE  = 50    # 経路生成終了後の余白時間 [回]
    
    # 2軸ロボットアーム用データ
    _GRASP_OBJECT_POS_2DOF    = [ 1.8, 1.0, 0.05]   # 把持対象物の位置
    _GRASP_OBJECT_OFFSET_2DOF = [-0.4, 0]           # 把持対象物の位置のオフセット
    _ENVIRONMENT_POS_2DOF     = [ 0,   0,   0]      # 環境のベース位置
    # 3軸ロボットアーム用データ
    _GRASP_OBJECT_POS_3DOF    = [ 1.25,  0.4, 0.55]  # 把持対象物の位置
    _GRASP_OBJECT_OFFSET_3DOF = [-0.15,  0,   0.15]     # 把持対象物の位置のオフセット
    _ENVIRONMENT_POS_3DOF     = [ 1.5,  0,   -0.5]     # 環境のベース位置
    
    
    def __init__(self, interpolation, n_robot_joint, environment_urdf=None, grasp_urdf=None, hand=False):
        """
        コンストラクタ

        パラメータ
            interpolation(str): 補間方法 (関節空間/位置空間)
            n_robot_joint(int): ロボットアームの関節数(2, 3, 6だけ)
            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(n_robot_joint, interpolation, hand)

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

        self._rrt = None


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

        パラメータ
            n_robot_joint(int): ロボットアームの関節数(2, 3, 6だけ)
            interpolation(str): 探索方法 (関節空間/位置空間)
            hand(bool): ハンドの装着有無 True/False = 装着/未装着
        """
        if n_robot_joint == DIMENTION_2D:       # 2軸ロボットアーム
            if hand:    # ハンド装着
                robot_urdf = ROBOTURDF.DOF2_HAND.value
            else:       # ハンド未装着
                robot_urdf = ROBOTURDF.DOF2.value
            # 2軸ロボットアームのクラス
            robot_cls = PyBullet2DoFRobot

        elif n_robot_joint == DIMENTION_3D:     # 3軸ロボットアーム
            if hand:    # ハンド装着
                robot_urdf = ROBOTURDF.DOF3_HAND.value
            else:       # ハンド未装着
                robot_urdf = ROBOTURDF.DOF3.value
            robot_cls = PyBullet3DoFRobot

        else:   # 異常
            raise ValueError(f"n_robot_joint is abnormal. n_robot_joint is {n_robot_joint}.")

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

        # ロボットクラスのインスタンス作成
        self._robot = robot_cls(self._robot_id, interpolation, hand)

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

    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:
            if self._n_robot_joint == DIMENTION_2D:     # 2軸ロボットアーム
                basePosition = self._ENVIRONMENT_POS_2DOF
            else:       # 3軸ロボットアーム
                basePosition = self._ENVIRONMENT_POS_3DOF

            print(f"basePosition = {basePosition}")
            self._environment_id = p.loadURDF(environment_urdf, basePosition=basePosition, useFixedBase=True)

        # 把持対象物を読み込む
        self._grasp_id = None
        self._grasp_constraint_id = None
        if grasp_urdf is not None:

            if self._n_robot_joint == DIMENTION_2D:     # 2軸ロボットアーム
                grasp_center = self._GRASP_OBJECT_POS_2DOF
            else:       # 3軸ロボットアーム
                grasp_center = self._GRASP_OBJECT_POS_3DOF

            self._grasp_id = p.loadURDF(grasp_urdf, basePosition=grasp_center)

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

            # 把持対象物の位置・姿勢を取得
            grasp_pos, grasp_ori = self._get_grasp_pos(offset=[0, 0, 0])
            # 把持対象物に拘束条件を付与
            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): 経路生成手法
        
        戻り値
            result(bool): True/False = 経路生成に成功/失敗
        """
        # リアルタイムでのシミュレーション
        p.setRealTimeSimulation(1)

        # 把持対象物の位置を取得
        if self._n_robot_joint == DIMENTION_2D:     # 2軸ロボットアーム
            offset = self._GRASP_OBJECT_OFFSET_2DOF
            end_pos, _ = self._get_grasp_pos(offset, dim2=True)
        else:       # その他
            offset = self._GRASP_OBJECT_OFFSET_3DOF
            end_pos, _ = self._get_grasp_pos(offset)

        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, np.array(offset))

        return result

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

        パラメータ
            end_pos(numpy.ndarray): 経路生成の終点
            offset(numpy.ndarray): 終点へのオフセット量
        """
        # オフセットの削除
        end_pos = end_pos - offset

        # オフセットを削除して，把持対象物への正しい位置への移動
        end_theta = self._robot.convert_pos_to_theta(end_pos)

        # 経路生成後の余白時間 (即座にPyBulletが終了するのを防ぐための余白時間)
        for _ in range(self._N_MARGIN_MOVE):
            # 終点に移動
            self._robot.set_joint(end_theta)
            # グリッパーの実行
            self._robot.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._robot.set_joint(end_theta)
            # グリッパーの実行
            self._robot.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_grasp_pos(self, offset=None, dim2=False):
        """
        把持対象物の位置を取得

        パラメータ
            offset(list): 把持対象物へのオフセット量 (x, y, z)
            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 offset is not None:
            # オフセット量の考慮
            grasp_pos_offset = [pos + off for pos, off in zip(grasp_pos, offset)]

        if dim2:
            grasp_pos_offset = grasp_pos_offset[:DIMENTION_2D]

        return grasp_pos_offset, 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._robot.convert_pos_to_theta(start_pos)
        self._robot.set_jump_joint(theta)
        # グリッパーの実行
        self._robot.run_gripper(open=True)

        # 始点から終点までの経路を移動
        for row_idx in range(self._rrt.pathes.shape[0]):
            next_theta = self._robot.convert_pos_to_theta(self._rrt.pathes[row_idx])
            self._robot.set_joint(next_theta)
            # グリッパーの実行
            self._robot.run_gripper(open=True)
            # 待機時間
            time.sleep(self._SIMULATION_SLEEP_TIME)
    # メイン処理 ↑


    # 干渉判定処理 ↓
    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._robot.convert_pos_to_theta(pos2)
        self._robot.set_joint(theta)
        # グリッパーの実行
        self._robot.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._robot.convert_pos_to_theta(pos)
        # print(f"theta = {theta}")
        # 位置にジャンプ
        self._robot.set_jump_joint(theta)
        # グリッパーの実行
        self._robot.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): 関節角度
        """
        thetas = self._robot.convert_pos_to_theta(pos, force=True)

        return thetas
    # 運動学関連 ↑

次に，上ソースコードより重要な箇所を抜粋して，説明していく．
上ソースコードの大方は前記事にて説明しているため，説明した内容は割愛する．
(前記事 https://qiita.com/haruhiro1020/items/43ddb8b3c4e379ab7983)

特に言いたいこととしては，物体把持ができるようなパラメータ調整の試行錯誤に時間がかかった．

経路生成手法である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 で3軸ロボットアームにグリッパーを追加
・PyBullet で干渉物が存在する環境下で，3軸ロボットアームの経路生成と把持 (経路生成手法としてRRTを採用)

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