はじめに

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

本記事では，グリッパー付きの6軸ロボットアームを使用して，干渉物が存在しない環境下にて，複数のカメラから把持物体の位置を取得して，物体を把持する(下図はPyBullet上のグリッパー付き6軸ロボットアームである)．赤枠がグリッパー，緑枠が把持したい物体，青枠がカメラ，青色の球が初期位置である．
カメラは5台用意して，「下(-Z方向)・左(-X方向)・右(+X方向)・手前(-Y方向)・奥行き(+Y方向)」方向を向いて，撮影している．5台のカメラから取得した把持物体の位置を組み合わせることで，把持物体の位置を計算する．下図の5つの青枠がカメラであり，全カメラが原点(ワールド座標系の原点)方向を向いている．

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

本記事で実装すること

・干渉物が存在しない環境下での経路生成 + 物体把持 + 複数のカメラによる把持物体の把握 + 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)

PyBulletの使用方法

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

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

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

物体把持の全体的な流れについて

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

１：カメラより把持物体の位置を取得
２：始点から把持物体の位置まで，干渉しない経路を探索
３：干渉しない始点から終点までの経路を移動
４：物体把持

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

後ほど，上記の「１」に関する内容を説明する．

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

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

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

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

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

グリッパーの定義

本記事では，下図のようなパラレルグリッパーをロボットアームに追加する．グリッパーは3軸ロボットアームに追加したのと大方同じである．座標系および形状を少し変更した．

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

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

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

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

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

経路生成手法であるRRT

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

複数カメラによる把持物体の位置把握

本記事では，複数カメラを使用して把持物体の位置を把握する．
PyBulletでは，カメラによる把持物体の位置を取得する関数が用意されているため，使用する．
本記事では，カメラと光線を使用して，把持物体の位置を取得する．位置を取得する流れは以下の通りである．

１：カメラによる把持物体の２次元位置を取得
２：カメラから把持物体に向かって光線を照射
３：返ってきた光線より把持物体の３次元位置を取得
４：複数カメラから取得した把持物体の３次元位置を組み合わせる

深度カメラ(RGB-Dカメラまたはデプスカメラ)により，把持物体の３次元位置を取得しようと奔走したが，私の知識不足なため本記事では光線を使用する(２週間程度，奔走したが深度カメラで３次元位置を取得できなかったため，光線に切り替えた)．
将来的には，深度カメラだけで把持物体の３次元位置を取得できるようになりたい．

次節からは，上記の流れに関しての詳細を説明する．

カメラによる把持物体の２次元位置を取得

PyBulletでカメラ情報を取得する関数は "getCameraImage" である．関数情報は下図の通りである．

引数に関しての内容は下表の通りである．

引数名	内容
width	カメラの水平(X軸)解像度のピクセル数
hight	カメラの垂直(Y軸)解像度のピクセル数
viewMatrix	カメラから見た空間(4*4の行列)
projectionMatrix	カメラ空間の座標をxyzが$-1~1$に収まるような空間(4*4の行列)

戻り値に関しての内容は下表の通りである．

戻り値	内容
width	画像幅(ピクセル)
height	画像高さ(ピクセル)
rgbImg	RGB(Red・Green・Blue)画像
depthImg	深度画像(深度値は[0, 1]の範囲)
segImg	セグメンテーションマスク．各物体IDが取れるから，「どの物体の画素か」特定可能

viewMatrix(ビュー行列)・projectionMatrix(プロジェクション行列)に関しては，以下記事がわかりやすいです．以下記事に詳細な情報が記載されている．
https://light11.hatenadiary.com/entry/2019/01/27/160541

カメラから物体の位置(ピクセル)を取得するには，以下の順番で物体の座標変換を実装している．

１：モデル変換
物体のローカル座標(URDFなどで定義されたモデルの原点位置)をワールド座標系に変換する．

２：ビュー変換
ワールド座標系からカメラ座標系へ変換する．カメラがカメラ座標系の原点にいて，$-Z$方向を向いている状態に正規化される(PyBulletのcomputeViewMatrix()関数)．

３：プロジェクション変換
カメラ座標系から正規化デバイス座標(NDC)へ変換する．正規化デバイス座標からビューポート変換をすることで，カメラからのピクセル座標へ変換される(PyBulletのcomputeProjectionMatrixFOV()関数)．

逆に，カメラから物体の位置(ピクセル)を取得して，物体のワールド座標の２次元位置を取得する方法は以下の通りとなる．

１：ピクセル座標を正規化デバイス座標(NDC)へ変換
カメラから取得した物体の位置はピクセル座標になっているため，[-1, 1]の範囲である正規化デバイス座標(NDC)へ変換する．

２：プロジェクション逆変換
正規化デバイス座標(NDC)をカメラ座標系に変換する．プロジェクション行列の逆行列を使用して，カメラ座標系に変換する．

３：ビュー逆変換
カメラ座標系からワールド座標系に変換する．ビュー行列の逆行列を使用して，ワールド座標系に変換する．

物体の２次元位置を取得したら，光線を使って物体の３次元位置を取得する．本来であれば深度情報を使って，３次元位置を取得したかったが，上手くいかなかったため，光線を使用する方針とした．

光線による把持物体の３次元位置を取得

PyBulletで光線の照射による物体の情報を取得する関数は "rayTest" である．関数情報は下図の通りである．

引数に関しての内容は下表の通りである．

引数名	内容
rayFromPosition	光線の照射元の位置(位置はワールド座標系で記載)
rayToPosition	光線の照射先の位置(位置はワールド座標系で記載)

戻り値に関しての内容は下表の通りである．

戻り値	内容
objectUniqueId	光線が当たった物体のID(物体に当たらなかったら，-1)
linkIndex	光線が当たった物体のリンクの要素番号(リンクがない場合は，-1)
hit fraction	光線の照射元から照射先までの距離に対する，当たった位置の割合(0.0 ... 照射元の位置，1.0 ... 照射先の位置)
hit position	光線が当たった物体の位置(ワールド座標系から見た位置)
hit normal	光線が当たった物体の法線ベクトル(ワールド座標系から見たベクトル)

光線はカメラが照射していると考えると，カメラの位置をrayFromPosition(光源の照射元の位置)として与えて，カメラ座標系から見た物体の２次元位置の方向ベクトルをrayToPosition(光線の照射先の位置)として，与えることで，把持物体に向かって光線を照射でき，位置を取得することが可能となる．

ただ，光線が当たった物体の位置は，物体の中心位置ではなく，光線が当たった物体の表面上の位置である．そのため，把持物体が大きいほど把持する際に，物体の中心位置からのズレが大きくなってしまう．
本記事では，カメラを１台とするが，カメラを複数台にすることで，把持物体の位置をより正確に取得できると考えている．

光線に当たった物体に関する情報は，PyBulletの内部で実装するため，内部処理は不明である．

複数カメラによる把持物体の３次元位置を組み合わせる

複数のカメラから取得した，把持物体の３次元位置を組み合わせて一意の位置に変換する．本節では，変換方法についての説明をする．
変換の順番は以下の通りである．

１：各カメラにて，把持物体の位置(ピクセル)を取得 + カメラ画像内の把持物体に関するピクセル数を計算
２：把持物体に向かって，光線を照射して，３次元位置を取得
３：各カメラ画像内の把持物体に関するピクセル数を使用して，重み付き平均により把持物体の３次元位置を一意に決定

上記の「１」，「２」は単一のカメラと同様の処理であるため，本節では割愛する．
上記の「３」は数式で表すと，以下のようになる．

\displaylines{
P = \sum_{i=1}^{n} (pixel_{i} * P_{i}) / \sum_{i=1}^{n} pixel_{i} \\
P ... 全カメラを組み合わせた把持物体の３次元位置 (ベクトル) \\
P_{i} ... カメラiから見た把持物体の３次元位置 (ベクトル) \\
n ... カメラ画像内に把持物体が存在するカメラ数 (スカラー) \\
pixel_{i} ... カメラiの把持物体に関するピクセル数 (スカラー) \\
\sum_{i=1}^{n} pixel_{i} ... カメラ画像内に把持物体が存在するピクセル数の総計 (スカラー) \\
}

把持物体の位置は，カメラ画像内のピクセル数で重み付き平均を取った値である．
複数カメラを使用して，３次元位置を計算する方法としては他手法もあるが，本記事では簡単な処理としたいから，上記の手法とした．

把持物体の姿勢把握

カメラから物体の姿勢を把握するための知識を私は持っていないため，把持物体の姿勢は固定とする．
将来的には，把持物体の姿勢もカメラから推定できるようにしたいと考えている．

URDFソースコード

今回使用する，URDFのソースコードについて説明する．
ソースコードとして，下表の９ファイルを作成する．

ファイル名	概要	備考
camera_back_to_front.urdf	手前方向(-Y方向)を向いているカメラの定義	前記事にて説明
camera_front_to_back.urdf	奥行き方向(+Y方向)を向いているカメラの定義	前記事にて説明
camera_left_to_right.urdf	右方向(+X方向)を向いているカメラの定義	前記事にて説明
camera_right_to_left.urdf	左方向(-X方向)を向いているカメラの定義	前記事にて説明
camera_up_to_down.urdf	下方向(-Z方向)を向いているカメラの定義	前記事にて説明
environment.urdf	環境の定義	前々記事にて説明済
grasp_object.urdf	把持物体の定義	前々記事にて説明済
robot_6dof_hand.urdf	グリッパー付き６軸ロボットアームの定義	前々記事にて説明済
robot_6dof.urdf	グリッパーなし６軸ロボットアームの定義の定義	前々記事にて説明済

上表内のファイルに関しては，前記事および前々記事にて説明済のため，本記事では割愛する．
前記事：https://qiita.com/haruhiro1020/items/e5252faeac3486ce4947
前々記事：https://qiita.com/haruhiro1020/items/a43e5de8978b040e3719

全体のソースコード

はじめに，本記事で使用する全体のソースコードについて説明する．
その後，重要な箇所を抜粋して別途解説をしていく．
ソースコードとして，下表の１０ファイルを作成する．

ファイル名	概要
constant.py	定数の定義
main.py	全体的なメイン処理
pybullet_environment.py	PyBulletの環境
pybullet_grasp.py	PyBulletの把持物体
pybullet_gripper.py	PyBulletのグリッパー
pybullet_interference.py	PyBulletの干渉判定
pybullet_robot.py	PyBulletのロボット
pybullet_camera.py	PyBulletのカメラ
pybullet_rrt.py	PyBulletの経路生成
pybullet_main.py	PyBulletのメイン処理

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

定数の定義 (constant.py)

定数を定義する constant.py について説明する．

constant.py

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


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

# 重力
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での最短ノード要素
# RRT-Connect
RRTCONNECT_NEAR_NODE_IDX    = -1    # RRT-Connectでの最短ノード要素
# RRT*
RRTSTAR_NEAR_NODE_IDX       = -2    # RRT*での最短ノード要素
RRTSTAR_COST_IDX            = -1    # RRT*でのコスト要素

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


# RRT-Connectの状態を定義
class RRTCONNECTSTATE(Enum):
    """
    RRT-Connectの状態
    """
    STREE_RAND      = 0         # 始点ツリーにランダム点を追加
    STREE_TO_ETREE  = auto()    # 始点ツリーから終点ツリーへノードを伸ばす
    ETREE_RAND      = auto()    # 終点ツリーにランダム点を追加
    ETREE_TO_STREE  = auto()    # 終点ツリーから始点ツリーへノードを伸ばす


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


# RRTによる経路生成手法の定義
class PATHPLAN(Enum):
    """
    経路生成手法
    """
    RRT         = "rrt"             # RRTによる経路生成
    RRTCONNECT  = "rrt-connect"     # RRT-Connectによる経路生成
    RRTSTAR     = "rrt-star"        # 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"  # ハンド(グリッパ)付き

    # 6軸ロボットアーム
    DOF6 = "robot_6dof.urdf"            # ハンド(グリッパ)なし
    DOF6_HAND = "robot_6dof_hand.urdf"  # ハンド(グリッパ)付き


# カメラが保存されている URDF ファイル名
class CAMERAURDF(Enum):
    """
    カメラのURDFファイル名
    """
    # X軸方向に向いているカメラ
    RIGHT2LEFT = "camera_right_to_left.urdf"    # 左向き(-X方向)のカメラ
    LEFT2RIGHT = "camera_left_to_right.urdf"    # 右向き(+X方向)のカメラ

    # Y軸方向に向いているカメラ
    FRONT2BACK = "camera_front_to_back.urdf"    # 奥向き(+Y方向)のカメラ
    BACK2FRONT = "camera_back_to_front.urdf"    # 手前向き(-Y方向)のカメラ

    # Z軸方向に向いているカメラ
    UP2DOWN = "camera_up_to_down.urdf"  # 下向き(-Z方向)のカメラ


# カメラ数を定義する定数
class CAMERANUM(Enum):
    """
    カメラ数
    """
    SINGLE = 1          # 1つのカメラ
    MULTI  = 5          # 複数のカメラ

カメラの数は，"1" または "5" で固定とした．カメラ数を自由に設定できるようにしても良いが，自由度は高くなり制御が困難になると考えたため，"1" と "5" の2パターンだけとした．

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

全体的なメイン処理を定義する main.py について説明する．
後ほど説明する pybullet_main.py 内のクラスを実装するのがメインな処理である．

main.py

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


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


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



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


def main():
    """
    メイン処理
    """
    # ロボットが保存されている URDF ファイル名
    if N_ROBOT_AXIS == DIMENTION_2D:    # 2軸ロボットアーム
        environment_urdf = "environment_2dof.urdf"      # 環境が保存されている URDF ファイル名
        interpolation = INTERPOLATION.POSITION.value    # 直交空間の探索
        start_pos = np.array([1.0, -1.0])               # 初期位置

    elif N_ROBOT_AXIS == DIMENTION_3D:  # 3軸ロボットアーム
        environment_urdf = "environment_3dof.urdf"      # 環境が保存されている URDF ファイル名
        interpolation = INTERPOLATION.POSITION.value    # 直交空間の探索
        start_pos = np.array([1.0, -1.0, 1.0])          # 初期位置

    elif N_ROBOT_AXIS == DIMENTION_6D:  # 6軸ロボットアーム
        environment_urdf = "environment_6dof.urdf"      # 環境が保存されている URDF ファイル名
        interpolation = INTERPOLATION.JOINT.value       # 関節空間の探索
        start_pos = np.array([1.0, -1.0, 1.0, 0, np.pi/2, 0])   # 位置(x, y, z)・姿勢(ロール, ピッチ, ヨー)

    else:
        raise ValueError(f"N_ROBOT_AXIS is abnormal. N_ROBOT_AXIS is {N_ROBOT_AXIS}")

    # 経路生成手法
    path_plan = PATHPLAN.RRT.value          # RRTによる経路生成
    # path_plan = PATHPLAN.RRTCONNECT.value   # RRT-Connectによる経路生成
    # path_plan = PATHPLAN.RRTSTAR.value      # RRT*による経路生成

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

    # カメラ数を指定
    # n_cameras = CAMERANUM.SINGLE.value              # 1台のカメラ
    n_cameras = CAMERANUM.MULTI.value               # 複数台のカメラ

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

    # シード値の設定
    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()

main.py内では，カメラの数だけを設定できる．別クラスにて，カメラの位置・姿勢を設定する．

PyBulletの環境設定 (pybullet_environment.py)

PyBulletの環境設定を定義する pybullet_environment.py について説明する．

pybullet_environment.py

# PyBulletの環境に関するクラス


# ライブラリの読み込み
import pybullet as p    # PyBullet


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



class PyBulletEnvironment:
    """
    PyBulletの環境クラス

    プロパティ
        __environment_id(): PyBulletでの環境に関するID番号

    メソッド
        public
            environment_id(): __environment_idプロパティのゲッター
    """
    # 定数の定義
    __PLANE_URDF = "plane.urdf"     # 地面に関する urdf ファイル
    
    # 各ロボットアームに応じた環境のベース位置
    __ENVIRONMENT_POS_2DOF = [0   ,  0,  0  ]   # 2軸ロボットアームの環境のベース位置
    __ENVIRONMENT_POS_3DOF = [1.5 ,  0, -0.5]   # 3軸ロボットアームの環境のベース位置
    __ENVIRONMENT_POS_6DOF = [1.75,  0,  0  ]   # 6軸ロボットアームの環境のベース位置
    
    
    def __init__(self, environment_urdf, n_robot_joint):
        """
        コンストラクタ

        パラメータ
            environment_urdf(str): 環境が保存されているファイル名
            n_robot_joint(int): ロボットの関節数 (グリッパーは含まない)
        """
        # 地面を読み込む (pybulletが提供している "plane.urdf" を読み込む)
        plane_id = p.loadURDF(self.__PLANE_URDF)
        print(f"plane_id = {plane_id}")

        # ロボットアームに応じて，環境のベース位置を変える
        if n_robot_joint == DIMENTION_2D:
            # 2軸ロボットアーム
            basePosition = self.__ENVIRONMENT_POS_2DOF
        elif n_robot_joint == DIMENTION_3D:
            # 3軸ロボットアーム
            basePosition = self.__ENVIRONMENT_POS_3DOF
        elif n_robot_joint == DIMENTION_6D:
            # 6軸ロボットアーム
            basePosition = self.__ENVIRONMENT_POS_6DOF
        else:
            # 異常
            raise ValueError(f"n_robot_joint is abnormal. n_robot_joint is {n_robot_joint}")

        # 環境を読み込む (環境は常に移動しない)
        self.__environment_id = p.loadURDF(environment_urdf, basePosition=basePosition, useFixedBase=True)
        print(f"self.__environment_id = {self.__environment_id}")


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

PyBulletの把持物体 (pybullet_grasp.py)

PyBulletの把持物体を定義する pybullet_grasp.py について説明する．

pybullet_grasp.py

# PyBulletの把持物体に関するクラス


# ライブラリの読み込み
import pybullet as p    # PyBullet
import numpy as np      # 数値計算


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



class PyBulletGraspObject:
    """
    PyBulletの把持物体クラス

    プロパティ
        __grasp_obj_id(int): PyBulletでの把持物体に関するID番号
        __grasp_obj_offset(list): 把持物体のオフセット
        __constraint_id(int): PyBulletでの把持物体の拘束に関するID番号

    メソッド
        public
            grasp_obj_id(): _grasp_obj_idプロパティのゲッター
            set_constraint(): 拘束条件の設定
            release_constraint(): 拘束条件の解除
            get_grasp_pos(): 把持物体の位置を取得
    """
    # 定数の定義
    # 各ロボットアームに応じた把持物体のベース位置
    __GRASP_OBJ_POS_2DOF = [1.8 , 1.0, 0.05] # 2軸ロボットアームの把持物体のベース位置
    __GRASP_OBJ_POS_3DOF = [1.25, 0.4, 0.55] # 3軸ロボットアームの把持物体のベース位置
    __GRASP_OBJ_POS_6DOF = [1.5 , 0.4, 1.05] # 6軸ロボットアームの把持物体のベース位置

    # 各ロボットアームに応じた把持物体のオフセット位置
    __GRASP_OBJ_OFFSET_2DOF = [-0.4 , 0      ]   # 2軸ロボットアームの把持物体のオフセット
    __GRASP_OBJ_OFFSET_3DOF = [-0.15, 0, 0.15]   # 3軸ロボットアームの把持物体のオフセット
    __GRASP_OBJ_OFFSET_6DOF = [-0.2 , 0, 0.2 ]   # 6軸ロボットアームの把持物体のオフセット

    # 把持物体のパラメータ
    __LATERAL_FRICTION  = 1.0    # 床との摩擦係数
    __SPINNING_FRICTION = 1.0    # 回転摩擦係数
    __ROLLING_FRICTION  = 1.0    # 転がり摩擦係数

    __GRASP_OBJ_OFFSET_PITCH = np.pi / 2


    def __init__(self, grasp_obj_urdf, n_robot_joint):
        """
        コンストラクタ

        パラメータ
            grasp_obj_urdf(str): 把持物体が保存されているファイル名
            n_robot_joint(int): ロボットの関節数 (グリッパーは含まない)
        """
        # ロボットアームに応じて，把持物体のベース位置・オフセットを変える
        if n_robot_joint == DIMENTION_2D:
            # 2軸ロボットアーム
            basePosition = self.__GRASP_OBJ_POS_2DOF
            self.__grasp_obj_offset = self.__GRASP_OBJ_OFFSET_2DOF

        elif n_robot_joint == DIMENTION_3D:
            # 3軸ロボットアーム
            basePosition = self.__GRASP_OBJ_POS_3DOF
            self.__grasp_obj_offset = self.__GRASP_OBJ_OFFSET_3DOF

        elif n_robot_joint == DIMENTION_6D:
            # 6軸ロボットアーム
            basePosition = self.__GRASP_OBJ_POS_6DOF
            self.__grasp_obj_offset = self.__GRASP_OBJ_OFFSET_6DOF

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

        # 把持物体を読み込む
        self.__grasp_obj_id = p.loadURDF(grasp_obj_urdf, basePosition=basePosition)
        print(f"self.__grasp_obj_id = {self.__grasp_obj_id}")

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

        # 把持物体の位置・姿勢を取得
        grasp_pos, grasp_ori = self.get_grasp_pos(offset=False)

        # 把持物体に拘束条件を付与
        self.__constraint_id = None
        self.set_constraint(grasp_pos, grasp_ori)


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

        パラメータ
            pos(list): 拘束したい位置
            ori(list): 拘束したい姿勢
        """
        if self.__constraint_id is not None:
            # 拘束条件を2重に設定しようとしている
            raise ValueError("set_constrant() is double exection. please run release_constraint()")

        self.__constraint_id = p.createConstraint(
                            self.__grasp_obj_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]) # 子の中心からの姿勢 (今回は，子を設定していないから，ワールド座標系から見た姿勢)

    def release_constraint(self):
        """
        拘束条件の解除
        """
        if self.__constraint_id is not None:
            # 拘束条件を解除
            p.removeConstraint(self.__constraint_id)
            self.__constraint_id = None

    def get_grasp_pos(self, offset=True, dim2=False):
        """
        把持対象物の位置・姿勢を取得

        パラメータ
            offset(bool): 把持対象物へのオフセットを設定するかどうか
            dim2(bool): 2次元位置として取得するかどうか

        戻り値
            list: 把持対象物の位置 [m]
            list: 把持対象物の姿勢 (ロール・ピッチ・ヨー [rad])
        """
        # 把持対象物の位置[m]・姿勢[rad]を取得
        grasp_pos, grasp_ori = p.getBasePositionAndOrientation(self.__grasp_obj_id)
        # 位置をタプルからリストへ変換
        grasp_pos = list(grasp_pos)

        # 姿勢をクォータニオンからロール・ピッチ・ヨーへ変換
        roll, pitch, yaw = p.getEulerFromQuaternion(grasp_ori)
        # ピッチ角を90度回転させる → 把持対象物の正面(x方向)から把持したいから
        pitch += self.__GRASP_OBJ_OFFSET_PITCH
        grasp_ori_rpy = [roll, pitch, yaw]

        if offset:
            # オフセット量の考慮
            grasp_pos = [pos + off for pos, off in zip(grasp_pos, self.__grasp_obj_offset)]

        if dim2:
            grasp_pos = grasp_pos[:DIMENTION_2D]

        return grasp_pos, grasp_ori_rpy


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

PyBulletのグリッパー (pybullet_gripper.py)

PyBulletのグリッパーを定義する pybullet_gripper.py について説明する．

pybullet_gripper.py

# PyBulletで使用するグリッパーを記載


# ライブラリの読み込み
import pybullet as p    # PyBullet
import numpy as np      # 数値計算ライブラリ


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



class BaseGripper:
    """
    グリッパーのベースクラス (抽象クラス)

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

    メソッド
        public
            run(): 実行 現在角度を関節に与える (オープン・クローズ以外で常に呼ぶこと)
    """
    # 定数の定義
    _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   # 左グリッパーの関節番号
    
    __LATERAL_FRICTION  = 1.0   # 摩擦係数
    
    __CLOSE_VAL = 0.03          # クローズ時のフィンガー角度 [m]
    __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=self.__LATERAL_FRICTION)    # 床との摩擦係数
    
    
    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.__OPEN_VAL
        elif close:
            # グリッパーのクローズ時
            # グリッパーのクローズに関するキーボードが押下された時
            direction = self.__get_move_direction(open=open)
            joint_values = direction * self.__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)

PyBulletの干渉判定 (pybullet_interference.py)

PyBulletの干渉判定を定義する pybullet_interference.py について説明する．

pybullet_interference.py

# PyBulletの干渉判定に関するクラス


# ライブラリの読み込み
import pybullet as p    # PyBullet
import numpy as np      # 数値計算
import time             # 時間


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



class PyBulletInteference:
    """
    PyBulletの干渉判定クラス

    プロパティ
        __robot(): ロボットクラス
        __environment(): 環境クラス


    メソッド
        public
            is_line_interference(): 2点間の干渉判定
    """
    # 定数の定義
    __SIMULATION_SLEEP_TIME = 0.05   # シミュレーションの待機時間 [sec]
    __INTERFERENCE_MARGIN   = 0.15   # 干渉判定のマージン [m]
    
    
    def __init__(self, robot:PyBulletRobotController, environment:PyBulletEnvironment):
        """
        コンストラクタ

        パラメータ
            robot(PyBulletRobotController): PyBulletでのロボットクラス
            environment(PyBulletEnvironment): PyBulletでの環境クラス
        """
        # プロパティの初期化
        self.__robot = robot
        self.__environment = environment


    def is_line_interference(self, pos1, pos2):
        """
        2点間の干渉判定

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

        戻り値
            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.robot_id, self.__environment.environment_id, self.__INTERFERENCE_MARGIN)
        if len(close_points) == 0:  # 干渉なし
            is_interference = False

        return is_interference


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

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

        # 位置から関節角度に変換
        theta = self.__robot.convert_pos_to_theta(pos)
        # 位置にジャンプ
        self.__robot.set_jump_joint(theta)
        # グリッパーの実行
        self.__robot.run_gripper(open=True)

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

        # ロボットと干渉物との干渉判定
        close_points = p.getClosestPoints(self.__robot.robot_id, self.__environment.environment_id, self.__INTERFERENCE_MARGIN)
        if len(close_points) == 0:  # 干渉なし
            is_interference = False

        return is_interference

PyBulletで使用するロボット (pybullet_robot.py)

PyBulletで使用するロボットを定義する pybullet_robot.py について説明する．

pybullet_robot.py

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


# ライブラリの読み込み
import pybullet as p    # PyBullet
import numpy as np      # 数値計算ライブラリ


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


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



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               # ハンド用の関節数 (パラレルグリッパーだけに対応するため，4固定)
    _N_HAND_JOINT_NOT_FIXED = 2     # ハンド用の関節で固定関節以外

    _WEIGHT_JOINT = None            # 各軸への重み (RRTで使用

    _JOINT_LIMIT_LOW_IDX = 8        # 関節限界の下限値インデックス
    _JOINT_LIMIT_UP_IDX  = 9        # 関節限界の上限値インデックス


    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

        # URDFから関節限界を取得
        self.__joint_limit()


    def __joint_limit(self):
        """
        関節限界
        """
        joint_limit = []

        # 全軸の関節限界
        for joint_index in range(self._DIM_JOINT):
            joint_info = p.getJointInfo(self._robot_id, joint_index)
            lower = joint_info[self._JOINT_LIMIT_LOW_IDX]
            upper = joint_info[self._JOINT_LIMIT_UP_IDX]
            joint_limit.append((lower, upper))

        # プロパティの更新
        self._joints_limit = np.array(joint_limit)


    # プロパティのゲッター ↓
    @property
    def robot_id(self):
        """
        _robot_idプロパティのゲッター
        """
        return self._robot_id

    @property
    def joints_limit(self):
        """
        _joints_limitプロパティのゲッター
            最小関節限界：[:, 0]
            最大関節限界：[:, 1]
        """
        return self._joints_limit

    @property
    def weight_joint(self):
        """
        _WEIGHT_JOINT (各関節の重み) のゲッター
        """
        return self._WEIGHT_JOINT

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


    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 len(thetas) != self._DIM_JOINT:
            # 異常
            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)

        if self._hand is not None:
            # グリッパー付きの場合は，グリッパー部分を削除
            thetas = thetas[:self._DIM_JOINT]

        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)

        if self._hand is not None:
            # グリッパー付きの場合は，グリッパー部分を削除
            thetas = thetas[:self._DIM_JOINT]

        return thetas


class _PyBullet6DoFRobot(_PyBulletRobot):
    """
    PyBulletの6軸ロボットクラス

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

        private
            __inverse_kinematics(): 逆運動学(位置から関節角度に変換)
    """
    # 定数の定義
    _DIM_JOINT = DIMENTION_6D     # 関節の次元数
    _DIM_POSE  = DIMENTION_6D     # 位置(姿勢も含む)の次元数
    
    # _WEIGHT_JOINT = np.array([1, 1, 1, 1, 1, 1])  # 各軸への重み (RRTで使用)
    
    
    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]・姿勢[rad]

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

        # パラメータ pos を位置と姿勢に分解
        position, orientation = pos[:DIMENTION_3D], pos[DIMENTION_3D:]
        # 姿勢を ロール・ピッチ・ヨー から クォータニオンへ変換
        # 逆運動学の解を算出するには，姿勢はクォータニオンでないといけない
        quaternion = p.getQuaternionFromEuler(orientation)

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

        if self._hand is not None:
            # グリッパー付きなら，グリッパー部分の角度は不要
            thetas = thetas[:self._DIM_JOINT]

        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)

                if self._hand is not None:
                # グリッパー付きなら，グリッパー部分の角度は不要
                    thetas = thetas[:self._DIM_JOINT]

        return thetas


class PyBulletRobotController:
    """
    PyBulletのロボットの制御クラス
        上位クラスとロボットクラスの仲介クラス

    プロパティ
        __robot(pybullet_robot.py内のクラス): ロボットクラス
        __n_robot_joint(int): ロボットアームの関節数 (グリッパーは含まない)

    メソッド
        purlic
            n_robot_joint(int): __n_robot_jointプロパティのゲッター
            set_joint(): 関節角度の設定
            set_jump_joint(): 関節角度をジャンプ
            convert_pos_to_theta(): 位置から関節角度に変換
            run_gripper(): グリッパーの実行
    """
    # 定数の定義
    __ROBOT_BASE_POSITION = [0, 0, 0]   # ロボットーのベース位置
    
    
    def __init__(self, n_robot_joint, interpolation, hand):
        """
        コンストラクタ

        パラメータ
            n_robot_joint(int): ロボットアームの関節数(2, 3, 6だけ)
            interpolation(str): 探索方法 (関節空間/位置空間)
            hand(bool): ハンドの装着有無 True/False = 装着/未装着
        """
        # ロボットアームに応じて，URDFを変える
        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

            # 3軸ロボットアームのクラス
            robot_cls = _PyBullet3DoFRobot

        elif n_robot_joint == DIMENTION_6D:
            # 6軸ロボットアーム
            if hand:    # ハンド装着
                robot_urdf = ROBOTURDF.DOF6_HAND.value
            else:       # ハンド未装着
                robot_urdf = ROBOTURDF.DOF6.value

            # 6軸ロボットアームのクラス
            robot_cls = _PyBullet6DoFRobot

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

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

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

        # プロパティの更新
        self.__n_robot_joint = n_robot_joint


    # プロパティのゲッター ↓
    @property
    def robot_id(self):
        """
        PyBulletで割り当てられたロボットID
        """
        return self.__robot.robot_id

    @property
    def joints_limit(self):
        """
        関節限界
            最小関節限界：[:, 0]
            最大関節限界：[:, 1]
        """
        return self.__robot.joints_limit

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

    @property
    def weight_joint(self):
        """
        各関節の重みを取得
        """
        return self.__robot.weight_joint

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


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

        パラメータ
            thetas(numpy.ndarray): 関節角度 [rad]
        """
        self.__robot.set_joint(thetas)

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

        パラメータ
            thetas(numpy.ndarray): 関節角度 [rad]
        """
        self.__robot.set_jump_joint(thetas)

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

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

        戻り値
            numpy.ndarray: 関節角度 [rad]
        """
        return self.__robot.convert_pos_to_theta(pos, force)

    def run_gripper(self, open=False, close=False):
        """
        グリッパーの実行
        
        パラメータ
            open(bool): グリッパーのオープンフラグ
            close(bool): グリッパーのクローズフラグ
        """
        self.__robot.run_gripper(open=open, close=close)

PyBulletの経路生成 (pybullet_rrt.py)

PyBulletの経路生成を定義する pybullet_rrt.py について説明する．

pybullet_rrt.py

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


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


# 自作モジュールの読み込み
from constant import *              # 定数
from pybullet_interference import PyBulletInteference   # 干渉判定に関して



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, weight):
        """
        最短距離のノードを取得

        パラメータ
            pos(numpy.ndarray): 位置
            weight(numpy.ndarray): 各次元の重み

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

        # ノードから位置を取得
        nodes_pos = self._nodes[:, :self._near_node_idx]
        # 差分を計算
        difference = nodes_pos - pos

        # 修正 ↓
        # 差分に重みを考慮
        if weight is None:
            weight = 1
        difference = difference * weight
        # 修正 ↑

        # 距離を計算 (各ノードとの距離を算出するため，引数に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 _CostedTree(_Tree):
    """
    コスト付きツリークラス

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

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


    def __init__(self, near_node_idx, cost_idx):
        """
        コンストラクタ

        パラメータ
            near_node_idx(int): 最短ノード列番号
            cost_idx(int): コスト列番号
        """
        # プロパティの初期化
        self._nodes = []
        self._near_node_idx = near_node_idx
        self._cost_idx = cost_idx

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

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

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

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

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

        return cost

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

        パラメータ
            node_idx(int): 変更したいノード番号
            near_node(int): 変更後の最短ノード
            cost(float): 変更後のコスト
        """
        # ノードの存在確認
        self._chk_node_exist()

        self._nodes[node_idx, self._near_node_idx] = near_node
        self._nodes[node_idx, self._cost_idx] = cost


class _PyBulletRRT:
    """
    PyBullet用の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]

    メソッド
        public

            プロパティのゲッター関連
                pathes(): _pathesプロパティのゲッター

            準備処理
                preparation(): 経路生成の準備
                set_strict_pos(): 探索範囲を設定

            メイン処理関連
                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データをテキストファイルに保存
    """
    # 定数の定義
    # 経路生成の名前
    _PATH_PLAN = PATHPLAN.RRT.value

    # ファイル名の定義
    # _pathesプロパティを保存するファイル名
    _FILE_NAME_PATHES = f"{_PATH_PLAN}_pathes.csv"
    # _start_treeプロパティを保存するファイル名
    _FILE_NAME_START_TREE = f"{_PATH_PLAN}_start_tree.csv"

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

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

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


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


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


    def _preparation_planning(self, start_pos, end_pos, interpolation, interference:PyBulletInteference, joints_limit):
        """
        経路生成の準備

        パラメータ
            start_pos(numpy.ndarray): 経路生成の始点
            end_pos(numpy.ndarray): 経路生成の終点
            interpolation(INTERPOLATION): 補間方法 (関節空間/位置空間)
            interference(PyBulletInteference): 干渉判定クラス
            joints_limit(numpy.ndarray): 関節限界
        """
        # データの初期化
        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._interference = interference

        # 探索空間の設定
        self._set_interpolation(interpolation)

        # 始点と終点の干渉判定
        self._is_interference_start_end_pos(start_pos, end_pos)

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

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

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

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

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

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

    # メイン処理メソッド ↓
    def planning(self, start_pos, end_pos, interpolation, interference, joints_limit, weight=None):
        """
        始点から終点までの経路生成の実行

        パラメータ
            start_pos(numpy.ndarray): 経路生成の始点
            end_pos(numpy.ndarray): 経路生成の終点
            interpolation(INTERPOLATION): 補間方法 (関節空間/位置空間)
            interference(PyBulletInteference): 干渉判定クラス
            joins_limits(numpy.ndarray): 関節限界 (0列目：最小値, 1列目：最大値)
            weight(numpy.ndarray): 各次元の重み

        戻り値
            bool: True/False = 経路生成の成功/失敗
        """
        # 戻り値
        result = False

        # 経路生成の準備
        self._preparation_planning(start_pos, end_pos, interpolation, interference, joints_limit)

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

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

        counter = 0

        # 終点までの経路生成が終わるまでループ
        while True:
            counter += 1
            # ランダムな位置を取得
            random_pos = self._get_random_pos(end_pos)
            # ランダムな位置と最短ノードを計算
            near_node  = self._start_tree.get_near_node(random_pos, weight)
            # 最短ノードの位置を取得
            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, weight)

            # 新規ノードと最短ノード間での干渉判定
            is_interference = self._interference.is_line_interference(new_node_pos, near_node_pos)
            if not is_interference:
                # 干渉なしのため，ノード追加
                self._add_node_start_tree(new_node_pos, near_node)

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

            now_time = time.time()
            if now_time - start_time >= self._TIMEOUT_VALUE:
                # タイムアウト
                break

        print(f"counter = {counter}")

        if result:
            # 経路生成に成功のため，経路生成の後処理
            self._fin_planning(start_pos, end_pos)

        return result

    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, target_pos):
        """
        目標点との距離が一定範囲内かつ干渉なしであるかの確認

        パラメータ
            pos(numpy.ndarray): ノード位置
            end_pos(numpy.ndarray): 目標点

        戻り値
            bool: True / False = 一定範囲内かつ干渉なしである / でない
        """
        # 戻り値
        is_near = False

        # 距離を計算
        dist = np.linalg.norm(target_pos - pos)

        # 一定範囲内かつ干渉なしかの確認
        if dist <= self._moving_value:  # 一定距離内
            if not self._interference.is_line_interference(pos, target_pos):    # 干渉なし
                is_near = True

        return is_near

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

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

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

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

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

        パラメータ
            random_pos(numpy.ndarray): ランダムな位置
            near_node_pos(numpy.ndarray): 最短ノード位置
            weight(numpy.ndarray): 各次元の重み

        戻り値
            new_pos(numpy.ndarray): 新しいノード
        """
        # 方向を計算
        direction = random_pos - near_node_pos

        # 修正後 ↓
        # 重みの計算
        if weight is None:
            weight = 1

        weighted_direction = direction * weight
        norm_direction = weighted_direction / (np.linalg.norm(weighted_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 set_strict_pos(self, min_pos, max_pos):
        """
        探索範囲の設定

        パラメータ
            min_pos(numpy.ndarray): 探索の最小範囲
            max_pos(numpy.ndarray): 探索の最大範囲
        """
        # パラメータの要素数確認
        if min_pos.shape[0] != max_pos.shape[0]:
            raise ValueError(f"min_pos'shape and max_pos'shape are not match. min_pos'shape is {min_pos.shape}. max_pos'shape is {max_pos.shape}")
        if min_pos.shape[0] != self._strict_min_pos.shape[0]:
            raise ValueError(f"min_pos'shape and _strict_min_pos'shape are not match. min_pos'shape is {min_pos.shape[0]}. _strict_min_pos'shape is {self._strict_min_pos.shape[0]}")

        # プロパティの更新
        self._strict_min_pos = min_pos
        self._strict_max_pos = max_pos

    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, joints_limit):
        """
        探索範囲を制限する

        パラメータ
            start_pos(numpy.ndarray): 始点
            end_pos(numpy.ndarray): 終点
            joints_limit(numpy.ndarray): 関節限界 (0列目：最小値, 1列目：最大値)
        """
        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:
            # 位置空間の探索
            self._strict_min_pos = min_pos - self._STRICT_PLANNING_ROB_POS
            self._strict_max_pos = max_pos + self._STRICT_PLANNING_ROB_POS
        else:
            # 関節空間の探索
            # 探索の最小値を関節の最小値以上に制限
            min_joints = np.array([min_pos - self._STRICT_PLANNING_ROB_JOINT, joints_limit[:, 0]])
            self._strict_min_pos = np.max(min_joints, axis=0)
            # 探索の最大値を関節の最大値以下に制限
            max_joints = np.array([max_pos + self._STRICT_PLANNING_ROB_JOINT, joints_limit[:, 1]])
            self._strict_max_pos = np.min(max_joints, axis=0)
    # 準備処理関連メソッド ↑


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


class _PyBulletRRTConnect(_PyBulletRRT):
    """
    PyBullet用のRRT-Connectクラス

    プロパティ
        _name(str): 経路生成手法名
        _end_tree(_Tree): 終点側のツリー
        _state(int): 現在のツリー状態
        _states(list): 全時刻のツリー状態

    メソッド
        public

            メイン処理
                expand_once(): 新規ノードおよび最短ノードの取得 (干渉判定は実装しない)

        protected
            (): 
    """
    # 定数の定義
    # 経路生成の名前
    _PATH_PLAN = PATHPLAN.RRTCONNECT.value

    # ツリーの要素番号を定義
    _NODE_NEAR_NODE_IDX = RRTCONNECT_NEAR_NODE_IDX


    def __init__(self):
        """
        コンストラクタ
        """
        # 親クラスのコンストラクタを呼ぶ
        super().__init__()
        # プロパティの初期化
        self._end_tree = _Tree(self._NODE_NEAR_NODE_IDX)
        self._state  = RRTCONNECTSTATE.STREE_RAND.value
        self._states = []


    # メイン処理メソッド ↓
    def planning(self, start_pos, end_pos, interpolation, interference, joints_limit, weight=None):
        """
        始点から終点までの経路生成の実行

        パラメータ
            start_pos(numpy.ndarray): 経路生成の始点
            end_pos(numpy.ndarray): 経路生成の終点
            interpolation(INTERPOLATION): 補間方法 (関節空間/位置空間)
            interference(PyBulletInteference): 干渉判定クラス
            joins_limits(numpy.ndarray): 関節限界 (0列目：最小値, 1列目：最大値)
            weight(numpy.ndarray): 各次元の重み

        戻り値
            bool: True/False = 経路生成の成功/失敗
        """
        # 戻り値
        result = False

        # 経路生成の準備
        self._preparation_planning(start_pos, end_pos, interpolation, interference, joints_limit)

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

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

        counter = 0

        # 始点から終点までの経路が生成できるまでループ
        while True:
            counter += 1
            # 1処理実施して，経路生成が完了したか確認する
            if self._exec_one_step(start_pos, end_pos, weight):
                # 経路生成の完了
                result = True
                break

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

        print(f"counter = {counter}")

        if result:
            # 経路生成に成功のため，経路生成の後処理
            self._fin_planning(start_pos, end_pos)

        return result

    def _state_transition(self):
        """
        状態を遷移させる
        """
        if self._state == RRTCONNECTSTATE.STREE_RAND.value:
            # 始点ツリーにランダム点を追加する状態
            self._state = RRTCONNECTSTATE.ETREE_TO_STREE.value

        elif self._state == RRTCONNECTSTATE.STREE_TO_ETREE.value:
            # 始点ツリーに終点ツリーで作成したノードへ伸ばす状態
            self._state = RRTCONNECTSTATE.STREE_RAND.value

        elif self._state == RRTCONNECTSTATE.ETREE_RAND.value:
            # 終点ツリーにランダム点を追加する状態
            self._state = RRTCONNECTSTATE.STREE_TO_ETREE.value

        else:
            # 終点ツリーに始点ツリーで作成したノードへ伸ばす状態
            self._state = RRTCONNECTSTATE.ETREE_RAND.value

    def _exec_one_step(self, start_pos, end_pos, weight):
        """
        1ステップの処理を実施 (探索を1回実装)

        パラメータ
            start_pos(numpy.ndarray): 始点
            end_pos(numpy.ndarray): 終点
            weight(numpy.ndarray): 各次元の重み

        戻り値
            bool: True/False = 経路生成の完了 / 未完了
        """
        # 戻り値
        complete_path = False

        # 最短ノードを算出したいツリーを取得
        if self._state == RRTCONNECTSTATE.STREE_RAND.value or self._state == RRTCONNECTSTATE.STREE_TO_ETREE.value:
            # 始点ツリーにランダム点を追加する状態または始点ツリーに終点ツリーで作成したノードへ伸ばす状態
            my_tree = self._start_tree
            your_tree = self._end_tree
            target_pos = end_pos
            set_tree_func = self._add_node_start_tree
        else:
            my_tree = self._end_tree
            your_tree = self._start_tree
            target_pos = start_pos
            set_tree_func = self._add_node_end_tree

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

            # 干渉物との干渉判定
            is_interference = self._interference.is_line_interference(new_node_pos, near_node_pos)
            if not is_interference:  # 干渉なし
                # ツリーにノードを追加
                set_tree_func(new_node_pos, near_node)
                # 状態を遷移させる
                self._state_transition()

        else:
            # ツリーで作成したノードを取得
            target_pos = your_tree.nodes[-1, :self._NODE_NEAR_NODE_IDX]
            # ツリー内の最短ノードを計算
            near_node = my_tree.get_near_node(target_pos, weight)
            # 位置と最短ノードが保存されているから，位置だけを取得
            near_node_pos = my_tree.nodes[near_node, :self._NODE_NEAR_NODE_IDX]
            # 最短ノードから終点ツリーのノード方向へ新しいノードを作成
            new_node_pos = self._calc_new_pos(target_pos, near_node_pos, weight)

            # 干渉物との干渉判定
            is_interference = self._interference.is_line_interference(new_node_pos, new_node_pos)
            if is_interference:  # 干渉あり
                # 状態を遷移させる
                self._state_transition()
            else:   # 干渉なし
                # ツリーにノードを追加
                set_tree_func(new_node_pos, near_node)
                # 始点から終点までの経路が生成できたかの確認
                if self._chk_end_pos_dist(new_node_pos, target_pos):
                    # 経路生成の完了
                    complete_path = True

        return complete_path

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

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

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

        パラメータ
            start_pos(list): 経路生成の始点
            end_pos(list): 経路生成の終点
        """
        # 始点ツリーから，終点ツリーと繋がったパスを取得
        start_path = []
        near_node  = -1

        while True:
            # 終点ツリーと繋がったノードから始点へノードを取得
            node = self._start_tree.nodes[near_node]
            pos  = node[:self._NODE_NEAR_NODE_IDX].reshape(1, -1)
            # 浮動小数点を整数型に型変換
            near_node = int(node[self._NODE_NEAR_NODE_IDX])

            if len(start_path) == 0:
                # 初回だけ
                start_path = pos
            else:
                start_path = np.append(start_path, pos, axis=0)

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

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

        # 終点ツリーから，始点ツリーと繋がったパスを取得
        end_path  = []
        near_node = -1

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

            if len(end_path) == 0:
                # 初回だけ
                end_path = pos
            else:
                end_path = np.append(end_path, pos, axis=0)

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

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


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

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

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



class _PyBulletRRTStar(_PyBulletRRT):
    """
    PyBullet用のRRT*クラス

    プロパティ
        _name(str): 経路生成手法名
        _start_tree(_CostedTree): 始点側のツリー
        _max_iterate(int): 最大探索回数
        _near_node_radius(float): 近傍ノードの探索半径

    メソッド
        public

            メイン処理
                expand_once(): 新規ノードおよび最短ノードの取得 (干渉判定は実装しない)

        protected
            (): 
    """
    # 定数の定義
    # 経路生成の名前
    _PATH_PLAN = PATHPLAN.RRTSTAR.value

    # ツリーの要素番号を定義
    _NODE_NEAR_NODE_IDX = RRTSTAR_NEAR_NODE_IDX     # 最短ノードの列番号
    _NODE_COST_IDX      = RRTSTAR_COST_IDX          # コストの列番号

    # その他
    _MAX_ITERATE = 3000             # 最大探索回数
    _TIMEOUT_VALUE = 600            # タイムアウト
    _NEAR_NODE_RADIUS_COEF = 10     # 近傍ノードとする探索球の半径の係数


    def __init__(self):
        """
        コンストラクタ
        """
        # プロパティの初期化
        self._name = self._PATH_PLAN
        self._start_tree = _CostedTree(self._NODE_NEAR_NODE_IDX, self._NODE_COST_IDX)
        self._pathes = []
        self._max_iterate = self._MAX_ITERATE
        self._near_node_radius = 0


    # メイン処理メソッド ↓
    def planning(self, start_pos, end_pos, interpolation, interference, joints_limit, weight=None):
        """
        始点から終点までの経路生成の実行

        パラメータ
            start_pos(numpy.ndarray): 経路生成の始点
            end_pos(numpy.ndarray): 経路生成の終点
            interpolation(INTERPOLATION): 補間方法 (関節空間/位置空間)
            interference(PyBulletInteference): 干渉判定クラス
            joins_limits(numpy.ndarray): 関節限界 (0列目：最小値, 1列目：最大値)
            weight(numpy.ndarray): 各次元の重み

        戻り値
            bool: True/False = 経路生成の成功/失敗
        """
        # 戻り値
        result = False

        # 経路生成の準備
        self._preparation_planning(start_pos, end_pos, interpolation, interference, joints_limit)

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

        # 近傍ノードを定義する半径
        self._near_node_radius = self._moving_value * self._NEAR_NODE_RADIUS_COEF

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

        # 全探索回数ループ
        for counter in range(self._max_iterate):
            # ランダムな値を取得
            random_pos = self._get_random_pos(end_pos)
            # ランダムな値と最短ノードを計算
            near_node  = self._start_tree.get_near_node(random_pos, weight)
            # 最短ノード位置を取得
            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, weight)

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

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

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

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

        if result:
            # 経路生成に成功のため，経路生成の終了処理
            self._fin_planning(start_pos, end_pos, weight)

        return result

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

        パラメータ
            pos(numpy.ndarray): 位置
            near_node(numpy.ndarray): ランダムな位置
            cost(int): 最短ノード
        """
        # ノード作成
        array_data = np.array([near_node, cost])
        node = np.append(pos, array_data).reshape(1, -1)

        # ツリーにノードを追加
        self._start_tree.add_node(node)

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

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

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

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

        return cost

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

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

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

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

        return near_node_list

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

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

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

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

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

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

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

        return min_cost_node, min_cost

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

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

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

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

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

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

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

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

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

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

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

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

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


class PyBulletRRTController:
    """
    PyBulletでの経路生成の制御クラス
        (上位クラスと経路生成クラスを仲介するクラス)

    プロパティ
        __rrt(pybullet_rrt.py内のクラス): 経路生成手法クラス
        __interference(PyBulletInteference): 干渉判定クラス

    メソッド
        public

            RRTクラスのプロパティのゲッター関連
                pathes(): 始点から終点までの経路

            準備関連
                preparation(): 経路生成の準備
                set_strict_pos(): 探索範囲の設定

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

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

            初期化関連
                reset(): データの初期化

        private

            初期化関連
                __get_each_path_plan_instance(): 経路生成手法に応じたクラスのインスタンスを取得
    """
    # 定数の定義


    def __init__(self, path_plan):
        """
        コンストラクタ

        パラメータ
            path_plan(PATHPLAN): 経路生成手法
            interference(PyBulletInteference): 干渉判定クラス
        """
        # プロパティの初期化
        self.__rrt = self.__get_each_path_plan_instance(path_plan)


    # プロパティのゲッター ↓
    @property
    def pathes(self):
        """
        始点から終点までの経路
        
        戻り値
            numpy.ndarray: 始点から終点までの経路
        """
        return self.__rrt.pathes
    # プロパティのゲッター ↑


    # 準備関連の処理 ↓
    def set_strict_pos(self, min_pos, max_pos):
        """
        探索範囲の設定

        パラメータ
            min_pos(numpy.ndarray): 探索の最小範囲
            max_pos(numpy.ndarray): 探索の最大範囲
        """
        self.__rrt.set_strict_pos(min_pos, max_pos)
    # 準備関連の処理 ↑


    # ファイル関連の処理 ↓
    def save(self):
        """
        生成した経路をファイル保存
        """
        self.__rrt.save()
    # ファイル関連の処理 ↑


    # メイン関連の処理 ↓
    def planning(self, start_pos, end_pos, interpolation, interference, joints_limit, weight=None):
        """
        始点から終点までの経路生成の実行

        パラメータ
            start_pos(numpy.ndarray): 経路生成の始点
            end_pos(numpy.ndarray): 経路生成の終点
            interpolation(INTERPOLATION): 補間方法 (関節空間/位置空間)
            interference(PyBulletInteference): 干渉判定クラス
            joins_limits(numpy.ndarray): 関節限界 (0列目：最小値, 1列目：最大値)
            weight(numpy.ndarray): 各次元の重み

        戻り値
            bool: True/False = 経路生成の成功/失敗
        """
        return self.__rrt.planning(start_pos, end_pos, interpolation, interference, joints_limit, weight)
    # メイン関連の処理 ↑


    # 初期化関連の処理 ↓
    def reset(self, path_plan):
        """
        データの初期化
        
        パラメータ
            path_plan(PATHPLAN): 経路生成手法
        """
        if self.__rrt is not None:
            # インスタンス作成済みの場合
            del self.__rrt
            self.__rrt = None

        # 再度インスタンスの作成
        self.__rrt = self.__get_each_path_plan_instance(path_plan)

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

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

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

        return rrt_cls()
    # 初期化関連の処理 ↑

本記事では，経路生成手法として RRT を採用するが，RRT-Connect や RRT* も使えるように，クラスを定義している．

PyBulletのカメラ処理 (pybullet_camera.py)

PyBulletのカメラ処理を定義する pybullet_camera.py について説明する．

pybullet_camera.py

# PyBulletで使用するグリッパーを記載


# ライブラリの読み込み
import pybullet as p    # PyBullet
import numpy as np      # 数値計算ライブラリ


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



class _PyBulletCamera:
    """
    カメラクラス (PyBullet用)


    プロパティ
        __camera_id(): カメラのID番号
        __camera_pos(): カメラ位置
        __view_matrix(): ワールド座標系からカメラ座標系へ変換するための同時変換行列
        __projection_matrix(): カメラ画像を3Dから2Dへ変換するための同時変換行列


    メソッド
        public
            get_pos(): 目標物体の位置を取得

        private
            __get_one_ray_result(): 光線を照射して，目標物体の位置を取得
    """
    # 定数の定義
    _FOV    = 90        # 視野角 ([degree])
    _ASPECT = 1.0       # 縦横比
    _NEAR   = 0.01      # 最近接面
    _FAR    = 10.0      # 最遠面
    _WIDTH  = 320       # 画像の横幅 [pixel]
    _HEIGHT = 240       # 画像の縦幅 [pixel]


    def __init__(self, urdf, base_pos=[0, 0, 5], target_pos=[0, 0, 0], camera_up_vector=[0, 1, 0]):
        """
        コンストラクタ

        パラメータ
            urdf(str): カメラのURDFファイル名
            base_pos(list): カメラ位置
            target_pos(list): カメラが向いている位置
            camera_up_vector(list): カメラの上方向ベクトル
        """
        # カメラを読み込む
        self.__camera_id = p.loadURDF(urdf, basePosition=base_pos, useFixedBase=True)

        # カメラ情報を取得
        camera_infos = p.getLinkState(self.__camera_id, 0)

        # カメラの位置を取得
        self.__camera_pos = camera_infos[0]

        # カメラ位置や方向の指定
        self.__view_matrix = p.computeViewMatrix(
            cameraEyePosition=self.__camera_pos,    # カメラ位置
            cameraTargetPosition=target_pos,        # カメラの目標点
            cameraUpVector=camera_up_vector         # 画面の上側ベクトル
        )

        # カメラの3D情報を2Dへ変換するための情報設定
        self.__projection_matrix = p.computeProjectionMatrixFOV(
            fov=self._FOV,              # 視野角 [degree]
            aspect=self._ASPECT,        # アスペクト比
            nearVal=self._NEAR,         # 最近距離
            farVal=self._FAR            # 最遠距離
        )


    def get_pos(self, target_id):
        """
        目標物の位置を取得

        パラメータ
            target_id(int): カメラで取得したい目標物のID番号 (ID番号はPyBulletのloadURDF()の戻り値)

        戻り値
            numpy.ndarray: 目標物の位置
        """
        # 目標物の位置と目標物を検出できたピクセル数を取得
        pos, ori, n_pixel = self.get_pos_and_pixelnum(target_id)

        return pos, ori


    def get_pos_and_pixelnum(self, target_id):
        """
        目標物の位置と目標物を検出できたピクセル数を取得

        パラメータ
            target_id(int): カメラで取得したい目標物のID番号 (ID番号はPyBulletのloadURDF()の戻り値)

        戻り値
            numpy.ndarray: 目標物の位置
            int: 目標物が検出できたピクセル数
        """
        # カメラ画像の取得
        width, height, rgbImg, depthImg, segImg = p.getCameraImage(
            width=self._WIDTH,
            height=self._HEIGHT,
            viewMatrix=self.__view_matrix,
            projectionMatrix=self.__projection_matrix
        )

        # Numpy型への型変換
        segImg = np.array(segImg)
        # 目標物体が描画されている画像の抽出
        ys, xs = np.where(segImg == target_id)
        if len(xs) == 0 or len(ys) == 0:
            # 目標物体が描画されていないため，エラー発行
            raise ValueError(f"target {target_id} is not visible in camera")

        # 物体の中心ピクセルを取得
        x_center = np.mean(xs)
        y_center = np.mean(ys)
        print(f"x_center = {x_center}")
        print(f"y_center = {y_center}")
        print(f"len(xs) = {len(xs)}")

        # 物体に向かって光線を照射して，位置を取得
        pos = self.__get_ray_result(x_center, y_center, target_id)

        # 固定値の姿勢を取得
        ori = self.__get_orientation()

        return pos, ori, len(xs)


    def __get_orientation(self):
        """
        姿勢の取得

        戻り値
            numpy.ndarray: 目標物の姿勢
        """
        # 定数の姿勢を返す (カメラから姿勢を取得する方法が不明なため)
        orientation = np.array([0.0, np.pi / 2, 0.0])

        return orientation


    def __get_ray_result(self, x_pixel, y_pixel, target_id):
        """
        光線を照射して，目標物体の位置を取得

        パラメータ
            x_pixel(float): 光線のX方向位置 [pixel]
            y_pixel(float): 光線のY方向位置 [pixel]
            target_id(int): カメラで取得したい目標物のID番号 (ID番号はPyBulletのloadURDF()の戻り値)

        戻り値
            numpy.ndarray: 目標物の位置
        """
        # 事前準備
        inv_view_matrix    = np.linalg.inv(np.array(self.__view_matrix).reshape(4, 4))
        inv_project_matrix = np.linalg.inv(np.array(self.__projection_matrix).reshape(4, 4))

        # 画像座標(ピクセル) を 正規化デバイス座標(NDC)へ変換する
        x_ndc = 2 * x_pixel / self._WIDTH  - 1
        y_ndc = 1 - 2 * y_pixel / self._HEIGHT

        # クリップ座標
        clip_coords = np.array([x_ndc, y_ndc, -1.0, 1.0])

        # カメラ
        eye_coords = np.dot(inv_project_matrix, clip_coords)
        eye_coords = np.array([eye_coords[0], eye_coords[1], -1.0, 0.0])

        # 光線の方向
        ray_dir = np.dot(inv_view_matrix, eye_coords)
        ray_dir = ray_dir[:3] / np.linalg.norm(ray_dir[:3])

        # 光線の照射先の位置
        ray_to = self.__camera_pos + ray_dir * self._FAR

        # 光線結果
        # rayTest()の引数は以下の通り
        # 第１引数：光線の照射元の位置
        # 第２引数：光線の照射先の位置
        # rayTest()の返り値は以下の通り
        # 第１要素：物体ID
        # 第２要素：リンクのインデックス番号
        # 第３要素：衝突位置の割合
        # 第４要素：位置
        # 第５要素：物体表面の法線ベクトル
        result = p.rayTest(self.__camera_pos, ray_to)
        print(f"self.__camera_pos = {self.__camera_pos}")
        print(f"ray_to = {ray_to}")

        ray_id = result[0][0]
        if ray_id != target_id:
            # 求めたい物体IDと光線で取得した物体IDが異なると，光線による物体位置が求めたい物体位置とは異なる
            print(f"ray_id and target_id are not matched. ray_id = {ray_id}. target_id = {target_id}")
            raise ValueError(f"ray_id and target_id are not matched. ray_id = {ray_id}. target_id = {target_id}")

        pos = np.array(result[0][3])

        return pos



class _PyBulletMultiCamera:
    """
    複数のカメラクラス (PyBullet用)

    プロパティ
        __cameras(_PyBulletCamera): 複数のカメラ

    メソッド
        public
            get_pos(): 目標物体の位置を取得

        private
            __get_all_cameras_pos(): 全カメラより，目標物の位置を取得する
            __calc_weighted_target_pos(): 重み付き平均による目標物の位置を計算
    """
    # 定数の定義

    # カメラ位置に関する定数
    __RIGHT2LEFT_POS = [ 5,  0, 1]      # 左向き(-X方向)のカメラ位置
    __LEFT2RIGHT_POS = [-5,  0, 1]      # 右向き(+X方向)のカメラ位置
    __FRONT2BACK_POS = [ 0, -5, 1]      # 奥向き(+Y方向)のカメラ位置
    __BACK2FRONT_POS = [ 0,  5, 1]      # 手前向き(-Y方向)のカメラ位置
    __UP2DOWN_POS    = [ 0,  0, 5]      # 下向き(-Z方向)のカメラ位置

    # カメラの上側に関する定数
    __RIGHT2LEFT_UP_VECTOR = [0, 0, 1]  # 左向きカメラの上側ベクトル
    __LEFT2RIGHT_UP_VECTOR = [0, 0, 1]  # 右向きカメラの上側ベクトル
    __FRONT2BACK_UP_VECTOR = [0, 0, 1]  # 奥向きカメラの上側ベクトル
    __BACK2FRONT_UP_VECTOR = [0, 0, 1]  # 手前向きカメラの上側ベクトル
    __UP2DOWN_UP_VECTOR    = [0, 1, 0]  # 下向きカメラの上側ベクトル

    # カメラの向いている位置に関する定数
    __RIGHT2LEFT_TARGET_POS = [0                  , __RIGHT2LEFT_POS[1], __RIGHT2LEFT_POS[2]]   # 左向きカメラが向いている位置
    __LEFT2RIGHT_TARGET_POS = [0                  , __LEFT2RIGHT_POS[1], __LEFT2RIGHT_POS[2]]   # 右向きカメラが向いている位置
    __FRONT2BACK_TARGET_POS = [__FRONT2BACK_POS[0], 0                  , __FRONT2BACK_POS[2]]   # 奥向きカメラが向いている位置
    __BACK2FRONT_TARGET_POS = [__BACK2FRONT_POS[0], 0                  , __BACK2FRONT_POS[2]]   # 手前向きカメラが向いている位置
    __UP2DOWN_TARGET_POS    = [__UP2DOWN_POS[0]   , __UP2DOWN_POS[1]   , 0                  ]   # 手前向きカメラが向いている位置


    def __init__(self, n_cameras):
        """
        コンストラクタ

        パラメータ
            n_cameras(CAMERANUM): カメラ数
        """
        # 今回はカメラ台数を 5 の固定数とする
        if n_cameras != CAMERANUM.MULTI.value:
            raise ValueError(f"n_cameras is abnormal. n_camera is {n_cameras}")

        # 5台のカメラ作成
        self.__cameras = []
        self.__make_multi_cameras()


    def __make_multi_cameras(self):
        """
        複数のカメラを作成
        """
        # 下向きのカメラを作成
        self.__cameras.append(_PyBulletCamera(
                            CAMERAURDF.UP2DOWN.value,
                            self.__UP2DOWN_POS,
                            self.__UP2DOWN_TARGET_POS,
                            self.__UP2DOWN_UP_VECTOR))

        # 左向きのカメラを作成
        self.__cameras.append(_PyBulletCamera(
                            CAMERAURDF.RIGHT2LEFT.value,
                            self.__RIGHT2LEFT_POS,
                            self.__RIGHT2LEFT_TARGET_POS,
                            self.__RIGHT2LEFT_UP_VECTOR))
        # 右向きのカメラを作成
        self.__cameras.append(_PyBulletCamera(
                            CAMERAURDF.LEFT2RIGHT.value,
                            self.__LEFT2RIGHT_POS,
                            self.__LEFT2RIGHT_TARGET_POS,
                            self.__LEFT2RIGHT_UP_VECTOR))

        # 奥向きのカメラを作成
        self.__cameras.append(_PyBulletCamera(
                            CAMERAURDF.BACK2FRONT.value,
                            self.__BACK2FRONT_POS,
                            self.__BACK2FRONT_TARGET_POS,
                            self.__BACK2FRONT_UP_VECTOR))
        # 手前向きのカメラを作成
        self.__cameras.append(_PyBulletCamera(
                            CAMERAURDF.FRONT2BACK.value,
                            self.__FRONT2BACK_POS,
                            self.__FRONT2BACK_TARGET_POS,
                            self.__FRONT2BACK_UP_VECTOR))


    def get_pos(self, target_id):
        """
        目標物の位置を取得

        パラメータ
            target_id(int): カメラで取得したい目標物のID番号 (ID番号はPyBulletのloadURDF()の戻り値)

        戻り値
            numpy.ndarray: 目標物の位置
            numpy.ndarray: 目標物の姿勢
        """
        # 全カメラを使って，目標物の位置を取得する
        target_pos, target_ori = self.__get_all_cameras_pos(target_id)

        return target_pos, target_ori


    def __get_all_cameras_pos(self, target_id):
        """
        全カメラより，目標物の位置を取得する

        パラメータ
            target_id(int): カメラで取得したい目標物のID番号 (ID番号はPyBulletのloadURDF()の戻り値)

        戻り値
            numpy.ndarray: 目標物の位置
            numpy.ndarray: 目標物の姿勢
        """
        # 全カメラから取得した，目標物の位置を保存
        all_cameras_target_pos = []
        # 全カメラから取得した，目標物を検出できたピクセル数を保存
        all_cameras_pixel_num  = []

        # 全カメラ分，ループする
        for idx, camera in enumerate(self.__cameras):

            try:
                # カメラから目標物の位置を取得する
                target_pos, target_ori, n_pixel = camera.get_pos_and_pixelnum(target_id)
                # 目標物の位置を保存
                all_cameras_target_pos.append(target_pos)
                # 目標物を検出できたピクセル数を保存
                all_cameras_pixel_num.append(n_pixel)
                print(f"{idx} camera search target. target is exist.")
            except Exception as e:
                # カメラから目標物の位置を取得できなかった
                print(f"{idx} camera doesn't search target. target is not exist.")

            print("")

        # デバッグ
        print(f"all_cameras_target_pos = {all_cameras_target_pos}")
        print(f"all_cameras_pixel_num  = {all_cameras_pixel_num}")

        if len(all_cameras_target_pos) == 0 or len(all_cameras_pixel_num) == 0:
            # 全カメラで目標物の位置を取得できなかった
            raise ValueError("len(all_cameras_target_pos) == 0 or len(all_cameras_pixel_num) == 0")

        if len(all_cameras_target_pos) != len(all_cameras_pixel_num):
            # データ数が不一致で異常
            raise ValueError(f"len(all_cameras_target_pos) and len(all_cameras_pixel_num) are not matched. len(all_cameras_target_pos) = {len(all_cameras_target_pos)}. len(all_cameras_pixel_num) = {len(all_cameras_pixel_num)}")

        # 重み付き平均による目標物の位置を計算
        target_pos = self.__calc_weighted_target_pos(all_cameras_target_pos, all_cameras_pixel_num)

        return target_pos, target_ori


    def __calc_weighted_target_pos(self, all_cameras_pos, all_cameras_pixels):
        """
        重み付き平均による目標物の位置を計算

        パラメータ
            all_cameras_pos(list): 全カメラで取得した目標物の位置
            all_cameras_pixels(list): 全カメラで取得した目標物のピクセル数

        戻り値
            numpy.ndarray: 目標物の位置
        """
        # 引数の確認
        if len(all_cameras_pos) == 0 or len(all_cameras_pixels) == 0:
            # データが存在しないため，異常
            raise ValueError("len(all_cameras_pos) == 0 or len(all_cameras_pixels) == 0")

        if len(all_cameras_pos) != len(all_cameras_pixels):
            # データ数が不一致で異常
            raise ValueError(f"len(all_cameras_pos) and len(all_cameras_pixels) are not matched. len(all_cameras_pos) = {len(all_cameras_pos)}. len(all_cameras_pixels) = {len(all_cameras_pixels)}")

        # list型からnumpy.ndarray型に型変換 (行列計算による処理速度の向上のために型変換)
        all_cameras_pos_np = np.array(all_cameras_pos)
        all_cameras_pixels_np = np.array(all_cameras_pixels)

        # デバッグ
        print(f"all_cameras_pos_np = {all_cameras_pos_np}")
        print(f"all_cameras_pixels_np = {all_cameras_pixels_np}")

        # 目標物の位置をピクセル数による重み付き平均によって，計算する
        target_pos = np.dot(all_cameras_pixels_np, all_cameras_pos_np) / np.sum(all_cameras_pixels_np)

        # デバッグ
        print(f"target_pos = {target_pos}")

        return target_pos


class PyBulletCameraContoller:
    """
    カメラ制御用クラス (PyBullet用)

    プロパティ
        __camera(): カメラクラス

    メソッド
        get_pos(): 目標物の位置を取得
    """
    # 定数の定義


    def __init__(self, n_cameras):
        """
        コンストラクタ

        パラメータ
            n_cameras(CAMERANUM): カメラ数
        """
        # 引数の確認
        if n_cameras == CAMERANUM.SINGLE.value:     # 1つのカメラ
            # 上から下に向いてる
            self.__camera = _PyBulletCamera(CAMERAURDF.UP2DOWN.value)
        elif n_cameras == CAMERANUM.MULTI.value:    # 複数のカメラ
            self.__camera = _PyBulletMultiCamera(n_cameras)
        else:                                       # 異常な数
            raise ValueError(f"n_cameras is abnormal. n_cameras is {n_cameras}")


    def get_pos(self, target_id):
        """
        目標物の位置を取得

        パラメータ
            target_id(int): カメラで取得したい目標物のID番号 (ID番号はPyBulletのloadURDF()の戻り値)

        戻り値
            numpy.ndarray: 目標物の位置
            numpy.ndarray: 目標物の姿勢
        """
        return self.__camera.get_pos(target_id)

外部クラスから，呼べるように "PyBulletCameraContoller" クラスを作成した．"PyBulletCameraContoller" クラス内で，単一のカメラとするか，複数のカメラを採用するかを設定する．

複数のカメラを設定するクラスとして "_PyBulletMultiCamera" を作成した．本記事では，複数カメラの場合は，数を "5" 固定としている．また，カメラの向きは5方向に分かれるように配置している．

把持物体の位置を取得するために，以下の順番で処理を実装している．
１：カメラから把持物体に関連するピクセルを取得 (yx, xs = np.where(segImg == target_id))
２：ピクセル座標から正規化デバイス座標へ変換
３：正規化デバイス座標から，カメラ座標系へ変換
４：カメラ座標系から，ワールド座標系へ変換
５：カメラから把持物体の方向に向けて，光線を照射 (p.rayTest())
６：光線の照射結果より，把持物体の３次元位置を取得
７：複数カメラより，把持物体の３次元位置を一意に決定

PyBulletのメイン処理 (pybullet_main.py)

PyBulletのメイン処理を定義する pybullet_main.py について説明する．

pybullet_main.py

# PyBulletのメイン処理を記載


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


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


# 自作モジュールの読み込み
from constant import *
from pybullet_rrt import PyBulletRRTController          # 経路生成に関して
from pybullet_environment import PyBulletEnvironment    # 環境に関して
from pybullet_grasp import PyBulletGraspObject          # 把持物体に関して
from pybullet_robot import PyBulletRobotController      # ロボットに関して
from pybullet_interference import PyBulletInteference   # 干渉判定に関して
from pybullet_camera import PyBulletCameraContoller     # カメラに関して



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


    プロパティ
        _robot_id(): ロボットアームのID番号
        _interpolation(str): 探索空間 (直交空間/関節空間)
        _environment(PyBulletEnvironment): 環境クラス
        _rrt(PyBulletRRTController): 経路生成クラス
        _camera(PyBulletCamera)：カメラクラス


    メソッド
        public

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

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


        private

            メイン処理関連
                __path_planning(): 始点から終点までの経路生成
                __post_path_planning(): 経路生成の後処理 (経路生成成功時だけ実装)
    """
    # 定数の定義
    __SIMULATION_SLEEP_TIME = 0.05  # シミュレーションの待機時間 [sec]
    __N_MARGIN_MOVE  = 20           # 経路生成終了後の余白時間 [回]

    # カメラから取得した物体位置のオフセットを定義 ↓
    __CAMERA_OFFSET_2DOF = np.array([ 0.0, 0.0])        # 2軸ロボットアームのカメラオフセット
    __CAMERA_OFFSET_3DOF = np.array([-0.1, 0.0, 0.1])   # 3軸ロボットアームのカメラオフセット
    __CAMERA_OFFSET_3DOF = np.array([ 0.0, 0.0, 0.2])   # 6軸ロボットアームのカメラオフセット
    # カメラから取得した物体位置のオフセットを定義 ↑


    def __init__(self, interpolation, n_robot_joint, environment_urdf, grasp_urdf, n_cameras, hand=False):
        """
        コンストラクタ

        パラメータ
            interpolation(str): 補間方法 (関節空間/位置空間)
            n_robot_joint(int): ロボットアームの関節数(2, 3, 6だけ)
            environment_urdf(str): 環境のファイル名 (urdf)
            grasp_urdf(str): 把持物体のファイル名 (urdf)
            n_cameras(CAMERANUM): カメラの数
            hand(bool): ハンドの装着有無 True/False = 装着/未装着
        """
        # PyBulletの初期化
        p.connect(p.GUI)
        # パスの追加
        p.setAdditionalSearchPath(pybullet_data.getDataPath())
        # シミュレーションの初期化
        p.resetSimulation()
        # 重力の設定 (下(-z軸)方向の加速度)
        p.setGravity(0, 0, -GRABITY_VALUE)

        # ロボットの初期化
        self.__robot = PyBulletRobotController(n_robot_joint, interpolation, hand)

        # 環境の初期化
        self.__environment = PyBulletEnvironment(environment_urdf, self.__robot.n_robot_joint)

        # 把持物体の初期化
        self.__grasp_obj = PyBulletGraspObject(grasp_urdf, self.__robot.n_robot_joint)

        # 干渉判定の初期化
        self.__interference = PyBulletInteference(self.__robot, self.__environment)

        # カメラの初期化
        self.__camera = PyBulletCameraContoller(n_cameras)

        # 経路生成の初期化
        self.__rrt = None


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

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

        # 把持物体の位置を取得
        end_pos = self.__get_grasp_pos_from_camera()
        print(f"start_pos = {start_pos}")
        print(f"end_pos = {end_pos}")

        # 経路生成手法の設定
        self.__rrt = PyBulletRRTController(path_plan)

        # 経路生成の実装
        result = self.__rrt.planning(start_pos, end_pos, self.__robot.interpolation, self.__interference, self.__robot.joints_limit, self.__robot.weight_joint)

        print(f"self.__rrt.planning() = {result}")

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

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

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

        return result


    def __get_grasp_pos_from_direct(self, offset=False):
        """
        把持物体の位置を取得 (直接)

        パラメータ
            offset(bool): オフセットの有無

        戻り値
            numpy.ndarray: 把持物体の位置
        """
        # 把持物体の位置を取得
        if self.__robot.n_robot_joint == DIMENTION_2D:  # 2軸ロボットアームの場合
            pos, _ = self.__grasp_obj.get_grasp_pos(offset=offset, dim2=True)
        else:   # 3軸・6軸ロボットアームの場合
            pos, _ = self.__grasp_obj.get_grasp_pos(offset=offset, dim2=False)

        # Numpy型に型変換
        pos = np.array(pos)

        return pos


    def __get_grasp_pos_from_camera(self, offset=True):
        """
        カメラから把持物体の位置を取得

        パラメータ
            offset(bool): オフセットの有無

        戻り値
            numpy.ndarray: 把持物体の位置
        """
        # カメラから物体位置(3次元)の取得
        pos, ori = self.__camera.get_pos(self.__grasp_obj.grasp_obj_id)
        if self.__robot.n_robot_joint == DIMENTION_2D:      # 2軸ロボットアーム
            # 物体位置を2次元に変換する
            pos = pos[:DIMENTION_2D] + self.__CAMERA_OFFSET_2DOF
            # オフセット
            offset_val = self.__CAMERA_OFFSET_2DOF

        elif self.__robot.n_robot_joint == DIMENTION_3D:    # 3軸ロボットアーム
            # オフセット
            offset_val = self.__CAMERA_OFFSET_3DOF

        else:   # 6軸ロボットアーム
            # オフセット
            offset_val = self.__CAMERA_OFFSET_3DOF

        # オフセット実施確認
        if offset:
            pos += offset_val

        # 姿勢を考慮する
        if not (self.__robot.n_robot_joint == DIMENTION_2D or self.__robot.n_robot_joint == DIMENTION_3D):
            # 6軸ロボットの場合は，姿勢を追加する
            pos = np.hstack((pos, ori))

        # 探索方法に応じた終点に変換
        if self.__robot.interpolation == INTERPOLATION.JOINT.value:
            # 逆運動学により，関節角度を取得
            pos = self.__robot.convert_pos_to_theta(pos, force=True)

        return pos


    def __exec_margin_time(self):
        """
        経路生成後の余白時間の処理
        """
        # 把持物体の位置を取得 (直接)
        # end_pos = self.__get_grasp_pos_from_direct(offset=False)

        # 把持物体の位置をカメラから取得
        end_pos = self.__get_grasp_pos_from_camera(offset=False)

        # 把持物体への位置へ移動
        end_theta = self.__robot.convert_pos_to_theta(end_pos)

        print(f"end_pos = {end_pos}")
        print(f"end_theta = {end_theta}")

        # 経路生成後の余白時間 (即座に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)

        # 把持物体の拘束を解除
        self.__grasp_obj.release_constraint()
        time.sleep(self.__SIMULATION_SLEEP_TIME)

        # 経路生成後の余白時間 (即座に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 __post_path_planning(self, start_pos, end_pos):
        """
        経路生成の後処理 (経路生成成功時だけ実装)

        パラメータ
            start_pos(numpy.ndarray): 経路生成の始点
            end_pos(numpy.ndarray): 経路生成の終点
        """
        # 始点に移動
        theta = self.__robot.convert_pos_to_theta(start_pos)
        self.__robot.set_jump_joint(theta)
        # グリッパーの実行
        self.__robot.run_gripper(open=True)

        # 始点から終点までの経路のログ出力
        print(f"self.__rrt.pathes = {self.__rrt.pathes}")
        for row_idx in range(self.__rrt.pathes.shape[0] - 1):
            now_data  = self.__rrt.pathes[row_idx]
            next_data = self.__rrt.pathes[row_idx + 1]
            diff = next_data - now_data
            distance = np.linalg.norm(diff)
            print(f"distance = {distance}")

        # 始点から終点までの経路を移動
        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 convert_pos_to_theta(self, pos):
        """
        位置から関節角度に変換

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

        戻り値
            thetas(numpy.ndarray): 関節角度
        """
        thetas = self.__robot.convert_pos_to_theta(pos, force=True)

        return thetas
    # 運動学関連 ↑

PyBulletでロボットを動かす

上記にて，ソースコードを説明した．
main.py ファイルを実施することによって，PyBullet上のロボットを動かしていく．
RRTによる経路生成の動画を載せる．

経路生成の動画

RRTによる経路生成の動画を載せる．
直交空間で探索した時の各経路生成の動画を下図に載せる．

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

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

おわりに

本記事では，Pythonを使用して，下記内容を実装しました
・PyBullet で干渉物が存在しない環境下で，複数のカメラによる把持物体の位置を取得し，６軸ロボットアームの経路生成と把持 (経路生成手法としてRRTを採用)

次記事では，下記内容を実装していきます．
・PyBullet で干渉物が存在しない環境下で，複数のカメラによる把持物体の位置を取得し，６軸ロボットアームの経路生成と把持 (経路生成手法としてRRTを採用)