【Pybulletサンプル解説】複数のレイキャストにより2DLidarを模擬する【batchRayCast.py】

お疲れ様です。秋並です。

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

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

本コードを実行すると、2DLidarのように円形にレイキャストを実施し、衝突している箇所が赤色、そうでない箇所が黄緑色に描画されます。

使用している機能

本コードは、以下の機能を使用して「円形にレイキャストを実施した結果の可視化」を実現しています。

• 複数のレイキャストを同時に行う
• ラインの描画

複数のレイキャストを同時に行う

pybulletでは、rayTestBatch関数を使用することで複数のレイを同時に行います。

レイキャストとは、下図のように「ある地点から特定の方向に線を引き、その線上に障害物があるかどうかを検出する」ものになります。

rayTestBatchに似た関数にrayTest関数がありますが、こちらは一つのレイによるレイキャストのみを実施できます。

hitResult = pybullet.rayTest(rayFrom, RayTo)

• rayFrom：レイキャストの始点([x, y, z])
• rayTo：レイキャストの終点（[x, y, z]）

一方で、rayTestBatch関数では、一度に複数のレイによるレイキャストを実施できるため、複数のレイが必要な場面ではrayTesyよりも効率的です。

hitResults = rayTestBatch(rayFromPositions, rayToPositions)

• rayFromPositions：レイの始点（[x,y,z]）が格納されたリスト
• rayToPositions：rayFromPositionsと対応するレイの終点（[x,y,z]）の座標が格納されたリスト

なお、rayFromPositionsとrayToPositinsのリストの長さは同じでなければなりません。

今回は、原点座標から円を描くようにレイキャストを行い、レイがr2d2と衝突しているかどうかを判定しています。

ラインの描画

pybulletでは、 addUserDebugLine関数を使用することでラインを描画できます。

addUserDebugLine(fromPosition, Topisition, lineColor, replaceItemUniqueId)

• fromPosition：描画するラインの始点（[x,y,z]）
• ToPosition：描画するラインの終点（[x,y,z]）
• lineColor：描画するラインの色（RGBで指定）
• replaceItemUniqueId：描画を上書きするラインのID
  • replaceItemUniqueIdを指定しないとラインを一から描画することになるため、同じラインを上書きする場合は指定したほうが効率的に描画できます

今回は、rayTestBatchにより衝突したレイは赤色、衝突していないレイは黄緑色に描画しています。

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

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

import pybullet as p
import time
import math
import pybullet_data

# GUIモードにするかどうかを設定
useGui = True

# GUIモードまたは、ダイレクトモードで接続
if (useGui):
  p.connect(p.GUI)
else:
  p.connect(p.DIRECT)

# Pybulletに関するデータへのパスを設定
p.setAdditionalSearchPath(pybullet_data.getDataPath())

# デバッグビジュアライザのGUIを無効化
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)

# r2d2のurdfファイルを[3,3,1]の位置に生成
p.loadURDF("r2d2.urdf", [3, 3, 1])

# レイの始点、終点、IDを格納するリストを定義
rayFrom = []
rayTo = []
rayIds = []

# レイの数を設定
numRays = 1024

# レイの長さを設定
rayLen = 13

# レイがヒットしたときと、ヒットしなかったときの色を設定
rayHitColor = [1, 0, 0]
rayMissColor = [0, 1, 0]

# レイを置き換えるかどうかを設定
replaceLines = True

# numRaysの例を生成し、その視点と終点をリストに追加。
# replaceLinesがTrueの場合は、レイのIDもリストに追加
for i in range(numRays):
  rayFrom.append([0, 0, 1])
  rayTo.append([
      rayLen * math.sin(2. * math.pi * float(i) / numRays),
      rayLen * math.cos(2. * math.pi * float(i) / numRays), 1
  ])
  if (replaceLines):
    rayIds.append(p.addUserDebugLine(rayFrom[i], rayTo[i], rayMissColor))
  else:
    rayIds.append(-1)

# GUIモードでない場合、ログの記録を開始する
if (not useGui):
  timingLog = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "rayCastBench.json")

# シミュレーションのステップ数を設定
# GUIモードの場合は、ステップ数を上書き
numSteps = 10
if (useGui):
  numSteps = 327680

for i in range(numSteps):
  # シミュレーションを1時刻分進める
  p.stepSimulation()

  # レイキャストを行う
  for j in range(8):
    results = p.rayTestBatch(rayFrom, rayTo, j + 1)

  # GUIモードの場合、レイの結果を描画
  if (useGui):
  
    # replaceLinesがFalseの場合、すべてのデバッグアイテムを削除
    if (not replaceLines):
      p.removeAllUserDebugItems()

    # 各レイキャストの結果を確認し、ヒットした場合と、していない場合で色を変えて描画
    for i in range(numRays):
      hitObjectUid = results[i][0]
      if (hitObjectUid < 0):
        hitPosition = [0, 0, 0]
        p.addUserDebugLine(rayFrom[i], rayTo[i], rayMissColor, replaceItemUniqueId=rayIds[i])
      else:
        hitPosition = results[i][3]
        p.addUserDebugLine(rayFrom[i], hitPosition, rayHitColor, replaceItemUniqueId=rayIds[i])

# GUIモードでない場合、ログの記録を停止
if (not useGui):
  p.stopStateLogging(timingLog)