Open AI Gym Box2D BipedalWalkerをColaboratoryで動かしてみる(2)
前回の続きです。ColaboratoryでBipedicalWalkerを動かすセッティング等は前回の記事を参照ください。
まずは、ニューラルネットワークを使わずにとにかく動かして、BipedicalWalkerがどのようなものなのか試してみます。歩くとこまで行くかどうか（現在も試行錯誤中）。
７．とにかく動かしてみる
(1)角度制御を追加してとにかく動かす
ソースリストを示します。計算部分と表示部分の2つにわけてありますので、Colaboratoryで動かす際は順番に動かしてください。
・計算部分
import torch
import numpy as np
import gym
from pyvirtualdisplay import Display
from scipy.interpolate import interp1d
display = Display(visible=0, size=(1024, 768))
display.start()
import os
#pyvirtualdisplayの仕様変更で次の文はエラーになるのでコメントアウトします（2021/6/5修正）
#os.environ["DISPLAY"] = ":" + str(display.display) + "." + str(display.screen)
#Bipedalwalkerのv2は無くなったのでv3に変更（2021/6/5）
env = gym.make('BipedalWalker-v3')
#角度制御ゲイン
#P(比例)項
pgain = np.array([4,4,4,4], dtype = 'float')
#D(微分)項
dgain = np.array([0.2,0.2,0.2,0.2], dtype = 'float')
#脚の指令角スケジュール
xsch = np.array([0,1,2,3,4,5,6,7,8,9,10],dtype = 'float')
yhip1 = np.array([ 0, -0.3, -0.6, -0.6, -0.3, 0, 0.85, 0.7, 0.5, 0.3, 0],dtype = 'float')
yknee1 = np.array([ 0.2, 0.9, 0.8, 0.7, -0.4, 0, 0.0, 0.0, 0.0, 0.0, 0.0],dtype = 'float')
yhip2 = np.array([ 0.85, 0.7, 0.5, 0.3, 0, -0.3, -0.6, -0.6, -0.3, 0, 0.85],dtype = 'float')
yknee2 = np.array([ -0.4, 0.0, 0.0, 0.0, 0.2, 0.9, 0.8, 0.7, -0.4, 0, 0],dtype = 'float')
numx = xsch.shape[0]
hip1demmand = interp1d(xsch, yhip1)
knee1demmand = interp1d(xsch, yknee1)
hip2demmand = interp1d(xsch, yhip2)
knee2demmand = interp1d(xsch, yknee2)
graphschys =[]
graphschys.append([hip1demmand(0)])
graphschys.append([knee1demmand(0)])
graphschys.append([hip2demmand(0)])
graphschys.append([knee2demmand(0)])
graphschxs =[0.0]
for graphschx in np.arange(0.1,10.0,0.1):
graphschxs.append(graphschx)
graphschys[0].append(hip1demmand(graphschx))
graphschys[1].append(knee1demmand(graphschx))
graphschys[2].append(hip2demmand(graphschx))
graphschys[3].append(knee2demmand(graphschx))
#モデルタイムステップ(1秒間に50フレーム = 0.02s)
FPS = 50.0
for i in range(1):
obs = env.reset()
done = False
frames = []
observations = []
actions = []
ts = []
legangle = np.array([0,0,0,0], dtype = 'float')
leganglespeed = np.array([0,0,0,0], dtype = 'float')
legangledemand = np.array([0,0,0,0], dtype = 'float')
speed = 5
x = 0.0
t = 0
leg1gcount = 0
leg2gcount = 0
leg1touchground =0
leg2touchground =0
while not done and t < 400:
#動画用画面保存-------------------------------
frames.append(env.render(mode = 'rgb_array'))
#指令値生成-----------------------------------
x += speed/FPS
if x>10.0 :x-=10
#接地でスケジュール強制的に進める
# if x>0.5 and x<2.5 and leg2touchground == 1:x = 4.5 - x
# if x>5.5 and x<7.5 and leg1touchground == 1:x = 9.5 - (x - 5)
legangledemand[0] = hip1demmand(x)
legangledemand[1] = knee1demmand(x)
legangledemand[2] = hip2demmand(x)
legangledemand[3] = knee2demmand(x)
#角度指令値上書き。スケジュールで動かす時はコメントアウトすること
legangledemand = np.array([1,1,1,1], dtype = 'float')
#脚の角度 PD制御
action = pgain * (legangledemand - legangle) - dgain * leganglespeed
#2足歩行物理モデル1ステップ(0.02s)-------------
observation, reward, done, info = env.step(action)
#脚の角度,角速度,胴体角取り出し----------------
legangle[0] = observation[4] #HIP1
leganglespeed[0] = observation[5]
legangle[1] = observation[6] #KNEE1
leganglespeed[1] = observation[7]
legangle[2] = observation[9] #HIP2
leganglespeed[2] = observation[10]
legangle[3] = observation[11] #KNEE2
leganglespeed[3] = observation[11]
#leg1 接地判定 チャタリング防止
if observation[8] == 1:
if leg1gcount <3 :leg1gcount += 1
else:
leg1gcount = 0
if leg1gcount == 3:
leg1touchground = 1
else:
leg1touchground = 0
#leg2 接地判定 チャタリング防止
if observation[13] == 1:
if leg2gcount <3 :leg2gcount += 1
else:
leg2gcount = 0;
if leg2gcount == 3:
leg2touchground = 1
else:
leg2touchground = 0
#グラフ用データ保存----------------------------
if t == 0 :
for i in range(24):
observations.append([observation[i]])
for i in range(4):
actions.append([action[i]])
else :
for i in range(24):
observations[i].append(observation[i])
for i in range(4):
actions[i].append(action[i])
ts.append(t/FPS)
#時間を進める----------------------------------
t += 1・表示部分
import matplotlib.pyplot as plt
import matplotlib.animation
import numpy as np
from IPython.display import HTML
fig1 = plt.figure(figsize=(12, 16))
sub1 = fig1.add_subplot(621)
sub1.plot(ts,observations[0],label="hull angle")
sub1.plot(ts,observations[1],label="hull angle Velocity")
sub1.legend()
sub2 = fig1.add_subplot(622)
sub2.plot(ts,observations[2],label="Velocity x")
sub2.plot(ts,observations[3],label="Velocity y")
sub2.legend()
sub3 = fig1.add_subplot(623)
sub3.plot(ts,observations[4],label="Hip1 Angle")
sub3.plot(ts,observations[9],label="Hip2 Angle")
sub3.legend()
sub4 = fig1.add_subplot(624)
sub4.plot(ts,observations[5],label="Hip1 Angle Velocity")
sub4.plot(ts,observations[10],label="Hip2 Angle Velocity")
sub4.legend()
sub5 = fig1.add_subplot(625)
sub5.plot(ts,observations[6],label="Knee1 Angle")
sub5.plot(ts,observations[11],label="Knee2 Angle")
sub5.legend()
sub6 = fig1.add_subplot(626)
sub6.plot(ts,observations[7],label="Knee1 Angle Velocity")
sub6.plot(ts,observations[12],label="Knee2 Angle Velocity")
sub6.legend()
sub7 = fig1.add_subplot(627)
sub7.plot(ts,observations[8],label="Leg1 Touch Ground")
sub7.plot(ts,observations[13],label="Leg2 Touch Ground")
sub7.legend()
sub8 = fig1.add_subplot(326)
sub8.plot(ts,actions[0],label="HIP1 Angle Velocity Demand")
sub8.plot(ts,actions[1],label="KNEE1 Angle Velocity Demand")
sub8.plot(ts,actions[2],label="HIP2 Angle Velocity Demand")
sub8.plot(ts,actions[3],label="KNEE2 Angle Velocity Demand")
sub8.set_ylim(-2.0,2.0)
sub8.legend()
sub9 = fig1.add_subplot(325)
sub9.plot(graphschxs,graphschys[0],label="HIP1 Angle Sch")
sub9.plot(graphschxs,graphschys[1],label="KNEE1 Angle Sch")
sub9.plot(graphschxs,graphschys[2],label="HIP2 Angle Sch")
sub9.plot(graphschxs,graphschys[3],label="KNEE2 Angle Sch")
sub8.set_ylim(-1.0,1.0)
sub9.legend()
plt.show()
fig2 = plt.figure(figsize=(6, 6))
plt.axis('off')
images = []
for f in frames:
image = plt.imshow(f)
images.append([image])
ani = matplotlib.animation.ArtistAnimation(fig2, images, interval=30, repeat_delay=1)
HTML(ani.to_jshtml())
モデルの入力は脚の関節の角速度です。これでは歩行させるためのスケジュールを決めずらいので、まずは決まった角度に制御するよう制御回路を追加します。制御回路の構造を図に示します。
上記じゃなきゃダメというわけでは無く、他にも様々な構造を考えることができます。制御構造は何を重視するか（目標角度と実際の角度の差、偏差を小さくするのを重視したいのか、差はちょっとくらいはあっても、即応性を重視したいのかなど）によって変わってくるのですが、今回はバランスの良い（というか、だいたい何でもそこそこ対応できる万能型）PID制御から、即応性を重視してP（比例）、D（微分）を使うPD制御としました（I（積分）項は使わないことにしました）。
図を見るとわかるように、P項、D項は次のように動きます。
- P項
目標と今の位置が大きくずれていたら、指令値もそれに応じて大きくし早く動かす。 - D項
目標へ向かう速度が早すぎ、最後に止まれず行き過ぎてしまう（オーバーシュート）。オーバシュート量を減らすため、速度が出たら指令値を少し小さくし、速度を加減するよう動く。
P項、D項をどのくらい効かせるかはPゲイン、Dゲインの値で決めますが、今回は何回か動かして適当（何となくうまく動く程度）に次の値に決めました（ソースリストの上の方）
#角度制御ゲイン
#P(比例)項
pgain = np.array([4,4,4,4], dtype = 'float')
#D(微分)項
dgain = np.array([0.2,0.2,0.2,0.2], dtype = 'float')
それぞれ、脚1の腿、脚1の膝、脚2の腿、脚2の膝のゲインです。
PD制御の計算部分はソースリストのforループの中です。それぞれの値はnumpyのテンソル(np.array)にして1行で書いてます。
#脚の角度 PD制御
action = pgain * (legangledemand - legangle) - dgain * leganglespeed
現在の脚の角度(legangle)、角速度(leganglespeed)はソースリストのforループの中で次のようにobservationから取り出します。
#脚の角度,角速度,胴体角取り出し----------------
legangle[0] = observation[4] #HIP1
leganglespeed[0] = observation[5]
legangle[1] = observation[6] #KNEE1
leganglespeed[1] = observation[7]
legangle[2] = observation[9] #HIP2
leganglespeed[2] = observation[10]
legangle[3] = observation[11] #KNEE2
leganglespeed[3] = observation[11]
では、ソースリストの次の文の角度指令値を色々変更して動かしてみます。
#角度指令値上書き。スケジュールで動かす時はコメントアウトすること
legangledemand = np.array([1,1,1,1], dtype = 'float')
結果
指令値(legangledemand)は脚1の腿の角度、脚1の膝の角度、脚2の腿の角度、脚2の膝の角度です。
この結果から、次のことがわかります。
- 脚1は薄い紫、脚2は濃い紫
- 腿の角度は1 ~ －1の範囲で動く。前が正、後ろが負の値。
- 膝の角度は1 ~ －0.6の範囲で動く。1が伸ばした状態、－0.6がもっとも曲げた状態。
さてさて、ソースリストの補足です
ソースリスト中で指令値を与えている行をコメントアウトすると、ソースリストの上の方で"#脚の指令角スケジュール”として与えているスケジュールに従い、脚を動かして歩こうとします（何歩か歩けよォ。心の中の叫び）。
が、結構難しくてうまく歩いてくれません・・・・。スケジュールの値、speed値（1秒間にスケジュールをどのくらい進めるか）を変えて試しているんですが・・。
スケジュールは0~10のステップを1巡（脚1で1歩、脚2で1歩、計2歩で元に戻る）として、繰り返します。speed=5だと、1秒でスケジュール半分相当なので、1秒1歩です。
このあたりは、うまく歩けてないので大幅に修正するかも知れませんが、現状でも色々試せると思います。
つづく
どうやったらもう少し歩けるか思案して、人力でのチューニングはあきらめ乱数を使って半自動でチューニングしました。なんとか2,3歩ですが、歩けるようになりました。歩き方が若干変ですが・・・（何かぎこちないスキップ？）。
