218 lines
6.7 KiB
Python
218 lines
6.7 KiB
Python
from __future__ import print_function
|
|
from __future__ import division
|
|
from __future__ import unicode_literals
|
|
|
|
from argparse import ArgumentParser
|
|
from time import time
|
|
|
|
import numpy as np
|
|
|
|
import matplotlib as mpl
|
|
mpl.use('TkAgg') # fixes my macOS bug
|
|
|
|
import matplotlib.pyplot as plt
|
|
|
|
|
|
P = 0.1
|
|
ALPHA = 0.90
|
|
EPSILON = 1e-12 # Convergence criterium
|
|
A2 = np.array([ # Action index to action mapping
|
|
[-1, 0], # Up
|
|
[ 1, 0], # Down
|
|
[ 0, -1], # Left
|
|
[ 0, 1], # Right
|
|
[ 0, 0], # Idle
|
|
])
|
|
|
|
# Global state
|
|
MAZE = None # Map of the environment
|
|
S_TO_IJ = None # Mapping of state vector to coordinates
|
|
SN = None # Number of states
|
|
U_OF_X = None # The allowed action space matrix representation
|
|
PW_OF_X_U = None # The probability distribution of disturbance
|
|
G1_X = None # The cost function vector representation (depends only on state)
|
|
G2_X = None # The second cost function vector representation
|
|
F_X_U_W = None # The System Equation
|
|
|
|
|
|
def h_matrix(j, g):
|
|
result = (PW_OF_X_U * (g[F_X_U_W] + ALPHA*j[F_X_U_W])).sum(axis=2)
|
|
result[~U_OF_X] = np.inf # discard invalid policies
|
|
return result
|
|
|
|
|
|
def _valid_target(target):
|
|
return (
|
|
0 <= target[0] < MAZE.shape[0] and
|
|
0 <= target[1] < MAZE.shape[1] and
|
|
MAZE[tuple(target)] != '1'
|
|
)
|
|
|
|
|
|
def init_global(maze_filename):
|
|
global MAZE, SN, S_TO_IJ
|
|
global U_OF_X, PW_OF_X_U, F_X_U_W, G1_X, G2_X
|
|
|
|
# Basic maze structure initialization
|
|
MAZE = np.genfromtxt(
|
|
maze_filename,
|
|
dtype=str,
|
|
)
|
|
state_mask = (MAZE != '1')
|
|
|
|
S_TO_IJ = np.indices(MAZE.shape).transpose(1, 2, 0)[state_mask]
|
|
SN = len(S_TO_IJ)
|
|
ij_to_s = np.zeros(MAZE.shape, dtype=np.int32)
|
|
ij_to_s[state_mask] = np.arange(SN)
|
|
|
|
# One step cost functions initialization
|
|
maze_cost = np.zeros(MAZE.shape, dtype=np.float64)
|
|
maze_cost[MAZE == '1'] = np.nan
|
|
maze_cost[(MAZE == '0') | (MAZE == 'S')] = 0
|
|
maze_cost[MAZE == 'T'] = 50
|
|
maze_cost[MAZE == 'G'] = -1
|
|
G1_X = maze_cost.copy()[state_mask]
|
|
maze_cost[maze_cost < 1] += 1 # assert np.nan < whatever == False
|
|
G2_X = maze_cost.copy()[state_mask]
|
|
|
|
# Actual environment modelling
|
|
U_OF_X = np.zeros((SN, len(A2)), dtype=np.bool)
|
|
PW_OF_X_U = np.zeros((SN, len(A2), len(A2)), dtype=np.float64)
|
|
F_X_U_W = np.zeros(PW_OF_X_U.shape, dtype=np.int32)
|
|
|
|
for ix, x in enumerate(S_TO_IJ):
|
|
for iu, u in enumerate(A2):
|
|
if _valid_target(x + u):
|
|
U_OF_X[ix, iu] = True
|
|
if iu in (0, 1): # (Up, Down)
|
|
possible_iw = [2, 3] # [Left, Right]
|
|
elif iu in (2, 3): # (Left, Right)
|
|
possible_iw = [0, 1] # [Up, Down]
|
|
for iw in possible_iw:
|
|
if _valid_target(x + u + A2[iw]):
|
|
PW_OF_X_U[ix, iu, iw] = P
|
|
F_X_U_W[ix, iu, iw] = ij_to_s[tuple(x + u + A2[iw])]
|
|
# Idle w is always possible
|
|
PW_OF_X_U[ix, iu, -1] = 1 - PW_OF_X_U[ix, iu].sum()
|
|
F_X_U_W[ix, iu, -1] = ij_to_s[tuple(x + u)]
|
|
|
|
|
|
def plot_j_policy_on_maze(j, policy):
|
|
heatmap = np.full(MAZE.shape, np.nan)
|
|
heatmap[S_TO_IJ[:, 0], S_TO_IJ[:, 1]] = j
|
|
cmap = mpl.cm.get_cmap('coolwarm')
|
|
cmap.set_bad(color='black')
|
|
plt.imshow(heatmap, cmap=cmap)
|
|
plt.colorbar()
|
|
# quiver has some weird behavior, the arrow y component must be flipped
|
|
plt.quiver(S_TO_IJ[:, 1], S_TO_IJ[:, 0], A2[policy, 1], -A2[policy, 0])
|
|
plt.gca().get_xaxis().set_visible(False)
|
|
plt.gca().get_yaxis().set_visible(False)
|
|
|
|
|
|
def plot_cost_history(hist):
|
|
error = np.sqrt(np.square(hist[:-1] - hist[-1]).mean(axis=1))
|
|
plt.xlabel('Number of iterations')
|
|
plt.ylabel('Cost function RMSE')
|
|
plt.plot(error)
|
|
|
|
|
|
def _policy_improvement(j, g):
|
|
h_mat = h_matrix(j, g)
|
|
return h_mat.argmin(axis=1), h_mat.min(axis=1)
|
|
|
|
|
|
def _evaluate_policy(policy, g):
|
|
pw_pi = PW_OF_X_U[np.arange(SN), policy] # p(w) given policy for all x
|
|
targs = F_X_U_W[np.arange(SN), policy] # all f(x, u(x), w(x, u(x)))
|
|
G = (pw_pi * g[targs]).sum(axis=1) # Expected one-step cost vector
|
|
|
|
# Markov matrix for given deterministic policy
|
|
M = np.zeros((SN, SN), dtype=np.float64)
|
|
x_from = [x_ff for x_f, nz in
|
|
zip(np.arange(SN), np.count_nonzero(pw_pi, axis=1))
|
|
for x_ff in [x_f] * nz]
|
|
M[x_from, targs[pw_pi > 0]] = pw_pi[pw_pi > 0]
|
|
return np.linalg.solve(np.eye(SN) - ALPHA*M, G)
|
|
|
|
|
|
def value_iteration(j, g):
|
|
return _policy_improvement(j, g)
|
|
|
|
|
|
def policy_iteration(j, g):
|
|
policy, _ = _policy_improvement(j, g)
|
|
j = _evaluate_policy(policy, g)
|
|
return policy, j
|
|
|
|
|
|
def _terminate(j, j_old):
|
|
# TODO: DIS
|
|
return np.abs(j - j_old).max() < EPSILON
|
|
|
|
|
|
def dynamic_programming(optimizer_step, g, return_history=False):
|
|
j = np.zeros(SN, dtype=np.float64)
|
|
history = []
|
|
while True:
|
|
j_old = j
|
|
policy, j = optimizer_step(j, g)
|
|
if return_history:
|
|
history.append(j)
|
|
if _terminate(j, j_old):
|
|
break
|
|
if not return_history:
|
|
return j, policy
|
|
else:
|
|
return np.array(history)
|
|
|
|
|
|
if __name__ == '__main__':
|
|
# Argument Parsing
|
|
ap = ArgumentParser()
|
|
ap.add_argument('maze_file', help='Path to maze file')
|
|
args = ap.parse_args()
|
|
|
|
# Initialization
|
|
start = time()
|
|
init_global(args.maze_file)
|
|
|
|
# J / policy for both algorithms for both cost functions for 3 alphas
|
|
costs = {'g1': G1_X, 'g2': G2_X}
|
|
optimizers = {'Value Iteration': value_iteration,
|
|
'Policy Iteration': policy_iteration}
|
|
|
|
for a in [0.9, 0.5, 0.01]:
|
|
plt.figure()
|
|
plt.suptitle('DISCOUNT = ' + str(a))
|
|
i = 1
|
|
for opt in ['Value Iteration', 'Policy Iteration']:
|
|
for cost in ['g1', 'g2']:
|
|
name = ' / '.join([opt, cost])
|
|
ALPHA = a
|
|
j, policy = dynamic_programming(optimizers[opt], costs[cost])
|
|
print(name, j)
|
|
plt.subplot(2, 2, i)
|
|
plt.gca().set_title(name)
|
|
plot_j_policy_on_maze(j, policy)
|
|
i += 1
|
|
|
|
# Error graphs
|
|
for opt in ['Value Iteration', 'Policy Iteration']:
|
|
plt.figure()
|
|
plt.suptitle(opt)
|
|
i = 1
|
|
for cost in ['g1', 'g2']:
|
|
for a in [0.9, 0.8, 0.7]:
|
|
name = 'Cost: {}, discount: {}'.format(cost, a)
|
|
ALPHA = a
|
|
history = dynamic_programming(optimizers[opt], costs[cost],
|
|
return_history=True)
|
|
plt.subplot(2, 3, i)
|
|
plt.gca().set_title(name)
|
|
plot_cost_history(history)
|
|
i += 1
|
|
|
|
print('I ran in {} seconds'.format(time() - start))
|
|
plt.show()
|