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 execute_charge(self):
main.system_state().draw_line(
robocup.Line(self.robot.pos, self.aim_target_point),
constants.Colors.White, "LineKick")
main.system_state().draw_line(
robocup.Line(main.ball().pos, self.aim_target_point),
constants.Colors.White, "LineKick")
# 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() + LineKick.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) < LineKick.ClosenessThreshold:
self._got_close = True
if self._got_close:
if self.use_chipper:
self.robot.chip(self.chip_power)
else:
self.robot.kick(self.kick_power)
def execute_running(self):
if self.mark_point is None and \
(self.mark_robot is None or
not main.ball().valid or
not self.mark_robot.visible):
return
ball_pos = main.ball().pos
pos = self.robot.pos
mark_pos = self.mark_point if self.mark_point is not None else self.mark_robot.pos
mark_line_dir = (ball_pos - mark_pos).normalized()
ball_mark_line = robocup.Segment(
ball_pos - mark_line_dir * constants.Ball.Radius,
mark_pos + mark_line_dir * 2.0 * constants.Robot.Radius)
main.system_state().draw_line(ball_mark_line, (0, 0, 255), "Mark")
mark_line_dist = ball_mark_line.dist_to(pos)
target_point = None
if mark_line_dist > self.mark_line_thresh:
target_point = ball_mark_line.nearest_point(pos)
else:
target_point = ball_pos + (mark_pos - ball_pos).normalized(
) * self.ratio * ball_mark_line.length()
main.system_state().draw_circle(mark_pos, constants.Robot.Radius * 1.2,
(0, 127, 255), "Mark")
if self.mark_robot is not None:
self.robot.approach_opponent(self.mark_robot.shell_id(), True)
self.robot.move_to(target_point)
self.robot.face(ball_pos)
def execute_setup(self):
move_goal = main.ball().pos - self._target_line.delta().normalized(
) * (self.drive_around_dist + constants.Robot.Radius)
left_field_edge = robocup.Segment(
robocup.Point(
-constants.Field.Width / 2.0 - constants.Robot.Radius, 0),
robocup.Point(
-constants.Field.Width / 2.0 - constants.Robot.Radius,
constants.Field.Length))
right_field_edge = robocup.Segment(
robocup.Point(constants.Field.Width / 2.0 + constants.Robot.Radius,
0),
robocup.Point(constants.Field.Width / 2.0 + constants.Robot.Radius,
constants.Field.Length))
# handle the case when the ball is near the field's edge
field_edge_thresh = 0.3
behind_line = robocup.Segment(
main.ball().pos - self._target_line.delta().normalized() *
self.drive_around_dist,
main.ball().pos - self._target_line.delta().normalized())
main.system_state().draw_line(behind_line, constants.Colors.Blue,
"LineKickOld")
for edge in [left_field_edge, right_field_edge]:
if edge.near_point(main.ball().pos, field_edge_thresh):
intersection = behind_line.segment_intersection(edge)
if intersection != None:
move_goal = intersection
self.robot.set_avoid_ball_radius(self.setup_ball_avoid)
self.robot.move_to(move_goal)
self.robot.face(self.robot.pos + (self.aim_target_point - main.ball(
).pos))
self.robot.unkick()
def execute_setup(self):
move_goal = main.ball().pos - self._target_line.delta().normalized(
) * (self.drive_around_dist + constants.Robot.Radius)
left_field_edge = robocup.Segment(
robocup.Point(
-constants.Field.Width / 2.0 - constants.Robot.Radius, 0),
robocup.Point(
-constants.Field.Width / 2.0 - constants.Robot.Radius,
constants.Field.Length))
right_field_edge = robocup.Segment(
robocup.Point(constants.Field.Width / 2.0 + constants.Robot.Radius,
0),
robocup.Point(constants.Field.Width / 2.0 + constants.Robot.Radius,
constants.Field.Length))
# handle the case when the ball is near the field's edge
field_edge_thresh = 0.3
behind_line = robocup.Segment(
main.ball().pos -
self._target_line.delta().normalized() * self.drive_around_dist,
main.ball().pos - self._target_line.delta().normalized())
main.system_state().draw_line(behind_line, constants.Colors.Blue,
"LineKick")
for edge in [left_field_edge, right_field_edge]:
if edge.near_point(main.ball().pos, field_edge_thresh):
intersection = behind_line.line_intersection(edge)
if intersection != None:
move_goal = intersection
self.robot.set_avoid_ball_radius(self.setup_ball_avoid)
self.robot.move_to(move_goal)
self.robot.face(self.robot.pos +
(self.aim_target_point - main.ball().pos))
self.robot.unkick()
def execute_charge(self):
self.robot.disable_avoid_ball()
main.system_state().draw_line(
robocup.Line(self.robot.pos, self.aim_target_point),
constants.Colors.White, "LineKickOld")
main.system_state().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 execute_running(self):
#pylint: disable=no-member
if self.mark_point is None and \
(self.mark_robot is None or
not main.ball().valid or
not self.mark_robot.visible):
return
ball_pos = main.ball().pos
pos = self.robot.pos
mark_pos = self.mark_point if self.mark_point is not None else self.mark_robot.pos
mark_line_dir = (ball_pos - mark_pos).normalized()
ball_mark_line = robocup.Segment(
ball_pos - mark_line_dir * constants.Ball.Radius,
mark_pos + mark_line_dir * 2.0 * constants.Robot.Radius)
main.system_state().draw_line(ball_mark_line, (0, 0, 255), "Mark")
mark_line_dist = ball_mark_line.dist_to(pos)
self._target_point = None
if mark_line_dist > self.mark_line_thresh:
self._target_point = ball_mark_line.nearest_point(pos)
else:
self._target_point = ball_pos + (
mark_pos -
ball_pos).normalized() * self.ratio * ball_mark_line.length()
main.system_state().draw_circle(mark_pos, constants.Robot.Radius * 1.2,
(0, 127, 255), "Mark")
if self.mark_robot is not None:
self.robot.approach_opponent(self.mark_robot.shell_id(), True)
self.robot.move_to(self._target_point)
self.robot.face(ball_pos)
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 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 execute_running(self):
super().execute_running()
self.robot.face(self.robot.pos + robocup.Point(
math.cos(self._angle_facing), math.sin(self._angle_facing)))
if self._kick_line != None:
main.system_state().draw_line(self._kick_line,
constants.Colors.Red, "Shot")
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_course_approach(self):
# don't hit the ball on accident
self.robot.set_avoid_ball_radius(Capture.CourseApproachAvoidBall)
pos = self.find_intercept_point()
self.robot.face(main.ball().pos)
if pos != None and main.ball().valid:
main.system_state().draw_circle(pos, constants.Ball.Radius, constants.Colors.White, "Capture")
self.robot.move_to(pos)
def execute_running(self):
super().execute_running()
self.robot.face(self.robot.pos + robocup.Point(
math.cos(self._angle_facing), math.sin(self._angle_facing)))
if self._kick_line != None:
main.system_state().draw_line(self._kick_line,
constants.Colors.Red, "Shot")
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_receiving(self):
super().execute_receiving()
# Kick the ball!
self.robot.kick(self.kick_power)
if self.target_point != None:
main.system_state().draw_circle(self.target_point, 0.03,
constants.Colors.Blue, "Target")
def execute_receiving(self):
super().execute_receiving()
# Kick the ball!
self.robot.kick(self.kick_power)
if self.target_point != None:
main.system_state().draw_circle(self.target_point, 0.03,
constants.Colors.Blue, "Target")
def execute_running(self):
self.recalculate()
self.robot.face(main.ball().pos)
if self._pass_line != None:
main.system_state().draw_line(self._pass_line,
constants.Colors.Blue, "Pass")
main.system_state().draw_circle(self._target_pos, 0.03,
constants.Colors.Blue, "Pass")
def execute_marking(self):
move = self.subbehavior_with_name('move')
move.pos = self.move_target
arc = robocup.Circle(robocup.Point(0,0), self._defend_goal_radius)
if move.pos != None:
main.system_state().draw_circle(move.pos, 0.02, constants.Colors.Green, "Mark")
main.system_state().draw_circle(robocup.Point(0,0), self._defend_goal_radius, constants.Colors.Green, "Mark")
def execute_running(self):
self.recalculate()
self.robot.face(main.ball().pos)
if self._pass_line != None:
main.system_state().draw_line(self._pass_line,
constants.Colors.Blue, "Pass")
main.system_state().draw_circle(self._target_pos, 0.03,
constants.Colors.Blue, "Pass")
def execute_marking(self):
move = self.subbehavior_with_name('move')
move.pos = self.move_target
arc = robocup.Circle(robocup.Point(0,0), self._defend_goal_radius)
if move.pos != None:
main.system_state().draw_circle(move.pos, 0.02, constants.Colors.Green, "Mark")
main.system_state().draw_circle(robocup.Point(0,0), self._defend_goal_radius, constants.Colors.Green, "Mark")
def execute_kicker_forbidden(self):
bot = None
for b in main.our_robots():
if b.shell_id() == self.kicker_shell_id:
bot = b
break
if self.kicker_shell_id and bot:
main.system_state().draw_text(
"Blocking double touch!", bot.pos,
constants.Colors.Red, "Double Touches")
def execute_kicker_forbidden(self):
bot = None
for b in main.our_robots():
if b.shell_id() == self.kicker_shell_id:
bot = b
break
if self.kicker_shell_id and bot:
main.system_state().draw_text("Blocking double touch!", bot.pos,
constants.Colors.Red,
"Double Touches")
def execute_running(self):
# make sure teammates don't bump into us
self.robot.shield_from_teammates(constants.Robot.Radius * 2.0)
self.recalculate()
self.robot.face(main.ball().pos)
if self._pass_line != None:
main.system_state().draw_line(self._pass_line, constants.Colors.Blue, "Pass")
main.system_state().draw_circle(self._target_pos, 0.03, constants.Colors.Blue, "Pass")
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 block_line(self, value):
self._block_line = value
# we move somewhere along this arc to mark our 'block_line'
arc_left = robocup.Arc(
robocup.Point(-constants.Field.GoalFlat / 2, 0),
constants.Field.ArcRadius + constants.Robot.Radius * 2,
math.pi / 2, math.pi)
arc_right = robocup.Arc(
robocup.Point(constants.Field.GoalFlat / 2, 0),
constants.Field.ArcRadius + constants.Robot.Radius * 2, 0,
math.pi / 2)
seg = robocup.Segment(
robocup.Point(
-constants.Field.GoalFlat / 2,
constants.Field.ArcRadius + constants.Robot.Radius * 2),
robocup.Point(
constants.Field.GoalFlat / 2,
constants.Field.ArcRadius + constants.Robot.Radius * 2))
default_pt = seg.center()
if self._block_line is not None:
# main.system_state().draw_line(self._block_line, constants.Colors.White, "SubmissiveDefender")
main.system_state().draw_circle(
self._block_line.get_pt(0), 0.1, constants.Colors.White,
"SubmissiveDefender")
threat_point = self._block_line.get_pt(0)
intersection_center = seg.line_intersection(self._block_line)
if threat_point.x < 0:
intersections_left = arc_left.intersects_line(self._block_line)
if len(intersections_left) > 0:
self._move_target = max(intersections_left,
key=lambda p: p.y)
elif intersection_center is not None:
self._move_target = intersection_center
else:
self._move_target = default_pt
elif threat_point.x >= 0:
intersections_right = arc_right.intersects_line(
self._block_line)
if len(intersections_right) > 0:
self._move_target = max(intersections_right,
key=lambda p: p.y)
elif intersection_center is not None:
self._move_target = intersection_center
else:
self._move_target = default_pt
else:
self._move_target = default_pt
def execute_running(self):
# make sure teammates don't bump into us
self.robot.shield_from_teammates(constants.Robot.Radius * 2.0)
self.recalculate()
self.robot.face(main.ball().pos)
if self._pass_line != None:
main.system_state().draw_line(self._pass_line,
constants.Colors.Blue, "Pass")
main.system_state().draw_circle(self._target_pos, 0.03,
constants.Colors.Blue, "Pass")
def block_line(self, value):
self._block_line = value
# we move somewhere along this arc to mark our 'block_line'
arc_left = robocup.Arc(
robocup.Point(-constants.Field.GoalFlat / 2, 0),
constants.Field.ArcRadius + constants.Robot.Radius * 2,
math.pi / 2, math.pi)
arc_right = robocup.Arc(
robocup.Point(constants.Field.GoalFlat / 2, 0),
constants.Field.ArcRadius + constants.Robot.Radius * 2, 0,
math.pi / 2)
seg = robocup.Segment(
robocup.Point(
-constants.Field.GoalFlat / 2,
constants.Field.ArcRadius + constants.Robot.Radius * 2),
robocup.Point(
constants.Field.GoalFlat / 2,
constants.Field.ArcRadius + constants.Robot.Radius * 2))
default_pt = seg.center()
if self._block_line != None:
# main.system_state().draw_line(self._block_line, constants.Colors.White, "SubmissiveDefender")
main.system_state().draw_circle(self._block_line.get_pt(0), 0.1,
constants.Colors.White,
"SubmissiveDefender")
threat_point = self._block_line.get_pt(0)
intersection_center = seg.line_intersection(self._block_line)
if threat_point.x < 0:
intersections_left = arc_left.intersects_line(self._block_line)
if len(intersections_left) > 0:
self._move_target = max(intersections_left,
key=lambda p: p.y)
elif intersection_center is not None:
self._move_target = intersection_center
else:
self._move_target = default_pt
elif threat_point.x >= 0:
intersections_right = arc_right.intersects_line(
self._block_line)
if len(intersections_right) > 0:
self._move_target = max(intersections_right,
key=lambda p: p.y)
elif intersection_center is not None:
self._move_target = intersection_center
else:
self._move_target = default_pt
else:
self._move_target = default_pt
def execute_course_approach(self):
# don't hit the ball on accident
self.robot.set_avoid_ball_radius(Capture.CourseApproachAvoidBall)
pos = self.find_intercept_point()
self.robot.face(main.ball().pos)
if (self.lastApproachTarget != None and (pos - self.lastApproachTarget).mag()<0.1):
self.robot.move_to(self.lastApproachTarget)
else:
main.system_state().draw_circle(pos, constants.Ball.Radius, constants.Colors.White, "Capture")
self.robot.move_to(pos)
self.lastApproachTarget = pos
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 block_line(self, value):
self._block_line = value
# we move somewhere along this arc to mark our 'block_line'
offset = constants.Robot.Radius * 1.2
left_seg = robocup.Segment(
robocup.Point(-constants.Field.PenaltyLongDist / 2 - offset, 0),
robocup.Point(-constants.Field.PenaltyLongDist / 2 - offset,
constants.Field.PenaltyShortDist + offset))
right_seg = robocup.Segment(
robocup.Point(constants.Field.PenaltyLongDist / 2 + offset, 0),
robocup.Point(constants.Field.PenaltyLongDist / 2 + offset,
constants.Field.PenaltyShortDist + offset))
top_seg = robocup.Segment(
robocup.Point(-constants.Field.PenaltyLongDist / 2,
constants.Field.PenaltyShortDist + offset),
robocup.Point(constants.Field.PenaltyLongDist / 2,
constants.Field.PenaltyShortDist + offset))
default_pt = top_seg.center()
if self._block_line is not None:
# main.system_state().draw_line(self._block_line, constants.Colors.White, "SubmissiveDefender")
main.system_state().draw_circle(self._block_line.get_pt(0), 0.1,
constants.Colors.White,
"SubmissiveDefender")
threat_point = self._block_line.get_pt(0)
intersection_center = top_seg.line_intersection(self._block_line)
if threat_point.x < 0:
intersections_left = left_seg.line_intersection(
self._block_line)
if intersections_left is not None:
self._move_target = intersections_left
elif intersection_center is not None:
self._move_target = intersection_center
else:
self._move_target = default_pt
elif threat_point.x >= 0:
intersections_right = right_seg.line_intersection(
self._block_line)
if intersections_right is not None:
self._move_target = intersections_right
elif intersection_center is not None:
self._move_target = intersection_center
else:
self._move_target = default_pt
else:
self._move_target = default_pt
def __init__(
self,
num_defenders=3, # number of defenders we're making the wall with (default 3)
curvature=.3, # 'curvature' (in radians) of the wall
mark_point=None, # what point we are defending against (default is ball)
defender_point=robocup.Point(
0, 0), # what point we are defending (default is goal)
defender_spacing=2.5, # number of robot radii between the centers of the defenders in the wall
dist_from_mark=.75, # distance from the mark point we want to build the wall
defender_priorities=[20, 19, 18, 17,
16]): # default defense priorities
super().__init__(continuous=True)
is_ball_free = lambda: main.ball().vel.mag() < 1 and min([
(main.ball().pos - rob.pos).mag()
for rob in main.system_state().their_robots
]) > min([(main.ball().pos - rob.pos).mag()
for rob in main.system_state().our_robots])
self.mark_moved = False
self.active_defenders = num_defenders
self.number_of_defenders = num_defenders
self.curvature = 1 * curvature
self._mark_point = main.ball(
).pos if mark_point == None else mark_point
self._defense_point = defender_point
self.dist_from_mark = dist_from_mark
self.defender_spacing = defender_spacing
self.defender_priorities = defender_priorities
# Information for movement calculations to reduce redundancy
self.midpoint = None
self.add_state(Wall.State.defense_wall,
behavior.Behavior.State.running)
self.add_state(Wall.State.shot, behavior.Behavior.State.running)
self.add_state(Wall.State.scramble, behavior.Behavior.State.running)
self.add_transition(behavior.Behavior.State.start,
Wall.State.defense_wall, lambda: True,
"immideately")
self.add_transition(Wall.State.defense_wall, Wall.State.shot,
lambda: False, "on shot")
self.add_transition(
Wall.State.defense_wall, Wall.State.scramble,
lambda: evaluation.ball.we_are_closer(
) and evaluation.ball.moving_slow(), "ball free")
self.add_transition(
Wall.State.scramble,
Wall.State.defense_wall, lambda: not evaluation.ball.we_are_closer(
) or not evaluation.ball.moving_slow(), "ball captured")
def block_line(self, value):
self._block_line = value
# we move somewhere along this arc to mark our 'block_line'
offset = constants.Robot.Radius * self._defend_goal_radius
left_seg = robocup.Segment(
robocup.Point(-constants.Field.PenaltyLongDist / 2 - offset, 0),
robocup.Point(-constants.Field.PenaltyLongDist / 2 - offset,
constants.Field.PenaltyShortDist + offset))
right_seg = robocup.Segment(
robocup.Point(constants.Field.PenaltyLongDist / 2 + offset, 0),
robocup.Point(constants.Field.PenaltyLongDist / 2 + offset,
constants.Field.PenaltyShortDist + offset))
top_seg = robocup.Segment(
robocup.Point(-constants.Field.PenaltyLongDist / 2 - offset,
constants.Field.PenaltyShortDist + offset),
robocup.Point(constants.Field.PenaltyLongDist / 2 + offset,
constants.Field.PenaltyShortDist + offset))
default_pt = top_seg.center()
if self._block_line is not None:
# main.system_state().draw_line(self._block_line, constants.Colors.White, "SubmissiveDefender")
main.system_state().draw_circle(
self._block_line.get_pt(0), 0.1, constants.Colors.White,
"SubmissiveDefender")
threat_point = self._block_line.get_pt(0)
intersection_center = top_seg.line_intersection(self._block_line)
if threat_point.x < 0:
intersections_left = left_seg.line_intersection(
self._block_line)
if intersections_left is not None:
self._move_target = intersections_left
elif intersection_center is not None:
self._move_target = intersection_center
else:
self._move_target = default_pt
elif threat_point.x >= 0:
intersections_right = right_seg.line_intersection(
self._block_line)
if intersections_right is not None:
self._move_target = intersections_right
elif intersection_center is not None:
self._move_target = intersection_center
else:
self._move_target = default_pt
else:
self._move_target = default_pt
def facing_opp_goal(self):
robot = self.subbehavior_with_name('aim').robot
if robot is None:
return False
# L is left post
# R is right post
# T is target aiming point
# U is us
# L T R
# \ | /
# \ | /
# \ | /
# \ | /
# \|/
# U
# Angle LUT + Angle RUT should equal Angle LUR if vector UT is between vectors UL and UR
#
#
# L R T
# \ | /
# \ | /
# \ | /
# \ | /
# \|/
# U
# Here, Angle LUT + Angle RUT is much larger than Angle LUR since vector UT is outside vectors UL and UR
left_goal_post = robocup.Point(-constants.Field.GoalWidth / 2, constants.Field.Length)
right_goal_post = robocup.Point(constants.Field.GoalWidth / 2, constants.Field.Length)
bot_to_left_goal_post = left_goal_post - robot.pos
bot_to_right_goal_post = right_goal_post - robot.pos
bot_forward_vector = robot.pos + robocup.Point(math.cos(robot.angle), math.sin(robot.angle))
angle_left_goal_post_diff = bot_forward_vector.angle_between( bot_to_left_goal_post )
angle_right_goal_post_diff = bot_forward_vector.angle_between( bot_to_right_goal_post )
angle_goal_post_diff = bot_to_left_goal_post.angle_between( bot_to_right_goal_post )
# Add a small amount for any errors in these math functions
small_angle_offset = 0.01
# We are aiming at the goal
if (angle_left_goal_post_diff + angle_right_goal_post_diff + small_angle_offset <= angle_goal_post_diff):
print('EARLY KIck')
main.system_state().draw_text('Early kick', robot.pos, 'PivotKick')
return True
return False
def execute_marking(self):
move = self.subbehavior_with_name('move')
move.pos = self.move_target
left_seg = robocup.Segment(
robocup.Point(-constants.Field.PenaltyLongDist / 2, 0),
robocup.Point(-constants.Field.PenaltyLongDist / 2,
constants.Field.PenaltyShortDist))
right_seg = robocup.Segment(
robocup.Point(constants.Field.PenaltyLongDist / 2, 0),
robocup.Point(constants.Field.PenaltyLongDist / 2,
constants.Field.PenaltyShortDist))
top_seg = robocup.Segment(
robocup.Point(-constants.Field.PenaltyLongDist / 2,
constants.Field.PenaltyShortDist),
robocup.Point(constants.Field.PenaltyLongDist / 2,
constants.Field.PenaltyShortDist))
if move.pos is not None:
main.system_state().draw_circle(move.pos, 0.02,
constants.Colors.Green, "Mark")
main.system_state().draw_segment(left_seg, constants.Colors.Green,
"Mark")
main.system_state().draw_segment(top_seg, constants.Colors.Green,
"Mark")
main.system_state().draw_segment(right_seg, constants.Colors.Green,
"Mark")
# make the defender face the threat it's defending against
if (self.robot is not None and self.block_line is not None):
self.robot.face(self.block_line.get_pt(0))
if self.robot.has_ball() and not main.game_state().is_stopped(
) and not self._self_goal(self.robot):
self.robot.kick(0.75)
def execute_marking(self):
move = self.subbehavior_with_name('move')
move.pos = self.move_target
left_seg = robocup.Segment(
robocup.Point(-constants.Field.PenaltyLongDist / 2, 0),
robocup.Point(-constants.Field.PenaltyLongDist / 2,
constants.Field.PenaltyShortDist))
right_seg = robocup.Segment(
robocup.Point(constants.Field.PenaltyLongDist / 2, 0),
robocup.Point(constants.Field.PenaltyLongDist / 2,
constants.Field.PenaltyShortDist))
top_seg = robocup.Segment(
robocup.Point(-constants.Field.PenaltyLongDist / 2,
constants.Field.PenaltyShortDist),
robocup.Point(constants.Field.PenaltyLongDist / 2,
constants.Field.PenaltyShortDist))
if move.pos is not None:
main.system_state().draw_circle(move.pos, 0.02,
constants.Colors.Green, "Mark")
main.system_state().draw_segment(left_seg, constants.Colors.Green,
"Mark")
main.system_state().draw_segment(top_seg, constants.Colors.Green,
"Mark")
main.system_state().draw_segment(right_seg, constants.Colors.Green,
"Mark")
# make the defender face the threat it's defending against
if (self.robot is not None and self.block_line is not None):
self.robot.face(self.block_line.get_pt(0))
if self.robot.has_ball() and not main.game_state().is_stopped() and not self._self_goal(self.robot):
self.robot.kick(0.75)
def execute_marking(self):
move = self.subbehavior_with_name("move")
move.pos = self.move_target
arc_left = robocup.Arc(
robocup.Point(-constants.Field.GoalFlat / 2, 0),
constants.Field.ArcRadius + constants.Robot.Radius * 2,
math.pi / 2,
math.pi,
)
arc_right = robocup.Arc(
robocup.Point(constants.Field.GoalFlat / 2, 0),
constants.Field.ArcRadius + constants.Robot.Radius * 2,
0,
math.pi / 2,
)
seg = robocup.Segment(
robocup.Point(-constants.Field.GoalFlat / 2, constants.Field.ArcRadius + constants.Robot.Radius * 2),
robocup.Point(constants.Field.GoalFlat / 2, constants.Field.ArcRadius + constants.Robot.Radius * 2),
)
if move.pos != None:
main.system_state().draw_circle(move.pos, 0.02, constants.Colors.Green, "Mark")
main.system_state().draw_segment(seg.get_pt(0), seg.get_pt(1), constants.Colors.Green, "Mark")
main.system_state().draw_arc(arc_left, constants.Colors.Green, "Mark")
main.system_state().draw_arc(arc_right, constants.Colors.Green, "Mark")
# make the defender face the threat it's defending against
if self.robot != None and self.block_line != None:
self.robot.face(self.block_line.get_pt(0))
if self.robot.has_ball():
self.robot.kick(0.75)
def execute_running(self):
#pylint: disable=no-member
#Skill does nothing if mark point isn't given AND the ball or robot to mark can't be found
if self.mark_point is None and \
(self.mark_robot is None or
not main.ball().valid or
not self.mark_robot.visible):
return
#Sets the position to mark as the given mark position
#or robot position if no mark position is given
ball_pos = main.ball().pos
pos = self.robot.pos
mark_pos = self.mark_point if self.mark_point is not None else self.mark_robot.pos
#Finds the line from the ball to the mark position and creates a line between them
#removing the overlap with the ball on one side and robot on the other
#This assumes even with mark position parameter that there is a robot there to avoid
mark_line_dir = (ball_pos - mark_pos).normalized()
ball_mark_line = robocup.Segment(
ball_pos - mark_line_dir * constants.Ball.Radius,
mark_pos + mark_line_dir * 2.0 * constants.Robot.Radius)
#Drawing for simulator
main.system_state().draw_line(ball_mark_line, (0, 0, 255), "Mark")
#Distance from robot to mark line
mark_line_dist = ball_mark_line.dist_to(pos)
#Sets target point to nearest point on mark line if the robot is over ball_mark_threshold
#from the mark line
# or
#Sets target point to place on line defined by ratio-
# - 0 being close to ball, 1 close to mark pt
self._target_point = None
if mark_line_dist > self.mark_line_thresh:
self._target_point = ball_mark_line.nearest_point(pos)
else:
self._target_point = ball_pos + (
mark_pos -
ball_pos).normalized() * self.ratio * ball_mark_line.length()
#Drawing for simulator
main.system_state().draw_circle(mark_pos, constants.Robot.Radius * 1.2,
(0, 127, 255), "Mark")
#Move robot into position and face the ball
self.robot.move_to(self._target_point)
self.robot.face(ball_pos)
def execute_running(self):
if self.robot != None and main.ball().valid:
if self.shape_constraint is None:
self.target_pos = self.ball_line().nearest_point(self.robot.pos)
elif isinstance(self.shape_constraint, robocup.Segment):
self.target_pos = self.shape_constraint.segment_intersection(self.ball_line())
if self.target_pos is None:
self.target_pos = self.ball_line().nearest_point(self.robot.pos)
self.target_pos = self.shape_constraint.nearest_point(self.target_pos)
main.system_state().draw_line(self.shape_constraint, (0,255,0), "Debug")
else:
self.target_pos = self.ball_line().nearest_point(self.robot.pos)
self.robot.move_to_direct(self.target_pos)
self.robot.face(main.ball().pos)
def setUp(self): main.set_system_state(self.system_state) for robot in main.system_state().their_robots: robot.set_vis_for_testing(True) self.length = constants.Field.Length self.width = constants.Field.Width self.botRadius = constants.Robot.Radius self.their_robots = main.system_state().their_robots[0:6] self.our_robots = main.system_state().our_robots[0:6] self.success = 1 self.failure = 0
def execute_running(self):
#pylint: disable=no-member
#Skill does nothing if mark point isn't given AND the ball or robot to mark can't be found
if self.mark_point is None and \
(self.mark_robot is None or
not main.ball().valid or
not self.mark_robot.visible):
return
#Sets the position to mark as the given mark position
#or robot position if no mark position is given
ball_pos = main.ball().pos
pos = self.robot.pos
mark_pos = self.mark_point if self.mark_point is not None else self.mark_robot.pos
#Finds the line from the ball to the mark position and creates a line between them
#removing the overlap with the ball on one side and robot on the other
#This assumes even with mark position parameter that there is a robot there to avoid
mark_line_dir = (ball_pos - mark_pos).normalized()
ball_mark_line = robocup.Segment(
ball_pos - mark_line_dir * constants.Ball.Radius,
mark_pos + mark_line_dir * 2.0 * constants.Robot.Radius)
#Drawing for simulator
main.system_state().draw_line(ball_mark_line, (0, 0, 255), "Mark")
#Distance from robot to mark line
mark_line_dist = ball_mark_line.dist_to(pos)
#Sets target point to nearest point on mark line if the robot is over ball_mark_threshold
#from the mark line
# or
#Sets target point to place on line defined by ratio-
# - 0 being close to ball, 1 close to mark pt
self._target_point = None
if mark_line_dist > self.mark_line_thresh:
self._target_point = ball_mark_line.nearest_point(pos)
else:
self._target_point = ball_pos + (mark_pos - ball_pos).normalized(
) * self.ratio * ball_mark_line.length()
#Drawing for simulator
main.system_state().draw_circle(mark_pos, constants.Robot.Radius * 1.2,
(0, 127, 255), "Mark")
#Move robot into position and face the ball
self.robot.move_to(self._target_point)
self.robot.face(ball_pos)
def find_robot_to_block(self):
target = None
for robot in main.system_state().their_robots:
if robot.visible and self._area.contains_point(robot.pos):
if target is None or target.pos.dist_to(main.ball().pos) > robot.pos.dist_to(main.ball().pos):
target = robot
return target
def execute_course_approach(self):
# don't hit the ball on accident
pos = self.find_intercept_point()
if (self.lastApproachTarget != None and
(pos - self.lastApproachTarget).mag() < 0.1):
self.robot.move_to(self.lastApproachTarget)
main.system_state().draw_circle(
self.lastApproachTarget, constants.Ball.Radius,
constants.Colors.White, "TouchBall")
else:
main.system_state().draw_circle(pos, constants.Ball.Radius,
constants.Colors.White,
"TouchBall")
self.robot.move_to(pos)
self.lastApproachTarget = pos
def __init__(self, defender_priorities=[20, 19]):
super().__init__(continuous=True)
# we could make the Defense tactic have more or less defenders, but right now we only support two
if len(defender_priorities) != 2:
raise RuntimeError(
"defender_priorities should have a length of two")
self.add_transition(behavior.Behavior.State.start,
behavior.Behavior.State.running, lambda: True,
"immediately")
goalie = tactics.positions.submissive_goalie.SubmissiveGoalie()
goalie.shell_id = main.root_play().goalie_id
self.add_subbehavior(goalie, "goalie", required=False)
# add defenders at the specified priority levels
for num, priority in enumerate(defender_priorities):
defender = tactics.positions.submissive_defender.SubmissiveDefender(
)
self.add_subbehavior(defender,
'defender' + str(num + 1),
required=False,
priority=priority)
self.debug = True
self.win_eval = robocup.WindowEvaluator(main.system_state())
def __init__(self, side=Side.center):
super().__init__(continuous=True)
self._block_robot = None
self._area = None
self._side = side
self._opponent_avoid_threshold = 2.0
self._defend_goal_radius = 0.9
self._win_eval = robocup.WindowEvaluator(main.system_state())
self._area = robocup.Rect(
robocup.Point(-constants.Field.Width / 2.0,
constants.Field.Length),
robocup.Point(constants.Field.Width / 2.0, 0))
if self._side is Defender.Side.right:
self._area.get_pt(0).x = 0
if self._side is Defender.Side.left:
self._area.get_pt(1).x = 0
self.add_state(Defender.State.marking, behavior.Behavior.State.running)
self.add_state(Defender.State.area_marking,
behavior.Behavior.State.running)
self.add_transition(behavior.Behavior.State.start,
Defender.State.marking, lambda: True,
"immediately")
self.add_transition(
Defender.State.marking, Defender.State.area_marking,
lambda: not self._area.contains_point(main.ball().pos) and self.block_robot is None,
"if ball not in area and no robot to block")
self.add_transition(
Defender.State.area_marking, Defender.State.marking,
lambda: self._area.contains_point(main.ball().pos) or self.find_robot_to_block() is not None,
"if ball or opponent enters my area")
def on_enter_passing(self):
# Do shot evaluation here
win_eval = robocup.WindowEvaluator(main.system_state())
for r in self.to_exclude:
win_eval.add_excluded_robot(r)
_, rob_1_best_shot = win_eval.eval_pt_to_opp_goal(self.passRobot1.pos)
_, rob_1_best_pass = win_eval.eval_pt_to_pt(self.captureRobot.pos, self.passRobot1.pos, 0.3)
rob_1_chance = 0
if rob_1_best_shot and rob_1_best_pass:
rob_1_chance = rob_1_best_pass.shot_success * rob_1_best_shot.shot_success
_, rob_2_best_shot = win_eval.eval_pt_to_opp_goal(self.passRobot2.pos)
_, rob_2_best_pass = win_eval.eval_pt_to_pt(self.captureRobot.pos, self.passRobot2.pos, 0.3)
rob_2_chance = 0
if rob_2_best_shot and rob_2_best_pass:
rob_2_chance = rob_2_best_pass.shot_success * rob_2_best_shot.shot_success
_, direct_shot = win_eval.eval_pt_to_opp_goal(self.captureRobot.pos)
direct_success = 0
if direct_shot:
if self.captureRobot.pos.y < 4:
direct_success = direct_shot.shot_success * 0.7
else:
direct_success = direct_shot.shot_success
if direct_shot and direct_success > rob_1_chance and direct_success > rob_2_chance:
self.kick_directly = True
return
if rob_1_chance > rob_2_chance:
self.add_subbehavior(tactics.coordinated_pass.CoordinatedPass(self.passRobot1.pos), "pass")
else:
self.add_subbehavior(tactics.coordinated_pass.CoordinatedPass(self.passRobot2.pos), "pass")
def find_robot_to_block(self): target = None for robot in main.system_state().their_robots: if robot.visible and self._area.contains_point(robot.pos): if target is None or target.pos.dist_to(main.ball().pos) > robot.pos.dist_to(main.ball().pos): target = robot return target
def __init__(self, side=Side.center):
super().__init__(continuous=True)
self._block_robot = None
self._area = None
self._side = side
self._opponent_avoid_threshold = 2.0
self._defend_goal_radius = 0.9
self._win_eval = robocup.WindowEvaluator(main.system_state())
self._area = robocup.Rect(
robocup.Point(-constants.Field.Width / 2.0,
constants.Field.Length),
robocup.Point(constants.Field.Width / 2.0, 0))
if self._side is Defender.Side.right:
self._area.get_pt(0).x = 0
if self._side is Defender.Side.left:
self._area.get_pt(1).x = 0
self.add_state(Defender.State.marking, behavior.Behavior.State.running)
self.add_state(Defender.State.area_marking,
behavior.Behavior.State.running)
self.add_transition(behavior.Behavior.State.start,
Defender.State.marking, lambda: True,
"immediately")
self.add_transition(
Defender.State.marking, Defender.State.area_marking,
lambda: not self._area.contains_point(main.ball().pos) and self.
block_robot is None, "if ball not in area and no robot to block")
self.add_transition(
Defender.State.area_marking, Defender.State.marking,
lambda: self._area.contains_point(main.ball(
).pos) or self.find_robot_to_block() is not None,
"if ball or opponent enters my area")
def should_clear_ball(self):
if main.game_state().is_stopped():
return False
#Returns true if our robot can reach the ball sooner than the closest opponent
safe_to_clear = False
if main.ball().pos.mag() < constants.Field.ArcRadius * 2 and main.ball(
).vel.mag() < .75 and not evaluation.ball.is_in_our_goalie_zone():
defender1 = self.subbehavior_with_name('defender1')
defender2 = self.subbehavior_with_name('defender2')
if (defender1.robot != None and defender2.robot != None):
max_vel = robocup.MotionConstraints.MaxRobotSpeed.value
max_accel = robocup.MotionConstraints.MaxRobotAccel.value
for robot in main.system_state().their_robots:
their_dist_to_ball = robot.pos.dist_to(main.ball().pos)
#if their robot is moving faster than ours, assume it is at its maximum speed, otherwise assume its max speed is the same as ours
their_max_vel = max(max_vel, robot.vel.mag())
#calculate time for the closest opponent to reach ball based on current /vel/pos data * .9 for safety
their_time_to_ball = (
their_dist_to_ball /
their_max_vel) * defender1.safety_multiplier
if their_time_to_ball > evaluation.ball.time_to_ball(
defender1.robot
) or their_time_to_ball > evaluation.ball.time_to_ball(
defender2.robot):
safe_to_clear = True
return safe_to_clear
def execute_running(self):
super().execute_running()
kicker = self.subbehavior_with_name('kicker')
center1 = self.subbehavior_with_name('center1')
center2 = self.subbehavior_with_name('center2')
# see if we have a direct shot on their goal
win_eval = robocup.WindowEvaluator(main.system_state())
win_eval.enable_chip = kicker.robot != None and kicker.robot.has_chipper(
)
win_eval.min_chip_range = OurGoalKick.MinChipRange
win_eval.max_chip_range = OurGoalKick.MaxChipRange
windows, best = win_eval.eval_pt_to_seg(
main.ball().pos, constants.Field.TheirGoalSegment)
# note: the min length value is tunable
if best != None and best.segment.length() > 0.3:
# we DO have a shot on the goal, take it!
kicker.target = constants.Field.TheirGoalSegment
# FIXME: make the other robots get out of the shot path
center1.target = robocup.Point(0.0, 1.5)
center2.target = robocup.Point(1.0, 1.5)
else:
# no open shot, shoot it in-between the two centers
center_x = constants.Field.Width * 0.15
center_y = constants.Field.Length * 0.6
center1.target = robocup.Point(-center_x, center_y)
center2.target = robocup.Point(center_x, center_y)
kicker.target = robocup.Segment(center1.target, center2.target)
def execute_lineup(self):
target_line = self.target_line()
target_dir = target_line.delta().normalized()
behind_line = robocup.Segment(main.ball().pos - target_dir * (Bump.DriveAroundDist + constants.Robot.Radius),
main.ball().pos - target_dir * 5.0)
if target_line.delta().dot(self.robot.pos - main.ball().pos) > -constants.Robot.Radius:
# we're very close to or in front of the ball
self.robot.set_avoid_ball_radius(Bump.LineupBallAvoidRadius)
self.robot.move_to(main.ball().pos - target_dir * (Bump.DriveAroundDist + constants.Robot.Radius))
else:
self.robot.set_avoid_ball_radius(Bump.LineupBallAvoidRadius)
self.robot.move_to(behind_line.nearest_point(self.robot.pos))
main.system_state().draw_line(behind_line, constants.Colors.Black, "Bump")
delta_facing = self.target - main.ball().pos
self.robot.face(self.robot.pos + delta_facing)
def chippable_robots():
bp = main.ball().pos
return [
rob for rob in main.system_state().their_robots
if (rob.pos - bp).mag() > constants.OurChipping.MIN_CARRY and
(rob.pos - bp).mag() < constants.OurChipping.MAX_CARRY
]
def execute_running(self):
kicker = self.subbehavior_with_name('kicker')
center1 = self.subbehavior_with_name('center1')
center2 = self.subbehavior_with_name('center2')
# see if we have a direct shot on their goal
win_eval = robocup.WindowEvaluator(main.system_state())
win_eval.enable_chip = kicker.robot != None and kicker.robot.has_chipper()
win_eval.min_chip_range = OurGoalKick.MinChipRange
win_eval.max_chip_range = OurGoalKick.MaxChipRange
windows, best = win_eval.eval_pt_to_seg(main.ball().pos, constants.Field.TheirGoalSegment)
# note: the min length value is tunable
if best != None and best.segment.length() > 0.3:
# we DO have a shot on the goal, take it!
kicker.target = constants.Field.TheirGoalSegment
# FIXME: make the other robots get out of the shot path
center1.target = robocup.Point(0.0, 1.5)
center2.target = robocup.Point(1.0, 1.5)
else:
# no open shot, shoot it in-between the two centers
center_x = constants.Field.Width * 0.15
center_y = constants.Field.Length * 0.6
center1.target = robocup.Point(-center_x, center_y)
center2.target = robocup.Point(center_x, center_y)
kicker.target = robocup.Segment(center1.target, center2.target)
def __init__(self, defender_priorities=[20, 19]):
super().__init__(continuous=True)
if len(defender_priorities) != 2:
raise RuntimeError("defender_priorities should have a length of 2")
self.add_state(Defense.State.defending,
behavior.Behavior.State.running)
self.add_state(Defense.State.clearing, behavior.Behavior.State.running)
self.add_transition(behavior.Behavior.State.start,
Defense.State.defending, lambda: True,
"immediately")
self.add_transition(Defense.State.defending, Defense.State.clearing,
lambda: self.should_clear_ball(),
"Clearing the ball")
self.add_transition(Defense.State.clearing, Defense.State.defending,
lambda: not self.should_clear_ball(),
"Done clearing")
goalie = submissive_goalie.SubmissiveGoalie()
goalie.shell_id = main.root_play().goalie_id
self.add_subbehavior(goalie, "goalie", required=False)
# Add the defenders
for num, priority in enumerate(defender_priorities):
defender = submissive_defender.SubmissiveDefender()
self.add_subbehavior(defender,
'defender' + str(num + 1),
required=False,
priority=priority)
self.debug = True
self.kick_eval = robocup.KickEvaluator(main.system_state())
def execute_lineup(self):
target_line = self.target_line()
target_dir = target_line.delta().normalized()
behind_line = robocup.Segment(main.ball().pos - target_dir * (Bump.DriveAroundDist + constants.Robot.Radius),
main.ball().pos - target_dir * 5.0)
if target_line.delta().dot(self.robot.pos - main.ball().pos) > -constants.Robot.Radius:
# we're very close to or in front of the ball
self.robot.set_avoid_ball_radius(Bump.LineupBallAvoidRadius)
self.robot.move_to(main.ball().pos - target_dir * (Bump.DriveAroundDist + constants.Robot.Radius))
else:
self.robot.set_avoid_ball_radius(Bump.LineupBallAvoidRadius)
self.robot.move_to(behind_line.nearest_point(self.robot.pos))
main.system_state().draw_line(behind_line, constants.Colors.Black, "Bump")
delta_facing = self.target - main.ball().pos
self.robot.face(self.robot.pos + delta_facing)
