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

はじめに

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

本記事で実装すること

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

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

・PyBullet で干渉物が存在する環境下での経路生成(経路生成手法として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)

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作成に関しては，前記事にて説明したため，割愛する．
(前記事 https://qiita.com/haruhiro1020/items/ecc5215234350b663770)

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 について説明する．
本記事では，2軸ロボットアームの説明だが，3軸や6軸ロボットアームにも拡張していく予定である．

constant.py

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


# 次元数を定義
DIMENTION_2D    =  2    # 2次元


# 重力
GRABITY_VALUE   = 9.81


# 補間方法の定義
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"

    # 探索空間を指定
    # 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 = 0.1    # シミュレーションの待機時間 [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 self._n_joints != DIMENTION_2D:
            # 2軸ロボットアーム以外
            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"]

            # 設定できる位置の最小値と最大値を取得
            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 したい．
        abs_max_value = 2.5

        # Z軸も欲しいから +1
        # PyBulletで，位置から関節角度を計算するときに(x, y, z)の情報が必要
        min_pose = np.ones(self._n_joints + 1) * abs_max_value * -1
        max_pose = np.ones(self._n_joints + 1) * abs_max_value
        # Z軸は動かして欲しくないから，0近傍
        min_pose[self._n_joints] = -self._ZERO_NEAR
        max_pose[self._n_joints] =  self._ZERO_NEAR

        return min_pose, max_pose

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

        戻り値
            init_pose(numpy.ndarray): 初期位置([m])
        """
        # 初期位置は適当に決めた
        init_pose = np.array([0.5, 0.5, 0.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:
            # 逆運動学により，関節角度を返す
            # エンドエフェクタのリンク要素はベースリンクを除いた要素番号となる
            thetas = p.calculateInverseKinematics(self._robot_id, self._n_joints, pos)
            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)
    # メイン処理 ↑

次に，上ソースコードより重要な箇所を抜粋して，説明していく．
上ソースコードの大方は前記事にて説明しているため，説明した内容は割愛する．
(前記事 https://qiita.com/haruhiro1020/items/ecc5215234350b663770)

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

スライダーの初期化の中身を説明する．

        # GUIにスライダーを追加
        if interpolation == INTERPOLATION.JOINT.value:  # 関節空間を探索
        ...
        elif interpolation == INTERPOLATION.POSITION.value: # 直交空間を探索
        ...

前回は関節角度を動かしていたが，今回は手先位置を動かしたいため，elifを追加した．

            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)

スライダーの作成するには，p.addUserDebugParameter()を使用する．引数に関しては下表にまとめた．ロボットの手先位置を別関数で取得して，p.addUserDebugParameter()の引数として使用する．

引数	概要
1	スライダー名
2	スライダーの最小値
3	スライダーの最大値
4	スライダーの初期値

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

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

        if self._interpolation == INTERPOLATION.POSITION.value:
            # 逆運動学により，関節角度を返す
            # エンドエフェクタのリンク要素はベースリンクを除いた要素番号となる
            thetas = p.calculateInverseKinematics(self._robot_id, self._n_joints, pos)
            thetas = np.array(thetas)
        else:
            # pos が関節角度のため，そのまま返す
            thetas = np.copy(pos)

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

「Inverse Kinematics」 に記載してある内容は，下記のようである．
・Samuel Bussさん の逆運動学を改良
・ダンピング付き最小二乗法 (Damped Least Squares) または Null Spce を使用して，逆運動学を解く
　→ 要するに，特異点であってもロバストである．

上ドキュメントより，p.calculateInverseKinematics() にロボットのID，エンドエフェクタのリンク要素番号，手先位置を与えた．
戻り値として，関節角度を取得した．

2軸ロボットアームの逆運動学

今回，ロボットの手先位置を関節角度に変換して，関節角度を与えることでロボットを動かしている．
ロボットの手先位置から関節角度を算出することを逆運動学と呼ぶ．
2軸ロボットアームの逆運動学については，下記で説明したため，本記事では割愛する．
https://qiita.com/haruhiro1020/items/4529e648161beac7754f
上記事内で，特異点(逆運動学により，関節角度を取得できない)を説明した．
その対応として，レーベンバーグ・マルカート法による微分逆運動学の説明を下記リンクで説明した．
(https://qiita.com/haruhiro1020/items/793eca1f6247fa1bd3f9)

先ほど _convert_pos_to_theta 関数の説明時に，PyBullet の逆運動学を実施する p.calculateInverseKinematics() の説明をした．
その時に，ダンピング付き最小二乗法 (Damped Least Squares)とNull Spceが記載されていた．
ダンピング付き最小二乗法 (Damped Least Squares) はレーベンバーグ・マルカート法と大方同じである．
Null Space に関しては，基本的に冗長ロボット(関節の数が直交空間の次元数よりも多いロボット)で使用する．今回は2軸ロボットアームを使用するため割愛する．冗長ロボットを使用するときに，説明する．

PyBulletでロボットを動かす

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

スライダー内のロボット手先位置を変更することで，シミュレータ上のロボットアームも動いていることがわかる．
また，スライダーにてロボットの手先位置が届かない範囲を設定しても，問題なくロボットが動いていることがわかる．特異点にロバストは逆運動学の設計となっているがわかる．
今回は2軸ロボットアームであるが，他のロボットにも応用できそうだ．

おわりに

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

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