====== pybulletの使い方メモ ======


[[articles:mujoco|mujocoの使い方メモ]]でmujocoをいろいろいじってみた．

使い勝手は悪くはないのだが，接触におけるslipの発生がどうしてもなくせない．
これは物体を把持したり，ものを積み重ねる（コーヒーカップを台の上に置くなど）などの
普通の作業をちゃんと模擬できなくて困る．

またmujocoでは，nameタグはitem種別内でユニークでなくてはならず
同じモデルファイルを読み込んで同一属性の複数物体を生成することは出来なかったが
pybulletは同じフアイルを読み込んで別オブジェクトとして操作することが可能である．

さらにpybulletは，シミュレータを起動後も（物理法則には何しているが）物体を生成することもできる．

ということでpybulletを試してみることにした．

===== install =====
ubuntu20.04ではとりあえず以下でOK.
<code>
$ sudo pip install pybullet
</code>

===== マニュアル類 =====

googleで "pybullet manual" で検索すればすぐ出てくるが，
pybulletのドキュメントページのurlは以下である．

[[https://pybullet.org/wordpress/index.php/forum-2/|Documentation | Bullet Real-Time Physics Simulation]]

そこからpybulletのクイックスタートガイドへのリンクが辿れる．

[[https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA/edit#heading=h.lnt8r6hpxey2|
PyBullet Quickstart Guide]]

基本的な使い方はこれで分かる．

===== 基本的な使い方 =====

最も基本的な使い方は，
[[https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA/edit#heading=h.lnt8r6hpxey2|
pybulletのクイックスタートガイド]]
の最初の　Hello PyBullet World で尽くされている．

ここではその説明では分かりにくい部分を順次補足していく．
==== モジュールの読込み ====

<code>
import pybullet as p
import time
import pybullet_data
</code>

pybulletは名前が長いので別名をpなどとしておくとよい．

timeは，シミュレーションステップの待ち時間に使う．
したがってシミュレーションステップをコントロールしないのであれば必須ではない．

pybullet_dataは，pybulletで用意してあるモデルを読み込む場合に以下のように使う．
<code>
p.setAdditionalSearchPath(pybullet_data.getDataPath())
</code>

ただし，上記パスの設定はサーバとの接続が完了してから出ないと出来ないので注意が必要である．

すべてのモデルを自前で用意するのであれば不要である．
==== bullet物理エンジンサーバの起動とサーバへの接続 ====

pybulletはbullet物理エンジンサーバをpythonから利用するためのインタフェースを提供している．

=== 物理エンジンサーバ起動と接続の種類 ===

pybulletでは，物理エンジンの立ち上げと接続に関して大きく３種類あり，
その中でguiの有無，接続の種類でさらに細かく分類される．
  - 物理エンジンの立ち上げと接続を行う. \\ サーバと1対1の接続になる．
    - DIRECT : guiインタフェースを持たない．表示画面は自前で用意する．
    - GUI : 表示画面を持つgui付きで接続する．
  - 物理エンジンをサーバとして立ち上げ接続を行う\\ 自身もそれへ接続するのみならず共有メモリ，TCP, UDPなどのクライアント接続を許すサーバを立ち上げる．\\ 複数のサーバを立ち上げる場合は key を指定する．\\ TCP, UDP接続を許す場合は，それぞれ IP addressやport numberを指定する．
    - SHARED_MEMORY_SERVER : DIRECTと同様にguiを持たない．
    - GUI_SERVER : GUIと同様にgui付きである．
  - 既存の物理エンジンサーバに接続を行う
    - SHARED_MEMORY : 共有メモリでサーバに接続する．guiを持たない． \\ 接続先がGUI_SERVERならその画面を共有する．
    - SHARED_MEMORY_GUI : 共有メモリでサーバに接続する．guiを持つ．\\ 接続先がSHARED_MEMORY_SERVERのときに画面を持てるので便利．
    - TCP : TCP接続．guiを持たない．
    - UDP : UDP接続．guiを持たない．

=== 接続の利用例 ===

最も簡単な利用例は，サーバと1対1でgui付きで立ち上げる．
<code>
pclient=p.connect(p.GUI)
</code>
接続の戻り値は，失敗の場合 -1, 成功の場合client_idが返る．

複数のクライアントとしての接続を区別するものであり，
この場合は接続が一つしかないので気にしなくて良い（最初の１つはdefaultで0）．

共有メモリサーバとして立ち上げる場合は，以下のようにする．
<code>
pclient=p.connect(p.GUI_SERVER)
</code>
この場合でも，クライアントとしての接続も行うので常にこれで立ち上げておいてもよい．

共有メモリサーバが（1つ）立ち上がっている場合は，そこに以下のように接続することが出来る．
<code>
pclient=p.connect(p.SHARED_MEMORY)
</code>
これによりpybulletモジュールを読み込んだ複数のpythonプロセスから
最初に立ち上げたGUI_SERVERに接続することが出来る．

サーバを立ち上げたpythonプロセスでシミュレーションループを走らせておけばthreadingモジュールを使うことなく，
実行中のサーバに他のpythonプロセスからアクセスできるので便利である．

もちろんシミュレーションの一時停止／再開などの制御を行いたい場合は，thread化しておく方が使いやすい．

==== モデルの読込み ====


=== urdf ファイルの読込み ===
== urdf ファイル ==
urdf ファイルでは，jointで接続された一連の物体を**一つだけ読み込む**.

以下は単一直方体モデルファイルの例である．

単一直方体と言いながら，baseとjointを定義しているのは，
直方体の底面に代表座標系を設定するためのダミーであり，
その必要がなければこれらはいらない．

また慣性モーメント(inertiaタグ部分)は，default設定では質量と形状から再計算されるが，
タグが存在しないと正しいurdfファイルとして認識されず読み込むことが出来ないので注意する必要がある．
<code>
<?xml version="1.0"?>
<!-- rho=1000.0 kg/m**3 -->

<robot name="box">
  <material name="green">
    <color rgba="0.2 0.8 0.2 1" />
  </material>

  <link name="base" />

  <joint name="base_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 0.03"/>
    <parent link="base"/>
    <child link="box_body"/>
  </joint>
  
  <link name="box_body">
    <visual>
      <geometry>
        <box size=".02 .04 .06" />
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <material name="green" />
    </visual>
    <collision>
      <geometry>
        <box size=".02 .04 .06" />
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>　
    <inertial>
      <mass value="4.8e-2" />
<!-- by default, pybullet recompute inertia based on mass and collision shape -->
      <inertia ixx="2.08e-5" ixy="0.0" iyy="1.6e-5" iyz="0.0" izz="8e-6" />
    </inertial>
    <contact>
      <lateral_friction value="1.0"/>
      <spinning_friction value="1.0"/>      
    </contact>    
  </link>
</robot>

</code>

== 読込み ==

<code>
obj_id=p.loadURDF(urdf_file,base_xyz,base_quat)
</code>

  * urdf_file : ファイル名．current directoryとp.setAdditionalSearchPathで設定されたdirectoryにパスが通っている．\\ 絶対／相対パスを付けてファイル名を指定すればそこから読み込むことが出来る．
  * base_xyz : ベースの位置．[x, y, z]のシーケンスとして与える．
  * base_quit : ベースの姿勢．[//i//x, //j//y, //k//z, w]のquaternionで与える．
  * obj_id : 返り値は，物体のidであり，物体に対する種々の操作や情報取得の際に必要となる．

=== mjcfファイルの読込み ===
== mjcfファイル ==
mjcfファイルの場合は，一つのファイルで複数の物体を読み込むことが出来る．

mujocoで読み込む場合は，読み込むファイルは一つだけで，そこで定義された物体やitemにはユニークな名前を付ける必要があった．

pybulletでは，複数のファイルや同じファイルの複数回の読込みが可能で，ファイル内の名前がユニークであればよく，
読み込んでしまえばobj_idで区別されるため，ファイル間での名前の衝突は問題にならない．

mjcfでは，一つのbody(urdfのlinkに相当する)に複数の形状を定義でき，
その形状の位置姿勢はbody座標からの相対できるので，
直方体の底面に代表座標を設定する場合にurdfのようなダミー設定は不要である．

逆に，bodyの座標を定義しておかなくてはいけないので，読込みの初期位置がファイルに書かれているものから変更できない．

もちろん読み込んだ後は，pybulletのインタフェースを通して変更可能である．

質量や慣性モーメントは，密度（defaultは水と同じ）を与えると自動的に計算する．

以下は，単一直方体の例である．
<code>
<mujoco model="Box">
  <worldbody>
    <body name="box" pos="0.5 0 1">
      <joint name='box_joint' type="free" />
      <geom name="box" type="box" pos="0 0 0.03"
	    size=".01 .02 .03" rgba="0 .9 0 1" />
    </body>
  </worldbody>
</mujoco>

</code>

== 読込み ==
<code>
obj_ids=p.loadMJCF(mjcf_file)
</code>
初期位置，姿勢を与えることが出来ないので呼び出しはシンプルである．

obj_idsは，obj_idのタプルである．

複数物体を一つのファイルで読み込めるのは便利そうではあるが，
各物体とタプルのobj_idの対応付けを事前に把握していないと逆に不便になる．

=== 複数クライアントからのモデルの読込み ===
複数のクライアントの上でそれぞれモデルを読み込むことが出来る．

ただし，それらのクライアントからアクセスすることの出来る物体モデルは以下に限られるので注意する必要がある．
  * サーバにconnectしたときにすでに存在する物体
  * 自身でloadした物体
つまり，サーバにconnectした後で，他のクライアントがloadした物体へのアクセスは出来ない．

とりわけ最初にサーバを立ち上げたクライアントは自身がloadした物体しかアクセスできない．

また別なクライアントを接続した場合も，そのクライアントが接続する前にloadされた物体
（および自身がloadした物体）にしかアクセスできないので接続のタイミングに注意する必要がある．

==== pybulletのバグ？ ====
上記のurdf, mjcfのどちらのboxモデルも，重心位置が底面（代表座標系）になりおきあがりこぼしのような振る舞いをする．

<del>mujocoやgazeboでどうなっているかはまだ調査していないが，これはpybulletのバグの可能性が高い．

今の所，モデル定義でこれを回避する方法が見つからない．

これを回避するには，諦めて外側で代表座標系のオフセットを持つしかなさそうである．
</del>

これは、形状を持たないlinkも質量を持つためであることがわかった。

そこで、座標値だけが必要な形状を持たないリンクでも以下のように極めて小さい質量を定義してやればほとんど問題がなくなる。

ただし質量をゼロにするとworldに固定されたリンクになってしまうので注意が必要である。
このとき慣性モーメントも定義しておかないと読み込み時にエラーになる。

逆にこれを利用するとworldに固定された物体を定義することが可能となる。

<code>
  <link name="base">
    <inertial>
      <mass value="1.0e-10" />
      <inertia ixx="1.0e-10" ixy="0.0" iyy="1.0e-10" iyz="0.0" izz="1.0e-10" />
    </inertial>
  </link>
  
</code>
==== シミュレーションループ　====

シミュレーションを走らせながら，その内部にアクセスするには
シミュレーションループを別threadで走らせるのが良い．

もちろん共有メモリサーバなどの形で走らせておいて，
別クライアントからアクセスすれば手軽に同様なことが出来るが
その場合，必ずしも全体が把握できるわけではないので
事前に十分なシステム設計が必要となる．

thread化は[[mujoco#mujoco_py お試しプログラミング |mujocoのとき]]と同様に以下のように簡単に行える．

<code>
import threading
 
class SimThread(threading.Thread) :
    def __init__(self) :
        super(SimThread, self).__init__()
        self.stopped=True
        self.exit=False
    def run(self) :
        self.stopped=False
        while not self.exit :
            if not self.stopped :
                p.stepSimulation()
            time.sleep(1.0/240)
        p.disconnect()
</code>
これで以下のようにしてシミュレーションループを走らせる．
<code>
pclient=p.connect(p.GUI)
# pclient=p.connect(p.GUI_SERVER)
p.setAdditionalSearchPath(pybullet_data.getDataPath())

p.setGravity(0,0,-9.8)
sim=SimThread()
sim.start()
</code>

シミュレションの一時停止/再開はsim.stoppedでコントロールする
<code>
sim.stopped=Ttrue
</code>
<code>
sim.stopped=False
</code>
シミュレーションを終了するのはsim.exitをTrueとする．
<code>
sim.exit=True
</code>

これらは本来関数化するのが良いのだがちょとサボっている．

==== 物体の位置の取得／変更 ====

boxの読込み例
<code>
base_xyz=[0.2,0.2,0]
base_quat=[0,0,0,1]
obj_id==p.loadURDF("box.urdf",base_xyz,base_quat)
</code>
boxの位置取得
<code>
tmp=p.getBasePositionAndOrientation(obj_id)
tmp
</code>
位置取得の結果
<code>
((0.1999830690124357, 0.20004510953643545, -1.0369948860879825e-05),
 (-5.842754809179002e-06,
  -8.354739944407987e-06,
  -1.9459314372555577e-05,
  0.9999999997586978))
</code>
boxの位置変更

位置を変えてpi/3(60°)回転
<code>
tmp_pos=[0.2,-0.2,0]
tmp_quat=[0,0,sin(pi/6),cos(pi/6)]
p.resetBasePositionAndOrientation(obj_id, tmp_pos, tmp_quat)
</code>
位置の確認
<code>
tmp=p.getBasePositionAndOrientation(obj_id)
tmp
</code>
結果
<code>
((0.200007533061792, -0.20020968355214314, -1.0007444750406526e-05),
 (2.2559890601557242e-07,
  -3.142648086800062e-07,
  0.5036072068098224,
  0.8639327411604789))
</code>
指定した位置とシミュレータ内の位置に多少のズレがあるところが気になるところ
===== シリアルリンクロボットアーム =====

シリアルリンクロボットアームはシリアルリンク構造で記述される．

[[articles:mujoco#model記述(MJCF)の基本構造|mujocoでは]]シリアルリンク構造は<body>タグの入れ子構造で
シリアルリンク構造を記述していた．自由に動ける物体も含めすべての物体は<worldroot>の下に入れ子構造で記述される．

一方，bulletでは，個々の物体<link>を必要に応じて<joint>で接続する形で記述していく．

また，bulletの<link>は，視覚形状<visual>，衝突形状<collision>と感性情報<inertial>を0,1でしか持てない．

linkを複数の形状で表現することが出来ないので，単純な形状の組み合わせで表現したい場合は固定の<joint>で接続して表現することになる．

ロボットアームは，jointを駆動して動作させることになる．

==== 関節<joint>の記述とリンクの座標関係 ====
基本的に関節<joint>は2つのリンク<link>を結びつける．

その2つのリンクの相対的位置関係は<joint>の中の<origin>で記述される．
<origin>では親リンク<parent>から見た子リンク<child>の相対位置姿勢をxyzとrpyで記述する．

また<joint>では関節の種類とその軸やその可動範囲なども記述されている．

<joint>の記述例を以下に示す．

<code>
  <joint name="joint1" type="revolute">
    <axis xyz="0 0 1" />
    <limit effort="1000.0" lower="-3.14" upper="3.14" velocity="0.5" />
    <xacro:j_dyn />
    <parent link="base" />
    <child link="link1" />
    <origin rpy="0 0 0" xyz="0 0 0.1" />
  </joint>  
</code>


====リンクとgeometoryの座標関係 ====

視覚形状や衝突形状の代表座標系はリンク座標系からの相対座標変換で記述される．

たとえば以下のようになる．これはロボットのベースの記述例である．
<code>
  <link name="base">
    <visual>
      <geometry>
	<cylinder length="0.1" radius="0.1" />
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0.05" />
      <material name="green" />
    </visual>
    <collision>
      <geometry>
	<cylinder length="0.1" radius="0.1" />
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0.05" />
    </collision>
    <inertial>
      <mass value="3.14" />
      <inertia ixx="1.05e-2" ixy="0.0" iyy="1.05e-2" iyz="0.0" izz="1.57e-2" />
    </inertial>
  </link>

</code>
関節の種類

関節の駆動方法の種類

関節の順番

関節パラメタ

関節の駆動

関節情報の取得

関節の順番