はじめに
PyBullet (Python上で動く物理シミュレータ) を使用して，ロボットアームを可視化して，動かしたい．
前記事では，PyBullet (Python上で動く物理シミュレータ) で2軸ロボットアームを可視化し，スライダーを用いて手先位置を簡単に変更できるようにした．
(https://qiita.com/haruhiro1020/items/1a564c8b5e0dd7e09024)
本記事では，下図のような干渉物の存在する環境下で，2軸ロボットアームの経路生成を実装する．黒色の直方体が干渉物である．青色の球が初期位置，赤色の球が目標位置である．
下記アニメーションは本記事で実装する内容である．RRTで干渉回避する経路を探索して，干渉しない始点から終点まで動くアニメーションである．
干渉判定に関しては，PyBulletで提供している関数を使用する．
以前までの記事では，干渉判定用ライブラリである python-fcl を使用していたが，PyBullet内で干渉判定の関数があるため，PyBulletに任せる．
本記事で実装すること
・PyBullet で干渉物が存在する環境下での経路生成(経路生成手法としてRRTを採用)
本記事では実装できないこと (将来実装したい内容)
・PyBullet と3,6軸ロボットアームを動かす
・PyBullet にカメラを搭載し，カメラより干渉物の位置・姿勢を把握して，干渉回避する経路生成
・PyBullet にハンドを搭載し，把持対象物を把持する
動作環境
・macOS Sequoia (バージョン15.5)
・Python3 (3.13.3)
・PyBullet (3.2.5) (物理シミュレータ)
・Numpy (2.3.0) (数値計算用ライブラリ)
PyBullet のインストール方法
PyBullet のインストール方法については前記事にて，説明したため，割愛する．
(前記事 https://qiita.com/haruhiro1020/items/ecc5215234350b663770)
2軸ロボットアームの定義
本記事で動かしたい，2軸ロボットアームについて説明する．
本記事では，下図のような2軸ロボットアームを考えている．
$\theta_{1}, \theta_{2}$ は，Z軸方向($X, Y$軸に直交する手前方向)へ回転する．
上図のパラメータ $l_{1}, l_{2}, m_{1}, m_{2}$ の値は下表として，考える(値は適当である)．
| パラメータ名 | 概要 | 値 |
|---|---|---|
| $l_{1}$ | リンク$1$の長さ | 1.0 [m] |
| $l_{2}$ | リンク$2$の長さ | 1.0 [m] |
| $m_{1}$ | リンク$1$の質量 | 1.0 [kg] |
| $m_{2}$ | リンク$2$の質量 | 1.0 [kg] |
PyBulletで，上図のロボットを使用するために，URDF (Unified Robot Description Format)を作成する必要がある．
2軸ロボットアームのURDF作成
PyBulletで，2軸ロボットアームを使用するためのURDF作成に関しては，前記事にて説明したため，割愛する．また，URDFの中身も前記事と同じであるため，割愛する．
(前記事 https://qiita.com/haruhiro1020/items/ecc5215234350b663770)
干渉物が存在する環境のURDF作成
本記事では，干渉物が存在する環境をURDFにて，作成する．
環境はロボットではないが，ロボットと見立ててURDFを作成する．
本記事では，URDFのソースコードを記載するが，URDF内のタグについての説明はしない．URDF内のタグについては前記事を参照ください．
(前記事 https://qiita.com/haruhiro1020/items/ecc5215234350b663770)
environment.urdf
<?xml version="1.0" ?>
<robot name="two_boxes_obstacle">
<link name="base">
<!-- 見た目，干渉判定，慣性はなし -->
</link>
<!-- 直方体を動かさないようのジョイント -->
<joint name="joint_box1" type="fixed">
<parent link="base"/>
<child link="box1"/>
<origin xyz="0 0 0" rpy="0 0 0"/>
</joint>
<!-- 直方体1 -->
<link name="box1">
<!-- 見た目 -->
<visual>
<origin xyz="1.8 0.5 0.0"/>
<geometry>
<box size="1.0 0.1 0.1"/>
</geometry>
<material name="black">
<color rgba="0 0 0 0.5"/>
</material>
</visual>
<!-- 干渉判定 -->
<collision>
<origin xyz="1.8 0.5 0.0"/>
<geometry>
<box size="1.0 0.1 0.1"/>
</geometry>
</collision>
<!-- 慣性行列 -->
<inertial>
<mass value="1.0" />
<inertia ixx="0.0017" iyy="0.0842" izz="0.0842" ixy="0.0" ixz="0.0" iyz="0.0"/>
</inertial>
</link>
<!-- 直方体を動かさないようのジョイント -->
<joint name="joint_box2" type="fixed">
<parent link="base"/>
<child link="box2"/>
<origin xyz="0 0 0" rpy="0 0 0"/>
</joint>
<!-- 直方体2 -->
<link name="box2">
<!-- 見た目 -->
<visual>
<origin xyz="0.0 -0.5 0.0"/>
<geometry>
<box size="1.0 0.1 0.1"/>
</geometry>
<material name="black">
<color rgba="0 0 0 0.5"/>
</material>
</visual>
<!-- 干渉判定 -->
<collision>
<origin xyz="0.0 -0.5 0.0"/>
<geometry>
<box size="1.0 0.1 0.1"/>
</geometry>
</collision>
<!-- 慣性行列 -->
<inertial>
<mass value="1.0" />
<inertia ixx="0.0017" iyy="0.0842" izz="0.0842" ixy="0.0" ixz="0.0" iyz="0.0"/>
</inertial>
</link>
<!-- 球を動かさないようのジョイント -->
<joint name="joint_sphere1" type="fixed">
<parent link="base"/>
<child link="sphere1"/>
<origin xyz="0 0 0" rpy="0 0 0"/>
</joint>
<!-- 球1 -->
<link name="sphere1">
<!-- 見た目 -->
<visual>
<origin xyz="1.0 -1.0 0.0"/>
<geometry>
<sphere radius="0.2"/>
</geometry>
<material name="blue">
<color rgba="0 0 1.0 0.5"/>
</material>
</visual>
<!-- 干渉判定，慣性はなし -->
</link>
<!-- 球を動かさないようのジョイント -->
<joint name="joint_sphere2" type="fixed">
<parent link="base"/>
<child link="sphere2"/>
<origin xyz="0 0 0" rpy="0 0 0"/>
</joint>
<!-- 球2 -->
<link name="sphere2">
<!-- 見た目 -->
<visual>
<origin xyz="1.0 1.0 0.0"/>
<geometry>
<sphere radius="0.2"/>
</geometry>
<material name="red">
<color rgba="1.0 0 0 0.5"/>
</material>
</visual>
<!-- 干渉判定，慣性はなし -->
</link>
</robot>
始点や終点を表示している球は，干渉判定の対象とはせずに，見た目だけを存在させている．球を表示しないと，始点と終点の位置がわからなくなるので，デバッグ用として表示させている．
経路生成手法であるRRT
本記事で使用する経路生成手法として，RRTを使用する．
RRTは以前に記事を作成したため，内容は割愛する．
(RRTに関する記事 https://qiita.com/haruhiro1020/items/6eaae645bd638c2b8897)
本記事では，RRTを使用して，干渉回避できる経路を作成する．
PyBulletの使用方法
Pythonの物理シミュレータであるPyBulletの使用方法について説明する．
下記リンクのPyBullet公式で調べながら，PyBulletを使用している．
・https://pybullet.org/wordpress/
上記リンクを開くと，下図のようなサイトに飛ぶ．使用方法を調べるときは，下図の赤枠内の「PYBULLET QUICKSTART GUIDE」タグをクリックする．
「PYBULLET QUICKSTART GUIDE」タグをクリックすると，下図のようなドキュメントを見ることができる．基本的には，ドキュメントに記載されている関数の使用方法を見て，ソースコードを作成している．
PyBulletの関数の引数や戻り値をもっと知りたいのでしたら，上記ドキュメントを見た方がわかりやすいです．
全体のソースコード
はじめに，本記事で使用する全体のソースコードについて説明する．
その後，重要な箇所を抜粋して別途解説をしていく．
ソースコードとして，下記の4ファイルを作成する．
・定数の定義 (constant.py)
・全体的なメイン処理 (main.py)
・PyBulletでロボットを動かす (pybullet_robot.py)
・経路生成手法であるRRT (pybullet_rrt.py)
各ファイルの中身を説明する．
定数の定義 (constant.py)
定数を定義する constant.py について説明する．
本記事では，2軸ロボットアームの説明だが，3軸や6軸ロボットアームにも拡張していく予定である．また，経路生成手法として，RRTを採用しているが，他手法にも拡張していく予定である．
constant.py
# 複数ファイルで使用する定数の定義
from enum import Enum
# 次元数を定義
DIMENTION_NONE = -1 # 未定義
DIMENTION_2D = 2 # 2次元
# 重力
GRABITY_VALUE = 9.81
# シード値の最小値と最大値
MIN_SEED = 0
MAX_SEED = 2 ** 32 - 1 # 4バイト (uint) の最大値
# 0割を防ぐための定数
EPSILON = 1e-6
# ノードの最短ノード要素とコスト要素を定義
# RRT
RRT_NEAR_NODE_IDX = -1 # RRTでの最短ノード要素
# ツリーの初期ノードの親ノード
INITIAL_NODE_NEAR_NODE = -1 # 初期ノードに親ノードが存在しないから-1
# 補間方法の定義
class INTERPOLATION(Enum):
"""
補間方法
"""
NONE = "none" # 未定義
JOINT = "joint" # 関節補間
POSITION = "pos" # 位置補間
# RRTによる経路生成手法の定義
class PATHPLAN(Enum):
"""
経路生成手法
"""
RRT = "rrt" # RRTによる経路生成
全体的なメイン処理 (main.py)
全体的なメイン処理を定義する main.py について説明する．
後ほど説明する pybullet_robot.py 内のクラスを実装するのがメインな処理である．
RRTでの探索空間を関節空間や直交空間を選択できるようにしている．
main.py
# Pybullet (Pythonでの3次元物理シミュレータ) による2軸ロボットアームの可視化
# 標準ライブラリの読み込み
import numpy as np
# 自作モジュールの読み込み
from pybullet_robot import MainPyBulletRobot
from constant import *
CONST_SEED = 1 # シード値 (常に同じ結果としたいから)
def main():
"""
メイン処理
"""
# ロボットが保存されている URDF ファイル名
robot_urdf = "robot_2dof.urdf"
# 寛容が保存されている URDF ファイル名
environment_urdf = "environment.urdf"
# 経路生成手法
path_plan = PATHPLAN.RRT.value
# 探索空間を指定
# interpolation = INTERPOLATION.JOINT.value # 関節空間の探索
interpolation = INTERPOLATION.POSITION.value # 直交空間の探索
# Pybulletを使用するインスタンス作成
my_robot = MainPyBulletRobot(robot_urdf, environment_urdf, interpolation)
# 初期位置・終点位置
start_pos = np.array([1.0, -1.0])
end_pos = np.array([1.0, 1.0])
# シード値の設定
np.random.seed(CONST_SEED)
if interpolation == INTERPOLATION.JOINT.value:
# 関節空間の探索時は位置を関節角度に変換
start_theta = my_robot.convert_pos_to_theta(start_pos)
end_theta = my_robot.convert_pos_to_theta(end_pos)
print(f"start_theta = {start_theta}")
print(f"end_theta = {end_theta}")
result = my_robot.run(start_theta, end_theta, path_plan)
else:
result = my_robot.run(start_pos, end_pos, path_plan)
print(f"result = {result}")
if __name__ == "__main__":
# 本ファイルがメインで呼ばれた時の処理
main()
PyBulletでロボットを動かす (pybullet_robot.py)
PyBulletでロボットを動かす処理を定義する pybullet_robot.py について説明する．
経路生成手法である RRT を使用して，シミュレータ上のロボットを動かして，干渉判定を実装している．
pybullet_robot.py
# PyBulletで使用するロボットを記載
# ライブラリの読み込み
import pybullet as p # PyBullet
import pybullet_data # PyBulletで使用するデータ
import time # 時間
import numpy as np # 数値計算ライブラリ
# サードパーティの読み込み
# 自作モジュールの読み込み
from constant import *
from pybullet_rrt import RRTPyBullet
class MainPyBulletRobot:
"""
PyBulletのメインクラス
プロパティ
_robot_id(): ロボットアームのID番号
_n_joints(int): ロボットアームの関節数
_environment_id(): 県境のID番号
_interpolation(str): 探索空間 (直交空間/関節空間)
_rrt(rrt.py内のクラス): 経路生成
メソッド
public
メイン処理関連
run(): 実行 (始点から終点まで，干渉しない経路を生成)
運動学関連
convert_pos_to_theta(): 位置から関節角度に変換 (クラス外で使う用)
protected
事前準備関連
_init_robot(): ロボットの初期化
_init_environment(): 環境の初期化
メイン処理関連
_set_path_plan(): 経路生成手法の設定
_path_planning(): 始点から終点までの経路生成
_post_path_planning(): 経路生成の後処理 (経路生成成功時だけ実装)
_set_jump_joint(): 関節角度にジャンプ
_set_joint(): 関節角度の設定 (現在位置から移動)
運動学関連
_convert_pos_to_theta(): 位置から関節角度に変換 (クラス内で使う用)
干渉判定処理
_is_line_interference(): 2点間の干渉判定
_is_interference_start_end_pos(): 始点と終点が干渉判定していないかの確認
_is_interference_pos(): 位置にジャンプして干渉判定
"""
# 定数の定義
_PLANE_URDF = "plane.urdf" # 地面に関する urdf ファイル
_IDX_MIN_JOINT = 8 # 関節の最小値が保存されている要素番号
_IDX_MAX_JOINT = 9 # 関節の最大値が保存されている要素番号
_SIMULATION_SLEEP_TIME = 0.05 # シミュレーションの待機時間 [sec]
_INTERFERENCE_MARGIN = 0.1 # 干渉判定のマージン [m]
_PATH_PLAN_TIME = 100 # 経路生成の最大時間 [sec]
_N_MARGIN_MOVE = 100 # 経路生成終了後の余白時間 [回]
def __init__(self, robot_urdf, environment_urdf, interpolation):
"""
コンストラクタ
パラメータ
robot_urdf(str): ロボットアームのファイル名 (urdf)
environment_urdf(str): 環境のファイル名 (urdf)
interpolation(str): 補間方法 (関節空間/位置空間)
"""
# PyBulletの初期化
p.connect(p.GUI)
# パスの追加
p.setAdditionalSearchPath(pybullet_data.getDataPath())
# シミュレーションの初期化
p.resetSimulation()
# 重力の設定 (下(-z軸)方向の加速度)
p.setGravity(0, 0, -GRABITY_VALUE)
# ロボットの初期化
self._init_robot(robot_urdf, interpolation)
# 環境の初期化
self._init_environment(environment_urdf)
self._rrt = None
# 事前準備メソッド ↓
def _init_robot(self, robot_urdf, interpolation):
"""
ロボットの初期化
パラメータ
robot_urdf(str): ロボットアームのファイル名 (urdf)
interpolation(str): 探索方法 (関節空間/位置空間)
"""
# 引数の確認
if not (interpolation == INTERPOLATION.JOINT.value or interpolation == INTERPOLATION.POSITION.value):
# 異常
raise ValueError(f"interpolation is abnormal. interpolation is {interpolation}")
# プロパティの更新
self._interpolation = interpolation
# ロボットを読み込む．ベースリンクの原点は (x, y, z) = (0, 0, 0) として，ベースリンクは地面に固定
self._robot_id = p.loadURDF(robot_urdf, basePosition=[0, 0, 0], useFixedBase=True)
# urdf よりロボットの関節数を取得 (エンドエフェクタ用の仮想関節は不要なため -1)
self._n_joints = p.getNumJoints(self._robot_id) - 1
# ロボットの関節数に応じて，robot.py内のクラスを決定
if self._n_joints != DIMENTION_2D:
# 2軸ロボットアーム以外
raise ValueError(f"self._n_joints is abnormal. {self._n_joints} is abnormal.")
def _init_environment(self, environment_urdf):
"""
環境の初期化
パラメータ
environment_urdf(str): 環境が保存されているファイル名
"""
# 地面を読み込む (pybulletが提供している "plane.urdf" を読み込む)
p.loadURDF(self._PLANE_URDF)
# 環境を読み込む
self._environment_id = p.loadURDF(environment_urdf, basePosition=[0, 0, 0], useFixedBase=True)
# 事前準備メソッド ↑
# メイン処理 ↓
def run(self, start_pos, end_pos, path_plan):
"""
実行
始点から終点まで，干渉しない経路を生成
パラメータ
start_pos(numpy.ndarray): 経路生成の始点
end_pos(numpy.ndarray): 経路生成の終点
path_plan(str): 経路生成手法
戻り値
result(bool): True/False = 経路生成に成功/失敗
"""
# リアルタイムでのシミュレーション
p.setRealTimeSimulation(1)
# 始点と終点で干渉していないかの確認
self._is_interference_start_end_pos(start_pos, end_pos)
# 経路生成手法の設定
self._set_path_plan(path_plan)
# 経路生成の実装
result = self._path_planning(start_pos, end_pos)
if result:
# 経路生成に成功
self._post_path_planning(start_pos, end_pos)
# 経路生成後の余白時間 (即座にPyBulletが終了するのを防ぐための余白時間)
for _ in range(self._N_MARGIN_MOVE):
# 終点に移動
# PyBulletで関節角度を与えても，与えた関節角度ピッタリにはならず，数値誤差が発生するから，終点へ移動する処理を実施する
end_theta = self._convert_pos_to_theta(end_pos)
self._set_joint(end_theta)
# 待機時間
time.sleep(self._SIMULATION_SLEEP_TIME)
return result
def _set_path_plan(self, path_plan):
"""
経路生成手法の設定
パラメータ
path_plan(str): 経路生成手法名
"""
if path_plan == PATHPLAN.RRT.value:
# RRT
self._rrt = RRTPyBullet()
else:
# 異常
raise ValueError(f"path_plan is abnormal. path_plan is {path_plan}")
def _path_planning(self, start_pos, end_pos):
"""
始点から終点までの経路生成
パラメータ
start_pos (_type_): _description_
end_pos (_type_): _description_
戻り値
result(bool): True/False = 経路生成に成功/失敗
"""
result = False
# 経路生成の準備
self._rrt.preparation(start_pos, end_pos, self._interpolation)
start_time = time.time()
# 始点から終点までの経路が生成するまでループ
while True:
now_time = time.time()
if (now_time - start_time) >= self._PATH_PLAN_TIME:
# タイムアウト
break
# 経路生成を1度実行
new_node_pos, near_node_pos, near_node = self._rrt.expand_once(end_pos)
if self._is_line_interference(new_node_pos, near_node_pos):
# 干渉あり
continue
# 干渉なしだから，ノード追加 + 経路生成の完了確認
if not self._rrt.add_node_and_chk_goal(end_pos, new_node_pos, near_node):
# 終点までの近傍ではない
continue
# 新規ノードと始点までの干渉確認
if not self._is_line_interference(new_node_pos, end_pos):
# 始点から終点までの経路生成に成功
result = True
break
return result
def _post_path_planning(self, start_pos, end_pos):
"""
経路生成の後処理 (経路生成成功時だけ実装)
パラメータ
start_pos(numpy.ndarray): 経路生成の始点
end_pos(numpy.ndarray): 経路生成の終点
"""
# 経路生成の終了処理
self._rrt.fin_planning(start_pos, end_pos)
# 始点に移動
theta = self._convert_pos_to_theta(start_pos)
self._set_jump_joint(theta)
# print(f"self._rrt.pathes = {self._rrt.pathes}")
# 始点から終点までの経路を移動
for row_idx in range(self._rrt.pathes.shape[0]):
next_theta = self._convert_pos_to_theta(self._rrt.pathes[row_idx])
self._set_joint(next_theta)
# 待機時間
time.sleep(self._SIMULATION_SLEEP_TIME)
def _set_jump_joint(self, thetas):
"""
関節角度をジャンプ
パラメータ
thetas(numpy.ndarray): 関節角度
"""
for i in range(len(thetas)):
# 関節角度を設定
p.resetJointState(
bodyUniqueId=self._robot_id, # IDの設定
jointIndex=i, # 関節番号の設定
targetValue=thetas[i] # 関節角度
)
def _set_joint(self, thetas):
"""
関節角度の設定 (現在位置から移動)
パラメータ
thetas(numpy.ndarray): 設定したい関節角度
"""
for i in range(len(thetas)):
# 関節角度を設定
p.setJointMotorControl2(
bodyUniqueId=self._robot_id, # IDの設定
jointIndex=i, # 関節番号の設定
controlMode=p.POSITION_CONTROL, # 位置制御
targetPosition=thetas[i] # 関節角度
)
# メイン処理 ↑
# 干渉判定処理 ↓
def _is_line_interference(self, pos1, pos2):
"""
2点間の干渉判定
パラメータ
pos1(numpy.ndarray): 位置1
pos2(numpy.ndarray): 位置2
戻り値
is_interference(bool): True/False = 干渉あり/干渉なし
"""
is_interference = True
# 2点の干渉判定
if self._is_interference_pos(pos2):
return is_interference
if self._is_interference_pos(pos1):
return is_interference
# pos1からpos2へ移動
theta = self._convert_pos_to_theta(pos2)
self._set_joint(theta)
# 待機時間
time.sleep(self._SIMULATION_SLEEP_TIME)
# ロボットと干渉物との干渉判定
close_points = p.getClosestPoints(self._robot_id, self._environment_id, self._INTERFERENCE_MARGIN)
if len(close_points) == 0: # 干渉なし
is_interference = False
return is_interference
def _is_interference_start_end_pos(self, start_pos, end_pos):
"""
始点と終点が干渉判定していないかの確認
パラメータ
start_pos(numpy.ndarray): 経路生成の始点 (直交空間/関節空間)
end_pos(numpy.ndarray): 経路生成の終点 (直交空間/関節空間)
"""
# 始点の干渉判定
if self._is_interference_pos(start_pos):
raise ValueError("start_pos is interference. change start_pos.")
# 終点の干渉判定
if self._is_interference_pos(end_pos):
raise ValueError("end_pos is interference. change end_pos.")
def _is_interference_pos(self, pos):
"""
位置にジャンプして干渉判定
パラメータ
pos(numpy.ndarray): 位置/関節
戻り値
is_interference(bool): True/False = 干渉あり/干渉なし
"""
is_interference = True
# 位置から関節角度に変換
theta = self._convert_pos_to_theta(pos)
print(f"theta = {theta}")
# 位置にジャンプ
self._set_jump_joint(theta)
# 待機時間
time.sleep(self._SIMULATION_SLEEP_TIME)
# ロボットと干渉物との干渉判定
close_points = p.getClosestPoints(self._robot_id, self._environment_id, self._INTERFERENCE_MARGIN)
if len(close_points) == 0: # 干渉なし
is_interference = False
return is_interference
# 干渉判定処理 ↑
# 運動学関連 ↓
def convert_pos_to_theta(self, pos):
"""
位置から関節角度に変換 (クラス外で使う用)
パラメータ
pos(numpy.ndarray): 位置 / 関節角度
戻り値
thetas(numpy.ndarray): 関節角度
"""
if pos.shape[0] == DIMENTION_2D:
pos = np.append(pos, 0.0)
# エンドエフェクタのリンク要素はベースリンクを除いた要素番号となる
thetas = p.calculateInverseKinematics(self._robot_id, self._n_joints, pos)
thetas = np.array(thetas)
return thetas
def _convert_pos_to_theta(self, pos):
"""
位置から関節角度に変換 (クラス内で使う用)
パラメータ
pos(numpy.ndarray): 位置 / 関節角度
戻り値
thetas(numpy.ndarray): 関節角度
"""
if self._interpolation == INTERPOLATION.POSITION.value:
# 逆運動学により，関節角度を返す
# 逆運動学には，(x, y, z)の3次元データが必須なため，2次元の場合はzのデータも増やす
if pos.shape[0] == DIMENTION_2D:
pos = np.append(pos, 0.0)
# エンドエフェクタのリンク要素はベースリンクを除いた要素番号となる
thetas = p.calculateInverseKinematics(self._robot_id, self._n_joints, pos)
thetas = np.array(thetas)
else:
# pos が関節角度のため，そのまま返す
thetas = np.copy(pos)
return thetas
# 運動学関連 ↑
次に，上ソースコードより重要な箇所を抜粋して，説明していく．
上ソースコードの大方は前記事にて説明しているため，説明した内容は割愛する．
(前記事 https://qiita.com/haruhiro1020/items/ecc5215234350b663770)
環境の初期化 (_init_environment)
環境の初期化の中身を説明する．
# 環境を読み込む
self._environment_id = p.loadURDF(environment_urdf, basePosition=[0, 0, 0], useFixedBase=True)
干渉物を定義した環境をURDFとしてシミュレータ上に出力するための処理を追加した．
関節角度をジャンプ (_set_jump_joint)
関節角度をジャンプの中身を説明する．
RRTを使用する際には，現在角度からツリー内のノードへ移動させる必要がある．
その際に，現在角度からノードへ移動させると干渉する可能性が高くなる．本来は，現在角度とノードが繋がっていないため，干渉してしまうとRRTの挙動とは異なる結果となる．
上記を防ぐために，関節角度へジャンプする処理を追加した．
for i in range(len(thetas)):
# 関節角度を設定
p.resetJointState(
bodyUniqueId=self._robot_id, # IDの設定
jointIndex=i, # 関節番号の設定
targetValue=thetas[i] # 関節角度
)
関節角度をジャンプさせるには，p.resetJointState()を使用する．p.resetJointState()の引数は公式ドキュメントを抜粋すると，下図の通りである．
要するに，関節角度をジャンプさせるには，1関節ずつしかできないということである．
位置にジャンプして干渉判定 (_is_interference_pos)
位置にジャンプして干渉判定の中身を説明する．
ジャンプする理由は _set_jump_joint と同じ理由である．
PyBullet上で干渉判定する処理を実装している．
# ロボットと干渉物との干渉判定
close_points = p.getClosestPoints(self._robot_id, self._environment_id, self._INTERFERENCE_MARGIN)
if len(close_points) == 0: # 干渉なし
is_interference = False
PyBulletで距離マージン付きの干渉判定を実施するには，p.getClosestPoints()関数を使用する．p.getClosestPoints()関数の引数と戻り値は公式ドキュメントを抜粋すると，下図の通りとなる．
干渉物との最短点を取得する関数である．引数のdistanceに干渉判定の距離マージンを入れることで，distance以下の距離となる点を戻り値として取得する．
要するに，distance以下の距離となる点が存在しない場合は，戻り値が空となるから干渉なしと判定することができる．
始点から終点までの経路生成 (_path_planning)
始点から終点までの経路生成の中身を説明する．
経路生成手法であるRRTを1回実装して，PyBullet側で干渉判定を実施する．
干渉なしなら，RRTのツリーを更新する．
while True:
...
# 経路生成を1度実行
new_node_pos, near_node_pos, near_node = self._rrt.expand_once(end_pos)
if self._is_line_interference(new_node_pos, near_node_pos):
# 干渉あり
continue
# 干渉なしだから，ノード追加 + 経路生成の完了確認
if not self._rrt.add_node_and_chk_goal(end_pos, new_node_pos, near_node):
# 終点までの近傍ではない
continue
# 新規ノードと始点までの干渉確認
if not self._is_line_interference(new_node_pos, end_pos):
# 始点から終点までの経路生成に成功
result = True
break
RRTを1回実装して，新規ノードに関する情報を取得する．新規ノードと親ノード間で干渉ありなら，ループの先頭に戻る．干渉なしなら，RRTのツリーにノードを追加する．
RRT側で始点から終点までの経路生成に終了したら，最終確認として，新規ノードと終点までの干渉判定を実施する．干渉なしなら，経路生成の成功とする．
経路生成の後処理 (経路生成成功時だけ実装) (_post_path_planning)
経路生成の後処理 (経路生成成功時だけ実装) の中身を説明する．
RRTのツリーは，始点から終点までの経路以外のノードを保持している．
本関数で，始点から終点までの経路を作成する後処理と，実際の経路をシミュレータ上のロボットに与える．
# 経路生成の終了処理
self._rrt.fin_planning(start_pos, end_pos)
# 始点から終点までの経路を移動
for row_idx in range(self._rrt.pathes.shape[0]):
next_theta = self._convert_pos_to_theta(self._rrt.pathes[row_idx])
self._set_joint(next_theta)
# 待機時間
time.sleep(self._SIMULATION_SLEEP_TIME)
self._rrt.fin_planning() によって，RRTのツリーから，始点と終点までの経路を計算する．
for内で，始点から終点までの経路をシミュレータ上のロボットに与えて，ロボットを動かす．
実行(始点から終点まで，干渉しない経路を生成) (run)
実行(始点から終点まで，干渉しない経路を生成)の中身を説明する．
今回，干渉判定はPyBullet側で実装したいため，RRT内では干渉判定しない．そのため，RRT単体では経路生成が実施できず，PyBulletを組み合わせる必要がある．
# 始点と終点で干渉していないかの確認
self._is_interference_start_end_pos(start_pos, end_pos)
そもそも，始点と終点で干渉していたら，経路は生成できないため，あらかじめ干渉判定を実施する．
# 経路生成後の余白時間 (即座にPyBulletが終了するのを防ぐための余白時間)
for _ in range(self._N_MARGIN_MOVE):
# 終点に移動
# PyBulletで関節角度を与えても，与えた関節角度ピッタリにはならず，数値誤差が発生するから，終点へ移動する処理を実施する
end_theta = self._convert_pos_to_theta(end_pos)
self._set_joint(end_theta)
# 待機時間
time.sleep(self._SIMULATION_SLEEP_TIME)
コメントにも記載してあるが，余白時間を用意しないと，PyBulletが即座に終了してしまう．また，関節角度を与えても，与えた関節角度にピッタリと移動するわけではないため，終点へ移動する処理も入れている．
経路生成手法であるRRT (pybullet_rrt.py)
経路生成手法であるRRTの処理を定義する pybullet_rrt.py について説明する．
RRT内ではツリーの作成を実施する．干渉判定はPyBulletで実施するため，RRT内には干渉判定を実装しない．
pybullet_rrt.py
# 経路生成手法であるRRT (Rapidly-exploring Random Tree) の実装 (PyBullet用)
# ライブラリの読み込み
import numpy as np
import os
# 自作モジュールの読み込み
from constant import * # 定数
class Tree:
"""
ツリークラス
プロパティ
_nodes(numpy.ndarray): ノード
_near_node_idx(int): _nodes内の最短ノードを保存している列番号
メソッド
public
nodes(): _nodesプロパティのゲッター
reset(): データの初期化
add_node(): ノードの追加
get_near_node(): 最短距離のノードを取得
get_near_node_list(): ノードと近傍ノードをリストで取得
protected
_chk_node_exist(): ノードが存在するかの確認
"""
# 定数の定義
def __init__(self, near_node_idx):
"""
コンストラクタ
"""
# プロパティの初期化
self._nodes = []
self._near_node_idx = near_node_idx
@property
def nodes(self):
"""
_nodesプロパティのゲッター
"""
return self._nodes
def reset(self):
"""
データの初期化
"""
if len(self._nodes) != 0:
# 何かしらのデータが保存
del self._nodes
self._nodes = []
def add_node(self, node):
"""
ノードの追加
パラメータ
node(numpy.ndarray): ノード
"""
if len(self._nodes) == 0: # 初回だけ実行
self._nodes = node
else:
self._nodes = np.append(self._nodes, node, axis=0)
def _chk_node_exist(self):
"""
ノードが存在するかの確認
"""
if len(self._nodes) == 0:
# 存在しない
raise ValueError("self._nodes is not exist")
def get_near_node(self, pos):
"""
最短距離のノードを取得
パラメータ
pos(numpy.ndarray): 位置
戻り値
min_dist_idx(int): 最短距離のノード番号
"""
# ノードの存在確認
self._chk_node_exist()
# ノードから位置を取得
nodes_pos = self._nodes[:, :self._near_node_idx]
# 差分を計算
difference = nodes_pos - pos
# 距離を計算 (各ノードとの距離を算出するため，引数にaxis=1を与えた)
distance = np.linalg.norm(difference, axis=1)
# 最短距離ノードを取得
min_dist_idx = np.argmin(distance)
return min_dist_idx
def get_near_node_list(self, pos, radius):
"""
ノードと近傍ノードをリストで取得
パラメータ
pos(numpy.ndarray): ノード位置
radius(float): 半径
戻り値
near_node_list(list): 近傍ノードリスト
"""
# ノードの存在確認
self._chk_node_exist()
near_node_list = []
# ツリー内全ノード位置を取得
all_node_pos = self._nodes[:, :self._near_node_idx]
# ノードとツリー内全ノードの差分を計算
difference = all_node_pos - pos
# 差分から距離(ユークリッド距離)を計算
distance = np.linalg.norm(difference, axis=1)
# 距離が一定以内のノードだけを保存
near_node_list = [idx for idx, dist in enumerate(distance) if dist <= radius]
return near_node_list
class RRTPyBullet:
"""
RRTにロボットを追加したクラス
プロパティ
_name(str): 経路生成手法名
_pathes(numpy.ndarray): 始点から終点までの経路 (PruningやShortcut済みの経路)
_start_tree(Tree): 始点ツリー
_strict_min_pos(numpy.ndarray): 探索の最小範囲
_strict_max_pos(numpy.ndarray): 探索の最大範囲
_moving_value(float): 1回あたりの移動量 [rad] or [m]
_before_modify_pathes(numpy.ndarray): PruningやShortcutなどの修正前の経路
_pruning_pathes(numpy.ndarray): Pruning後の経路
_shortcut_pathes(numpy.ndarray): Shortcut後の経路
_partial_shortcut_pathes(numpy.ndarray): Partial Shortcut後の経路
メソッド
public
プロパティのゲッター関連
name(): _nameプロパティのゲッター
pathes(): _pathesプロパティのゲッター
準備処理
preparation(): 経路生成の準備
メイン処理関連
expand_once(): ランダム探索による新規ノードおよび最短ノードの取得 (干渉判定は実装しない)
add_node_and_check_goal(): ノード追加 + 経路生成の完了確認
fin_planning(): 経路生成の終了処理
ファイル関連
save(): 生成した経路をファイル保存
protected
メイン処理関連
_add_node_start_tree(): 始点ツリーにノードを追加
_chk_end_pos_dist(): 終点との距離が一定範囲内であるかの確認
_calc_new_pos(): 最短ノードからランダムな値方向へ新しいノード(位置)を作成
_get_random_pos(): ランダムな位置を取得
準備処理関連
_reset(): データの初期化
_set_interpolation(): 経路生成したい探索空間の設定
_strict_planning_pos(): 探索範囲を制限する
ファイル関連
_make_folder(): フォルダーの作成
_reset_folder(): フォルダー内のファイルを全削除
_get_path_plan_folders(): 経路生成結果を保存する複数のフォルダー名を取得
_make_path_plan_folder(): 経路生成結果を保存するフォルダー作成
_save_numpy_data_to_txt(): Numpyデータをテキストファイルに保存
"""
# 定数の定義
# ファイル名の定義
# 各経路生成手法で絶対に定義するべきファイル名 ↓
# _pathesプロパティを保存するファイル名
_FILE_NAME_PATHES = f"{PATHPLAN.RRT.value}_pathes.csv"
# _start_treeプロパティを保存するファイル名
_FILE_NAME_START_TREE = f"{PATHPLAN.RRT.value}_start_tree.csv"
# ツリーの要素番号を定義
_NODE_NEAR_NODE_IDX = RRT_NEAR_NODE_IDX # ノード内の最短ノード要素
# 探索に関する定義
_MOVING_VALUE_JOINT = 0.1 # 1回の移動量 [rad] (ロボットの関節空間)
_MOVING_VALUE_POS = 0.2 # 1回の移動量 [m] (ロボットの位置空間)
_STRICT_PLANNING_ROB_JOINT = np.pi / 4 # 探索範囲の制限 [rad] (ロボットの関節空間)
_STRICT_PLANNING_ROB_POS = 1.0 # 探索範囲の制限 [m] (ロボットの位置空間)
_GOAL_SAMPLE_RATE = 0.1 # ランダムな値を取るときに，終点を選択する確率
def __init__(self):
"""
コンストラクタ
"""
self._name = PATHPLAN.RRT.value
self._pathes = []
self._start_tree = Tree(self._NODE_NEAR_NODE_IDX)
self._interpolation = INTERPOLATION.NONE.value
self._moving_value = self._MOVING_VALUE_JOINT
self._dim = DIMENTION_NONE
# プロパティのゲッターメソッド ↓
@property
def name(self):
"""
_nameプロパティのゲッター
"""
return self._name
@property
def pathes(self):
"""
_pathesプロパティのゲッター
"""
return self._pathes
# プロパティのゲッターメソッド ↑
# メイン処理メソッド ↓
def expand_once(self, end_pos):
"""
ランダム探索による新規ノードおよび最短ノードの取得 (干渉判定は実装しない)
パラメータ
end_pos(list): 経路生成の終点
戻り値
new_node_pos(numpy.ndarray): 新規ノード
near_node_pos(numpy.ndarray): 最短ノード位置
near_node(int): 最短ノード番号
"""
# ランダムな値を取得
random_pos = self._get_random_pos(end_pos)
# ランダムな値と最短ノードを計算
near_node = self._start_tree.get_near_node(random_pos)
# 最短ノードの位置
near_node_pos = self._start_tree.nodes[near_node, :self._NODE_NEAR_NODE_IDX]
# 最短ノードからランダムな値方向へ新しいノード(位置)を作成
new_node_pos = self._calc_new_pos(random_pos, near_node_pos)
return new_node_pos, near_node_pos, near_node
def add_node_and_chk_goal(self, end_pos, node_pos, near_node):
"""
ノード追加 + 経路生成の完了確認
パラメータ
end_pos(numpy.ndarray): 経路生成の終点
node_pos(numpy.ndarray): ノード位置
near_node(int): 親ノード
戻り値
is_successful(bool): True/False = 経路生成の完了/未完了
"""
# 処理結果
is_successful = False
# 始点ツリーにノードを追加
self._add_node_start_tree(node_pos, near_node)
# 終点との距離が一定範囲内であるかの確認
if self._chk_end_pos_dist(node_pos, end_pos):
# 一定範囲内のため，経路生成の完了
is_successful = True
return is_successful
def _add_node_start_tree(self, pos, near_node):
"""
始点ツリーにノードを追加
パラメータ
pos(numpy.ndarray): 位置
near_node(int): 最短ノード
"""
# _start_treeにノードを追加
node = np.append(pos, near_node).reshape(1, -1)
self._start_tree.add_node(node)
def _chk_end_pos_dist(self, pos, end_pos):
"""
終点との距離が一定範囲内であるかの確認
パラメータ
pos(numpy.ndarray): ノード位置
end_pos(numpy.ndarray): 経路生成の終点
戻り値
is_near(bool): True / False = 一定範囲内である / でない
"""
is_near = False
# 距離を計算
dist = np.linalg.norm(end_pos - pos)
# 一定範囲内であるかの確認
if dist <= self._moving_value:
is_near = True
return is_near
def fin_planning(self, start_pos, end_pos):
"""
経路生成の終了処理
パラメータ
start_pos(list): 経路生成の始点
end_pos(list): 経路生成の終点
"""
# 始点から終点までの経路に関係するノードを選択
revers_path = end_pos.reshape(1, -1)
near_node = -1
while True:
# 終点から始点方向へノードを取得
node = self._start_tree.nodes[near_node]
pos = node[:self._NODE_NEAR_NODE_IDX].reshape(1, -1)
# 浮動小数型になっているので，整数型に型変換
near_node = int(node[self._NODE_NEAR_NODE_IDX])
revers_path = np.append(revers_path, pos, axis=0)
if near_node == INITIAL_NODE_NEAR_NODE:
# 始点ノードまで取得できたため，処理終了
break
# 経路が終点からの順番になっているため，始点から終点とする
self._pathes = revers_path[::-1]
# ツリーに終点を追加 (要素番号を指定するため -1)
self._add_node_start_tree(end_pos, self._start_tree.nodes.shape[0] - 1)
def _calc_new_pos(self, random_pos, near_node_pos):
"""
最短ノードからランダムな値方向へ新しいノード(位置)を作成
パラメータ
random_pos(numpy.ndarray): ランダムな位置
near_node_pos(numpy.ndarray): 最短ノード位置
戻り値
new_pos(numpy.ndarray): 新しいノード
"""
# 方向を計算
direction = random_pos - near_node_pos
# 方向の大きさを1にする
norm_direction = direction / (np.linalg.norm(direction) + EPSILON)
# 新しいノードを作成
new_pos = near_node_pos + norm_direction * self._moving_value
return new_pos
def _get_random_pos(self, target_pos):
"""
ランダムな位置を取得
パラメータ
target_pos(numpy.ndarray): 目標点
戻り値
random_pos(numpy.ndarray): ランダムな位置
"""
# 乱数を取って，目標点を選択するかランダムを選択するか
select_goal = np.random.rand()
if select_goal < self._GOAL_SAMPLE_RATE:
# 目標点を選択s
random_pos = target_pos
else:
random_pos = np.random.uniform(self._strict_min_pos, self._strict_max_pos)
return random_pos
# メイン処理メソッド ↑
# 準備処理関連メソッド ↓
def preparation(self, start_pos, end_pos, interpolation):
"""
経路生成の準備
パラメータ
start_pos(list): 経路生成の始点
end_pos(list): 経路生成の終点
interpolation(int): 補間方法 (関節空間/位置空間)
"""
# データの初期化
self._reset()
# 始点と終点の次元数が一致しているかの確認
start_pos_dim = np.size(start_pos)
end_pos_dim = np.size(end_pos)
if start_pos_dim != end_pos_dim:
# 次元数が異なるので異常
raise ValueError(f"start_pos_dim and end_pos_dim are not matched. start_pos_dim is {start_pos_dim}, end_pos_dim is {end_pos_dim}")
self._dim = start_pos_dim
# 探索空間の設定
self._set_interpolation(interpolation)
# 始点ノードをツリーに追加
self._add_node_start_tree(start_pos, INITIAL_NODE_NEAR_NODE)
# 探索範囲を設定
self._strict_planning_pos(start_pos, end_pos)
# 結果を保存するフォルダ作成
self._make_path_plan_folder(interpolation)
# フォルダー内のファイルを全部削除
self._reset_folder(interpolation)
def _reset(self):
"""
データの初期化
"""
self._start_tree.reset()
if len(self._pathes) != 0:
# 何かしらのデータが保存
del self._pathes
self._pathes = []
self._interpolation = INTERPOLATION.NONE.value
self._dim = DIMENTION_NONE
def _set_interpolation(self, interpolation):
"""
経路生成したい探索空間の設定
パラメータ
interpolation(int): 補間の種類 (関節補間/位置補間)
"""
if interpolation == INTERPOLATION.POSITION.value:
# 位置空間
self._moving_value = self._MOVING_VALUE_POS
elif interpolation == INTERPOLATION.JOINT.value:
# 関節空間
self._moving_value = self._MOVING_VALUE_JOINT
else:
# 異常値
raise ValueError(f"interpolation is abnormal. interpolation is {interpolation}")
# 補間種類の更新
self._interpolation = interpolation
def _strict_planning_pos(self, start_pos, end_pos):
"""
探索範囲を制限する
パラメータ
start_pos(numpy.ndarray): 始点
end_pos(numpy.ndarray): 終点
"""
all_pos = np.array([start_pos, end_pos])
# 各列の最大/最小値を取得
min_pos = np.min(all_pos, axis=0)
max_pos = np.max(all_pos, axis=0)
if self._interpolation == INTERPOLATION.POSITION:
# 位置空間の探索
strict_planning_pos = self._STRICT_PLANNING_ROB_POS
else:
# 関節空間の探索
strict_planning_pos = self._STRICT_PLANNING_ROB_JOINT
self._strict_min_pos = min_pos - strict_planning_pos
self._strict_max_pos = max_pos + strict_planning_pos
# print(f"self._strict_min_pos = {self._strict_min_pos}")
# print(f"self._strict_max_pos = {self._strict_max_pos}")
# 準備処理関連メソッド ↑
# ファイル関連メソッド ↓
def _make_folder(self, folder_name):
"""
フォルダーの作成
パラメータ
folder_name(str): 作成したいフォルダー名
"""
# フォルダーが作成済みでもエラーを出力しないよう，exist_ok=Trueとした．
os.makedirs(folder_name, exist_ok=True)
def _reset_folder(self, interpolation):
"""
フォルダー内のファイルを全削除
パラメータ
interpolation(str): 探索方法 (位置空間/関節空間)
"""
# フォルダー名を取得
folder_names = self._get_path_plan_folders(interpolation)
for folder_name in folder_names:
for entry in os.listdir(folder_name):
full_path = os.path.join(folder_name, entry)
if os.path.isfile(full_path) or os.path.islink(full_path):
os.remove(full_path) # 通常ファイル・シンボリックリンクを削除
def _get_path_plan_folders(self, interpolation):
"""
経路生成結果を保存する複数のフォルダー名を取得
パラメータ
interpolation(str): 探索方法 (位置空間/関節空間)
戻り値
folder_names(list): 複数のフォルダー名
"""
folder_names = [os.path.join(self._name, interpolation), ]
return folder_names
def _make_path_plan_folder(self, interpolation):
"""
経路生成結果を保存するフォルダー作成
パラメータ
interpolation(str): 探索方法 (位置空間/関節空間)
"""
# フォルダー名を取得
folder_names = self._get_path_plan_folders(interpolation)
for folder_name in folder_names:
self._make_folder(folder_name)
def save(self):
"""
生成した経路をファイル保存
"""
# 始点から終点までの修正済みの経路をファイル保存
self._save_numpy_data_to_txt(self._pathes, self._FILE_NAME_PATHES)
# 始点のツリーを保存
self._save_numpy_data_to_txt(self._start_tree.nodes, self._FILE_NAME_START_TREE)
def _save_numpy_data_to_txt(self, data, file_name):
"""
Numpyデータをテキストファイルに保存
パラメータ
data(numpy.ndarray): ファイル保存したいデータ
file_name(str): 保存したいファイル名
"""
# 引数の確認
if len(data) == 0:
# データが存在しないため，処理終了
return
if not file_name:
# ファイル名が存在しないため，処理終了
return
# ファイル名にフォルダ名を追加 (各経路生成手法で異なるフォルダにデータを保存)
full_path = f"{self._name}/{self._interpolation}/{file_name}"
np.savetxt(full_path, data)
# ファイル関連メソッド ↑
RRT内で干渉判定を実施できないため，RRTクラスの実装側で干渉判定の結果に応じて，処理を実装できるようにpubic関数を用意した．
RRTクラスの実装側で，呼んでほしくない処理はprotected関数として，public関数を最小限とした．
今回は，RRTによる経路生成だが将来的にはRRTの拡張アルゴリズムを使用する予定である．
RRTに関しては，私の他記事内やコメントを記載したため，各関数についての説明は割愛する．
PyBulletでロボットを動かす
上記にて，ロボットのURDF，ソースコードを説明した．
main.py ファイルを実施することによって，PyBullet上のロボットを動かしていく．
探索空間を直交空間と関節空間に分けてロボットを動かした動画を載せる．
直交空間で探索した時の動画
直交空間で探索した時のロボットの動画を下図に2つ載せる．
動画１：始点から終点まで干渉しない経路を生成するための探索
動画２：探索後の始点から終点までの経路
ロボットが色々と動いて，干渉が生じないような経路を探索しているのがわかる．
また，最終的な始点から終点までの経路では，干渉回避できていることが見て取れる．
経路生成をシミュレータ上で見ることで，ロボットが試行錯誤しながら，環境に対応した経路を生成してく過程を見られて楽しい．
関節空間で探索した時の動画
関節空間で探索した時のロボットの動画を下図に2つ載せる．
動画３：始点から終点まで干渉しない経路を生成するための探索
関節空間で探索しても，干渉しない経路が生成できなかった．
要因として，pybullet_rrt.py 内で，探索空間を制限する処理 (_strict_planning_pos) を入れて，関節1の探索範囲が狭いことを挙げられる．
また，関節空間内では始点から終点まで直線的に移動させても，直交空間上では少し遠回りをするような経路となることも要因として挙げられる．
直交空間では容易に干渉回避できる経路が作成できるけど，関節空間では経路が作成できないことはありえるということだ．
また，経路生成手法をRRTからRRT-Conect(始点ツリーと終点ツリーの双方向から探索)に変換したら，干渉しない経路が生成できたかもしれない．
関節が2つしかないので，自由度が少ないことも原因かもしれない．
おわりに
本記事では，Pythonを使用して，下記内容を実装しました
・PyBullet で干渉物が存在する環境下で，2軸ロボットアームの経路生成 (経路生成手法としてRRTを採用)
次記事では，下記内容を実装していきます．
・PyBullet で3軸ロボットアームを動かす (関節角度を自由に設定)
(https://qiita.com/haruhiro1020/items/2958018f76ae2b9251e7)