def execute_charge(self):
self.robot.disable_avoid_ball()
main.debug_drawer().draw_line(
robocup.Line(self.robot.pos, self.aim_target_point),
constants.Colors.White, "LineKickOld")
main.debug_drawer().draw_line(
robocup.Line(main.ball().pos, self.aim_target_point),
constants.Colors.White, "LineKickOld")
# drive directly into the ball
ball2target = (self.aim_target_point - main.ball().pos).normalized()
robot2ball = (main.ball().pos - self.robot.pos).normalized()
speed = min(self.robot.vel.mag() + LineKickOld.AccelBias,
self.MaxChargeSpeed)
self.robot.set_world_vel(robot2ball.normalized() * speed)
self.robot.face(self.aim_target_point)
if main.ball().pos.dist_to(
self.robot.pos) < LineKickOld.ClosenessThreshold:
self._got_close = True
if self.robot.has_ball():
if self.use_chipper:
self.robot.chip(self.chip_power)
else:
self.robot.kick(self.kick_power)
def in_shot_triangle(self, best_point, alt_point):
# get the two points of the enemies goal
goalSegment = constants.Field.TheirGoalSegment
right_post = goalSegment.get_pt(0)
left_post = goalSegment.get_pt(1)
# draw the line from the ideal passing position to the goal corners
main.system_state().draw_line(robocup.Line(right_post, best_point),
(255, 0, 255), "Shot Range")
main.system_state().draw_line(robocup.Line(left_post, best_point),
(255, 0, 255), "Shot Range")
# angle between the line from ideal pass point and the goal corner and between the line ideal pass point and other goal corner
shot_angle = (right_post - best_point).angle_between(
(left_post - best_point))
# angle between the line from ideal pass point and goal corner 1 and between the line from ideal pass point and 2nd best pass point
left_post_alt_pos_angle = (best_point -
alt_point).angle_between(best_point -
right_post)
# angle between the line from ideal pass point and goal corner 2 and between the line from ideal pass point and 2nd best pass point
right_post_alt_pos_angle = (best_point -
alt_point).angle_between(best_point -
left_post)
# decides which side is closer to point by the smaller theta
if left_post_alt_pos_angle < right_post_alt_pos_angle:
self.closest_post = left_post
else:
self.closest_post = right_post
# if interior angles near sum to 0 then robot is inside the the zone.
return abs(shot_angle - left_post_alt_pos_angle -
right_post_alt_pos_angle) < self.epsilon
def recalculate(self):
# can't do squat if we don't know what we're supposed to do
if self.receive_point == None or self.robot == None:
return
ball = main.ball()
if self.ball_kicked:
# when the ball's in motion, the line is based on the ball's velocity
self._pass_line = robocup.Line(ball.pos, ball.pos + ball.vel * 10)
else:
# if the ball hasn't been kicked yet, we assume it's going to go through the receive point
self._pass_line = robocup.Line(ball.pos, self.receive_point)
target_angle_rad = (ball.pos - self.robot.pos).angle()
angle_rad = self.robot.angle
self._angle_error = target_angle_rad - angle_rad
if self.ball_kicked:
actual_receive_point = self._pass_line.nearest_point(
self.robot.pos)
else:
actual_receive_point = self.receive_point
pass_line_dir = (self._pass_line.get_pt(1) -
self._pass_line.get_pt(0)).normalized()
self._target_pos = actual_receive_point + pass_line_dir * constants.Robot.Radius
# vector pointing down the pass line toward the kicker
pass_dir = (self._pass_line.get_pt(0) -
self._pass_line.get_pt(1)).normalized()
pos_error = self._target_pos - self.robot.pos
self._x_error = pos_error.dot(pass_dir.perp_ccw())
self._y_error = pos_error.dot(pass_dir)
def recalculate(self):
if abs(self.robot.angle_vel) > self.max_steady_ang_vel:
self._last_unsteady_time = time.time()
# find the point we're actually aiming at that's on the line going through target_point
# and perpendicular to the line from the ball to target_point
if self.target_point == None:
self._shot_point = None
else:
ball2target = self.target_point - main.ball().pos
target_line = robocup.Line(
self.target_point, self.target_point + ball2target.perp_ccw()
) # line perpendicular to aim_line that passes through the target
# ideally the angle we're aiming at would be the angle of the robot, but the bot doesn't kick straight
# it tends to kick in the direction of the side of the mouth that the ball is in
# we draw a line from the center of the bot through the ball and a line along the angle the bot is facing
# our 'actual' aim line is somewhere in-between the two
bot_angle_rad = self.robot.angle
ball_angle_rad = (main.ball().pos - self.robot.pos).angle()
# if the ball angle rad is off by too much, we probably lost sight of it and are going off last known position
# if we detect this big of an error, we just default to using bot_angle_rad
if abs(ball_angle_rad - bot_angle_rad) > math.pi / 3.0:
ball_angle_rad = bot_angle_rad
#TODO: TUNE THIS
ball_angle_bias = 0.4 # NOTE: THIS IS TUNABLE
aim_angle = ball_angle_rad * ball_angle_bias + (
1.0 - ball_angle_bias) * bot_angle_rad
# the line we're aiming down
angle_dir = robocup.Point.direction(aim_angle)
aim_line = robocup.Line(self.robot.pos, self.robot.pos + angle_dir)
# we need to change our face target a bit to account for the difference between bot angle and aim angle
face_angle_offset = bot_angle_rad - aim_angle
target_angle_rad = (self.target_point - self.robot.pos).angle()
face_dir_offset = robocup.Point.direction(target_angle_rad +
face_angle_offset)
self._face_target = self.robot.pos + face_dir_offset
# self._shot_poitn is the point along target_line that we'll hit if we shoot now
# We check to make sure we're not facing backwards from where we want to be or else
# the line intersection will return us a point 180 degrees off from our aim angle
if angle_dir.dot(ball2target) < 0:
self._shot_point = None
else:
self._shot_point = aim_line.line_intersection(target_line)
# error
if self.target_point != None and self._shot_point != None:
self._error = (
self.target_point -
self._shot_point).mag() if self._shot_point != None else float(
"inf"
) # distance in meters off that we'll be if we shoot right now
else:
self._error = float("inf")
def obstacle_robot(self, windows, origin, target, bot_pos):
n = (bot_pos - origin).normalized()
t = n.perp_ccw()
r = constants.Robot.Radius + constants.Ball.Radius
seg = robocup.Segment(bot_pos - n*constants.Robot.Radius + t * r,
bot_pos - n*constants.Robot.Radius - t * r)
if self.debug:
main.system_state().draw_line(seg, constants.Colors.Red, "debug")
end = target.delta().magsq()
extent = [0, end]
for i in range(2):
edge = robocup.Line(origin, seg.get_pt(i))
d = edge.delta().magsq()
intersect = edge.line_intersection(target)
if intersect != None and (intersect - origin).dot(edge.delta()) > d:
t = (intersect - target.get_pt(0)).dot(target.delta())
if t < 0:
extent[i] = 0
elif t > end:
extent[i] = end
else:
extent[i] = t
else:
# Obstacle has no effect
return
self.obstacle_range(windows, extent[0], extent[1])
def __init__(self):
super().__init__(continuous=False)
self.target = robocup.Point(0, constants.Field.Length)
self.add_state(Bump.State.lineup, behavior.Behavior.State.running)
self.add_state(Bump.State.charge, behavior.Behavior.State.running)
self.add_transition(behavior.Behavior.State.start, Bump.State.lineup,
lambda: True, 'immediately')
self.add_transition(
Bump.State.lineup, Bump.State.charge, lambda: self.target_line(
).dist_to(self.robot.pos) <= Bump.LineupToChargeThresh and self.
target_line().delta().dot(self.robot.pos - main.ball().pos) <=
-constants.Robot.Radius and not self.facing_err_above_threshold(),
'lined up')
# FIXME: this condition was never setup in the C++ one...
self.add_transition(
Bump.State.charge, behavior.Behavior.State.completed, lambda:
(main.ball().pos - self.robot.pos).mag() <
(constants.Robot.Radius + constants.Ball.Radius + 0.03),
'ball has been bumped')
self.add_transition(
Bump.State.charge, Bump.State.lineup,
lambda: robocup.Line(self.robot.pos, self.target).dist_to(
main.ball().pos) > Bump.EscapeChargeThresh,
'bad ball placement')
def execute_running(self):
# checks if there is an offensive behavior for the various plays and set the normal speed
for sub in self.subbehaviors_by_name():
if (sub != 'passer'):
robo = self.subbehavior_with_name(sub).robot
# checks if there is a chasing robot and set their speed to defense speed
if (robo != None):
if (sub == 'chasing'):
robo.set_max_speed(self.defense_speed)
# otherwise sets to the offensive normal speed rate
else:
robo.set_max_speed(self.normal_speed)
# chasing robot positon should always follow the ball within a smaller inside box of the
# four corners.
if (self.has_subbehavior_with_name('chasing')):
self.chaser.pos = self.cut_off_pos(main.ball().pos)
#draw the four courner field
# takes in the square points and form lines and create a square on the field
for i in range(len(self.square_points)):
main.debug_drawer().draw_line(
robocup.Line(self.square_points[i],
self.square_points[(i + 1) % 4]), (135, 0, 255),
"Square")
# speed of the hunting robots
#TODO Create python pull from Config values. Currently this breaks the world.
#tmp = copy.deepcopy(robocup.Configuration.FromRegisteredConfigurables().nameLookup("MotionConstraints/Max Velocity").value)
self.normal_speed = 1.0 # tmp
#speed of the defending robots can decrease value to make it easier for offense
self.defense_speed = 2 * self.normal_speed / 3.0 #self.normal_speed/2.0#self.variable_square/(min(self.width,self.length) * 2) * self.normal_speed
def execute_running(self):
if self.robot.has_ball():
self.last_ball_time = time.time()
self.startBallLocation = main.ball().pos
self.recalculate()
self.robot.set_max_speed(0.3)
self.robot.set_max_accel(0.3)
# slowly pivot toward the target
#self.robot.set_max_angle_speed(4)
self.robot.pivot(self._face_target)
self.robot.set_dribble_speed(self.dribbler_power)
# draw current shot line
if self._shot_point != None:
color = constants.Colors.Green if self.is_aimed(
) else constants.Colors.Red
main.system_state().draw_line(
robocup.Line(self.robot.pos, self._shot_point), color, "Aim")
main.system_state().draw_circle(self._shot_point, 0.02, color,
"Aim")
# draw where we're supposed to be aiming
if self.target_point != None:
main.system_state().draw_circle(self.target_point, 0.02,
constants.Colors.Blue, "Aim")
def execute_running(self):
# make sure teammates don't bump into us
self.robot.shield_from_teammates(constants.Robot.Radius * 2.0)
if self.robot.has_ball():
self.last_ball_time = time.time()
self.recalculate()
# slowly pivot toward the target
self.robot.set_max_angle_speed(4)
self.robot.pivot(self._face_target)
self.robot.set_dribble_speed(self.dribbler_speed)
# draw current shot line
if self._shot_point != None:
color = constants.Colors.Green if self.is_aimed(
) else constants.Colors.Red
main.system_state().draw_line(
robocup.Line(self.robot.pos, self._shot_point), color, "Aim")
main.system_state().draw_circle(self._shot_point, 0.02, color,
"Aim")
# draw where we're supposed to be aiming
if self.target_point != None:
main.system_state().draw_circle(self.target_point, 0.02,
constants.Colors.Blue, "Aim")
def execute_block(self):
opposing_kicker = evaluation.ball.opponent_with_ball()
if opposing_kicker is not None:
winEval = evaluation.window_evaluator.WindowEvaluator()
winEval.excluded_robots = [self.robot]
best = winEval.eval_pt_to_our_goal(main.ball().pos)[1]
if best is not None:
shot_line = robocup.Line(opposing_kicker.pos, main.ball().pos)
block_line = robocup.Line(robocup.Point(best.segment.get_pt(0).x - constants.Robot.Radius, constants.Robot.Radius),
robocup.Point(best.segment.get_pt(1).x + constants.Robot.Radius, constants.Robot.Radius))
main.system_state().draw_line(block_line, (255,0,0), "Debug")
dest = block_line.line_intersection(shot_line)
dest.x = min(Goalie.MaxX, dest.x)
dest.x = max(-Goalie.MaxX, dest.x)
self.robot.move_to(dest)
return
self.robot.move_to(robocup.Point(0,constants.Robot.Radius))
def find_robot_intercept_point(robot):
if (robot is not None):
passline = robocup.Line(
main.ball().pos, main.ball().pos + main.ball().vel * 10)
pos = passline.nearest_point(robot.pos) + (main.ball().vel * Capture.DampenMult)
return pos
else:
return None
def execute_aiming(self):
self.set_aim_params()
if isinstance(self.target, robocup.Segment):
for i in range(2):
main.system_state().draw_line(
robocup.Line(main.ball().pos, self.target.get_pt(i)),
constants.Colors.Blue, "PivotKick")
def execute_charge(self):
main.system_state().draw_line(robocup.Line(self.robot.pos, self.target),
constants.Colors.White,
"bump")
main.system_state().draw_line(robocup.Line(main.ball().pos, self.target),
constants.Colors.White,
"bump")
ball2target = (self.target - main.ball().pos).normalized()
drive_dir = (main.ball().pos - ball2target * constants.Robot.Radius) - self.robot.pos
# we want to drive toward the ball without using the path planner
# we do this by setting the speed directly
# AccelBias forces us to accelerate a bit more
speed = self.robot.vel.mag() + Bump.AccelBias
self.robot.set_world_vel(drive_dir.normalized() * speed)
self.robot.face(main.ball().pos)
def set_block_lines_for_threat_handlers(threat):
if len(threat.assigned_handlers) == 0:
return
# make sure goalie is in the middle
if len(threat.assigned_handlers) > 1:
if goalie in threat.assigned_handlers:
idx = threat.assigned_handlers.index(goalie)
if idx != 1:
del threat.assigned_handlers[idx]
threat.assigned_handlers.insert(1, goalie)
if threat.best_shot_window != None:
center_line = robocup.Line(
threat.pos, threat.best_shot_window.segment.center())
else:
center_line = robocup.Line(
threat.pos, constants.Field.OurGoalSegment.center())
# find the angular width that each defender can block. We then space these out accordingly
angle_widths = []
for handler in threat.assigned_handlers:
dist_from_threat = handler.robot.pos.dist_to(threat.pos)
w = min(
2.0 * math.atan2(constants.Robot.Radius, dist_from_threat),
0.15)
angle_widths.append(w)
# start on one edge of our available angle coverage and work counter-clockwise,
# assigning block lines to the bots as we go
spacing = 0.01 if len(
threat.assigned_handlers
) < 3 else 0.0 # spacing between each bot in radians
total_angle_coverage = sum(angle_widths) + (len(angle_widths) -
1) * spacing
start_vec = center_line.delta().normalized()
start_vec.rotate(robocup.Point(0, 0), -total_angle_coverage / 2.0)
for i in range(len(angle_widths)):
handler = threat.assigned_handlers[i]
w = angle_widths[i]
start_vec.rotate(robocup.Point(0, 0), w / 2.0)
handler.block_line = robocup.Line(threat.pos,
threat.pos + start_vec * 10)
start_vec.rotate(robocup.Point(0, 0), w / 2.0 + spacing)
def role_assignment_cost(robot):
# Estimate the time it takes to intercept the ball
# Use straight line distance to closest point along line
# Then movement down the line
# Represents ball movement line
ball_move_line = robocup.Line(main.ball().pos,
main.ball().pos + main.ball().vel * 10)
# Closest point to robot on ball movement line
ball_line_intercept = ball_move_line.nearest_point(robot.pos)
ball_to_robot = robot.pos - main.ball().pos
ball_to_intercept = ball_line_intercept - main.ball().pos
robot_to_intercept = ball_line_intercept - robot.pos
# If we are actually in front of the ball angle wise
# OR close enough that it could be a bounce off the robot due to noisy vision
in_front = ball_move_line.delta().angle_between(ball_to_robot) < math.pi/2 or \
ball_to_robot.mag() < 1.5*(constants.Robot.Radius + constants.Ball.Radius)
# Calculate how much further the ball will move before we reach the point
max_speed = robocup.MotionConstraints.MaxRobotSpeed.value
max_acc = robocup.MotionConstraints.MaxRobotAccel.value
# How long it will take the robot/ball to reach that intercept point
ball_secs = main.ball().estimate_seconds_to(ball_line_intercept)
robot_secs = robocup.get_trapezoidal_time(robot_to_intercept.mag(),
robot_to_intercept.mag(),
max_speed, max_acc, 0, 0)
time_to_hit = robot_secs
# If we aren't in front of the ball, we need to get time to catch up
if (not in_front):
# Difference in speed between us and ball
delta_speed = max_speed - main.ball().vel.mag()
# if we are slower than the ball, don't even try and just exclude it automatically
if (delta_speed < 0):
time_to_hit += 100
# If we are fast, figure out how much more time we need
else:
delta_time = robocup.get_trapezoidal_time(
ball_to_intercept.mag(), ball_to_intercept.mag(),
delta_speed, max_acc, 0, 0)
time_to_hit += delta_time
#main.debug_drawer().draw_text(str(time_to_hit), robot.pos + robocup.Point(0.1,0.1), (255,255,255), "test")
return time_to_hit * Capture.SETTLE_COST_MULTIPLIER
def execute_running(self):
# draw laps
# indices = list(range(len(self.points))) + [0]
# for i in range(len(indices)):
main.system_state().draw_line(
robocup.Line(self.points[0], self.points[1]), (255, 0, 0),
"StressTest")
m = self.subbehavior_with_name('move')
if m.state == behavior.Behavior.State.completed:
# increment index
self.index = (self.index + 1) % len(self.points)
m.pos = self.points[self.index]
def on_enter_kick(self):
OurFreeKick.Running = False
kicker = skills.line_kick.LineKick()
kicker.use_chipper = True
kicker.min_chip_range = OurFreeKick.MinChipRange
kicker.max_chip_range = OurFreeKick.MaxChipRange
kicker.target = self.gap
shooting_line = robocup.Line(main.ball().pos, self.gap)
# If we are at their goal, shoot full power
if shooting_line.segment_intersection(
constants.Field.TheirGoalSegment) is not None:
kicker.kick_power = self.FullKickPower
# If we are aiming in the forward direction and not at one of the "endzones", shoot full power
elif (shooting_line.line_intersection(constants.Field.FieldBorders[0])
or
shooting_line.line_intersection(constants.Field.FieldBorders[2])
and self.gap.y - main.ball().pos.y > 0):
kicker.kick_power = self.FullKickPower
# If we are probably aiming down the field, slowly kick so we dont carpet
else:
kicker.kick_power = self.BumpKickPower
# Try passing if we are doing an indirect kick
if self.indirect:
pass
# Check for valid target pass position
if self.receive_value != 0 and len(main.our_robots()) >= 5:
self.remove_all_subbehaviors()
pass_behavior = tactics.coordinated_pass.CoordinatedPass(
self.receive_pt,
None, (kicker, lambda x: True),
receiver_required=False,
kicker_required=False,
prekick_timeout=7,
use_chipper=True)
# We don't need to manage this anymore
self.add_subbehavior(pass_behavior, 'kicker')
else:
kicker.target = (self.pos_up_field)
self.add_subbehavior(kicker,
'kicker',
required=False,
priority=5)
else:
kicker.target = constants.Field.TheirGoalSegment
self.add_subbehavior(kicker, 'kicker', required=False, priority=5)
def is_moving_towards_our_goal():
# see if the ball is moving much
if main.ball().vel.mag() > 0.1:
# see if it's moving somewhat towards our goal
if main.ball().vel.dot(robocup.Point(0, -1)) > 0:
ball_path = robocup.Line(main.ball().pos, (main.ball().pos + main.ball().vel.normalized()))
fudge_factor = 0.15 # TODO: this could be tuned better
WiderGoalSegment = robocup.Segment(robocup.Point(constants.Field.GoalWidth / 2.0 + fudge_factor, 0),
robocup.Point(-constants.Field.GoalWidth / 2.0 - fudge_factor, 0))
pt = ball_path.line_intersection(WiderGoalSegment)
return pt != None and abs(pt.x) < WiderGoalSegment.delta().mag() / 2.0
return False
def eval_pt_to_seg(self, origin, target):
end = target.delta().magsq()
# if target is a zero-length segment, there are no windows
if end == 0:
return [], None
if self.debug:
main.system_state().draw_line(target, constants.Colors.Blue, "Debug")
windows = [Window(0, end)]
# apply the obstacles
bots = filter(lambda bot: bot not in self.excluded_robots and bot.visible, (list(main.our_robots()) + list(main.their_robots())))
bot_locations = list(map(lambda bot: bot.pos, bots))
bot_locations.extend(self.hypothetical_robot_locations)
for pos in bot_locations:
d = (pos - origin).mag()
# whether or not we can chip over this bot
chip_overable = (self.chip_enabled
and (d < self.max_chip_range - constants.Robot.Radius)
and (d > self.min_chip_range + constants.Robot.Radius))
if not chip_overable:
self.obstacle_robot(windows, origin, target, pos)
# set the segment and angles for each window
p0 = target.get_pt(0)
delta = target.delta() / end
for w in windows:
w.segment = robocup.Segment(p0 + delta * w.t0, p0 + delta * w.t1)
w.a0 = (w.segment.get_pt(0) - origin).angle() * constants.RadiansToDegrees
w.a1 = (w.segment.get_pt(1) - origin).angle() * constants.RadiansToDegrees
best = max(windows, key=lambda w: w.segment.delta().magsq()) if len(windows) > 0 else None
if self.debug and best is not None:
main.system_state().draw_line(best.segment, constants.Colors.Green, "Debug")
main.system_state().draw_line(robocup.Line(origin, best.segment.center()), constants.Colors.Green, "Debug")
# # draw the windows if we're in debug mode
# if self.debug:
# for w in windows:
# pts = [origin, w.segment.get_pt(0), w.segment.get_pt(1)]
# color = (255, 0, 0) if w == best else (0, 255, 0)
# main.system_state().draw_polygon(pts, 3, color, "Windows")
return windows, best
def execute_setup_penalty(self):
pt = robocup.Point(0, robocup.Field_PenaltyDist)
penalty_kicker = min(main.their_robots(),
key=lambda r: (r.pos - pt).mag())
angle_rad = penalty_kicker.angle
shot_line = robocup.Line(
penalty_kicker.pos,
penalty_kicker.pos + robocup.Point.direction(angle_rad))
dest = shot_line.intersection(Goalie.RobotSegment)
if dest == None:
self.robot.move_to(robocup.Point(0, constants.Robot.Radius))
else:
dest.x = max(-Goalie.MaxX + constants.Robot.Radius, dest.x)
dest.y = min(Goalie.MaxX - constants.Robot.Radius, dest.x)
self.robot.move_to(dest)
def get_transition_point(self):
segment = constants.Field.OurGoalSegment
line = robocup.Line(main.ball().pos, main.their_robots()[0].pos)
goalie_pos = constants.Field.OurGoalSegment.center()
if (self.has_subbehavior_with_name('block')):
block_robot = self.subbehavior_with_name('block').robot
if (block_robot != None):
goalie_pos = block_robot.pos
#find the point that the robot is most likely going to shoot at
goal_intercept = segment.line_intersection(line)
#if robot is not lined up with the goal assume the bot will shoot center
if goal_intercept == None:
goal_intercept = segment.center()
#get the vector of the ball to the ideal shot point
ball_to_goal_intercept = goal_intercept - main.ball().pos
#normalize the vector
ball_to_goal_intercept = ball_to_goal_intercept.normalized()
#the ideal spot to be will be the chip distance times the normalized vector from the ball's
#current point.
ideal_defence = ball_to_goal_intercept * self.chip_distance + main.ball(
).pos
#find the line of the shot the robot will make
ball_to_goal_segment = robocup.Segment(main.ball().pos, goal_intercept)
#this is the point to get our robot to block the shot the quickest.
fastest_defence = ball_to_goal_segment.nearest_point(goalie_pos)
#if robot is blocking the shot already just go to the ideal point otherwise average the vectors
#based on the blocking percentage.
if (goalie_pos.dist_to(fastest_defence) <
(constants.Robot.Radius / 4)):
move_to_point = ideal_defence
else:
move_to_point = robocup.Point(
(ideal_defence.x * (self.block_percentage)) +
(fastest_defence.x * (1 - self.block_percentage)),
((ideal_defence.y * self.block_percentage) +
fastest_defence.y * (1 - self.block_percentage)))
return move_to_point
def estimate_potential_recievers_score(self, bot):
ball_travel_line = robocup.Line(main.ball().pos,
main.ball().pos + main.ball().vel)
dot_product = (bot.pos - main.ball().pos).dot(ball_travel_line.delta())
nearest_pt = ball_travel_line.nearest_point(bot.pos)
dx = (nearest_pt - main.ball().pos).mag()
dy = (bot.pos - nearest_pt).mag()
angle = abs(math.atan2(dy, dx))
# Only returns 1 if the opp is moving in the opposite direction as the ball
# and the angle between the ball ray starting at its current position and the opp position
# is less than pi/4
if (angle < math.pi / 4 and dot_product > 0):
return 1
else:
return 0
def find_intercept_point(self, adjusted=True):
approach_vec = self.approach_vector()
adjFactor = robocup.Point.direction(self.robot.angle) \
* -TouchBall.AdjDist
robotPos = self.robot.pos - adjFactor
# multiply by a large enough value to cover the field.
approach_line = robocup.Line(
main.ball().pos,
main.ball().pos + approach_vec * constants.Field.Length)
pos = approach_line.nearest_point(robotPos)
if adjusted:
pos += adjFactor
return pos
def execute_setup_penalty(self):
pt = robocup.Point(0, constants.Field.PenaltyDist)
penalty_kicker = min(main.their_robots(),
key=lambda r: (r.pos - pt).mag())
angle_rad = penalty_kicker.angle
shot_line = robocup.Line(
penalty_kicker.pos,
penalty_kicker.pos + robocup.Point.direction(angle_rad))
dest = Goalie.RobotSegment.line_intersection(shot_line)
if dest is None:
self.robot.move_to(
robocup.Point(0, constants.Robot.Radius + Goalie.OFFSET))
else:
dest.x = max(-Goalie.MaxX + constants.Robot.Radius, dest.x)
dest.x = min(Goalie.MaxX - constants.Robot.Radius, dest.x)
# Shots don't follow the top threat, they follow the inverse
# FIXME this is kind of a hack
dest.x = -dest.x
self.robot.move_to(dest)
def opp_robot_blocking(self):
if (self.robot is None):
return False
# Closest opp robot in any direction
# To us, not the ball
closest_opp_robot = None
closest_opp_dist = float("inf")
for r in main.their_robots():
if ((r.pos - self.robot.pos).mag() < closest_opp_dist):
closest_opp_robot = r
closest_opp_dist = (r.pos - self.robot.pos).mag()
# Only do this if a robot is in range
robot_in_range = closest_opp_dist < .2 + 2 * constants.Robot.Radius
aim_dir = robocup.Point.direction(self.robot.angle)
robot_dir = (closest_opp_robot.pos - self.robot.pos)
# Only trigger if they are infront of us
robot_in_front = aim_dir.dot(robot_dir) > 0
closest_pt = robocup.Line(
self.robot.pos,
self.robot.pos + aim_dir).nearest_point(closest_opp_robot.pos)
does_hit_robot = (closest_opp_robot.pos - closest_pt
).mag() < constants.Robot.Radius
facing_their_side = robocup.Point.direction(self.robot.angle).y > 0
ret = (facing_their_side and robot_in_range and robot_in_front and
does_hit_robot)
if ret:
print("Panic kick")
main.debug_drawer().draw_text('panic kick', self.robot.pos,
(255, 255, 255), 'PivotKick')
return ret
def execute_running(self):
if self.robot.has_ball():
self.startBallLocation = main.ball().pos
self.recalculate()
self.robot.set_max_speed(0.9)
self.robot.set_max_accel(0.9)
# slowly pivot toward the target
#self.robot.set_max_angle_speed(4)
self.robot.pivot(self._face_target)
self.robot.set_dribble_speed(self.dribbler_power)
# draw current shot line
if self._shot_point != None:
color = constants.Colors.Green if self.is_aimed(
) else constants.Colors.Red
main.debug_drawer().draw_line(
robocup.Line(self.robot.pos, self._shot_point), color, "Aim")
main.debug_drawer().draw_circle(self._shot_point, 0.02, color,
"Aim")
# draw where we're supposed to be aiming
if self.target_point != None:
main.debug_drawer().draw_circle(self.target_point, 0.02,
constants.Colors.Blue, "Aim")
# If we are within X degrees of the target, start the fine timeout
if (self.target_point is not None and self._fine_start == 0
and robocup.Point.direction(
self.robot.angle).angle_between(self.target_point -
self.robot.pos) <
45 * constants.DegreesToRadians and robocup.Point.direction(
self.robot.angle).dot(self.target_point - self.robot.pos) >
0):
self._fine_start = time.time()
def recalculate(self):
self._target_line = robocup.Line(main.ball().pos,
self.aim_target_point)
def recalculate(self):
goalie = self.subbehavior_with_name('goalie')
defender1 = self.subbehavior_with_name('defender1')
defender2 = self.subbehavior_with_name('defender2')
behaviors = [goalie, defender1, defender2]
# if we don't have any bots to work with, don't waste time calculating
if all(bhvr.robot == None for bhvr in behaviors):
return
# A threat to our goal - something we'll actively defend against
class Threat:
def __init__(self, source=None):
self.source = source
self.ball_acquire_chance = 1.0
self.shot_chance = 1.0
self.assigned_handlers = []
self.best_shot_window = None
# an OpponentRobot or Point
@property
def source(self):
return self._source
@source.setter
def source(self, value):
self._source = value
# our source can be a Point or an OpponentRobot, this method returns the location of it
@property
def pos(self):
if self.source != None:
return self.source if isinstance(
self.source, robocup.Point) else self.source.pos
# a list of our behaviors that will be defending against this threat
# as of now only Defender and Goalie
@property
def assigned_handlers(self):
return self._assigned_handlers
@assigned_handlers.setter
def assigned_handlers(self, value):
self._assigned_handlers = value
# our estimate of the chance that this threat will acquire the ball
# 1.0 if it already has it
# otherwise, a value from 0 to 1 gauging its likelihood to receive a pass
@property
def ball_acquire_chance(self):
return self._ball_acquire_chance
@ball_acquire_chance.setter
def ball_acquire_chance(self, value):
self._ball_acquire_chance = value
# our estimate of the chance of this threat making its shot on the goal given that it gets/has the ball
# NOTE: this is calculated excluding all of our robots on the field as obstacles
@property
def shot_chance(self):
return self._shot_chance
@shot_chance.setter
def shot_chance(self, value):
self._shot_chance = value
# his best window on our goal
@property
def best_shot_window(self):
return self._best_shot_window
@best_shot_window.setter
def best_shot_window(self, value):
self._best_shot_window = value
# our assessment of the risk of this threat
# should be between 0 and 1
@property
def score(self):
return self.ball_acquire_chance * self.shot_chance
# available behaviors we have to assign to threats
# only look at ones that have robots
# as we handle threats, we remove the handlers from this list
unused_threat_handlers = list(
filter(lambda bhvr: bhvr.robot != None,
[goalie, defender1, defender2]))
def set_block_lines_for_threat_handlers(threat):
if len(threat.assigned_handlers) == 0:
return
# make sure goalie is in the middle
if len(threat.assigned_handlers) > 1:
if goalie in threat.assigned_handlers:
idx = threat.assigned_handlers.index(goalie)
if idx != 1:
del threat.assigned_handlers[idx]
threat.assigned_handlers.insert(1, goalie)
if threat.best_shot_window != None:
center_line = robocup.Line(
threat.pos, threat.best_shot_window.segment.center())
else:
center_line = robocup.Line(
threat.pos, constants.Field.OurGoalSegment.center())
# find the angular width that each defender can block. We then space these out accordingly
angle_widths = []
for handler in threat.assigned_handlers:
dist_from_threat = handler.robot.pos.dist_to(threat.pos)
w = min(
2.0 * math.atan2(constants.Robot.Radius, dist_from_threat),
0.15)
angle_widths.append(w)
# start on one edge of our available angle coverage and work counter-clockwise,
# assigning block lines to the bots as we go
spacing = 0.01 if len(
threat.assigned_handlers
) < 3 else 0.0 # spacing between each bot in radians
total_angle_coverage = sum(angle_widths) + (len(angle_widths) -
1) * spacing
start_vec = center_line.delta().normalized()
start_vec.rotate(robocup.Point(0, 0), -total_angle_coverage / 2.0)
for i in range(len(angle_widths)):
handler = threat.assigned_handlers[i]
w = angle_widths[i]
start_vec.rotate(robocup.Point(0, 0), w / 2.0)
handler.block_line = robocup.Line(threat.pos,
threat.pos + start_vec * 10)
start_vec.rotate(robocup.Point(0, 0), w / 2.0 + spacing)
def recalculate_threat_shot(threat_index):
if not isinstance(threat_index, int):
raise TypeError("threat_index should be an int")
# ignore all of our robots
excluded_robots = list(main.our_robots())
# behaviors before this threat are counted as obstacles in their TARGET position (where we've
# assigned them to go, not where they are right now)
hypothetical_obstacles = []
for t in threats[0:threat_index]:
hypothetical_obstacles.extend(
map(lambda bhvr: bhvr.move_target, t.assigned_handlers))
threat = threats[threat_index]
self.win_eval.excluded_robots.clear()
for r in excluded_robots:
self.win_eval.add_excluded_robot(r)
_, threat.best_shot_window = self.win_eval.eval_pt_to_our_goal(
threat.pos)
if threat.best_shot_window is not None:
threat.shot_chance = threat.best_shot_window.shot_success
else:
threat.shot_chance = 0.0
threats = []
# secondary threats are those that are somewhat close to our goal and open for a pass
# if they're farther than this down the field, we don't consider them threats
threat_max_y = constants.Field.Length / 2.0
potential_threats = [
opp for opp in main.their_robots() if opp.pos.y < threat_max_y
]
# find the primary threat
# if the ball is not moving OR it's moving towards our goal, it's the primary threat
# if it's moving, but not towards our goal, the primary threat is the robot on their team most likely to catch it
if main.ball().vel.mag() > 0.4:
# the line the ball's moving along
ball_travel_line = robocup.Line(main.ball().pos,
main.ball().pos + main.ball().vel)
# this is a shot on the goal!
if evaluation.ball.is_moving_towards_our_goal():
ball_threat = Threat(main.ball().pos)
ball_threat.ball_acquire_chance = 1.0
ball_threat.shot_chance = 1.0
threats.append(ball_threat)
else:
# Check for a bot that's about to capture this ball and potentially shoot on the goal
# potential_receivers is an array of (OpponentRobot, angle) tuples, where the angle
# is the angle between the ball_travel_line and the line from the ball to the opponent
# bot - this is our metric for receiver likeliness.
potential_receivers = []
for opp in potential_threats:
# see if the bot is in the direction the ball is moving
if (opp.pos - ball_travel_line.get_pt(0)).dot(
ball_travel_line.delta()) > 0:
# calculate the angle and add it to the list if it's within reason
nearest_pt = ball_travel_line.nearest_point(opp.pos)
dx = (nearest_pt - main.ball().pos).mag()
dy = (opp.pos - nearest_pt).mag()
angle = abs(math.atan2(dy, dx))
if angle < math.pi / 4.0:
potential_receivers.append((opp, 1.0))
# choose the receiver with the smallest angle from the ball travel line
if len(potential_receivers) > 0:
best_receiver_tuple = min(
potential_receivers,
key=lambda rcrv_tuple: rcrv_tuple[1])
if best_receiver_tuple != None:
receiver_threat = Threat(best_receiver_tuple[0])
receiver_threat.ball_acquire_chance = 0.9 # note: this value is arbitrary
receiver_threat.shot_chance = 0.9 # FIXME: calculate this
threats.append(receiver_threat)
else:
ball_threat = Threat(main.ball().pos)
ball_threat.ball_acquire_chance = 1.0
ball_threat.shot_chance = 0.9
threats.append(ball_threat)
else:
# primary threat is the ball or the opponent holding it
opp_with_ball = evaluation.ball.opponent_with_ball()
threat = Threat(
opp_with_ball if opp_with_ball != None else main.ball().pos)
threat.ball_acquire_chance = 1.0
threat.shot_chance = 1.0 # FIXME: calculate, don't use 1.0
threats.append(threat)
# if an opponent has the ball or is potentially about to receive the ball,
# we look at potential receivers of it as threats
if isinstance(threats[0].source, robocup.OpponentRobot):
for opp in filter(lambda t: t.visible, potential_threats):
pass_chance = evaluation.passing.eval_pass(
main.ball().pos, opp.pos, excluded_robots=[opp])
# give it a small chance because the obstacles in the way could move soon and we don't want to consider it a zero threatos, )
if pass_chance < 0.001: pass_chance = 0.4
# record the threat
threat = Threat(opp)
threat.ball_acquire_chance = pass_chance
threats.append(threat)
# Now we evaluate this opponent's shot on the goal
# exclude robots that have already been assigned to handle other threats
self.win_eval.excluded_robots.clear()
for r in map(lambda bhvr: bhvr.robot, unused_threat_handlers):
self.win_eval.add_excluded_robot(r)
_, threat.best_shot_window = self.win_eval.eval_pt_to_our_goal(
opp.pos)
if threat.best_shot_window is not None:
threat.shot_chance = threat.best_shot_window.shot_success
else:
threat.shot_chance = 0.0
if threat.shot_chance == 0:
# gve it a small chance because the shot could clear up a bit later and we don't want to consider it a zero threat
threat.shot_chance = 0.2
else:
# the ball isn't possessed by an opponent, so we just look at opponents with shots on the goal
for opp in potential_threats:
# record the threat
lurker = Threat(opp)
lurker.ball_acquire_chance = 0.5 # note: this is a bullshit value
threats.append(lurker)
recalculate_threat_shot(len(threats) - 1)
# only consider the top three threats
threats.sort(key=lambda threat: threat.score, reverse=True)
threats = threats[0:3]
# print("sorted threats:")
# for idx, t in enumerate(threats):
# print("t[" + str(idx) + "]: " + str(t.source) + "shot: " + str(t.shot_chance) + "; pass:"******"; score:" + str(t.score))
# print("sorted threat scores: " + str(list(map(lambda t: str(t.score) + " " + str(t.source), threats))))
# print("Unused handlers: " + str(unused_threat_handlers))
# print("---------------------")
smart = False
if not smart:
# only deal with top two threats
threats_to_block = threats[0:2]
# print('threats to block: ' + str(list(map(lambda t: t.source, threats_to_block))))
threat_idx = 0
while len(unused_threat_handlers) > 0:
threats_to_block[threat_idx].assigned_handlers.append(
unused_threat_handlers[0])
del unused_threat_handlers[0]
threat_idx = (threat_idx + 1) % len(threats_to_block)
for t_idx, t in enumerate(threats_to_block):
recalculate_threat_shot(t_idx)
set_block_lines_for_threat_handlers(t)
else:
# assign all of our defenders to do something
while len(unused_threat_handlers) > 0:
# prioritize by threat score, highest first
top_threat = max(threats, key=lambda threat: threat.score)
# assign the next handler to this threat
handler = unused_threat_handlers[0]
top_threat.assigned_handlers.append(handler)
del unused_threat_handlers[0]
# reassign the block line for each handler of this threat
set_block_lines_for_threat_handlers(top_threat)
# recalculate the shot now that we have
recalculate_threat_shot(0)
# tell the bots where to move / what to block and draw some debug stuff
for idx, threat in enumerate(threats):
# recalculate, including all current bots
# FIXME: do we want this?
# recalculate_threat_shot(idx)
# the line they'll be shooting down/on
if threat.best_shot_window != None:
shot_line = robocup.Segment(
threat.pos, threat.best_shot_window.segment.center())
else:
shot_line = robocup.Segment(threat.pos, robocup.Point(0, 0))
# debug output
if self.debug:
for handler in threat.assigned_handlers:
# handler.robot.add_text("Marking: " + str(threat.source), constants.Colors.White, "Defense")
main.system_state().draw_circle(handler.move_target, 0.02,
constants.Colors.Blue,
"Defense")
# draw some debug stuff
if threat.best_shot_window != None:
# draw shot triangle
pts = [
threat.pos,
threat.best_shot_window.segment.get_pt(0),
threat.best_shot_window.segment.get_pt(1)
]
shot_color = (255, 0, 0, 150) # translucent red
main.system_state().draw_polygon(pts, shot_color,
"Defense")
main.system_state().draw_segment(
threat.best_shot_window.segment, constants.Colors.Red,
"Defense")
self.win_eval.excluded_robots.clear()
_, best_window = self.win_eval.eval_pt_to_our_goal(
threat.pos)
if best_window is not None:
chance = best_window.shot_success
else:
chance = 0.0
main.system_state().draw_text(
"Shot: " + str(int(threat.shot_chance * 100.0)) +
"% / " + str(int(chance * 100)) + "%",
shot_line.center(), constants.Colors.White, "Defense")
# draw pass lines
if idx > 0:
pass_line = robocup.Segment(main.ball().pos, threat.pos)
main.system_state().draw_line(pass_line,
constants.Colors.Red,
"Defense")
main.system_state().draw_text(
"Pass: "******"%",
pass_line.center(), constants.Colors.White, "Defense")
def execute_marking(self):
#main.system_state().draw_line(robocup.Line(self._area.get_pt(0), self._area.get_pt(1)), (127,0,255), "Defender")
self.block_robot = self.find_robot_to_block()
if self.block_robot is not None:
# self.robot.add_text("Blocking Robot " + str(self.block_robot.shell_id()), (255,255,255), "RobotText")
pass
if self.robot.pos.near_point(robocup.Point(0, 0),
self._opponent_avoid_threshold):
self.robot.set_avoid_opponents(False)
else:
self.robot.set_avoid_opponents(True)
target = None
if self.block_robot is None:
target = main.ball().pos + main.ball().vel * 0.3
else:
target = self.block_robot.pos + self.block_robot.vel * 0.3
goal_line = robocup.Segment(
robocup.Point(-constants.Field.GoalWidth / 2.0, 0),
robocup.Point(constants.Field.GoalWidth / 2.0, 0))
self._win_eval.excluded_robots = [self.robot]
# TODO defenders should register themselves with some static list on init
# TODO make this happen in python-land
# for (Defender *f : otherDefenders)
# {
# if (f->robot)
# {
# _winEval.exclude.push_back(f->robot->pos);
# }
# }
windows = self._win_eval.eval_pt_to_seg(target, goal_line)[0]
best = None
goalie = main.our_robot_with_id(main.root_play().goalie_id)
if goalie is not None and self.side is not Defender.Side.center:
for window in windows:
if best is None:
best = window
elif self.side is Defender.Side.left and window.segment.center.x < goalie.pos.x and window.segment.length > best.segment.length:
best = window
elif self.side is Defender.Side.right and window.segment.center.x > goalie.pos.x and window.segment.length > best.segment.length:
best = window
else:
best_dist = 0
for window in windows:
seg = robocup.Segment(window.segment.center(), main.ball().pos)
new_dist = seg.dist_to(self.robot.pos)
if best is None or new_dist < best_dist:
best = window
best_dist = new_dist
shoot_seg = None
if best is not None:
if self.block_robot is not None:
dirvec = robocup.Point.direction(self.block_robot.angle *
(math.pi / 180.0))
shoot_seg = robocup.Segment(
self.block_robot.pos, self.block_robot.pos + dirvec * 7.0)
else:
shoot_seg = robocup.Segment(
main.ball().pos,
main.ball().pos + main.ball().vel.normalized() * 7.0)
need_task = False
if best is not None:
winseg = best.segment
if main.ball().vel.magsq() > 0.03 and winseg.segment_intersection(
shoot_seg) != None:
self.robot.move_to(shoot_seg.nearest_point(self.robot.pos))
self.robot.face_none()
else:
winsize = winseg.length()
if winsize < constants.Ball.Radius:
need_task = True
else:
arc = robocup.Circle(robocup.Point(0, 0),
self._defend_goal_radius)
shot = robocup.Line(winseg.center(), target)
dest = [robocup.Point(0, 0), robocup.Point(0, 0)]
intersected, dest[0], dest[1] = shot.intersects_circle(arc)
if intersected:
self.robot.move_to(
dest[0] if dest[0].y > 0 else dest[1])
if self.block_robot is not None:
self.robot.face(self.block_robot.pos)
else:
self.robot.face(main.ball().pos)
else:
need_task = True
if need_task:
self.robot.face(main.ball().pos)
backVec = robocup.Point(1, 0)
backPos = robocup.Point(-constants.Field.Width / 2, 0)
shotVec = robocup.Point(main.ball().pos - self.robot.pos)
backVecRot = robocup.Point(backVec.perp_ccw())
facing_back_line = (backVecRot.dot(shotVec) < 0)
if not facing_back_line and self.robot.has_ball():
if self.robot.has_chipper():
self.robot.chip(255)
else:
self.robot.kick(255)
def recalculate(self):
# can't do squat if we don't know what we're supposed to do
if self.receive_point == None or self.robot == None or self.target_point == None:
return
ball = main.ball()
if self.ball_kicked:
# when the ball's in motion, the line is based on the ball's velocity
self._pass_line = robocup.Line(ball.pos, ball.pos + ball.vel * 10)
# After kicking, apply angle calculations
target_angle_rad = self.adjust_angle(
(self.target_point - self.robot.pos).angle())
# Removes angle adjustment
# target_angle_rad = (self.target_point - self.robot.pos).angle()
self._kick_line = robocup.Line(
self.robot.pos,
robocup.Point(
self.robot.pos.x + math.cos(self.robot.angle) * 10,
self.robot.pos.y + math.sin(self.robot.angle) * 10))
else:
# if the ball hasn't been kicked yet, we assume it's going to go through the receive point
self._pass_line = robocup.Line(ball.pos, self.receive_point)
# Assume ball is kicked at max speed and is coming from the ball point to the location of our robot. Then average this with the target angle.
target_angle_rad = self.adjust_angle(
(self.target_point - self.robot.pos).angle(),
(self.robot.pos - main.ball().pos).angle(),
constants.Robot.MaxKickSpeed)
# TODO make this faster by caching the .angle() part
target_angle_rad = (
target_angle_rad +
(self.target_point - self.robot.pos).angle()) / 2
self._kick_line = robocup.Line(self.receive_point,
self.target_point)
self._angle_facing = target_angle_rad
angle_rad = self.robot.angle
self._angle_error = target_angle_rad - angle_rad
if self.ball_kicked:
receive_before_adjust = self._pass_line.nearest_point(
self.robot.pos)
else:
receive_before_adjust = self.receive_point
# Make the receive point be the mouth, rather than the center of the robot.
# Assumes mouth of robot is at the edge.
self._target_pos = receive_before_adjust - robocup.Point(
constants.Robot.Radius * math.cos(self.robot.angle),
constants.Robot.Radius * math.sin(self.robot.angle))
# Code to provide slipback when receiving the ball
# pass_line_dir = (self._pass_line.get_pt(1) - self._pass_line.get_pt(0)).normalized()
# self._target_pos = actual_receive_point + pass_line_dir * constants.Robot.Radius
# vector pointing down the pass line toward the kicker
pass_dir = (self._pass_line.get_pt(0) -
self._pass_line.get_pt(1)).normalized()
pos_error = self._target_pos - self.robot.pos
self._x_error = self._target_pos.x - self.robot.pos.x
self._y_error = self._target_pos.y - self.robot.pos.y
