def go_to(self, robot_id, x, y): sign = 1 kd = 7 if ((robot_id == 1) or (robot_id == 2)) else 5 ka = 0.3 tod = 0.005 # tolerance of distance tot = math.pi / 360 # tolerance of theta dx = x - self.cur_posture[robot_id][X] dy = y - self.cur_posture[robot_id][Y] d_e = math.sqrt(math.pow(dx, 2) + math.pow(dy, 2)) desired_th = math.atan2(dy, dx) d_th = helper.wrap_to_pi(desired_th - self.cur_posture[robot_id][TH]) if (d_th > helper.degree2radian(90)): d_th -= math.pi sign = -1 elif (d_th < helper.degree2radian(-90)): d_th += math.pi sign = -1 if (d_e < tod): kd = 0 if (abs(d_th) < tot): ka = 0 if self.go_fast(robot_id): kd *= 5 left_wheel, right_wheel = helper.set_wheel_velocity( self.max_linear_velocity[robot_id], sign * (kd * d_e - ka * d_th), sign * (kd * d_e + ka * d_th)) return left_wheel, right_wheel
def position(self, robot_id, x, y): damping = 0.35 mult_lin = 3.5 mult_ang = 0.4 ka = 0 sign = 1 dx = x - self.cur_my_posture[robot_id][X] dy = y - self.cur_my_posture[robot_id][Y] d_e = math.sqrt(math.pow(dx, 2) + math.pow(dy, 2)) desired_th = (math.pi / 2) if (dx == 0 and dy == 0) else math.atan2(dy, dx) d_th = desired_th - self.cur_my_posture[robot_id][TH] while (d_th > math.pi): d_th -= 2 * math.pi while (d_th < -math.pi): d_th += 2 * math.pi if (d_e > 1): ka = 17 / 90 elif (d_e > 0.5): ka = 19 / 90 elif (d_e > 0.3): ka = 21 / 90 elif (d_e > 0.2): ka = 23 / 90 else: ka = 25 / 90 if (d_th > helper.degree2radian(95)): d_th -= math.pi sign = -1 elif (d_th < helper.degree2radian(-95)): d_th += math.pi sign = -1 if (abs(d_th) > helper.degree2radian(85)): self.set_wheel_velocity(robot_id, -mult_ang * d_th, mult_ang * d_th) else: if (d_e < 5 and abs(d_th) < helper.degree2radian(40)): ka = 0.1 ka *= 4 self.set_wheel_velocity( robot_id, sign * (mult_lin * (1 / (1 + math.exp(-3 * d_e)) - damping) - mult_ang * ka * d_th), sign * (mult_lin * (1 / (1 + math.exp(-3 * d_e)) - damping) + mult_ang * ka * d_th))
def turn_to(self, robot_id, angle): ka = 0.5 tot = math.pi / 360 desired_th = angle d_th = helper.wrap_to_pi(desired_th - self.cur_posture[robot_id][TH]) if (d_th > helper.degree2radian(90)): d_th -= math.pi elif (d_th < helper.degree2radian(-90)): d_th += math.pi if (abs(d_th) < tot): ka = 0 left_wheel, right_wheel = helper.set_wheel_velocity( self.max_linear_velocity[robot_id], -ka * d_th, ka * d_th) return left_wheel, right_wheel
def go_to(self, robot_id, x, y): #로봇이 (x, y)로 이동 sign = 1 kd = 10 if ((robot_id == 1) or (robot_id == 2)) else 5 #거리 편차 위한 상수 ka = 0.4 #각도 편차 위한 상수 tod = 0.005 # tolerance of distance tot = math.pi / 360 # tolerance of theta dx = x - self.cur_posture[robot_id][X] dy = y - self.cur_posture[robot_id][Y] d_e = math.sqrt(math.pow(dx, 2) + math.pow(dy, 2)) #거리 편차 desired_th = math.atan2(dy, dx) d_th = helper.wrap_to_pi(desired_th - self.cur_posture[robot_id][TH]) #각도 편차 if (d_th > helper.degree2radian(90)): d_th -= math.pi sign = -1 elif (d_th < helper.degree2radian(-90)): d_th += math.pi sign = -1 #d_e가 0에 가까울때(목표 지점에 매우 가까울때) go_to함수는 로봇을 거의 멈추다시피함. 이때 go_fast사용 #if (d_e < tod): # kd = 0 #if (abs(d_th) < tot): # ka = 0 #if self.go_fast(robot_id): # kd *= 5 left_wheel, right_wheel = helper.set_wheel_velocity( self.max_linear_velocity[robot_id], sign * (kd * d_e - ka * d_th), sign * (kd * d_e + ka * d_th)) #left_wheel = left_wheel * 1.2 #right_wheel = right_wheel * 1.2 return left_wheel, right_wheel #오른쪽, 왼쪽 바퀴 속도 반환
# ####### Parameters ######### eps = 0.05 min_pts = 10 file_name = "_dataSet.txt" # file name of Test Data, should be in the same folder of program plot_origin = 0 # plot origin graph or clustered graph, 1 = origin, else = clustered sieve_angle = 90 # Data which intersection angle with mean vector larger than that will be removed # ############################ reload(dbs) reload(plt) reload(hp) test_data = np.mat(hp.loadDataSet(file_name)) if plot_origin == 1: test_data_show = hp.degree2radian(test_data, 0) plt.createOrigin(test_data_show.A) else: # get and transfer Data test_data_radian = hp.degree2radian(test_data, -1) test_data_vector = hp.orientation2vector(test_data_radian) # run dbscan Algorithm print("******************* start dbscan ********************") print("") cluster_result, noise_result, k = dbs.dbscan(test_data_vector, eps, min_pts) # transfer result result_data = np.mat(np.zeros((cluster_result.shape[0], 3))) result_data[:, 0:2] = hp.vector2orientation(cluster_result[:, 0:3])
calc_round = 1 # round of running k-means Algorithm to get the best result file_name = "_dataSet.txt" # file name of Test Data, should be in the same folder of program plot_origin = 0 # plot origin graph or clustered graph, 1 = origin, else = clustered rand_method = 0 # method of create initial centroids, 0 = remotest centroids, 1 = random centroids sieve_angle = 90 # Data which intersection angle with mean vector larger than that will be removed show_ini_centroids = False # whether to show initial random centroids # ############################ reload(km) reload(plt) reload(hp) test_data = np.mat(hp.loadDataSet(file_name)) if plot_origin == 1: test_data_show = hp.degree2radian(test_data, 0) plt.createOrigin(test_data_show.A) else: # get and transfer Data test_data_radian = hp.degree2radian(test_data, -1) test_data_vector = hp.orientation2vector(test_data_radian) # run k-means Algorithm best_ini_centroids = km.randCenter(test_data_vector, k) best_centroids = km.randCenter(test_data_vector, k) best_cluster = np.zeros((test_data_vector.shape[0], 2)) min_mean_error = np.inf print("") for i in range(calc_round): print("************ round ", i + 1, " of calculating ************") print("")
# ####### Parameters ######### k = 3 # number of cluster file_name = "_dataSet.txt" # file name of Test Data, should be in the same folder of program plot_origin = 0 # plot origin graph or clustered graph, 1 = origin, else = clustered sieve_angle = 35 # Data which intersection angle with mean vector larger than that will be removed # ############################ reload(ag) reload(plt) reload(hp) test_data = np.mat(hp.loadDataSet(file_name)) if plot_origin == 1: test_data_show = hp.degree2radian(test_data, 0) plt.createOrigin(test_data_show.A) else: # get and transfer data test_data_radian = hp.degree2radian(test_data, -1) test_data_vector = hp.orientation2vector(test_data_radian) # run gnes algorithm print("*************** calculating started ***************") print("") cluster_centroids, cluster_list = ag.agnes(test_data_vector, k, dist_calc=ag.dist_min) print("") print("*************** calculating finished ***************")