【Pybulletサンプル解説】デバッグスライダーに指定した位置/姿勢にロボットを設定【rollPitchYaw.py】

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

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

本コードを実行すると、ロボットをデバッグスライダーで指定した位置/姿勢に設定できます。

使用している機能

本コードは、以下の機能を使用して「ロボットをデバッグスライダーで指定した位置/姿勢に設定」することができます。

• デバッグスライダーの設定
• ロボットを指定した位置/姿勢に設定

デバッグスライダーの設定

addUserDebugParameter 関数を使用することで、デバッグ用のスライダーを設定できます。

debugSliderId = p.addUserDebugParameter(paramName, minVal, maxVal, initVal)

• debugSliderId：デバッグスライダーのID
• paramName：パラメータの名称
• minVal：パラメータの最小値
• maxVal：パラメータの最大値
• initVal：パラメータの初期値

今回は、ロボットの位置/姿勢を設定するために使用します。

# ロボットの位置/姿勢を設定可能なデバッグ用のスライダーを定義
rollId = p.addUserDebugParameter("roll", -1.5, 1.5, 0)
pitchId = pybullet.addUserDebugParameter("pitch", -1.5, 1.5, 0)
yawId = p.addUserDebugParameter("yaw", -1.5, 1.5, 0)
fwdxId = p.addUserDebugParameter("fwd_x", -1, 1, 0)
fwdyId = p.addUserDebugParameter("fwd_y", -1, 1, 0)
fwdzId = p.addUserDebugParameter("fwd_z", -1, 1, 0)

ロボットを指定した位置/姿勢に設定

resetBasePositionAndOrientation 関数を使用することで、指定したオブジェクトを指定した位置/姿勢に設定できます。

 pybulllet.resetBasePositionAndOrientation(objectId, objectPosition, obJectPosture)

• objectId：位置/姿勢を設定するオブジェクトのID
• objectPosition：オブジェクトの位置（[x,y,z]）
• objectPosture：オブジェクトの姿勢（[qx, qy, qz, qw]）
  • クォータニオンで指定する必要があることに注意

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

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

import pybullet as p
import time
import pybullet_data

# 共有メモリモードでPybulletに接続
cid = p.connect(p.SHARED_MEMORY)

# 接続が失敗したら、GUIモードで再接続
if (cid < 0):
  p.connect(p.GUI)

# Pybulletに関するデータパスを取得
p.setAdditionalSearchPath(pybullet_data.getDataPath())

# 四足歩行ロボットをベースを固定した状態で生成
q = p.loadURDF("quadruped/quadruped.urdf", useFixedBase=True)

# ロボットの位置/姿勢を設定可能なデバッグ用のスライダーを定義
rollId = p.addUserDebugParameter("roll", -1.5, 1.5, 0)
pitchId = p.addUserDebugParameter("pitch", -1.5, 1.5, 0)
yawId = p.addUserDebugParameter("yaw", -1.5, 1.5, 0)
fwdxId = p.addUserDebugParameter("fwd_x", -1, 1, 0)
fwdyId = p.addUserDebugParameter("fwd_y", -1, 1, 0)
fwdzId = p.addUserDebugParameter("fwd_z", -1, 1, 0)

while True:
  # デバッグスライダーに設定された値を取得
  roll = p.readUserDebugParameter(rollId)
  pitch = p.readUserDebugParameter(pitchId)
  yaw = p.readUserDebugParameter(yawId)
  x = p.readUserDebugParameter(fwdxId)
  y = p.readUserDebugParameter(fwdyId)
  z = p.readUserDebugParameter(fwdzId)

  # オイラー角からクォータニオンを取得
  orn = p.getQuaternionFromEuler([roll, pitch, yaw])

  # 指定した「位置/姿勢」にロボットを設定
  p.resetBasePositionAndOrientation(q, [x, y, z], orn)