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 ***************")
