はじめに
PyBullet (Python上で動く物理シミュレータ) を使用して，ロボットアームを可視化して，動かしたい．
前記事では，PyBullet (Python上で動く物理シミュレータ) で干渉物が存在する環境において，グリッパー付き6軸ロボットアームの経路生成および物体把持を実装した．経路生成手法として，RRT-Connectを採用した．RRT-Connectで干渉しない経路を生成できたが，最適な経路の生成はできない．
(https://qiita.com/haruhiro1020/items/c09508eee2761ff9380a)
本記事では，前記事の続きで，最適な経路生成を実装するために，経路生成手法をRRT*を採用する(6軸ロボットアームにグリッパーを追加して，干渉物が存在する環境下にて，物体を把持する)．下図はPyBullet上のグリッパー付き6軸ロボットアームである．赤枠がグリッパー，緑枠が把持したい物体である．把持したい物体は，机の上に置いた．今回は机を干渉物として考えて，机に干渉しないように，物体把持を実装する．また，RRT，RRT-Connect，RRT* による経路生成を比較する．
本記事では，ロボットが自動で把持物体の位置まで干渉しないように移動し，物体を把持するまでを実装する．そのため，把持した物体を他の場所へ移動させることは実装していない．実際に動かしている動画は下図の通りである(RRT* では動画時間が長くなってしまったため，経路探索の中間部分は削除した)．
本記事で実装すること
・干渉物が存在する環境下での経路生成 + 物体把持
・RRT，RRT-Connect，RRT* による経路生成を比較
本記事では実装できないこと (将来実装したい内容)
・カメラによる物体の位置把握 + 干渉物が存在する環境下での経路生成 + 物体把持
動作環境
・macOS Sequoia (バージョン15.5)
・Python3 (3.13.3)
・PyBullet (3.2.5) (物理シミュレータ)
・Numpy (2.3.0) (数値計算用ライブラリ)
PyBullet のインストール方法
PyBullet のインストール方法については前記事にて，説明したため，割愛する．
(前記事 https://qiita.com/haruhiro1020/items/ecc5215234350b663770)
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を作成するのが困難であるため，採用しない．
把持対象物に関して
本記事では，把持対象物を立方体とする．
また，把持までの流れは以下の通りとなる．
１：始点から終点まで，干渉しない経路を探索する
２：干渉しない始点から終点までの経路を移動する
３：物体把持
ここで，重要なことは上記の「１」の時に，ロボットと把持対象物がぶつかってしまう可能性があること．ぶつかってしまうと，把持対象物が指定した位置から大きくずれた位置に動いてしまう．
上記を考慮して，把持対象物は経路探索時に，ぶつかっても動かないように拘束させる．PyBulletでは，拘束させる(動かさないようにする)ことが可能である．
把持対象物のURDF作成
把持対象物(立方体)のUDEFを作成する．立方体を原点$(x, y, z) = (0, 0, 0)$に設定して，Python側で立方体の位置を変更していく．
grasp_object.urdf
<?xml version="1.0"?>
<robot name="grasp_cube">
<link name="cube_link">
<!-- 見た目 -->
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.1 0.1 0.1"/>
</geometry>
</visual>
<!-- 干渉判定 -->
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.1 0.1 0.1"/>
</geometry>
</collision>
<!-- 慣性 -->
<inertial>
<origin xyz="0 0 0" rpy="0 0 0"/>
<mass value="0.1"/>
<inertia ixx="0.01" iyy="0.01" izz="0.01" ixy="0.0" ixz="0.0" iyz="0.0"/>
</inertial>
</link>
</robot>
経路生成の環境に関して
本記事では，先頭で説明した環境より干渉しない経路を生成する．
机を配置して，机の上に把持対象物が存在する環境としている．今回は，机と把持対象物は別URDFファイルを作成して，対応している．
環境(机)のURDF作成
環境(机)のURDFを作成する．机の色は，ネットで「木の色 カラーコード」と検索した値を使用している．机の天板の中心位置を$(x, y, z) = (0, 0, 1)$に設定して，Python側で机の位置を変更していく．
environment_6dof.urdf
<?xml version="1.0" ?>
<robot name="table">
<!-- 色を先に定義 (共通化したいから) ↓ -->
<material name="wood">
<color rgba="0.871 0.722 0.529 1"/>
</material>
<!-- 色を先に定義 (共通化したいから) ↑ -->
<link name="base">
<!-- 見た目，干渉判定，慣性はなし -->
</link>
<!-- テーブルの作成 ↓ -->
<!-- テーブルの天板を動かさないようのジョイント -->
<joint name="joint_table_top" type="fixed">
<parent link="base"/>
<child link="table_top"/>
<origin xyz="0 0 1" rpy="0 0 0"/>
</joint>
<!-- テーブルの天板 -->
<link name="table_top">
<!-- 見た目 -->
<visual>
<origin xyz="0 0 0"/>
<geometry>
<box size="0.5 1 0.01"/>
</geometry>
<material name="wood"/>
</visual>
<!-- 干渉判定 -->
<collision>
<origin xyz="0 0 0"/>
<geometry>
<box size="0.5 1 0.01"/>
</geometry>
</collision>
<!-- 慣性行列 -->
<inertial>
<origin xyz="0 0 0"/>
<mass value="1.0"/>
<inertia ixx="0.083" iyy="0.021" izz="0.104" ixy="0.0" ixz="0.0" iyz="0.0"/>
</inertial>
</link>
<!-- テーブルの天板とテーブルの足を固定化する関節 -->
<joint name="joint_table_leg1" type="fixed">
<parent link="table_top"/>
<child link="table_leg1"/>
<origin xyz="0.2 0.45 0" rpy="0 0 0"/>
</joint>
<!-- テーブルの足1 (+x, +y方向) -->
<link name="table_leg1">
<!-- 見た目 -->
<visual>
<origin xyz="0 0 -0.5"/>
<geometry>
<box size="0.1 0.1 1"/>
</geometry>
<material name="wood"/>
</visual>
<!-- 干渉判定 -->
<collision>
<origin xyz="0 0 -0.5"/>
<geometry>
<box size="0.1 0.1 1"/>
</geometry>
</collision>
<!-- 慣性行列 -->
<inertial>
<origin xyz="0 0 -0.5"/>
<mass value="2.0"/>
<inertia ixx="0.1683" iyy="0.1683" izz="0.0033" ixy="0.0" ixz="0.0" iyz="0.0"/>
</inertial>
</link>
<!-- テーブルの天板とテーブルの足を固定化する関節 -->
<joint name="joint_table_leg2" type="fixed">
<parent link="table_top"/>
<child link="table_leg2"/>
<origin xyz="-0.2 0.45 0" rpy="0 0 0"/>
</joint>
<!-- テーブルの足2 (-x, +y方向) -->
<link name="table_leg2">
<!-- 見た目 -->
<visual>
<origin xyz="0 0 -0.5"/>
<geometry>
<box size="0.1 0.1 1"/>
</geometry>
<material name="wood"/>
</visual>
<!-- 干渉判定 -->
<collision>
<origin xyz="0 0 -0.5"/>
<geometry>
<box size="0.1 0.1 1"/>
</geometry>
</collision>
<!-- 慣性行列 -->
<inertial>
<origin xyz="0 0 -0.5"/>
<mass value="2.0"/>
<inertia ixx="0.1683" iyy="0.1683" izz="0.0033" ixy="0.0" ixz="0.0" iyz="0.0"/>
</inertial>
</link>
<!-- テーブルの天板とテーブルの足を固定化する関節 -->
<joint name="joint_table_leg3" type="fixed">
<parent link="table_top"/>
<child link="table_leg3"/>
<origin xyz="-0.2 -0.45 0" rpy="0 0 0"/>
</joint>
<!-- テーブルの足3 (-x, -y方向) -->
<link name="table_leg3">
<!-- 見た目 -->
<visual>
<origin xyz="0 0 -0.5"/>
<geometry>
<box size="0.1 0.1 1"/>
</geometry>
<material name="wood"/>
</visual>
<!-- 干渉判定 -->
<collision>
<origin xyz="0 0 -0.5"/>
<geometry>
<box size="0.1 0.1 1"/>
</geometry>
</collision>
<!-- 慣性行列 -->
<inertial>
<origin xyz="0 0 -0.5"/>
<mass value="2.0"/>
<inertia ixx="0.1683" iyy="0.1683" izz="0.0033" ixy="0.0" ixz="0.0" iyz="0.0"/>
</inertial>
</link>
<!-- テーブルの天板とテーブルの足を固定化する関節 -->
<joint name="joint_table_leg4" type="fixed">
<parent link="table_top"/>
<child link="table_leg4"/>
<origin xyz="0.2 -0.45 0" rpy="0 0 0"/>
</joint>
<!-- テーブルの足4 (+x, -y方向) -->
<link name="table_leg4">
<!-- 見た目 -->
<visual>
<origin xyz="0 0 -0.5"/>
<geometry>
<box size="0.1 0.1 1"/>
</geometry>
<material name="wood"/>
</visual>
<!-- 干渉判定 -->
<collision>
<origin xyz="0 0 -0.5"/>
<geometry>
<box size="0.1 0.1 1"/>
</geometry>
</collision>
<!-- 慣性行列 -->
<inertial>
<origin xyz="0 0 -0.5"/>
<mass value="2.0"/>
<inertia ixx="0.1683" iyy="0.1683" izz="0.0033" ixy="0.0" ixz="0.0" iyz="0.0"/>
</inertial>
</link>
<!-- テーブルの作成 ↑ -->
</robot>
経路生成手法であるRRT*
本記事で使用する経路生成手法として，RRT*を使用する．
RRT* は以前に記事を作成したため，内容は割愛する．
(RRT* に関する記事 https://qiita.com/haruhiro1020/items/7c5bdbe530711c9d331b)
本記事では，RRT* を使用して，干渉回避できる経路を作成する．前記事では，RRT-Connectを使用していたが，最適な経路の生成できておらず，最適な経路生成を実装したいからRRT* を採用した．
(前記事 https://qiita.com/haruhiro1020/items/c09508eee2761ff9380a)
PyBulletの使用方法
Pythonの物理シミュレータであるPyBulletの使用方法について説明する．
下記リンクのPyBullet公式で調べながら，PyBulletを使用している．
・https://pybullet.org/wordpress/
上記リンクを開くと，下図のようなサイトに飛ぶ．使用方法を調べるときは，下図の赤枠内の「PYBULLET QUICKSTART GUIDE」タグをクリックする．
「PYBULLET QUICKSTART GUIDE」タグをクリックすると，下図のようなドキュメントを見ることができる．基本的には，ドキュメントに記載されている関数の使用方法を見て，ソースコードを作成している．
PyBulletの関数の引数や戻り値をもっと知りたいのでしたら，上記ドキュメントを見た方がわかりやすいです．
全体のソースコード
はじめに，本記事で使用する全体のソースコードについて説明する．
その後，重要な箇所を抜粋して別途解説をしていく．
ソースコードとして，下記の9ファイルを作成する．
・定数の定義 (constant.py)
・全体的なメイン処理 (main.py)
・PyBulletの環境 (pybullet_environment.py)
・PyBulletの把持物体 (pybullet_grasp.py)
・PyBulletのグリッパー (pybullet_gripper.py)
・PyBulletの干渉判定 (pybullet_interference.py)
・PyBulletのメイン処理 (pybullet_main.py)
・PyBulletのロボット (pybullet_robot.py)
・PyBulletの経路生成 (pybullet_rrt.py)
各ファイルの中身を説明する．
定数の定義 (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*でのコスト要素
# RRT-Connectの状態を定義
class RRTCONNECTSTATE(Enum):
"""
RRT-Connectの状態
"""
STREE_RAND = 0 # 始点ツリーにランダム点を追加
STREE_TO_ETREE = auto() # 始点ツリーから終点ツリーへノードを伸ばす
ETREE_RAND = auto() # 終点ツリーにランダム点を追加
ETREE_TO_STREE = auto() # 終点ツリーから始点ツリーへノードを伸ばす
# ツリーの初期ノードの親ノード
INITIAL_NODE_NEAR_NODE = -1 # 初期ノードに親ノードが存在しないから-1
# 補間方法の定義
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" # ハンド(グリッパ)付き
2軸ロボットアーム，3軸ロボットアームのURDFは以前の記事で作成したため，内容は割愛する．
(2軸ロボットアームのURDF：https://qiita.com/haruhiro1020/items/43ddb8b3c4e379ab7983)
(3軸ロボットアームのURDF：https://qiita.com/haruhiro1020/items/d26009d50be190d97368)
全体的なメイン処理 (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"
interpolation = INTERPOLATION.POSITION.value # 直交空間の探索
start_pos = np.array([1.0, -1.0])
elif N_ROBOT_AXIS == DIMENTION_3D: # 3軸ロボットアーム
environment_urdf = "environment_3dof.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"
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"
# Pybulletを使用するインスタンス作成
my_robot = MainPyBulletRobot(interpolation, N_ROBOT_AXIS, environment_urdf=environment_urdf, grasp_urdf=grasp_urdf, hand=HAND_FLG)
# シード値の設定
np.random.seed(CONST_SEED)
if 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()
6軸ロボットアームの初期位置は，RRT* による経路生成が可能となるように，調整した．
2, 3軸ロボットアームの環境URDFファイルは，以下記事で定義した．
(2軸ロボットアームの環境URDFファイル：https://qiita.com/haruhiro1020/items/43ddb8b3c4e379ab7983
3軸ロボットアームの環境URDFファイル：https://qiita.com/haruhiro1020/items/d26009d50be190d97368)
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" を読み込む)
p.loadURDF(self.__PLANE_URDF)
# ロボットアームに応じて，環境のベース位置を変える
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)
@property
def environment_id(self):
"""
__environment_idプロパティのゲッター
"""
return self.__environment_id
ロボットアームに応じた環境を読み込んでいる．また，環境IDを別クラスで使用したいため，publicの関数を用意した．
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)
# 把持対象物に摩擦を付与する
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
ロボットアームに応じた把持物体を読み込んでいる．また，把持物体の拘束条件を別クラスで使用したいため，publicの関数を用意した．
今回は，把持物体を立方体かつ姿勢はワールド座標系と一致するようにURDFを定義しているが，URDFを変更したらクラスも修正する必要がある．
PyBulletのグリッパー (pybullet_gripper.py)
PyBulletのグリッパーを定義する pybullet_gripper.py について説明する．
pybullet_gripper.py
# PyBulletで使用するグリッパーを記載
# ライブラリの読み込み
import pybullet as p # PyBullet
import time # 時間
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)
今回は，パラレルグリッパー(2本の指が並行に動く)だけを採用しているが，他グリッパーを採用したい時は，新規クラスを作成する必要がある．
また，グリッパー開閉時の移動量は把持物体の大きさに応じて変更する必要がある．今回の把持物体は一辺が$0.1 [m]$の立方体である．
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上のロボットを動かす役割を与えた．干渉判定を伴わずに，PyBullet上のロボットを動かす場合は，他クラスの役割にするよう考えた．
PyBulletで使用するロボット (pybullet_robot.py)
PyBulletで使用するロボットを定義する pybullet_robot.py について説明する．
pybullet_robot.py
# PyBulletで使用するロボットを記載
# ライブラリの読み込み
import pybullet as p # PyBullet
import time # 時間
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)}")
# 修正後 ↑
# 修正前 ↓
# if self._hand is None: # ハンドなし
# # 引数の確認
# if len(thetas) != self._DIM_JOINT:
# # 異常
# raise ValueError(f"theta's shape is abnormal. thetas'size is {len(thetas)}")
# else: # ハンドあり
# # 引数の確認
# if len(thetas) != (self._DIM_JOINT + self._N_HAND_JOINT_NOT_FIXED):
# # 異常
# raise ValueError(f"theta's shape is abnormal. thetas'size is {len(thetas)}")
# 修正前 ↑
def set_joint(self, thetas):
"""
関節角度の設定
パラメータ
thetas(numpy.ndarray): 関節角度 [rad]
"""
# 引数の確認
self._chk_thetas_dim(thetas)
for i in range(len(thetas)):
# 関節角度を設定
p.setJointMotorControl2(
bodyUniqueId=self._robot_id, # IDの設定
jointIndex=i, # 関節番号の設定
controlMode=p.POSITION_CONTROL, # 位置制御
targetPosition=thetas[i] # 関節角度
)
def set_jump_joint(self, thetas):
"""
関節角度をジャンプ
パラメータ
thetas(numpy.ndarray): 関節角度 [rad]
"""
# 引数の確認
self._chk_thetas_dim(thetas)
for i in range(thetas.shape[0]):
# 関節角度を設定
p.resetJointState(
bodyUniqueId=self._robot_id, # IDの設定
jointIndex=i, # 関節番号の設定
targetValue=thetas[i] # 関節角度
)
def convert_pos_to_theta(self, pos, force=False):
"""
位置から関節角度に変換
パラメータ
pos(numpy.ndarray): 位置 / 関節角度
force(bool): パラメータposを絶対に位置とみなす
戻り値
numpy.ndarray: 関節角度 [rad]
"""
raise InterruptedError("convert_pos_to_theta() is necessary override.")
def run_gripper(self, open=False, close=False):
"""
グリッパーの実行
パラメータ
open(bool): グリッパーのオープンフラグ
close(bool): グリッパーのクローズフラグ
"""
if self._hand is None:
# ハンド非装着のため，処理終了
return
self._hand.run(open, close)
class _PyBullet2DoFRobot(_PyBulletRobot):
"""
PyBulletの2軸ロボットクラス
メソッド
public
convert_pos_to_theta(): 位置から関節角度に変換
private
__inverse_kinematics(): 逆運動学(位置から関節角度に変換)
"""
# 定数の定義
_Z_VALUE = 0.0 # 位置を3次元変換する時のZ値
_DIM_JOINT = DIMENTION_2D # 関節の次元数
_DIM_POSE = DIMENTION_2D # 位置(姿勢も含む)の次元数
def __init__(self, robot_id, interpolation, hand):
"""
コンストラクタ
パラメータ
robot_id(int): ロボットID (loadURDF()の戻り値)
interpolation(str): 探索方法 (関節空間/位置空間)
hand(bool): ハンド装着の有無 True/False = あり/なし
"""
# 親クラスのコンストラクを実行
super().__init__(robot_id, interpolation, hand)
def __inverse_kinematics(self, pos):
"""
逆運動学(位置から関節角度に変換)
パラメータ
pos(numpy.ndarray): 位置 [m]
戻り値
numpy.ndarray: 関節角度 [rad]
"""
# 引数の確認
self._chk_pos_dim(pos)
# posは2次元データであるため，3次元データへ変換する
# (PyBulletの逆運動学を実装するため)
pos = np.append(pos, self._Z_VALUE)
# エンドエフェクタのリンク要素はベースリンクを除いた要素番号となる
thetas = p.calculateInverseKinematics(self._robot_id, self._n_joints, pos)
thetas = np.array(thetas)
return thetas
def convert_pos_to_theta(self, pos, force=False):
"""
位置から関節角度に変換
パラメータ
pos(numpy.ndarray): 位置 [m] / 関節角度 [rad]
force(bool): パラメータposを絶対に位置とみなす
戻り値
numpy.ndarray: 関節角度 [rad]
"""
if force: # posを位置とみなす
# 逆運動学
thetas = self.__inverse_kinematics(pos)
else: # プロパティ "_interpolation" より決定
if self._interpolation == INTERPOLATION.POSITION.value:
# 逆運動学
thetas = self.__inverse_kinematics(pos)
else:
# pos が関節角度のため，そのまま返す
thetas = np.copy(pos)
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
# 2軸ロボットアームのクラス
robot_cls = _PyBullet3DoFRobot
elif n_robot_joint == DIMENTION_6D:
# 6軸ロボットアーム
if hand: # ハンド装着
robot_urdf = ROBOTURDF.DOF6_HAND.value
else: # ハンド未装着
robot_urdf = ROBOTURDF.DOF6.value
# 2軸ロボットアームのクラス
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)
# ロボットクラスのインスタンス作成
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で使用する，2, 3, 6軸ロボットアームを定義している．また，別クラスから呼ぶロボットを制御するクラスを作成した．
各軸のロボットアームクラスは他ファイルから呼ばずに，PyBulletRobotController クラスを呼ぶようにした．そのため，各軸のロボットアームクラスのメソッドは同じ名前とした．
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.1 # 1回の移動量 [rad] (ロボットの関節空間)
_MOVING_VALUE_POS = 0.2 # 1回の移動量 [m] (ロボットの位置空間)
_STRICT_PLANNING_ROB_JOINT = np.pi / 2 # 探索範囲の制限 [rad] (ロボットの関節空間)
_STRICT_PLANNING_ROB_POS = 1.0 # 探索範囲の制限 [m] (ロボットの位置空間)
_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()
# 初期化関連の処理 ↑
PyBulletで使用する，経路生成手法を定義している．また，別クラスから経路生成手法を制御するクラスを作成した．
各経路生成手法クラスは他ファイルから呼ばずに，PyBulletRRTController クラスを呼ぶようにした．そのため，各経路生成手法クラスのメソッドは同じ名前とした．
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 # 干渉判定に関して
class MainPyBulletRobot:
"""
PyBulletのメインクラス
プロパティ
_robot_id(): ロボットアームのID番号
_environment(PyBulletEnvironment): 環境クラス
_interpolation(str): 探索空間 (直交空間/関節空間)
_rrt(rrt.py内のクラス): 経路生成
メソッド
public
メイン処理関連
run(): 実行 (始点から終点まで，干渉しない経路を生成)
運動学関連
convert_pos_to_theta(): 位置から関節角度に変換 (クラス外で使う用)
protected
メイン処理関連
_set_path_plan(): 経路生成手法の設定
_path_planning(): 始点から終点までの経路生成
_post_path_planning(): 経路生成の後処理 (経路生成成功時だけ実装)
"""
# 定数の定義
__SIMULATION_SLEEP_TIME = 0.05 # シミュレーションの待機時間 [sec]
__N_MARGIN_MOVE = 20 # 経路生成終了後の余白時間 [回]
def __init__(self, interpolation, n_robot_joint, environment_urdf=None, grasp_urdf=None, hand=False):
"""
コンストラクタ
パラメータ
interpolation(str): 補間方法 (関節空間/位置空間)
n_robot_joint(int): ロボットアームの関節数(2, 3, 6だけ)
environment_urdf(str): 環境のファイル名 (urdf)
grasp_object(str): 把持対象物のファイル名 (urdf)
hand(bool): ハンドの装着有無 True/False = 装着/未装着
"""
# PyBulletの初期化
p.connect(p.GUI)
# パスの追加
p.setAdditionalSearchPath(pybullet_data.getDataPath())
# シミュレーションの初期化
p.resetSimulation()
# 重力の設定 (下(-z軸)方向の加速度)
p.setGravity(0, 0, -GRABITY_VALUE)
# ロボットの初期化
self.__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.__rrt = None
# メイン処理 ↓
def run(self, start_pos, path_plan):
"""
実行
始点から終点まで，干渉しない経路を生成
パラメータ
start_pos(numpy.ndarray): 経路生成の始点
path_plan(str): 経路生成手法
戻り値
result(bool): True/False = 経路生成に成功/失敗
"""
# リアルタイムでのシミュレーション
p.setRealTimeSimulation(1)
# 把持対象物の位置を取得
if self.__robot.n_robot_joint == DIMENTION_2D: # 2軸ロボットアーム
end_pos, _ = self.__grasp_obj.get_grasp_pos(dim2=True)
elif self.__robot.n_robot_joint == DIMENTION_3D: # 3軸ロボットアーム
end_pos, _ = self.__grasp_obj.get_grasp_pos()
else: # 6軸ロボットアーム
end_pos, end_ori = self.__grasp_obj.get_grasp_pos()
end_pos += end_ori
# 逆運動学(位置から関節角度へ変換) → 6軸ロボットアームは関節空間だけの対応だから
end_pos = self.__robot.convert_pos_to_theta(end_pos, force=True)
end_pos = np.array(end_pos)
# print(f"start_pos = {start_pos}")
# print(f"end_theta = {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 _exec_margin_time(self):
"""
経路生成後の余白時間の処理
"""
# 把持対象物の位置を取得
end_pos, end_ori = self.__grasp_obj.get_grasp_pos(offset=False)
if self.__robot.n_robot_joint == DIMENTION_2D:
end_pos = np.array(end_pos)[:DIMENTION_2D]
# 把持対象物への位置へ移動
end_theta = self.__robot.convert_pos_to_theta(end_pos)
elif self.__robot.n_robot_joint == DIMENTION_3D:
end_pos = np.array(end_pos)
# 把持対象物への位置へ移動
end_theta = self.__robot.convert_pos_to_theta(end_pos)
else:
print(f"end_pos = {end_pos}")
print(f"end_ori = {end_ori}")
end_pos = np.array(end_pos + end_ori)
# 把持対象物への位置へ移動
end_theta = self.__robot.convert_pos_to_theta(end_pos, force=True)
# 経路生成後の余白時間 (即座に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-Connect，RRT* による経路生成の動画を載せる．
経路生成の動画
RRT，RRT-Connect，RRT* による経路生成の動画を載せる．
関節空間で探索した時の各経路生成の動画を下図に載せる．
動画１：始点から終点まで干渉しない経路を生成するための探索 (RRT)
動画２：始点から終点まで干渉しない経路を生成するための探索 (RRT-Connect)
動画３：始点から終点まで干渉しない経路を生成するための探索 (RRT*)
(RRT* では動画時間が長くなってしまったため，経路探索の中間部分は削除した)．
経路生成までの時間が最短な手法は，RRT-Connectである．逆に最長な手法は，RRT* である．
干渉しない始点から終点までの動画
RRT，RRT-Connect，RRT* による干渉しない始点から終点までの動画を載せる．
各経路生成の動画を下図に載せる．
動画４：探索後の始点から終点までの経路 + 物体把持 (RRT)
動画５：探索後の始点から終点までの経路 + 物体把持 (RRT-Connect)
動画６：探索後の始点から終点までの経路 + 物体把持 (RRT*)
上記から，RRT，RRT-Connect，RRT* による経路生成の違いは下表の通りである．
| 比較項目 | RRT | RRT-Connect | RRT* |
|---|---|---|---|
| 処理時間 | 普通 | 短い | 長い |
| 経路の最適性 | 低い | 普通 | 高い |
私としては，処理時間を考慮したいから，RRT-Connectを使用したいと考えている．
RRT* の処理時間が長い原因は「干渉判定」であると考えている．そのため，「干渉判定」の回数を減らすことができれば，RRT* だけでなく，RRTやRRT-Connectの処理時間も短縮することができる．
干渉判定の回数・処理時間を減らすための方法として，以下を挙げる．
・最初はPyBulletのシミュレーションによるロボットと環境の正確な干渉判定ではなく，python-fclでAABBによる簡単な干渉判定を実装
・AABBで干渉がなければ干渉なしと断定する
・AABBで干渉があればPyBulletシミュレーションを動かして，正確な干渉判定を実装
要するに，絶対に干渉しない時は簡単な処理で判定して，干渉が微妙な時は正確な処理で判定すれば，干渉判定の回数・処理時間を減らすことができると考えている．
おわりに
本記事では，Pythonを使用して，下記内容を実装しました
・PyBullet で干渉物が存在する環境下で，6軸ロボットアームの経路生成と把持 (経路生成手法としてRRT*を採用)
次記事では，下記内容を実装していきます．
・PyBullet で2軸ロボットアームにカメラを追加
・Unity で2軸ロボットアームを可視化して，関節を動かす
・今までの記事をPythonからC++に変換