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 turn_to(self, robot_id, x, y):
ka = 0.2
tot = math.pi / 360
dx = x - self.cur_posture[robot_id][X]
dy = y - self.cur_posture[robot_id][Y]
desired_th = math.atan2(dy, dx)
d_th = helper.wrap_to_pi(desired_th - self.cur_posture[robot_id][TH])
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, 0, 0, 0, 1]
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 #오른쪽, 왼쪽 바퀴 속도 반환
def move(self, id, idx_d, idx_a, cur_posture, cur_posture_opp, previous_ball, cur_ball, predicted_ball):
# speed list: left wheel, right wheel, front slider, bottom slider
speeds = [0,0,0,0]
# default desired position
x_default = -self.field[X] / 2 + self.robot_size / 2 + 0.05
y_default = max(min(cur_ball[Y], (self.goal[Y] / 2 - self.robot_size / 2)),
-self.goal[Y] / 2 + self.robot_size / 2)
# if the robot is inside the goal, try to get out
if (cur_posture[id][X] < -self.field[X] / 2):
if (cur_posture[id][Y] < 0):
x = x_default
y = cur_posture[id][Y] + 0.2
else:
x = x_default
y = cur_posture[id][Y] - 0.2
# if the goalkeeper is outside the penalty area
elif (not helper.in_penalty_area(self, cur_posture[id], 0)):
# return to the desired position
x = x_default
y = y_default
# if the goalkeeper is inside the penalty area
else:
# if the ball is inside the penalty area
if (helper.in_penalty_area(self, cur_ball, 0)):
# if the ball is behind the goalkeeper
if (cur_ball[X] < cur_posture[id][X]):
# if the ball is not blocking the goalkeeper's path
if (abs(cur_ball[Y] - cur_posture[id][Y]) > 2 * self.robot_size):
# try to get ahead of the ball
x = cur_ball[X] - self.robot_size
y = cur_posture[id][Y]
else:
# just give up and try not to make a suicidal goal
speeds[0], speeds[1] = helper.turn_angle(self, id, math.pi /2, cur_posture)
return speeds
# if the ball is ahead of the goalkeeper
else:
desired_th = helper.direction_angle(self, id, cur_ball[X], cur_ball[Y], cur_posture)
rad_diff = helper.wrap_to_pi(desired_th - cur_posture[id][TH])
# if the robot direction is too away from the ball direction
if (rad_diff > math.pi / 3):
# give up kicking the ball and block the goalpost
x = x_default
y = y_default
else:
# try to kick the ball away from the goal
x = cur_ball[X]
y = cur_ball[Y]
if cur_posture[id][BALL_POSSESSION]:
speeds[2] = 10
# if the ball is not in the penalty area
else:
# if the ball is within alert range and y position is not too different
if cur_ball[X] < -self.field[X] / 2 + 1.5 * self.penalty_area[X] and \
abs(cur_ball[Y]) < 1.5 * self.penalty_area[Y] / 2 and \
abs(cur_ball[Y] - cur_posture[id][Y]) < 0.2:
speeds[0], speeds[1] = helper.turn_to(self, id, cur_ball[X], cur_ball[Y], cur_posture)
return speeds
# otherwise
else:
x = x_default
y = y_default
speeds[0], speeds[1] = helper.go_to(self, id, x, y, cur_posture, cur_ball)
return speeds
def move(self,
robot_id,
idx,
idx_opp,
defense_angle,
attack_angle,
cur_posture,
cur_posture_opp,
prev_posture,
previous_ball,
cur_ball,
predicted_ball,
cross=False,
shoot=False,
quickpass=False,
jump=False,
dribble=False):
self.action.update_state(cur_posture, prev_posture, cur_ball,
previous_ball)
self.flag = 0
protection_radius = self.goal_area[Y] / 2 - 0.1
angle = defense_angle
protection_x = math.cos(angle) * protection_radius - self.field[X] / 2
protection_y = math.sin(angle) * protection_radius
if helper.ball_is_own_goal(predicted_ball, self.field, self.goal_area):
# if ball inside goal area: DANGER
x = protection_x
y = protection_y
elif helper.ball_is_own_penalty(predicted_ball, self.field,
self.penalty_area):
# if the ball is behind the goalkeeper
if (cur_ball[X] < cur_posture[robot_id][X]):
# if the ball is not blocking the goalkeeper's path
if (abs(cur_ball[Y] - cur_posture[robot_id][Y]) >
2 * self.robot_size):
# try to get ahead of the ball
x = cur_ball[X] - self.robot_size
y = cur_posture[robot_id][Y]
else:
# just give up and try not to make a suicidal goal
left_wheel, right_wheel = self.action.turn_to(
robot_id, math.pi / 2)
kick_speed, kick_angle = self.action.kick(
cross, shoot, quickpass)
jump_speed = self.action.jump(jump)
dribble_mode = self.action.dribble(dribble)
return left_wheel, right_wheel, kick_speed, kick_angle, jump_speed, dribble_mode
# if the ball is ahead of the goalkeeper
else:
desired_th = helper.direction_angle(self, robot_id,
cur_ball[X], cur_ball[Y],
cur_posture)
rad_diff = helper.wrap_to_pi(desired_th -
cur_posture[robot_id][TH])
# if the robot direction is too away from the ball direction
if (rad_diff > math.pi / 3):
# give up kicking the ball and block the goalpost
x = -self.field[X] / 2 + self.robot_size / 2 + 0.05
y = max(
min(cur_ball[Y],
(self.goal[Y] / 2 - self.robot_size / 2)),
-self.goal[Y] / 2 + self.robot_size / 2)
else:
if cur_ball[Z] > 0.2:
# try to jump in the ball direction
x = cur_ball[X]
y = cur_ball[Y]
jump = True
else:
# try to kick the ball away from the goal
x = cur_ball[X]
y = cur_ball[Y]
#슛을 F2에게로
try:
self.action.shoot_to(robot_id, cur_posture[5][X],
cur_posture[5][Y])
self.flag = 1
except:
shoot = True
self.flag = 2
else:
# if ball outside penalty area, protect against kicks
x = protection_x
y = protection_y
left_wheel, right_wheel = self.action.go_to(robot_id, x, y)
kick_speed, kick_angle = self.action.kick(cross, shoot, quickpass)
jump_speed = self.action.jump(jump)
dribble_mode = self.action.dribble(dribble)
return left_wheel, right_wheel, kick_speed, kick_angle, jump_speed, dribble_mode