PyBulletで6軸ロボットアームを動かす Part2 (手先位置を自由に設定)

はじめに

PyBullet (Python上で動く物理シミュレータ) を使用して，ロボットアームを可視化して，動かしたい．
前記事では，PyBullet (Python上で動く物理シミュレータ) で6軸ロボットアームを可視化し，スライダーを用いて関節角度を簡単に変更できるようにした．
(https://qiita.com/haruhiro1020/items/75d95e1628fdea0743f4)
本記事では，下図のような6軸ロボットアームを動かす(下図はPyBullet上の6軸ロボットアームである)．
下図の赤枠内のスライダーを動かすことによって，ロボットの手先位置を自由に設定して，ロボットを動かす．

本記事で実装すること

・PyBullet で6軸ロボットアームの手先位置を設定することで，動かす

本記事では実装できないこと (将来実装したい内容)

・PyBullet で6軸ロボットアームを使用して，干渉物が存在する環境下での経路生成(経路生成手法として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)を作成する必要がある．

6軸ロボットアームのURDF作成

PyBulletで，6軸ロボットアームを使用するためのURDF作成に関しては，前記事にて説明したため，割愛する．
(前記事 https://qiita.com/haruhiro1020/items/75d95e1628fdea0743f4)

PyBulletの使用方法

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

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

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

全体のソースコード

はじめに，本記事で使用する全体のソースコードについて説明する．
その後，重要な箇所を抜粋して別途解説をしていく．
ソースコードとして，下記の3ファイルを作成する．
・定数の定義 (constant.py)
・全体的なメイン処理 (main.py)
・PyBulletでロボットを動かす (pybullet_robot.py)

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

定数の定義 (constant.py)

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

constant.py

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


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


# 重力
GRABITY_VALUE   = 9.81


# 0割を防ぐための定数
EPSILON         = 1e-6


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

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

全体的なメイン処理を定義する main.py について説明する．
後ほど説明する pybullet_robot.py 内のクラスを実装するのがメインな処理である．
main.py 内で使用したいロボットを定義する．

main.py

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


# 標準ライブラリの読み込み


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



def main():
    """
    メイン処理
    """
    # ロボットが保存されている URDF ファイル名
    # robot_urdf = "robot_2dof.urdf"      # 2軸ロボットアーム
    # robot_urdf = "robot_3dof.urdf"      # 3軸ロボットアーム
    robot_urdf = "robot_6dof.urdf"      # 6軸ロボットアーム

    # 探索空間を指定
    # interpolation = INTERPOLATION.JOINT.value     # 関節空間の探索
    interpolation = INTERPOLATION.POSITION.value    # 直交空間の探索

    # Pybulletを使用するインスタンス作成
    my_robot = MainPyBulletRobot(robot_urdf, interpolation)
    my_robot.run()


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 MainPyBulletRobot:
    """
    PyBulletのメインクラス

    プロパティ
        _robot_id(): ロボットアームのID番号
        _n_joints(int): ロボットアームの関節数
        _interpolation(str): 探索空間 (直交空間/関節空間)

    メソッド
        public

            メイン処理関連
                run(): 実行 (スライダー内の値を取得して，シミュレータ上のロボットを動かす)


        protected
        
            事前準備関連
                _init_robot(): ロボットの初期化
                _init_environment(): 環境の初期化
                _init_sliders(): スライダーの初期化
                _get_joint_limit(): 関節の限界値 (最小値 + 最大値) を取得
                _get_init_thetas(): 初期角度を取得
                _get_pose_limit(): 位置の限界値 (最小値 + 最大値) を取得
                _get_init_pose(): 初期位置を取得

            メイン処理関連
                _convert_pos_to_theta(): 手先位置から関節角度に変換
                _get_slider_values(): スライダー内の値を取得
                _set_joint(): 関節角度の設定
                _set_text(): GUIにテキストを設定
    """
    # 定数の定義
    _PLANE_URDF     = "plane.urdf"  # 地面に関する urdf ファイル
    
    _IDX_MIN_JOINT  = 8             # 関節の最小値が保存されている要素番号
    _IDX_MAX_JOINT  = 9             # 関節の最大値が保存されている要素番号
    _JOINT_INIT     = 0.0           # 関節の初期値
    
    _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近傍の値
    
    
    def __init__(self, robot_urdf, interpolation):
        """
        コンストラクタ

        パラメータ
            robot_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)

        # 環境の初期化
        self._init_environment()

        # スライダーの初期化
        self._init_sliders(interpolation)

        # スライダー作成に時間がかかるため，少しまつ
        time.sleep(self._SLIDER_MAKE_WAIT_TIME)


    # 事前準備メソッド ↓
    def _init_robot(self, robot_urdf):
        """
        ロボットの初期化

        パラメータ
            robot_urdf(str): ロボットアームのファイル名 (urdf)
        """
        # ロボットを読み込む．ベースリンクの原点は (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 not (self._n_joints == DIMENTION_2D or self._n_joints == DIMENTION_3D or self._n_joints == DIMENTION_6D):
            # 2軸，3軸，6軸ロボットアーム以外
            raise ValueError(f"self._n_joints is abnormal. {self._n_joints} is abnormal.")

    def _init_environment(self):
        """
        環境の初期化
        """
        # 地面を読み込む (pybulletが提供している "plane.urdf" を読み込む)
        p.loadURDF(self._PLANE_URDF)

    def _init_sliders(self, interpolation):
        """
        スライダーの初期化

        パラメータ
            interpolation(str): 補間方法 (関節空間/位置空間)
        """
        sliders = []

        # GUIにスライダーを追加
        if interpolation == INTERPOLATION.JOINT.value:  # 関節空間を探索
            # 設定できる関節の最小値と最大値を取得
            min_joints, max_joints = self._get_joint_limit()
            # 初期角度を取得
            init_thetas = self._get_init_thetas()

            # 設定できる関節の最小値・最大値・初期値を設定
            for joint_idx, (min_joint, max_joint, init_theta) in enumerate(zip(min_joints, max_joints, init_thetas)):
                # 関節情報を取得して，スライダーの追加
                slider = p.addUserDebugParameter(f"joiint {joint_idx + 1}", min_joint, max_joint, init_theta)
                sliders.append(slider)

        elif interpolation == INTERPOLATION.POSITION.value: # 直交空間を探索
            slider_names = ["X", "Y", "Z", "roll", "pitch", "yaw"]

            # 設定できる位置の最小値と最大値を取得
            min_positions, max_positions = self._get_pose_limit()
            # 初期位置・姿勢を取得
            init_pose = self._get_init_pose()

            for idx, (min_position, max_position, init_pos) in enumerate(zip(min_positions, max_positions, init_pose)):
                # 位置情報を取得して，スライダーの追加
                slider = p.addUserDebugParameter(f"{slider_names[idx]}", min_position, max_position, init_pos)
                sliders.append(slider)

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

        # プロパティの更新
        self._interpolation = interpolation
        self._sliders = sliders

    def _get_joint_limit(self):
        """
        関節の限界値 (最小値 + 最大値) を取得

        戻り値
            min_joints (list): 関節の最小値 [rad]
            max_joints (list): 関節の最大値 [rad]
        """
        min_joints = []
        max_joints = []

        # 全関節の最小値と最大値を取得
        for i in range(self._n_joints):
            # 関節に関する情報を取得
            joint_info = p.getJointInfo(self._robot_id, i)
            min_joint  = joint_info[self._IDX_MIN_JOINT]
            max_joint  = joint_info[self._IDX_MAX_JOINT]
            # 最小値と最大値をリストに保存
            min_joints.append(min_joint)
            max_joints.append(max_joint)

        return min_joints, max_joints

    def _get_init_thetas(self):
        """
        初期角度を取得

        戻り値
            init_thetas(numpy.ndarray): 初期角度 [rad]
        """
        init_thetas = np.ones(self._n_joints) * self._JOINT_INIT

        return init_thetas

    def _get_pose_limit(self):
        """
        位置の限界値 (最小値 + 最大値) を取得

        戻り値
            min_pose(numpy.ndarray): 位置([m])の最小値
            max_pose(numpy.ndarray): 位置([m])の最大値
        """
        # スライダー内のデータをロボットの全リンク長 +0.5 とした．
        # urdfから，全リンク長を取得してから，+0.5 したい．
        if self._n_joints == DIMENTION_2D:      # 2軸ロボットアーム
            abs_max_value = 2.5
            n_pose_dim = DIMENTION_3D
        elif self._n_joints == DIMENTION_3D:    # 3軸ロボットアーム
            abs_max_value = 3.5
            n_pose_dim = DIMENTION_3D
        else:       # 6軸ロボットアーム
            abs_max_value = 3.8
            n_pose_dim = DIMENTION_6D

        # PyBulletで，位置から関節角度を計算するときに(x, y, z)の情報が必要
        min_pose = np.ones(n_pose_dim) * abs_max_value * -1
        max_pose = np.ones(n_pose_dim) * abs_max_value

        if self._n_joints == DIMENTION_2D:  # 2軸ロボットアーム
            # Z軸は動かして欲しくないから，0近傍
            min_pose[DIMENTION_2D] = -self._ZERO_NEAR
            max_pose[DIMENTION_2D] =  self._ZERO_NEAR

        return min_pose, max_pose

    def _get_init_pose(self):
        """
        初期位置・姿勢を取得

        戻り値
            init_pose(numpy.ndarray): 初期位置([m])・姿勢([rad])
        """
        # 初期位置は適当に決めた
        if self._n_joints == DIMENTION_2D:      # 2軸ロボットアーム
            init_pose = np.array([0.5, 0.5, 0.0])
        elif self._n_joints == DIMENTION_3D:    # 3軸ロボットアーム
            init_pose = np.array([0.5, 0.5, 1.0])
        else:       # 6軸ロボットアーム
            # 初期位置(x, y, z)と初期姿勢(roll, pitch, yaw)
            init_pose = np.array([0.5, 0.5, 1.0, 0, np.pi/2, 0])

        return init_pose
    # 事前準備メソッド ↑


    # メイン処理 ↓
    def run(self):
        """
        実行
            スライダー内の値を取得して，シミュレータ上のロボットを動かす
        """
        # 文字列の出力ID
        text_id = p.addUserDebugText("", textPosition=[0, 0, 0])

        # リアルタイムでのシミュレーション
        p.setRealTimeSimulation(1)

        while True:
            # スライダー内の値を取得
            slider_values = self._get_slider_values()

            # 回転角度に変換 (ロボットへ渡せるのは関節角度だけだから)
            thetas = self._convert_pos_to_theta(slider_values)
            # 関節角度を設定
            self._set_joint(thetas)

            # 値をGUIに設定
            self._set_text(text_id, thetas)

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

    def _convert_pos_to_theta(self, pos):
        """
        手先位置から関節角度に変換

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

        戻り値
            thetas(numpy.ndarray): 関節角度
        """
        if self._interpolation == INTERPOLATION.POSITION.value:
            # 逆運動学により，関節角度を返す
            # エンドエフェクタのリンク要素はベースリンクを除いた要素番号となる
            if self._n_joints == DIMENTION_2D or self._n_joints == DIMENTION_3D:
                # 2軸，3軸ロボットアーム
                thetas = p.calculateInverseKinematics(self._robot_id, self._n_joints, pos)
            else:
                # 6軸ロボットアーム
                # ロール・ピッチ・ヨーから，クォータニオンへ変換
                quaternion = p.getQuaternionFromEuler(pos[DIMENTION_3D:])
                # 逆運動学の解を算出するには，姿勢はクォータニオンでないといけない
                thetas = p.calculateInverseKinematics(self._robot_id, self._n_joints, targetPosition=pos[:DIMENTION_3D], targetOrientation=quaternion)
            thetas = np.array(thetas)
        else:
            # pos が関節角度のため，そのまま返す
            thetas = np.copy(pos)

        return thetas

    def _get_slider_values(self):
        """
        スライダー内の値を取得

        戻り値
            slider_values(numpy.ndarray): スライダー内の情報
        """
        slider_values = []

        for slider in self._sliders:
            # スライダー1つずつ値を取得
            slider_value = p.readUserDebugParameter(slider)
            slider_values.append(slider_value)

        return np.array(slider_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 _set_text(self, text_id, thetas):
        """
        GUIにテキストを設定
        
        パラメータ
            text_id(): 設定したテキストID
            thetas(): 関節角度
        """
        # エンドエフェクタの位置にテキストを出力したい
        ee_pos = p.getLinkState(self._robot_id, self._n_joints)[0]

        if self._interpolation == INTERPOLATION.POSITION.value:     # 直交空間を探索
            # 関節角度をテキストに出力
            text = "thetas:\n"
            for i in range(thetas.size):
                text += f"joint{i + 1}={thetas[i]:.2f}"

        else:   # 関節空間の探索
            # エンドエフェクタの位置をテキストに出力
            text = f"end effecter pos:\nx={ee_pos[0]:.2f}, y={ee_pos[1]:.2f}, z={ee_pos[2]:.2f}"

        p.addUserDebugText(text, ee_pos, textColorRGB=[0, 0, 0], textSize=self._DEBUG_TEXT_SIZE, lifeTime=self._DEBUG_TEXT_LIFE_TIME, replaceItemUniqueId=text_id)
    # メイン処理 ↑

上ソースコードは，3軸ロボットアームを動かした時と大方同じであるため，本記事では割愛する．
(前記事 https://qiita.com/haruhiro1020/items/89ed03b9a63e2cb9b76b)

ただし，6軸ロボットアームは逆運動学を解く際に，手先位置だけでなく手先姿勢も必要となる．
そのため，手先姿勢に関する関数だけ説明する．

スライダーの初期化 (_init_sliders)

スライダーの初期化の中身を説明する．6軸ロボットアームで逆運動学を解くには手先姿勢も必要であるため，姿勢を変更できるようなスライダーを作成した．

        elif interpolation == INTERPOLATION.POSITION.value: # 直交空間を探索
            slider_names = ["X", "Y", "Z", "roll", "pitch", "yaw"]

            # 設定できる位置の最小値と最大値を取得
            min_positions, max_positions = self._get_pose_limit()
            # 初期位置・姿勢を取得
            init_pose = self._get_init_pose()

            for idx, (min_position, max_position, init_pos) in enumerate(zip(min_positions, max_positions, init_pose)):
                # 位置情報を取得して，スライダーの追加
                slider = p.addUserDebugParameter(f"{slider_names[idx]}", min_position, max_position, init_pos)
                sliders.append(slider)

直感的に姿勢を動かせれるように，姿勢表現として，ロール・ピッチ・ヨーを採用した．
ロール・ピッチ・ヨーに関しては，下記記事がわかりやすいです．
(https://qiita.com/harmegiddo/items/96004f7c8eafbb8a45d0)

手先位置から関節角度に変換 (_convert_pos_to_theta)

手先位置から関節角度に変換の中身を説明する．ロボットを動かすためには，関節に角度を与える必要があるため，手先位置を関節角度に変換する必要がある．

        if self._interpolation == INTERPOLATION.POSITION.value:
            # 逆運動学により，関節角度を返す
            # エンドエフェクタのリンク要素はベースリンクを除いた要素番号となる
            if self._n_joints == DIMENTION_2D or self._n_joints == DIMENTION_3D:
                # 2軸，3軸ロボットアーム
                thetas = p.calculateInverseKinematics(self._robot_id, self._n_joints, pos)
            else:
                # 6軸ロボットアーム
                # ロール・ピッチ・ヨーから，クォータニオンへ変換
                quaternion = p.getQuaternionFromEuler(pos[DIMENTION_3D:])
                # 逆運動学の解を算出するには，姿勢はクォータニオンでないといけない
                thetas = p.calculateInverseKinematics(self._robot_id, self._n_joints, targetPosition=pos[:DIMENTION_3D], targetOrientation=quaternion)
            thetas = np.array(thetas)
        else:
            # pos が関節角度のため，そのまま返す
            thetas = np.copy(pos)

PyBulletで手先位置・姿勢から関節角度への変換(逆運動学)は，p.calculateInverseKinematics() を使用する．
calculateInverseKinematics() 関数の引数および返り値は公式ドキュメントより，下図の通りとなる．

特に4行目の「targetOrientation」を確認してほしい．「targetOrientation」の列の一番右に，"目標姿勢(target orientation)はクォータニオン(quaternion)でないといけない"と記載されている．
今回，スライダーでは姿勢表現を「ロール・ピッチ・ヨー」としたため，クォータニオンへ変換する必要がある．p.getQuaternionFromEuler() 関数を使用して，「ロール・ピッチ・ヨー」からクォータニオンへ変換する．
クォータニオンに関しては，下記サイトがわかりやすいです．
(https://qiita.com/harmegiddo/items/96004f7c8eafbb8a45d0)

PyBulletでロボットを動かす

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

位置だけでなく，姿勢を変更しても想定した通りに動いた．
スライダー内のロボット手先位置を変更することで，シミュレータ上のロボットアームも動いていることがわかる．
また，スライダーにてロボットの手先位置が届かない範囲を設定しても，問題なくロボットが動いていることがわかる．特異点にロバストは逆運動学の設計となっているがわかる．

おわりに

本記事では，Pythonを使用して，下記内容を実装しました
・PyBullet で6軸ロボットアームの手先位置をスライダーで設定することよって，ロボットを動かす

次記事では，下記内容を実装していきます．
・PyBullet で干渉物が存在する環境下で，6軸ロボットアームの経路生成 (経路生成手法としてRRTを採用)
(https://qiita.com/haruhiro1020/items/f8c807257a498522f5be)