はじめに
PyBullet (Python上で動く物理シミュレータ) を使用して，ロボットアームを可視化して，動かしたい．
前記事では，PyBullet (Python上で動く物理シミュレータ) で2軸ロボットアームにグリッパーを追加して，立方体を把持するシミュレーションを実装した．
(https://qiita.com/haruhiro1020/items/01f661a571c6fe76c551)
本記事では，前記事の続きで，グリッパー付きの2軸ロボットアームを使用して，干渉物が存在する環境下にて，物体を把持する(下図はPyBullet上のグリッパー付き2軸ロボットアームである)．黒枠が干渉物，赤枠がグリッパー，緑枠が把持したい物体である．
本記事では，ロボットが自動で把持物体の位置まで干渉しないように移動し，物体を把持するまでを実装する．そのため，把持した物体を他の場所へ移動させることは実装していない．実際に動かしている動画は下図の通りである．
本記事で実装すること
・干渉物が存在する環境下での経路生成 + 物体把持
本記事では実装できないこと (将来実装したい内容)
・3軸，6軸ロボットアームへの対応
・カメラによる物体の位置把握 + 干渉物が存在する環境下での経路生成 + 物体把持
動作環境
・macOS Sequoia (バージョン15.5)
・Python3 (3.13.3)
・PyBullet (3.2.5) (物理シミュレータ)
・Numpy (2.3.0) (数値計算用ライブラリ)
PyBullet のインストール方法
PyBullet のインストール方法については前記事にて，説明したため，割愛する．
(前記事 https://qiita.com/haruhiro1020/items/ecc5215234350b663770)
2軸ロボットアームの定義
本記事で動かしたい，2軸ロボットアームについて説明する．
本記事では，下図のような2軸ロボットアームを考えている．
$\theta_{1}, \theta_{2}$ は，Z軸方向($X, Y$軸に直交する手前方向)へ回転する．
上図のパラメータ $l_{1}, l_{2}, m_{1}, m_{2}$ の値は下表として，考える(値は適当である)．
| パラメータ名 | 概要 | 値 |
|---|---|---|
| $l_{1}$ | リンク$1$の長さ | 1.0 [m] |
| $l_{2}$ | リンク$2$の長さ | 1.0 [m] |
| $m_{1}$ | リンク$1$の質量 | 1.0 [kg] |
| $m_{2}$ | リンク$2$の質量 | 1.0 [kg] |
PyBulletで，上図のロボットを使用するために，URDF (Unified Robot Description Format)を作成する必要がある．
グリッパーの定義
本記事では，下図のようなパラレルグリッパーをロボットアームに追加する．
上図はグリッパーが開いた時の画像である．下図はグリッパーを閉じた時の画像である．2本の指の間に把持物体を置いた状態で，グリッパーを閉じることで，物体把持を実施する．
パラレルグリッパーを採用した理由は下記の通りである．
・URDFの作成および制御が容易
・立方体および直方体の場合，接触する面積が大きくなるため，摩擦力が大きくなり滑りにくい
人の手である "多指グリッパー" も存在するが，URDFを作成するのが困難であるため，採用しない．
把持対象物に関して
本記事では，把持対象物を立方体とする．
また，把持までの流れは以下の通りとなる．
１：始点から終点まで，干渉しない経路を探索する
２：干渉しない始点から終点までの経路を移動する
３：物体把持
ここで，重要なことは上記の「１」の時に，ロボットと把持対象物がぶつかってしまう可能性があること．ぶつかってしまうと，把持対象物が指定した位置から大きくずれた位置に動いてしまう．
上記を考慮して，把持対象物は経路探索時に，ぶつかっても動かないように拘束させる．PyBulletでは，拘束させる(動かさないようにする)ことが可能である．
グリッパー付き2軸ロボットアームのURDF作成
グリッパー付き2軸ロボットアームのURDFを作成する．URDFに関しては，前記事で説明したため，本記事では割愛する．
(前記事 https://qiita.com/haruhiro1020/items/01f661a571c6fe76c551)
経路生成手法であるRRT
本記事で使用する経路生成手法として，RRTを使用する．
RRTは以前に記事を作成したため，内容は割愛する．
(RRTに関する記事 https://qiita.com/haruhiro1020/items/6eaae645bd638c2b8897)
本記事では，RRTを使用して，干渉回避できる経路を作成する．
PyBulletの使用方法
Pythonの物理シミュレータであるPyBulletの使用方法について説明する．
下記リンクのPyBullet公式で調べながら，PyBulletを使用している．
・https://pybullet.org/wordpress/
上記リンクを開くと，下図のようなサイトに飛ぶ．使用方法を調べるときは，下図の赤枠内の「PYBULLET QUICKSTART GUIDE」タグをクリックする．
「PYBULLET QUICKSTART GUIDE」タグをクリックすると，下図のようなドキュメントを見ることができる．基本的には，ドキュメントに記載されている関数の使用方法を見て，ソースコードを作成している．
PyBulletの関数の引数や戻り値をもっと知りたいのでしたら，上記ドキュメントを見た方がわかりやすいです．
全体のソースコード
はじめに，本記事で使用する全体のソースコードについて説明する．
その後，重要な箇所を抜粋して別途解説をしていく．
ソースコードとして，下記の4ファイルを作成する．
・定数の定義 (constant.py)
・全体的なメイン処理 (main.py)
・PyBulletでロボットを動かす (pybullet_robot.py)
・経路生成手法であるRRT (pybullet_rrt.py)
各ファイルの中身を説明する．
定数の定義 (constant.py)
定数を定義する constant.py について説明する．
constant.py
# 複数ファイルで使用する定数の定義
from enum import Enum
# 次元数を定義
DIMENTION_NONE = -1 # 未定義
DIMENTION_2D = 2 # 2次元
# 重力
GRABITY_VALUE = 9.81
# シード値の最小値と最大値
MIN_SEED = 0
MAX_SEED = 2 ** 32 - 1 # 4バイト (uint) の最大値
# 0割を防ぐための定数
EPSILON = 1e-6
# ノードの最短ノード要素とコスト要素を定義
# RRT
RRT_NEAR_NODE_IDX = -1 # RRTでの最短ノード要素
# ツリーの初期ノードの親ノード
INITIAL_NODE_NEAR_NODE = -1 # 初期ノードに親ノードが存在しないから-1
# 補間方法の定義
class INTERPOLATION(Enum):
"""
補間方法
"""
NONE = "none" # 未定義
JOINT = "joint" # 関節補間
POSITION = "pos" # 位置補間
# RRTによる経路生成手法の定義
class PATHPLAN(Enum):
"""
経路生成手法
"""
RRT = "rrt" # RRTによる経路生成
全体的なメイン処理 (main.py)
全体的なメイン処理を定義する main.py について説明する．
後ほど説明する pybullet_robot.py 内のクラスを実装するのがメインな処理である．
main.py
# Pybullet (Pythonでの3次元物理シミュレータ) による2軸ロボットアームの可視化
# 標準ライブラリの読み込み
import numpy as np
# 自作モジュールの読み込み
from pybullet_robot import MainPyBulletRobot
from constant import *
CONST_SEED = 1 # シード値 (常に同じ結果としたいから)
HAND_FLG = True # ハンドの装着有無
def main():
"""
メイン処理
"""
# ロボットが保存されている URDF ファイル名
if HAND_FLG:
# ハンド付きのURDF
robot_urdf = "robot_2dof_hand.urdf"
else:
# ハンドなしのURDF
robot_urdf = "robot_2dof.urdf"
# 経路生成手法
path_plan = PATHPLAN.RRT.value
# 干渉物が保存されている URDF ファイル名
environment_urdf = "environment.urdf"
# 把持対象物が保存されている URDF ファイル名
grasp_urdf = "grasp_object.urdf"
# 探索空間を指定
interpolation = INTERPOLATION.POSITION.value # 直交空間の探索
# PyBulletを使用するインスタンス作成
my_robot = MainPyBulletRobot(interpolation, robot_urdf, environment_urdf=environment_urdf, grasp_urdf=grasp_urdf, hand=HAND_FLG)
# 初期位置
start_pos = np.array([1.0, -1.0])
# シード値の設定
np.random.seed(CONST_SEED)
if interpolation == INTERPOLATION.JOINT.value:
# 関節空間の探索時は位置を関節角度に変換
start_theta = my_robot.convert_pos_to_theta(start_pos)
print(f"start_theta = {start_theta}")
result = my_robot.run(start_theta, path_plan)
else:
result = my_robot.run(start_pos, path_plan)
print(f"result = {result}")
if __name__ == "__main__":
# 本ファイルがメインで呼ばれた時の処理
main()
PyBulletでロボットを動かす (pybullet_robot.py)
PyBulletでロボットを動かす処理を定義する pybullet_robot.py について説明する．
pybullet_robot.py
# PyBulletで使用するロボットを記載
# ライブラリの読み込み
import pybullet as p # PyBullet
import pybullet_data # PyBulletで使用するデータ
import time # 時間
import numpy as np # 数値計算ライブラリ
# サードパーティの読み込み
# 自作モジュールの読み込み
from constant import *
from pybullet_rrt import RRTPyBullet
class BaseGripper:
"""
グリッパーのベースクラス (抽象クラス)
プロパティ
_robot_id(): ロボットアームのID番号
_n_joints(int): ロボットアームの関節数
メソッド
public
run(): 実行 現在角度を関節に与える (オープン・クローズ以外で常に呼ぶこと)
open(): グリッパーのオープン
close(): グリッパーのクローズ
"""
# 定数の定義
_JOINT_CURRENT_VALUE_IDX = 0 # 関節の現在値の要素番号
def __init__(self, robot_id, n_joint):
"""
コンストラクタ
パラメータ
robot_id(p.loadURDFの戻り値): ロボットURDFを読み込んだ際のID
n_joint(int): ロボットの関節数 (グリッパーは含むが，グリッパー先端は含まない)
"""
# プロパティの初期化
self._robot_id = robot_id
self._n_joint = n_joint
def run(self):
"""
実行 (毎時刻，本関数を呼ぶこと)
"""
raise InterruptedError("run() is necessary override.")
class ParallelGripper(BaseGripper):
"""
パラレルグリッパー (2本の指が並行に動く)
メソッド
public
実行 (毎時刻，本関数を呼ぶこと)
protected
_get_move_direction(): グリッパーの移動方向を取得
_get_gripper_right_left_idx(): グリッパーの右・左の関節番号
_set_joint_values(): 関節角度[m]を設定
_get_joint_values(): 関節角度[m]を取得
"""
# 定数の定義
_GRIPPER_RIGHT_IDX = -2 # 右グリッパーの関節番号
_GRIPPER_LEFT_IDX = -1 # 左グリッパーの関節番号
_GRIPPER_MOVE_VAL = 0.01 # グリッパーの1回あたりの移動量 [m]
_GRIPPER_LATERAL_FRIC = 1.0 # グリッパーの摩擦係数
_GRIPPER_CLOSE_VAL = 0.03 # グリッパーのクローズ時のフィンガー角度 [m]
_GRIPPER_OPEN_VAL = 0 # グリッパーのクローズ時のフィンガー角度 [m]
def __init__(self, robot_id, n_joint):
"""
コンストラクタ
パラメータ
robot_id(p.loadURDFの戻り値): ロボットURDFを読み込んだ際のID
n_joint(int): ロボットの関節数 (グリッパーは含むが，グリッパー先端は含まない)
"""
# 親クラスのコンストラクタ
super().__init__(robot_id, n_joint)
# ダイナミクスの変更
self._chg_dynamics()
def _chg_dynamics(self):
"""
ダイナミクスの変更
"""
# グリッパーの右・左の関節番号を取得
gripper_right_left_idx = self._get_gripper_right_left_idx()
for idx in gripper_right_left_idx:
p.changeDynamics(self._robot_id, # 把持対象物ID
idx, # 関節番号
lateralFriction=1.0) # 床との摩擦係数
def run(self, open=False, close=False):
"""
実行 (毎時刻，本関数を呼ぶこと)
パラメータ
open(bool): グリッパーのオープンフラグ
close(bool): グリッパーのクローズフラグ
"""
# グリッパーの現在の関節角度[m]を取得
joint_values = self._get_joint_values()
# オープンとクローズが同時実行の時，安全の観点よりクローズよりもオープンを優先
if open:
# グリッパーのオープン時
# 移動方向を取得して，設定したい関節角度[m]を計算
direction = self._get_move_direction(open=open)
joint_values = direction * self._GRIPPER_OPEN_VAL
elif close:
# グリッパーのクローズ時
# グリッパーのクローズに関するキーボードが押下された時
direction = self._get_move_direction(open=open)
joint_values = direction * self._GRIPPER_CLOSE_VAL
else:
# 押下されていないため，何もしない
pass
# 関節角度の設定
self._set_joint_values(joint_values)
def _get_move_direction(self, open):
"""
グリッパーの移動方向を取得
パラメータ
open(bool): True/False = オープン/クローズ
戻り値
numpy.ndarray: グリッパーの移動方向 (右関節・左関節の順番)
"""
# グリッパーの右関節・左関節の移動方向
move_direction = np.array([-1.0, 1.0])
if not open: # クローズ
move_direction *= -1
return move_direction
def _get_gripper_right_left_idx(self):
"""
グリッパーの右・左の関節番号
戻り値
list: グリッパーの右・左の関節番号 (右・左の順番にデータ保存)
"""
right_left_idx = [self._n_joint + self._GRIPPER_RIGHT_IDX, self._n_joint + self._GRIPPER_LEFT_IDX]
return right_left_idx
def _set_joint_values(self, values):
"""
関節角度[m]を設定
パラメータ
values(numpy.ndarray): 設定値
"""
# パラメータのサイズを確認
if values.shape[0] != DIMENTION_2D:
# 異常
raise ValueError(f"values'shape[0] is abnormal. values'shape[0] is {values.shape[0]}")
# グリッパーの右・左の関節番号
gripper_right_left_idx = self._get_gripper_right_left_idx()
for idx, gripper_idx in enumerate(gripper_right_left_idx):
p.setJointMotorControl2(
bodyIndex=self._robot_id,
jointIndex=gripper_idx,
controlMode=p.POSITION_CONTROL,
targetPosition=values[idx],
positionGain=0.5, # デフォルトよりやや高め
velocityGain=1.0 # 高速応答（必要に応じて調整）
)
def _get_joint_values(self):
"""
関節角度[m]を取得
戻り値
numpy.ndarray: 関節角度 (グリッパーの右関節，グリッパーの左関節)
"""
# グリッパーの右・左の関節番号
gripper_right_left_idx = self._get_gripper_right_left_idx()
joint_values = []
for gripper_idx in gripper_right_left_idx:
# グリッパー関節の状態を取得
joint_state = p.getJointState(bodyUniqueId=self._robot_id, jointIndex=gripper_idx)
# 関節の値を保存
joint_values.append(joint_state[self._JOINT_CURRENT_VALUE_IDX])
return np.array(joint_values)
class MainPyBulletRobot:
"""
PyBulletのメインクラス
プロパティ
_robot_id(): ロボットアームのID番号
_n_joints(int): ロボットアームの関節数
_environment_id(): 県境のID番号
_interpolation(str): 探索空間 (直交空間/関節空間)
_rrt(pybullet_rrt.py内のクラス): 経路生成
_hand(RobotHand): ハンドクラス
メソッド
public
メイン処理関連
run(): 実行 (始点から終点まで，干渉しない経路を生成)
運動学関連
convert_pos_to_theta(): 位置から関節角度に変換 (クラス外で使う用)
protected
事前準備関連
_init_robot(): ロボットの初期化
_init_environment(): 環境の初期化
メイン処理関連
_set_path_plan(): 経路生成手法の設定
_path_planning(): 始点から終点までの経路生成
_post_path_planning(): 経路生成の後処理 (経路生成成功時だけ実装)
_set_jump_joint(): 関節角度にジャンプ
_set_joint(): 関節角度の設定 (現在位置から移動)
_set_gripper(): グリッパー関節に角度を設定
運動学関連
_convert_pos_to_theta(): 位置から関節角度に変換 (クラス内で使う用)
干渉判定処理
_is_line_interference(): 2点間の干渉判定
_is_interference_start_end_pos(): 始点と終点が干渉判定していないかの確認
_is_interference_pos(): 位置にジャンプして干渉判定
"""
# 定数の定義
_PLANE_URDF = "plane.urdf" # 地面に関する urdf ファイル
_IDX_MIN_JOINT = 8 # 関節の最小値が保存されている要素番号
_IDX_MAX_JOINT = 9 # 関節の最大値が保存されている要素番号
_SLIDER_MAKE_WAIT_TIME = 0.2 # スライダー作成の待機時間 [sec]
_SIMULATION_SLEEP_TIME = 1. / 200. # シミュレーションの待機時間 [sec]
_DEBUG_TEXT_LIFE_TIME = 0 # テキストの生存時間 [sec] (0は無限時間)
_DEBUG_TEXT_SIZE = 0.5 # テキストの大きさ
_ZERO_NEAR = 1e-4 # 0近傍の値
_INTERFERENCE_MARGIN = 0.1 # 干渉判定のマージン [m]
_PATH_PLAN_TIME = 1000 # 経路生成の最大時間 [sec]
_N_MARGIN_MOVE = 50 # 経路生成終了後の余白時間 [回]
_N_HAND_JOINT = 4 # ハンド用の関節数
_N_INVERSE_HAND_JOINT = -2 # 逆運動学計算時のハンド関節数
_KEY_DOWN_MOVE_POS = 0.1 # キーボード押下時のロボット手先位置の移動量 [m]
_KEY_DOWN_MOVE_ORI = 0.05 # キーボード押下時のロボット手先姿勢の移動量 [rad]
_GRASP_OBJECT_POS = [ 1.8, 1.0, 0.05] # 把持対象物の位置
_GRASP_OBJECT_OFFSET = [-0.4, 0, 0] # 把持対象物の位置のオフセット
def __init__(self, interpolation, robot_urdf, environment_urdf=None, grasp_urdf=None, hand=False):
"""
コンストラクタ
パラメータ
interpolation(str): 補間方法 (関節空間/位置空間)
robot_urdf(str): ロボットアームのファイル名 (urdf)
environment_urdf(str): 環境のファイル名 (urdf)
grasp_object(str): 把持対象物のファイル名 (urdf)
hand(bool): ハンドの装着有無 True/False = 装着/未装着
"""
# PyBulletの初期化
p.connect(p.GUI)
# パスの追加
p.setAdditionalSearchPath(pybullet_data.getDataPath())
# シミュレーションの初期化
p.resetSimulation()
# 重力の設定 (下(-z軸)方向の加速度)
p.setGravity(0, 0, -GRABITY_VALUE)
# ロボットの初期化
self._init_robot(robot_urdf, interpolation, hand)
# 環境の初期化
self._init_environment(environment_urdf, grasp_urdf)
self._rrt = None
# 事前準備メソッド ↓
def _init_robot(self, robot_urdf, interpolation, hand):
"""
ロボットの初期化
パラメータ
robot_urdf(str): ロボットアームのファイル名 (urdf)
interpolation(str): 探索方法 (関節空間/位置空間)
hand(bool): ハンドの装着有無 True/False = 装着/未装着
"""
# 引数の確認
if not (interpolation == INTERPOLATION.JOINT.value or interpolation == INTERPOLATION.POSITION.value):
# 異常
raise ValueError(f"interpolation is abnormal. interpolation is {interpolation}")
# プロパティの更新
self._interpolation = interpolation
# ロボットを読み込む．ベースリンクの原点は (x, y, z) = (0, 0, 0) として，ベースリンクは地面に固定
self._robot_id = p.loadURDF(robot_urdf, basePosition=[0, 0, 0], useFixedBase=True)
# urdf よりロボットの関節数を取得 (エンドエフェクタ用のデバッグ関節は不要なため -1)
self._n_joints = p.getNumJoints(self._robot_id) - 1
# ハンド装着有無
if hand:
# ハンド装着
if self._n_joints != (DIMENTION_2D + self._N_HAND_JOINT):
# 2軸ロボットアーム以外
raise ValueError(f"self._n_joints is abnormal. {self._n_joints} is abnormal.")
self._hand = ParallelGripper(self._robot_id, self._n_joints)
else:
# ハンド非装着
if self._n_joints != DIMENTION_2D:
# 2軸ロボットアーム以外
raise ValueError(f"self._n_joints is abnormal. {self._n_joints} is abnormal.")
self._hand = None
def _init_environment(self, environment_urdf, grasp_urdf):
"""
環境の初期化
パラメータ
environment_urdf(str): 環境が保存されているファイル名
grasp_urdf(str): 把持対象物が保存されているファイル名
"""
# 地面を読み込む (pybulletが提供している "plane.urdf" を読み込む)
p.loadURDF(self._PLANE_URDF)
# 環境を読み込む
self._environment_id = None
if environment_urdf is not None:
self._environment_id = p.loadURDF(environment_urdf, basePosition=[0, 0, 0], useFixedBase=True)
# 把持対象物を読み込む
self._grasp_id = None
self._grasp_constraint_id = None
if grasp_urdf is not None:
self._grasp_id = p.loadURDF(grasp_urdf, basePosition=[pos + offset for pos, offset in zip(self._GRASP_OBJECT_POS, self._GRASP_OBJECT_OFFSET)])
# 把持対象物に摩擦を付与する
p.changeDynamics(self._grasp_id, # 把持対象物ID
-1, # ベースに対して
lateralFriction=1.0, # 床との摩擦係数
spinningFriction=1.0, # 回転摩擦係数
rollingFriction=1) # 転がり摩擦
# 把持対象物の位置・姿勢を取得
grasp_pos, grasp_ori = self._get_grasp_pos()
# 把持対象物に拘束条件を付与
self._grasp_constraint_id = self._set_constraint(self._grasp_id, grasp_pos, grasp_ori)
# 事前準備メソッド ↑
# メイン処理 ↓
def run(self, start_pos, path_plan):
"""
実行
始点から終点まで，干渉しない経路を生成
パラメータ
start_pos(numpy.ndarray): 経路生成の始点
path_plan(str): 経路生成手法
戻り値
bool: True/False = 経路生成に成功/失敗
"""
# リアルタイムでのシミュレーション
p.setRealTimeSimulation(1)
# 把持対象物の位置を取得
end_pos, _ = self._get_grasp_pos(dim2=True)
end_pos = np.array(end_pos)
# 始点と終点で干渉していないかの確認
self._is_interference_start_end_pos(start_pos, end_pos)
# 経路生成手法の設定
self._set_path_plan(path_plan)
# 経路生成の実装
result = self._path_planning(start_pos, end_pos)
if result: # 経路生成に成功
self._post_path_planning(start_pos, end_pos)
# ファイル保存
self._rrt.save()
# 経路生成後の余白時間
self._exec_margin_time(end_pos)
return result
def _exec_margin_time(self, end_pos):
"""
経路生成後の余白時間の処理
パラメータ
end_pos(numpy.ndarray): 経路生成の終点
"""
# 長めの待機時間
time.sleep(self._SIMULATION_SLEEP_TIME * 10)
# PyBulletで関節角度を与えても，与えた関節角度ピッタリにはならず，数値誤差が発生するから，終点へ移動する処理を実施する
end_theta = self._convert_pos_to_theta(end_pos)
# 経路生成後の余白時間 (即座にPyBulletが終了するのを防ぐための余白時間)
for _ in range(self._N_MARGIN_MOVE):
# 終点に移動
self._set_joint(end_theta)
# グリッパーの実行
self._run_gripper(open=True)
# 待機時間
time.sleep(self._SIMULATION_SLEEP_TIME)
# 把持物体の拘束を解除
if self._grasp_constraint_id is not None:
self._release_constraint(self._grasp_constraint_id)
time.sleep(self._SIMULATION_SLEEP_TIME)
self._grasp_constraint_id = None
# 経路生成後の余白時間 (即座にPyBulletが終了するのを防ぐための余白時間)
for _ in range(self._N_MARGIN_MOVE):
# 終点に移動
self._set_joint(end_theta)
# グリッパーの実行
self._run_gripper(close=True)
# 待機時間
time.sleep(self._SIMULATION_SLEEP_TIME)
def _set_constraint(self, object_id, pos, ori):
"""
拘束条件の設定
パラメータ
object_id(int): 拘束したい対象物ID
pos(list): 拘束したい位置
ori(list): 拘束したい姿勢
戻り値
int: 拘束条件ID
"""
constraint_id = p.createConstraint(
object_id, # 親番号(拘束したい対象物ID)
-1, # 親リンクの要素番号("-1"はベース)
-1, # 子番号("-1"はなし)
-1, # 子リンクの要素番号("-1"はベース)
p.JOINT_FIXED, # 関節タイプ(今回は固定"JOINT_FIXED")
[0, 0, 0], # 関節軸
[0, 0, 0], # 親の中心からの位置
pos, # 子の中心からの位置 (今回は子を設定していないから，ワールド座標系から見た関節位置)
parentFrameOrientation=ori, # 親の中心からの姿勢
childFrameOrientation=[0, 0, 0, 1]) # 子の中心からの姿勢 (今回は，子を設定していないから，ワールド座標系から見た姿勢)
return constraint_id
def _release_constraint(self, constraint_id):
"""
拘束条件の解除
"""
if constraint_id is not None:
# 拘束条件を解除
p.removeConstraint(constraint_id)
def _get_robot_joint_from_grasp_pos(self):
"""
把持対象物の位置となるロボットの関節角度を取得
戻り値
numpy.ndarray: ロボットの関節角度
"""
# 把持対象物の位置を取得
grasp_pos, grasp_ori = self._get_grasp_pos()
# 逆運動学 (手先位置から関節角度へ変換) の実施
thetas = self._convert_pos_to_theta(grasp_pos)
return thetas
def _get_grasp_pos(self, dim2=False):
"""
把持対象物の位置を取得
パラメータ
dim2(bool): 2次元位置として取得するかどうか
戻り値
list: 把持対象物の位置 ([rad] or [m])
list: 把持対象物の姿勢
"""
if self._grasp_id is None: # 把持対象物が存在しない
raise ValueError("self._grasp_id is abnorma. please set grasp_urdf.")
# 把持対象物の位置[m]を取得
grasp_pos, grasp_ori = p.getBasePositionAndOrientation(self._grasp_id)
if dim2:
grasp_pos = grasp_pos[:DIMENTION_2D]
return grasp_pos, grasp_ori
def _set_path_plan(self, path_plan):
"""
経路生成手法の設定
パラメータ
path_plan(str): 経路生成手法名
"""
if path_plan == PATHPLAN.RRT.value:
# RRT
self._rrt = RRTPyBullet()
else:
# 異常
raise ValueError(f"path_plan is abnormal. path_plan is {path_plan}")
def _path_planning(self, start_pos, end_pos):
"""
始点から終点までの経路生成
パラメータ
start_pos(numpy.ndarray): 始点
end_pos(numpy.ndarray): 終点
戻り値
result(bool): True/False = 経路生成に成功/失敗
"""
result = False
# 経路生成の準備
self._rrt.preparation(start_pos, end_pos, self._interpolation)
start_time = time.time()
# 始点から終点までの経路が生成するまでループ
while True:
now_time = time.time()
if (now_time - start_time) >= self._PATH_PLAN_TIME:
# タイムアウト
break
# 経路生成を1度実行
new_node_pos, near_node_pos, near_node = self._rrt.expand_once(end_pos)
if self._is_line_interference(new_node_pos, near_node_pos):
# 干渉あり
continue
# 干渉なしだから，ノード追加 + 経路生成の完了確認
if not self._rrt.add_node_and_chk_goal(end_pos, new_node_pos, near_node):
# 終点までの近傍ではない
continue
# 新規ノードと始点までの干渉確認
if not self._is_line_interference(new_node_pos, end_pos):
# 始点から終点までの経路生成に成功
result = True
break
return result
def _post_path_planning(self, start_pos, end_pos):
"""
経路生成の後処理 (経路生成成功時だけ実装)
パラメータ
start_pos(numpy.ndarray): 経路生成の始点
end_pos(numpy.ndarray): 経路生成の終点
"""
# 経路生成の終了処理
self._rrt.fin_planning(start_pos, end_pos)
# 始点に移動
theta = self._convert_pos_to_theta(start_pos)
self._set_jump_joint(theta)
# グリッパーの実行
self._run_gripper(open=True)
# 始点から終点までの経路を移動
for row_idx in range(self._rrt.pathes.shape[0]):
next_theta = self._convert_pos_to_theta(self._rrt.pathes[row_idx])
self._set_joint(next_theta)
# グリッパーの実行
self._run_gripper(open=True)
# 待機時間
time.sleep(self._SIMULATION_SLEEP_TIME)
def _set_jump_joint(self, thetas):
"""
関節角度をジャンプ
パラメータ
thetas(numpy.ndarray): 関節角度
"""
for i in range(len(thetas)):
# 関節角度を設定
p.resetJointState(
bodyUniqueId=self._robot_id, # IDの設定
jointIndex=i, # 関節番号の設定
targetValue=thetas[i] # 関節角度
)
def _set_joint(self, thetas):
"""
関節角度の設定 (現在位置から移動)
パラメータ
thetas(numpy.ndarray): 設定したい関節角度
"""
for i in range(len(thetas)):
# 関節角度を設定
p.setJointMotorControl2(
bodyUniqueId=self._robot_id, # IDの設定
jointIndex=i, # 関節番号の設定
controlMode=p.POSITION_CONTROL, # 位置制御
targetPosition=thetas[i] # 関節角度
)
def _run_gripper(self, open=False, close=False):
"""
グリッパーの実行
パラメータ
open(bool): グリッパーのオープンフラグ
close(bool): グリッパーのクローズフラグ
"""
if self._hand is None:
# ハンド非装着のため，処理終了
return
self._hand.run(open, close)
# メイン処理 ↑
# 干渉判定処理 ↓
def _is_line_interference(self, pos1, pos2):
"""
2点間の干渉判定
パラメータ
pos1(numpy.ndarray): 位置1
pos2(numpy.ndarray): 位置2
戻り値
is_interference(bool): True/False = 干渉あり/干渉なし
"""
is_interference = True
# 2点の干渉判定
if self._is_interference_pos(pos2):
return is_interference
if self._is_interference_pos(pos1):
return is_interference
# pos1からpos2へ移動
theta = self._convert_pos_to_theta(pos2)
self._set_joint(theta)
# グリッパーの実行
self._run_gripper(open=True)
# 待機時間
time.sleep(self._SIMULATION_SLEEP_TIME)
# ロボットと干渉物との干渉判定
close_points = p.getClosestPoints(self._robot_id, self._environment_id, self._INTERFERENCE_MARGIN)
if len(close_points) == 0: # 干渉なし
is_interference = False
return is_interference
def _is_interference_start_end_pos(self, start_pos, end_pos):
"""
始点と終点が干渉判定していないかの確認
パラメータ
start_pos(numpy.ndarray): 経路生成の始点 (直交空間/関節空間)
end_pos(numpy.ndarray): 経路生成の終点 (直交空間/関節空間)
"""
# 始点の干渉判定
if self._is_interference_pos(start_pos):
raise ValueError("start_pos is interference. change start_pos.")
# 終点の干渉判定
if self._is_interference_pos(end_pos):
raise ValueError("end_pos is interference. change end_pos.")
def _is_interference_pos(self, pos):
"""
位置にジャンプして干渉判定
パラメータ
pos(numpy.ndarray): 位置/関節
戻り値
is_interference(bool): True/False = 干渉あり/干渉なし
"""
is_interference = True
# 位置から関節角度に変換
theta = self._convert_pos_to_theta(pos)
# print(f"theta = {theta}")
# 位置にジャンプ
self._set_jump_joint(theta)
# グリッパーの実行
self._run_gripper(open=True)
# 待機時間
time.sleep(self._SIMULATION_SLEEP_TIME)
# ロボットと干渉物との干渉判定
close_points = p.getClosestPoints(self._robot_id, self._environment_id, self._INTERFERENCE_MARGIN)
if len(close_points) == 0: # 干渉なし
is_interference = False
return is_interference
# 干渉判定処理 ↑
# 運動学関連 ↓
def convert_pos_to_theta(self, pos):
"""
位置から関節角度に変換 (クラス外で使う用)
パラメータ
pos(numpy.ndarray): 位置 / 関節角度
戻り値
thetas(numpy.ndarray): 関節角度
"""
if pos.shape[0] == DIMENTION_2D:
pos = np.append(pos, 0.0)
# エンドエフェクタのリンク要素はベースリンクを除いた要素番号となる
thetas = p.calculateInverseKinematics(self._robot_id, self._n_joints, pos)
thetas = np.array(thetas)[:-self._N_INVERSE_HAND_JOINT]
return thetas
def _convert_pos_to_theta(self, pos):
"""
位置から関節角度に変換 (クラス内で使う用)
パラメータ
pos(numpy.ndarray): 位置 / 関節角度
戻り値
thetas(numpy.ndarray): 関節角度
"""
if self._interpolation == INTERPOLATION.POSITION.value:
# 逆運動学により，関節角度を返す
# 逆運動学には，(x, y, z)の3次元データが必須なため，2次元の場合はzのデータも増やす
if pos.shape[0] == DIMENTION_2D:
pos = np.append(pos, 0.0)
# エンドエフェクタのリンク要素はベースリンクを除いた要素番号となる
thetas = p.calculateInverseKinematics(self._robot_id, self._n_joints, pos)
thetas = np.array(thetas)
else:
# pos が関節角度のため，そのまま返す
thetas = np.copy(pos)
return thetas
# 運動学関連 ↑
次に，上ソースコードより重要な箇所を抜粋して，説明していく．
上ソースコードの大方は前記事にて説明しているため，説明した内容は割愛する．
(前記事 https://qiita.com/haruhiro1020/items/01f661a571c6fe76c551)
拘束条件の設定 (_set_constraint)
拘束条件の設定の中身を説明する．
def _set_constraint(self, object_id, pos, ori):
"""
拘束条件の設定
パラメータ
object_id(int): 拘束したい対象物ID
pos(list): 拘束したい位置
ori(list): 拘束したい姿勢
戻り値
int: 拘束条件ID
"""
constraint_id = p.createConstraint(
object_id, # 親番号(拘束したい対象物ID)
-1, # 親リンクの要素番号("-1"はベース)
-1, # 子番号("-1"はなし)
-1, # 子リンクの要素番号("-1"はベース)
p.JOINT_FIXED, # 関節タイプ(今回は固定"JOINT_FIXED")
[0, 0, 0], # 関節軸
[0, 0, 0], # 親の中心からの位置
pos, # 子の中心からの位置 (今回は子を設定していないから，ワールド座標系から見た関節位置)
parentFrameOrientation=ori, # 親の中心からの姿勢
childFrameOrientation=[0, 0, 0, 1]) # 子の中心からの姿勢 (今回は，子を設定していないから，ワールド座標系から見た姿勢)
return constraint_id
経路探索中にロボットと把持対象物がぶつかっても，把持対象物が移動しないように拘束条件を追加した．拘束条件はPyBulletのcreateConstraint()関数を使用する．createConstraint()関数の引数および戻り値は公式ドキュメントを抜粋すると，下図の通りである．
上図の青枠が引数，赤枠が戻り値である．今回は位置と姿勢を変更しないような拘束条件を追加した．
把持対象物の位置を取得 (_get_grasp_pos)
把持対象物の位置を取得の中身を説明する．
def _get_grasp_pos(self, dim2=False):
"""
把持対象物の位置を取得
パラメータ
dim2(bool): 2次元位置として取得するかどうか
戻り値
list: 把持対象物の位置 ([rad] or [m])
list: 把持対象物の姿勢
"""
...
# 把持対象物の位置[m]を取得
grasp_pos, grasp_ori = p.getBasePositionAndOrientation(self._grasp_id)
...
今回定義した把持対象物のURDFファイルには，関節(joint)が定義されていない．リンクだけを定義している．リンクだけを定義したURDFファイルから，位置・姿勢を取得するには，PyBulletのgetBasePositionAndOrientation()を使用する必要がある．
経路生成後の余白時間の処理 (_exec_margin_time)
経路生成後の余白時間の処理の中身を説明する．
def _exec_margin_time(self, end_pos):
"""
経路生成後の余白時間の処理
パラメータ
end_pos(numpy.ndarray): 経路生成の終点
"""
# 長めの待機時間
time.sleep(self._SIMULATION_SLEEP_TIME * 10)
# PyBulletで関節角度を与えても，与えた関節角度ピッタリにはならず，数値誤差が発生するから，終点へ移動する処理を実施する
end_theta = self._convert_pos_to_theta(end_pos)
# 経路生成後の余白時間 (即座にPyBulletが終了するのを防ぐための余白時間)
for _ in range(self._N_MARGIN_MOVE):
# 終点に移動
self._set_joint(end_theta)
# グリッパーの実行
self._run_gripper(open=True)
# 待機時間
time.sleep(self._SIMULATION_SLEEP_TIME)
# 把持物体の拘束を解除
if self._grasp_constraint_id is not None:
self._release_constraint(self._grasp_constraint_id)
time.sleep(self._SIMULATION_SLEEP_TIME)
self._grasp_constraint_id = None
# 経路生成後の余白時間 (即座にPyBulletが終了するのを防ぐための余白時間)
for _ in range(self._N_MARGIN_MOVE):
# 終点に移動
self._set_joint(end_theta)
# グリッパーの実行
self._run_gripper(close=True)
# 待機時間
time.sleep(self._SIMULATION_SLEEP_TIME)
把持(グリッパーのクローズ)する時に，把持対象物の位置へロボットが正確に移動していないといけない．正確に移動させるために，終点に移動させる余白時間を用意した．
終点へ正確に移動させたら，把持物体が動けるように，拘束条件(把持対象物の位置・姿勢の固定化)を解除した．
把持物体の拘束条件を解除してから，把持を実行した．
経路生成手法であるRRT (pybullet_rrt.py)
経路生成手法であるRRTの処理を定義する pybullet_rrt.py について説明する．
RRT内ではツリーの作成を実施する．干渉判定はPyBulletで実施するため，RRT内には干渉判定を実装しない．
pybullet_rrt.py
# 経路生成手法であるRRT (Rapidly-exploring Random Tree) の実装 (PyBullet用)
# ライブラリの読み込み
import numpy as np
import os
# 自作モジュールの読み込み
from constant import * # 定数
class Tree:
"""
ツリークラス
プロパティ
_nodes(numpy.ndarray): ノード
_near_node_idx(int): _nodes内の最短ノードを保存している列番号
メソッド
public
nodes(): _nodesプロパティのゲッター
reset(): データの初期化
add_node(): ノードの追加
get_near_node(): 最短距離のノードを取得
get_near_node_list(): ノードと近傍ノードをリストで取得
protected
_chk_node_exist(): ノードが存在するかの確認
"""
# 定数の定義
def __init__(self, near_node_idx):
"""
コンストラクタ
"""
# プロパティの初期化
self._nodes = []
self._near_node_idx = near_node_idx
@property
def nodes(self):
"""
_nodesプロパティのゲッター
"""
return self._nodes
def reset(self):
"""
データの初期化
"""
if len(self._nodes) != 0:
# 何かしらのデータが保存
del self._nodes
self._nodes = []
def add_node(self, node):
"""
ノードの追加
パラメータ
node(numpy.ndarray): ノード
"""
if len(self._nodes) == 0: # 初回だけ実行
self._nodes = node
else:
self._nodes = np.append(self._nodes, node, axis=0)
def _chk_node_exist(self):
"""
ノードが存在するかの確認
"""
if len(self._nodes) == 0:
# 存在しない
raise ValueError("self._nodes is not exist")
def get_near_node(self, pos):
"""
最短距離のノードを取得
パラメータ
pos(numpy.ndarray): 位置
戻り値
min_dist_idx(int): 最短距離のノード番号
"""
# ノードの存在確認
self._chk_node_exist()
# ノードから位置を取得
nodes_pos = self._nodes[:, :self._near_node_idx]
# 差分を計算
difference = nodes_pos - pos
# 距離を計算 (各ノードとの距離を算出するため，引数にaxis=1を与えた)
distance = np.linalg.norm(difference, axis=1)
# 最短距離ノードを取得
min_dist_idx = np.argmin(distance)
return min_dist_idx
def get_near_node_list(self, pos, radius):
"""
ノードと近傍ノードをリストで取得
パラメータ
pos(numpy.ndarray): ノード位置
radius(float): 半径
戻り値
near_node_list(list): 近傍ノードリスト
"""
# ノードの存在確認
self._chk_node_exist()
near_node_list = []
# ツリー内全ノード位置を取得
all_node_pos = self._nodes[:, :self._near_node_idx]
# ノードとツリー内全ノードの差分を計算
difference = all_node_pos - pos
# 差分から距離(ユークリッド距離)を計算
distance = np.linalg.norm(difference, axis=1)
# 距離が一定以内のノードだけを保存
near_node_list = [idx for idx, dist in enumerate(distance) if dist <= radius]
return near_node_list
class RRTPyBullet:
"""
RRTにロボットを追加したクラス
プロパティ
_name(str): 経路生成手法名
_pathes(numpy.ndarray): 始点から終点までの経路 (PruningやShortcut済みの経路)
_start_tree(Tree): 始点ツリー
_strict_min_pos(numpy.ndarray): 探索の最小範囲
_strict_max_pos(numpy.ndarray): 探索の最大範囲
_moving_value(float): 1回あたりの移動量 [rad] or [m]
_before_modify_pathes(numpy.ndarray): PruningやShortcutなどの修正前の経路
_pruning_pathes(numpy.ndarray): Pruning後の経路
_shortcut_pathes(numpy.ndarray): Shortcut後の経路
_partial_shortcut_pathes(numpy.ndarray): Partial Shortcut後の経路
メソッド
public
プロパティのゲッター関連
name(): _nameプロパティのゲッター
pathes(): _pathesプロパティのゲッター
準備処理
preparation(): 経路生成の準備
メイン処理関連
expand_once(): ランダム探索による新規ノードおよび最短ノードの取得 (干渉判定は実装しない)
add_node_and_check_goal(): ノード追加 + 経路生成の完了確認
fin_planning(): 経路生成の終了処理
ファイル関連
save(): 生成した経路をファイル保存
protected
メイン処理関連
_add_node_start_tree(): 始点ツリーにノードを追加
_chk_end_pos_dist(): 終点との距離が一定範囲内であるかの確認
_calc_new_pos(): 最短ノードからランダムな値方向へ新しいノード(位置)を作成
_get_random_pos(): ランダムな位置を取得
準備処理関連
_reset(): データの初期化
_set_interpolation(): 経路生成したい探索空間の設定
_strict_planning_pos(): 探索範囲を制限する
ファイル関連
_make_folder(): フォルダーの作成
_reset_folder(): フォルダー内のファイルを全削除
_get_path_plan_folders(): 経路生成結果を保存する複数のフォルダー名を取得
_make_path_plan_folder(): 経路生成結果を保存するフォルダー作成
_save_numpy_data_to_txt(): Numpyデータをテキストファイルに保存
"""
# 定数の定義
# ファイル名の定義
# 各経路生成手法で絶対に定義するべきファイル名 ↓
# _pathesプロパティを保存するファイル名
_FILE_NAME_PATHES = f"{PATHPLAN.RRT.value}_pathes.csv"
# _start_treeプロパティを保存するファイル名
_FILE_NAME_START_TREE = f"{PATHPLAN.RRT.value}_start_tree.csv"
# 各経路生成手法で絶対に定義するべきファイル名 ↑
# ツリーの要素番号を定義
_NODE_NEAR_NODE_IDX = RRT_NEAR_NODE_IDX # ノード内の最短ノード要素
# 探索に関する定義
_MOVING_VALUE_JOINT = 0.1 # 1回の移動量 [rad] (ロボットの関節空間)
_MOVING_VALUE_POS = 0.2 # 1回の移動量 [m] (ロボットの位置空間)
_STRICT_PLANNING_ROB_JOINT = np.pi / 2 # 探索範囲の制限 [rad] (ロボットの関節空間)
_STRICT_PLANNING_ROB_POS = 1.0 # 探索範囲の制限 [m] (ロボットの位置空間)
_GOAL_SAMPLE_RATE = 0.1 # ランダムな値を取るときに，終点を選択する確率
def __init__(self):
"""
コンストラクタ
"""
self._name = PATHPLAN.RRT.value
self._pathes = []
self._start_tree = Tree(self._NODE_NEAR_NODE_IDX)
self._interpolation = INTERPOLATION.NONE.value
self._moving_value = self._MOVING_VALUE_JOINT
self._dim = DIMENTION_NONE
# プロパティのゲッターメソッド ↓
@property
def name(self):
"""
_nameプロパティのゲッター
"""
return self._name
@property
def pathes(self):
"""
_pathesプロパティのゲッター
"""
return self._pathes
# プロパティのゲッターメソッド ↑
# メイン処理メソッド ↓
def expand_once(self, end_pos):
"""
ランダム探索による新規ノードおよび最短ノードの取得 (干渉判定は実装しない)
パラメータ
end_pos(list): 経路生成の終点
戻り値
new_node_pos(numpy.ndarray): 新規ノード
near_node_pos(numpy.ndarray): 最短ノード位置
near_node(int): 最短ノード番号
"""
# ランダムな値を取得
random_pos = self._get_random_pos(end_pos)
# ランダムな値と最短ノードを計算
near_node = self._start_tree.get_near_node(random_pos)
# 最短ノードの位置
near_node_pos = self._start_tree.nodes[near_node, :self._NODE_NEAR_NODE_IDX]
# 最短ノードからランダムな値方向へ新しいノード(位置)を作成
new_node_pos = self._calc_new_pos(random_pos, near_node_pos)
return new_node_pos, near_node_pos, near_node
def add_node_and_chk_goal(self, end_pos, node_pos, near_node):
"""
ノード追加 + 経路生成の完了確認
パラメータ
end_pos(numpy.ndarray): 経路生成の終点
node_pos(numpy.ndarray): ノード位置
near_node(int): 親ノード
戻り値
is_successful(bool): True/False = 経路生成の完了/未完了
"""
# 処理結果
is_successful = False
# 始点ツリーにノードを追加
self._add_node_start_tree(node_pos, near_node)
# 終点との距離が一定範囲内であるかの確認
if self._chk_end_pos_dist(node_pos, end_pos):
# 一定範囲内のため，経路生成の完了
is_successful = True
return is_successful
def _add_node_start_tree(self, pos, near_node):
"""
始点ツリーにノードを追加
パラメータ
pos(numpy.ndarray): 位置
near_node(int): 最短ノード
"""
# _start_treeにノードを追加
node = np.append(pos, near_node).reshape(1, -1)
self._start_tree.add_node(node)
def _chk_end_pos_dist(self, pos, end_pos):
"""
終点との距離が一定範囲内であるかの確認
パラメータ
pos(numpy.ndarray): ノード位置
end_pos(numpy.ndarray): 経路生成の終点
戻り値
is_near(bool): True / False = 一定範囲内である / でない
"""
is_near = False
# 距離を計算
dist = np.linalg.norm(end_pos - pos)
# 一定範囲内であるかの確認
if dist <= self._moving_value:
is_near = True
return is_near
def fin_planning(self, start_pos, end_pos):
"""
経路生成の終了処理
パラメータ
start_pos(list): 経路生成の始点
end_pos(list): 経路生成の終点
"""
# 始点から終点までの経路に関係するノードを選択
revers_path = end_pos.reshape(1, -1)
near_node = -1
while True:
# 終点から始点方向へノードを取得
node = self._start_tree.nodes[near_node]
pos = node[:self._NODE_NEAR_NODE_IDX].reshape(1, -1)
# 浮動小数型になっているので，整数型に型変換
near_node = int(node[self._NODE_NEAR_NODE_IDX])
revers_path = np.append(revers_path, pos, axis=0)
if near_node == INITIAL_NODE_NEAR_NODE:
# 始点ノードまで取得できたため，処理終了
break
# 経路が終点からの順番になっているため，始点から終点とする
self._pathes = revers_path[::-1]
# ツリーに終点を追加 (要素番号を指定するため -1)
self._add_node_start_tree(end_pos, self._start_tree.nodes.shape[0] - 1)
def _calc_new_pos(self, random_pos, near_node_pos):
"""
最短ノードからランダムな値方向へ新しいノード(位置)を作成
パラメータ
random_pos(numpy.ndarray): ランダムな位置
near_node_pos(numpy.ndarray): 最短ノード位置
戻り値
new_pos(numpy.ndarray): 新しいノード
"""
# 方向を計算
direction = random_pos - near_node_pos
# 方向の大きさを1にする
norm_direction = direction / (np.linalg.norm(direction) + EPSILON)
# 新しいノードを作成
new_pos = near_node_pos + norm_direction * self._moving_value
return new_pos
def _get_random_pos(self, target_pos):
"""
ランダムな位置を取得
パラメータ
target_pos(numpy.ndarray): 目標点
戻り値
random_pos(numpy.ndarray): ランダムな位置
"""
# 乱数を取って，目標点を選択するかランダムを選択するか
select_goal = np.random.rand()
if select_goal < self._GOAL_SAMPLE_RATE:
# 目標点を選択s
random_pos = target_pos
else:
random_pos = np.random.uniform(self._strict_min_pos, self._strict_max_pos)
return random_pos
# メイン処理メソッド ↑
# 準備処理関連メソッド ↓
def preparation(self, start_pos, end_pos, interpolation):
"""
経路生成の準備
パラメータ
start_pos(list): 経路生成の始点
end_pos(list): 経路生成の終点
interpolation(int): 補間方法 (関節空間/位置空間)
"""
# データの初期化
self._reset()
# 始点と終点の次元数が一致しているかの確認
start_pos_dim = np.size(start_pos)
end_pos_dim = np.size(end_pos)
if start_pos_dim != end_pos_dim:
# 次元数が異なるので異常
raise ValueError(f"start_pos_dim and end_pos_dim are not matched. start_pos_dim is {start_pos_dim}, end_pos_dim is {end_pos_dim}")
self._dim = start_pos_dim
# 探索空間の設定
self._set_interpolation(interpolation)
# 始点ノードをツリーに追加
self._add_node_start_tree(start_pos, INITIAL_NODE_NEAR_NODE)
# 探索範囲を設定
self._strict_planning_pos(start_pos, end_pos)
# 結果を保存するフォルダ作成
self._make_path_plan_folder(interpolation)
# フォルダー内のファイルを全部削除
self._reset_folder(interpolation)
def _reset(self):
"""
データの初期化
"""
self._start_tree.reset()
if len(self._pathes) != 0:
# 何かしらのデータが保存
del self._pathes
self._pathes = []
self._interpolation = INTERPOLATION.NONE.value
self._dim = DIMENTION_NONE
def _set_interpolation(self, interpolation):
"""
経路生成したい探索空間の設定
パラメータ
interpolation(int): 補間の種類 (関節補間/位置補間)
"""
if interpolation == INTERPOLATION.POSITION.value:
# 位置空間
self._moving_value = self._MOVING_VALUE_POS
elif interpolation == INTERPOLATION.JOINT.value:
# 関節空間
self._moving_value = self._MOVING_VALUE_JOINT
else:
# 異常値
raise ValueError(f"interpolation is abnormal. interpolation is {interpolation}")
# 補間種類の更新
self._interpolation = interpolation
def _strict_planning_pos(self, start_pos, end_pos):
"""
探索範囲を制限する
パラメータ
start_pos(numpy.ndarray): 始点
end_pos(numpy.ndarray): 終点
"""
all_pos = np.array([start_pos, end_pos])
# 各列の最大/最小値を取得
min_pos = np.min(all_pos, axis=0)
max_pos = np.max(all_pos, axis=0)
if self._interpolation == INTERPOLATION.POSITION:
# 位置空間の探索
strict_planning_pos = self._STRICT_PLANNING_ROB_POS
else:
# 関節空間の探索
strict_planning_pos = self._STRICT_PLANNING_ROB_JOINT
self._strict_min_pos = min_pos - strict_planning_pos
self._strict_max_pos = max_pos + strict_planning_pos
# print(f"self._strict_min_pos = {self._strict_min_pos}")
# print(f"self._strict_max_pos = {self._strict_max_pos}")
# 準備処理関連メソッド ↑
# ファイル関連メソッド ↓
def _make_folder(self, folder_name):
"""
フォルダーの作成
パラメータ
folder_name(str): 作成したいフォルダー名
"""
# フォルダーが作成済みでもエラーを出力しないよう，exist_ok=Trueとした．
os.makedirs(folder_name, exist_ok=True)
def _reset_folder(self, interpolation):
"""
フォルダー内のファイルを全削除
パラメータ
interpolation(str): 探索方法 (位置空間/関節空間)
"""
# フォルダー名を取得
folder_names = self._get_path_plan_folders(interpolation)
for folder_name in folder_names:
for entry in os.listdir(folder_name):
full_path = os.path.join(folder_name, entry)
if os.path.isfile(full_path) or os.path.islink(full_path):
os.remove(full_path) # 通常ファイル・シンボリックリンクを削除
def _get_path_plan_folders(self, interpolation):
"""
経路生成結果を保存する複数のフォルダー名を取得
パラメータ
interpolation(str): 探索方法 (位置空間/関節空間)
戻り値
folder_names(list): 複数のフォルダー名
"""
folder_names = [os.path.join(self._name, interpolation), ]
return folder_names
def _make_path_plan_folder(self, interpolation):
"""
経路生成結果を保存するフォルダー作成
パラメータ
interpolation(str): 探索方法 (位置空間/関節空間)
"""
# フォルダー名を取得
folder_names = self._get_path_plan_folders(interpolation)
for folder_name in folder_names:
self._make_folder(folder_name)
def save(self):
"""
生成した経路をファイル保存
"""
# 始点から終点までの修正済みの経路をファイル保存
self._save_numpy_data_to_txt(self._pathes, self._FILE_NAME_PATHES)
# 始点のツリーを保存
self._save_numpy_data_to_txt(self._start_tree.nodes, self._FILE_NAME_START_TREE)
def _save_numpy_data_to_txt(self, data, file_name):
"""
Numpyデータをテキストファイルに保存
パラメータ
data(numpy.ndarray): ファイル保存したいデータ
file_name(str): 保存したいファイル名
"""
# 引数の確認
if len(data) == 0:
# データが存在しないため，処理終了
return
if not file_name:
# ファイル名が存在しないため，処理終了
return
# ファイル名にフォルダ名を追加 (各経路生成手法で異なるフォルダにデータを保存)
full_path = f"{self._name}/{self._interpolation}/{file_name}"
np.savetxt(full_path, data)
# ファイル関連メソッド ↑
RRT内で干渉判定を実施できないため，RRTクラスの実装側で干渉判定の結果に応じて，処理を実装できるようにpubic関数を用意した．
RRTクラスの実装側で，呼んでほしくない処理はprotected関数として，public関数を最小限とした．
今回は，RRTによる経路生成だが将来的にはRRTの拡張アルゴリズムを使用する予定である．
RRTに関しては，私の他記事内やコメントを記載したため，各関数についての説明は割愛する．
PyBulletでロボットを動かす
上記にて，ソースコードを説明した．
main.py ファイルを実施することによって，PyBullet上のロボットを動かしていく．
直交空間で探索した時のロボットの動画を下図に2つ載せる．
動画１：始点から終点まで干渉しない経路を生成するための探索
動画２：探索後の始点から終点までの経路 + 物体把持
把持したい物体まで，干渉しない経路を作成して，物体把持ができることを確認した．物体把持まで自動化できたため，次なる目標は下記の通りとなる．
目標１：カメラを付けて，把持したい物体の位置を取得
目標２：物体把持後に他の場所へ物体を移動 (複数本の経路を生成)
目標３：強化学習の実装
おわりに
本記事では，Pythonを使用して，下記内容を実装しました
・PyBullet で干渉物が存在する環境下で，2軸ロボットアームの経路生成と把持 (経路生成手法としてRRTを採用)
次記事では，下記内容を実装していきます．
・PyBullet で干渉物が存在する環境下で，3軸ロボットアームの経路生成と把持 (経路生成手法としてRRTを採用)
(https://qiita.com/haruhiro1020/items/d26009d50be190d97368#%E3%81%8A%E3%82%8F%E3%82%8A%E3%81%AB)