Skip to content

Instantly share code, notes, and snippets.

@peace098beat

peace098beat/CircleRaceGUI3.py

Created November 20, 2015 05:20
Show Gist options
  • Save peace098beat/d543724ae3a1ba4296df to your computer and use it in GitHub Desktop.
Save peace098beat/d543724ae3a1ba4296df to your computer and use it in GitHub Desktop.
Download ZIP
[機械学習] 車のレース. GUI付き。
Raw
CircleRaceGUI3.py
#! coding:utf-8
"""
CircleRace main.py
サークルレースゲーム
(2015/11/20) 4:40 ver0.3 センサグラフ追加
(2015/11/19) 21:50 ver0.2 センサ追加
(2015/11/19) 4:47 ver0.1作成
Created by 0160929 on 2015/11/19 7:28
"""
# TODO: デバッグ用のテキストを表示(ステップ数, 速度, ステアリング, 旋回半径)
__version__ = '0.3'
import sys
import os
import numpy as np
# PySide系モジュール
from PySide.QtGui import *
from PySide.QtCore import *
# logを保存
# --------------------
from datetime import datetime
import csv
def resetlog():
os.remove('log.csv')
# ログファイルを削除
resetlog()
def debuglog(s=None, data=None):
d = datetime.now().isoformat()
with open('log.csv', 'a') as f:
writer = csv.writer(f, lineterminator='\n')
if not s is None:
writer.writerow([d, s])
print d, s
if not data is None:
writer.writerow([d, data])
print d, data
return
class Car(object):
"""
車オブジェクト
自車の速度・位置の情報を持つ
"""
SENCER_LENGTH = 100.
def __init__(self, iniPos=[0, 0], iniVelocity=[1, 1], sencer_angle=30):
# -- 自車情報 ----------------------
self.position = np.array([0, 0])
# velocityはnorm=1ではない
self.velocity = np.array([0, 0])
self.velocity_n = np.array([0, 0])
self.speed = 1
# -- 初期値代入 ---------------------
if not iniPos is None:
_iniPos = np.asarray(iniPos)
self.position = _iniPos
if not iniVelocity is None:
_iniVelocity = np.asarray(iniVelocity)
self.velocity_n = self.vectorNormalize(_iniVelocity)
self.velocity = self.velocity_n * self.speed
# -- センサの情報-------------------
self.sencerL_angle_deg = sencer_angle
self.sencerR_angle_deg = -1. * self.sencerL_angle_deg
self.sencer_length = self.SENCER_LENGTH
self.sencerF = np.array([0, 0])
self.sencerL = np.array([0, 0])
self.sencerR = np.array([0, 0])
self.sencerF_val = 0.0
self.sencerL_val = 0.0
self.sencerR_val = 0.0
# -- センサの更新 ------------------
self.calc_sencer()
# -- ゲームの情報-------------------
self.reward = 0
# -- その他 ------------------------
self.deltaT = 1
def update(self, deg=0, speed=1):
"""
車の次の位置を予想し更新
(※ 等速運動仮定)
:param theta: 回転角度 [deg]
"""
self.speed = speed
# -- 位置速度計算 --------------------------------------
# (ノルム計算がややこしい. 速度ノルムは常に1を確保する必要あり)
new_velocity_n = np.dot(self.rotate(deg), self.velocity_n.T)
self.velocity = new_velocity_n * self.speed
self.velocity_n = new_velocity_n.copy()
self.position = self.position + self.velocity * self.deltaT
# -- debug.log -----------------------------------------
s = 'pos:[%0.1f, %0.1f], vel:[%0.1f, %0.1f], vel:%d' % (
self.position[0], self.position[1], self.velocity[0], self.velocity[1], self.speed)
debuglog(s)
# -- センサ情報の計算 ----------------------------------
self.calc_sencer()
def calc_sencer(self):
"""
センサのヴェクトルを更新
フロントセンサは速度ベクトルと同じ向き
大きさは, SENCER_LENGTH倍
:return:
"""
self.sencerF = self.velocity_n.copy() * self.sencer_length
self.sencerL = np.dot(self.rotate(self.sencerL_angle_deg), self.sencerF.T)
self.sencerR = np.dot(self.rotate(self.sencerR_angle_deg), self.sencerF.T)
# -- debug.log -----------------------------------------
s = 'sencerF:[%0.1f, %0.1f], sencerL:[%0.1f, %0.1f], sencerR:[%0.1f, %0.1f]' % (
self.sencerF[0], self.sencerF[1], self.sencerL[0], self.sencerL[1], self.sencerR[0], self.sencerR[1],)
debuglog(s)
def rotate(self, deg):
"""
回転行列
:param theta: 回転角度 [deg]
"""
rad = np.deg2rad(deg)
return np.array([[np.cos(rad), -np.sin(rad)],
[np.sin(rad), np.cos(rad)]])
def set_pos(self, pos):
"""
車の位置を再度セット
"""
self.position = pos.copy()
# -- debug.log -----------------------------------------
s = 'reset>> pos:[%0.1f, %0.1f], vel:[%0.1f, %0.1f], vel:%d' % (
self.position[0], self.position[1], self.velocity[0], self.velocity[1], self.speed)
debuglog(s)
def vectorNormalize(self, _vec):
vec = np.asarray(_vec)
return vec / np.linalg.norm(vec)
class CircleRace(object):
"""
サークルレースゲームオブジェクト
車の状態更新、フィールド情報、得点等をコントロール
"""
STEER_ANGLES = [10, 20, 30, 40, 90]
REWARD_CRASH = -1.0
SENCER_RESOLUTION = 10 # センサ解像度(何分割するか)
def __init__(self, area_width, area_height):
# -- 領域情報 --- -----------------------------
self.area_width = area_width
self.area_height = area_height
self.xlim = [0, self.area_width]
self.ylim = [0, self.area_height]
# -- オブジェクト -----------------------------
initial_pos = [self.area_width / 2., self.area_height / 2.]
self.car = Car(iniPos=initial_pos, iniVelocity=[1, 1], sencer_angle=30)
def update_game(self, steer='forward', steer_level=0, speed=1):
"""
ゲーム更新
:param steer_angle: 車の回転角度 [deg]
:param steer_level: 車の回転角度のレベル
"""
# -- t時刻の位置・速度を保管 ------------
old_pos = self.car.position.copy()
old_vel = self.car.velocity.copy()
# -- 角度の計算(levelからdegreeに変更) --
if steer is 'left':
steer_angle = self.STEER_ANGLES[steer_level]
elif steer is 'right':
steer_angle = (-1.) * self.STEER_ANGLES[steer_level]
else:
steer_angle = 0
# -- 車の状態更新 ------------------------
self.car.update(deg=steer_angle, speed=speed)
new_pos = self.car.position.copy()
new_vel = self.car.velocity.copy()
# -- 衝突判定 ----------------------------
if self.isCrash(pos=new_pos):
# -- 位置のみ前時刻に戻す --------
self.car.set_pos(old_pos.copy())
# -- はねかえす ------------------
self.car.set_pos(old_pos - (1. / 2.) * old_vel * self.car.deltaT)
# -- 報酬の支払い ----------------
self.car.reward = self.REWARD_CRASH
debuglog(s='reward:%0.1f' % self.car.reward)
# -- センサ値の更新 ----------------------
self.car.sencerF_val = self.calc_sencer_value(self.car.sencerF, self.car.position)
self.car.sencerR_val = self.calc_sencer_value(self.car.sencerR, self.car.position)
self.car.sencerL_val = self.calc_sencer_value(self.car.sencerL, self.car.position)
def calc_sencer_value(self, sencer_vec, car_pos):
"""
センサ値の計算
:param sencer: (ndarry) センサの方向ベクトル
:return: センサ値 {0~1}={1:reso}/reso
"""
sencer_value = 0
# -- センサ値の衝突判定 ------------------
# for i in range(self.SENCER_RESOLUTION, -1, -1):
# ratio = i / self.SENCER_RESOLUTION
# pos = (sencer_vec * ratio) + car_pos
#
# if not self.isCrash(pos):
# sencer_value = i
# break
# -- センサ値の衝突判定 ------------------
# 計算負荷がかかるが、試作のため、自車位置から検索
# TODO: ソート方式に変更
for i in range(self.SENCER_RESOLUTION):
ratio = float(i) / self.SENCER_RESOLUTION
sencer_pos = (sencer_vec * ratio) + car_pos
if self.isCrash(sencer_pos):
return i
return sencer_value
def isCrash(self, pos):
"""
車が壁に衝突したか判定
:param pos: (ndarry) 位置
:return: (bool) 衝突:true, 以外:false
"""
x, y = [0, 1]
xlim_min, xlim_max = self.xlim
ylim_min, ylim_max = self.ylim
if pos[x] < xlim_min or xlim_max < pos[x]:
return True
if pos[y] < ylim_min or ylim_max < pos[y]:
return True
return False
# 描画用PySideクラス
class GameWindow(QWidget):
# -- 定数(画面サイズ) -----------------
SCREEN_HEIGHT = 500
SCREEN_WIDTH = 700
MARGIN_HEIGHT = 50
MARGIN_WIDTH = 50
STAGE_HEIGHT = SCREEN_HEIGHT - 2 * MARGIN_HEIGHT
STAGE_WIDTH = SCREEN_WIDTH - 2 * MARGIN_WIDTH
# -- 定数(タイマー) -------------------
INTERVAL_TIME = 1
# -- 定数(その他) ---------------------
SIZE_CAR = 15
def __init__(self, parent=None):
QWidget.__init__(self, parent)
self.resize(self.SCREEN_WIDTH, self.SCREEN_HEIGHT)
# -- 画面バッファ --------------------
self.pixmap = QPixmap(self.size())
# -- 解析用オブジェクト --------------
self.game = CircleRace(area_width=self.STAGE_WIDTH, area_height=self.STAGE_HEIGHT)
# -- 操作用変数 ----------------------
self.car_steer = 'right'
self.car_speed = 1
self.car_steer_level = 1
self.user_car_steer = None
# -- 初期画面の準備 ------------------
# 画面バッファの初期化
self.refreshPixmap()
# グリッドの表示
painter = QPainter(self.pixmap)
self.drawGrid(painter)
# メインループの準備と開始
# -------------------------
if True:
self.timer = QTimer()
self.timer.timeout.connect(self.mainloop)
self.timer.start(self.INTERVAL_TIME)
# self.timer.setInterval(self.INTERVAL_TIME)
# self.timer.start()
# ************************************************************* #
# メインループ
# ************************************************************* #
def mainloop(self):
"""
アニメーションのメインループ
アルゴリズムの時間更新等はここで行う
"""
# -- アルゴリズム処理-----------
# 定常円
# self.car_steer = 'right'
# -- アルゴリズム処理-----------
sencerL = self.game.car.sencerL_val
sencerR = self.game.car.sencerR_val
if sencerL < sencerR:
self.car_steer = 'left'
elif sencerL > sencerR:
self.car_steer = 'right'
else:
self.car_steer = ''
if not self.user_car_steer is None:
self.car_steer = self.user_car_steer
self.game.update_game(steer=self.car_steer, steer_level=1, speed=self.car_speed)
self.car_steer = None
self.user_car_steer = None
# -- 描画準備 ------------------
painter = QPainter(self.pixmap)
self.drawGrid(painter)
self.drawGeoPoints(painter)
# 画面更新
self.update()
def paintEvent(self, *args, **kwargs):
# -- おまじない ---------------------------
# QPainterを生成
painter = QStylePainter(self)
# QPainterでバッファに準備したデータを描画
painter.drawPixmap(0, 0, self.pixmap)
painter.setRenderHint(QPainter.Antialiasing, True)
_painter = painter.style()
# 描画用QPenのデフォルト
pen_default = QPen()
pen_default.setColor(Qt.black)
pen_default.setWidth(2)
brush_default = QBrush(Qt.red, Qt.NoBrush)
# -- アクセス用定数 -----------------------
x, y = 0, 1
# -- 車両の位置情報を取得 -----------------
_pos = self.game.car.position.copy()
_vel = self.game.car.velocity.copy()
pos = self.locateXY(_pos)
vel = self.locateVec(_vel)
vel_n = vel / np.linalg.norm(vel)
# -- 車両位置のプロット -------------------
painter.drawPoint(pos[x], pos[y])
# -- 車両位置に 円を描画 -----------------------------
pen = QPen(Qt.black, 1, Qt.DotLine, Qt.RoundCap, Qt.RoundJoin)
painter.setPen(pen)
reward = self.game.car.reward
if reward < 0:
painter.setBrush(QBrush(QColor(255, 0, 0, 175)))
painter.setBrush(QBrush(QColor(0, 255, 0, 175)))
painter.drawEllipse(pos[x] - self.SIZE_CAR / 2, pos[y] - self.SIZE_CAR / 2, self.SIZE_CAR, self.SIZE_CAR)
# -- 車両位置に四角を描画 -----------------------------
pen = QPen(Qt.black, 0.8, Qt.SolidLine, Qt.RoundCap, Qt.RoundJoin)
painter.setPen(pen)
def rotate(deg):
"""
回転行列
:param theta: 回転角度 [deg]
"""
rad = np.deg2rad(deg)
return np.array([[np.cos(rad), -np.sin(rad)],
[np.sin(rad), np.cos(rad)]])
car_size = self.SIZE_CAR
ny = vel_n.copy() * car_size
nx = np.dot(rotate(90), vel_n.copy()) * car_size / 2.0
v1 = nx + ny + pos
v2 = nx - ny + pos
v3 = -nx - ny + pos
v4 = -nx + ny + pos
v = [vary.tolist() for vary in [v1, v2, v3, v4]]
qpoints = [QPoint(*lv) for lv in v]
polygon = QPolygon(qpoints)
painter.drawPolygon(polygon)
# -- 進行方向(速度ベクトル)を描画 ---------
pen = QPen(Qt.gray, 1, Qt.SolidLine, Qt.RoundCap, Qt.RoundJoin)
painter.setPen(pen)
painter.drawLine(pos[x], pos[y], (pos[x] + vel[x]), (pos[y] + vel[y]))
# -- センサを表示 -------------------------
sencerF = self.locateVec(self.game.car.sencerF) + pos
sencerL = self.locateVec(self.game.car.sencerL) + pos
sencerR = self.locateVec(self.game.car.sencerR) + pos
sencer_linewide = 0.6
painter.setPen(QPen(Qt.red, sencer_linewide))
painter.drawLine(pos[x], pos[y], sencerF[x], sencerF[y])
painter.setPen(QPen(Qt.blue, sencer_linewide))
painter.drawLine(pos[x], pos[y], sencerL[x], sencerL[y])
painter.setPen(QPen(Qt.green, sencer_linewide))
painter.drawLine(pos[x], pos[y], sencerR[x], sencerR[y])
# -- センサグラフを表示 ---------------------
sencerF_val = self.game.car.sencerF_val
sencerR_val = self.game.car.sencerR_val
sencerL_val = self.game.car.sencerL_val
# 棒グラフのサイズ
sencer_graph_width = 20
sencer_graph_height = 60
# Rectの左下位置
DEBUG_MARGIN_WIDTH = 30
DEBUG_MARGIN_HEIGHT = 30
pL = self.locateXY([DEBUG_MARGIN_WIDTH + 0, DEBUG_MARGIN_HEIGHT + 0]).tolist()
pF = self.locateXY([DEBUG_MARGIN_WIDTH + sencer_graph_width * 2, DEBUG_MARGIN_HEIGHT + 0]).tolist()
pR = self.locateXY([DEBUG_MARGIN_WIDTH + sencer_graph_width * 4, DEBUG_MARGIN_HEIGHT + 0]).tolist()
# センサー(レフト)の棒グラフ
r = QRect(0, 0, sencer_graph_width, sencer_graph_height)
r.setHeight(sencerL_val * 10)
r.moveBottomLeft(QPoint(*pL))
# 描画色の設定
painter.setPen(QPen(Qt.gray, 1, Qt.SolidLine, Qt.RoundCap, Qt.RoundJoin))
painter.setBrush(QBrush(Qt.blue, Qt.Dense4Pattern))
painter.drawRect(r)
# センサー(センター)の棒グラフ
r = QRect(0, 0, sencer_graph_width, sencer_graph_height)
r.setHeight(sencerF_val * 10)
r.moveBottomLeft(QPoint(*pF))
painter.setBrush(QBrush(Qt.red, Qt.Dense4Pattern))
painter.drawRect(r)
# センサー(ライト)の棒グラフ
r = QRect(0, 0, sencer_graph_width, sencer_graph_height)
r.setHeight(sencerR_val * 10)
r.moveBottomLeft(QPoint(*pR))
painter.setBrush(QBrush(Qt.green, Qt.Dense4Pattern))
painter.drawRect(r)
# ************************************************************* #
# 描画系補助関数
# ************************************************************* #
def drawDebugLog(self, painter):
pass
def drawGeoPoints(self, painter):
pass
def drawGrid(self, painter):
""" マップを表示する関数
"""
for x in range(self.STAGE_WIDTH):
for y in range(self.STAGE_HEIGHT):
# painter.drawPoint(x,y)
pass
painter.setPen(QPen(Qt.black, 3))
painter.setBrush(QBrush(QColor(200, 200, 200, 150), Qt.Dense1Pattern))
painter.drawRect(self.MARGIN_WIDTH, self.MARGIN_HEIGHT, self.STAGE_WIDTH, self.STAGE_HEIGHT)
pass
def locateXY(self, pos):
"""
オブジェクトのローカル座標を、スクリーン上の座標へ変換
"""
pos = np.asarray(pos)
local_x, local_y = pos.copy()
srn_x = local_x + self.MARGIN_WIDTH
srn_y = (self.SCREEN_HEIGHT - local_y - self.MARGIN_HEIGHT)
return np.asarray([srn_x, srn_y])
def locateVec(self, vel):
"""
オブジェクトのベクトルを、スクリーン上の座標空間へ変換
(※yを反転させるだけ)
"""
srn_vec = np.array([vel[0], -1 * vel[1]])
return srn_vec
# ************************************************************* #
# その他Qt関連補助関数
# ************************************************************* #
def refreshPixmap(self):
"""
画面バッファの初期化関数
"""
# 画面バッファの初期化
self.pixmap = QPixmap(self.size())
# 画面を塗りつぶし (おまじない)
self.pixmap.fill(self, 0, 0)
self.pixmap.fill(Qt.white)
# ぺインターの生成 (おまじない)
painter = QPainter(self.pixmap)
# ぺインターによる初期化 (おまじない)
painter.initFrom(self)
pass
def sizeHint(self):
return QSize(self.SCREEN_WIDTH, self.SCREEN_HEIGHT)
def keyPressEvent(self, event):
e = event.key()
if e == Qt.Key_Up:
self.car_speed += 1
pass
elif e == Qt.Key_Down:
self.car_speed -= 1
if self.car_speed < 0:
self.car_speed = 0
elif e == Qt.Key_Left:
self.user_car_steer = 'left'
pass
elif e == Qt.Key_Right:
self.user_car_steer = 'right'
pass
elif e == Qt.Key_Plus:
pass
elif e == Qt.Key_Minus:
pass
elif e == Qt.Key_Q:
self.close()
else:
pass
print 'Presskey', e
self.update()
# *******************************************************
# main関数
# *******************************************************
def mainAlgorizm():
game = CircleRace()
for i in range(50):
game.update_game(steer='left', steer_level=1)
pass
def mainGUI():
app = QApplication(sys.argv)
win = GameWindow()
win.show()
sys.exit(app.exec_())
if __name__ == "__main__":
# mainAlgorizm()
mainGUI()
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment