def test_pack_unpack_special(self):
packed = pack_special_instruction(op_code=OPCODE.JSR,
a=Value.reg(REG.A))
op_code, a = unpack_special_instruction(packed)
self.assertEqual(op_code, OPCODE.JSR)
self.assertEqual(a, Value.reg(REG.A))
def test_set_reg_to_literal(self):
""" Sets a register to a literal """
# SET A, 0x10
self.cpu.ram[0].value = pack_instruction(op_code=OPCODE.SET,
a=Value.reg(REG.A),
b=Value.literal(0x10))
self.emulator.dispatch()
self.assertTrue(self.cpu.registers[REG.A].value == 0x10, "Register value error")
def test_set_pc(self):
""" Sets PC """
self.cpu.ram[0].value = pack_instruction(op_code=OPCODE.SET,
a=Value.pc(),
b=Value.literal(0x5))
self.emulator.dispatch()
self.assertEqual(self.cpu.PC.value, 0x5, "PC Loading failed")
def test_pack_unpack(self):
packed = pack_instruction(op_code=OPCODE.IFG,
a=Value.reg(REG.A),
b=Value.reg(REG.B))
op_code, b, a = unpack_instruction(packed)
self.assertEqual(op_code, OPCODE.IFG)
self.assertEqual(b, Value.reg(REG.A))
self.assertEqual(a, Value.reg(REG.B))
def compute_path(grid,start,goal,cost,heuristic):
# Use the OrderedSet for your closed list
closed_set = OrderedSet()
# Use thePriorityQueue for the open list
open_set = PriorityQueue(order=min, f=lambda v: v.f)
# Keep track of the parent of each node. Since the car can take 4 distinct orientations,
# for each orientation we can store a 2D array indicating the grid cells.
# E.g. parent[0][2][3] will denote the parent when the car is at (2,3) facing up
parent = [[[' ' for row in range(len(grid[0]))] for col in range(len(grid))],
[[' ' for row in range(len(grid[0]))] for col in range(len(grid))],
[[' ' for row in range(len(grid[0]))] for col in range(len(grid))],
[[' ' for row in range(len(grid[0]))] for col in range(len(grid))]]
# The path of the car
path =[['-' for row in range(len(grid[0]))] for col in range(len(grid))]
x = start[0]
y = start[1]
theta = start[2]
h = heuristic[x][y]
g = 0
f = g+h
open_set.put(start, Value(f=f,g=g))
# your code: implement A*
# Initially you may want to ignore theta, that is, plan in 2D.
# To do so, set actions=forward, cost = [1, 1, 1, 1], and action_name = ['U', 'L', 'R', 'D']
# Similarly, set parent=[[' ' for row in range(len(grid[0]))] for col in range(len(grid))]
while open_set:
node = open_set.pop()
closed_set.add(node[0])
if node[0] == goal:
break
#finding child of node
#write function for finding child node
#setting the cost values for neighboring nodes
neighbors = get_neighbors(node[0])
print(neighbors)
for i in neighbors:
x = i[0]
y = i[1]
theta = i[2]
h = heuristic[x][y]
g = node[1].g + costfunction(node[0],i)
f = g+h
if i not in open_set or i not in closed_set:
open_set.put(i,Value(f,g))
elif i in open_set and f < open_set.get(i).f:
open_set.put(i,Value(f,g))
return path, closed_set
def test_set_sp(self):
""" Sets SP """
# SET SP, 0x02
self.cpu.ram[0].value = pack_instruction(op_code=OPCODE.SET,
a=Value.sp(),
b=Value.literal(0x02))
self.emulator.dispatch()
self.assertTrue(self.cpu.SP.value == 0x2, "SP Loading failed")
def test_read_ex(self):
""" Reads O into register """
self.cpu.EX.value = 0xffff
# SET A, EX
self.cpu.ram[0].value = pack_instruction(op_code=OPCODE.SET,
a=Value.reg(REG.A),
b=Value.reg(REG.EX))
self.emulator.dispatch()
self.assertTrue(self.cpu.EX.value == 0xffff, "O Loading failed")
def test_peek_into_register(self):
""" Pushed value in register to stack """
self.cpu.ram[self.cpu.SP.value].value = 0xbeef
# SET A, PEEK
self.cpu.ram[0].value = pack_instruction(op_code=OPCODE.SET,
a=Value.reg(REG.A),
b=Value.peek())
self.emulator.dispatch()
self.assertTrue(self.cpu.registers[REG.A].value == 0xbeef, "Stack peeking b0rked")
def test_push_from_register(self):
""" Pushed value in register to stack """
self.cpu.registers[REG.A].value = 0xdead
# SET PUSH, A
self.cpu.ram[0].value = pack_instruction(op_code=OPCODE.SET,
a=Value.push_pop(),
b=Value.reg(REG.A))
self.emulator.dispatch()
self.assertTrue(self.cpu.ram[self.cpu.SP.value].value == 0xdead, "Stack pushing b0rked")
def test_pop_into_reg(self):
""" Pops from stack into register """
self.cpu.ram[self.cpu.SP.value].value = 0xdead
# SET A POP
self.cpu.ram[0].value = pack_instruction(op_code=OPCODE.SET,
a=Value.reg(REG.A),
b=Value.push_pop())
self.emulator.dispatch()
self.assertTrue(self.cpu.registers[REG.A].value == 0xdead, "Stack popping b0rked")
def test_ifc_equal(self):
self.cpu.registers[REG.A].value = 0x0000
self.cpu.registers[REG.B].value = 0x00ff
# IFC A,B
self.cpu.ram[0].value = pack_instruction(op_code=OPCODE.IFC,
a=Value.reg(REG.A),
b=Value.reg(REG.B))
self.emulator.dispatch()
self.assertEqual(self.cpu.skip_instruction, False, "PC not set right")
def test_ife_equal(self):
self.cpu.registers[REG.A].value = 0x00ff
self.cpu.registers[REG.B].value = 0x00ff
# IFE A,B
self.cpu.ram[0].value = pack_instruction(op_code=OPCODE.IFE,
a=Value.reg(REG.A),
b=Value.reg(REG.B))
self.emulator.dispatch()
self.assertTrue(self.cpu.PC.value == 1, "PC not set right")
def test_ifb_notequal(self):
self.cpu.registers[REG.A].value = 0x00ff
self.cpu.registers[REG.B].value = 0x0000
# IFB A,B
self.cpu.ram[0].value = pack_instruction(op_code=OPCODE.IFB,
a=Value.reg(REG.A),
b=Value.reg(REG.B))
self.emulator.dispatch()
self.assertEqual(self.cpu.skip_instruction, True, "Skip error")
def test_ifu(self):
self.cpu.registers[REG.A].value = 3
self.cpu.registers[REG.B].value = -4
# IFU A,B
self.cpu.ram[0].value = pack_instruction(op_code=OPCODE.IFU,
a=Value.reg(REG.A),
b=Value.reg(REG.B))
self.emulator.dispatch()
self.assertEqual(self.cpu.skip_instruction, True, "IFU failed")
def test_adx(self):
self.cpu.registers[REG.A].value = 0xfffe
self.cpu.registers[REG.B].value = 0x0002
self.cpu.EX.value = 0
# ADX A,B
self.cpu.ram[0].value = pack_instruction(op_code=OPCODE.ADX,
a=Value.reg(REG.A),
b=Value.reg(REG.B))
self.emulator.dispatch()
self.assertEqual(self.cpu.EX.value, 1, "ADX failed")
def test_xor(self):
""" Sets REG.A to REG.A ^ REG.B """
self.cpu.registers[REG.A].value = 0x00ff
self.cpu.registers[REG.B].value = 0x000f
# XOR A,B
self.cpu.ram[0].value = pack_instruction(op_code=OPCODE.XOR,
a=Value.reg(REG.A),
b=Value.reg(REG.B))
self.emulator.dispatch()
self.assertTrue(self.cpu.registers[REG.A].value == 0xf0, "xor failed")
def test_and(self):
""" Sets REG.A to REG.A & REG.B """
self.cpu.registers[REG.A].value = 0x00ff
self.cpu.registers[REG.B].value = 0x000f
# AND A,B
self.cpu.ram[0].value = pack_instruction(op_code=OPCODE.AND,
a=Value.reg(REG.A),
b=Value.reg(REG.B))
self.emulator.dispatch()
self.assertTrue(self.cpu.registers[REG.A].value == 0xf, "and failed")
def test_shr(self):
""" Sets REG.A to REGREG.> REG.B """
self.cpu.registers[REG.A].value = 0x0008
self.cpu.registers[REG.B].value = 0x0002
# SHR A,B
self.cpu.ram[0].value = pack_instruction(op_code=OPCODE.SHR,
a=Value.reg(REG.A),
b=Value.reg(REG.B))
self.emulator.dispatch()
self.assertTrue(self.cpu.registers[REG.A].value == 0x2, "shr failed")
def test_mod(self):
""" Sets REG.A to REG.A % REG.B """
self.cpu.registers[REG.A].value = 0x0009
self.cpu.registers[REG.B].value = 0x0002
# MOD A,B
self.cpu.ram[0].value = pack_instruction(op_code=OPCODE.MOD,
a=Value.reg(REG.A),
b=Value.reg(REG.B))
self.emulator.dispatch()
self.assertTrue(self.cpu.registers[REG.A].value == 0x1, "Mod failed")
def test_dvi(self):
""" Sets REG.A to REG.A / REG.B """
self.cpu.registers[REG.A].value = -4
self.cpu.registers[REG.B].value = -2
# DVI A,B
self.cpu.ram[0].value = pack_instruction(op_code=OPCODE.DVI,
a=Value.reg(REG.A),
b=Value.reg(REG.B))
self.emulator.dispatch()
self.assertTrue(self.cpu.registers[REG.A].value == 2, "Div failed")
def test_div(self):
""" Sets REG.A to REG.A / REG.B """
self.cpu.registers[REG.A].value = 0x0009
self.cpu.registers[REG.B].value = 0x0002
# DIV A,B
self.cpu.ram[0].value = pack_instruction(op_code=OPCODE.DIV,
a=Value.reg(REG.A),
b=Value.reg(REG.B))
self.emulator.dispatch()
self.assertTrue(self.cpu.registers[REG.A].value == 0x4, "Div failed")
def test_next_word_literal(self):
""" Reads next word as a literal """
# SET A 0x1f
self.cpu.ram[0].value = pack_instruction(op_code=OPCODE.SET,
a=Value.reg(REG.A),
b=Value.next_word_literal())
self.cpu.ram[1].value = 0x1234
self.emulator.dispatch()
self.assertTrue(self.cpu.registers[REG.A].value == 0x1234, "Next word as literal error")
def test_ram_of_next_word(self):
""" Reads from register to ram of next word into register"""
self.cpu.ram[0x44].value = 0x1212
# SET X [next word]
self.cpu.ram[0].value = pack_instruction(op_code=OPCODE.SET,
a=Value.reg(REG.X),
b=Value.next_word_addr())
self.cpu.ram[1].value = 0x44
self.emulator.dispatch()
self.assertTrue(self.cpu.registers[REG.X].value == 0x1212, "Ram of next word error")
def test_sbx(self):
self.cpu.registers[REG.A].value = 0x0003
self.cpu.registers[REG.B].value = 0x0008
self.cpu.EX.value = 0
# SBX A,B
self.cpu.ram[0].value = pack_instruction(op_code=OPCODE.SBX,
a=Value.reg(REG.A),
b=Value.reg(REG.B))
self.emulator.dispatch()
self.assertEqual(self.cpu.EX.value, 1, "SBX failed")
def test_sub_underflow(self):
""" Sets REG.A to REG.A - REG.B, underflows """
self.cpu.registers[REG.A].value = 0x0001
self.cpu.registers[REG.B].value = 0x0002
# SUB A, B
self.cpu.ram[0].value = pack_instruction(op_code=OPCODE.SUB,
a=Value.reg(REG.A),
b=Value.reg(REG.B))
self.emulator.dispatch()
self.assertTrue(self.cpu.registers[REG.A].value == 0x0001, "Sub failed")
self.assertTrue(self.cpu.EX.value == 0xffff, "Ex flag not set")
def test_mul_overflows(self):
""" Sets REG.A to REG.A * REG.B, overflows """
self.cpu.registers[REG.A].value = 0xffff
self.cpu.registers[REG.B].value = 0x0002
# MUL A, B
self.cpu.ram[0].value = pack_instruction(op_code=OPCODE.MUL,
a=Value.reg(REG.A),
b=Value.reg(REG.B))
self.emulator.dispatch()
self.assertTrue(self.cpu.registers[REG.A].value == 0xfffe, "Mul failed")
self.assertTrue(self.cpu.EX.value == 0x1, "Ex flag not set")
def test_asr(self):
""" Sets REG.A to REG.A << REG.B """
self.cpu.registers[REG.A].value = -3
self.cpu.registers[REG.B].value = 2
# ASR A,B
self.cpu.ram[0].value = pack_instruction(op_code=OPCODE.ASR,
a=Value.reg(REG.A),
b=Value.reg(REG.B))
self.emulator.dispatch()
signed_result = c_int16(self.cpu.registers[REG.A].value)
self.assertEqual(signed_result.value, -1, "ASR failed")
def test_set_addr_of_reg(self):
""" Sets ram pointed by REG.I to value of REG.B """
# Arrange
self.cpu.registers[REG.I].value = 0xff
self.cpu.registers[REG.B].value = 0x11
# SET [I] B
self.cpu.ram[0].value = pack_instruction(op_code=OPCODE.SET,
a=Value.addr_reg(REG.I),
b=Value.reg(REG.B))
self.emulator.dispatch()
self.assertTrue(self.cpu.ram[0xff].value == 0x11, "Ram not updated correctly")
def test_mdi(self):
""" Sets REG.A to REG.A % REG.B """
self.cpu.registers[REG.A].value = -7
self.cpu.registers[REG.B].value = 16
# MDI A,B
self.cpu.ram[0].value = pack_instruction(op_code=OPCODE.MDI,
a=Value.reg(REG.A),
b=Value.reg(REG.B))
self.emulator.dispatch()
signed_result = c_int16(self.cpu.registers[REG.A].value)
self.assertEqual(signed_result.value, -7, "MDI failed")
def test_mul(self):
""" Sets REG.A to REG.A * REG.B """
self.cpu.registers[REG.A].value = 0x0002
self.cpu.registers[REG.B].value = 0x0003
# MUL A, B
self.cpu.ram[0].value = pack_instruction(op_code=OPCODE.MUL,
a=Value.reg(REG.A),
b=Value.reg(REG.B))
self.emulator.dispatch()
self.assertTrue(self.cpu.registers[REG.A].value == 0x6, "Mul failed")
self.assertTrue(self.cpu.EX.value == 0x0, "Ex flag not reset")
def test_add_overflow(self):
""" Sets REG.A to REG.A + REG.B, overflows """
self.cpu.registers[REG.A].value = 0xffff
self.cpu.registers[REG.B].value = 0x0001
# ADD A, B
self.cpu.ram[0].value = pack_instruction(op_code=OPCODE.ADD,
a=Value.reg(REG.A),
b=Value.reg(REG.B))
self.emulator.dispatch()
self.assertTrue(self.cpu.registers[REG.A].value == 0x0000, "Add failed")
self.assertTrue(self.cpu.EX.value == 0xffff, "Ex flag not reset")
def compute_path(grid,start,goal,cost,heuristic):
# Use the OrderedSet for your closed list
closed_set = OrderedSet()
# Use thePriorityQueue for the open list
open_set = PriorityQueue(order=min, f=lambda v: v.f)
# Keep track of the parent of each node. Since the car can take 4 distinct orientations,
# for each orientation we can store a 2D array indicating the grid cells.
# E.g. parent[0][2][3] will denote the parent when the car is at (2,3) facing up
parent = [[[' ' for row in range(len(grid[0]))] for col in range(len(grid))],
[[' ' for row in range(len(grid[0]))] for col in range(len(grid))],
[[' ' for row in range(len(grid[0]))] for col in range(len(grid))],
[[' ' for row in range(len(grid[0]))] for col in range(len(grid))]]
# The path of the car
path =[['-' for row in range(len(grid[0]))] for col in range(len(grid))]
x = start[0]
y = start[1]
theta = start[2]
h = heuristic[x][y]
g = 0
f = g+h
open_set.put(start, Value(f=f,g=g))
# your code: implement A*
# Initially you may want to ignore theta, that is, plan in 2D.
# To do so, set actions=forward, cost = [1, 1, 1, 1], and action_name = ['U', 'L', 'R', 'D']
# Similarly, set parent=[[' ' for row in range(len(grid[0]))] for col in range(len(grid))]
return path, closed_set
def compute_path(grid, start, goal, cost, heuristic):
# Use the OrderedSet for your closed list
closed_set = OrderedSet()
# Use thePriorityQueue for the open list
open_set = PriorityQueue(order=min, f=lambda v: v.f)
parent = [[[' ' for row in range(len(grid[0]))]
for col in range(len(grid))],
[[' ' for row in range(len(grid[0]))]
for col in range(len(grid))],
[[' ' for row in range(len(grid[0]))]
for col in range(len(grid))],
[[' ' for row in range(len(grid[0]))]
for col in range(len(grid))]]
# The path of the car
path = [['-' for row in range(len(grid[0]))] for col in range(len(grid))]
x = start[0]
y = start[1]
theta = start[2]
g_value = 0
h_value = heuristic[x][y]
f = g_value + h_value
parent_cost = [[[1000000000 for row in range(len(grid[0]))]
for col in range(len(grid))],
[[1000000000 for row in range(len(grid[0]))]
for col in range(len(grid))],
[[1000000000 for row in range(len(grid[0]))]
for col in range(len(grid))],
[[1000000000 for row in range(len(grid[0]))]
for col in range(len(grid))]]
open_set.put(start, Value(f=f, g=g_value))
########################### IMPLEMENTATION OF DIJKSTRA ALGORITHM ###################################
####################################################################################################
####################################################################################################
def dijkstra(grid, source):
length = len(grid)
Q_value = [(0, source)]
value0 = [inf for i in range(length)]
value0[source] = 0
while len(Q) != 0:
(cost, u) = hq.heappop(Q_value)
for v in range(n):
if value0[v] > value0[u] + grid[u][v]:
value0[v] = value0[u] + grid[u][v]
hq.heappush(Q, (value0[v], v))
return value0
print('The dijkstra value is:', value0)
return path, closed_set
def train_loop(options):
Print = get_printer(options)
Print(options)
Save = get_func_on_master(torch.save, options)
# distributed stuff
gpus = get_gpu_ids()
options.cuda_device = f"cuda:{options.local_rank}"
torch.cuda.set_device(options.local_rank)
if options.distributed:
torch.distributed.init_process_group(backend='nccl',
init_method="env://",
world_size=len(gpus),
rank=options.local_rank)
model = get_model(options)
# Print(model)
dataset = getattr(datasets, options.dataset)(options)
if options.use_trainval:
train_loader = get_loader(dataset.trainval_set, options)
else:
train_loader = get_loader(dataset.train_set, options)
num_train_classes = len(train_loader.dataset.classes)
# Switch off for validation and testing
options.shuffle = False
plain_train_loader = get_loader(dataset.plain_train_set, options)
test_loader = get_loader(dataset.test_set, options)
num_test_classes = len(test_loader.dataset.classes)
valid_loader = get_loader(dataset.valid_set, options)
num_valid_classes = len(valid_loader.dataset.classes)
criterion = getattr(losses, options.loss_function)(options)
final_optimizer = get_optimizer(model, options)
scheduler = get_scheduler(final_optimizer, options)
time_track = AverageMeter()
best_model_state = model.state_dict()
loss_val = Value(1e6, min, name="loss")
loss_printer = ValuePrinter()
loss_printer.track(loss_val)
test_eval = Value(-1e6, max, name="test_acc")
val_eval = Value(-1e6, max, name="val_acc")
# train_eval = Value(-1e6, max, name="train_acc")
eval_printer = ValuePrinter()
# eval_printer.track(train_eval)
eval_printer.track(val_eval)
eval_printer.track(test_eval)
Print((f"Starting Training on:\n"
f"Train: {num_train_classes:>3d} classes\n"
f"Valid: {num_valid_classes:>3d} classes\n"
f"Test: {num_test_classes:>3d} classes"))
Print("-" * 18)
for epoch in range(options.num_epochs):
model.train()
epoch_loss_track = AverageMeter()
# epoch start
epoch_start = time.time()
for aug1, aug2, _ in train_loader:
final_optimizer.zero_grad()
feat1 = model(aug1.to(device=options.cuda_device))
feat2 = model(aug2.to(device=options.cuda_device))
loss = criterion(feat1, feat2)
loss.backward()
final_optimizer.step()
epoch_loss_track.accumulate(loss.item())
scheduler.step()
# epoch end
time_track.accumulate(time.time() - epoch_start)
loss_val.update(epoch_loss_track.value())
Print(
f"({time_track.latest():>7.3f}s) Epoch {epoch+1:0>3}/{options.num_epochs:>3}:",
end='')
Print(loss_printer.get_formatted_line(), end='')
if loss_val.current_is_best:
best_model_state = model.state_dict()
if options.local_rank == 0 and epoch % options.eval_freq == options.eval_freq - 1:
eval_start = time.time()
model.eval()
# train_eval.update(kmeans_on_data(model, plain_train_loader, options))
val_eval.update(kmeans_on_data(model, valid_loader, options))
test_eval.update(kmeans_on_data(model, test_loader, options))
model.train()
eval_time = time.time() - eval_start
Print(f" ({eval_time:>7.3f}s) ", end='')
Print(eval_printer.get_formatted_line(), end='')
Print()
Print((
f"Training for {options.num_epochs} epochs took {time_track.total():.3f}s total "
f"and {time_track.value():.3f}s average"))
Print(
"Calculating mean of transformed dataset using the best model state ...",
end='')
# since this is what will be saved later
model.load_state_dict(best_model_state)
model.eval()
scaler = StandardScaler(copy=False, with_std=False)
mean_time = time.time()
for data, _ in plain_train_loader:
# feat = model.module.backbone(data.to(device=options.cuda_device)).detach().cpu().numpy()
feat = model(
data.to(device=options.cuda_device)).detach().cpu().numpy()
scaler.partial_fit(feat)
mean_time = time.time() - mean_time
Print(f" {mean_time:.3f}s")
options.train_scaler = scaler
options.log_file = options.log_file.name
Print(f"Saving best model and options to {options.save_path}")
save_dict = {'option': options}
if options.save_model:
save_dict['model_state_dict'] = best_model_state
Save(save_dict, options.save_path)
def compute_path(grid, start, goal, cost, heuristic):
# Use the OrderedSet for your closed list
closed_set = OrderedSet()
# Use thePriorityQueue for the open list
open_set = PriorityQueue(order=min, f=lambda v: v.f)
# Keep track of the parent of each node. Since the car can take 4 distinct orientations,
# for each orientation we can store a 2D array indicating the grid cells.
# E.g. parent[0][2][3] will denote the parent when the car is at (2,3) facing up
parent = [[[' ' for row in range(len(grid[0]))]
for col in range(len(grid))],
[[' ' for row in range(len(grid[0]))]
for col in range(len(grid))],
[[' ' for row in range(len(grid[0]))]
for col in range(len(grid))],
[[' ' for row in range(len(grid[0]))]
for col in range(len(grid))]]
#print(parent)
# The path of the car
path = [['-' for row in range(len(grid[0]))] for col in range(len(grid))]
x = start[0]
y = start[1]
theta = start[2]
h = heuristic[x][y]
g = 0
f = g + h
open_set.put(start, Value(f=f, g=g))
# your code: implement A*
# Initially you may want to ignore theta, that is, plan in 2D.
# To do so, set actions=forward, cost = [1, 1, 1, 1], and action_name = ['U', 'L', 'R', 'D']
# Similarly, set parent=[[' ' for row in range(len(grid[0]))] for col in range(len(grid))]
while open_set:
node = open_set.pop()
closed_set.add(node[0])
if (node[0] == goal):
break
# Finding child for each node.
neighbours = find_child(node[0])
for neighbour in neighbours:
#print(neighbour)
#print(node[0])
g = node[1].g + cost_neigh(neighbour, node[0])
x = neighbour[0]
y = neighbour[1]
theta = neighbour[2]
h = heuristic[x][y]
f = g + h
if (neighbour not in open_set or neighbour not in closed_set):
open_set.put(neighbour, Value(f, g))
a = parent[theta][x][y]
if (a == ' ' or f < a[1].g + cost_neigh(neighbour, a[0]) + h):
parent[theta][x][y] = node
elif (neighbour in open_set and f < open_set.get(neighbour).f):
open_set.put(neighbour, Value(f, g))
if (a == ' ' or f < a[1].g + cost_neigh(neighbour, a[0]) + h):
parent[theta][x][y] = node
g = goal
d = []
while (g != start):
#print(g)
d.append(g)
g = parent[g[2]][g[0]][g[1]][0]
d.append(g)
d.reverse()
#print(d)
direction_list = []
for i in range(0, len(d) - 1):
direction = get_direction(d[i + 1], d[i])
direction_list.append(direction)
#print(direction_list);
direction_list.append("*")
for i in range(0, len(d)):
x = d[i][0]
y = d[i][1]
path[x][y] = direction_list[i]
return path, closed_set
def compute_path(grid, start, goal, cost, heuristic):
# Use the OrderedSet for your closed list
closed_set = OrderedSet()
# Use thePriorityQueue for the open list
open_set = PriorityQueue(order=min, f=lambda v: v.f)
# Keep track of the parent of each node. Since the car can take 4 distinct orientations,
# for each orientation we can store a 2D array indicating the grid cells.
# E.g. parent[0][2][3] will denote the parent when the car is at (2,3) facing up
parent = [[[' ' for row in range(len(grid[0]))]
for col in range(len(grid))],
[[' ' for row in range(len(grid[0]))]
for col in range(len(grid))],
[[' ' for row in range(len(grid[0]))]
for col in range(len(grid))],
[[' ' for row in range(len(grid[0]))]
for col in range(len(grid))]]
# The path of the car
path = [['-' for row in range(len(grid[0]))] for col in range(len(grid))]
x = start[0]
y = start[1]
theta = start[2]
h = heuristic[x][y]
g = 0
f = g + h
open_set.put(start, Value(f=f, g=g))
# your code: implement A*
# Initially you may want to ignore theta, that is, plan in 2D.
# To do so, set actions=forward, cost = [1, 1, 1, 1], and action_name = ['U', 'L', 'R', 'D']
# Similarly, set parent=[[' ' for row in range(len(grid[0]))] for col in range(len(grid))]
path[goal[0]][goal[1]] = '*'
l = []
l.append(start)
while (len(open_set) != 0):
curr_node = open_set.pop()
if (curr_node[0][:2] != start[:2]):
path[l[-1][0]][l[-1][1]] = curr_node[1].g
l.append(curr_node[0])
if (curr_node[0] == goal):
print("get path!")
closed_set.add(curr_node)
break
closed_set.add(curr_node)
x_current = curr_node[0][0] #row of current node
y_current = curr_node[0][1] #col of current node
o_current = curr_node[0][2] #orientation of current node
for i_act, act in enumerate(action):
o_next = (o_current + act) % 4 #orientation of next node
x_next = x_current + forward[o_next][0] #row of next node
y_next = y_current + forward[o_next][1] #col of next node
n_act = action_name[
i_act] #action to get this next node, this cause the one step delay in the display
if (x_next >= 0 and x_next < len(grid) and y_next >= 0
and y_next < len(grid[0])
): #filter the available child (map boundery and barrier)
if (grid[x_next][y_next] == 0):
F_cost = cost[i_act] + heuristic[x_next][
y_next] # abs(x_next - goal[0]) + abs(y_next - goal[1]) #calculate the f(n) = g(n) + h(n)
if ((not open_set.has((x_next, y_next, o_next)))
or (not closed_set.has((x_next, y_next, o_next)))):
open_set.put((x_next, y_next, o_next),
Value(f=F_cost, g=n_act))
#parent[x_next][y_next] = (x_next, y_next, o_next)
elif (open_set.has((x_next, y_next, o_next))
and F_cost < open_set.get(
(x_next, y_next, o_next)).f):
open_set.get((x_next, y_next, o_next)).f = F_cost
open_set.get((x_next, y_next, o_next)).g = n_act
if (curr_node[0] != goal): print("fail to find the path!")
return path, closed_set
def dijkstra(grid, start, goal, cost): #dijkstra algorithm
closed_set = OrderedSet()
open_set = PriorityQueue(order=min, f=lambda v: v.f)
l = []
l.append(goal[:2])
path = [['-' for row in range(len(grid[0]))] for col in range(len(grid))]
dist = [[float('inf') for row in range(len(grid[0]))]
for col in range(len(grid))]
parent = [['NULL' for row in range(len(grid[0]))]
for col in range(len(grid))]
open_set.put(start[:2],
Value(f=dist[start[0]][start[1]], g=(goal[2], 'null')))
path[goal[0]][goal[1]] = '*'
for x in range(len(grid)):
for y in range(len(grid[0])):
if ((x, y) != (goal[0], goal[1])):
dist[x][y] = float('inf')
parent[x][y] = 'NULL'
open_set.put((x, y), Value(f=dist[x][y], g=('NULL', 'NULL')))
open_set.put(goal[:2], Value(f=0, g=(goal[2], 'null')))
dist[goal[0]][goal[1]] = 0
while (len(open_set) != 0):
curr_node = open_set.pop()
closed_set.add(curr_node)
if (curr_node[0][:2] != goal[:2]):
path[l[-1][0]][l[-1][1]] = curr_node[1].g[1]
l.append(curr_node[0][:2])
x_current = curr_node[0][0] #row of current node
y_current = curr_node[0][1] #col of current node
o_current = curr_node[1].g[0] #orientation of current node
if (curr_node[0] == start[:2]):
print("get path! ")
break
for i_act, act in enumerate(action):
o_next = (o_current - act) % 4 #orientation of next node
x_next = x_current - forward[o_next][0] #row of next node
y_next = y_current - forward[o_next][1] #col of next node
n_act = action_name[
i_act] #action to get this next node, this cause the one step delay in the display
if (x_next >= 0 and x_next < len(grid) and y_next >= 0
and y_next < len(grid[0])
): #filter the available child (map boundery and barrier)
if (grid[x_next][y_next] == 0):
F_cost = cost[i_act] + dist[x_current][
y_current] #calculate f(n) = g(n)
F_raw = dist[x_next][y_next]
if (((x_next, y_next, o_next) not in closed_set)
and F_cost < F_raw): #child not in closed_set
open_set.get((x_next, y_next)).f = F_cost
open_set.get((x_next, y_next)).g = (o_next, n_act)
dist[x_next][y_next] = F_cost
parent[x_next][y_next] = (x_current, y_current,
o_current)
if (curr_node[0][:2] != start[:2]): print("fail to find the path!")
return path, closed_set, parent
def compute_path(grid, start, goal, cost, heuristic):
# Use the OrderedSet for your closed list
closed_set = OrderedSet()
# Use thePriorityQueue for the open list
open_set = PriorityQueue(order=min, f=lambda v: v.f)
# Keep track of the parent of each node. Since the car can take 4 distinct orientations,
# for each orientation we can store a 2D array indicating the grid cells.
# E.g. parent[0][2][3] will denote the parent when the car is at (2,3) facing up
parent = [[[' ' for row in range(len(grid[0]))]
for col in range(len(grid))],
[[' ' for row in range(len(grid[0]))]
for col in range(len(grid))],
[[' ' for row in range(len(grid[0]))]
for col in range(len(grid))],
[[' ' for row in range(len(grid[0]))]
for col in range(len(grid))]]
# The path of the car
path = [['-' for row in range(len(grid[0]))] for col in range(len(grid))]
x = start[0]
y = start[1]
theta = start[2]
h = heuristic[x][y]
g = 0
f = g + h
open_set.put(start, Value(f=f, g=g))
# your code: implement A*
# I just modified how the walls and traversable cells appear. So that it's easier to see.
# ☐ indicates a wall
# O indicates a traversable path
for i in range(0, len(path)):
for j in range(0, len(path[0])):
if (grid[i][j] == 1):
path[i][j] = '☐'
else:
path[i][j] = 'O'
# Represent goal as *
path[goal[0]][goal[1]] = '*'
# A dictionary that uses a state(grid row, grid col, orientation) as an index to store its parent and its f and g values.
# This helps us keep track of g values as we expand the nodes.
cost_set = {}
cost_set[start] = ([None], 0, 0)
print("Cost: ", cost)
while open_set:
node = open_set.pop()
closed_set.add(node[0])
if node[0] == goal:
break
p = node[0]
children = []
#Iterate over all potential children of a node
for a in action:
x = -1
y = -1
#Calculate new orientation
newOrient = p[2] + a
if (newOrient < 0):
newOrient = 3
elif (newOrient > 3):
newOrient = newOrient % 4
x = p[0] + forward[newOrient][0]
y = p[1] + forward[newOrient][1]
#Only compute if the child is valid and traversable
if (x >= 0 and x < len(grid) and y >= 0 and y < len(grid[0])
and grid[x][y] == 0):
child = (x, y, newOrient)
h = heuristic[child[0]][child[1]]
g = cost_set[p][2] + cost[a + 1]
f = g + h
if (child not in open_set and child not in closed_set):
open_set.put(child, Value(f=f, g=g))
cost_set[child] = (p, f, g)
elif (g < cost_set[child][2]):
open_set.remove(child)
open_set.put(child, Value(f=f, g=g))
cost_set[child] = (p, f, g)
# Starting from the goal, backtrack through the solution the construct the path
path_node = goal
while path_node != start:
curr_node = path_node
path_node = cost_set[path_node][0]
x = curr_node[0] - path_node[0]
y = curr_node[1] - path_node[1]
if ([x, y] in forward):
a = curr_node[2] - path_node[2]
if (abs(a) == 3):
a = -int(a / abs(a))
if (a in action):
path[path_node[0]][path_node[1]] = action_name[a + 1]
# Initially you may want to ignore theta, that is, plan in 2D.
# To do so, set actions=forward, cost = [1, 1, 1, 1], and action_name = ['U', 'L', 'R', 'D']
# Similarly, set parent=[[' ' for row in range(len(grid[0]))] for col in range(len(grid))]
return path, closed_set
def compute_path_djikstra(grid, start, goal, cost, heuristic):
# Use the OrderedSet for your closed list
closed_set = OrderedSet()
# Use thePriorityQueue for the open list
open_set = PriorityQueue(order=min, f=lambda v: v.f)
# Keep track of the parent of each node. Since the car can take 4 distinct orientations,
# for each orientation we can store a 2D array indicating the grid cells.
# E.g. parent[0][2][3] will denote the parent when the car is at (2,3) facing up
parent = [[[' ' for row in range(len(grid[0]))]
for col in range(len(grid))],
[[' ' for row in range(len(grid[0]))]
for col in range(len(grid))],
[[' ' for row in range(len(grid[0]))]
for col in range(len(grid))],
[[' ' for row in range(len(grid[0]))]
for col in range(len(grid))]]
# The path of the car
path = [['-' for row in range(len(grid[0]))] for col in range(len(grid))]
x = start[0]
y = start[1]
theta = start[2]
h = heuristic[x][y]
g = 0
f = g + h
open_set.put(goal, Value(f=g, g=g))
# your code: implement Djikstra
for i in range(0, len(path)):
for j in range(0, len(path[0])):
if (grid[i][j] == 1):
path[i][j] = '☐'
else:
path[i][j] = 'O'
path[goal[0]][goal[1]] = '*'
cost_set = {}
cost_set[goal] = ([None], 0, 0)
print("Cost: ", cost)
# For Djikstra's, we start from the goal, and expand all the nodes and compute the shortest path from each expanded node to the goal.
while open_set:
node = open_set.pop()
closed_set.add(node[0])
c = node[0]
children = []
for a in action:
x = -1
y = -1
orient = c[2]
x = c[0] - forward[orient][0]
y = c[1] - forward[orient][1]
if (x >= 0 and x < len(grid) and y >= 0 and y < len(grid[0])
and grid[x][y] == 0):
preorient = orient - a
if (preorient < 0):
preorient = 3
elif (preorient > 3):
preorient = preorient % 4
parent = (x, y, preorient)
g = cost_set[c][2] + cost[a + 1]
if (parent not in open_set and parent not in closed_set):
open_set.put(parent, Value(f=g, g=g))
cost_set[parent] = (c, g, g)
if (g < cost_set[parent][2]):
open_set.remove(parent)
open_set.put(parent, Value(f=g, g=g))
cost_set[parent] = (c, g, g)
# Logic is similar to A* but here we begin from the start node and compute the path till the goal.
path_node = start
while path_node != goal:
curr_node = path_node
path_node = cost_set[path_node][0]
x = path_node[0] - curr_node[0]
y = path_node[1] - curr_node[1]
if ([x, y] in forward):
a = path_node[2] - curr_node[2]
if (abs(a) == 3):
a = -int(a / abs(a))
if (a in action):
path[curr_node[0]][curr_node[1]] = action_name[a + 1]
# Initially you may want to ignore theta, that is, plan in 2D.
# To do so, set actions=forward, cost = [1, 1, 1, 1], and action_name = ['U', 'L', 'R', 'D']
# Similarly, set parent=[[' ' for row in range(len(grid[0]))] for col in range(len(grid))]
return path, closed_set
def compute_path(grid, start, goal, cost, heuristic):
# Use the OrderedSet for your closed list
closed_set = OrderedSet()
# Use thePriorityQueue for the open list
open_set = PriorityQueue(order=min, f=lambda v: v.f)
# Keep track of the parent of each node. Since the car can take 4 distinct orientations,
# for each orientation we can store a 2D array indicating the grid cells.
# E.g. parent[0][2][3] will denote the parent when the car is at (2,3) facing up
parent = [[[' ' for row in range(len(grid[0]))]
for col in range(len(grid))],
[[' ' for row in range(len(grid[0]))]
for col in range(len(grid))],
[[' ' for row in range(len(grid[0]))]
for col in range(len(grid))],
[[' ' for row in range(len(grid[0]))]
for col in range(len(grid))]]
# creating a node cost list similar in structure to parent list with inital value of 100
# used to determine whether the parent list needs to be updated for a particular child
# if the new cost to reach a child is less than the previous calculated cost
# then parent list and node_cost list will be updated with new found parent node and cost
node_cost = [[[100 for row in range(len(grid[0]))]
for col in range(len(grid))],
[[100 for row in range(len(grid[0]))]
for col in range(len(grid))],
[[100 for row in range(len(grid[0]))]
for col in range(len(grid))],
[[100 for row in range(len(grid[0]))]
for col in range(len(grid))]]
# The path of the car
path = [['-' for row in range(len(grid[0]))] for col in range(len(grid))]
x = start[0]
y = start[1]
theta = start[2]
h = heuristic[x][y]
g = 0
f = g + h
open_set.put(start, Value(f=f, g=g))
# your code: implement A*
while open_set: #code runs in loop as long as there is a node in open_set
current_node, current_value = open_set.pop(
) #current node is minimum from open_set
if current_node == goal: #to stop the code once goal is reached
closed_set.add(current_node) #adding goal to closed_set
while current_node != start: #to back track, from end to start, loop runs till we backtrack to start node
parent_node = parent[current_node[2]][current_node[0]][
current_node[
1]] #accessing parent list to find the parent node of current node
action = move_action(
parent_node, current_node
) #to determine what action was required to move from parent node to current node
path[parent_node[0]][parent_node[1]] = action_name[
action +
1] #updating path list with action required to move from parent to child node
current_node = parent_node #now parent node becomes current node before path identification code runs again
pprint.pprint(path)
return path, closed_set
closed_set.add(current_node) #adding current node to closed_set
neighbours = find_neighbour(
grid, current_node
) #to determine the list of feasible children for current node
for neighbour in neighbours: #running through all feasible childs
actions = move_action(
current_node, neighbour
) #determining action required to move from current node to child node
total_cost_neighbour, step_cost_neighbour = find_value(
actions, current_value, neighbour
) #calculating total & path cost for moving to child node
if neighbour not in open_set or closed_set: # if child is not present in open or closed set then add to open_set
open_set.put(
neighbour,
Value(f=total_cost_neighbour, g=step_cost_neighbour))
# incase child is present in open set then new & old value are compared and if new value is low then it is updated
elif neighbour in open_set:
f_prev = open_set.get(
neighbour) #accessing old value of child from open_set
f_prev_f = f_prev.f
if total_cost_neighbour < f_prev_f: #comparing old and new values
open_set.put(
neighbour,
Value(f=total_cost_neighbour, g=step_cost_neighbour)
) #updating open set if new value is less than old value
#if cost to travel to child is found to be lesser than previously calculated cost for the child then its parent and cost is updated
if node_cost[neighbour[2]][neighbour[0]][
neighbour[1]] > total_cost_neighbour:
parent[neighbour[2]][neighbour[0]][neighbour[
1]] = current_node[0], current_node[1], current_node[
2] #updating parent node
node_cost[neighbour[2]][neighbour[0]][
neighbour[1]] = total_cost_neighbour #updating cost
# Initially you may want to ignore theta, that is, plan in 2D.
# To do so, set actions=forward, cost = [1, 1, 1, 1], and action_name = ['U', 'L', 'R', 'D']
# Similarly, set parent=[[' ' for row in range(len(grid[0]))] for col in range(len(grid))]
return path, closed_set
def __init__(self, **kwargs):
Value.__init__(self, **kwargs)
def compute_path(grid, start, goal, cost, heuristic):
# Use the OrderedSet for your closed list
closed_set = OrderedSet()
# Use thePriorityQueue for the open list
open_set = PriorityQueue(order=min, f=lambda v: v.f)
# Keep track of the parent of each node. Since the car can take 4 distinct orientations,
# for each orientation we can store a 2D array indicating the grid cells.
# E.g. parent[0][2][3] will denote the parent when the car is at (2,3) facing up
parent = [[['' for row in range(len(grid[0]))]
for col in range(len(grid))],
[['' for row in range(len(grid[0]))]
for col in range(len(grid))],
[['' for row in range(len(grid[0]))]
for col in range(len(grid))],
[['' for row in range(len(grid[0]))]
for col in range(len(grid))]]
# The path of the car
path = [['-' for row in range(len(grid[0]))] for col in range(len(grid))]
x = start[0]
y = start[1]
theta = start[2]
path[x][y] = action_name[1]
h = heuristic[x][y]
g = 0
f = g + h
open_set.put(start, Value(f=f, g=g))
closed_set.clear()
# start from here
for child in range(
0, 20
): # A for loop to to run the children and update the open and closed sets.
node = open_set.pop(
) # Initially the open loop contains start and later on the min cost children gets updated.
if node not in open_set or closed_set:
closed_set.add(
node
) # Only if the node is not already present in the closed or open set, it adds.
x = node[0][0] # Assigning the x coordinate of node to x variable.
y = node[0][1] # Assigning the y coordinate of node to y variable.
current_orient = node[0][
2] # Assigning the current orientation of the node to a variable.
orient_f = action[
1] # assigning the action values to new variables for easy understanding
orient_rf = action[0]
orient_lf = action[2]
print('current_orient', current_orient)
print('x', x)
print('y', y)
print('theta', theta)
if current_orient == 1:
path[x][y - 2] = action_name[1]
if current_orient == 0:
path[x][y] = action_name[1]
if current_orient == 3 and theta == 0:
path[x][y - 1] = action_name[0]
if current_orient == 2 and theta == 2:
path[x - 2][y] = action_name[2]
if current_orient == 0: # Moving in North direction
if x - 1 >= 0 and grid[x -
1][y] == 0: # Moving in forward direction
theta = current_orient + orient_f # Adding the orientation value locally based on the global orientation
node = (x - 1, y, theta)
h = heuristic[x - 1][y]
g = cost[1] # Using the cost funtion to the g value.
f = g + h
open_set.put(node, Value(
f=f, g=g)) # Adding the each node to the open set.
# path[x-1][y] = action_name[1]
# if x -1 == 0:
# path[x-1][y] = action_name[0]
if y - 1 >= 0 and grid[x][y - 1] == 0: # Moving in left direction.
theta = orient_lf + current_orient
node = (x, y - 1, theta)
# path[x][y-1] = 'L'
h = heuristic[x][y - 1]
g = cost[2]
f = g + h
open_set.put(node, Value(f=f, g=g))
if y + 1 < 5 and grid[x][y + 1] == 0: # Moving in right direction.
if current_orient == 0:
orient_rf = 3
theta = orient_rf + current_orient
node = (x, y + 1, theta)
# path[x][y + 1] = 'R'
g = cost[0]
h = heuristic[x][y + 1]
f = g + h
open_set.put(node, Value(f=f, g=g))
# print('current_orient = 0')
if current_orient == 1: # Moving in West direction
if x - 1 >= 0 and grid[x - 1][y] == 0: # Moving towards right.
theta = current_orient + orient_rf
node = (x - 1, y, theta)
# path[x-1][y] = 'R'
h = heuristic[x - 1][y]
g = cost[0]
f = g + h
open_set.put(node, Value(f=f, g=g))
if y - 1 >= 0 and grid[x][y -
1] == 0: # Moving in forward direction
theta = orient_f + current_orient
node = (x, y - 1, theta)
# path[x][y - 1] = 'F'
h = heuristic[x][y - 1]
g = cost[1]
f = g + h
open_set.put(node, Value(f=f, g=g))
# path[x][y-1] = action_name[1]
if x + 1 < 5 and grid[x + 1][y] == 0: # Moving in left direction
theta = orient_lf + current_orient
node = (x + 1, y, theta)
# path[x+1][y] = 'L'
h = heuristic[x + 1][y]
g = cost[2]
f = g + h
open_set.put(node, Value(f=f, g=g))
# if node not in open_set or closed_set:
# closed_set.add(node)
# print('current_orient = 1')
if current_orient == 3: # Moving in east direction
if x + 1 < 5 and grid[x + 1][y] == 0: # Moving in right direction
theta = current_orient + orient_rf
current_orient = theta
node = (x + 1, y, theta)
h = heuristic[x + 1][y]
g = cost[0]
f = g + h
if x - 1 >= 0 and grid[x - 1][y] == 0: # Moving in left direction
theta = orient_lf + current_orient
if theta == 4:
theta = 0
current_orient = theta
node = (x - 1, y, theta)
h = heuristic[x - 1][y]
g = cost[2]
f = g + h
open_set.put(node, Value(f=f, g=g))
if y + 1 < 6 and grid[x][y +
1] == 0: # Moving in forward direction
theta = orient_f + current_orient
current_orient = theta
node = (x, y + 1, theta)
h = heuristic[x][y + 1]
g = cost[1]
f = g + h
open_set.put(node, Value(f=f, g=g))
# path[x][y+ 1] = action_name[0]
# path[x][y] = action_name[1]
# print('current_orient = 3')
if current_orient == 2: # Moving in South direction.
if x + 1 < 6 and grid[x +
1][y] == 0: # Moving in forward direction
theta = orient_f + current_orient
node = (x + 1, y, theta)
h = heuristic[x + 1][y]
g = cost[1]
f = g + h
open_set.put(node, Value(f=f, g=g))
# path[x][y] = action_name[1]
if y + 1 < 6 and grid[x][y + 1] == 0: # Moving in left direction
theta = orient_lf + current_orient
node = (x, y + 1, theta)
h = heuristic[x][y + 1]
g = cost[2]
f = g + h
open_set.put(node, Value(f=f, g=g))
if y - 1 >= 0 and grid[x][y -
1] == 0: # Moving in right direction.
theta = orient_rf + current_orient
node = (x, y - 1, theta)
h = heuristic[x][y - 1]
g = cost[0]
f = g + h
open_set.put(node, Value(f=f, g=g))
# print('current_orient = 2')
if node == goal: # If the node is currently at goal, then break the loop by adding that node to the closed set.
open_set.put(node, Value(f=f, g=g))
closed_set.add(node)
path[2][0] = '*'
break
# your code: implement A*
# Initially you may want to ignore theta, that is, plan in 2D.
# To do so, set actions=forward, cost = [1, 1, 1, 1], and action_name = ['U', 'L', 'R', 'D']
# Similarly, set parent=[[' ' for row in range(len(grid[0]))] for col in range(len(grid))]
return path, closed_set
def compute_path(grid, start, goal, cost, heuristic):
# Use the OrderedSet for your closed list
closed_set = OrderedSet()
# Use thePriorityQueue for the open list
open_set = PriorityQueue(order=min, f=lambda v: v.f)
# Keep track of the parent of each node. Since the car can take 4 distinct orientations,
# for each orientation we can store a 2D array indicating the grid cells.
# E.g. parent[0][2][3] will denote the parent when the car is at (2,3) facing up
parent = [[[' ' for row in range(len(grid[0]))]
for col in range(len(grid))],
[[' ' for row in range(len(grid[0]))]
for col in range(len(grid))],
[[' ' for row in range(len(grid[0]))]
for col in range(len(grid))],
[[' ' for row in range(len(grid[0]))]
for col in range(len(grid))]]
# The path of the car
path = [['-' for row in range(len(grid[0]))] for col in range(len(grid))]
x = start[0]
y = start[1]
theta = start[2]
h = heuristic[x][y]
g = 0
f = g + h
# your code: implement A*
open_set.put(start, Value(f=f, g=g))
while open_set.__len__() != 0:
node = open_set.pop()
g = node[1].g
x = node[0][0]
y = node[0][1]
theta = node[0][2]
#if reach the goal, break
if (node[0] == goal):
closed_set.add(node[0])
break
closed_set.add(node[0])
for idx, act in enumerate(action):
new_theta = (theta + act) % 4
#use orientation to determine the position
new_x = x + forward[new_theta][0]
new_y = y + forward[new_theta][1]
#to make sure it won't break the node is naviable
if (new_x < 0 or new_x > 4 or new_y < 0 or new_y > 5
or grid[new_x][new_y] == 1):
continue
new_g = g + cost[idx]
new_h = heuristic[new_x][new_y]
new_f = new_g + new_h
child = [(new_x, new_y, new_theta), Value(f=new_f, g=new_g)]
if (child[0] not in closed_set and child[0] not in open_set):
open_set.put(child[0], child[1])
parent[new_theta][new_x][new_y] = (action_name[idx], node[0])
#if the cost decreased, add it to the openlist
elif (open_set.has(child[0]) and open_set.get(child[0]).g > new_g):
open_set.put(child[0], child[1])
parent[new_theta][new_x][new_y] = (action_name[idx], node[0])
#recording the path in parent
#reach the goal
path[x][y] = '*'
#find out the path by recalling step by step
while (x, y, theta) != start:
pre_step = parent[theta][x][y]
x = pre_step[1][0]
y = pre_step[1][1]
theta = pre_step[1][2]
path[x][y] = pre_step[0]
# Initially you may want to ignore theta, that is, plan in 2D.
# To do so, set actions=forward, cost = [1, 1, 1, 1], and action_name = ['U', 'L', 'R', 'D']
# Similarly, set parent=[[' ' for row in range(len(grid[0]))] for col in range(len(grid))]
return path, closed_set
def compute_path(grid,start,goal,cost,heuristic):
# Use the OrderedSet for your closed list
closed_set = OrderedSet()
# Use thePriorityQueue for the open list
open_set = PriorityQueue(order=min, f=lambda v: v.f)
# Keep track of the parent of each node. Since the car can take 4 distinct orientations,
# for each orientation we can store a 2D array indicating the grid cells.
# E.g. parent[0][2][3] will denote the parent when the car is at (2,3) facing up
parent = [[[' ' for row in range(len(grid[0]))] for col in range(len(grid))],
[[' ' for row in range(len(grid[0]))] for col in range(len(grid))],
[[' ' for row in range(len(grid[0]))] for col in range(len(grid))],
[[' ' for row in range(len(grid[0]))] for col in range(len(grid))]]
# The path of the car
path =[['-' for row in range(len(grid[0]))] for col in range(len(grid))]
dist =[]
P =[]
x = start[0]
y = start[1]
theta = start[2]
h = heuristic[x][y]
g = 0
f = g+h
open_set.put(start, Value(f=f,g=g))
for i in range(0,6):
for j in range(0,5):
if grid[i][j] == 0:
dist = 10000000
x = P(i)
y = P(j
c dfd #cIm just)
open_set.put([i][j], dist)
open_set.insert(goal,0)
while not open_set.empty():
x,y = open_set.pop()
# your code: implement A*
# Initially you may want to ignore theta, that is, plan in 2D.
# To do so, set actions=forward, cost = [1, 1, 1, 1], and action_name = ['U', 'L', 'R', 'D']
# Similarly, set parent=[[' ' for row in range(len(grid[0]))] for col in range(len(grid))]
return path, closed_set
if __name__ == "__main__":
path,closed=compute_path(grid, init, goal, cost, heuristic)
for i in range(len(path)):
print(path[i])
print("\nExpanded Nodes")
for node in closed:
print(node)
"""
def fine_tune(options):
# get print and save functions
Print = get_printer(options)
Save = get_func_on_master(torch.save, options)
# get old_options
model, old_opts = get_old_state(options)
# subsample old dataset
dataset = getattr(datasets, old_opts.dataset)(old_opts).plain_train_set
indices = choose_indices(options, dataset)
loader = torch.utils.data.DataLoader(torch.utils.data.Subset(
dataset, indices),
batch_size=options.batch_size)
full_loader = torch.utils.data.DataLoader(dataset,
batch_size=options.batch_size)
# complete model
num_classes = len(dataset.classes)
intermediate_dim = int((num_classes + old_opts.projection_dim) / 2)
full_model = torch.nn.Sequential(
model, torch.nn.Linear(old_opts.projection_dim, intermediate_dim),
torch.nn.ReLU(inplace=True),
torch.nn.Linear(intermediate_dim, num_classes),
torch.nn.LogSoftmax(dim=1)).to(device=options.cuda_device)
# get loss
criterion = torch.nn.NLLLoss()
optimizer = get_optimizer(full_model, options)
scheduler = get_scheduler(optimizer, options)
# train for num_epochs
full_model.train()
# pretty printer for loss
loss_val = Value(1e6, min, name="loss")
loss_printer = ValuePrinter()
loss_printer.track(loss_val)
timer = AverageMeter()
for epoch in range(options.fine_tune_epochs):
t = time.time()
epoch_loss = AverageMeter()
for data, labels in loader:
Print('.', end='')
optimizer.zero_grad()
out = full_model(data.to(device=options.cuda_device))
loss = criterion(out, labels.to(device=options.cuda_device))
loss.backward()
optimizer.step()
epoch_loss.accumulate(loss.item())
scheduler.step()
loss_val.update(epoch_loss.value())
timer.accumulate(time.time() - t)
Print(
f" ({timer.latest():>6.2f}s) epoch {epoch+1:>3}/{options.fine_tune_epochs:>3}:"
f"{loss_printer.get_formatted_line()}")
Print(
f"Fine tuning: {timer.total():.3f}s {options.fine_tune_epochs} epochs / {timer.value():.3f}s avg"
)
# evaluate on train set once for sanity
full_model.eval()
acc = AverageMeter()
for data, labels in full_loader:
predicts = full_model(data.to(device=options.cuda_device))
predicts = predicts.argmax(dim=1)
labels = labels.to(device=options.cuda_device)
acc.accumulate((predicts == labels).sum().item() / predicts.size(0))
Print(
f"Saving old options, model state, and base path to {options.save_path}"
)
Save(
{
'options': old_opts,
'model_state_dict': model.state_dict(),
'loaded_from': options.load_from.name
}, options.save_path)
Print(acc.value())
return acc.value()
def compute_path(grid, start, goal, cost, heuristic):
# Use the OrderedSet for your closed list
closed_set = OrderedSet()
# Use thePriorityQueue for the open list
open_set = PriorityQueue(order=min, f=lambda v: v.f)
parent = [[[' ' for row in range(len(grid[0]))]
for col in range(len(grid))],
[[' ' for row in range(len(grid[0]))]
for col in range(len(grid))],
[[' ' for row in range(len(grid[0]))]
for col in range(len(grid))],
[[' ' for row in range(len(grid[0]))]
for col in range(len(grid))]]
# The path of the car
path = [['-' for row in range(len(grid[0]))] for col in range(len(grid))]
x = start[0]
y = start[1]
theta = start[2]
g_value = 0
h_value = heuristic[x][y]
f = g_value + h_value
parent_cost = [[[1000000000 for row in range(len(grid[0]))]
for col in range(len(grid))],
[[1000000000 for row in range(len(grid[0]))]
for col in range(len(grid))],
[[1000000000 for row in range(len(grid[0]))]
for col in range(len(grid))],
[[1000000000 for row in range(len(grid[0]))]
for col in range(len(grid))]]
open_set.put(start, Value(f=f, g=g_value))
# Implement of A* algorithm:
while open_set:
node1, cost_new = open_set.pop()
print('The current node is:', node1)
print('---------------------------------')
if node1 == goal:
closed_set.add(node1)
while node1 != start:
node0 = parent[node1[2]][node1[0]][node1[1]]
action = rotate(node0, node1)
node1 = node0
path[node0[0]][node0[1]] = action_name[action + 1]
return path, closed_set
children = children_nodes(grid, node1)
print('The children present are:', children)
print(
'=============================================================================='
)
closed_set.add(node1)
for child in children:
direction = rotate(node1, child)
cost_child, child_new = heuristics_value(cost_new, direction,
child)
if child not in open_set or closed_set:
open_set.put(child, Value(f=cost_child, g=child_new))
if child in open_set:
cost_need = open_set.get(child)
cost_f = cost_need.f
if cost_child < cost_f:
open_set.put(child, Value(f=cost_child, g=child_new))
if parent_cost[child[2]][child[0]][child[1]] > cost_child:
parent_cost[child[2]][child[0]][child[1]] = cost_child
parent[child[2]][child[0]][
child[1]] = node1[0], node1[1], node1[2]
return path, closed_set
def compute_path(grid, start, goal, cost, heuristic):
# Use the OrderedSet for your closed list
closed_set = OrderedSet()
# Use thePriorityQueue for the open list
open_set = PriorityQueue(order=min, f=lambda v: v.f)
# Keep track of the parent of each node. Since the car can take 4 distinct orientations,
# for each orientation we can store a 2D array indicating the grid cells.
# E.g. parent[0][2][3] will denote the parent when the car is at (2,3) facing up
parent = [[[' ' for row in range(len(grid[0]))]
for col in range(len(grid))],
[[' ' for row in range(len(grid[0]))]
for col in range(len(grid))],
[[' ' for row in range(len(grid[0]))]
for col in range(len(grid))],
[[' ' for row in range(len(grid[0]))]
for col in range(len(grid))]]
# The path of the car
path = [['-' for row in range(len(grid[0]))] for col in range(len(grid))]
x = start[0]
y = start[1]
theta = start[2]
h = heuristic[x][y]
g = 0
f = g + h
children = []
# print(start)
open_set.put(start, Value(f=f, g=g))
while open_set:
node, new_cost = open_set.pop()
print('The current node is:', node)
print('==============================')
if node == goal:
return path, closed_set
closed_set.add(node)
row_grid = len(grid) - 1 #4 Boundry of the grids
column_grid = (len(grid[0])) - 1 #5
x1 = node[0] #Parrent x co-ordinate
y1 = node[1] # Parrent y co-ordinate
theta1 = node[2] # Parrent orientation
f1 = new_cost.f #Parent total cost
h = heuristic[x1][y1] #Heuristic of parrent
g1 = new_cost.g #Path cost of parrent
neighbor = []
f12 = []
h12 = []
g12 = []
for i in range(len(action)):
temp_dir = collections.deque(
direction
) # creating a deque tuple # 0 N :::: 1 west 2 south :::: east 3
index = theta1 # orientation of the Parent
child_index = action[i] # possible index of the child
n = -1 * child_index
temp_dir.rotate(n)
child_theta = temp_dir[
theta1] # using the index of the parent to find out the current orientation of the child
""" calculating the distances of children"""
x_pos = node[0] + forward[child_theta][
0] # this gives the position of x,y of the child
y_pos = node[1] + forward[child_theta][1]
pos = (x_pos, y_pos, child_theta)
g1 = []
if (pos[0] > row_grid or pos[1] > (column_grid) or pos[1] < 0
or pos[0] < 0):
continue
elif (grid[pos[0]][pos[1]] == 1):
continue
else:
neighbor.append(pos)
h1 = heuristic[x_pos][
y_pos] # heuristic of neighbor that is dist from the goal
g1 = new_cost.g + cost[i] # path function
print(new_cost.g)
print("---------------------")
print(cost[i])
f1 = g1 + h1 # total cost
f12 = np.append(f12, f1)
g12 = np.append(g12, g1)
h12 = np.append(h12, h1)
gmin = min(g12)
posi = np.argmin(g12)
c = 0
for neigh in neighbor: #Condition for child which is closest to the goal
if neigh not in open_set or neigh not in closed_set:
open_set.put(neigh, Value(f=f12[c], g=g12[c]))
# print(open_set.get(neigh).g)
print(neigh)
print(open_set.get(neigh).g)
elif neigh in open_set and gmin < open_set.get(neigh).g:
open_set.put(neigh, Value(f=f12[c], g=g12[c]))
# print("ggggggggg")
c = c + 1
c = 0
return path, closed_set
def main_worker(gpu, num_gpus_per_node, config, args):
config.defrost()
config.system.gpu = gpu
config.freeze()
if config.system.multiprocessing_distributed and config.system.gpu != 0:
def print_pass(*args):
pass
builtins.print = print_pass
if config.system.gpu is not None:
print("using {} gpu for training".format(config.system.gpu))
# initialize distributed process group
if config.system.distributed:
config.defrost()
if config.system.rank == -1 and config.system.dist_url == "env://":
config.system.rank = int(os.environ["RANK"])
if config.system.multiprocessing_distributed:
config.system.rank = config.system.rank * num_gpus_per_node + int(
config.system.gpu)
dist.init_process_group(backend=config.system.dist_backend,
init_method=config.system.dist_url,
world_size=config.system.world_size,
rank=config.system.rank)
print('creating model : {}'.format(config.model.arch))
model = get_sscl_method(config.method)(config)
# distribute model
if config.system.distributed:
if (config.method == 'simclr'):
nn.SyncBatchNorm.convert_sync_batchnorm(model.encoder)
# issue : need to implement SyncBN for Batchnorm1D for MLP => CVPODS can handle it but it requires PyTorch >= 1.3
elif (config.method == 'byol'):
pass
# nn.SyncBatchNorm.convert_sync_batchnorm(model.online_network)
# nn.SyncBatchNorm.convert_sync_batchnorm(model.target_network)
if config.system.gpu is not None:
torch.cuda.set_device(config.system.gpu)
model.cuda(config.system.gpu)
config.defrost()
config.train.batch_size = int(config.train.batch_size /
num_gpus_per_node)
config.train.num_workers = int(
(config.system.num_workers + num_gpus_per_node - 1) /
num_gpus_per_node)
model = DDP(model, device_ids=[config.system.gpu])
config.freeze()
else:
model.cuda()
model = DDP(model)
raise NotImplementedError
else:
raise NotImplementedError
print(model)
cudnn.benchmark = True
# defining optimizer and scheduler
optimizer = optim.__dict__[config.train.optim](
model.parameters(),
config.train.base_lr,
momentum=config.train.momentum,
weight_decay=config.train.wd)
writer = SummaryWriter(log_dir=config.system.log_dir)
total_step = Value()
# resume
if config.train.resume:
print('resuming train...')
if args.gpu is None:
ckpt = torch.load(config.train.resume)
else:
map_location = "cuda:{}".format(args.gpu)
ckpt = torch.load(config.train.resume, map_location=map_location)
model.load_state_dict(ckpt['model'])
optimizer.load_state_dict(ckpt['optimizer'])
print('load params from {}'.format(config.train.resume))
if 'total_step' in ckpt.keys():
total_step = ckpt['total_step']
# imagenet_100
# defining dataset and data loader
train_dataset = get_dataset(config, mode='train')
if config.system.distributed:
train_sampler = DistributedSampler(train_dataset)
else:
train_sampler = None
train_loader = get_loader(config, train_dataset, train_sampler)
if config.method != 'byol':
criterion = nn.CrossEntropyLoss().cuda(config.system.gpu)
else:
criterion = None
# run train
for epoch in range(config.train.start_epoch, config.train.epochs):
if config.system.distributed:
train_sampler.set_epoch(epoch)
adjust_learning_rate(optimizer, epoch, config)
epoch_start = time.time()
train_one_epoch(train_loader, model, criterion, optimizer, writer,
epoch, total_step, config)
epoch_end = time.time()
print()
print('EPOCH {} train time : {:.2f} min'.format(
epoch, (epoch_end - epoch_start) / 60))
if not config.system.multiprocessing_distributed or (
config.system.multiprocessing_distributed
and config.system.rank % num_gpus_per_node == 0):
if (epoch + 1) % config.system.save_period == 0:
filename = '{}/{}_{:04d}.pth.tar'.format(
config.system.save_dir, config.model.arch, epoch + 1)
save_model(
{
"epoch": epoch + 1,
"optimizer": optimizer.state_dict(),
"model": model.state_dict(),
"arch": config.model.arch,
'total_step': total_step
}, filename)
print(
'--------------------------- model saved at {} ---------------------------'
.format(filename))
print()
else:
print()
filename = '{}/{}_final.pth.tar'.format(config.system.save_dir,
config.model.arch)
save_model(
{
"epoch": epoch + 1,
"optimizer": optimizer.state_dict(),
"model": model.state_dict(),
"arch": config.model.arch,
'total_step': total_step
}, filename)
print(
'############################ final model saved at {} ############################'
.format(filename))
def compute_path(grid,start,goal,cost,heuristic):
global forward
# Use the OrderedSet for your closed list
closed_set = OrderedSet()
# Use thePriorityQueue for the open list
open_set = PriorityQueue(order=min, f=lambda v: v.f)
# Keep track of the parent of each node. Since the car can take 4 distinct orientations,
# for each orientation we can store a 2D array indicating the grid cells.
# E.g. parent[0][2][3] will denote the parent when the car is at (2,3) facing up
parent = [[[' ' for row in range(len(grid[0]))] for col in range(len(grid))],
[[' ' for row in range(len(grid[0]))] for col in range(len(grid))],
[[' ' for row in range(len(grid[0]))] for col in range(len(grid))],
[[' ' for row in range(len(grid[0]))] for col in range(len(grid))]]
# The path of the car
path =[['-' for row in range(len(grid[0]))] for col in range(len(grid))]
x = start[0]
y = start[1]
theta = start[2]
h = heuristic[x][y]
g = 0
f = g+h
open_set.put(start, Value(f=f,g=g))
# your code: implement A*
# Create start and end node
while len(open_set)!=0:
current_node,current_node_add=open_set.pop()
if current_node==goal:
path[current_node[0]][current_node[1]]='*'
closed_set.add(current_node)
print("Goal reached")
while current_node!=start:
parent_node = parent[current_node[2]][current_node[0]][current_node[1]]
or_diff = parent_node[2] - current_node[2]
#print(or_diff,"\t",current_node,"\n")
if or_diff== -3:
or_diff = -1
elif or_diff>0:
or_diff = -1
elif or_diff<0:
or_diff = 1
action_value = action.index(or_diff)
act = action_name[action_value]
path[parent_node[0]][parent_node[1]] = act
current_node = parent_node
break
closed_set.add(current_node)
# Generate children
for index,_ in enumerate(action): # Adjacent squares
theta = current_node[2] + action[index]
if theta==4:
theta=0
if theta==-1:
theta=3
# Get node position
child_node = ( current_node[0]+forward[theta][0], current_node[1]+forward[theta][1],theta)
# Make sure within range
if child_node[0] <= (len(grid)-1) and child_node[0] >= 0 and child_node[1] <= (len(grid[len(grid)-1])-1) and child_node[1] >= 0:
#Make sure walkable terrain
if (grid[child_node[0]][child_node[1]]) == 0:
if child_node in closed_set:
continue
else:
g = current_node_add.g + cost[index]
h = heuristic[child_node[0]][child_node[1]]
g = current_node_add.g + cost[index]
f = g+h
open_set.put(child_node, Value(f=f,g=g))
parent[theta][child_node[0]][child_node[1]] = current_node[0],current_node[1],current_node[2]
"""if child_node not in open_set:
#print(False)
h = heuristic[child_node[0]][child_node[1]]
g = current_node_add.g + cost[index]
f = g+h
open_set.put(child_node, Value(f=f,g=g))
parent[theta][child_node[0]][child_node[1]] = current_node[0],current_node[1],current_node[2]
elif child_node in open_set and g>current_node_add.g:
#print(True)
h = heuristic[child_node[0]][child_node[1]]
g = current_node_add.g + cost[index]
f = g+h
open_set.put(child_node, Value(f=f,g=g))
parent[theta][child_node[0]][child_node[1]] = current_node[0],current_node[1],current_node[2]"""
# Initially you may want to ignore theta, that is, plan in 2D.
# To do so, set actions=forward, cost = [1, 1, 1, 1], and action_name = ['U', 'L', 'R', 'D']
# Similarly, set parent=[[' ' for row in range(len(grid[0]))] for col in range(len(grid))]
return path, closed_set
def compute_path(grid, start, goal, cost, heuristic):
global forward
# Use the OrderedSet for your closed list
closed_set = OrderedSet()
# Use thePriorityQueue for the open list
open_set = PriorityQueue(order=min, f=lambda v: v.f)
# Keep track of the parent of each node. Since the car can take 4 distinct orientations,
# for each orientation we can store a 2D array indicating the grid cells.
# E.g. parent[0][2][3] will denote the parent when the car is at (2,3) facing up
parent = [[[' ' for row in range(len(grid[0]))]
for col in range(len(grid))],
[[' ' for row in range(len(grid[0]))]
for col in range(len(grid))],
[[' ' for row in range(len(grid[0]))]
for col in range(len(grid))],
[[' ' for row in range(len(grid[0]))]
for col in range(len(grid))]]
# The path of the car
path = [['-' for row in range(len(grid[0]))] for col in range(len(grid))]
x = start[0]
y = start[1]
theta = start[2]
h = heuristic[x][y]
g = 0
f = g + h
open_set.put(start, Value(f=f, g=g))
# your code: implement A*
while len(open_set) != 0:
current_node, current_node_add = open_set.pop(
) # This will pop the node and its address with least path cost
if current_node == goal: # Check if the current node has reached the goal
path[current_node[0]][
current_node[1]] = '*' # Make an asterisk mark on the goal
closed_set.add(current_node) # Add the goal node to closed set
print("Goal reached")
while current_node != init: # This while loop will be used to back track the path traversed
parent_node = parent[current_node[2]][current_node[0]][
current_node[1]]
or_diff = parent_node[2] - current_node[
2] # Calculate orientation difference
#print(or_diff,"\t",current_node,"\n")
if or_diff == -3: # This special condition was given because the loop while backtracking turns from south to east which is right direction but it does not give the action corresponding to right
or_diff = -1
elif or_diff > 0: # While backtracking the orientations are flipped, so this flipping operation was performed to get the correct action
or_diff = -1
elif or_diff < 0:
or_diff = 1
action_value = action.index(
or_diff) # Will given the index of the action list
act = action_name[
action_value] # Will give the corresponding action name to the above index
path[parent_node[0]][parent_node[1]] = act
current_node = parent_node # Current node becomes parent node so that the next parent can be traced back
break
closed_set.add(current_node)
# Generate children
for index, _ in enumerate(action): # Adjacent squares
theta = current_node[2] + action[index]
if theta == 4: #If bound exceeded
theta = 0
if theta == -1:
theta = 3
# Get child node position
child_node = (current_node[0] + forward[theta][0],
current_node[1] + forward[theta][1], theta)
# Make sure within range
if child_node[0] <= (
len(grid) -
1) and child_node[0] >= 0 and child_node[1] <= (
len(grid[len(grid) - 1]) - 1) and child_node[1] >= 0:
#Make sure walkable terrain
if (grid[child_node[0]][child_node[1]]) == 0:
if child_node in closed_set:
continue
else:
g = current_node_add.g + cost[index]
if child_node not in open_set:
#print(False)
h = heuristic[child_node[0]][child_node[1]]
g = current_node_add.g + cost[index]
f = g + h
open_set.put(child_node, Value(f=f, g=g))
parent[theta][child_node[0]][
child_node[1]] = current_node[0], current_node[
1], current_node[2]
elif child_node in open_set and g > current_node_add.g:
#print(True)
h = heuristic[child_node[0]][child_node[1]]
g = current_node_add.g + cost[index]
f = g + h
open_set.put(child_node, Value(f=f, g=g))
parent[theta][child_node[0]][
child_node[1]] = current_node[0], current_node[
1], current_node[2]
# Initially you may want to ignore theta, that is, plan in 2D.
# To do so, set actions=forward, cost = [1, 1, 1, 1], and action_name = ['U', 'L', 'R', 'D']
# Similarly, set parent=[[' ' for row in range(len(grid[0]))] for col in range(len(grid))]
return path, closed_set
def __init__(self, sernaDoc, props):
Value.__init__(self, _sernaDoc=sernaDoc, _props=props)
def compute_path(grid, start, goal, cost, heuristic):
# Use the OrderedSet for your closed list
closed_set = OrderedSet()
# Use thePriorityQueue for the open list
open_set = PriorityQueue(order=min, f=lambda v: v.f)
# Keep track of the parent of each node. Since the car can take 4 distinct orientations,
# for each orientation we can store a 2D array indicating the grid cells.
# E.g. parent[0][2][3] will denote the parent when the car is at (2,3) facing up
parent = [[[' ' for row in range(len(grid[0]))]
for col in range(len(grid))],
[[' ' for row in range(len(grid[0]))]
for col in range(len(grid))],
[[' ' for row in range(len(grid[0]))]
for col in range(len(grid))],
[[' ' for row in range(len(grid[0]))]
for col in range(len(grid))]]
# The path of the car
path = [['-' for row in range(len(grid[0]))] for col in range(len(grid))]
x = start[0]
y = start[1]
theta = start[2]
h = heuristic[x][y]
g = 0
f = g + h
open_set.put(start, Value(f=f, g=g))
add = [0, 0]
# your code: implement A*
# Initially you may want to ignore theta, that is, plan in 2D.
# To do so, set actions=forward, cost = [1, 1, 1, 1], and action_name = ['U', 'L', 'R', 'D']
# Similarly, set parent=[[' ' for row in range(len(grid[0]))] for col in range(len(grid))]
while len(open_set) != 0:
(current_node, current_g_val) = open_set.pop()
if current_node == goal: # Exiting loop if current_node is found to be the goal node
closed_set.add(current_node)
path[current_node[0]][current_node[1]] = '*'
break
closed_set.add(current_node)
for j in range(len(action)):
# Computing theta and thereafter computing the corresponding children based on the orientation
theta = current_node[2] + action[j]
# Keeping the theta values in range of the array to avoid indexing errors
if theta >= len(theta_array):
theta = 0
elif theta < 0:
theta = 3
add = forward[theta]
child = (current_node[0] + add[0], current_node[1] + add[1], theta)
# Ensuring that the children in navigable soaces are taken
if 0 <= child[0] < len(grid) and 0 <= child[1] < len(grid[0]):
if grid[child[0]][child[1]] == 0:
h = heuristic[child[0]][child[1]]
g = current_g_val.g + cost[j]
f = g + h
# Implementing the psuedo code provided in the lecture slides to put the correct child in open list and to keep track of their respective parents
if child in closed_set:
continue
else:
if child not in open_set:
open_set.put(child, Value(f=f, g=g))
parent[theta][child[0]][child[1]] = current_node[
0], current_node[1], current_node[2]
elif child in open_set and g > current_g_val.g:
h = h = heuristic[child[0]][child[1]]
f = g + h
open_set.put(child, Value(f=f, g=g))
parent[theta][child[0]][child[1]] = current_node[
0], current_node[1], current_node[2]
# Computing path
while current_node != start:
# Exiting loop if current node is start
if current_node == start:
break
parent_node = parent[current_node[2]][current_node[0]][current_node[1]]
# This tells us the action taken by parent to reach the child
action_type = parent_node[2] - current_node[2]
# This is a special case encountered, when the parent is north facing and child is east facing, so to make it print 'R', this was done
if action_type == -3:
action_type = 1
check = action.index(action_type)
path[parent_node[0]][parent_node[1]] = action_name_flip[check]
current_node = parent_node
# The rest of the conditions keep, the index in range and print the correct path
elif action_type < 0:
action_type = -1
check = action.index(action_type)
path[parent_node[0]][parent_node[1]] = action_name_flip[check]
current_node = parent_node
else:
action_type = parent_node[2] - current_node[2]
check = action.index(action_type)
path[parent_node[0]][parent_node[1]] = action_name_flip[check]
current_node = parent_node
return path, closed_set
