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