D18：自定義環境(2/2)

2022 iThome 鐵人賽

DAY 18

自我挑戰組

高中生也可以！利用強化學習讓機器人動起來！系列第 18 篇

14th鐵人賽

golucky_sir

2022-09-19 00:10:34

983 瀏覽

分享至

大家好，不知不覺的我們的強化學習專案開始慢慢步入正軌了，今天會帶大家創造一個強化學習的環境主體。希望大家可以跟緊我的腳步喔，有了一次動手做的經驗後，相信各位之後要調整或者創造其他專案都會比較輕鬆喔！雖然程式不會寫的那麼嚴謹，不過該有的都還是會有。

匯入模組

模組當然就是這些啦，src是昨天我們所寫的專案用的模型。

import gym
import pybullet as p
from gym import spaces
import numpy as np
from src import Plane,R2D2,Target,Wall

架構

環境架構大概是這樣，除了繼承gym.Env中的方法外還有get_obs()跟get_reward()，顧名思義就是取的當前觀察值跟reward的。觀察值的組成為R2D2的x,y座標跟目標點的x,y座標以及他們之間的距離因為可能要轉彎尋找物體，所以R2D2跟目標點的方向也要納入考量。reward為將他們的位置投影到xy平面再取距離以及判斷R2D2是否有面向目標點，以他們的角度差為reward的扣分項。

這些設定並沒有絕對，各位可以自己嘗試看看使用其他觀察值等(今天撰文的時候才想到或許可以使用極座標系，這樣可以降低觀察值的數量，使訓練加速)。

class R2D2Walking(gym.Env):
    def __init__(self):
    def step(self, action):
        return obs, reward, self.done, {}
    def reset(self):
        return obs
    def render(self, mode='human', clode=False):
    def close(self):
    def get_obs(self):
        return obs
    def get_reward(self):

初始化環境

在這邊我會先把我需要的物件等都先宣告出他們的屬性等，並且環境中的可變參數也會在這邊設定(通常是設定一些實驗用的參數等)。初始化pybullet以及他的設定也會在這邊處理。

def __init__(self):
        physicsClient = p.connect(p.GUI)
        p.setGravity(0, 0, -9.8)
        p.setRealTimeSimulation(1)
        Plane()
        Wall()

        low_action = np.array([-10, -10, -10, -10],dtype=np.float32)
        high_action = np.array([10, 10, 10, 10],dtype=np.float32)
        self.action_space = spaces.Box(low=low_action,high=high_action)
        self.observation_space = spaces.Box(low=-10,high=10,shape=(7, ), dtype=np.float32)

        self.r2d2 = R2D2()
        self.target=Target()
        self.targetpos=None

        self.hitRayColor = [1, 0, 1]
        self.missRayColor = [0, 1, 0]
        self.rayFroms = None
        self.rayTos = None

        self.rewardlst = []#該次任務的reward變化
		self.trainreward = []#整個訓練的reward變化
        self.timesteps = 0
        self.testtime = 0 #訓練次數
        self.successtime = 0 #成功次數

我們先寫了pybullet的基本設定，包括匯入牆體跟地板，接下來設定動作空間跟觀察空間、將機器人請出來、設定雷射偵測還有目標點的位置，最後設定一些助於訓練跟查看訓練情況的值，需要注意的是self.timesteps代表action的次數，為了避免卡入死迴圈，所以在測試一段時間後若未能達成任務需要重新reset。有了這些後這樣基本設定就大功告成了。

每一step中該做的事

每一個step中我們要做的事就是讓輪子移動嘛，我的測試發現action的值都偏小，所以我讓他們的值乘了10倍，希望輪子動快一點。移動過後我們希望可以得到當前的觀察值跟reward所以分別呼叫這兩個方法來取得當前的觀察值跟reward，接下來就是雷射偵測，如果R2D2有走到雷射的地方的話，就可以加分，且成功次數加一次！並且要reset。

如果R2D2未能在1000次移動中達成任務的話則判為失敗，會被扣2分，且reset。

如果R2D2的x,y座標都超過+-4.5的話就會reset。

def step(self, action):
        self.timesteps += 1
        action *= 10
        self.r2d2.move(action[0],action[1],action[2],action[3])#rf,rb,lf,lb
        obs = self.get_obs()
        reward=self.get_reward()

				self.done = self.rz<0.2#判斷機器人有沒有跌倒
        if self.rx>4.5 or self.rx<-4.5 or self.ry>4.5 or self.ry<-4.5:#判斷機器人有沒有跑太遠
            reward -= 2
            self.done=True
				#雷射偵測
        results = p.rayTestBatch(self.rayFroms, self.rayTos)
        p.removeAllUserDebugItems()
        for index, result in enumerate(results):
            if result[0] != -1:
                p.addUserDebugLine(self.rayFroms, self.rayTos, self.hitRayColor)
                reward += 5
                self.successtime += 1
                self.done = True
            else:
                p.addUserDebugLine(self.rayFroms, self.rayTos, self.missRayColor)

        self.rewardlst.append(reward)
        if self.timesteps>1000:
            reward-=2
            self.done=True

        return obs, reward, self.done, {}

觀察值

首先我們來看看left跟right分別是R2D2的左右腳位置，取中點就可以看成是其所在位置(pos)，這邊將他拆成三個座標self.rx, self.ry, self.rz，因為在step()中要判斷是否跌倒跟是否跑太遠。接下來要來看dis，這個是R2D2跟目標點的歐式距離。

接下來有點複雜了，請各位先準備好一杯咖啡~且聽我慢慢介紹。首先我們要先找出R2D2的方向(r2d2_quaternion)並且將此四元數轉成尤拉角，接著要取繞z軸旋轉的角度，所以索引值是2，然後R2D2預設是面向y軸，所以在初始設定的時候我們讓他轉了3?/2(等同-?/2)的弧度使其面向x軸，所以在最後還要加?/2來校正回歸(self.r2d2_angle)(不懂的話沒關係)。接下來就是求目標點繞z軸旋轉的角度了，只要求出他的x,y座標並取arctan就可以得到角度囉(self.target_angle)！最後再合併就好了(obs)。

def get_obs(self):
        left = np.array(p.getLinkState(self.r2d2.r2d2ID, 0)[0])#0,4是左腳右腳關節處 所以要取平均
        right = np.array(p.getLinkState(self.r2d2.r2d2ID, 4)[0])
        self.rx, self.ry, self.rz = pos = (left+right)/2
        dis = np.linalg.norm(pos-self.targetpos)
        r2d2_quaternion = p.getBasePositionAndOrientation(self.r2d2.r2d2ID)[1]
        self.r2d2_angle = p.getEulerFromQuaternion(r2d2_quaternion)[2] + np.pi / 2
        tx, ty, _ = self.targetpos
        self.target_angle = np.arctan(ty / tx)

        obs = np.array([self.rx,self.ry,tx,ty])
        obs = np.append(obs, dis)
        obs = np.append(obs, self.r2d2_angle)
        obs = np.append(obs, self.target_angle)

        return obs

最終輸出是一個長度為7的陣列。分別是R2D2的x座標、R2D2的y座標、目標點的x座標、目標點的y座標、R2D2跟目標點的歐式距離、R2D2的方向、目標點的方向。

reward

這個就比較簡單了，求出R2D2跟目標點在xy平面上投影的歐式距離並取負數(pos_reward)，取負數是代表距離越遠會扣越多分。同理兩者的角度差也是一樣，根據get_obs()方法中取得的角度來計算兩者的角度差當作扣分項(angle_reward)。最後兩者相加就好。

def get_reward(self):
        pos=self.get_obs()[0:2]
        pos_reward = -np.linalg.norm(pos-self.targetpos[0:2])
        angle_reward = -abs(self.r2d2_angle-self.target_angle)
        return pos_reward+angle_reward

重置環境

reset沒有甚麼技術含量，就只是將一切初始化而已，並且print出當前執行任務的次數跟成功次數，各位也可以在任何地方print要確認的變數，這樣有助於確認訓練的情況。

def reset(self):
				if self.testtime>0:
            self.trainreward.append(sum(self.rewardlst)/len(self.rewardlst))
        self.rewardlst = []
        self.timesteps = 0
        self.testtime += 1
        self.done=False
        self.targetpos = self.target.reset_spawn()
        self.rayFroms = np.array(self.targetpos, dtype=np.float32)
        self.rayTos = np.array(self.targetpos + np.array([0, 0, 1]), dtype=np.float32)

        p.resetBasePositionAndOrientation(self.r2d2.r2d2ID,self.r2d2.r2d2StartPos,self.r2d2.r2d2StartOrientation)
        obs=self.get_obs()

        print(f'[INFO] testtime: {self.testtime}')
        print(f'[INFO] successtime: {self.successtime}')
        print(f'--------------------------------------')
        return obs

完整程式碼

import gym
import pybullet as p
from gym import spaces
import numpy as np
from src import Plane,R2D2,Target,Wall

class R2D2Walking(gym.Env):
    def __init__(self):
        physicsClient = p.connect(p.GUI)
        p.setGravity(0, 0, -9.8)
        p.setRealTimeSimulation(1)
        Plane()
        Wall()

        low_action = np.array([-10, -10, -10, -10],dtype=np.float32)
        high_action = np.array([10, 10, 10, 10],dtype=np.float32)
        self.action_space = spaces.Box(low=low_action,high=high_action)
        self.observation_space = spaces.Box(low=-10,high=10,shape=(7, ), dtype=np.float32)

        self.r2d2 = R2D2()
        self.target=Target()
        self.targetpos=None

        self.hitRayColor = [1, 0, 1]
        self.missRayColor = [0, 1, 0]
        self.rayFroms = None
        self.rayTos = None

        self.rewardlst = []
        self.trainreward = []
        self.timesteps = 0
        self.testtime = 0
        self.successtime = 0

    def step(self, action):
        self.timesteps += 1
        action *= 10
        self.r2d2.move(action[0],action[1],action[2],action[3])
        obs = self.get_obs()
        reward=self.get_reward()
        self.done = self.rz<0.2
        if self.rx>4.5 or self.rx<-4.5 or self.ry>4.5 or self.ry<-4.5:
            reward -= 2
            self.done=True
        results = p.rayTestBatch(self.rayFroms, self.rayTos)
        p.removeAllUserDebugItems()
        for index, result in enumerate(results):
            if result[0] != -1:
                p.addUserDebugLine(self.rayFroms, self.rayTos, self.hitRayColor)
                reward += 5
                self.successtime += 1
                self.done = True
            else:
                p.addUserDebugLine(self.rayFroms, self.rayTos, self.missRayColor)

        self.rewardlst.append(reward)
        if self.timesteps>1000:
            reward-=2
            self.done=True
        return obs, reward, self.done, {}
    def reset(self):
        if self.testtime>0:
            self.trainreward.append(sum(self.rewardlst)/len(self.rewardlst))
        self.rewardlst = []
        self.timesteps = 0
        self.testtime += 1
        self.done=False
        self.targetpos = self.target.reset_spawn()
        self.rayFroms = np.array(self.targetpos, dtype=np.float32)
        self.rayTos = np.array(self.targetpos + np.array([0, 0, 1]), dtype=np.float32)
        
        p.resetBasePositionAndOrientation(self.r2d2.r2d2ID,
                                          self.r2d2.r2d2StartPos,
                                          self.r2d2.r2d2StartOrientation)
                                          
        obs=self.get_obs()
        print(f'[INFO] testtime: {self.testtime}')
        print(f'[INFO] successtime: {self.successtime}')
        print(f'--------------------------------------')

        return obs
    def render(self, mode='human', clode=False):
        pass
    def close(self):
        p.disconnect()

    def get_obs(self):
        left = np.array(p.getLinkState(self.r2d2.r2d2ID, 0)[0])
        right = np.array(p.getLinkState(self.r2d2.r2d2ID, 4)[0])
        self.rx, self.ry, self.rz = pos = (left+right)/2
        dis = np.linalg.norm(pos-self.targetpos)
        r2d2_quaternion = p.getBasePositionAndOrientation(self.r2d2.r2d2ID)[1]
        self.r2d2_angle = p.getEulerFromQuaternion(r2d2_quaternion)[2] + np.pi / 2
        tx, ty, _ = self.targetpos
        self.target_angle = np.arctan(ty / tx)
        obs = np.array([self.rx,self.ry,tx,ty])
        obs = np.append(obs, dis)
        obs = np.append(obs, self.r2d2_angle)
        obs = np.append(obs, self.target_angle)

        return obs
    def get_reward(self):
        pos=self.get_obs()[0:2]
        pos_reward = -np.linalg.norm(pos-self.targetpos[0:2])
        angle_reward = -abs(self.r2d2_angle-self.target_angle)
        return pos_reward+angle_reward