PyBulletで2軸ロボットアームを動かす Part1 (関節角度を自由に設定)

はじめに

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

本記事で実装すること

・PyBullet で2軸ロボットアームの関節角度を設定することで，動かす

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

・PyBullet で2軸ロボットアームの手先位置を設定することで，動かす
・PyBullet で干渉物が存在する環境下での経路生成

動作環境

・macOS Sequoia (バージョン15.5)
・Python3 (3.13.3)
・PyBullet (3.2.5) (物理シミュレータ)
・Numpy (2.3.0) (数値計算用ライブラリ)

PyBullet のインストール方法

PyBullet のインストール方法について説明する．
下記コマンドをターミナル上で実行する．

pip install pybullet

上記内容は以下サイトを参考とした．
(https://zenn.dev/ymd_h/articles/14397e6ae7ea3d)

私は，上記コマンドをターミナル上で実行したら，何のエラーも発生せずにインストールできた．

2軸ロボットアームの定義

本記事で動かしたい，2軸ロボットアームについて説明する．
本記事では，下図のような2軸ロボットアームを考えている．
$\theta_{1}, \theta_{2}$ は，Z軸方向($X, Y$軸に直交する手前方向)へ回転する．

上図のパラメータ $l_{1}, l_{2}, m_{1}, m_{2}$ の値は下表として，考える(値は適当である)．

パラメータ名	概要	値
$l_{1}$	リンク$1$の長さ	1.0 [m]
$l_{2}$	リンク$2$の長さ	1.0 [m]
$m_{1}$	リンク$1$の質量	1.0 [kg]
$m_{2}$	リンク$2$の質量	1.0 [kg]

PyBulletで，上図のロボットを使用するために，URDF (Unified Robot Description Format)を作成する必要がある．

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

PyBulletで，2軸ロボットアームを使用するためのURDF作成に関して説明する．
URDF作成に関しては，下記サイトを参考とした．
(https://qiita.com/srs/items/35bbaadd6c4be1e39bb9)

URDFを作成するためには，ロボットのリンクとジョイントを定義する必要がある．
下図のようにジョイントは，2つのリンクを繋げる役割がある．

ジョイントには，親リンクと子リンクの2つのリンクを設定しないといけない．
上図からジョイント1に注視して，親リンクと子リンクを考えてみる．
親リンクはリンク1となり，子リンクはリンク2となる．
ジョイントで繋げるリンク間のうち，原点に近いリンクが親リンク，原点に遠いリンクが子リンクとなる．

本記事で作成する2軸ロボットアームを3次元で表現すると，下図のようになる．

URDFのソースコード

上記で定義した2軸ロボットアームのURDFソースコードを作成する．
下記がURDFのソースコードである．
URDFの中身について，後ほど説明する．

robot_2dof.urdf

<?xml version="1.0"?>
<robot name="robot_2dof">

    <!-- ベースリンク -->
    <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="100" velocity="5.0"/>

    </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.0066667" iyy="0.0866667" izz="0.0866667" 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="100" velocity="5.0"/>

    </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.0066667" iyy="0.0866667" izz="0.0866667" ixy="0.0" ixz="0.0" iyz="0.0"/>
        </inertial>

    </link>


    <!-- リンク2とエンドエフェクタ間の関節 -->
    <joint name="ee_joint" type="fixed">

        <parent link="link2"/>
        <child link="ee_link"/>
        <origin xyz="1.0 0 0" rpy="0 0 0"/>

    </joint>


    <!-- エンドエフェクタ -->
    <link name="ee_link">

        <visual>
            <geometry>
                <sphere radius="0.05"/>
            </geometry>
            <material name="red">
                <color rgba="1 0 0 1"/>
            </material>
        </visual>

        <!-- 見た目だけ定義して，干渉判定および慣性はなし -->

    </link>


</robot>

URDFの説明

上記のURDFファイルの説明をする．

・< ?xml ? >タグ：URDFを作成する時に，必要なタグ．バージョンを設定する．
・< robot >タグ：ロボットを作成する時に，必要なタグ．nameにより，ロボット名を設定する．タグ内で，リンクやジョイントを定義していく．

< link >タグには記載する内容が多いため，下表より説明する．

タグ名	概要	備考
visual	リンクの見た目を定義	本タグを定義しないと見た目が存在しない
origin	リンクの中心点を設定	($x, y, z$)の順番に記載 単位は[m]
geometry	リンクの形状を定義	直方体(box)，球(sphere)などの定義 単位は[m]
box	直方体の形状を定義	直方体の中心点が< origin >タグとなることに注意して，大きさを定義すること
collision	リンクの干渉判定を定義	本タグを定義しないと< visual >タグで見た目が存在していても，常に干渉なしとなる
inertial	リンクの慣性特性を定義	正確な値を入れないと物理シミュレータの挙動がおかしくなる
mass	リンクの質量	単位は [kg]
inertia	リンクの慣性行列	リンクの形状に合わせて計算する

リンクの慣性行列(タグ)を算出する方法に関しては，後ほど説明する．

< joint >タグには記載する内容が多いため，下表より説明する．

タグ名	概要	備考
joint	関節名と種類の設定	ロボットの関節を回転関節にしたいからtype="revolute"とした
parent	親リンク	ジョイントと繋がっている2つのリンクのうち原点に近いリンク
child	子リンク	ジョイントと繋がっている2つのリンクのうち原点に遠いリンク
origin	親リンクの座標系から見た位置	($x, y, z$)の順番に記載 単位は[m]
axis	回転する軸	z軸に回転するからxyz="0 0 1"とした
limit	ジョイントの限界値	lower，upperは関節の下限値，上限値[rad]．effortは最大のトルク[N*m]．velocityは最大の速度[rad/s]

上記のタグを駆使することで，2軸ロボットアームのURDFを作成した．

URDFの慣性に関して

先ほど，タグ内にで慣性行列を定義することを説明した．
慣性行列に関しては，リンクの形状に応じて変わってくる．直方体の慣性行列は下記サイトを参考にした．
https://note.com/npaka/n/n37dbe4eae232

上図のように，直方体の質量を$m$，幅を$w$，奥行きを$d$，高さを$h$とすると，慣性行列は下式の通りとなる．

\displaylines{
I = 
\begin{pmatrix}
I_{xx} & I_{xy} & I_{xz} \\
I_{yx} & I_{yy} & I_{yz} \\
I_{zx} & I_{zy} & I_{zz}
\end{pmatrix}
=
\begin{pmatrix}
1/12 * m * (h^2 + d^2) & 0                      & 0                      \\
0                      & 1/12 * m * (w^2 + h^2) & 0                      \\
0                      & 0                      & 1/12 * m * (w^2 + d^2) 
\end{pmatrix} \\
}

上式の$I_{xx}, I_{yy}, I_{zz}$の値をURDFに記載した．

以上でURDFの記述方法の説明を終了する．
次は，PyBulletの使い方を説明する．

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

    # 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_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    # テキストの大きさ
    
    
    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)

        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 run(self):
        """
        実行
            スライダー内の関節角度を取得して，シミュレータ上のロボットを動かす
        """
        # 文字列の出力ID
        text_id = p.addUserDebugText("", textPosition=[0, 0, 0])

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

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

            # 関節角度を設定
            self._set_joint(slider_values)

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

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

    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):
        """
        GUIにテキストを設定
        
        パラメータ
            text_id(): 設定したテキストID
        """
        # エンドエフェクタの位置を取得
        ee_pos = p.getLinkState(self._robot_id, self._n_joints)[0]
        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)
    # メイン処理 ↑

次に，上ソースコードより重要な箇所を抜粋して，説明していく．

コンストラクタの説明 (init())

コンストラクタの中身を説明する．

        # PyBulletの初期化
        p.connect(p.GUI)

今回はPyBulletをGUI(Graphical User Interface)(視覚要素を使用して動かすインターフェース)で動かしたいため，PyBulletの初期化にp.GUIを選択した．GUIで動かしたくない場合は，p.connectの引数にp.DIRECTを与える．

        # パスの追加
        p.setAdditionalSearchPath(pybullet_data.getDataPath())

今回は，PyBulletで提供されている標準的なURDF(plane.urdf)を使用したいから，コードを記載した．

        # シミュレーションの初期化
        p.resetSimulation()

シミュレーションを初期化しないと，前回の情報が残っている可能性があり，不具合になるため実施する．

ロボットの初期化の説明 (_init_robot)

ロボットの初期化の中身を説明する．

        # ロボットを読み込む．ベースリンクの原点は (x, y, z) = (0, 0, 0) として，ベースリンクは地面に固定
        self._robot_id = p.loadURDF(robot_urdf, basePosition=[0, 0, 0], useFixedBase=True)

PyBulletでURDFを読み込む際には，p.loadURDF()を使用する．戻り値を後ほど使用していくため，クラスのプロパティに保存している．

        # urdf よりロボットの関節数を取得 (エンドエフェクタ用の仮想関節は不要なため -1)
        self._n_joints = p.getNumJoints(self._robot_id) - 1

読み込んだURDF内のジョイント(関節)数を取得するには，p.getNumJoints()を使用する．

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

スライダーの初期化の中身を説明する．
今回は，スライダーを使用して，ロボットの関節角度を自由に設定したいから，本関数を作成した．

            # 設定できる関節の最小値・最大値・初期値を設定
            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)

スライダーの作成するには，p.addUserDebugParameter()を使用する．引数に関しては下表にまとめた．

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

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

関節の限界値 (最小値 + 最大値) を取得の中身を説明する．
今回，スライダー内の最小値・最大値はURDFに記載した関節の最小値・最大値を使用したい．

        # 全関節の最小値と最大値を取得
        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)

URDFに記載した関節に関する情報を取得するには，p.getJointInfo()を使用する．
getJointInfo()の引数や戻り値は下図の通りである(下図は公式ドキュメントから抜粋した)．

今回は jointLowerLimit，jointUpperLimit をローカル変数に保存している．

スライダー内の値を取得 (_get_slider_values)

スライダー内の値を取得の中身を説明する．

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

スライダーの値を取得するには，p.readUserDebugParameter()を使用する．
for文を使用して，スライダー1つずつの値を取得している．

関節角度の設定 (_set_joint)

関節角度の設定の中身を説明する．

        for i in range(len(thetas)):
            # 関節角度を設定
            p.setJointMotorControl2(
                bodyUniqueId=self._robot_id,    # IDの設定
                jointIndex=i,                   # 関節番号の設定
                controlMode=p.POSITION_CONTROL, # 位置制御
                targetPosition=thetas[i]        # 関節角度
            )

本関数で，ロボットアームの各関節に角度を設定している．
setJointMotorControl2()の引数は下図の通りである(下図は公式ドキュメントから抜粋した)．制御には位置制御，速度制御，力制御がある．今回は位置制御を使用したいため，p.POSITION_CONTROLを採用した．

GUIにテキストを設定 (_set_text)

GUIにテキストを設定の中身を説明する．
スライダー内の関節角度を取得して，ロボットを動かしているが，ロボットの手先位置($x, y, z$)が不明である．
そのため，テキストを作成して，ロボットの手先位置をPyBullet上に出力したから，本関数を作成した．

        # エンドエフェクタの位置を取得
        ee_pos = p.getLinkState(self._robot_id, self._n_joints)[0]
        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)

テキストをPyBullet上で出力するには，p.addUserDebugText()を使用する．
テキストの位置をエンドエフェクタの位置(ロボットの手先位置)に出力されるように設定した．
また，replaceItemUniqueId=text_id により，テキストを毎処理で作成・削除しないような設定とした．テキストを毎処理で作成・削除してしまうと，テキストがちらついてしまう(ちかちかする)から．

実行 (run)

PyBulletを実行する関数の中身を説明する．
処理の概要としては，スライダー内の関節角度を取得して，シミュレータ上のロボットを動かす処理である．

        # 文字列の出力ID
        text_id = p.addUserDebugText("", textPosition=[0, 0, 0])

PyBullet上でテキストを作成するための処理である．
_set_text 関数内でも説明したが，テキストを毎処理で作成・削除してしまうと，テキストがちらついてしまう(ちかちかする)から，あらかじめテキストを空文字で作成した．

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

今回，ロボットをリアルタイムでシミュレーションしたいから，p.setRealTimeSimulation(1) を実装した．リアルタイムでのシミュレーションを実施したくない場合は，p.setRealTimeSimulation(0) とする．

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

シミュレーションのタイムラグをなくすために，内部で待機処理を実装した．
これは公式ドキュメントの使用方法を参考にした．

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

今回，ロボットに関節角度を与えることで，ロボットを動かしている．
ロボットの手先にテキストとして，ロボットの手先位置を記載している．
ロボットの関節角度から手先位置を算出することを順運動学と呼ぶ．
2軸ロボットアームの順運動学については，下記で説明したため，本記事では割愛する．
https://qiita.com/haruhiro1020/items/4529e648161beac7754f

PyBulletでロボットを動かす

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

スライダー内の関節角度を変更することで，シミュレータ上のロボットアームも動いていることがわかる．
今回は2軸ロボットアームであるが，他のロボットにも応用できそうだ．

おわりに

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

次記事では，下記内容を実装していきます．
・PyBullet で2軸ロボットアームの手先位置をスライダーで設定することよって，ロボットを動かす
(https://qiita.com/haruhiro1020/items/1a564c8b5e0dd7e09024)