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

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

https://qiita.com/KamikawaTakato/items/f7ae65c25bc30b51e96d

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

![Racecar_moving1.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/909214/17c317d8-14d6-e445-3286-4ad5751a6afd.gif)

[bulletのGitHub](https://github.com/bulletphysics/bullet3)にはいくつか読み込んで使えるURDFが用意されています。
その中には、R2D2、Kukaのロボットアーム、ドローン、ヒューマノイドなど様々なものが用意されています。
![Various_robots-2.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/909214/de271c33-1ecb-9b4e-7d00-c46466745c9c.png)

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

# Racecarの仕様


Racecarの仕様は[GitHubにあるURDFファイル](https://github.com/bulletphysics/bullet3/blob/master/data/racecar/racecar.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と省略させてください

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

Colabではp.DIRECTを指定
```python
physicsClient = p.connect(p.DIRECT) 
```


実際にRacecarを読み込み表示させます。
```python
#シミュレーションの状態をリセット
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](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/909214/aa518357-34fb-f5e3-b8e9-8e33d9b03700.png)



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

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

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

指定します

例として
```python
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 とするとジョイントを

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

例
```python
p.setJointMotorControl2(carId, 4, p.POSITION_CONTROL, targetPosition= 1)
```
carId(Racecar)のジョイント4番(左側の方向転換ヒンジ)を位置が1になるように動かすことができます。

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

例
```python
p.setJointMotorControlArray(carId, [2,3,5,7], p.VELOCITY_CONTROL, targetVelocities=[
10,10,10,10])
```
ジョイント2,3,5,7番(四輪全て)を速さが10になるように動かせます。

# 実際に動かす


直進させる

```python
#直進
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](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/909214/17c317d8-14d6-e445-3286-4ad5751a6afd.gif)


左折
```python
#左折
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](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/909214/d03bea97-2357-5a03-154a-bef472c1ea2d.gif)



右折
```python
#右折
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](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/909214/25c1190a-a950-95c2-4d1a-91969b79b66c.gif)


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

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