def main(grid):
n_domains = 1
n_traj = 1
max_obs = 30, #max number of obstacles
max_obs_size = None,
n_actions = 8
gen = False
if grid.size.width == 100 or grid.size.height == 100:
k = 48
im_size = 100
training_file = 'trained/30k_no_block_dataset_vin_64x64.pth'
elif grid.size.width == 64 or grid.size.height == 64:
k = 48
im_size = 64
training_file = 'trained/30k_no_block_dataset_vin_64x64.pth'
elif grid.size.width == 28 or grid.size.height == 28:
k = 36
im_size = 28
training_file = 'trained/60k_no_block_att3_vin_28x28.pth'
elif grid.size.width == 16 or grid.size.height == 16:
k = 20
im_size = 16
training_file = 'trained/60k_no_block_att3_vin_16x16.pth'
elif grid.size.width == 8 or grid.size.height == 8:
k = 10
im_size = 8
training_file = 'trained/60k_no_block_att3_vin_8x8.pth'
else:
k = 48
im_size = grid.size.width
training_file = 'trained/30k_no_block_dataset_vin_64x64.pth'
# Parsing training parameters
parser = argparse.ArgumentParser()
parser.add_argument(
'--weights',
type=str,
# default='trained/60k_no_block_att3_vin_8x8.pth',
default=training_file,
help='Path to trained weights')
parser.add_argument('--plot', action='store_true', default=False)
parser.add_argument('--gen', action='store_true', default=False)
parser.add_argument('--imsize',
type=int,
default=im_size,
help='Size of image')
parser.add_argument('--k',
type=int,
default=k,
help='Number of Value Iterations')
parser.add_argument('--l_i',
type=int,
default=2,
help='Number of channels in input layer')
parser.add_argument('--l_h',
type=int,
default=150,
help='Number of channels in first hidden layer')
parser.add_argument(
'--l_q',
type=int,
default=10,
help='Number of channels in q layer (~actions) in VI-module')
config = parser.parse_args()
# print("@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@config ",config)
use_GPU = torch.cuda.is_available()
# Instantiate a VIN model
vin = VIN(config)
# Load model parameters
vin.load_state_dict(torch.load(config.weights))
# Use GPU if available
if use_GPU:
vin = vin.cuda()
counter, total_no_soln = 0, 0
global data
data = []
t_list = []
total_dev_non_rel, total_dev_rel = 0.0, 0.0
total_dist, total_astar_dist = 0.0, 0.0
metrics = True #this enables displaying the distance left to reach goal upon a failure
dist_remain_avg = 0.0
for dom in range(n_domains):
if gen:
goal = [
np.random.randint(config.imsize),
np.random.randint(config.imsize)
]
obs = obstacles([config.imsize, config.imsize], goal, max_obs_size)
# Add obstacles to map
n_obs = obs.add_n_rand_obs(max_obs)
# Add border to map
border_res = obs.add_border()
# Ensure we have valid map
if n_obs == 0 or not border_res:
continue
start = None
else:
wpn = True
# path = './resources/maps/'
# path = './resources/testing_maps/8x8/'
# mp, goal, start = open_map(dom,path)
print(grid)
mp = grid.grid
goal = [grid.goal.position.x, grid.goal.position.y]
start = [grid.agent.position.x, grid.agent.position.y]
# path = './maps/8_data_300'
# mp, goal, start = open_map_list(dom,path)
mp[start[1]][
start[0]] = 0 #Set the start position as freespace too
mp[goal[1]][goal[0]] = 0 #Set the goal position as freespace too
goal = [
goal[1], goal[0]
] #swap them around, for the row col format (x = col not row)
start = [start[1], start[0]]
obs = obstacles([config.imsize, config.imsize], goal, max_obs_size)
obs.dom = mp
# print('Goal:', goal,'agent', start, 'gridb4im', mp)
# Get final map
#Between mp and im, the 0 should become 1., and the 1 should become 0.
im = obs.get_final()
# print('Grid: ', mp, '\n Agent', start, '\n Goal', goal)
# print('Im:', im)
#1 is obstacles.
#set obs.dom as the mp
# Generate gridworld from obstacle map
# print(' im: %s ', im)
# print('goal:', goal)
# print('goal:', start)
G = gridworld(im, goal[0], goal[1])
# Get value prior
# print('144')
value_prior = G.get_reward_prior()
# Sample random trajectories to our goal
states_xy, states_one_hot = sample_trajectory(
G, n_traj, start, gen) #dijkstra trajectory
# print('states_xy', states_xy[0] , len(states_xy[0]))
if gen and len(states_xy[0]) > 0:
save_image(G.image, (goal[0], goal[1]), states_xy[0][0], states_xy,
states_one_hot, counter) #this saves the maps
counter += 1
for i in range(n_traj):
if len(states_xy[i]) > 1:
t0 = time.time()
# Get number of steps to goal
L = len(states_xy[i]) * 2
# Allocate space for predicted steps
pred_traj = np.zeros((L, 2))
# Set starting position
pred_traj[0, :] = states_xy[i][0, :]
for j in range(1, L):
# Transform current state data
state_data = pred_traj[j - 1, :]
state_data = state_data.astype(np.int)
# Transform domain to Networks expected input shape
im_data = G.image.astype(np.int)
im_data = 1 - im_data
im_data = im_data.reshape(1, 1, config.imsize,
config.imsize)
# Transfrom value prior to Networks expected input shape
value_data = value_prior.astype(np.int)
value_data = value_data.reshape(1, 1, config.imsize,
config.imsize)
# Get inputs as expected by network
X_in = torch.from_numpy(
np.append(im_data, value_data, axis=1)).float()
S1_in = torch.from_numpy(state_data[0].reshape(
[1, 1])).float()
S2_in = torch.from_numpy(state_data[1].reshape(
[1, 1])).float()
# Send Tensors to GPU if available
if use_GPU:
X_in = X_in.cuda()
S1_in = S1_in.cuda()
S2_in = S2_in.cuda()
# Wrap to autograd.Variable
X_in, S1_in, S2_in = Variable(X_in), Variable(
S1_in), Variable(S2_in)
# Forward pass in our neural net
_, predictions = vin(X_in, S1_in, S2_in, config)
_, indices = torch.max(predictions.cpu(), 1, keepdim=True)
a = indices.data.numpy()[0][0]
# Transform prediction to indices
s = G.map_ind_to_state(pred_traj[j - 1, 0],
pred_traj[j - 1, 1])
ns = G.sample_next_state(s, a)
nr, nc = G.get_coords(ns)
pred_traj[j, 0] = nr
pred_traj[j, 1] = nc
if nr == goal[0] and nc == goal[1]:
# We hit goal so fill remaining steps
pred_traj[j + 1:, 0] = nr
pred_traj[j + 1:, 1] = nc
break
# Plot optimal and predicted path (also start, end)
if pred_traj[-1, 0] == goal[0] and pred_traj[-1, 1] == goal[1]:
print('success!')
# print('pred_traj', pred_traj)
return pred_traj
return pred_traj
def main(config,
n_domains=100,
max_obs=30,
max_obs_size=None,
n_traj=1,
n_actions=8):
# Correct vs total:
correct, total = 0.0, 0.0
# Automatic swith of GPU mode if available
use_GPU = torch.cuda.is_available()
# Instantiate a VIN model
vin = VIN(config)
# Load model parameters
vin.load_state_dict(torch.load(config.weights))
# Use GPU if available
if use_GPU:
vin = vin.cuda()
for dom in range(n_domains):
# Randomly select goal position
goal = [
np.random.randint(config.imsize),
np.random.randint(config.imsize)
]
# Generate obstacle map
obs = obstacles([config.imsize, config.imsize], goal, max_obs_size)
# Add obstacles to map
n_obs = obs.add_n_rand_obs(max_obs)
# Add border to map
border_res = obs.add_border()
# Ensure we have valid map
if n_obs == 0 or not border_res:
continue
# Get final map
im = obs.get_final()
# Generate gridworld from obstacle map
G = gridworld(im, goal[0], goal[1])
# Get value prior
value_prior = G.get_reward_prior()
# Sample random trajectories to our goal
states_xy, states_one_hot = sample_trajectory(G, n_traj)
for i in range(n_traj):
if len(states_xy[i]) > 1:
# Get number of steps to goal
L = len(states_xy[i]) * 2
# Allocate space for predicted steps
pred_traj = np.zeros((L, 2))
# Set starting position
pred_traj[0, :] = states_xy[i][0, :]
for j in range(1, L):
# Transform current state data
state_data = pred_traj[j - 1, :]
state_data = state_data.astype(np.int)
# Transform domain to Networks expected input shape
im_data = G.image.astype(np.int)
im_data = 1 - im_data
im_data = im_data.reshape(1, 1, config.imsize,
config.imsize)
# Transfrom value prior to Networks expected input shape
value_data = value_prior.astype(np.int)
value_data = value_data.reshape(1, 1, config.imsize,
config.imsize)
# Get inputs as expected by network
X_in = torch.from_numpy(
np.append(im_data, value_data, axis=1)).float()
S1_in = torch.from_numpy(state_data[0].reshape(
[1, 1])).float()
S2_in = torch.from_numpy(state_data[1].reshape(
[1, 1])).float()
# Send Tensors to GPU if available
if use_GPU:
X_in = X_in.cuda()
S1_in = S1_in.cuda()
S2_in = S2_in.cuda()
# Wrap to autograd.Variable
X_in, S1_in, S2_in = Variable(X_in), Variable(
S1_in), Variable(S2_in)
# Forward pass in our neural net
_, predictions = vin(X_in, S1_in, S2_in, config)
_, indices = torch.max(predictions.cpu(), 1, keepdim=True)
a = indices.data.numpy()[0][0]
# Transform prediction to indices
s = G.map_ind_to_state(pred_traj[j - 1, 0],
pred_traj[j - 1, 1])
ns = G.sample_next_state(s, a)
nr, nc = G.get_coords(ns)
pred_traj[j, 0] = nr
pred_traj[j, 1] = nc
if nr == goal[0] and nc == goal[1]:
# We hit goal so fill remaining steps
pred_traj[j + 1:, 0] = nr
pred_traj[j + 1:, 1] = nc
break
# Plot optimal and predicted path (also start, end)
if pred_traj[-1, 0] == goal[0] and pred_traj[-1, 1] == goal[1]:
correct += 1
total += 1
if config.plot == True:
visualize(G.image.T, states_xy[i], pred_traj)
sys.stdout.write("\r" + str(int((float(dom) / n_domains) * 100.0)) +
"%")
sys.stdout.flush()
sys.stdout.write("\n")
print('Rollout Accuracy: {:.2f}%'.format(100 * (correct / total)))
def make_data(dom_size, n_domains, max_obs, max_obs_size, n_traj,
state_batch_size):
X_l = []
S1_l = []
S2_l = []
Labels_l = []
dom = 0.0
while dom <= n_domains:
goal = [np.random.randint(dom_size[0]), np.random.randint(dom_size[1])]
# Generate obstacle map
obs = obstacles([dom_size[0], dom_size[1]], goal, max_obs_size)
# Add obstacles to map
n_obs = obs.add_n_rand_obs(max_obs)
# Add border to map
border_res = obs.add_border()
# Ensure we have valid map
if n_obs == 0 or not border_res:
continue
# Get final map
im = obs.get_final()
# Generate gridworld from obstacle map
G = gridworld(im, goal[0], goal[1])
# Get value prior
value_prior = G.t_get_reward_prior()
# Sample random trajectories to our goal
states_xy, states_one_hot = sample_trajectory(G, n_traj)
for i in range(n_traj):
if len(states_xy[i]) > 1:
# Get optimal actions for each state
actions = extract_action(states_xy[i])
ns = states_xy[i].shape[0] - 1
# Invert domain image => 0 = free, 1 = obstacle
image = 1 - im
# Resize domain and goal images and concate
image_data = np.resize(image, (1, 1, dom_size[0], dom_size[1]))
value_data = np.resize(value_prior,
(1, 1, dom_size[0], dom_size[1]))
iv_mixed = np.concatenate((image_data, value_data), axis=1)
X_current = np.tile(iv_mixed, (ns, 1, 1, 1))
# Resize states
S1_current = np.expand_dims(states_xy[i][0:ns, 0], axis=1)
S2_current = np.expand_dims(states_xy[i][0:ns, 1], axis=1)
# Resize labels
Labels_current = np.expand_dims(actions, axis=1)
# Append to output list
X_l.append(X_current)
S1_l.append(S1_current)
S2_l.append(S2_current)
Labels_l.append(Labels_current)
dom += 1
sys.stdout.write("\r" + str(int((dom / n_domains) * 100)) + "%")
sys.stdout.flush()
sys.stdout.write("\n")
# Concat all outputs
X_f = np.concatenate(X_l)
S1_f = np.concatenate(S1_l)
S2_f = np.concatenate(S2_l)
Labels_f = np.concatenate(Labels_l)
return X_f, S1_f, S2_f, Labels_f
def X2R(self,X):
goal = [np.argmax(X[1])//config.imsize,np.argmax(X[1])%config.imsize]
G = gridworld(1-X[0], goal[0], goal[1])
R = X[1] - (1-X[0])*0.02 - 2 * X[0]
return R,goal
def __init__(self,X):
self.R ,self.goal = self.X2R(X)
self.G = gridworld(1-X[0], self.goal[0], self.goal[1])
self.actions = np.asarray([[-1, 0], [1, 0], [0, 1], [0, -1],
[-1, 1], [-1, -1], [1, 1], [1, -1]])
def make_data(dom_size, n_domains, max_obs, max_obs_size, n_traj,
state_batch_size):
X_l = []
S1_l = []
S2_l = []
Labels_l = []
dom = 0.0
while dom <= n_domains:
goal = [np.random.randint(dom_size[0]), np.random.randint(dom_size[1])]
# Generate obstacle map
obs = obstacles([dom_size[0], dom_size[1]], goal, max_obs_size)
# Add obstacles to map
n_obs = obs.add_n_rand_obs(max_obs)
# Add border to map
border_res = obs.add_border()
# Ensure we have valid map
if n_obs == 0 or not border_res:
continue
# Get final map
im = obs.get_final()
# Generate gridworld from obstacle map
G = gridworld(im, goal[0], goal[1])
# Get value prior
value_prior = G.t_get_reward_prior()
# Sample random trajectories to our goal
states_xy, states_one_hot = sample_trajectory(G, n_traj)
for i in range(n_traj):
if len(states_xy[i]) > 1:
# Get optimal actions for each state
actions = extract_action(states_xy[i])
ns = states_xy[i].shape[0] - 1
# Invert domain image => 0 = free, 1 = obstacle
image = 1 - im
# Resize domain and goal images and concate
image_data = np.resize(image, (1, 1, dom_size[0], dom_size[1]))
value_data = np.resize(value_prior,
(1, 1, dom_size[0], dom_size[1]))
iv_mixed = np.concatenate((image_data, value_data), axis=1)
X_current = np.tile(iv_mixed, (ns, 1, 1, 1))
# Resize states
S1_current = np.expand_dims(states_xy[i][0:ns, 0], axis=1)
S2_current = np.expand_dims(states_xy[i][0:ns, 1], axis=1)
# Resize labels
Labels_current = np.expand_dims(actions, axis=1)
# Append to output list
X_l.append(X_current)
S1_l.append(S1_current)
S2_l.append(S2_current)
Labels_l.append(Labels_current)
dom += 1
sys.stdout.write("\r" + str(int((dom / n_domains) * 100)) + "%")
sys.stdout.flush()
sys.stdout.write("\n")
# Concat all outputs
X_f = np.concatenate(X_l)
S1_f = np.concatenate(S1_l)
S2_f = np.concatenate(S2_l)
Labels_f = np.concatenate(Labels_l)
return X_f, S1_f, S2_f, Labels_f
def main(config,
n_domains=100,
max_obs=30,
max_obs_size=None,
n_traj=1,
n_actions=8):
# Correct vs total:
correct, total = 0.0, 0.0
# Automatic swith of GPU mode if available
use_GPU = torch.cuda.is_available()
# Instantiate a VIN model
vin = VIN(config)
# Load model parameters
vin.load_state_dict(torch.load(config.weights))
# Use GPU if available
if use_GPU:
vin = vin.cuda()
for dom in range(n_domains):
# Randomly select goal position
goal = [
np.random.randint(config.imsize),
np.random.randint(config.imsize)
]
# Generate obstacle map
obs = obstacles([config.imsize, config.imsize], goal, max_obs_size)
# Add obstacles to map
n_obs = obs.add_n_rand_obs(max_obs)
# Add border to map
border_res = obs.add_border()
# Ensure we have valid map
if n_obs == 0 or not border_res:
continue
# Get final map
im = obs.get_final()
# Generate gridworld from obstacle map
G = gridworld(im, goal[0], goal[1])
# Get value prior
value_prior = G.get_reward_prior()
# Sample random trajectories to our goal
states_xy, states_one_hot = sample_trajectory(G, n_traj)
for i in range(n_traj):
if len(states_xy[i]) > 1:
# Get number of steps to goal
L = len(states_xy[i]) * 2
# Allocate space for predicted steps
pred_traj = np.zeros((L, 2))
# Set starting position
pred_traj[0, :] = states_xy[i][0, :]
for j in range(1, L):
# Transform current state data
state_data = pred_traj[j - 1, :]
state_data = state_data.astype(np.int)
# Transform domain to Networks expected input shape
im_data = G.image.astype(np.int)
im_data = 1 - im_data
im_data = im_data.reshape(1, 1, config.imsize,
config.imsize)
# Transfrom value prior to Networks expected input shape
value_data = value_prior.astype(np.int)
value_data = value_data.reshape(1, 1, config.imsize,
config.imsize)
# Get inputs as expected by network
X_in = torch.from_numpy(
np.append(im_data, value_data, axis=1)).float()
S1_in = torch.from_numpy(state_data[0].reshape(
[1, 1])).float()
S2_in = torch.from_numpy(state_data[1].reshape(
[1, 1])).float()
# Send Tensors to GPU if available
if use_GPU:
X_in = X_in.cuda()
S1_in = S1_in.cuda()
S2_in = S2_in.cuda()
# Wrap to autograd.Variable
X_in, S1_in, S2_in = Variable(X_in), Variable(
S1_in), Variable(S2_in)
# Forward pass in our neural net
_, predictions = vin(X_in, S1_in, S2_in, config)
_, indices = torch.max(predictions.cpu(), 1, keepdim=True)
a = indices.data.numpy()[0][0]
# Transform prediction to indices
s = G.map_ind_to_state(pred_traj[j - 1, 0],
pred_traj[j - 1, 1])
ns = G.sample_next_state(s, a)
nr, nc = G.get_coords(ns)
pred_traj[j, 0] = nr
pred_traj[j, 1] = nc
if nr == goal[0] and nc == goal[1]:
# We hit goal so fill remaining steps
pred_traj[j + 1:, 0] = nr
pred_traj[j + 1:, 1] = nc
break
# Plot optimal and predicted path (also start, end)
if pred_traj[-1, 0] == goal[0] and pred_traj[-1, 1] == goal[1]:
correct += 1
total += 1
if config.plot == True:
visualize(G.image.T, states_xy[i], pred_traj)
sys.stdout.write("\r" + str(int(
(float(dom) / n_domains) * 100.0)) + "%")
sys.stdout.flush()
sys.stdout.write("\n")
print('Rollout Accuracy: {:.2f}%'.format(100 * (correct / total)))
