def generate_new_goal(self, pose=np.zeros((1, 3)), other_poses=np.zeros((1, 3))): # nans = np.load(open('E:/ETSI/Proyecto/data/Databases/numpy_files/nans.npy', 'rb')) # smapz = np.zeros((1500, 1000)) # max_mapz = None # c_max = 0.0 if self.acq_mod == "split_path": if len(self.splitted_goals) > 0: new_pos = self.splitted_goals[0, :] self.splitted_goals = self.splitted_goals[1:, :] return np.append(new_pos, 0) xi = 1.0 _, reg = calc_voronoi(pose, other_poses, self.map_data) all_acq = [] c_max = 0.0 new_pos = None sum_all_acq = None if self.acquisition == "predictive_entropy_search": gps = self.surrogate(self.vector_pos, return_std=True) sum_sigmas = None for _, sigma in gps: sum_sigmas = sigma if sum_sigmas is None else sigma + sum_sigmas x_star = self.vector_pos[np.where( sum_sigmas == np.max(sum_sigmas))[0][0]] for i in range(len(self.sensors)): mu, sigma = gps[i] all_acq = predictive_entropy_search(self.vector_pos, mu, sigma, model=self.gps[list( self.sensors)[i]], x_star=x_star) if self.acq_fusion == "decoupled": arr1inds = all_acq.argsort() sorted_arr1 = self.vector_pos[arr1inds[::-1]] best_pos, idx = find_vect_pos4region(sorted_arr1, reg, return_idx=True) if all_acq[arr1inds[::-1][idx]] > c_max: new_pos = best_pos c_max = all_acq[arr1inds[::-1][idx]] elif self.acq_fusion == "coupled": sum_all_acq = sum_all_acq + all_acq if sum_all_acq is not None else all_acq else: for key in self.sensors: if self.acquisition == "gaussian_sei": all_acq = gaussian_sei(self.vector_pos, self.gps[key], np.min(self.train_targets[key]), c_point=pose[:2], xi=xi, masked=self.acq_mod == "masked") elif self.acquisition == "maxvalue_entropy_search": all_acq = maxvalue_entropy_search( self.vector_pos, self.gps[key], np.min(self.train_targets[key]), c_point=pose[:2], xi=xi, masked=self.acq_mod == "masked") elif self.acquisition == "gaussian_pi": all_acq = gaussian_pi(self.vector_pos, self.gps[key], np.min(self.train_targets[key]), c_point=pose[:2], xi=xi, masked=self.acq_mod == "masked") elif self.acquisition == "gaussian_ei": all_acq = gaussian_ei( self.vector_pos, # self.gps[key], self.surrogate(keys=[key], return_std=True)[0], np.min(self.train_targets[key]), c_point=pose[:2], xi=xi, masked=self.acq_mod == "masked") elif self.acquisition == "max_std": all_acq = max_std(self.vector_pos, self.gps[key], np.min(self.train_targets[key]), masked=self.acq_mod == "masked") if self.acq_fusion == "decoupled": arr1inds = all_acq.argsort() sorted_arr1 = self.vector_pos[arr1inds[::-1]] best_pos, idx = find_vect_pos4region(sorted_arr1, reg, return_idx=True) if all_acq[arr1inds[::-1][idx]] > c_max: new_pos = best_pos c_max = all_acq[arr1inds[::-1][idx]] elif self.acq_fusion == "coupled": sum_all_acq = sum_all_acq + all_acq if sum_all_acq is not None else all_acq # mapz = gaussian_ei(self.all_vector_pos, self.gps[key], np.min(self.train_targets[key]), # c_point=pose[:2], # xi=xi, # masked=self.acq_mod == "masked").reshape((1000, 1500)).T # smapz += mapz # for nnan in nans: # mapz[nnan[0], nnan[1]] = -1 # mapz = np.ma.array(mapz, mask=(mapz == -1)) # if key == "s1": # plt.subplot(231) # elif key == "s2": # plt.subplot(232) # else: # plt.subplot(233) # plt.imshow(mapz, origin='lower', cmap=cmo.cm.matter_r) # plt.title("$AF_{}(x)$".format(str("{" + key + "}"))) # plt.plot(new_pos[0], new_pos[1], 'r.') # for pm in self.train_inputs: # plt.plot(pm[0], pm[1], 'y^') # plt.plot(pose[0], pose[1], 'b^') # plt.colorbar() # if max_mapz is None or all_acq[arr1inds[::-1][idx]] > c_max: # max_mapz = mapz # c_max = all_acq[arr1inds[::-1][idx]] # plt.subplot(235) # for nnan in nans: # smapz[nnan[0], nnan[1]] = -1 # smapz = np.ma.array(smapz, mask=(smapz == -1)) # plt.imshow(smapz, origin='lower', cmap=cmo.cm.matter_r) # plt.title("$\sum AF_i(x)$") # maxx = np.where(smapz == np.max(smapz)) # plt.plot(maxx[1][0], maxx[0][0], 'r.') # plt.plot(pose[0], pose[1], 'b^', zorder=9) # for pm in self.train_inputs: # plt.plot(pm[0], pm[1], 'y^') # plt.colorbar() # plt.legend(["best_next", "c_pose", "prev. measurements"], bbox_to_anchor=(3.5, 1.0), fancybox=True, # shadow=True) # plt.subplot(234) # plt.imshow(max_mapz, origin='lower', cmap=cmo.cm.matter_r) # for pm in self.train_inputs: # plt.plot(pm[0], pm[1], 'y^') # plt.plot(pose[0], pose[1], 'b^') # plt.title("$max(AF_i(x))$") # maxx = np.where(max_mapz == np.max(max_mapz)) # plt.plot(maxx[1][0], maxx[0][0], 'r.') # plt.colorbar() # plt.show(block=True) # if self.acq_fusion == "maxcoupled": # for best_pos in new_poses: # suma = 0 # for key in self.sensors: # this_acq = gaussian_ei(best_pos[0], # self.surrogate(best_pos[0].reshape(1, -1), return_std=True, keys=[key])[0], # np.min(self.train_targets[key]), # c_point=pose[:2], xi=xi, masked=self.acq_mod == "masked") # suma += this_acq # if suma > c_max: # new_pos = best_pos[0] # c_max = suma if self.acq_fusion == "coupled": arr1inds = sum_all_acq.argsort() sorted_arr1 = self.vector_pos[arr1inds[::-1]] best_pos = find_vect_pos4region(sorted_arr1, reg, return_idx=False) new_pos = best_pos if self.acq_mod == "split_path" or self.acq_mod == "truncated": beacons_splitted = [] vect_dist = np.subtract(new_pos, pose[:2]) ang = np.arctan2(vect_dist[1], vect_dist[0]) # d = 50 d = np.exp( np.min([ self.gps[key].kernel_.theta[0] for key in list(self.sensors) ])) * self.proportion for di in np.arange(0, np.linalg.norm(vect_dist), d)[1:]: mini_goal = np.array( [di * np.cos(ang) + pose[0], di * np.sin(ang) + pose[1]]).astype(np.int) if self.map_data[mini_goal[1], mini_goal[0]] == 0: beacons_splitted.append(mini_goal) beacons_splitted.append(np.array(new_pos)) self.splitted_goals = np.array(beacons_splitted) new_pos = self.splitted_goals[0, :] self.splitted_goals = self.splitted_goals[1:, :] new_pos = np.append(new_pos, 0) return new_pos
def generate_sensor_image(self): self.db.updating_sensors = True raw_data = self.db.sensors_df.loc[self.db.sensors_df['type'] == 's1'].to_numpy() last_index = len(raw_data) raw_data = raw_data[self.db.sensors_c_index:, 1:6] # if self.db.sensors_c_index == 0: # new_data = [[row[:2], row[2]] for row in raw_data] # self.coordinator.initialize_data_gpr(new_data) # if self.ui.actionPredicci_n_GP.isChecked(): # self.axes[0].plot(new_data[0][0][0], new_data[0][0][1], 'Dy', markersize=12, # label="Initial Information", zorder=6) # self.axes[0].plot(new_data[1][0][0], new_data[1][0][1], 'Dy', markersize=12, zorder=6) # self.axes[0].plot(new_data[2][0][0], new_data[2][0][1], '^y', markersize=12, zorder=6, # label="Previous Positions") # if self.ui.actionIncertidumbre_GP.isChecked(): # self.axes[1].plot(new_data[0][0][0], new_data[0][0][1], 'Dy', markersize=12, # label="Initial Information", zorder=6) # self.axes[1].plot(new_data[1][0][0], new_data[1][0][1], 'Dy', markersize=12, zorder=6) # self.axes[1].plot(new_data[2][0][0], new_data[2][0][1], '^y', markersize=12, # label="Previous Positions", zorder=6) # if self.ui.actionFuncion_de_Adquisici_n.isChecked(): # self.axes[2].plot(new_data[0][0][0], new_data[0][0][1], 'Dy', markersize=12, zorder=6) # self.axes[2].plot(new_data[1][0][0], new_data[1][0][1], 'Dy', markersize=12, zorder=6) # self.axes[2].plot(new_data[2][0][0], new_data[2][0][1], '^y', markersize=12, zorder=6) # else: for data in raw_data: if self.db.sensors_c_index > 0: poses = self.db.drones_df.to_numpy() _, reg = calc_voronoi(data[:2], [ np.array(read[:2]) for read in poses if read[2] != data[4] ], self.data[0]) else: _, reg = calc_voronoi(data[:2], [ np.array(read[:2]) for read in raw_data if read[4] != data[4] ], self.data[0]) reg = np.vstack([reg, reg[0, :]]) if self.colors[int(data[4])] in self.voronoi_ax.keys(): for line in self.voronoi_ax[self.colors[int(data[4])]]: line.pop(0).remove() self.voronoi_ax[self.colors[int(data[4])]] = [] if self.ui.actionMapa.isChecked(): # self.axes[0].plot(data[0], data[1], '^{}'.format(drone_color), markersize=12, # zorder=6, alpha=0.7, label="Drone: {}".format(int(data[4]))) self.axes[0].plot(data[0], data[1], '^', color=self.colors[int(data[4])], markersize=12, zorder=6, alpha=0.7, label="Drone: {}".format(int(data[4]))) self.voronoi_ax[self.colors[int(data[4])]].append( self.axes[0].plot(reg[:, 0], reg[:, 1], '-', color=self.colors[int(data[4])], zorder=9, lw=3)) # self.axes[0].legend(loc="lower left", prop={"size": 20}) if self.ui.actionPredicci_n_GP.isChecked(): self.axes[1].plot(data[0], data[1], '^', color=self.colors[int(data[4])], markersize=12, zorder=6, alpha=0.7) self.voronoi_ax[self.colors[int(data[4])]].append( self.axes[1].plot(reg[:, 0], reg[:, 1], '-', color=self.colors[int(data[4])], zorder=9)) if self.ui.actionIncertidumbre_GP.isChecked(): self.axes[2].plot(data[0], data[1], '^', color=self.colors[int(data[4])], markersize=12, zorder=6, alpha=0.7) self.voronoi_ax[self.colors[int(data[4])]].append( self.axes[2].plot(reg[:, 0], reg[:, 1], '-', color=self.colors[int(data[4])], zorder=9)) # if self.ui.actionFuncion_de_Adquisici_n.isChecked(): # self.axes[3].plot(data[0], data[1], '^', color=self.colors[int(data[4])], markersize=12, # zorder=6, alpha=0.7) # self.voronoi_ax[self.colors[int(data[4])]].append( # self.axes[3].plot(reg[:, 0], reg[:, 1], '-', color=self.colors[int(data[4])], zorder=9)) # print(data) self.coordinator.add_data({"pos": data[:2], "s1": data[2]}) print(raw_data, data[:2], data[2]) self.coordinator.fit_data() self.db.sensors_c_index = last_index # print(raw_data) self.current_drone_pos = raw_data[-1][0:3] observe_maps = dict() sensor_name = "s1" if self.ui.actionPredicci_n_GP.isChecked( ) or self.ui.actionIncertidumbre_GP.isChecked(): if self.ui.actionIncertidumbre_GP.isChecked(): result = self.coordinator.surrogate(return_std=True, keys=["s1"]) mu, std = result[0][0], result[0][1] else: mu = self.coordinator.surrogate(return_std=False) if self.ui.actionPredicci_n_GP.isChecked(): observe_maps["{} gp".format(sensor_name)] = mu # observe_maps["{} gp".format(sensor_name)] = (mu - self.real_map) ** 2 if self.ui.actionIncertidumbre_GP.isChecked(): observe_maps["{} gp un".format(sensor_name)] = std # if self.ui.actionFuncion_de_Adquisici_n.isChecked(): # acq, sensor_name = self.coordinator.get_acq(self.current_drone_pos, self.acq_func) # observe_maps["{} acq".format(sensor_name)] = acq self.observe_maps(observe_maps) self.db.updating_sensors = False if self.auto_BO: self.send_request()
def generate_new_goal(self, pose=np.zeros((1, 3)), idx=-1, other_poses=np.zeros((1, 3))): if self.acq_mod == "split_path": if len(self.splitted_goals) > 0: new_pos = self.splitted_goals[0, :] self.splitted_goals = self.splitted_goals[1:, :] # print('new pos is', new_pos) return np.append(new_pos, 0) xi = 1.0 _, reg = calc_voronoi(pose, other_poses, self.map_data) if self.acquisition == "gaussian_sei": all_acq = gaussian_sei(self.vector_pos, self.gp, np.min(self.data[1]), c_point=pose[:2], xi=xi, masked=self.acq_mod == "masked") elif self.acquisition == "maxvalue_entropy_search": all_acq = maxvalue_entropy_search(self.vector_pos, self.gp, np.min(self.data[1]), c_point=pose[:2], xi=xi, masked=self.acq_mod == "masked") elif self.acquisition == "gaussian_pi": all_acq = gaussian_pi(self.vector_pos, self.gp, np.min(self.data[1]), c_point=pose[:2], xi=xi, masked=self.acq_mod == "masked") elif self.acquisition == "gaussian_ei": all_acq = gaussian_ei(self.vector_pos, self.gp, np.min(self.data[1]), c_point=pose[:2], xi=xi, masked=self.acq_mod == "masked") elif self.acquisition == "max_std": all_acq = max_std(self.vector_pos, self.gp, np.min(self.data[1]), masked=self.acq_mod == "masked") # all_acq = gaussian_ei(self.vector_pos, self.gp, np.min(self.data[1]), xi=xi, return_grad=False) # print('no, cvp isnt ', self.vector_pos) # curr_vect_pos = find_vect_pos4region(self.point_v_pos, reg) # print('yes, cvp is ', curr_vect_pos) # print('no, cvp isnt ', self.vector_pos) optimize_method = 'maximum' if optimize_method == "maximum": arr1inds = all_acq.argsort() sorted_arr1 = self.vector_pos[arr1inds[::-1]] new_pos = find_vect_pos4region(sorted_arr1, reg) elif optimize_method == "cvt": from time import time t0 = time() new_pos = find_cvt_pos4region(all_acq, self.vector_pos, reg) # print("ne: ", new_pos) # print("t0: ", time() - t0) # t0 = time() # arr1inds = all_acq.argsort() # sorted_arr1 = self.vector_pos[arr1inds[::-1]] # new_pos = find_vect_pos4region(sorted_arr1, reg) # print("ne: ", new_pos) # print("t1: ", time() - t0) # print('---') # print(self.vector_pos[np.where(all_acq == np.nanmax(all_acq))][0]) # print(new_pos) # print('---') if self.acq_mod == "split_path" or self.acq_mod == "truncated": beacons_splitted = [] # print('new pos', new_pos) # print('old pos', pose) vect_dist = np.subtract(new_pos, pose[:2]) # print('vd', vect_dist) ang = np.arctan2(vect_dist[1], vect_dist[0]) # print('ang', ang) d = np.exp(self.gp.kernel_.theta)[0] / 2 # d = 220 # print(d) # print('x', d * np.cos(ang), vect_dist[0]) # print('y', d * np.sin(ang), vect_dist[1]) # if d * np.cos(ang) < vect_dist[0]: # lx = np.round(np.arange(pose[0], new_pos[0], d * np.cos(ang))[1:]).astype(np.int) # else: # lx = [] # if d * np.sin(ang) < vect_dist[1]: # ly = np.round(np.arange(pose[1], new_pos[1], d * np.sin(ang))[1:]).astype(np.int) # else: # ly = [] # for dx, dy in zip(lx, ly): # if self.map_data[dy, dx] == 0: # beacons_splitted.append(np.array([dx, dy])) # print(beacons_splitted) # beacons_splitted = [] # print(np.linalg.norm(vect_dist)) # print('di', np.arange(0, np.linalg.norm(vect_dist), d)[1:]) for di in np.arange(0, np.linalg.norm(vect_dist), d)[1:]: mini_goal = np.array([di * np.cos(ang) + pose[0], di * np.sin(ang) + pose[1]]).astype(np.int) if self.map_data[mini_goal[1], mini_goal[0]] == 0: beacons_splitted.append(mini_goal) # print(beacons_splitted) beacons_splitted.append(np.array(new_pos)) self.splitted_goals = np.array(beacons_splitted) new_pos = self.splitted_goals[0, :] # if idx == 5: # plt.plot(np.append(pose[0], self.splitted_goals[:, 0]), np.append(pose[1], self.splitted_goals[:, 1]), # '-*b') # plt.plot([pose[0], new_pos[0]], [pose[1], new_pos[1]], '-*b') self.splitted_goals = self.splitted_goals[1:, :] # if idx == 0: # all_acq, all_grad = gaussian_ei(self.all_vector_pos, self.gp, np.min(self.data[1]), xi=xi, # return_grad=True) # all_acq = gaussian_sei(self.vector_pos, self.gp, np.max(self.data[1]), c_point=pose[:2], # masked=self.acq_mod == "masked") # best_acqs = self.vector_pos[np.where(all_acq == np.nanmax(all_acq))] # new_pos = best_acqs[np.random.choice(np.arange(0, len(best_acqs)))] # # self.acq_mod = "masked" # # all_acq = gaussian_sei(self.vector_pos, self.gp, np.max(self.data[1]), c_point=pose[:2], # masked=self.acq_mod == "masked") # best_acqs = self.vector_pos[np.where(all_acq == np.nanmax(all_acq))] # aux_pos = best_acqs[np.random.choice(np.arange(0, len(best_acqs)))] # plt.plot(pose[0], pose[1], '^r', markersize=12, label="Current Position") # # # beacons_splitted = [] # # vect_dist = np.subtract(new_pos, pose[:2]) # # ang = np.arctan2(vect_dist[1], vect_dist[0]) # # lx = np.round(np.arange(pose[0], new_pos[0], 150 * np.cos(ang))[1:]).astype(np.int) # # ly = np.round(np.arange(pose[1], new_pos[1], 150 * np.sin(ang))[1:]).astype(np.int) # # for dx, dy in zip(lx, ly): # # if self.map_data[dy, dx] == 0: # # beacons_splitted.append(np.array([dx, dy])) # # beacons_splitted.append(np.array(new_pos)) # # asdf = np.array(beacons_splitted) # # plt.plot(asdf[:, 0], asdf[:, 1], 'Xb', markersize=12, label="Next positions") # # plt.plot(aux_pos[0], aux_pos[1], 'Xb', markersize=12, label="Next Position") # # plt.plot(new_pos[0], new_pos[1], 'Xk', markersize=12, alpha=0.2, label="Optimal Position") # # extent = [0, 999, 0, 1499] # img, _ = self.get_acq([pose], "gaussian_ei") # img = img.reshape((1000, 1000)).T # # plt.imshow(all_grad.reshape((1000, 1500)), origin='lower', cmap='YlGn_r', extent=extent) # plt.show(block=True) # from scipy.spatial.distance import cdist # # cmap = plt.cm.gray # # my_cmap = cmap(np.arange(cmap.N)) # # my_cmap[:, -1] = np.linspace(0.8, 0, cmap.N) # # my_cmap = ListedColormap(my_cmap) # img = np.exp(-cdist([pose[:2]], self.all_vector_pos) / 250).reshape((1000, 1000)).T # # for nnan in nans: # # img[nnan[0], nnan[1]] = -1 # plt.imshow(img, origin='lower', cmap="gray") # # plt.legend(loc="upper right", prop={"size": 20}) # cbar = plt.colorbar(orientation='vertical') # cbar.ax.tick_params(labelsize=20) # plt.show(block=True) # plt.plot(new_pos[0], new_pos[1], '^y', markersize=12) new_pos = np.append(new_pos, 0) # print('new pos is', new_pos) return new_pos
def generate_new_goal(self, pose=np.zeros((1, 3)), other_poses=np.zeros((1, 3))): global reg_poly if self.acq_mod == "split_path": if len(self.splitted_goals) > 0: new_pos = self.splitted_goals[0, :] self.splitted_goals = self.splitted_goals[1:, :] return np.append(new_pos, 0) _, reg = calc_voronoi(pose, other_poses, self.map_data) reg_poly = Polygon(reg) res = minimize(self.problem, self.algorithm, ("n_gen", 150), verbose=False) prev_min_dist = 10000 new_pos = pose[:2] for point in res.X: curr_dist = np.linalg.norm(np.subtract(point, pose[:2])) if curr_dist < prev_min_dist: new_pos = np.round(point).astype(np.int) prev_min_dist = curr_dist if True and len(self.train_inputs) > 1: # from pymoo.visualization.scatter import Scatter # plot = Scatter() # plot.add(res.F, color="red") # plot.show() plt.style.use("seaborn") # results = res.F # img_gen = [ # [0, 1], # [0, 2] # ] # for s in img_gen: # f1 = results[:, s[0]] # f4 = results[:, s[1]] # plt.figure() # plt.title("Objective Space", fontsize=20) # plt.plot(f1, f4, 'ob', label=f"Pareto Set: $\\alpha_{s[0] + 1}(x) \\;,\\; \\alpha_{s[1] + 1}(x)$") # plt.xlabel(f"$\\alpha_{s[0] + 1}(x)$", fontsize=20) # plt.ylabel(f"$\\alpha_{s[1] + 1}(x)$", fontsize=20) # plt.xticks(fontsize=20) # plt.yticks(fontsize=20) # plt.legend(loc='lower left', prop={'size': 17}, fancybox=True, shadow=True, frameon=True) xticks = np.arange(0, 1000, 200) yticks = np.arange(0, 1500, 200) xnticks = [str(format(num * 10, ',')) for num in xticks] ynticks = [str(format(num * 10, ',')) for num in yticks] surr = self.surrogate(keys=["s1"])[0] # mapz = np.full_like(self.map_data, np.nan) # for pos in enumerate(self.vector_pos): # mapz[pos[1][1], pos[1][0]] = surr[pos[0]] # mapz = np.power(np.subtract(mapz, lol), 2) plt.imshow(self.map_data, cmap="BuGn_r", origin='lower', zorder=8) # CS = plt.contour(mapz, colors='k', # alpha=0.6, linewidths=1.0, zorder=9) plt.grid(True, zorder=0, color="white") plt.gca().set_facecolor('#eaeaf2') # plt.clabel(CS, inline=1, fontsize=10) plt.xlabel("x (m)", fontsize=20) plt.ylabel("y (m)", fontsize=20) plt.xticks(xticks, labels=xnticks, fontsize=20) plt.yticks(yticks, labels=ynticks, fontsize=20) k = True # for point in self.train_inputs: # if k: # plt.plot(point[0], point[1], 'yo', zorder=10, label='Previous Locations') # k = False # else: # plt.plot(point[0], point[1], 'yo', zorder=10) plt.plot(pose[0], pose[1], 'ro', zorder=10, label='Current Location') # k = True # for pareto in res.X: # pareto = np.round(pareto).astype(np.int) # if self.map_data[pareto[1], pareto[0]] == 0: # if k: # plt.plot(pareto[0], pareto[1], 'Xb', zorder=10, label="Pareto Points") # k = False # else: # plt.plot(pareto[0], pareto[1], 'Xb', zorder=10) # plt.plot(new_pos[0], new_pos[1], 'Xg', zorder=10, label='Closest Pareto Point') plt.plot(np.append(reg[:, 0], reg[0, 0]), np.append(reg[:, 1], reg[0, 1]), '-b', zorder=10) # plt.show(block=True) if self.acq_mod == "split_path" or self.acq_mod == "truncated": beacons_splitted = [] vect_dist = np.subtract(new_pos, pose[:2]) ang = np.arctan2(vect_dist[1], vect_dist[0]) d = np.exp( np.min([ self.gps[key].kernel_.theta[0] for key in list(self.sensors) ])) * self.proportion for di in np.arange(0, np.linalg.norm(vect_dist), d)[1:]: mini_goal = np.array( [di * np.cos(ang) + pose[0], di * np.sin(ang) + pose[1]]).astype(np.int) if self.map_data[mini_goal[1], mini_goal[0]] == 0: beacons_splitted.append(mini_goal) beacons_splitted.append(np.array(new_pos)) self.splitted_goals = np.array(beacons_splitted) new_pos = self.splitted_goals[0, :] self.splitted_goals = self.splitted_goals[1:, :] if len(self.train_inputs) > 1: # plt.plot(new_pos[0], new_pos[1], 'Xk', zorder=10, label='Next Meas. Location') plt.legend(loc='lower left', prop={'size': 17}, fancybox=True, shadow=True, frameon=True) plt.show(block=True) new_pos = np.append(new_pos, 0) return new_pos