【Pybulletサンプル解説】sleep設定でオブジェクトを生成【sleeping.py】

Pybullet公式gitリポジトリのサンプルコードを解説するシリーズです（一覧はこちら）。

今回は、sleeping.pyを解説します。（コードのリンクはこちら）

本コードを実行すると、スリープ状態に設定したr2d2が生成されます。

使用している機能

本コードは、以下の機能を使用して「オブジェクトのsleep, 強制wakeup」を実現しています。

• スリープを設定した状態でurdfを読み込み
• 指定したオブジェクトを強制wake up

スリープを設定した状態でurdfを読み込み

pybulletでは、loadURDF関数を使用することで、URDFファイルを読み込むことができます。

今回は、スリープ状態のオブジェクトを生成しています。

r2d2 = pybullet.loadURDF(
  UrdfFile, 
  position, 
  useMaximalCoordinates,              
  flags
)

• UrdfFile：読み込むURDFファイル名
• position：ロボットの生成位置
• useMaximalCoordinates：最大座標系を使うかどうか
  • True：最大座標系を使用
  • False：最小座標系を使用
• flags：フラグの設定
  • pybullet.URDF_ENABLE_SLEEPINGを指定すると、スリープ状態でロボットが生成される

スリープ状態の場合、ロボットが静止している間は計算がスキップされCPUの負荷が軽減されます。

そして、何かにぶつかった際はその瞬間だけwake upし、自動で計算が再開されます（静止したら、またsleepに戻る）。

一方で、スリープ状態のオブジェクトは、「ぶつかる → wakeup → 反応」となり、反応までに多少のラグが生じるデメリットも存在します。

上記の特徴から

• 基本は静止している物体

に対して、スリープを設定すると良いでしょう。

指定したオブジェクトを強制wake up

pybulletでは、changeDynamics関数を使用することで、指定したオブジェクトのダイナミクスを変更することができます。

pybullet.changeDynamics(
  bodyId, 
  linkId, 
  activationState
)

• bodyId：ダイナミクスを変更する対象のオブジェクトのId
• linkId：対象のリンクのId
  • -1の場合、ベースリンク
• activationState：スリープ状態を制御するパラメータ
  • pybullet.ACTIVATION_STATE_WAKE_UP：wake upさせる

基本的に、スリープ状態のオブジェクトは何かにぶつかったときだけ、wake upしますが、changeDynamics関数で、activationState=pybullet.ACTIVATION_STATE_WAKE_UPを引数に指定すると、強制wake upが可能です。

コメントをつけたサンプルコード

サンプルコードにコメントをつけたものが以下になります（もともとあった不要と思われるコメント等については削除しています）

import pybullet as p
import time
import pybullet_data

# 縮小座標系を使用（精度重視）
useMaximalCoordinates = False

# スリープ機能を有効化するフラグ
flags = p.URDF_ENABLE_SLEEPING

# GUIでシミュレータ起動
p.connect(p.GUI)

# pybulletのデータのパスを追加
p.setAdditionalSearchPath(pybullet_data.getDataPath())

# ロード前に、GUIのレンダリングを無効化
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)

# plane100.urdfから床オブジェクトをスリープ状態で生成
p.loadURDF("plane100.urdf", flags=flags, useMaximalCoordinates=useMaximalCoordinates)

# 5×5の位置に25体r2d2を生成（全てスリープ状態で生成）
for k in range(5):
  for i in range(5):
    r2d2 = p.loadURDF(
      "r2d2.urdf", 
      [k * 2, i * 2, 1],
      useMaximalCoordinates=useMaximalCoordinates,
      flags=p.URDF_ENABLE_CACHED_GRAPHICS_SHAPES + flags
    )

    # 全ての関節の速度を0に設定（関節の固定）
    for j in range(p.getNumJoints(r2d2)):
      p.setJointMotorControl2(
        r2d2, 
        j, 
        p.VELOCITY_CONTROL, 
        targetVelocity=0
      )

# ロードが完了したので、GUIのレンダリングを再度有効化
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)

# タイムステップの設定
timestep = 1. / 240.
p.setTimeStep(timestep)

# 重力の設定
p.setGravity(0, 0, -10)

# pybulletが接続されている間、無限ループ
while p.isConnected():

  # シミュレーションを1step進める
  p.stepSimulation()
  time.sleep(timestep)
  
  # 最後に生成したr2d2のベースのリンクを強制wake upする
  p.changeDynamics(r2d2, -1, activationState=p.ACTIVATION_STATE_WAKE_UP)