# cen_1 = np.array([0,0])
# rad_1 = 0.5
# avoid_func_1 = lambda x: hypersphere_ext(x, center=cen_1,radius=rad_1,
# dims=[0, 1])
# cen_2 = np.array([1,1])
# rad_2 = 1
# avoid_func_2 = lambda x: hypersphere_ext(x, center=cen_2,radius=rad_2,
# dims=[0, 1])
# avoid_func = intersect([avoid_func_1, avoid_func_2])
# Make MDP
lamb = 0.0001 #lambda
my_world = Avoid(num_nodes,
s_lims,
num_nodes_a,
a_lims,
dynamics=dynamics,
avoid_func=avoid_func,
lamb=lamb)
grid = my_world._all_states_c
grid_axes = my_world.axes
value, _ = my_world.v_pi_opt(method='pi')
reward = my_world.reward
# Take slice of value and reward grids
dim_fix = [2] # dims to be held fixed
val_fix = [0] # value along fixed dimensions
val_slice, new_shape, new_axes = my_world.slice_grid(
value, dim_fix, val_fix)
# Construct avoid region (circle in 2D euclidean space)
cen = np.array([0, 0]) #center
rad = 5.0 # radius
dist_dims = [0, 1] # dimensions contributing to distance computation
avoid_func = lambda x: hypersphere_ext(
x, center=cen, radius=rad, dims=dist_dims)
# Make MDP
lamb = 0 #lambda
my_world = Avoid(num_nodes,
s_lims,
num_nodes_a,
a_lims,
num_nodes_d,
d_lims,
dynamics=dynamics,
avoid_func=avoid_func,
lamb=lamb,
sparse=True,
angular=[2])
lamb_2 = 0.0001 #lambda
my_world_2 = Avoid(num_nodes,
s_lims,
num_nodes_a,
a_lims,
num_nodes_d,
d_lims,
dynamics=dynamics,
avoid_func=avoid_func,
sys_params['v_u'] = 5
sys_params['v_d'] = 5
dynamics = partial(pursuit_evasion, **sys_params)
# Construct avoid region (circle in 2D euclidean space)
cen = np.array([0,0]) #center
rad = 5.0 # radius
dist_dims = [0, 1] # dimensions contributing to distance computation
avoid_func = lambda x: hypersphere_ext(x, center=cen,radius=rad,
dims= dist_dims)
# Make MDP
lamb = 0.01 #lambda
my_world_c = Avoid(num_nodes_c, s_lims_c, num_nodes_a, a_lims,
num_nodes_d, d_lims, dynamics=dynamics,
avoid_func=avoid_func, lamb=lamb, sparse=True)
my_world_f = Avoid(num_nodes_f, s_lims_f, num_nodes_a, a_lims,
num_nodes_d, d_lims, dynamics=dynamics,
avoid_func=avoid_func, lamb=lamb, sparse=True)
grid_f = my_world_f._all_states_c
# Compute value function on coarse
t_start = time.time()
value_c, _ = my_world_c.v_pi_opt(method='vi')
t_coarse = time.time() - t_start
# Compute value function on fine with warm start value_c
radius = 0.25
avoid_func = lambda x: hypercube_int(x, cube_lims=cube_lims)
face_func = lambda x: hypersphere_ext(x, np.array([2, 0]), 4)
lip_func = lambda x: -hypercube_int(x, lips_lims)
nose_func = lambda x: -hypercube_int(x, nose_lims)
left_eye_func = lambda x: hypersphere_ext(x, left_eye_center, radius)
right_eye_func = lambda x: hypersphere_ext(x, right_eye_center, radius)
union_func = union(shapes=[left_eye_func, right_eye_func])
# With discounting
lamb = 0.1 # lambda
my_world = Avoid(num_nodes,
s_lims,
num_nodes_a,
a_lims,
dynamics,
avoid_func,
lamb=lamb)
all_states = my_world._all_states_c
init_vi = set_minus(face_func, union_func)(all_states)
#init_vi = avoid_func(all_states)
# Compute value function and policy
#v_opt, pi_opt = my_world.v_pi_opt(v=init_vi,method='pi')
'''
# Without discounting
lamb = 0.0 # lambda
my_world_wd = Avoid(num_nodes, s_lims, num_nodes_a, a_lims, dynamics,
avoid_func, lamb=lamb)
max_u = 0.8 * grav
min_u = -0.8 * grav
sys_params['max_u'] = max_u
sys_params['min_u'] = min_u
dynamics_2 = partial(double_integrator, **sys_params)
# Construct avoid region, system should stay within hypercube
cube_lims = np.array([[0, -3], [4, 3]])
avoid_func = lambda x: hypercube_int(x, cube_lims=cube_lims)
# Make MDP
lamb = 0.1 #lambda
my_world = Avoid(num_nodes,
s_lims,
num_nodes_a,
a_lims,
dynamics,
avoid_func,
lamb=lamb)
my_world_2 = Avoid(num_nodes,
s_lims,
num_nodes_a,
a_lims,
dynamics_2,
avoid_func,
lamb=lamb)
# Compute value function and policy
v_opt_1, _ = my_world.v_pi_opt()
max_u_h = 0.4 * grav
min_u_h = -0.4 * grav
sys_params_h['max_u'] = max_u_h
sys_params_h['min_u'] = min_u_h
dynamics_h = partial(double_integrator, **sys_params_h)
# Construct avoid region, system should stay within hypercube
cube_lims = np.array([[0, -3], [4, 3]])
avoid_func = lambda x: hypercube_int(x, cube_lims=cube_lims)
# Make MDP
lamb = 0.1 #lambda
my_world = Avoid(num_nodes,
s_lims,
num_nodes_a,
a_lims,
dynamics=dynamics,
avoid_func=avoid_func,
lamb=lamb,
sparse=True)
# light model
my_world_l = Avoid(num_nodes,
s_lims,
num_nodes_a,
a_lims,
dynamics=dynamics_l,
avoid_func=avoid_func,
lamb=lamb,
sparse=True)
# heavy model
#Dynamical system_2
grav = 9.81 # gravity
sys_params = {} # parameters of dynamical system
max_u = 0.8 * grav
min_u = -0.8 * grav
sys_params['max_u'] = max_u
sys_params['min_u'] = min_u
dynamics_2 = partial(double_integrator, **sys_params)
# Construct avoid region, system should stay within hypercube
cube_lims = np.array([[0, -3], [4, 3]])
avoid_func = lambda x: dist_hypercube_int(x, cube_lims=cube_lims)
# Make MDP
my_world = Avoid(num_nodes, s_lims, num_nodes_a, a_lims, dynamics,
avoid_func)
# Make MDP
#my_world_2 = Avoid(num_nodes, s_lims, num_nodes_a,
# a_lims, dynamics_2, avoid_func)
# Compute value function and policy
v_opt, pi_opt = my_world.v_pi_opt(method='pi')
#v_opt, pi_opt = my_world_2.v_pi_opt(method='pi',pi=pi_opt)
# Gradient of value function
grad, grad_mag = my_world.gradient()
# Computing anaylytic safe set
s_min = s_lims[0]
s_max = s_lims[1]
# Construct avoid region (circle in 2D euclidean space)
cen = np.array([0, 0]) #center
rad = 5.0 # radius
dist_dims = [0, 1] # dimensions contributing to distance computation
avoid_func = lambda x: hypersphere_ext(
x, center=cen, radius=rad, dims=dist_dims)
# Make MDP
lamb = 0.01 #lambda
my_world = Avoid(num_nodes,
s_lims,
num_nodes_a,
a_lims,
num_nodes_d,
d_lims,
dynamics=dynamics,
avoid_func=avoid_func,
lamb=lamb,
sparse=True,
angular=[2])
# Evader has advantage
my_world_a = Avoid(num_nodes,
s_lims,
num_nodes_a,
a_lims_s,
num_nodes_d,
d_lims,
dynamics=dynamics,
avoid_func=avoid_func,
sys_params['min_u'] = min_u
sys_params['max_d'] = max_d
sys_params['min_d'] = min_d
dynamics = partial(double_integrator_dist, **sys_params)
# Construct avoid region, system should stay within hypercube
cube_lims = np.array([[0, -3], [4, 3]])
avoid_func = lambda x: hypercube_int(x, cube_lims=cube_lims)
# Make MDP
lamb = 0.1 #lambda
my_world = Avoid(num_nodes,
s_lims,
num_nodes_a,
a_lims,
num_nodes_d,
d_lims,
dynamics=dynamics,
avoid_func=avoid_func,
lamb=lamb)
# Compute value function and policy
v_opt, pi_opt = my_world.v_pi_opt(method='vi')
# Computing anaylytic safe set
s_min = s_lims[0]
s_max = s_lims[1]
x = range(my_world.num_nodes[0]) * my_world.ds[0] + s_min[0]
y = range(my_world.num_nodes[1]) * my_world.ds[1] + s_min[1]
u_lims = cube_lims[1]
l_lims = cube_lims[0]
sys_params['min_u'] = min_u
dynamics = partial(double_integrator, **sys_params)
#Final model gain
gain_f = 0.2
# Construct avoid region, system should stay within hypercube
cube_lims = np.array([[0, -3], [4, 3]])
avoid_func = lambda x: hypercube_int(x, cube_lims=cube_lims)
# Make MDP
lamb = 0.1 #lambda
my_world = Avoid(num_nodes,
s_lims,
num_nodes_a,
a_lims,
dynamics,
avoid_func,
lamb=lamb)
# Compute value function and policy
v_opt_i, _ = my_world.v_pi_opt()
# Model estimation and value function update
horizon = 101
for k in range(horizon):
print("Online Iteration {}".format(k))
alpha = k / horizon
gain = gain_f * alpha + gain_i * (1 - alpha)
sys_params = {}