【pybullet】移動ロボットの超音波センサを模擬する

お疲れ様です。秋並です。
今回は、pybulletで超音波センサを模擬する方法について紹介します。

本記事で紹介するコードを実行すると、下動画のような

• 移動ロボットが前進し、ロボットが障害物に一定以内まで近づくと停止する

といったシミュレーションが実現できます。

超音波センサの模擬

pybulletでは、rayTest()という関数を使用することで「レイキャスト」を実施することが出来ます。

hitResult = pybullet.RayTest(rayFrom, RayTo)

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

レイキャストとは

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

今回使用する移動ロボットのurdfファイル

今回は、下図のような車体の前方に「超音波センサ用のリンク」を持った移動ロボットを定義したurdfファイルを使用します。

<?xml version="1.0" ?>
<robot name="ultrasonic_four_wheel_car">
  <!--車体リンク-->
  <link name="body_link">
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <box size="0.5 0.3 0.1"/>
      </geometry>
      <material name="white">
        <color rgba="0 0 1 1"/>
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <box size="0.5 0.3 0.1"/>
      </geometry>
    </collision>
    <inertial>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <mass value="10.0" />
        <inertia ixx="0.000034533333" 
            ixy="0"  ixz="0"
            iyx="0"  iyy="0.000034533333" 
            iyz="0"
            izx="0"  izy="0"  izz="0.0000648" />
    </inertial>
  </link>

  <!-- 左前車輪リンク  -->
  <link name="front_left_wheel_link">
    <visual>
      <origin xyz="0 0 0" rpy="1.57 0 0 "/>
      <geometry>
        <cylinder length="0.025" radius="0.05"/>
      </geometry>
      <material name="black">
        <color rgba="1 1 1 1"/>
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="1.57 0 0"/>
      <geometry>
        <cylinder length="0.025" radius="0.05"/>
      </geometry>
    </collision>
    <inertial>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <mass value="0.1" />
        <inertia ixx="0.000034533333" 
            ixy="0"  ixz="0"
            iyx="0"  iyy="0.000034533333" 
            iyz="0"
            izx="0"  izy="0"  izz="0.0000648" />
    </inertial>
  </link>

  <!-- 右前車輪リンク  -->
  <link name="front_right_wheel_link">
    <visual>
      <origin xyz="0 0 0" rpy="1.57 0 0 "/>
      <geometry>
        <cylinder length="0.025" radius="0.05"/>
      </geometry>
      <material name="black">
        <color rgba="1 1 1 1"/>
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="1.57 0 0 "/>
      <geometry>
        <cylinder length="0.025" radius="0.05"/>
      </geometry>
    </collision>
    <inertial>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <mass value="0.1" />
        <inertia ixx="0.000034533333" 
            ixy="0"  ixz="0"
            iyx="0"  iyy="0.000034533333" 
            iyz="0"
            izx="0"  izy="0"  izz="0.0000648" />
    </inertial>
  </link>

  <!-- 左後車輪リンク  -->
  <link name="back_left_wheel_link">
    <visual>
      <origin xyz="0 0 0" rpy="1.57 0 0 "/>
      <geometry>
        <cylinder length="0.025" radius="0.05"/>
      </geometry>
      <material name="black">
        <color rgba="1 1 1 1"/>
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="1.57 0 0"/>
      <geometry>
        <cylinder length="0.025" radius="0.05"/>
      </geometry>
    </collision>
    <inertial>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <mass value="0.1" />
        <inertia ixx="0.000034533333" 
            ixy="0"  ixz="0"
            iyx="0"  iyy="0.000034533333" 
            iyz="0"
            izx="0"  izy="0"  izz="0.0000648" />
    </inertial>
  </link>

  <!-- 右後車輪リンク  -->
  <link name="back_right_wheel_link">
    <visual>
      <origin xyz="0 0 0" rpy="1.57 0 0 "/>
      <geometry>
        <cylinder length="0.025" radius="0.05"/>
      </geometry>
      <material name="black">
        <color rgba="1 1 1 1"/>
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="1.57 0 0 "/>
      <geometry>
        <cylinder length="0.025" radius="0.05"/>
      </geometry>
    </collision>
    <inertial>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <mass value="0.1" />
        <inertia ixx="0.000034533333" 
            ixy="0"  ixz="0"
            iyx="0"  iyy="0.000034533333" 
            iyz="0"
            izx="0"  izy="0"  izz="0.0000648" />
    </inertial>
  </link>

  <!-- 車体リンク <-> 左前車輪リンク をつなげるジョイント -->
  <joint name="front_left_wheel_joint" type="continuous">
    <parent link="body_link"/>
    <child link="front_left_wheel_link"/>
    <origin xyz="0.15 -0.1625 -0.05" rpy="0 0 0"/>
    <axis xyz="0 1 0"/>
  </joint>

  <!-- 車体リンク <-> 右前車輪リンク をつなげるジョイント -->
  <joint name="front_right_wheel_joint" type="continuous">
    <parent link="body_link"/>
    <child link="front_right_wheel_link"/>
    <origin xyz="0.15 0.1625 -0.05" rpy="0 0 0"/>
    <axis xyz="0 1 0"/>
  </joint>

  <!-- 車体リンク <-> 左後車輪リンク をつなげるジョイント -->
  <joint name="back_left_wheel_joint" type="continuous"> 
    <parent link="body_link"/>
    <child link="back_left_wheel_link"/>
    <origin xyz="-0.15 -0.1625 -0.05" rpy="0 0 0"/>
    <axis xyz="0 1 0"/>
  </joint>

  <!-- 車体リンク <-> 右後車輪リンク をつなげるジョイント -->
  <joint name="back_right_wheel_joint" type="continuous">
    <parent link="body_link"/>
    <child link="back_right_wheel_link"/>
    <origin xyz="-0.15 0.1625 -0.05" rpy="0 0 0"/>
    <axis xyz="0 1 0"/>
  </joint>

  <!--超音波センサのリンク-->
  <link name="ultrasonic_link">
    <visual>
      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
      <geometry>
        <box size="0.01 0.01 0.01"/>
      </geometry>
      <material name="black">
        <color rgba="0 0 0 1"/>
      </material>
    </visual>
    <collision>
      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
      <geometry>
        <box size="0.01 0.01 0.01"/>
      </geometry>
    </collision>
    <inertial>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <mass value="0.0001" />
        <inertia ixx="0.000034533333" 
            ixy="0"  ixz="0"
            iyx="0"  iyy="0.000034533333" 
            iyz="0"
            izx="0"  izy="0"  izz="0.0000648" />
    </inertial>
  </link>

  <!--車体リンク <-> 超音波センサのリンク をつなげるジョイント-->
  <joint name="ultrasonic_joint" type="fixed">
    <parent link="body_link"/>
    <child link="ultrasonic_link"/>
    <origin xyz="0.255 0.0 0.0" rpy="0 0 0"/>
    <axis xyz="0 0 1"/>
  </joint>

</robot>

超音波センサの結果を取得

上記のurdfにおいて、ロボットの超音波センサからの結果（hit or not hit)を取得するコードは以下のようになります。（下記コード単体で動くわけでないので注意してください）

def CheckHit(rayFromPosition, rayLength, rayDirectionRad):
    """
    「位置rayFromPosition」から「rayDirectionRad」方向に「rayLength」進んだ距離に障害物があるかをチェック
    Parameters
    ---
    rayFromPosition : list
        rayが始まる位置[x,y,z]
    rayLength : float
        rayの長さ
    rayDirectionRad : float
        rayを発射する方向(rad)
    
    Returns
    ---
    hitResult: tuple
        rayのhitの結果が格納される。具体的には以下のtupleになる
        ((hitしたオブジェクトのid),
         (ヒットしたオブジェクトのlinkのid), 
         (rayの全体長に対するhitした位置の比率[0～1]),
         (hitした位置の座標),
         (hitした法線の座標))
    """
    rayFrom = [rayFromPosition[0], rayFromPosition[1], rayFromPosition[2]]
    rayTo = rayFrom.copy()
    rayTo[0] += rayLength * math.cos(rayDirectionRad)
    rayTo[1] += rayLength * math.sin(rayDirectionRad)
    hitResult = pybullet.rayTest(rayFrom, rayTo)
    return hitResult

# rayの設定
ultrasonicLinkIdx = 4 # 超音波センサ（＝rayの発射位置）のlinkのインデックス
rayDirectionDeg = 0 # rayを発射する方向
rayDirectionRad = math.radians(rayDirectionDeg)
rayLength = 0.1 # rayの長さ

# carの前方にあるカメラからrayLengthの位置に障害物があるかチェック（超音波センサの模擬）
ultrasonicPosition = pybullet.getLinkState(carId, ultrasonicLinkIdx)
rayFromPosition = [ultrasonicPosition[0][0], ultrasonicPosition[0][1], ultrasonicPosition[0][2]]
hitResult = CheckHit(rayFromPosition, rayLength, rayDirectionRad)

移動ロボットの超音波センサを使って障害物を検知

それでは、ここからは実際に超音波センサを搭載した移動ロボットを用いて障害物の検地をおこないます（jupyterまたは、google colablatoryで実行する前提とします）。

以下コードを実行すると移動ロボットが前進し、ロボットが障害物のrayLength = 0.1m 内まで近づくと停止します。

最初に、以下セルを実行します

以下セルを複数回実行してしまうと、正しくシミュレーションできなくなります。
複数回実行してしまった場合は、jupyter（または、google colab）のランタイムを再起動してください

import pybullet
import pybullet_data
import time
import numpy as np
import math
import matplotlib.pyplot as plt
from matplotlib.animation import FuncAnimation
from IPython.display import HTML
# physicsClient = pybullet.connect(pybullet.GUI) # GUIモード（ローカル環境の場合、こちらでもOK） 
physicsClient = pybullet.connect(pybullet.DIRECT) 

次に、以下セルを実行し、シミュレーションの初期設定を行います。

実行する前に、先ほど紹介したurdfファイルを任意の場所に保存し、そこまでのパスをご自身の環境に合わせloadURDF() の第一引数にご記入ください。

（下記コードでは ultrasonic_four_wheel_car.urdfという名前で /content下に配置しています ）。

pybullet.resetSimulation() # シミュレーション空間をリセット
pybullet.setAdditionalSearchPath(pybullet_data.getDataPath()) # pybulletに必要なデータへのパスを追加
pybullet.setGravity(0.0, 0.0, -9.8) # 地球上における重力に設定
timeStep = 1./240.
pybullet.setTimeStep(timeStep)

#床の読み込み
planeId = pybullet.loadURDF("plane.urdf")

# ボックスの読み込み
## ボックスの重さ、サイズ、位置·姿勢を決める
mass = 5 # kg
box_size = [0.3, 0.3, 0.3]
position = [2, 0, 0.3]
orientation = [1, 0, 0, 0] # 四元数
box_cid = pybullet.createCollisionShape(pybullet.GEOM_BOX, halfExtents=box_size, physicsClientId=physicsClient)
box_vid = pybullet.createVisualShape(pybullet.GEOM_BOX, halfExtents=box_size, physicsClientId=physicsClient, rgbaColor=[1,0,0,1]) # 赤・半透明
box_bid = pybullet.createMultiBody(mass, box_cid, box_vid, position, orientation, physicsClientId=physicsClient)

# ロボットの読み込み
carStartPos = [0, 0, 0.1]  # 初期位置(x,y,z)を設定
carStartOrientation = pybullet.getQuaternionFromEuler([0,0,0])  # 初期姿勢(roll, pitch, yaw)を設定
carId = pybullet.loadURDF("/content/ultrasonic_four_wheel_car.urdf",carStartPos, carStartOrientation)

# GUIモードの際のカメラの位置などを設定
cameraDistance = 5.0
cameraYaw = 0.0 # deg
cameraPitch = -90.1 # deg
cameraTargetPosition = [0.0, 0.0, 0.0]
pybullet.resetDebugVisualizerCamera(cameraDistance, cameraYaw, cameraPitch, cameraTargetPosition)

次に、以下セルを実行し、超音波センサの判定結果を取得する関数CheckHitを定義します。

def CheckHit(rayFromPosition, rayLength, rayDirectionRad):
    """
    「位置rayFromPosition」から「rayDirectionRad」方向に「rayLength」進んだ距離に障害物があるかをチェック
    Parameters
    ---
    rayFromPosition : list
        rayが始まる位置[x,y,z]
    rayLength : float
        rayの長さ
    rayDirectionRad : float
        rayを発射する方向(rad)
    
    Returns
    ---
    hitResult: tuple
        rayのhitの結果が格納される。具体的には以下のtupleになる
        ((hitしたオブジェクトのid),
         (ヒットしたオブジェクトのlinkのid), 
         (rayの全体長に対するhitした位置の比率[0～1]),
         (hitした位置の座標),
         (hitした法線の座標))
    """
    rayFrom = [rayFromPosition[0], rayFromPosition[1], rayFromPosition[2]]
    rayTo = rayFrom.copy()
    rayTo[0] += rayLength * math.cos(rayDirectionRad)
    rayTo[1] += rayLength * math.sin(rayDirectionRad)
    hitResult = pybullet.rayTest(rayFrom, rayTo)
    return hitResult

最後に、以下セルを実行するとロボットが前進し、ロボットが障害物のrayLength = 0.1m 内まで近づくと停止する様子が動画に出力されます。

rayLengthの値を変更すると、ロボットが停止するタイミングが変わるため試してみてください。

# カメラ設定
projectionMatrix = pybullet.computeProjectionMatrixFOV(fov=45.0,aspect=1.0,nearVal=0.1,farVal=10)
save_img_freq = 1 # どの程度の間隔でカメラ画像を取得するか

# viewMatrixを取得
cameraTargetPosition = [0.0, 0.0, 0.0]
distance = 5.0
yaw = 180.0
pitch = 0.0
roll = 180.0
upAxisIndex = 1
viewMatrix = pybullet.computeViewMatrixFromYawPitchRoll(cameraTargetPosition, distance, yaw, pitch, roll, upAxisIndex)

# カメラ画像格納用配列
frames = []

# ロボットを初期位置にセット
carStartPos = [0, 0, 0.1]
carStartOrientation = pybullet.getQuaternionFromEuler([0.0, 0.0, 0.0])
carBodyIdx = 0
pybullet.resetBasePositionAndOrientation(carId, carStartPos, carStartOrientation)

# rayの設定
ultrasonicLinkIdx = 4 # rayの発射位置のlinkのインデックス
rayDirectionDeg = 0 # rayを発射する方向
rayDirectionRad = math.radians(rayDirectionDeg)
rayLength = 0.1 # rayの長さ

# carの前方「rayLength(m)」に障害物が見つかるまで前進する
for t in range(800):
    # carの前方にあるカメラからrayLengthの位置に障害物があるかチェック（超音波センサの模擬）
    ultrasonicPosition = pybullet.getLinkState(carId, ultrasonicLinkIdx)
    rayFromPosition = [ultrasonicPosition[0][0], ultrasonicPosition[0][1], ultrasonicPosition[0][2]]
    hitResult = CheckHit(rayFromPosition, rayLength, rayDirectionRad)
    isHit = hitResult[0][0]

    if isHit == -1:
        # carの4輪を前進するように速度制御
        pybullet.setJointMotorControl2(carId, 0, pybullet.VELOCITY_CONTROL, targetVelocity=10)
        pybullet.setJointMotorControl2(carId, 1, pybullet.VELOCITY_CONTROL, targetVelocity=10)
        pybullet.setJointMotorControl2(carId, 2, pybullet.VELOCITY_CONTROL, targetVelocity=10)
        pybullet.setJointMotorControl2(carId, 3, pybullet.VELOCITY_CONTROL, targetVelocity=10)
    else:
        # 障害物が見つかったら停止
        pybullet.setJointMotorControl2(carId, 0, pybullet.VELOCITY_CONTROL, targetVelocity=0)
        pybullet.setJointMotorControl2(carId, 1, pybullet.VELOCITY_CONTROL, targetVelocity=0)
        pybullet.setJointMotorControl2(carId, 2, pybullet.VELOCITY_CONTROL, targetVelocity=0)
        pybullet.setJointMotorControl2(carId, 3, pybullet.VELOCITY_CONTROL, targetVelocity=0)       
    
    time.sleep(timeStep)
    pybullet.stepSimulation()
    
    # save_img_freqごとにカメラ画像を取得
    if t % save_img_freq == 0:
        # カメラ情報を設定
        width, height, rgbImg, depthImg, segImg = pybullet.getCameraImage(300,300, viewMatrix, projectionMatrix, renderer=pybullet.ER_BULLET_HARDWARE_OPENGL)
        
        # 取得したカメラ画像をリストに格納
        frames.append(rgbImg)

# jupyter book内にアニメーションを表示
def update(time, frames):
    plt.cla()
    camera_link_frame_np = np.asarray(frames[time])
    plt.imshow(camera_link_frame_np)
fig = plt.figure()
timeStepMilliSec = timeStep * 1000
ani = FuncAnimation(fig, update, interval=timeStepMilliSec*save_img_freq, frames=len(frames), fargs=(frames,))
HTML(ani.to_jshtml()) # HTMLに
# ani.save('ultrasonic.mp4', writer="ffmpeg") # mp4で保存．これを実行すると処理時間が増加します
# ani.save('ultrasonic.gif', writer="imagemagick") # gifで保存．これを実行すると処理時間が増加します