PyBulletでURDF(Unified Robot Description Format) 　URDF解説編(4)

　前回はロボットtr1のurdfモデルを作りました。

　今回はロボットtr1をPyBulletでシミュレーションさせます。

1.plane.urdf（床）の準備

　シミュレーションするには地上に相当する床を定義する必要があります。マニュアルを見るとコード上で定義できるはずですが、うまくいかなかったので各種サンプルと同様urdfで定義しました。

　次のコードをパソコン上にplane.urdfとして保存します。縦横10mで厚さ1cm、摩擦係数(friction)=1.0の床を定義しています。


<?xml version="1.0" ?>
<robot name="plane">
  <link name="planeLink">
  <contact>
      <lateral_friction value="1"/>
  </contact>
  <inertial>
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <mass value=".0"/>
    <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
  </inertial>
  <visual>
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <geometry>
      <box size="10 0.01 10"/>
    </geometry>
    <material name="Gray">
      <color rgba="0.8 0.8 0.8 1" />
    </material>
  </visual>
  <collision>
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <geometry>
    <box size="10 0.01 10"/>
    </geometry>
  </collision>
  </link>
</robot>

2.PyBulletで動かしてみる

　いつもの通りColaboratoryを使います。

(1)ライブラリのインストール

!apt-get update
!apt-get -qq -y install xvfb freeglut3-dev ffmpeg
!pip -q install pyglet
!pip -q install pyopengl
!pip -q install pyvirtualdisplay
!apt-get install x11-utils
!pip install pybullet

(2)plane.urdfをパソコンからColaboratoryにアップロード

!rm p*.urdf
from google.colab import files
uploaded = files.upload()

(3)tr1.urdfをパソコンからColaboratoryにアップロード

　tr1.urdfは前回の記事で作ったものを使います。

!rm t*.urdf
from google.colab import files
uploaded = files.upload()

(4)PyBulletで動かしてみます。

import matplotlib
import matplotlib.pyplot as plt
import matplotlib.animation
import numpy as np
import time
from pyvirtualdisplay import Display
import pybullet as p
from pybullet_utils import bullet_client
from IPython.display import HTML
from matplotlib.animation import PillowWriter


#表示の準備
display = Display(visible=0, size=(1024, 768))
display.start()
import os
os.environ["DISPLAY"] = ":" + str(display.display) + "." + str(display.screen)

#PyBullet開始
p.connect(p.DIRECT)
#PCの場合はp.GUI接続もあり
#p.connect(p.GUI)
#PyBulletで複数モデルをシミュレーションする場合はclientを作成し、
#以降はpの代わりに_pを使う（参考）。
#_p=bullet_client.BulletClient()


#urdfファイルをPyBulletに読み込む
IDtr1 = p.loadURDF("tr1.urdf",basePosition = [0,0.3,0])
p.loadURDF("plane.urdf",basePosition = [0,0,0], useFixedBase=True)

#表示のための投影マトリクス計算
proj_matrix = p.computeProjectionMatrixFOV(
			fov=60, aspect=320/240,
			nearVal=0.1, farVal=100.0)

#jointコントローラ
for joint in range(p.getNumJoints(IDtr1)):
  info = p.getJointInfo(IDtr1, joint)
  print(info)
	# info[2]=4は固定軸
  if info[2] != 4:
    p.setJointMotorControl2(IDtr1, joint, p.POSITION_CONTROL, targetPosition=0.0, force=10000,positionGain = 0.8)

#シミュレーション ---------------------------------------------------
#画像保存リスト初期化
frames = []

#重力
p.setGravity(0, -9.8, 0)

#step time
p.setTimeStep(0.005)

for i in range(100):
  tr1pos, tr1orn = p.getBasePositionAndOrientation(IDtr1)

#表示のためのカメラ設定
#cameraTargetPosition 対象の位置
#distance　対象からカメラの距離
#upAxisIndex 1:y軸が上下 2:z軸が上下
  view_matrix = p.computeViewMatrixFromYawPitchRoll(
			cameraTargetPosition=tr1pos,
			distance=0.4,
			yaw=-60,
			pitch=-10,
			roll=0,
			upAxisIndex=1)

#描画計算
  (_, _, px, _, _) = p.getCameraImage(1024,1024,viewMatrix=view_matrix,projectionMatrix=proj_matrix,renderer=p.ER_BULLET_HARDWARE_OPENGL)

#pyplotで画面に表示させるためリスト形式をnumpy array形式に変換
#alphaを削除して表示
  rgb_array = np.array(px)
  rgb_array = rgb_array[:, :, :3]
  frames.append(rgb_array)
  p.stepSimulation()

p.disconnect()
#シミュレーション終了 -----------------------------------------------

#動画作成
fig = plt.figure(figsize=(3.2, 2.5))
plt.axis('off')
plt.subplots_adjust(left=0, right=1, bottom=0, top=1)
  
images = []
for f in frames:
  image = plt.imshow(f)
  images.append([image])

ani = matplotlib.animation.ArtistAnimation(fig, images, interval=30, repeat_delay=1)

#gifを保存する場合はGoogleDriveをマウントすること
#ani.save("/content/drive/My Drive/images.gif", writer="pillow")

HTML(ani.to_jshtml())

　jointの一覧が表示された後、次のような動画が表示されます。

　カメラがロボットに追従しているのでわかりずらいですが、5cmくらい降下して着地した時、jointが衝撃で曲がったのを制御で角度0に戻しています。

　ちなみにロボットは正面から見ています。

f:id:akifukka:20200301123643g:plain

(5)PyBulletの解説

PyBulletの開始、終了
p.connect(p.DIRECT)：開始　p.disconnect()：終了
tr1.urdfを読み込み、tr1の原点を高さ0.3mの位置に配置します。以降ロボットtr1をIDtr1で指定します。
IDtr1 = p.loadURDF("tr1.urdf",basePosition = [0,0.3,0])
plane.urdfを原点に読み込み、固定します。
p.loadURDF("plane.urdf",basePosition = [0,0,0], useFixedBase=True)
IDtr1のjoint情報を一覧で表示します。
for joint in range(p.getNumJoints(IDtr1)):
info = p.getJointInfo(IDtr1, joint)
print(info)
表示内容の詳細は、PyBullet Quickstart Guideを参照してください。
jointの制御を指定します。上記のforループ内で実施しています。jointのinfo[2]はjoint種別を示していて、4はFix jointです。TR1のjointはFixとRevolutionしかないので、Revolutionについて制御を指定しています。
if info[2] != 4:
p.setJointMotorControl2(IDtr1, joint, p.POSITION_CONTROL, targetPosition=0.0, force=10000,positionGain = 0.8)

p.POSITION_CONTROL：制御方式。まずは位置制御としました。targetPosition　　　　：位置目標値
force　　　　　　　　：最大トルク(N･m)。10000N･mは約10kg･cmに相当positionGain　　　　　：位置制御のゲイン。指定しないと踏ん張らなかったので
重力を指定します。y軸を上下方向に使っているので、y軸のマイナス方向に重力を指定します。
p.setGravity(0, -9.8, 0)
1回の計算のタイムステップを指定します。タイムステップが長いとjointの制御が発散してしまいます。今回は5msにしています。
p.setTimeStep(0.005)
繰り返し各ステップを演算します。とりあえず100ステップなので0.5秒を計算しています。
for i in range(100):
・・・・
p.stepSimulation()
ロボットTR1の中心座標を取得してカメラをセッティング、画像を作ってリストframesに追加します。
tr1pos, tr1orn = p.getBasePositionAndOrientation(IDtr1)
view_matrix = p.computeViewMatrixFromYawPitchRoll(
・・・・・
frames.append(rgb_array)