はじめに

Pybulletの基本的な説明は以下の記事を参照ください。

今回は、Pybulletで実際にロボットを動かす方法をRacecarを使って説明していきます。

bulletのGitHubにはいくつか読み込んで使えるURDFが用意されています。
その中には、R2D2、Kukaのロボットアーム、ドローン、ヒューマノイドなど様々なものが用意されています。

その中でも今回はRacecarについて解説していきます。
(実行環境はGoogle Colaboratoryです)

Racecarの仕様

Racecarの仕様はGitHubにあるURDFファイルを見ることでわかります。しかし、かなり行数があるのとURDFファイルに馴染みがない人にとってはわかりづらいと思うので簡単に解説します。

まずURDFにはリンクとジョイントというものがあります。とりあえず今はリンクが骨、ジョイントが関節というイメージで考えて頂ければ大丈夫です。

実際にロボットを動かすときには、ジョイントをどのくらい動かすかを指定して動かします。そのため、ロボットのどこにどんなジョイントがあるかが分かれば、ロボットを動かすことができるのです。

以下がRacecarにある全てのジョイントです。
それぞれのジョイントには番号がついており動かすときはその番号で指定します

jointIndex	名称
0	base_link_joint
1	chassis_inertia_joint
2	left_rear_wheel_joint
3	right_rear_wheel_joint
4	left_steering_hinge_joint
5	left_front_wheel_joint
6	right_steering_hinge_joint
7	right_front_wheel_joint
8	hokuyo_joint
9	zed_camera_joint
10	zed_camera_left_joint
11	zed_camera_right_joint

このRacecarを実際に走らせる際には


2	left_rear_wheel_joint (左後輪)
3	right_rear_wheel_joint (右後輪)
5	left_front_wheel_joint (左前輪)
7	right_front_wheel_joint (右前輪)

をどのくらい動かすかを指定して動かします。

ロボットの読み込み

実際にロボットを読み込んでみます。

まずはPybulletのインストールと準備です。

(実行環境はGoogle Colaboratoryです)

!pip install pybullet
!git clone https://github.com/bulletphysics/bullet3.git

以後コードの中では、pyublletをpと省略させてください

import pybullet as p
import pybullet_data
import numpy as np
import matplotlib.pyplot as plt
from PIL import Image

Colabではp.DIRECTを指定

physicsClient = p.connect(p.DIRECT)

実際にRacecarを読み込み表示させます。

#シミュレーションの状態をリセット
p.resetSimulation()
p.setAdditionalSearchPath(pybullet_data.getDataPath())

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

#どのくらいの時間間隔でシミュレーションの計算を行うか(ここでは240分の１秒)
timestep = 1. / 240.
p.setTimeStep(timestep)

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

#Racecarの初期状態(位置と向き)を設定
StartPos = [0,0,0]  
StartOrient = p.getQuaternionFromEuler([0,0,3.14]) 

#Racecarの読み込み
carId = p.loadURDF("/content/bullet3/data/racecar/racecar.urdf",StartPos, StartOrient)

#画像の取得
width, height, rgbImg, depthImg, segImg = p.getCameraImage(360,240)

#画像の表示
plt.imshow(rgbImg)
plt.show()

Racecarが表示されます。

ジョイントの動かし方

Pybulletで実際にロボットを動かす際にはいくつかの方法があります。
基本的な方法はsetJointMotorControl2を用いる方法です。

p.setJointMotorControl2(bodyUniqueId, jointIndex, controlMode,...)

bodyUniqueIdでどのロボットを動かすか
jointIndexでそのロボットのどのジョイントを動かすか
controlModeでどう動かすか

指定します

例として

p.setJointMotorControl2(carId, 2, p.VELOCITY_CONTROL, targetVelocity=10)

とするとcarId(Racecar)のジョイント2番(左後輪)を速さが10になるように動かすことができます。
control modeについてはいくつかのコントロールの仕方があり、

p.VELOCITY_CONTROL, targetVelocity=v とするとジョイントを動かす速度を　
p.POSITION_CONTROL, targetPosition=x とするとジョイントを

どの位置に動かすかを指定できます。

例

p.setJointMotorControl2(carId, 4, p.POSITION_CONTROL, targetPosition= 1)

carId(Racecar)のジョイント4番(左側の方向転換ヒンジ)を位置が1になるように動かすことができます。

また、複数のジョイントを同時に動かすときは、p.setJointMotorControlArrayを使うことができます。

例

p.setJointMotorControlArray(carId, [2,3,5,7], p.VELOCITY_CONTROL, targetVelocities=[
10,10,10,10])

ジョイント2,3,5,7番(四輪全て)を速さが10になるように動かせます。

実際に動かす

直進させる

#直進
p.resetSimulation()
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.setGravity(0,0,-10)
timestep = 1. / 240.
p.setTimeStep(timestep)
planeId = p.loadURDF("plane.urdf")
cubeStartPos = [0,0,0]  
cubeStartOrientation = p.getQuaternionFromEuler([0,0,3.14]) 
carId = p.loadURDF("/content/bullet3/data/racecar/racecar.urdf",cubeStartPos, cubeStartOrientation)

frame = []
for t in range (640):
    p.setJointMotorControl2(carId, 2, p.VELOCITY_CONTROL, targetVelocity=10)
    p.setJointMotorControl2(carId, 3, p.VELOCITY_CONTROL, targetVelocity=10)
    p.setJointMotorControl2(carId, 5, p.VELOCITY_CONTROL, targetVelocity=10)
    p.setJointMotorControl2(carId, 7, p.VELOCITY_CONTROL, targetVelocity=10)
    p.stepSimulation()
    if t % 8 == 0:
        width, height, rgbImg, depthImg, segImg = p.getCameraImage(360,240)
        frame.append(rgbImg)

images =[]
for im in frame:
  img = Image.fromarray(im)
  images.append(img)
images[0].save('Racecar_moving1.gif',save_all=True, append_images=images[:], optimize=False, duration=40, loop=0)

実行結果

左折

#左折
p.resetSimulation()
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.setGravity(0,0,-10)
timestep = 1. / 240.
p.setTimeStep(timestep)
planeId = p.loadURDF("plane.urdf")
cubeStartPos = [1,0,0]  
cubeStartOrientation = p.getQuaternionFromEuler([0,0,3.14]) 

viewMatrix = p.computeViewMatrix(cameraEyePosition=[-4, -4, 2],cameraTargetPosition=[0, 0, 0.5],cameraUpVector=[0, 0, 1])
projectionMatrix = p.computeProjectionMatrixFOV(fov=20.0,aspect=1.5,nearVal=0.1,farVal=50)


carId = p.loadURDF("/content/bullet3/data/racecar/racecar.urdf",cubeStartPos, cubeStartOrientation)

planeId = p.loadURDF("plane.urdf")

v=30
frame = []
for t in range (640):
  p.setJointMotorControlArray(carId, [2,3,5,7], p.VELOCITY_CONTROL, targetVelocities=[30,30,30,30])

  p.setJointMotorControl2(carId, 4, p.POSITION_CONTROL, targetPosition= 1)
  p.setJointMotorControl2(carId, 6, p.POSITION_CONTROL, targetPosition= 1)
  p.stepSimulation()
  if t % 8 == 0:
      width, height, rgbImg, depthImg, segImg = p.getCameraImage(360,240,viewMatrix=viewMatrix,projectionMatrix=projectionMatrix)
      frame.append(rgbImg)
      Position, Orientation = p.getBasePositionAndOrientation(carId)

images =[]
for im in frame:
  img = Image.fromarray(im)
  images.append(img)
images[0].save('Racecar_turningleft.gif',save_all=True, append_images=images[:], optimize=False, duration=40, loop=0)

実行結果

右折

#右折
p.resetSimulation()
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.setGravity(0,0,-10)
timestep = 1. / 240.
p.setTimeStep(timestep)
planeId = p.loadURDF("plane.urdf")
cubeStartPos = [1,0,0]  
cubeStartOrientation = p.getQuaternionFromEuler([0,0,3.14]) 

viewMatrix = p.computeViewMatrix(cameraEyePosition=[-4, -4, 2],cameraTargetPosition=[0, 0, 0.5],cameraUpVector=[0, 0, 1])
projectionMatrix = p.computeProjectionMatrixFOV(fov=20.0,aspect=1.5,nearVal=0.1,farVal=50)


carId = p.loadURDF("/content/bullet3/data/racecar/racecar.urdf",cubeStartPos, cubeStartOrientation)

planeId = p.loadURDF("plane.urdf")

v=30
frame = []
for t in range (640):
  p.setJointMotorControlArray(carId, [2,3,5,7], p.VELOCITY_CONTROL, targetVelocities=[30,30,30,30])
  p.setJointMotorControl2(carId, 4, p.POSITION_CONTROL, targetPosition= -1)
  p.setJointMotorControl2(carId, 6, p.POSITION_CONTROL, targetPosition= -1)
  p.stepSimulation()
  if t % 8 == 0:
      width, height, rgbImg, depthImg, segImg = p.getCameraImage(360,240,viewMatrix=viewMatrix,projectionMatrix=projectionMatrix)
      frame.append(rgbImg)
      Position, Orientation = p.getBasePositionAndOrientation(carId)

images =[]
for im in frame:
  img = Image.fromarray(im)
  images.append(img)
images[0].save('Racecar_turningright.gif',save_all=True, append_images=images[:], optimize=False, duration=40, loop=0)

実行結果

最後に

今回は、Pybulletにおけるロボットの動かし方をRacecarを使って説明しました。

間違いやコードに動かない部分があれば、遠慮なくコメントで指摘をしてください。

Pybullet ロボットシミュレーション~ Racecar~

はじめに

Racecarの仕様

ロボットの読み込み

ジョイントの動かし方

実際に動かす

最後に