Pybulletでロボットアームを動かす方法

　「ロボットシミュレーションをするためにPybulletをインストールしたものの、日本語の記事や説明が少なすぎてきつい…」と感じたので、Pybulletの環境構築からPybulletの関数の説明、そして実際にロボットを動かすまでの手順を簡単に解説しました。

動作環境

Windows10
WSL2
Ubunu18.04

準備

　まだPybulletを動かす準備が何もできていない人のための章なので、「もうPybulletが動くよっ！」って人は読み飛ばしても問題ないです。

環境構築のための手順

　Windows10でPybulletを動かすための流れを簡単に説明します。
Pybulletを動かすためには、Pybullet自体のインストールの前に

WSL2環境をつくる
Ubuntu18.04のインストール
Xserverのインストール

が必要になっています。

1. WSL2環境をつくる
　Windows10でPybulletを使うためには、まずWSL (Windows Subsystem for Linux) の環境をつくることが望ましいです。

WSL2のインストール手順 (公式ページ)

2. Ubuntu18.04のインストール
　WSL2の準備ができたら、次はUbuntuのインストールをします。
　インストールはMicrosoft Storeから簡単にすることができます。Storeには色々なバージョンのUbuntuがありますが、基本的にはUbuntu18.04をインストールするのが無難だと思います。
　Pybulletをこのまま直接Ubuntuで動作させることができますが、筆者はWindows TerminalでUbuntuを使っています。
　Ubuntuがインストールできたら、Ubuntuを立ち上げてWSL2の設定をしましょう。

Windows TerminalでUbuntuを使う方法
UbuntuでWSL2を有効にする方法

については、ここでは省略します。

3. Xserverのインストール
　Pybulletでロボットシミュレーションをするためには、当たり前ですがシミュレーションの様子を見る必要があります。このシミュレーションの様子を見るためのモニターの役割を持つのがXserverです。

Xserver (VcXsrv) のインストール (公式ページ)

↑クリック後は自動でインストールが始まるので注意

Xserverのインストール後は

Xserverの設定
WindowsのファイアウォールからXserverを許可

が必要になります。これについてもここでは省略します。

Pybulletのインストール
　Xserverが無事動作するようになったら、いよいよPybulletのインストールです。Pybulletのインストール時、WSL上のどの場所にインストールするか、どこにインストールしたかをしっかり覚えておきましょう。

また、Pybullet以外にも

matplotlib
numpy
math

などのライブラリをインストールしておくと後々便利です。

Pybulletの動作確認

　Pybulletをインストールしたディレクトリに移動し、

$ cd bullet3/examples/pybullet/examples
$ ls

を実行すると、Pybulletインストール時に付いてきたサンプルコードが並んでいると思います。
　そのサンプルコードのうちのどれかを適当に起動し、しっかりと動作していればPybulletの動作環境が整っていると思って大丈夫でしょう。

(例)ヒューマノイドロボットの手動操作プログラムの実行

$ python3 humanoid_manual_control.py

結果

右のバーをマウスで動かすと、連動してロボットが動きます

Pybulletの使い方

humanoid_manual_control.pyコード解説

　Pybulletは名前のとおりPythonで動作します。
例えば、先ほどのhumanoid_manual_control.py のプログラムの中身はこんな感じ。

humanoid_manual_control.py

import pybullet as p
import time

import pybullet_data

p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
obUids = p.loadMJCF("mjcf/humanoid.xml")
humanoid = obUids[1]

gravId = p.addUserDebugParameter("gravity", -10, 10, -10)
jointIds = []
paramIds = []

p.setPhysicsEngineParameter(numSolverIterations=10)
p.changeDynamics(humanoid, -1, linearDamping=0, angularDamping=0)

for j in range(p.getNumJoints(humanoid)):
  p.changeDynamics(humanoid, j, linearDamping=0, angularDamping=0)
  info = p.getJointInfo(humanoid, j)
  #print(info)
  jointName = info[1]
  jointType = info[2]
  if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE):
    jointIds.append(j)
    paramIds.append(p.addUserDebugParameter(jointName.decode("utf-8"), -4, 4, 0))

p.setRealTimeSimulation(1)
while (1):
  p.setGravity(0, 0, p.readUserDebugParameter(gravId))
  for i in range(len(paramIds)):
    c = paramIds[i]
    targetPos = p.readUserDebugParameter(c)
    p.setJointMotorControl2(humanoid, jointIds[i], p.POSITION_CONTROL, targetPos, force=5 * 240.)
  time.sleep(0.01)

p.が付いてるのがPybulletの関数です。
　なのでif文やfor文、リストなどの基本的なPython知識さえあれば、あとはPybulletの関数を使うだけで簡単にシミュレーションができるということです。

　さて、ここからは軽く上記のコードの解説をします。
このコードの内容が大体理解できるようになれば、Pybulletのことを大体理解できるかと思います。ちなみに、Pybulletには公式のスターターガイドが存在していますので、Pybullet関数の詳細を知りた場合についてはこちらを参考に。(英語ですが...)

Pybulletスターターガイド

1. コード前半部分

import pybullet as p
import time

import pybullet_data

p.connect(p.GUI) 
p.setAdditionalSearchPath(pybullet_data.getDataPath())
obUids = p.loadMJCF("mjcf/humanoid.xml")
humanoid = obUids[1]

p.connect(p.GUI) でGUI (Graphical User Interface) に接続します。Pybulletでシミュレーションをするときにはとりあえず入れときましょう。

p.setAdditionalSearchPath(pybullet_data.getDataPath()) では、おそらくインポートしたpybullet_data というファイルに収まったヒューマノイドロボットのパスを通しています。Pybulletインストール時についてきたサンプルロボットを使いたいときには、このようにパスを通しておきましょう。

p.loadMJCF("mjcf/humanoid.xml") では、ヒューマノイドロボットのデータが入っているファイルを読み込んでいます。
　つまり先ほどの画像で表示されていたヒューマノイドロボットはPythonファイルで作成しているわけではなく、別にあるMCDFファイルで作成されていたのです。
　このサンプルコードではMJCFで記述されたロボットを読み込んでいますが、MJCFの他にもSDFやURDFといった形式で記述されます。

2. コード中盤部分

gravId = p.addUserDebugParameter("gravity", -10, 10, -10)
jointIds = []
paramIds = []

p.setPhysicsEngineParameter(numSolverIterations=10)
p.changeDynamics(humanoid, -1, linearDamping=0, angularDamping=0)

for j in range(p.getNumJoints(humanoid)):
  p.changeDynamics(humanoid, j, linearDamping=0, angularDamping=0)
  info = p.getJointInfo(humanoid, j)
  #print(info)
  jointName = info[1]
  jointType = info[2]
  if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE):
    jointIds.append(j)
    paramIds.append(p.addUserDebugParameter(jointName.decode("utf-8"), -4, 4, 0))

p.addUserDebugParameter() はデバッグ用の関数です。これを使うとシミュレーションの実行中にユーザーが手動でパラメータを操作することができるようになるため、非常に便利な関数となってます(さっきの画像の右に表示されていたバーが、この関数で出現している)。例えば、
gravId = p.addUserDebugParameter("gravity", -10, 10, -10)
では、gravIdというパラメータに対して、-10から10の範囲でユーザーが操作できるということです。また、gravIdの初期値は-10に設定されています。
シミュレーション中のバーの上に表示されるパラメータの名前をgravityという名前に設定しています。

p.getNumJoints(humanoid) ではhumanoid (loadMJCFで読み込んだロボット) のジョイントの数を返します。

for文中の
info = p.getJointInfo(humanoid, j)では、ヒューマノイドロボットのj番目のジョイントの情報をリスト形式で返しています。
jointNameはジョイントの名前、jointTypeはジョイントの種類を収めるパラメータです。
　つまりif文部分は、「PRISMATICかREVOLUTEのjointのみをデバッグする」ためのコードです。
PRISMATIC(平行移動ジョイント)とREVOLUTE(軸回転ジョイント)は、のちのちURDFを記述するときにちゃんと登場します。
　ちなみにわざわざif文でジョイントの種類を制限しているのは、他にもFIXED(固定ジョイント)という動かないジョイントなどが存在するからです。

3. コード後半部分

p.setRealTimeSimulation(1)
while (1):
  p.setGravity(0, 0, p.readUserDebugParameter(gravId))
  for i in range(len(paramIds)):
    c = paramIds[i]
    targetPos = p.readUserDebugParameter(c)
    p.setJointMotorControl2(humanoid, jointIds[i], p.POSITION_CONTROL, targetPos, force=5 * 240.)
  time.sleep(0.01)

　さて、さっきまでやっていたことはシミュレーションをするための下準備でした。そしてここからやることが、メインのシミュレーションです。

p.setRealTimeSimulation(1)は、リアルタイムでシミュレーションをするための設定をする関数です。次のwhile文部分が繰り返されることでシミュレーションが進行します。
Pybulletでシミュレーションをする際には、while文の前にこれを入れときましょう。while文が一周するたびにシミュレーションが動きます。

まずp.setGravity(0, 0, p.readUserDebugParameter(gravId))では重力加速度を設定します。さっきデバッグ用パラメータとして用意したgravIdを入力値にしているので、ユーザーがシミュレーション中に重力を変えられるようになってます。

　for文の中身は、各ジョイントのパラメータをユーザーが動かすためのコードです。
p.setJointMotorControl2(humanoid, jointIds[i], p.POSITION_CONTROL, targetPos, force=5 * 240.)
これがロボットを動かすための関数です。
humanoid で動かすロボットを指定、jointIds[i] で動かすジョイントを指定、p.POSITION_CONTROL は制御する物理量の種類(p.POSITION_CONTROLは座標もしくは角度、p.VELOCITY_CONTROLは速度など)、targetPosが目標値(今回の場合は角度)、force はジョイントが出せる最大の力(今回の場合)となっています。。

　ざっくりと説明すると、targetPos はデバッグ用パラメータなので、ユーザーがバーを動かすとジョイントが動くという仕組みになっています。

time.sleep(0.01) で0.01秒間だけプログラム (シミュレーション) をストップさせているので、シミュレーションがスムーズに動きます。とりあえず最後に入れときましょう。
例えばtime.sleep(1)にしてみると、1秒ごとにパラメータが更新されるのでロボットの動きがカクカクになります。

　humanoid_manual_control.py はロボットを手動で動かすプログラムなのでデバッグ用関数が多用されていました。Pybulletの関数が多いので見た目は難しそうなコードですが、逆にPybulletの関数の使い方さえ覚えれば意外と単純なプログラムだということが分かりますね。

　実はロボットアームを動かしたいだけなら、コード前半のloadMJCF を使いたいロボットアームの読み込みに差し替えるだけでできます。

humanoid_manual_control.pyコードをいじってロボットアームを動かす

　では早速humanoid_manual_control.py のコードをいじっていきます。
まず、念のためコードをコピーしておきましょう。

$ cp humanoid_manual_control.py robotArm_manual_control.py

　ここからはコピーしたrobotArm_manual_control.py で作業していきます。
まずはヒューマノイドロボットからロボットアームに差し替えてみます。

robotArm_manual_control.py

import pybullet as p
import time

#import pybullet_data #必要ないので削除

p.connect(p.GUI)
#p.setAdditionalSearchPath(pybullet_data.getDataPath()) #必要ないので削除
kukaId = p.loadURDF("kuka_iiwa/model.urdf", [0, 0, 0], useFixedBase=True) #追加
#obUids = p.loadMJCF("mjcf/humanoid.xml") #削除
#humanoid = obUids[1] #削除

　Pybulletインストール時に、サンプルロボットアームとしてkuka_iiwaというロボットアームが付いてきますので、今回はこれを使います。
上記コードのように差し替えたら、残りのコードのhumanoid 部分も全てkukaId に差し替えて下さい。

ちゃんと全部変更できたら、実行して確認します。

$ python3 robotArm_manual_control.py

結果

無事ヒューマノイドロボットがロボットアームに変わりました。右のバーを動かせばちゃんとアームが動かせます。

　しかしさっきとは違って床がなくなってしまいました。
おそらく、削除したobUids = p.loadMJCF("mjcf/humanoid.xml") の中に床のデータも入っていたのでしょう。

ということで次は床を追加します。

robotArm_manual_control.py

import pybullet as p
import time

p.connect(p.GUI)
p.loadURDF("plane.urdf", [0, 0, 0], useFixedBase=True) #追加
kukaId = p.loadURDF("kuka_iiwa/model.urdf", [0, 0, 0], useFixedBase=True)

結果

　こんな感じで、使いたいロボットアームや置きたい物体があればloadURDF() で簡単に設置することができます。
ただし、Pybulletに最初から入っているサンプル以外を使いたい場合は、個別にURDFを記述したり、CADで作成したりする必要があります。

URDFを記述してオブジェクトを作成

　最後に、URDFの記述方法について少しだけ説明しておきます。
URDFが記述できるようになると、簡単なオブジェクトやロボットアームが自作できるようになります。

まずは簡単な箱を生成するurdfを記述してみます。

box.urdf

<?xml version="1.0" ?>
<robot name="box">
  <link name="base">
    <inertial>
      <origin rpy = "0 0 0" xyz = "0 0 0" />
      <mass value = "0.0001" />
      <inertia ixx = "0.0001" ixy = "0" ixz = "0" iyy = "0.0001" iyz = "0" izz = "0.0001" />
    </inertial>
  </link>
  <link name="box">
    <contact>
      <lateral_friction value="1"/>
    </contact>
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="0.01"/>
      <inertia ixx="0.005" ixy="0" ixz="0" iyy="0.005" iyz="0" izz="0.005"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size="0.02 0.02 0.02"/>
      </geometry>
      <material name="Purple">
        <color rgba="1 0 1 1" />
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size="0.02 0.02 0.02"/>
      </geometry>
    </collision>
  </link>
  <joint name="base_box" type="fixed" >
    <parent link = "base" />
    <child link = "box" />
    <origin rpy = "0 0 0" xyz = "0.000 0.000 0.00" />
  </joint>
</robot>

これをPybulletで表示してみます

小さくてわかりずらいですが、紫色の立方体が生成されています。
この立方体がbox.urdf によって生成されています。

「たったこれだけの箱を生成するのに、こんな長くてめんどくさそうなコードを書かなくてはいけないのか…」と思ったかもしれませんが、決まった骨組みに沿って記述するだけなので、コピペ＆微調整を重ねるだけで比較的楽に書くことができます。

URDFは基本的に、link というパーツをjoint という関節で接続するということを繰り返すことで記述していきます。
例えば上のbox.urdfでは、baseという名前のリンクとboxという名前のリンクがbase_boxという名前のジョイントによって繋がれています。

ここからは各リンク、ジョイントの解説をしていきます。

baseリンク

base_link

<link name="base">
  <inertial>
    <origin rpy = "0 0 0" xyz = "0 0 0" />
    <mass value = "0.0001" />
    <inertia ixx = "0.0001" ixy = "0" ixz = "0" iyy = "0.0001" iyz = "0" izz = "0.0001" />
  </inertial>
</link>

　baseリンクは、大きさ(体積) が0で、微小な質量を持つリンクです。
わざわざこのリンクをつくっているのは、作成したオブジェクトのベースポジションを決めるためです。
シミュレーションで作成したオブジェクトを設置するときにこのベースリンクの座標で位置を指定するので、例えば
p.loadURDF(box.urdf, [0, 0, 0]) を実行すると[0, 0, 0]の位置にbaseリンクが来るようにbox.urdf が設置されます。

boxリンク

box_link

<link name="box">
  <contact>
    <lateral_friction value="1"/>
  </contact>
  <inertial>
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <mass value="0.01"/>
    <inertia ixx="0.005" ixy="0" ixz="0" iyy="0.005" iyz="0" izz="0.005"/>
  </inertial>
  <visual>
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <geometry>
      <box size="0.02 0.02 0.02"/>
    </geometry>
    <material name="Purple">
      <color rgba="1 0 1 1" />
    </material>
  </visual>
  <collision>
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <geometry>
      <box size="0.02 0.02 0.02"/>
    </geometry>
  </collision>
</link>

　boxリンクは一辺が0.02 (m) の立方体のリンクです。
box.urdf の本体パーツがこれになります。
コードを１つ１つ見ていきます。

<contact> 接触について。
<lateral_friction value="1"/> 摩擦係数を1に設定。
<inertial> 慣性モーメントについて。
<origin rpy="0 0 0" xyz="0 0 0"/> 初期姿勢(rpy)と初期位置(xyz)を0に設定。
<mass value="0.01"/> 質量を0.01 (kg)に設定。
<inertia ixx="0.005" ixy="0" ixz="0" iyy="0.005" iyz="0" izz="0.005"/> 慣性モーメントの設定。
<visual> 見た目について。
<geometry> 形について。
<box size="0.02 0.02 0.02"/> 箱型の一辺0.02 (m)の形に設定。
<material name="Purple"> 物体の色について。
<color rgba="1 0 1 1" /> 色をrgbaで設定。この場合は紫色。
<collision> 衝突判定について。

　URDFを記述するときは必ず</> で閉じる必要があるので注意しましょう。
(例1) <contact>の中で接触の情報を記述したら</contact> で閉じる。
(例2) <mass value="0.01"/> のように、最後に/ で閉じる。

base_boxジョイント

base_box_joint

<joint name="base_box" type="fixed" >
  <parent link = "base" />
  <child link = "box" />
  <origin rpy = "0 0 0" xyz = "0.000 0.000 0.00" />
</joint>

base_boxジョイントでは、baseリンクとboxリンクを接続しています。

<joint name="base_box" type="fixed" > "fixed"(固定)。つまり、ジョイントが動かないように設定。
<parent link = "base" /> 親リンクの設定。親リンクから子リンクにジョイントを繋ぐ。
<child link = "box" /> 子リンクの設定。親リンクから繋がれるリンク。
<origin rpy = "0 0 0" xyz = "0.000 0.000 0.00" /> ジョイントの位置の設定。親リンクからの相対距離を決めます。

　URDFの記述は慣れるまでは難しいですが、リンクとジョイントを繋げていくだけなので、プログラミング能力を必要としないのが良いですね。
　しかし1つ注意点として、URDFの記述をミスしたとき、どこをミスしているのかエラーコードが出てきません。なのでこまめに実行してチェックしないと、しらみつぶしに1行ずつコードを見直すことになるので気を付けましょう。

URDFの記述方法については以下の記事が参考になると思います。

イナーシャ(慣性モーメント)については以下の記事が参考になると思います。

ROS(Robot Operating System)を使う

最後に

この記事は早稲田大学の尾形研究室での活動の一環として書きました。
ロボットや深層学習について研究しているので、興味のある方は以下の研究室HPをご覧下さい。

尾形哲也研究室

Pybulletでロボットアームシミュレーションをするまでの手順