def get_blocking_position(self, world):
# Calculate blocking position
possession = world.robot_in_possession
if not possession:
error(
"Attempting to find blocking position while no robot in possession"
)
return Vector(0, 0, 0, 0)
assert (not possession.is_our_team)
target = None
if world.in_our_half(possession):
info("Blocking our goal which is at ({0}, {1})".format(
world.our_goal.x, world.our_goal.y))
target = world.our_goal
else:
# Use their other robot as target
info("Blocking opponent pass")
target = ([r for r in world.their_robots if r != possession])[0]
if (possession.x - target.x) == 0:
error("Robot in possession in line on x!")
return Vector(0, 0, 0, 0)
# m = (possession.y - target.y) / (possession.x - target.x)
# return Vector(self.x, possession.y + m * (self.x - possession.x), 0, 0)
return Vector((target.x + possession.x) / 2,
(target.y + possession.y) / 2, 0, 0)
"""dx = target.x - possession.x
def defender_get_rotation_to_catch_point(robot_vec, ball_vec, catch_distance):
'''
Finds a relative angle by which the defending robot needs to rotate to face the catching point.
Catching point is the location where the robot is facing the ball and so can grab
the ball easily.
There are usually two grabbing point from each side of the ball, so the 'is_ball_between_angles'
method is used to find the one that is suitable for taking the ball.
Also, it tries to approach the ball from the left side and the righ side from the kicker and
return the rotation that is shorter.
positive angle - clockwise rotation
negative angle - counter-clockwise rotation
'''
# Two vectors from the left and the right side of movement
robot_vec_left = Vector(robot_vec.x, robot_vec.y, robot_vec.angle - math.pi / 2, 0)
robot_vec_right = Vector(robot_vec.x, robot_vec.y, robot_vec.angle + math.pi / 2, 0)
# calculates catching point angles for each side of movement
first_front_angle, first_theta, first_alpha = defender_get_rotation_to_catch_point_helper(robot_vec_left, ball_vec, catch_distance, "left")
second_front_angle, second_theta, second_alpha = defender_get_rotation_to_catch_point_helper(robot_vec_right, ball_vec, catch_distance, "right")
# gets the best rotation angle for each side if movement
first_relative_angle = is_ball_between_angles(first_front_angle, robot_vec.angle, first_theta, first_alpha)
second_relative_angle = is_ball_between_angles(second_front_angle, robot_vec.angle, second_theta, second_alpha)
# returns the rotation angle that is shorter
if abs(first_relative_angle) < abs(second_relative_angle):
logging.debug("Returning first:" + str(first_relative_angle) + " in degrees " + str(math.degrees(first_relative_angle)))
# logging.info(">>>>>>>>> LEFT")
return (first_relative_angle, "left")
else:
logging.debug("Returning second:" + str(second_relative_angle) + " in degrees " + str(math.degrees(second_relative_angle)))
# logging.info(">>>>>>>>> RIGHT")
return (second_relative_angle, "right")
def perform(self, comms):
# get robot
robots = filter(lambda r: not r.is_missing(), self.world.their_robots)
robots.sort(key=lambda r: math.hypot(r.x - self.robot.x, r.y - self.robot.y))
if len(robots) == 0:
logging.error("There is no enemy here. Gimme someone to destroy!")
return 1
robot = robots[0]
# Find our goal
goal = Vector(0, 225, 0, 0)
if self.robot.x > 300:
goal = Vector(600, 225, 0, 0)
# get the point
robots = self.world.their_robots
y_diff = goal.y - robot.y
x_diff = goal.x - robot.x
ratio = (self.world.our_defender.x - robot.x) / x_diff
y_mean = robot.y + (y_diff * ratio)
distance = utils.defender_distance_on_y(self.world.our_defender.vector, y_mean)
print("DISTANCE: " + str(distance))
logging.info("Wants to move by: " + str(distance))
comms.move(distance)
return utils.defender_move_delay(distance)
class GoToStaticBall(Action):
'''
Move defender to the ball when static
'''
preconditions = [(lambda w, r: abs(
utils.defender_get_rotation_to_catch_point(
Vector(r.x, r.y, r.angle, 0), Vector(w.ball.x, w.ball.y, 0, 0), r.
catch_distance)[0]) < ROTATION_THRESHOLD,
"Defender is facing ball"),
(lambda w, r: r.catcher == 'OPEN', "Grabbers are open."),
(lambda w, r: utils.ball_is_static(w), "Ball is static.")]
def perform(self, comms):
# reset dynamic threshold
# TODO: has to be restarted everywhere!
global ROTATION_THRESHOLD
ROTATION_THRESHOLD = 0.1
dx = self.world.ball.x - self.robot.x
dy = self.world.ball.y - self.robot.y
displacement = math.hypot(dx, dy)
if displacement == 0:
alpha = 0
else:
# TODO: has to handle this edge case better
try:
alpha = math.degrees(
math.asin(self.robot.catch_distance / float(displacement)))
except ValueError as er:
print(er)
x = self.world.ball.x
y = self.world.ball.y
angle = utils.get_rotation_to_point(self.robot.vector,
Vector(x, y, 0, 0))
logging.info("Wants to close-rotate by: " + str(angle))
comms.turn(angle)
return 1 # TODO: NO FIXED DELAY
beta = 90 - alpha
if math.sin(math.radians(alpha)) == 0:
distance_to_move = 15 # something is obviously wrong so we have to move a bit
else:
distance_to_move = math.sin(
math.radians(beta)) * self.robot.catch_distance / math.sin(
math.radians(alpha))
# Find the movement side
angle, side = utils.defender_get_rotation_to_catch_point(
Vector(self.robot.x, self.robot.y, self.robot.angle, 0),
Vector(self.world.ball.x, self.world.ball.y, 0, 0),
self.robot.catch_distance)
if side == "left":
distance_to_move = -distance_to_move
logging.info("Wants to move by: " + str(distance_to_move))
comms.move(distance_to_move)
return utils.defender_move_delay(distance_to_move)
def test3(self):
initial_state = {
'task': 'move-grab',
'max_steps': 2,
'our_defender': Vector(250, 220, math.radians(200), 0),
'our_attacker': None,
'ball': Vector(200, 170, 0, 0),
}
self.run(initial_state, "Test 3")
def testm3_1(self):
initial_state = {
'task': 'm31',
'max_steps': 2,
'our_defender': Vector(400, 300, math.radians(90), 0),
'our_attacker': Vector(50, 200, math.radians(45), 0),
'ball': Vector(100, 200, 0, 0),
}
self.run(initial_state, "Test M3 - Receiving a pass")
def defender_rotation_to_defend_point(robot_vec, ball_vec, center, center_radius, side):
def defender_get_defend_point(robot_vec, ball_vec, center, center_radius, side):
dx = ball_vec.x - center.x
dy = ball_vec.y - center.y
alpha = math.atan2(dx, dy) % (math.pi * 2)
# lovely for ifs to make your life easier reader
offset = math.radians(20)
if side == "left":
if alpha < offset:
alpha = offset
elif alpha > math.radians(180) - offset:
alpha = math.radians(180) - offset
else:
if alpha < math.radians(180) + offset:
alpha = math.radians(180) + offset
elif alpha > math.radians(360) - offset:
alpha = math.radians(360) - offset
x = center.x + center_radius * math.sin(alpha)
y = center.y + center_radius * math.cos(alpha)
return Vector(x, y, 0, 0)
defend_point = defender_get_defend_point(robot_vec, ball_vec, center, center_radius, side)
global DEFEND_POINT
DEFEND_POINT = Vector(defend_point.x, 470 - defend_point.y, 0, 0)
angle, side = defender_get_rotation_to_catch_point(robot_vec, defend_point, 1)
return (angle, side, defend_point)
def perform(self, comms):
global ROTATION_THRESHOLD
logging.info('---- ' + str(ROTATION_THRESHOLD))
x = self.world.ball.x
y = self.world.ball.y
angle = utils.defender_get_rotation_to_catch_point(
Vector(self.robot.x, self.robot.y, self.robot.angle, 0),
Vector(x, y, 0, 0), self.robot.catch_distance)[0]
logging.info("Wants to rotate by: " + str(angle))
comms.turn(angle)
if ROTATION_THRESHOLD < 0.30:
ROTATION_THRESHOLD += 0.07
return utils.defender_turn_delay(angle)
def get_defence_point(world):
if world.our_side == 'left':
x = 100
else:
x = world.pitch.width - 100
y = world.pitch.height / 2
return Vector(x, y, 0, 0)
def check_object(vec3):
dx = vec2.x - vec1.x
if dx == 0:
dx = SMALL_VALUE
m_straight = float(vec2.y - vec1.y) / dx
if m_straight == 0:
m_straight = SMALL_VALUE
c_straight = vec1.y - vec1.x * m_straight
m_perp = float(-1) / m_straight
c_perp = vec3.y - vec3.x * m_perp
dm = m_straight - m_perp
if dm == 0:
dm = SMALL_VALUE
ix = -(c_straight - c_perp) / dm
iy = m_straight * ix + c_straight
intercept = Vector(ix, iy, 0, 0)
if dist(vec1, intercept) > dist(vec1, vec2):
return None
distance_from_line = dist(intercept, vec3)
if distance_from_line < AVOID_DISTANCE:
return dist(vec1, intercept)
else:
return None
def perform(self, comms):
rot_angle = utils.defender_get_rotation_to_catch_point(
Vector(self.robot.x, self.robot.y, self.robot.angle, 0),
utils.get_defence_point(self.world), 0)[0]
logging.info("Facing direction to move to defence area. Rotating %f degrees.", math.degrees(rot_angle))
comms.turn(rot_angle)
return utils.defender_turn_delay(rot_angle)
def perform(self, comms):
# reset dynamic threshold
# TODO: has to be restarted everywhere!
global ROTATION_THRESHOLD
ROTATION_THRESHOLD = 0.1
dx = self.world.ball.x - self.robot.x
dy = self.world.ball.y - self.robot.y
displacement = math.hypot(dx, dy)
if displacement == 0:
alpha = 0
else:
# TODO: has to handle this edge case better
try:
alpha = math.degrees(
math.asin(self.robot.catch_distance / float(displacement)))
except ValueError as er:
print(er)
x = self.world.ball.x
y = self.world.ball.y
angle = utils.get_rotation_to_point(self.robot.vector,
Vector(x, y, 0, 0))
logging.info("Wants to close-rotate by: " + str(angle))
comms.turn(angle)
return 1 # TODO: NO FIXED DELAY
beta = 90 - alpha
if math.sin(math.radians(alpha)) == 0:
distance_to_move = 15 # something is obviously wrong so we have to move a bit
else:
distance_to_move = math.sin(
math.radians(beta)) * self.robot.catch_distance / math.sin(
math.radians(alpha))
# Find the movement side
angle, side = utils.defender_get_rotation_to_catch_point(
Vector(self.robot.x, self.robot.y, self.robot.angle, 0),
Vector(self.world.ball.x, self.world.ball.y, 0, 0),
self.robot.catch_distance)
if side == "left":
distance_to_move = -distance_to_move
logging.info("Wants to move by: " + str(distance_to_move))
comms.move(distance_to_move)
return utils.defender_move_delay(distance_to_move)
def translate_position(self, obj, v):
deltax = v.x - (self._pitch.width / 2)
deltay = v.y - (self._pitch.height / 2)
factor = (self._camera_height - obj.height) / self._camera_height
deltax *= factor
deltay *= factor
return Vector(deltax + self._pitch.width / 2,
deltay + self._pitch.height / 2, v.angle, v.velocity)
def perform(self, comms):
x = self.world.ball.x
y = self.world.ball.y
angle = utils.get_rotation_to_point(self.robot.vector, Vector(x, y, 0, 0))
logging.info("Wants to rotate by: " + str(angle))
comms.turn(angle)
return utils.defender_turn_delay(angle)
def attacker_can_score_from_position(world, position):
assert(position.angle == 0)
goal_points = [Vector(world.their_goal.x, world.their_goal.y + (world.their_goal.width / 4) * i, 0, 0) for i in [1, 2, 3]]
for point in goal_points:
obstacle = find_obstacle(world, position, point, exclude=[world.our_attacker])
if not obstacle:
return True
return False
def defender_distance_on_y(robot_vec, y_value):
'''
Calculates the moving distance on y axis.
'''
distance = abs(y_value - robot_vec.y)
target_point = Vector(robot_vec.x, y_value, 0, 0)
direction = get_movement_direction_from_vector(robot_vec, target_point)
return (distance + 2) * direction
class KickToScoreZone(Action):
'''
Pass ball to our attacker's score zone
'''
preconditions = [(lambda w, r: abs(r=utils.defender_get_rotation_to_catch_point(Vector(r.x, r.y, r.angle, 0)[0], Vector(w.score_zone.x, w.score_zone.y, 0, 0), r.catch_distance)) < FACING_ROTATION_THRESHOLD,
"Defender is facing attacker's score zone"),
(lambda w, r: r.has_ball(w.ball), "Defender has ball")]
def perform(self, comms):
raise NotImplementedError
def vector(self, new_vector):
if new_vector is None: # or not isinstance(new_vector, Vector):
raise ValueError(
'The new vector can not be None and must be an instance of a Vector'
)
else:
self._vector = Vector(new_vector.x, new_vector.y,
new_vector.angle - self._angle_offset,
new_vector.velocity)
self._is_missing = False
class MoveToDefenceArea(Action):
preconditions = [(lambda w, r: abs(utils.defender_get_rotation_to_catch_point(Vector(r.x, r.y, r.angle, 0), utils.get_defence_point(w), 0)[0]) < ROTATION_THRESHOLD, "Aligned to move to defence point.")]
def perform(self, comms):
dist = utils.dist(self.robot, utils.get_defence_point(self.world))
dist *= utils.get_movement_direction_from_vector(self.robot, utils.get_defence_point(self.world))
logging.info("Moving to defence area. Moving %f right.", dist)
comms.move(dist)
return utils.defender_move_delay(abs(dist))
def defender_follow_ball_distance(robot_vec, ball_vec):
robot_vec_left = Vector(robot_vec.x, robot_vec.y, robot_vec.angle - math.pi / 2, 0)
alpha_left = get_rotation_to_point(robot_vec_left, ball_vec)
robot_vec_right = Vector(robot_vec.x, robot_vec.y, robot_vec.angle + math.pi / 2, 0)
alpha_right = get_rotation_to_point(robot_vec_right, ball_vec)
# Find the closes angle (side to move)
side = -1
alpha = alpha_left
if alpha_left > alpha_right:
side = 1
alpha = alpha_right
dx = ball_vec.x - robot_vec.x
dy = ball_vec.y - robot_vec.y
hypotenus = math.hypot(dx, dy)
distance_to_move = math.cos(alpha) * hypotenus * side
return distance_to_move
def defender_angle_to_pass_upfield(world, defender_robot, enemy_zone_radius=40):
"""
Calculates the angle for defender to rotate by to pass to attacker or, if not possible,
angle to kick upfield which is not contested by enemy robots.
:param world: World object
:param defender_robot: The robot who is trying to kick
:param enemy_zone_radius: The radius of the circle around enemy robots in which the ball should not pass
:return: The calculated angle
"""
def can_pass_to_attacker(defender_vec, attacker_vec, their_robots_vecs):
can_pass = True
defender_attacker_line = ((defender_vec.x, defender_vec.y), (attacker_vec.x, attacker_vec.y))
for t in their_robots_vecs:
if line_intersects_circle(defender_attacker_line, ((t.x, t.y), enemy_zone_radius)):
can_pass = False
break
return can_pass
aim_vector = world.our_attacker.vector
if world.our_attacker.is_missing():
aim_vector = world.their_goal.vector
their_vecs = [t.vector for t in world.their_robots if not t.is_missing()]
if can_pass_to_attacker(defender_robot.vector, aim_vector, their_vecs):
return get_rotation_to_point(defender_robot.vector, aim_vector)
else:
# TODO possibly improve this to be less brute force
x = aim_vector.x
num_of_iterations = 100
step = world.pitch.height / num_of_iterations
# Upper range
for y in range(world.pitch.height / 2, world.pitch.height, step):
new_vec = Vector(x, y, 0, 0)
# Could skip by enemy_zone_radius but can't work out a way to update loop variable in python
if can_pass_to_attacker(defender_robot.vector, new_vec, their_vecs):
return get_rotation_to_point(defender_robot.vector, new_vec)
# Lower range
for y in range(world.pitch.height / 2, 0, -step):
new_vec = Vector(x, y, 0, 0)
# Could skip by enemy_zone_radius but can't work out a way to update loop variable in python
if can_pass_to_attacker(defender_robot.vector, new_vec, their_vecs):
return get_rotation_to_point(defender_robot.vector, new_vec)
# Could not find an angle, choose a random one within the cone between the defender and their corners
top_corner = Vector(0, 0, 0, 0)
bottom_corner = Vector(0, world.pitch.height, 0, 0)
if world.our_side == 'left':
top_corner = Vector(world.pitch.width, 0, 0, 0)
bottom_corner = Vector(world.pitch.width, world.pitch.height, 0, 0)
top_angle = get_rotation_to_point(defender_robot.vector, top_corner)
bottom_angle = get_rotation_to_point(defender_robot.vector, bottom_corner)
return random.uniform(min(top_angle, bottom_angle), max(top_angle, bottom_angle))
def get_avoiding_angle_to_point(world, vec1, vec2):
straight_angle = get_rotation_to_point(vec1, vec2)
# straight_distance = dist(vec1, vec2)
# straight_vector = Vector(vec1.x, vec1.y, vec1.angle, 0)
obstacle = find_obstacle(world, vec1, vec2)
if not obstacle:
return straight_angle
logging.info("Obstacle on path")
m_straight = float(vec2.y - vec1.y) / ((vec2.x - vec1.x) + 1)
c_straight = vec1.y - vec1.x * m_straight
m_perp = float(-1) / m_straight
c_perp = obstacle.y - obstacle.x * m_perp
ix = -(c_straight - c_perp) / ((m_straight - m_perp) + 1)
iy = m_straight * ix + c_straight
intercept = Vector(ix, iy, 0, 0)
obstacle_from_line = dist(intercept, obstacle) + 1
delta_length = AVOID_DISTANCE / 2
dx = (obstacle.x - ix) / obstacle_from_line * delta_length
dy = (obstacle.y - iy) / obstacle_from_line * delta_length
closest = None
for j in range(-10, 10):
v = Vector(ix + dx * j, iy + dy * j, 0, 0)
if not find_obstacle(world, vec1, v) and world.is_possible_position(world.our_attacker, v.x, v.y):
angle = get_rotation_to_point(vec1, v)
if not closest or abs(angle - straight_angle) < abs(closest - straight_angle):
closest = angle
if not closest:
logging.info("Didn't find any suitable angle!!!")
else:
logging.info("Found suitable angle {0}".format(closest))
return closest
def get_movement_direction_from_vector(robot_vec, point_vec):
'''
Returns either
1 - move right; or
-1 - move left
'''
robot_vec_left = Vector(robot_vec.x, robot_vec.y, robot_vec.angle - math.pi / 2, 0)
# point_vec = Vector(point[0], point[1], 0, 0)
rotation_for_left = get_rotation_to_point(robot_vec_left, point_vec)
# rotation_for_right = get_rotation_to_point(robot_vec_right, direction_vec)
if abs(math.degrees(rotation_for_left)) < 90:
return -1
else:
return 1
def __init__(self,
x,
y,
angle,
velocity,
width,
length,
height,
angle_offset=0):
if width < 0 or length < 0:
raise ValueError('Object dimensions must be positive')
else:
self._width = width
self._length = length
self._height = height
self._angle_offset = angle_offset
self._vector = Vector(x, y, angle, velocity)
self._is_missing = True
def get_new_score_zone(self):
halfway = self.pitch.centre_line_x
x = halfway + (self.pitch.goal_box_x['right'] - halfway) / 2 if self._our_side == 'left'\
else halfway - (halfway - self.pitch.goal_box_x['left']) / 2
y_step = (self.pitch.top - self.pitch.bottom) / 5
centroids = [Vector(x, y_step * i, 0, 0) for i in range(1, 5)]
filtered_centroids = filter(
lambda v: utils.defender_can_pass_to_position(self, v) and utils.
attacker_can_score_from_position(self, v) and self.
is_possible_position(self.our_attacker, v.x, v.y), centroids)
sorted_centroids = sorted(filtered_centroids,
key=lambda v: (v.x)**2 + (v.y)**2,
reverse=True)
if not sorted_centroids:
self._score_zone = None
else:
self._score_zone = sorted_centroids[0]
info("Found score zone at {0}".format(self.score_zone))
return self.score_zone
def defender_scan_angle_to_pass_absolute(world, defender_vec, enemy_zone_radius=40):
'''
Calculates the angle for defender to rotate by to pass to attacker or, if not possible,
angle to kick upfield which is not contested by enemy robots.
'''
target_vec = world.our_attacker.vector
if world.our_attacker.is_missing():
target_vec = world.their_goal.vector
possible_angles = defender_scan_area_to_pass(world, defender_vec, enemy_zone_radius)
# offset
offset = 0
if world.our_side == 'right':
offset = 180
# find degree of our attacker
zero_vec = Vector(defender_vec.x, defender_vec.y, 0 + offset, 0)
friend_degree = int(math.degrees(get_rotation_to_point(zero_vec, target_vec)))
# find the closest degree possible
low = 0
for n in xrange(friend_degree, -1, -1):
if possible_angles[n]:
low = n
break
hi = 180
for n in xrange(friend_degree, 180):
if possible_angles[n]:
hi = n
break
print("low: " + str(low))
print("hi: " + str(hi))
if abs(friend_degree - low) < abs(friend_degree - hi):
print("Return by: " + str(math.radians(low + offset - math.degrees(defender_vec.angle))))
return math.radians(low + offset - math.degrees(defender_vec.angle))
else:
print("Return by: " + str(math.radians(low + offset - math.degrees(defender_vec.angle))))
return math.radians(hi + offset - math.degrees(defender_vec.angle))
def defender_get_defend_point(robot_vec, ball_vec, center, center_radius, side):
dx = ball_vec.x - center.x
dy = ball_vec.y - center.y
alpha = math.atan2(dx, dy) % (math.pi * 2)
# lovely for ifs to make your life easier reader
offset = math.radians(20)
if side == "left":
if alpha < offset:
alpha = offset
elif alpha > math.radians(180) - offset:
alpha = math.radians(180) - offset
else:
if alpha < math.radians(180) + offset:
alpha = math.radians(180) + offset
elif alpha > math.radians(360) - offset:
alpha = math.radians(360) - offset
x = center.x + center_radius * math.sin(alpha)
y = center.y + center_radius * math.cos(alpha)
return Vector(x, y, 0, 0)
def rotation_precondition(self, w, r):
possessing = w.robot_in_posession
rotation = utils.defender_get_rotation_to_catch_point(Vector(r.x, r.y, r.angle, 0), Vector(possessing.x, possessing.y, 0, 0), r.catch_distance)[0]
return abs(rotation) < FACING_ROTATION_THRESHOLD
def defender_move_vec(vec):
return Vector(vec.x, vec.y, vec.angle + pi / 2, vec.velocity)
def __init__(self, x, y, angle=0):
self.vector = Vector(x, y, angle, 0)