はじめに
PyBullet (Python上で動く物理シミュレータ) を使用して，ロボットアームを可視化して，動かしたい．
前記事では，PyBullet (Python上で動く物理シミュレータ) で2軸ロボットアームを可視化し，干渉物が存在する環境下での経路生成を実装した．
(https://qiita.com/haruhiro1020/items/cc29d72d291158fdc1ce)
本記事では，下図のような2軸ロボットアームにグリッパーを追加して，物体を把持する(下図はPyBullet上のグリッパー付き2軸ロボットアームである)．赤枠がグリッパー，緑枠が把持したい物体である．
本記事では，ロボットアームをキーボードで動かすことによって，物体を把持できるようにした．以前の記事ではスライダーを使用して動かしていたが，把持に向いていないため，入力装置にキーボードを採用した．実際に動かしている動画は下図の通りである．
本記事で実装すること
・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を作成するのが困難であるため，採用しない．
グリッパー付き2軸ロボットアームのURDF作成
グリッパー付き2軸ロボットアームのURDFを作成する．下記がURDFの中身である．
robot_2dof_hand.urdf
<? xml version="1.0" ?>
<robot name="robot_2dof_hand">
<!-- 色を先に定義 (共通化したいから) ↓ -->
<material name="black">
<color rgba="0 0 0 1"/>
</material>
<!-- 色を先に定義 (共通化したいから) ↑ -->
<!-- ベースリンク -->
<link name="base_link">
<!-- 見た目と干渉判定なし -->
</link>
<!-- ベースリンクとリンク1間の関節 -->
<joint name="joint1" type="revolute">
<parent link="base_link"/>
<child link="link1"/>
<origin xyz="0 0 0" rpy="0 0 0"/>
<axis xyz="0 0 1"/>
<limit lower="-3.14" upper="3.14" effort="1" velocity="1"/>
</joint>
<!-- リンク1 -->
<link name="link1">
<!-- 見た目 -->
<visual>
<origin xyz="0.5 0.0 0.0"/>
<geometry>
<box size="1.0 0.2 0.2"/>
</geometry>
</visual>
<!-- 干渉判定 -->
<collision>
<origin xyz="0.5 0.0 0.0"/>
<geometry>
<box size="1.0 0.2 0.2"/>
</geometry>
</collision>
<!-- 慣性行列 -->
<inertial>
<origin xyz="0.5 0.0 0.0"/>
<mass value="1.0" />
<inertia ixx="0.0067" iyy="0.0867" izz="0.0867" ixy="0.0" ixz="0.0" iyz="0.0"/>
</inertial>
</link>
<!-- リンク1とリンク2間の関節 -->
<joint name="joint2" type="revolute">
<parent link="link1"/>
<child link="link2"/>
<origin xyz="1.0 0 0" rpy="0 0 0"/>
<axis xyz="0 0 1"/>
<limit lower="-3.14" upper="3.14" effort="1" velocity="1"/>
</joint>
<!-- リンク2 -->
<link name="link2">
<!-- 見た目 -->
<visual>
<origin xyz="0.5 0.0 0.0"/>
<geometry>
<box size="1.0 0.2 0.2"/>
</geometry>
</visual>
<!-- 干渉判定 -->
<collision>
<origin xyz="0.5 0.0 0.0"/>
<geometry>
<box size="1.0 0.2 0.2"/>
</geometry>
</collision>
<!-- 慣性行列 -->
<inertial>
<origin xyz="0.5 0.0 0.0"/>
<mass value="1.0" />
<inertia ixx="0.0067" iyy="0.0867" izz="0.0867" ixy="0.0" ixz="0.0" iyz="0.0"/>
</inertial>
</link>
<!-- リンク2とグリッパー間の関節 -->
<joint name="gripper_joint" type="fixed">
<parent link="link2"/>
<child link="gripper_base_link"/>
<origin xyz="1.0 0 0" rpy="0 0 0"/>
</joint>
<!-- グリッパーのベース部分 -->
<!-- グリッパーはT字型とする -->
<!-- T字型の縦棒作成 -->
<link name="gripper_base_link">
<!-- 見た目 -->
<visual>
<origin xyz="0.05 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.1 0.1 0.2"/>
</geometry>
<material name="black"/>
</visual>
<!-- 干渉判定 -->
<visual>
<origin xyz="0.05 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.1 0.1 0.2"/>
</geometry>
<material name="black"/>
</visual>
<!-- 慣性 -->
<inertial>
<mass value="0.05"/>
<origin xyz="0.05 0 0" rpy="0 0 0"/>
<inertia ixx="0.000208" iyy="0.000208" izz="0.000083" ixy="0.0" ixz="0.0" iyz="0.0"/>
</inertial>
</link>
<!-- グリッパーとフィンガー間の関節 -->
<joint name="finger_joint" type="fixed">
<parent link="gripper_base_link"/>
<child link="gripper_finger_link"/>
<origin xyz="0.1 0 0" rpy="0 0 0"/>
</joint>
<!-- T字型の横棒作成 -->
<link name="gripper_finger_link">
<!-- 見た目 -->
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.1 0.2 0.2"/>
</geometry>
<material name="black"/>
</visual>
<!-- 干渉判定 -->
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.1 0.2 0.2"/>
</geometry>
</collision>
<!-- 慣性 -->
<inertial>
<mass value="0.1"/>
<origin xyz="0 0 0" rpy="0 0 0"/>
<inertia ixx="0.000667" iyy="0.000417" izz="0.000417" ixy="0.0" ixz="0.0" iyz="0.0"/>
</inertial>
</link>
<!-- フィンガーと右フィンガー間の関節 -->
<joint name="right_finger_joint" type="prismatic">
<parent link="gripper_finger_link"/>
<child link="right_finger_link"/>
<origin xyz="0.05 -0.095 0" rpy="0 0 0"/>
<axis xyz="0 1 0"/>
<limit lower="0.0" upper="0.090" effort="100.0" velocity="1.0"/>
</joint>
<!-- 右フィンガー -->
<link name="right_finger_link">
<!-- 見た目 -->
<visual>
<origin xyz="0.15 0.0 0.0"/>
<geometry>
<box size="0.3 0.01 0.2"/>
</geometry>
<material name="black"/>
</visual>
<!-- 干渉判定 -->
<collision>
<origin xyz="0.15 0.0 0.0"/>
<geometry>
<box size="0.3 0.01 0.2"/>
</geometry>
</collision>
<!-- 慣性行列 -->
<inertial>
<origin xyz="0.15 0.0 0.0"/>
<mass value="0.015"/>
<inertia ixx="0.000051" iyy="0.000113" izz="0.000163" ixy="0.0" ixz="0.0" iyz="0.0"/>
</inertial>
</link>
<!-- ハンドベースと左フィンガー間の関節 -->
<joint name="left_finger_joint" type="prismatic">
<parent link="gripper_finger_link"/>
<child link="left_finger_link"/>
<origin xyz="0.05 0.095 0" rpy="0 0 0"/>
<axis xyz="0 1 0"/>
<limit lower="-0.090" upper="0.0" effort="100.0" velocity="1.0"/>
</joint>
<!-- 左グリッパー -->
<link name="left_finger_link">
<!-- 見た目 -->
<visual>
<origin xyz="0.15 0.0 0.0"/>
<geometry>
<box size="0.3 0.01 0.2"/>
</geometry>
<material name="black"/>
</visual>
<!-- 干渉判定 -->
<collision>
<origin xyz="0.15 0.0 0.0"/>
<geometry>
<box size="0.3 0.01 0.2"/>
</geometry>
</collision>
<!-- 慣性行列 -->
<inertial>
<origin xyz="0.15 0.0 0.0"/>
<mass value="0.015"/>
<inertia ixx="0.000051" iyy="0.000113" izz="0.000163" ixy="0.0" ixz="0.0" iyz="0.0"/>
</inertial>
</link>
<!-- フィンガーと先端 (エンドエフェクター) 間の関節 -->
<joint name="ee_joint" type="fixed">
<parent link="gripper_finger_link"/>
<child link="ee_link"/>
<origin xyz="0.35 0 0" rpy="0 0 0"/>
</joint>
<!-- グリッパー先端 (エンドエフェクター) -->
<link name="ee_link">
<!-- 見た目，干渉判定，慣性はなし -->
</link>
</robot>
"gripper_base_link" 以降のリンクと関節がグリッパーに関する内容である．URDFのタグに関しては，前記事にて説明したため，割愛する．グリッパーなしのURDFファイルも前記事にて説明した．
(前記事 https://qiita.com/haruhiro1020/items/ecc5215234350b663770)
把持対象物(立方体)の定義
本記事で把持したい，立方体について説明する．
把持対象物(立方体)のURDF作成
把持対象物(立方体)のURDFを作成する．下記がURDFの中身である．
grasp_object.urdf
<?xml version="1.0"?>
<robot name="grasp_cube">
<link name="cube_link">
<!-- 見た目 -->
<visual>
<origin xyz="0 -2.2 0.05" rpy="0 0 0"/>
<geometry>
<box size="0.1 0.1 0.1"/>
</geometry>
</visual>
<!-- 干渉判定 -->
<collision>
<origin xyz="0 -2.2 0.05" rpy="0 0 0"/>
<geometry>
<box size="0.1 0.1 0.1"/>
</geometry>
</collision>
<!-- 慣性 -->
<inertial>
<origin xyz="0 -2,2 0.05" 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>
PyBulletの使用方法
Pythonの物理シミュレータであるPyBulletの使用方法について説明する．
下記リンクのPyBullet公式で調べながら，PyBulletを使用している．
・https://pybullet.org/wordpress/
上記リンクを開くと，下図のようなサイトに飛ぶ．使用方法を調べるときは，下図の赤枠内の「PYBULLET QUICKSTART GUIDE」タグをクリックする．
「PYBULLET QUICKSTART GUIDE」タグをクリックすると，下図のようなドキュメントを見ることができる．基本的には，ドキュメントに記載されている関数の使用方法を見て，ソースコードを作成している．
PyBulletの関数の引数や戻り値をもっと知りたいのでしたら，上記ドキュメントを見た方がわかりやすいです．
キーボードの割り当て
本記事では，キーボードを使用してロボットアームを動かしていく．キーボードの割り当てに関して，説明する．
キーボードの割り当ては，下表の通りである．
| キー | 内容 |
|---|---|
| d | +X方向へ移動 |
| a | -X方向へ移動 |
| z | +Y方向へ移動 |
| x | -Y方向へ移動 |
| f | グリッパーのオープン |
| r | グリッパーのクローズ |
本記事では，2軸ロボットアームであるため，Z方向への移動や姿勢に関しては実装していない．
全体のソースコード
はじめに，本記事で使用する全体のソースコードについて説明する．
その後，重要な箇所を抜粋して別途解説をしていく．
ソースコードとして，下記の3ファイルを作成する．
・定数の定義 (constant.py)
・全体的なメイン処理 (main.py)
・PyBulletでロボットを動かす (pybullet_robot.py)
各ファイルの中身を説明する．
定数の定義 (constant.py)
定数を定義する constant.py について説明する．
本記事では，キーボードを使用してロボットを動かす．そのため，ロボットの手先位置とキーボードの関係を定義している．
constant.py
# 複数ファイルで使用する定数の定義
from enum import Enum
from enum import auto
# 次元数を定義
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による経路生成
# ロボット手先位置のキーボード割り当て
class KEYBOARD(Enum):
"""
キーボード割り当て
(ord()により，文字からUnicode番号へ変換)
"""
PLUS_X = ord("d") # +x方向へ移動
MINUS_X = ord("a") # -x方向へ移動
PLUS_Y = ord("z") # +y方向へ移動
MINUS_Y = ord("x") # -y方向へ移動
PLUS_Z = ord("e") # +z方向へ移動
MINUS_Z = ord("q") # -z方向へ移動
PLUS_ROLL = ord("d") # +Roll(X)方向へ移動
MINUS_ROLL = ord("a") # -Roll(X)方向へ移動
PLUS_PITCH = ord("w") # +Pitch(Y)方向へ移動
MINUS_PITCH = ord("s") # -Pitch(Y)方向へ移動
PLUS_YAW = ord("e") # +YAW(Z)方向へ移動
MINUS_YAW = ord("q") # -YAW(Z)方向へ移動
GRIP_OPEN = ord("f") # グリッパーのオープン
GRIP_CLOSE = ord("r") # グリッパーのクローズ
PyBulletからキーボードの押下状態を取得するためには，Unicode番号にしないといけないから，KEYBOARDの定数を作成した．
全体的なメイン処理 (main.py)
全体的なメイン処理を定義する main.py について説明する．
後ほど説明する pybullet_robot.py 内のクラスを実装するのがメインな処理である．
main.py
# Pybullet (Pythonでの3次元物理シミュレータ) による2軸ロボットアームの可視化
# 標準ライブラリの読み込み
# 自作モジュールの読み込み
from pybullet_robot import MainPyBulletRobot
from constant import *
HAND_FLG = True # ハンドの装着有無
def main():
"""
メイン処理
"""
# ロボットが保存されている URDF ファイル名
if HAND_FLG:
# ハンド付きのURDF
robot_urdf = "robot_2dof_hand.urdf"
else:
# ハンドなしのURDF
robot_urdf = "robot_2dof.urdf"
# 把持対象物が保存されている URDF ファイル名
grasp_urdf = "grasp_object.urdf"
# 探索空間を指定
interpolation = INTERPOLATION.POSITION.value # 直交空間の探索
# PyBulletを使用するインスタンス作成
my_robot = MainPyBulletRobot(interpolation, robot_urdf, grasp_urdf=grasp_urdf, hand=HAND_FLG)
# PyBullet上のロボットを動かす
result = my_robot.run()
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 *
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 # グリッパーの摩擦係数
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):
"""
実行 (毎時刻，本関数を呼ぶこと)
"""
# キーボードの押下状況を取得
keys = p.getKeyboardEvents()
# グリッパーの現在の関節角度[m]を取得
joint_values = self._get_joint_values()
# オープンとクローズが同時実行の時，安全の観点よりクローズよりもオープンを優先
if self._chk_down_key(KEYBOARD.GRIP_OPEN.value, keys):
# グリッパーのオープンに関するキーボードが押下された時
# 移動方向を取得して，設定したい関節角度[m]を計算
direction = self._get_move_direction(open=True)
joint_values += direction * self._GRIPPER_MOVE_VAL
elif self._chk_down_key(KEYBOARD.GRIP_CLOSE.value, keys):
# グリッパーのクローズに関するキーボードが押下された時
direction = self._get_move_direction(open=False)
joint_values += direction * self._GRIPPER_MOVE_VAL
else:
# 押下されていないため，何もしない
pass
# 関節角度の設定
self._set_joint_values(joint_values)
def _chk_down_key(self, key, keys):
"""
特定のキーが押下されているかの確認
パラメータ
key(int): 特定のキー
keys(list): キーボード情報
戻り値
bool: True/False = 押下されている/されていない
"""
is_down = False
# 押下確認
if key in keys and keys[key] & p.KEY_IS_DOWN:
is_down = True
return is_down
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. / 240. # シミュレーションの待機時間 [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 = 100 # 経路生成終了後の余白時間 [回]
_N_HAND_JOINT = 4 # ハンド用の関節数
_KEY_DOWN_MOVE_POS = 0.1 # キーボード押下時のロボット手先位置の移動量 [m]
_KEY_DOWN_MOVE_ORI = 0.05 # キーボード押下時のロボット手先姿勢の移動量 [rad]
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)
# 事前準備メソッド ↓
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
if grasp_urdf is not None:
self._grasp_id = p.loadURDF(grasp_urdf, basePosition=[0, 0, 0.1])
# 把持対象物に摩擦を付与する
p.changeDynamics(self._grasp_id, # 把持対象物ID
-1, # ベースに対して
lateralFriction=1.0, # 床との摩擦係数
spinningFriction=1.0, # 回転摩擦係数
rollingFriction=1) # 転がり摩擦
# 事前準備メソッド ↑
# メイン処理 ↓
def run(self):
"""
実行
始点から終点まで，干渉しない経路を生成
"""
# リアルタイムでのシミュレーション
p.setRealTimeSimulation(1)
while True:
# 新しい位置を取得
new_pos = self._calc_new_pos()
# 回転角度に変換 (ロボットへ渡せるのは関節角度だけだから)
thetas = self._convert_pos_to_theta(new_pos)
# 関節角度を設定
self._set_joint(thetas)
# グリッパーの実行
self._run_gripper()
def _calc_new_pos(self):
"""
新しい位置を計算
戻り値
numpy.ndarray: 新しい位置
"""
# ロボットの手先位置を取得
ee_pos = np.array(self._get_ee_state()[0])
# キーボードの値を取得
keys = p.getKeyboardEvents()
# x方向への移動量を取得
x_value = self._get_move_pos(KEYBOARD.PLUS_X.value, KEYBOARD.MINUS_X.value, keys)
ee_pos[0] += x_value
# y方向への移動量を取得
y_value = self._get_move_pos(KEYBOARD.PLUS_Y.value, KEYBOARD.MINUS_Y.value, keys)
ee_pos[1] += y_value
return ee_pos
def _get_move_pos(self, plus_key, minus_key, keys):
"""
位置の移動量を取得
パラメータ
plus_key(int): +方向を割り当てたキーボード
minus_key(int): -方向を割り当てたキーボード
keys(list): キーボード情報
戻り値
float: 特定方向への移動量
"""
value = 0.0
if self._chk_down_key(plus_key, keys):
# +x方向へ移動
value += self._KEY_DOWN_MOVE_POS
if self._chk_down_key(minus_key, keys):
# -x方向へ移動
value -= self._KEY_DOWN_MOVE_POS
return value
def _chk_down_key(self, key, keys):
"""
特定のキーが押下されているかの確認
パラメータ
key(int): 特定のキー
keys(list): キーボード情報
戻り値
bool: True/False = 押下されている/されていない
"""
is_down = False
# 押下確認
if key in keys and keys[key] & p.KEY_IS_DOWN:
is_down = True
return is_down
def _get_ee_state(self):
"""
エンドエフェクタの状態を取得
戻り値
list: エンドエフェクタの状態
要素0: ワールド座標系から見た，エンドエフェクタの重心位置
要素1: ワールド座標系から見た，エンドエフェクタの重心姿勢
要素2: エンドエフェクタの座標系から見た，エンドエフェクタの重心位置
要素3: エンドエフェクタの座標系から見た，エンドエフェクタの重心姿勢
要素4: ワールド座標系から見た，エンドエフェクタの座標系位置
要素5: ワールド座標系から見た，エンドエフェクタの座標系姿勢
要素6: ワールド座標系から見た，エンドエフェクタの速度
要素7: ワールド座標系から見た，エンドエフェクタの角速度
"""
# エンドエフェクタの位置にテキストを出力したい
ee_state = p.getLinkState(self._robot_id, self._n_joints)
return ee_state
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)
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):
"""
グリッパーの実行
"""
if self._hand is None:
# ハンド非装着のため，処理終了
return
self._hand.run()
# メイン処理 ↑
# 運動学関連 ↓
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)
ダイナミクスの変更 (_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) # 床との摩擦係数
PyBullet上でグリッパーより把持物体を把持するために，摩擦係数を定義する必要がある．そのために，PyBulletのchangeDynamics()を使用して，グリッパーのダイナミクスを変更する．
摩擦係数を試行錯誤した結果，上記がうまくいった．
実行 (毎時刻，本関数を呼ぶこと) (run)
実行 (毎時刻，本関数を呼ぶこと)の中身を説明する．
def run(self):
"""
実行 (毎時刻，本関数を呼ぶこと)
"""
# キーボードの押下状況を取得
keys = p.getKeyboardEvents()
# グリッパーの現在の関節角度[m]を取得
joint_values = self._get_joint_values()
# オープンとクローズが同時実行の時，安全の観点よりクローズよりもオープンを優先
if self._chk_down_key(KEYBOARD.GRIP_OPEN.value, keys):
# グリッパーのオープンに関するキーボードが押下された時
# 移動方向を取得して，設定したい関節角度[m]を計算
direction = self._get_move_direction(open=True)
joint_values += direction * self._GRIPPER_MOVE_VAL
elif self._chk_down_key(KEYBOARD.GRIP_CLOSE.value, keys):
# グリッパーのクローズに関するキーボードが押下された時
direction = self._get_move_direction(open=False)
joint_values += direction * self._GRIPPER_MOVE_VAL
else:
# 押下されていないため，何もしない
pass
# 関節角度の設定
self._set_joint_values(joint_values)
グリッパーの開閉以外では，角度をそのままPyBulletのグリッパーに与える．
試行錯誤した結果，グリッパーの開閉以外でもグリッパーに角度を与えないと上手く動かなかったため，実装している．
環境の初期化 (_init_environment)
環境の初期化の中身を説明する．
def _init_environment(self, environment_urdf, grasp_urdf):
...
# 把持対象物を読み込む
self._grasp_id = None
if grasp_urdf is not None:
self._grasp_id = p.loadURDF(grasp_urdf, basePosition=[0, 0, 0.1])
# 把持対象物に摩擦を付与する
p.changeDynamics(self._grasp_id, # 把持対象物ID
-1, # ベースに対して
lateralFriction=1.0, # 床との摩擦係数
spinningFriction=1.0, # 回転摩擦係数
rollingFriction=1) # 転がり摩擦
把持対象物に摩擦係数を設定しないと，PyBullet上で滑ってしまう．摩擦係数を試行錯誤した．
新しい位置を計算 (_calc_new_pos)
新しい位置を計算の中身を説明する．
def _calc_new_pos(self):
"""
新しい位置を計算
戻り値
numpy.ndarray: 新しい位置
"""
# ロボットの手先位置を取得
ee_pos = np.array(self._get_ee_state()[0])
# キーボードの値を取得
keys = p.getKeyboardEvents()
# x方向への移動量を取得
x_value = self._get_move_pos(KEYBOARD.PLUS_X.value, KEYBOARD.MINUS_X.value, keys)
ee_pos[0] += x_value
# y方向への移動量を取得
y_value = self._get_move_pos(KEYBOARD.PLUS_Y.value, KEYBOARD.MINUS_Y.value, keys)
ee_pos[1] += y_value
return ee_pos
ロボットアームをキーボードで動かすために，キーボード情報を取得する必要がある．キーボード情報は getKeyboardEvents() で取得できる．キーボードが押下されているかどうかを self._get_move_pos() 内部で実装してる．
PyBulletでロボットを動かす
上記にて，ロボットのURDF，ソースコードを説明した．
main.py ファイルを実施することによって，PyBullet上のロボットを動かしていく．
実際に動かして，物体を把持した動画を下図に記載した．
キーボードで特定のキーを押下することで，ロボットが動き，グリッパーの回避もできた．
また，把持物体やグリッパーのパラメータ(摩擦係数など)を調整することで，シミュレータ上でも物体把持ができる．
今回は，干渉物が存在しない環境下で，手動でロボットアームを動かしたが，次回は干渉物が存在する環境下で，RRTによる経路生成と把持を合わせる．
おわりに
本記事では，Pythonを使用して，下記内容を実装しました
・PyBullet で2軸ロボットアームにグリッパーを追加
・PyBullet で立方体の物体を把持
次記事では，下記内容を実装していきます．
・PyBullet で干渉物が存在する環境下で，2軸ロボットアームの経路生成と把持 (経路生成手法としてRRTを採用)