はじめに

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を採用)