def add_shot_obstacle(self, excluded_robots=[]): pt = self.aim_target_point if pt != None: # segment centered at the target point that's @width wide and perpendicular to the shot shot_perp = (main.ball().pos - pt).perp_ccw().normalized() # Make the shot triangle obstacle a fixed width at the end unless # we're aiming at a segment. In that case, just use the length of # the segment. width = 0.5 if isinstance(self.target, robocup.Segment): width = self.target.length() a = pt + shot_perp * width / 2.0 b = pt - shot_perp * width / 2.0 # build the obstacle polygon obs = robocup.Polygon() obs.add_vertex(main.ball().pos) obs.add_vertex(a) obs.add_vertex(b) # tell the bots to not go there for bot in main.our_robots(): if bot not in excluded_robots + [self.robot]: bot.add_local_obstacle(obs)
def __init__(self): super().__init__(continuous=True) self.add_transition(behavior.Behavior.State.start, behavior.Behavior.State.running, lambda: True, 'immediately') self.add_transition(behavior.Behavior.State.running, behavior.Behavior.State.completed, lambda: self.all_subbehaviors_completed(), 'all robots reach target positions') self.add_transition(behavior.Behavior.State.completed, behavior.Behavior.State.running, lambda: not self.all_subbehaviors_completed(), "robots aren't lined up") # Define circle to circle up on radius = constants.Field.CenterRadius + constants.Robot.Radius + 0.01 perRobot = (2 * constants.Robot.Radius* 1.25) / radius ball_pos = main.ball().pos if main.ball() != None else robocup.Point(constants.Field.Width / 2, constants.Field.Length / 2) dirvec = (robocup.Point(0,0) - ball_pos).normalized() * radius for i in range(6): pt = ball_pos + dirvec self.add_subbehavior(skills.move.Move(pt), name="robot" + str(i), required=False, priority=6 - i) dirvec.rotate(robocup.Point(0,0), perRobot)
def approach_vector(robot): if main.ball().vel.mag() > 0.25 \ and robot.pos.dist_to(main.ball().pos) > 0.2: # ball's moving, get on the side it's moving towards return main.ball().vel.normalized() else: return (robot.pos - main.ball().pos).normalized()
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 = evaluation.window_evaluator.WindowEvaluator() 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 role_requirements(self): reqs = super().role_requirements() reqs.require_kicking = True # try to be near the ball if main.ball().valid: reqs.destination_shape = main.ball().pos return reqs
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 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_dribbling(self): # Grab best pass self.pass_target, self.pass_score = evaluation.passing_positioning.eval_best_receive_point( main.ball().pos, main.our_robots(), AdaptiveFormation.FIELD_POS_WEIGHTS, AdaptiveFormation.NELDER_MEAD_ARGS, AdaptiveFormation.PASSING_WEIGHTS) # Grab shot chance self.shot_chance = evaluation.shooting.eval_shot(main.ball().pos) # Recalculate dribbler pos self.check_dribbling_timer += 1 if (self.check_dribbling_timer > self.check_dribbling_timer_cutoff): self.check_dribbling_timer = 0 self.dribbler.pos, _ = evaluation.passing_positioning.eval_best_receive_point( main.ball().pos, main.our_robots(), AdaptiveFormation.FIELD_POS_WEIGHTS, AdaptiveFormation.NELDER_MEAD_ARGS, AdaptiveFormation.DRIBBLING_WEIGHTS) # TODO: Get list of top X pass positions and have robots in good positions to reach them # Good positions can be definied by offensive / defensive costs # Offensive positions move onto the ball in the direction of the goal # Defensive cover the center of the field # Setup previous values (Basic complementary filter) c = .8 self.prev_shot_chance = c * self.shot_chance + \ (1 - c) * self.prev_shot_chance self.prev_pass_score = c * self.pass_score + \ (1 - c) * self.prev_pass_score
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_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_running(self): super().execute_running() # abort if we can't see the ball if not main.ball().valid: return ball_pos = main.ball().pos # the closest of their bots to the ball is their kicker their_kicker = min(main.their_robots(), key=lambda opp: opp.pos.dist_to(ball_pos)) # we build an array of OpponentRobots sorted rudimentarily by their threat # Right now, this is (inverse of) distance to the ball * 2 + y position. # Needs tuning/improvement. Right now this is excessively defensive sorted_opponents = sorted( filter(lambda robot: robot != their_kicker, main.their_robots()), key=lambda robot: robot.pos.dist_to(ball_pos) * 2 + robot.pos.y) # Decide what each marking robot should do # @sorted_opponents contains the robots we want to mark by priority # any robot that isn't assigned a mark_robot will move towards the ball for i, mark_i in enumerate(self.marks): if i < len(sorted_opponents): mark_i.mark_robot = sorted_opponents[i]
def execute_running(self): # run subbehaviors num_robots = 0 for b in self.all_subbehaviors(): if b.robot is not None: num_robots+=1 radius = constants.Field.CenterRadius + constants.Robot.Radius + 0.01 perRobot = (2 * constants.Robot.Radius * 1.25) / radius * (180.0 / math.pi) ball_pos = main.ball().pos if main.ball() != None else robocup.Point(constants.Field.Width / 2, constants.Field.Length / 2) dirvec = (robocup.Point(0,0) - ball_pos).normalized() * radius dirvec.rotate(robocup.Point(0,0), -perRobot * ((num_robots - 1) / 2)) for i in range(6): pt = ball_pos + dirvec self.subbehavior_with_name("robot" + str(i)).pos = pt dirvec.rotate(robocup.Point(0,0), perRobot) # set robot attributes for b in self.all_subbehaviors(): if b.robot is not None: b.robot.set_avoid_ball_radius(constants.Field.CenterRadius) b.robot.face(main.ball().pos) b.robot.avoid_all_teammates(True)
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_fine_approach(self): self.robot.disable_avoid_ball() self.robot.set_dribble_speed(Capture.DribbleSpeed) self.robot.face(main.ball().pos) bot2ball = (main.ball().pos - self.robot.pos).normalized() self.robot.set_world_vel(bot2ball * Capture.FineApproachSpeed + main.ball().vel)
def __init__(self): super().__init__(continuous=False) self.add_state(Capture.State.course_approach, behavior.Behavior.State.running) self.add_state(Capture.State.fine_approach, behavior.Behavior.State.running) self.add_transition(behavior.Behavior.State.start, Capture.State.course_approach, lambda: True, 'immediately') self.add_transition(Capture.State.course_approach, Capture.State.fine_approach, lambda: self.bot_in_approach_pos() and main.ball().valid, 'dist to ball < threshold') self.add_transition(Capture.State.fine_approach, behavior.Behavior.State.completed, lambda: self.robot.has_ball(), 'has ball') self.add_transition(Capture.State.fine_approach, Capture.State.course_approach, lambda: main.ball().pos.dist_to(self.robot.pos) > Capture.CourseApproachDist * 1.5 or not main.ball().valid, 'ball ran away')
def __init__(self): super().__init__(continuous=False) self.add_state(TouchBall.State.course_approach, behavior.Behavior.State.running) self.add_state(TouchBall.State.hit_ball, behavior.Behavior.State.running) self.add_transition(behavior.Behavior.State.start, TouchBall.State.course_approach, lambda: True, 'immediately') self.add_transition( TouchBall.State.course_approach, TouchBall.State.hit_ball, lambda: (self.ball_in_front_of_bot()) and main.ball().valid, 'dist to ball < threshold') self.add_transition(TouchBall.State.hit_ball, behavior.Behavior.State.completed, lambda: self.robot.has_ball(), 'has ball') self.add_transition( TouchBall.State.hit_ball, behavior.Behavior.State.failed, lambda: not (self.ball_in_front_of_bot()) and not main.ball().pos, 'ball went into goal') self.lastApproachTarget = None
def __init__(self, faceBall=True): super().__init__(continuous=False) self.add_state(Capture.State.course_approach, behavior.Behavior.State.running) self.add_state(Capture.State.fine_approach, behavior.Behavior.State.running) self.add_transition(behavior.Behavior.State.start, Capture.State.course_approach, lambda: True, "immediately") self.add_transition( Capture.State.course_approach, Capture.State.fine_approach, lambda: (self.bot_in_front_of_ball() or self.bot_near_ball(Capture.CourseApproachDist)) and main.ball().valid, "dist to ball < threshold", ) self.add_transition( Capture.State.fine_approach, behavior.Behavior.State.completed, lambda: self.robot.has_ball(), "has ball" ) self.add_transition( Capture.State.fine_approach, Capture.State.course_approach, lambda: not (self.bot_in_front_of_ball() or self.bot_near_ball(Capture.CourseApproachDist)) and (not self.bot_near_ball(Capture.CourseApproachDist * 1.5) or not main.ball().pos), "ball went into goal", ) self.lastApproachTarget = None self.faceBall = faceBall
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 on_enter_passing(self): # kickFrom = min(self.shooting_points, # key=lambda pt: pt.dist_to(main.ball().pos)) # kickFromIdx = self.shooting_points.index(kickFrom) # kickToIdx = (kickFromIdx + 1) % len(self.shooting_points) # kickToPt = self.shooting_points[kickToIdx] line0 = robocup.Segment( self.shooting_points[0]-robocup.Point(0.5,0) self.shooting_points[0]+robocup.Point(0.5,0)) line1 = robocup.Segment( self.shooting_points[1]-robocup.Point(0.5,0) self.shooting_points[1]+robocup.Point(0.5,0) if shot.eval_shot(main.ball().pos, line0) > shot.eval_shot(main.ball().pos, line1): self.add_subbehavior(tactics.coordinated_pass.CoordinatedPass(self.shooting_points[0]), 'pass') else: self.add_subbehavior(tactics.coordinated_pass.CoordinatedPass(self.shooting_points[1]), 'pass') # kickTo = max(shot.eval_shot(main.ball().pos, self.shooting_points[0]), # shot.eval_shot(main.ball().pos, self.shooting_points[1]), # key=lambda pt: pt.dist_to(main.ball().pos)) # kickToIdx = self.shooting_points.index(kickTo) # self.add_subbehavior(tactics.coordinated_pass.CoordinatedPass(kickToPt), # 'pass') def on_exit_passing(self): self.remove_all_subbehaviors() def on_enter_scoring(self): self.add_subbehavior(tactics.coordinated_pass.CoordinatedPass(min(pt.dist_to(main.ball().pos))), 'pass') def on_exit_winning(self): self.remove_all_subbehaviors()
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 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 time_to_ball(robot): max_vel = robocup.MotionConstraints.MaxRobotSpeed.value max_accel = robocup.MotionConstraints.MaxRobotAccel.value delay = .1 #TODO: tune this better rpos = robot.pos bpos = main.ball().pos #calculate time for self to reach ball using max_vel + a slight delay for capture dist_to_ball = robot.pos.dist_to(main.ball().pos) return (dist_to_ball / max_vel) + delay
def execute_setup(self): penalty_mark = robocup.Point(0, constants.Field.Length - constants.Field.PenaltyDist) backoff = 0.5 if main.ball().pos.near_point(penalty_mark, 0.5): self.robot.move_to(main.ball().pos + (main.ball().pos - robocup.Point(0, constants.Field.Length).normalized()) * backoff) else: self.robot.move_to(penalty_mark - robocup.Point(0, backoff)) self.robot.face(main.ball().pos)
def execute_running(self): # abort if we can't see the ball if not main.ball().valid: return ball_pos = main.ball().pos # the closest of their bots to the ball is their kicker their_kicker = min(main.their_robots(), key=lambda opp: opp.pos.dist_to(ball_pos)) # we build an array of (OpponentRobot, float distToClosestOfOurBots) tuples # we'll use these in a sec to assign our marking robots open_opps_and_dists = [] for opp in main.their_robots(): # ignore their kicker if opp == their_kicker: continue ball_dist = opp.pos.dist_to(ball_pos) # see if the opponent is close enough to the ball for us to care # if it is, we record the closest distance from one of our robots to it if ball_dist < 3.0: # which of our robots is closest to this opponent closest_self_dist = min([bot.pos.dist_to(opp.pos) for bot in main.our_robots()]) open_opps_and_dists.append( (opp, closest_self_dist) ) # Decide what each marking robot should do # @open_opps contains the robots we want to mark (if any) # any robot that isn't assigned a mark_robot will move towards the ball for i, mark_i in enumerate(self.marks): if mark_i.robot != None: if i < len(open_opps_and_dists): # mark the opponent mark_i.mark_robot = open_opps_and_dists[i][0] else: pass # NOTE: the old code ran these motion commands INSTEAD of running the mark command # we could do that, but it'd require removing the subbehavior and re-adding a move or something... # # move towards the ball and face it, but don't get within field center's radius # pos = mark_i.robot.pos # target = pos + (ball_pos - pos).normalized() * (ball_pos.dist_to(pos) - constants.Field.CenterRadius) # mark_i.robot.move(target) # mark_i.face(ball_pos) # tell the marking robots to avoid eachother more than normal for i, mark_i in enumerate(self.marks): for j, mark_j in enumerate(self.marks): if i == j: continue if mark_i.robot != None and mark_j.robot != None: mark_i.robot.set_avoid_teammate_radius(mark_j.robot.shell_id(), 0.5)
def role_requirements(self): reqs = super().role_requirements() # try to be near the ball if main.ball().valid: reqs.destination_shape = main.ball().pos reqs.require_kicking = True if self.use_chipper: reqs.chipper_preference_weight = role_assignment.PreferChipper return reqs
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 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 __init__(self, pos=None): super().__init__(continuous=True) self._shape_constraint = None self.ball_line = lambda: robocup.Segment(main.ball().pos, main.ball().pos + (main.ball().vel.normalized() * 8.)) self.add_transition(behavior.Behavior.State.start, behavior.Behavior.State.running, lambda: True, 'immediately')
def role_requirements(self): reqs = super().role_requirements() reqs.require_kicking = True # try to be near the ball if main.ball().valid: if main.ball().vel.mag() < self.InterceptVelocityThresh: reqs.destination_shape = main.ball().pos else: # TODO Make this less complicated and remove magic numbers reqs.destination_shape = robocup.Segment(main.ball().pos, main.ball().pos + main.ball().vel * 10) return reqs
def execute_delay(self): self.robot.disable_avoid_ball() self.robot.set_dribble_speed(self.dribbler_power) ball_dir = (main.ball().pos - self.robot.pos).normalized() if main.ball().vel.mag() < Capture.DelaySpeed: self.robot.set_world_vel(ball_dir*Capture.DelaySpeed) elif main.ball().vel.mag() < Capture.DelaySpeed: delay_speed = main.ball().vel.mag() * Capture.DelayBallSpeedMultiplier - Capture.DelaySpeed self.robot.set_world_vel(ball_dir*delay_speed) self.robot.face(main.ball().pos)
def __init__(self, indirect=None): super().__init__(continuous=True) # If we are indirect we don't want to shoot directly into the goal gs = main.game_state() if indirect != None: self.indirect = indirect elif main.ball().pos.y > constants.Field.Length / 2.0: self.indirect = gs.is_indirect() else: self.indirect = False self.add_transition(behavior.Behavior.State.start, behavior.Behavior.State.running, lambda: True, 'immediately') # FIXME: this could also be a PivotKick kicker = skills.line_kick.LineKick() # kicker.use_chipper = True kicker.min_chip_range = 0.3 kicker.max_chip_range = 3.0 # This will be reset to something else if indirect on the first iteration kicker.target = constants.Field.TheirGoalSegment # add two 'centers' that just move to fixed points center1 = skills.move.Move(robocup.Point(0, 1.5)) self.add_subbehavior(center1, 'center1', required=False, priority=4) center2 = skills.move.Move(robocup.Point(0, 1.5)) self.add_subbehavior(center2, 'center2', required=False, priority=3) if self.indirect: receive_pt, target_point, probability = evaluation.touchpass_positioning.eval_best_receive_point( main.ball().pos) pass_behavior = tactics.coordinated_pass.CoordinatedPass( receive_pt, None, (kicker, lambda x: True), receiver_required=False, kicker_required=False, prekick_timeout=9) # We don't need to manage this anymore self.add_subbehavior(pass_behavior, 'kicker') kicker.target = receive_pt else: kicker = skills.line_kick.LineKick() kicker.target = constants.Field.TheirGoalSegment self.add_subbehavior(kicker, 'kicker', required=False, priority=5) self.add_transition( behavior.Behavior.State.running, behavior.Behavior.State.completed, lambda: self.subbehavior_with_name('kicker').is_done_running() and self.subbehavior_with_name('kicker').state != tactics.coordinated_pass.CoordinatedPass.State.timeout, 'kicker completes')
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_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 execute_running(self): pt = main.ball().pos seg = constants.Field.OurGoalSegment win_eval = evaluation.window_evaluator.WindowEvaluator() win_eval.debug = True win_eval.eval_pt_to_seg(pt, seg)
def execute_running(self): num_robots = 0 for b in self.all_subbehaviors(): if b.robot is not None: num_robots += 1 #if the number of robots has changed, recreate move behaviors to match new number of robots if (self.num_robots != num_robots): self.num_robots = num_robots self.remove_all_subbehaviors() for i in range(6): self.add_subbehavior(skills.move.Move(), name="robot" + str(i), required=False, priority=6 - i) #assign destinations for the number of robots we have for i, pt in enumerate(self.get_circle_points(num_robots)): self.subbehavior_with_name("robot" + str(i)).pos = pt #unassign destinations from behaviors without robots for i in range(num_robots, 6): self.subbehavior_with_name("robot" + str(i)).pos = None # set robot attributes for b in self.all_subbehaviors(): if b.robot is not None: b.robot.set_avoid_ball_radius(constants.Field.CenterRadius) b.robot.face(main.ball().pos)
def score(cls): gs = main.game_state() if gs.is_ready_state() and gs.is_our_direct() and main.ball().pos.y > ( constants.Field.Length - 1.0): return 1 else: return float("inf")
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 __init__(self): super().__init__(continuous=False) self.probably_held_cnt = 0 self.timeout = 0 self.add_transition(behavior.Behavior.State.start, behavior.Behavior.State.running, lambda: True, 'immediately') # Restart the collect path planner # Since there is no definition of which direction a robot is facing # We cannot figure out if the ball is directly behind us or in it's mouth # We can restart the collect and it'll try it again # Only restart when the ball is close, both are stopped, and we dont have the ball in the mouth self.add_transition( behavior.Behavior.State.running, behavior.Behavior.State.start, lambda: self.is_bot_ball_stopped() and not evaluation.ball. robot_has_ball(self.robot) and self.probably_held_cnt < Collect. PROBABLY_HELD_CUTOFF and self.timeout == 0, 'restart') # Complete when we have the ball self.add_transition( behavior.Behavior.State.running, behavior.Behavior.State.completed, lambda: self.robot is not None and evaluation.ball.robot_has_ball( self.robot) and self.probably_held_cnt > Collect. PROBABLY_HELD_CUTOFF, 'ball collected') # Go back if we loose the ball self.add_transition( behavior.Behavior.State.completed, behavior.Behavior.State.running, lambda: self.robot is not None and ((self.robot.pos - main.ball( ).pos).mag() > Collect.RESTART_MIN_DIST or self.probably_held_cnt < Collect.PROBABLY_HELD_CUTOFF), 'ball lost')
def num_on_offense(): # Complementary filter based on... # Distance to their goal # Distance to the ball goal_loc = robocup.Point(0, constants.Field.Length) corner_loc = robocup.Point(constants.Field.Width / 2, 0) ball_loc = main.ball().pos max_goal_dis = (goal_loc - corner_loc).mag() ball_to_goal = (goal_loc - ball_loc).mag() offense_ctr = 0 filter_coeff = 0.7 score_cutoff = .3 # For each of their robots for bot in main.their_robots(): if bot.visible: dist_to_ball = (bot.pos - ball_loc).mag() dist_to_goal = (bot.pos - goal_loc).mag() goal_coeff = dist_to_goal / max_goal_dis if ball_to_goal != 0: ball_coeff = 1 - (dist_to_ball / ball_to_goal) else: ball_coeff = 1 ball_coeff = max(0, ball_coeff * ball_coeff) score = filter_coeff * goal_coeff + (1 - filter_coeff) * ball_coeff # Only count if their score is above the cutoff if (score > score_cutoff): offense_ctr += 1 return offense_ctr
def reset_receive_point(self): angle_receive = skills.angle_receive.AngleReceive() pass_bhvr = self.subbehavior_with_name('pass') receive_pt, target_point, probability = OneTouchPass.tpass.eval_best_receive_point( main.ball().pos, None, pass_bhvr.get_robots()) # only change if increase of beyond the threshold. if self.force_reevauation == True or pass_bhvr.receive_point == None or pass_bhvr.target_point == None \ or probability > OneTouchPass.tpass.eval_single_point(main.ball().pos, pass_bhvr.receive_point, ignore_robots=pass_bhvr.get_robots()) \ + OneTouchPass.receivePointChangeThreshold: pass_bhvr.receive_point = receive_pt angle_receive.target_point = target_point self.force_reevauation = False pass_bhvr.skillreceiver = angle_receive
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 is_goalie_close(self): # if any of there robots are in near chip range chip over them close_check = False for r in main.their_robots(): close_check = close_check or ( r.pos - main.ball().pos).mag() < self.goalie_range return close_check
def bot_in_approach_pos(self): bot2ball = main.ball().pos - self.robot.pos ball2bot = bot2ball * -1 approach_vec = self.approach_vector() return (ball2bot.normalized().dot(approach_vec) > Capture.CourseApproachErrorThresh and bot2ball.mag() < Capture.CourseApproachDist * 1.5)
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 goto_center(self): num_robots = 0 for b in self.all_subbehaviors(): if b.robot is not None: num_robots += 1 num_robots = max(self.min_robots, num_robots) radius = constants.Field.CenterRadius + constants.Robot.Radius + 0.01 perRobot = math.pi / max(num_robots, 1) ball_pos = robocup.Point(0, constants.Field.Length / 2) dirvec = (robocup.Point(0, 0) - ball_pos).normalized() * radius dirvec.rotate(robocup.Point(0, 0), -perRobot * ((num_robots - 1) / 2)) for i in range(6): pt = ball_pos + dirvec self.subbehavior_with_name("robot" + str(i)).pos = pt dirvec.rotate(robocup.Point(0, 0), perRobot) # set robot attributes for b in self.all_subbehaviors(): if b.robot is not None: b.robot.set_avoid_ball_radius(constants.Field.CenterRadius) b.robot.face(main.ball().pos)
def __init__(self, num_defenders = 3, # number of defenders we're making the wall with (default 3) curvature = 0, # '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 = 3.5, # number of robot radii between the centers of the defenders in the wall dist_from_mark = 1, # 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) 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_transition(behavior.Behavior.State.start, Wall.State.defense_wall, lambda: True, "immideately")
def __init__(self): super().__init__(continuous=False) self.pause_time = 0 for substate in OurPlacement.State: self.add_state(substate, behavior.Behavior.State.running) self.add_transition(behavior.Behavior.State.start, OurPlacement.State.dribble, lambda: True, 'immediately') #place the ball in the target area self.add_transition( OurPlacement.State.dribble, OurPlacement.State.pause, lambda: self.subbehavior_with_name('dribble').state == behavior. Behavior.State.completed, 'finished dribbling') self.add_transition(OurPlacement.State.pause, OurPlacement.State.avoid, lambda: time.time() - self.pause_time >= .5, 'paused for .5 seconds') #if the ball comes out of the target area, put it back self.add_transition( OurPlacement.State.avoid, OurPlacement.State.dribble, lambda: (main.ball().pos - main.game_state().get_ball_placement_point() ).mag() > 0.1, 'ball moved out of target area')
def on_enter_running(self): kicker = self.skillkicker[0] kicker.target = self.receive_point kickpower = (main.ball().pos - self.receive_point).mag() / 17 kickpower = max(0.05, min(kickpower, 1.0)) # Very simple tuning right now # It is setup to use kickerpower once the distance scale has been # tuned correctly kicker.kick_power = 0.6 #kickpower kicker.enable_kick = False # we'll re-enable kick once both bots are ready # we use tighter error thresholds because passing is hard kicker.aim_params['error_threshold'] = 0.1 kicker.aim_params['max_steady_ang_vel'] = 0.1 kicker.aim_params['min_steady_duration'] = 0.15 kicker.aim_params['desperate_timeout'] = 2.0 self.add_subbehavior(kicker, 'kicker', required=self.kicker_required, priority=5) receiver = self.skillreceiver receiver.receive_point = self.receive_point self.add_subbehavior(receiver, 'receiver', required=self.receiver_required, priority=5)
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 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_setup(self): penalty_mark = robocup.Point( 0, constants.Field.Length - constants.Field.PenaltyDist) backoff = 0.5 self.robot.disable_avoid_ball() self.robot.move_to(main.ball().pos - robocup.Point(0, backoff)) # FIXME this is old code to stick to the penalty area if possible. # Now we just track the ball. Find out if this is a good idea. # if main.ball().pos.near_point(penalty_mark, 0.5): # self.robot.move_to(main.ball().pos - robocup.Point(0, backoff)) # else: # self.robot.move_to(penalty_mark - robocup.Point(0, backoff)) self.robot.face(main.ball().pos)
def goto_center(self): num_robots = 0 for b in self.all_subbehaviors(): if b.robot is not None: num_robots += 1 #if the number of robots has changed, recreate move behaviors to match new number of robots if (self.num_robots != num_robots): self.num_robots = num_robots self.add_circle_subbehaviors() num_robots = max(self.min_robots, num_robots) radius = constants.Field.CenterRadius + constants.Robot.Radius + 0.01 perRobot = math.pi / max(num_robots, 1) ball_pos = robocup.Point(0, constants.Field.Length / 2) dirvec = (robocup.Point(0, 0) - ball_pos).normalized() * radius dirvec.rotate(robocup.Point(0, 0), -perRobot * ((num_robots - 1) / 2)) #assign points to the behaviors with robots for i in range(num_robots): pt = ball_pos + dirvec self.subbehavior_with_name("robot" + str(i)).pos = pt dirvec.rotate(robocup.Point(0, 0), perRobot) # set robot attributes for b in self.all_subbehaviors(): if b.robot is not None: b.robot.set_avoid_ball_radius(constants.Field.CenterRadius) b.robot.face(main.ball().pos)
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 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 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 facing_err_above_threshold(self): dirVec = robocup.Point(math.cos(self.robot.angle), math.sin(self.robot.angle)) facing_thresh = math.cos(Bump.FacingThresh) facing_err = dirVec.dot((self.target - main.ball().pos).normalized()) # NOTE: yes, the comparator is backwards, that's the way it was in the c++ one... # TODO: remove the above note and clarify what's going on return facing_err < facing_thresh
def find_intercept_point(self): approach_vec = self.approach_vector() # sample every 5 cm in the -approach_vector direction from the ball pos = None for i in range(50): dist = i * 0.05 pos = main.ball().pos + approach_vec * dist ball_time = evaluation.ball.rev_predict(main.ball().vel, dist - Capture.CourseApproachDist) # how long will it take the ball to get there bot_time = (pos - self.robot.pos).mag() * 10.0 # FIXME: evaluate trapezoid # print('bot: ' + str(bot_time) + ';; ball: ' + str(ball_time)) if bot_time < ball_time: break return pos
def should_clear_from_dribble(self): # If outside clear zone if (self.dribbler.pos.y > AdaptiveFormation.CLEAR_FIELD_CUTOFF): return False # If pass chances are getting better, hold off if (self.prev_pass_score + AdaptiveFormation.INCREASING_CHANCE_CUTOFF < self.pass_score): return False # TODO: See if there is space to dribble closest_distance = (evaluation.opponent.get_closest_opponent( main.ball().pos, 1).pos - main.ball().pos).mag() if (closest_distance > AdaptiveFormation.CLEAR_DISTANCE_CUTOFF): return False return True
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_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 on_enter_move(self): self.move_pos = self.calc_move_pos() self.pos_up_field = robocup.Point(main.ball().pos.x, constants.Field.Length * .75) if (main.ball().pos.y > constants.Field.Length / 2): sign = (main.ball().pos.x) / abs(main.ball().pos.x) * -1 x = sign * constants.Field.Width * 3 / 8 y = max(constants.Field.Length * .75, (main.ball().pos.y + constants.Field.Length) * 0.5) self.pos_up_field = robocup.Point(x, y) if self.indirect and (self.receive_value == 0 and len(main.our_robots()) >= 5): self.add_subbehavior(skills.move.Move(self.pos_up_field), 'receiver', required=False, priority=5)