PyBulletの調査2: 複数コンポーネントから構成されるオブジェクト
0. はじめに
先日書いたPyBulletの調査記事の続きです。
前回、PyBulletの基本的な情報や、単純なオブジェクトの構築について調査して記載しましたが、今回は複数のコンポーネントから構成されるより複雑なモデルの構築について調べた事を記載します。（前回の記事を未読の方は、そちらから目を通してください。）
細かい中身は必要なく、既存のURDF形式のファイルに保存されたモデルを使って、さくっと強化学習を実行したいという場合はFacebook Research製の pybulletX が良さそうです。
紹介記事も書いるので、気になった方はそちらも御覧ください。
1. 複数コンポーネント構成の理解
前回同様、PyBulletのAPIを利用してモデルを構築することが目的ですが、URDF形式について解説してくれている次のブログ記事が、複数コンポーネントからなるオブジェクトの概念を理解するために参考になりました。（個人的にはXMLはやっぱり書きたくないので、プログラムで対応したい。）
また、URDF形式の公式チュートリアルのページもイメージを掴むのに役立ちました。（特に冒頭の画像がとても分かりやすいので、一旦開いて見ていただけると助かります。）
「リンク(=link)」と呼ばれる部品が、「ジョイント(=joint)」によって結合しています。リンク自体は独立した座標系であり、リンクの座標系の中に、前回説明した衝突判定用オブジェクト等を配置します。
ジョイントで結合したリンクには、親子の非対称な関係があり、子のリンクは親のリンクの原点から、移動・回転した先に子のリンクの原点を配置します。
ジョイントには可動の仕方によって種類があり、子リンクは原点を中心に動かせます。
PyBulletのAPIから指定できるのは、 JOINT_REVOLUTE(軸周りに回転)・JOINT_PRISMATIC(軸方向に移動)・JOINT_FIXED(固定) のみと（ちょっと古い）公式ドキュメントには記載があります。
Bullet3では、親を持たない主となるリンクを「ベース(=base)」と呼び特別な取り扱いをしています。オブジェクトに含まれるリンクをリンクidで管理・指定するのですが、ベースのリンクidは -1 となります。
2. PyBullet上での構築
複数の(子)リンクを含むオブジェクトを配置するには、単純なオブジェクトを設置する場合と同じくpybullet.createMultiBody関数を利用します。
その際に、linkMasses(質量)・linkCollisionShapeIndices(衝突判定用)・linkVisualShapeIndices(外見)・linkPositions(親リンクからの子リンク原点の位置)・linkOrientations(親リンクからの子リンクの方向・回転)・linkInertialFramePositions(??)・linkInertialFrameOrientations(??)・linkParentIndices(親リンクのid)・linkJointTypes(ジョイントの種類)・linkJointAxis(ジョイントの軸)を同じ長さのリストで明示的に指定する必要があります。
たくさんあって大変ですが、指定しないと"All link arrays need to be same size."とエラーが発生します。（エラー・メッセージからは何が不足しているのか不明瞭で、ソースコードからエラーになる条件を確認しました。。。）
obj_id = pybullet.createMultiBody(mass, box_cid, -1,
basePosition=[0, 0, 0.1],
linkMasses=[mass, mass],
linkCollisionShapeIndices=[-1, cylinder_cid],
linkVisualShapeIndices=[-1, -1],
linkPositions=[[0, 0, 0],[0,0.5,0.5]],
linkOrientations=[[0,0,0,1], [1,0,0,math.cos((3/8)*math.pi)]],
linkInertialFramePositions=[[0,0,0],[0,0,0]],
linkInertialFrameOrientations=[[0,0,0,1],[0,0,0,1]],
linkParentIndices=[0, 1],
linkJointTypes=[pybullet.JOINT_REVOLUTE, pybullet.JOINT_FIXED],
linkJointAxis=[[1,0,0],[1,0,0]],
physicsClientId=engine_id)
また、ジョイントの可動範囲の指定は、pybullet.changeDynamics関数で指定できます。
実装を読む限りでは、jointLowerLimit <= jointUpperLimit の時に、可動範囲が更新されます。
pybullet.changeDynamics(obj_id,
1, # link id
jointLowerLimit=-1,
jointUpperLimit=1,
physicsClientId=engine_id)
3. 実際にやってみる
メトロノームのように箱の周りを円状に可動できるシリンダーのオブジェクトを作ってみました。
- 前半は重力によってシリンダーが落下して床に衝突。
- 後半は重力に抗うようにトルクをかけてシリンダーを持ち上げる。
上記GIFのシミュレーションのコード on Google Colab
import pybullet
import matplotlib.pyplot as plt
from PIL import Image
from IPython import display as dd
import base64
import io
import math
# 物理エンジンの初期化
engine_id = pybullet.connect(pybullet.DIRECT)
# 重力
pybullet.setGravity(0,0,-10, physicsClientId=engine_id)
# 質量
mass = 50.0
fix_mass = 0
# 床の設置
plane_cid = pybullet.createCollisionShape(pybullet.GEOM_PLANE, meshScale=[0.05,0.05,0.05], physicsClientId=engine_id)
plane_id = pybullet.createMultiBody(fix_mass, plane_cid, physicsClientId=engine_id)
# 衝突判定用のオブジェクト（の雛形）
cylinder_cid = pybullet.createCollisionShape(pybullet.GEOM_CYLINDER, radius=0.1, height=1, physicsClientId=engine_id)
box_cid = pybullet.createCollisionShape(pybullet.GEOM_BOX, halfExtents=[0.1, 0.1, 0.1], physicsClientId=engine_id)
# 外観オブジェクト（の雛形）
r_vid = pybullet.createVisualShape(pybullet.GEOM_BOX, halfExtents=[0.1,0.1,0.1], rgbaColor=[1,0,0,1], physicsClientId=engine_id)
# 複数コンポーネントオブジェクトの構築
obj_id = pybullet.createMultiBody(10*mass, box_cid, r_vid,
basePosition=[0, 0, 0.1],
linkMasses=[fix_mass, mass],
linkCollisionShapeIndices=[-1, cylinder_cid],
linkVisualShapeIndices=[-1, -1], # 外観用オブジェクトを指定しない時は、 -1 を指定。
linkPositions=[[0, 0, 0],[0,0.5,0.5]],
linkOrientations=[[0,0,0,1], [1,0,0,math.cos((3/8)*math.pi)]],
linkInertialFramePositions=[[0,0,0],[0,0,0]],
linkInertialFrameOrientations=[[0,0,0,1],[0,0,0,1]],
linkParentIndices=[0, 1],
linkJointTypes=[pybullet.JOINT_REVOLUTE, pybullet.JOINT_FIXED],
linkJointAxis=[[1,0,0],[1,0,0]],
physicsClientId=engine_id)
# シミュレーションとカメラ画像の保存
gif = []
for i in range(2000):
if i % 10 == 0:
gif.append(Image.fromarray(pybullet.getCameraImage(512, 384, physicsClientId=engine_id)[2]))
pybullet.stepSimulation(physicsClientId=engine_id)
for i in range(2000):
pybullet.setJointMotorControl2(obj_id, 0, pybullet.TORQUE_CONTROL, force=600, physicsClientId=engine_id)
if i % 10 == 0:
gif.append(Image.fromarray(pybullet.getCameraImage(512, 384, physicsClientId=engine_id)[2]))
pybullet.stepSimulation(physicsClientId=engine_id)
# 画像をGIFとして書き出して表示
f = io.BytesIO()
gif[0].save(f, save_all=True, append_images=gif[1:],format="gif", loop=0)
f.seek(0)
b64 = base64.b64encode(f.read()).decode('ascii')
f.close()
display(dd.HTML(f'<img src="data:image/gif;base64,{b64}" />'))
4. 苦労した点
子リンクの原点は、linkPositions で指定しますが、（衝突判定）オブジェクトはその子リンクの原点に配置されてしまい、（おそらく）ずらして配置することはできませんでした。
そのため、上記のメトロノーム（仮）のシリンダーのように、オブジェクトの中心周り以外にジョイントを持ちたい場合は、ジョイント用のリンクをJOINT_REVOLUTE等で作って、そのリンクに紐付ける形で JOINT_FIXED で平行移動（＋必要なら回転）させたオブジェクトを配置することが必要でした。
うまく行かないときには、非常に大きい力（トルクや重力）をかけてみるみると、ジョイントの制約の設定の仕方が悪いのか、力のかけ方が悪いのかを切り分け易かったように感じます。
また、 setJointMotorControl2 で指定したトルクは、1ステップ (stepSimulation 1回) のみ有効なようです。
5. おわりに
前回も書きましたが、PyBullet結構しんどいですね。引き続き調査してまた記事にします。
可動範囲の制約で少し出てきましたが、changeDynamics 関数は、摩擦等様々な指定ができるようなので調査します。
操作面では、setJointMotorControl2 や setJointMotorControlArray や、オブジェクトの状態を取得する方法を深堀りする必要があります。
また、getCameraImage の画面の範囲・角度の調整などを調べないと使い物になりません。
（今回のシミュレーションの各オブジェクトのサイズが小さいのは、カメラを動かせずこれ以上大きくなると画面からはみ出てしまうからでして。。。）
Discussion