LoginSignup
0
0

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?

@haruhiro1020

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

Last updated at Posted at 2025-07-25

はじめに

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

本記事で実装すること

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

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

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

動作環境

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

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

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

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

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

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

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

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

URDFのソースコード

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

robot_3dof.urdf
<?xml version="1.0"?>
<robot name="robot_3dof">

    <!-- ベースリンク -->
    <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.0 0.0 0.5"/>
            <geometry>
                <box size="0.2 0.2 1.0"/>
            </geometry>
        </visual>

        <!-- 干渉判定 -->
        <collision>
            <origin xyz="0.0 0.0 0.5"/>
            <geometry>
                <box size="0.2 0.2 1.0"/>
            </geometry>
        </collision>

        <!-- 慣性行列 -->
        <inertial>
            <origin xyz="0.0 0.0 0.5"/>
            <!-- 質量は適当 -->
            <mass value="1.0"/>
            <inertia ixx="0.0866667" iyy="0.0866667" izz="0.0066667" 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="0 0 1.0" rpy="0 0 0"/>
        <axis xyz="0 1 0"/>
        <limit lower="-3.14" upper="3.14" effort="100" velocity="5.0"/>

    </joint>


    <!-- リンク2 -->
    <link name="link2">

        <!-- 見た目 -->
        <visual>
            <origin xyz="0.0 0.0 0.5"/>
            <geometry>
                <box size="0.2 0.2 1.0"/>
            </geometry>
        </visual>

        <!-- 干渉判定 -->
        <collision>
            <origin xyz="0.0 0.0 0.5"/>
            <geometry>
                <box size="0.2 0.2 1.0"/>
            </geometry>
        </collision>

        <!-- 慣性行列 -->
        <inertial>
            <origin xyz="0.0 0.0 0.5"/>
            <!-- 質量は適当 -->
            <mass value="1.0"/>
            <inertia ixx="0.0866667" iyy="0.0866667" izz="0.0066667" ixy="0.0" ixz="0.0" iyz="0.0"/>
        </inertial>

    </link>


    <!-- リンク2とリンク3間の関節 -->
    <joint name="joint3" type="revolute">

        <parent link="link2"/>
        <child link="link3"/>
        <origin xyz="0 0 1.0" rpy="0 0 0"/>
        <axis xyz="0 1 0"/>
        <limit lower="-3.14" upper="3.14" effort="100" velocity="5.0"/>

    </joint>


    <!-- リンク3 -->
    <link name="link3">

        <!-- 見た目 -->
        <visual>
            <origin xyz="0.0 0.0 0.5"/>
            <geometry>
                <box size="0.2 0.2 1.0"/>
            </geometry>
        </visual>

        <!-- 干渉判定 -->
        <collision>
            <origin xyz="0.0 0.0 0.5"/>
            <geometry>
                <box size="0.2 0.2 1.0"/>
            </geometry>
        </collision>

        <!-- 慣性行列 -->
        <inertial>
            <origin xyz="0.0 0.0 0.5"/>
            <!-- 質量は適当 -->
            <mass value="1.0"/>
            <inertia ixx="0.0866667" iyy="0.0866667" izz="0.0066667" ixy="0.0" ixz="0.0" iyz="0.0"/>
        </inertial>

    </link>


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

        <parent link="link3"/>
        <child link="ee_link"/>
        <origin xyz="0 0 1.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ファイルの説明をする．
URDFファイルの各タグに関しては，前記事で説明したため，本記事では割愛する．
(前記事 https://qiita.com/haruhiro1020/items/ecc5215234350b663770)

PyBulletの使用方法

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

「PYBULLET QUICKSTART GUIDE」タグをクリックすると，下図のようなドキュメントを見ることができる．基本的には，ドキュメントに記載されている関数の使用方法を見て，ソースコードを作成している．
PyBulletドキュメント.png
PyBulletの関数の引数や戻り値をもっと知りたいのでしたら，上記ドキュメントを見た方がわかりやすいです．

全体のソースコード

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

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

定数の定義 (constant.py)

定数を定義する constant.py について説明する．
本記事では，3軸ロボットアームの説明だが，6軸ロボットアームにも拡張していく予定である．

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


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


# 重力
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次元物理シミュレータ) によるロボットアームの可視化


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


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



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

    # 探索空間を指定
    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 not (self._n_joints == DIMENTION_2D or self._n_joints == DIMENTION_3D):
            # 2軸，3軸ロボットアーム以外
            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)
    # メイン処理 ↑

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

PyBulletでロボットを動かす

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

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

おわりに

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

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

0
0
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
0
0

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?