【pybullet】ロボットにカメラを取り付ける

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

今回は、pybulletのロボットにカメラを取り付ける方法について紹介します。

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

• 移動ロボットの前方にあるカメラから画像を取得する

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

pybulletではROSとは異なり、ロボットに直接カメラを取り付けるようなことは（恐らく）できません。

そこで、urdfに「カメラ用の仮想的なリンク」を設定することで、「疑似的に」ロボットにカメラを取り付けることにします。

pybulletにおけるカメラ

まず、pybulletにおけるカメラの扱いについて解説します。
pybulletでは getCameraImageという関数を使用することでカメラ画像を取得することが出来ます。

width, height, rgbImg, depthImg, segImg = pybullet.getCameraImage(300,300, viewMatrix, projectionMatrix)

ここで、

• viewMatrixは「カメラをどの位置から撮影するか」などの情報が含まれた変数
• projectionMatrixは「カメラの設定（どの程度の距離の物体まで撮影するかなど）」に関する情報が含まれた変数

になります。

ここで、重要となるのがviewMatrix関数になります。

viewMatrix

viewMatrixはcomputeViewMatrix関数を使用して作成します。

viewMatrix = pybullet.computeViewMatrix(cameraEyePosition,cameraTargetPosition,cameraUpVector)

ここで、

• cameraEyePositionはカメラの始点
• cameraTargetPositionはカメラの注視点
• cameraUpVectorは、上方向のベクトル（どのベクトル方向を上とするか。ここは、認識が少し曖昧なので間違っているかもしれません。。。）

となり、図にすると以下のようになります。

urdfをpybullet用に編集する

上記で説明した通り、pybulletにおけるカメラは「始点」と「注視点」の座標が分かればいいので、
urdfで

• カメラリンク（始点）
• 注視点用の仮想的なリンク

を定義すればよいということになります。

今回は、以下のようなシンプルな4輪駆動の移動ロボットを使用します。

<?xml version="1.0" ?>
<robot name="simple_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="camera_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="camera_joint" type="fixed">
    <parent link="body_link"/>
    <child link="camera_link"/>
    <origin xyz="0.16 0.0 0.0" rpy="0 0 0"/>
    <axis xyz="0 0 1"/>
  </joint>

  <!--カメラの注視点用の仮想的なリンク-->
  <link name="target_position_vertual_link">
    <!--何も設定しないと、質量が1kgになってしまうので、すごく小さな値に設定しておく-->
    <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="camera_target_joint" type="fixed">
    <parent link="camera_link"/>
    <child link="target_position_vertual_link"/>
    <origin xyz="0.1 0.0 0.0" rpy="0 0 0"/>
    <axis xyz="0 0 1"/>
  </joint>

</robot>

ここで、 「注視点用の仮想的なリンク」 を設定していることがポイントです。

カメラを設定する

上記のurdfをもとに、ロボットのカメラからの画像を取得するコードは以下のようになります。

cameraIdx = 4 # カメラリンク
cameraTargetIdx = 5 # 注視点用の仮想的なリンク

# カメラリンクの位置を取得
camera_link_pose = pybullet.getLinkState(carId, cameraIdx)[0] 

# 注視点用の仮想的なリンクの位置を取得
camera_target_link_pose = pybullet.getLinkState(carId, cameraTargetIdx)[0] 

# カメラリンク -> 注視点用の仮想的なリンク 方向のviewMatrixを取得
viewMatrix = pybullet.computeViewMatrix(cameraEyePosition=[camera_link_pose[0], camera_link_pose[1], camera_link_pose[2]],cameraTargetPosition=[camera_target_link_pose[0], camera_target_link_pose[1], camera_target_link_pose[2]],cameraUpVector=[0, 0, 1])

# カメラ情報を設定
projectionMatrix = pybullet.computeProjectionMatrixFOV(fov=45.0,aspect=1.0,nearVal=0.1,farVal=10)

# viewMatrix, projectionMatrixの情報をもとに300×300の画像を取得
width, height, rgbImg, depthImg, segImg = pybullet.getCameraImage(300,300, viewMatrix, projectionMatrix)

ロボットを動かしながら、カメラ画像を取得する

それでは、実際にロボットを動かしながらカメラ撮影をしてみましょう（jupyterまたは、google colablatoryで実行する前提とします）。

以下コードを実行すると、200時刻の間、前進しながらロボットについているカメラで撮影を行います。

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

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

import pybullet
import pybullet_data

# physicsClient = pybullet.connect(pybullet.GUI) # GUIモード（ローカル環境の場合、こちらでもOK） 
physicsClient = pybullet.connect(pybullet.DIRECT) 

pybullet.setAdditionalSearchPath(pybullet_data.getDataPath()) # pybulletに必要なデータへのパスを追加

次に、以下セルを実行します。

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

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

import matplotlib.pyplot as plt
import numpy as np
from matplotlib.animation import FuncAnimation
from IPython.display import HTML

pybullet.resetSimulation(physicsClient) # シミュレーション空間をリセット
pybullet.setGravity(0.0, 0.0, -9.8) # 地球上における重力に設定

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

# 赤色の物体を生成
box_cid = pybullet.createCollisionShape(pybullet.GEOM_BOX, halfExtents=[0.3,0.3,0.3], physicsClientId=physicsClient)
box_vid = pybullet.createVisualShape(pybullet.GEOM_BOX, halfExtents=[0.3,0.3,0.3], physicsClientId=physicsClient, rgbaColor=[1,0,0,1]) # 赤・半透明
mass = 5 # kg
position = [2, 0, 0.3] # 生成位置
orientation = [1, 0, 0, 0] # 姿勢（クォータニオン）
box_bid = pybullet.createMultiBody(mass, box_cid, box_vid, position, orientation, physicsClientId=physicsClient)

# 4輪カーロボットのurdfファイルを読み込む
carStartPos = [0, 0, 0.1]  # 初期位置(x,y,z)を設定
carStartOrientation = pybullet.getQuaternionFromEuler([0,0,0])  # 初期姿勢(roll, pitch, yaw)を設定

# ここのパスは、上記のurdfを保存した場所のパスを指定してください。
carId = pybullet.loadURDF("/content/simple_four_wheel_car.urdf",carStartPos, carStartOrientation)

# 時刻設定
time_length = 200
save_img_freq = 1 # 画像を保存する頻度（値が小さいほどより滑らかな動画になるが、処理時間が増加する）

# カメラ設定
projectionMatrix = pybullet.computeProjectionMatrixFOV(fov=45.0,aspect=1.0,nearVal=0.1,farVal=10)

# カメラ画像格納用配列
camera_link_frame = []
    
# リンクのインデックス
cameraIdx = 4
cameraTargetIdx = 5    

# ジョイントのインデックス
frontLeftWheelJointIdx = 0
frontRightWheelJointIdx = 1
BackLeftWheelJointIdx = 2
BackRightWheelJointIdx = 3

for t in range (time_length):
    # 4輪のリンクを速度制御
    pybullet.setJointMotorControl2(carId, frontLeftWheelJointIdx, pybullet.VELOCITY_CONTROL, targetVelocity=10)
    pybullet.setJointMotorControl2(carId, frontRightWheelJointIdx, pybullet.VELOCITY_CONTROL, targetVelocity=10)
    pybullet.setJointMotorControl2(carId, BackLeftWheelJointIdx, pybullet.VELOCITY_CONTROL, targetVelocity=10)
    pybullet.setJointMotorControl2(carId, BackRightWheelJointIdx, pybullet.VELOCITY_CONTROL, targetVelocity=10)
    
    # 一時刻分、シミュレーションを進める
    pybullet.stepSimulation()

    # save_img_freqごとにカメラ画像を取得
    if t % save_img_freq == 0:
        # カメラリンクの位置を取得
        camera_link_pose = pybullet.getLinkState(carId, cameraIdx)[0]

        # 注視点用の仮想的なリンクの位置を取得
        camera_target_link_pose = pybullet.getLinkState(carId, cameraTargetIdx)[0] 
        
        # カメラリンク -> 注視点用の仮想的なリンク 方向のviewMatrixを取得
        viewMatrix = pybullet.computeViewMatrix(cameraEyePosition=[camera_link_pose[0], camera_link_pose[1], camera_link_pose[2]],cameraTargetPosition=[camera_target_link_pose[0], camera_target_link_pose[1], camera_target_link_pose[2]],cameraUpVector=[0, 0, 1])
        
        # カメラ情報を設定
        width, height, rgbImg, depthImg, segImg = pybullet.getCameraImage(300,300, viewMatrix, projectionMatrix)
        
        # 取得したカメラ画像をリストに格納
        camera_link_frame.append(rgbImg)
        
# jupyter book内にアニメーションを表示
def update(time, camera_link_frame):
    plt.cla()
    camera_link_frame_np = np.asarray(camera_link_frame[time])
    plt.imshow(camera_link_frame_np)
fig = plt.figure()
ani = FuncAnimation(fig, update, interval=4, frames=len(camera_link_frame), fargs=(camera_link_frame,))
HTML(ani.to_jshtml()) # HTMLに
#ani.save('robot_camera.mp4', writer="ffmpeg") # mp4で保存．これを実行すると処理時間が増加します
# ani.save('robot_camera.gif', writer="imagemagick") # gifで保存．これを実行すると処理時間が増加します

セルを実行して、しばらくすると下記のような動画が生成されます。

ロボットが前進し、前方にある赤い物体に近づいていく様子が撮影されていることが分かります。

GUIモードで起動していた場合、以下のように俯瞰からもロボットが赤い物体に近づいていることが分かります。

さいごに

今回は、pybulletでロボットにカメラを取り付ける方法について解説しました。

pybulletはgoogle colabなどで手軽に試すことが出来るので、ぜひ試してみてください。