319 lines
8.8 KiB
Python
319 lines
8.8 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')
|
|
|
|
import matplotlib.pyplot as plt
|
|
|
|
|
|
P = 0.1
|
|
ALPHA = 0.90
|
|
EPSILON = 1e-8 # Convergence criterium
|
|
|
|
# Global state
|
|
MAZE = None # Map of the environment
|
|
STATE_MASK = None # Fields of maze belonging to state space
|
|
S_TO_IJ = None # Mapping of state vector to coordinates
|
|
IJ_TO_S = None # Mapping of coordinates to state vector
|
|
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 state function
|
|
SN = None # Number of states
|
|
|
|
A2 = np.array([
|
|
[-1, 0],
|
|
[1, 0],
|
|
[0, -1],
|
|
[0, 1],
|
|
[0, 0]
|
|
])
|
|
|
|
ACTIONS = {
|
|
'UP': (-1, 0),
|
|
'DOWN': (1, 0),
|
|
'LEFT': (0, -1),
|
|
'RIGHT': (0, 1),
|
|
'IDLE': (0, 0)
|
|
}
|
|
|
|
|
|
def _ij_to_s(ij):
|
|
return np.argwhere(np.all(ij == S_TO_IJ, axis=1)).flatten()[0]
|
|
|
|
|
|
# TODO: for all x and u in one go
|
|
def h_function(x, u, j, g):
|
|
"""Return E_pi_w[g(x, pi(x), w) + alpha*J(f(x, pi(x), w))]."""
|
|
pw = pw_of_x_u(x, u)
|
|
expectation = sum(
|
|
pw[w] * (g(x, u, w) + ALPHA*j[_ij_to_s(f(x, u, w))])
|
|
for w in pw
|
|
)
|
|
return expectation
|
|
|
|
|
|
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 f(x, u, w):
|
|
return _move(_move(x, ACTIONS[u]), ACTIONS[w])
|
|
|
|
|
|
def cost_treasure(x, u, w):
|
|
xt = f(x, u, w)
|
|
options = {
|
|
'T': 50,
|
|
'G': -1,
|
|
}
|
|
return options.get(MAZE[xt], 0)
|
|
|
|
|
|
def cost_energy(x, u, w):
|
|
xt = f(x, u, w)
|
|
options = {
|
|
'T': 50,
|
|
'G': 0
|
|
}
|
|
return options.get(MAZE[xt], 1)
|
|
|
|
|
|
def _move(start, move):
|
|
return start[0] + move[0], start[1] + move[1]
|
|
|
|
|
|
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_file):
|
|
global MAZE, STATE_MASK, SN, S_TO_IJ, IJ_TO_S
|
|
global U_OF_X, PW_OF_X_U, F_X_U_W, G1_X, G2_X
|
|
|
|
# Basic maze structure initialization
|
|
MAZE = np.genfromtxt(
|
|
maze_file,
|
|
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)
|
|
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 == True
|
|
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)))
|
|
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):
|
|
possible_iw = [2, 3]
|
|
elif iu in (2, 3):
|
|
possible_iw = [0, 1]
|
|
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 u_of_x(x):
|
|
"""Return a list of allowed actions for the given state x."""
|
|
return [u for u in ACTIONS if _valid_target(_move(x, ACTIONS[u]))]
|
|
|
|
|
|
def pw_of_x_u(x, u):
|
|
"""Calculate probabilities of disturbances given state and action.
|
|
|
|
Parameters
|
|
----------
|
|
x : tuple of ints
|
|
The state coordinate
|
|
(it is up to user to ensure this is a valid state).
|
|
u : str
|
|
The name of the action (again, up to the user to ensure validity).
|
|
|
|
Returns
|
|
-------
|
|
dict
|
|
A mapping of valid disturbances to their probabilities.
|
|
|
|
"""
|
|
if u in ('LEFT', 'RIGHT'):
|
|
possible_w = ('UP', 'IDLE', 'DOWN')
|
|
elif u in ('UP', 'DOWN'):
|
|
possible_w = ('LEFT', 'IDLE', 'RIGHT')
|
|
else: # I assume that the IDLE action is deterministic
|
|
possible_w = ('IDLE',)
|
|
|
|
allowed_w = [
|
|
w for w in possible_w if
|
|
_valid_target(f(x, u, w))
|
|
]
|
|
probs = {w: P for w in allowed_w if w != 'IDLE'}
|
|
probs['IDLE'] = 1 - sum(probs.values())
|
|
return probs
|
|
|
|
|
|
def plot_j_policy_on_maze(j, policy):
|
|
heatmap = np.ones(MAZE.shape) * np.nan # Ugly
|
|
heatmap[STATE_MASK] = j # Even uglier
|
|
cmap = mpl.cm.get_cmap('coolwarm')
|
|
cmap.set_bad(color='black')
|
|
plt.imshow(heatmap, cmap=cmap)
|
|
plt.colorbar()
|
|
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 = [((h - hist[-1])**2).sum()**0.5 for h in hist[:-1]]
|
|
plt.xlabel('Number of iterations')
|
|
plt.ylabel('Cost function error')
|
|
plt.plot(error)
|
|
|
|
|
|
def _policy_improvement(j, g):
|
|
h_mat = h_matrix(j, g)
|
|
return np.argmin(h_mat, 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))
|
|
G = (pw_pi * g[targs]).sum(axis=1)
|
|
|
|
M = np.zeros((SN, SN)) # Markov matrix for given determ policy
|
|
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]
|
|
# M[np.arange(SN), F_X_U_W[PW_OF_X_U > 0]] = PW_OF_X_U[PW_OF_X_U > 0]
|
|
# for x, u in zip(S_TO_IJ, policy):
|
|
# pw = pw_of_x_u(x, u)
|
|
# G.append(sum(pw[w] * g(x, u, w) for w in pw))
|
|
# targets = [(_ij_to_s(f(x, u, w)), pw[w]) for w in pw]
|
|
# iox = _ij_to_s(x)
|
|
# for t, pww in targets:
|
|
# M[iox, t] = pww
|
|
# G = np.array(G)
|
|
return np.linalg.solve(np.eye(SN) - ALPHA*M, G)
|
|
|
|
|
|
def value_iteration(g, return_history=False):
|
|
j = np.zeros(SN)
|
|
history = [j]
|
|
while True:
|
|
# print(j)
|
|
policy, j_new = _policy_improvement(j, g)
|
|
j_old = j
|
|
j = j_new
|
|
if return_history:
|
|
history.append(j)
|
|
if np.abs(j - j_old).max() < EPSILON:
|
|
break
|
|
if not return_history:
|
|
return j, policy
|
|
else:
|
|
return history
|
|
|
|
|
|
def policy_iteration(g, return_history=False):
|
|
j = None
|
|
policy = np.full(SN, len(A2) - 1)
|
|
history = []
|
|
while True:
|
|
j_old = j
|
|
j = _evaluate_policy(policy, g)
|
|
history.append(j)
|
|
if j_old is not None and max(abs(j - j_old)) < EPSILON:
|
|
break
|
|
policy, _ = _policy_improvement(j, g)
|
|
if not return_history:
|
|
return j, policy
|
|
else:
|
|
return history
|
|
|
|
|
|
if __name__ == '__main__':
|
|
# Argument Parsing
|
|
ap = ArgumentParser()
|
|
ap.add_argument('maze_file', help='Path to maze file')
|
|
args = ap.parse_args()
|
|
|
|
# start = time()
|
|
# 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 g in ['g1', 'g2']:
|
|
name = ' / '.join([opt, g])
|
|
ALPHA = a
|
|
j, policy = optimizers[opt](costs[g])
|
|
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 g in ['g1', 'g2']:
|
|
for a in [0.9, 0.8, 0.7]:
|
|
name = 'Cost: {}, discount: {}'.format(g, a)
|
|
ALPHA = a
|
|
history = optimizers[opt](costs[g], 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()
|