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_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_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 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 __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())