Go to Qiita Advent Calendar 2024 Top
LoginSignup
5
1

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?

More than 1 year has passed since last update.

@KamikawaTakato(Kamikawa Takato)

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

Last updated at Posted at 2023-07-09

はじめに

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

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

Racecar_moving1.gif

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

その中でも今回は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が表示されます。
Unknown-4.png

ジョイントの動かし方

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)

実行結果
Racecar_moving1.gif

左折

#左折
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)

実行結果
Racecar_turningleft.gif

右折

#右折
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)

実行結果
Racecar_turningright.gif

最後に

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

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

5
1
1

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
5
1

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?