[機械学習] 車のレース

CircleRace.py

#! coding:utf-8
"""
CircleRace main.py
サークルレースゲーム

(2015/11/19) 4:47
"""
import numpy as np


# logを保存
# --------------------
from datetime import datetime
import csv


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


class Car(object):
    """
    車オブジェクト
    自車の速度・位置の情報を持つ
    """
    SENCER_LENGTH = 10.

    def __init__(self, iniPos=None, iniVelocity=None, sencer_angle=30):
        # -- 自車情報 ----------------------
        self.position = np.array([0, 0])
        self.velocity = np.array([0, 0])
        self.velocity_val = 0.0

        # -- 初期値代入 ---------------------
        if not iniPos is None:
            _iniPos = np.asarray(iniPos)
            self.position = _iniPos / np.linalg.norm(_iniPos)
        if not iniVelocity is None:
            _iniVelocity = np.asarray(iniVelocity)
            self.velocity = _iniVelocity / np.linalg.norm(_iniVelocity)

        # -- センサの情報-------------------
        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):
        """
        車の次の位置を予想し更新
        (※ 等速運動仮定)
        :param theta: 回転角度 [deg]
        """
        # -- 位置速度計算 --------------------------------------
        self.velocity = np.dot(self.rotate(deg), self.velocity.T)
        self.position = self.position + self.velocity * self.deltaT
        self.velocity_val = np.linalg.norm(self.velocity)
        # -- debug.log -----------------------------------------
        s = 'pos:[%0.1f, %0.1f], vel:[%0.1f, %0.1f], vel:%0.1f' % (
            self.position[0], self.position[1], self.velocity[0], self.velocity[1], self.velocity_val)
        debuglog(s)
        # -- センサ情報の計算 ----------------------------------
        self.calc_sencer()

    def calc_sencer(self):
        """
        センサのヴェクトルを更新
        フロントセンサは速度ベクトルと同じ向き
        大きさは, SENCER_LENGTH倍
        :return:
        """
        self.sencerF = self.velocity.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)]])


class CircleRace(object):
    """
    サークルレースゲームオブジェクト
    車の状態更新、フィールド情報、得点等をコントロール
    """
    STEER_ANGLES = [45, 90]
    REWARD_CRASH = -1.0
    SENCER_RESOLUTION = 10  # センサ解像度(何分割するか)

    def __init__(self):
        # -- オブジェクト -----------------------------
        self.car = Car(iniPos=[1, 1], iniVelocity=[1, 1], sencer_angle=10)

    def update_game(self, steer='forward', steer_level=0):
        """
        ゲーム更新
        :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)
        new_pos = self.car.position.copy()
        new_vel = self.car.velocity.copy()
        # -- 衝突判定 ----------------------------
        if self.isCrash(pos=new_pos):
            # -- 位置のみ前時刻に戻す --------
            self.car.pos = old_pos.copy()
            # -- はねかえす ------------------
            # self.car.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.sencerR_val = self.calc_sencer_value(self.car.sencerR)
        self.car.sencerL_val = self.calc_sencer_value(self.car.sencerL)

    def calc_sencer_value(self, sencer_vec):
        """
        センサ値の計算
        :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

            if not self.isCrash(pos):
                sencer_value = ratio
                break

        return sencer_value

    def isCrash(self, pos):
        """
        車が壁に衝突したか判定
        :param pos: (ndarry) 位置
        :return: (bool) 衝突:true, 以外:false
        """
        x, y = [0, 1]
        xlim_min, xlim_max = -10, 10
        ylim_min, ylim_max = -10, 10
        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


def main():
    game = CircleRace()
    for i in range(50):
        game.update_game(steer='', steer_level=0)
    pass


if __name__ == "__main__":
    main()