add penalty cycle to .proto, starter_setplay is done

Author: SoroushMazloum

idl/service.proto

@@ -135,6 +135,7 @@ message PenaltyKickState {
   int32 our_score = 5;
   int32 their_score = 6;
   bool is_kick_taker = 7;
+  int32 cycle = 8;
 }
 
 /**

src/behaviors/starter_setplay/bhv_starter_setplay.py

@@ -0,0 +1,269 @@
+from src.interfaces.IBehavior import IBehavior
+from src.interfaces.IAgent import IAgent
+import math
+from service_pb2 import *
+from src.behaviors.starter_setplay.bhv_starter_setplay_kickoff import *
+from src.behaviors.starter_setplay.bhv_starter_their_goal_kick_move import *
+from src.behaviors.starter_setplay.bhv_starter_setplay_freekick import *
+from src.behaviors.starter_setplay.bhv_starter_setplay_goal_kick import *
+from src.behaviors.starter_setplay.bhv_starter_setplay_kickin import *
+from src.behaviors.starter_setplay.bhv_starter_setplay_indirect_freekick import *
+from pyrusgeom.vector_2d import Vector2D
+from pyrusgeom.segment_2d import Segment2D
+from pyrusgeom.circle_2d import Circle2D
+from src.utils.convertor import Convertor
+from src.strategy.starter_strategy import StarterStrategy
+class BhvStarterSetPlay(IBehavior):
+    def __init__(self):
+        pass
+
+    def execute(self, agent: IAgent):
+        actions = BhvStarterSetPlay.decision(agent)
+        for i in actions:
+            if not i == None:
+               agent.add_action(i) 
+               
+    def decision(agent: IAgent):
+        wm = agent.wm
+        if wm.self.is_goalie:
+            if wm.game_mode_type != GameModeType.BackPass_ and wm.game_mode_type != GameModeType.IndFreeKick_:
+                return [PlayerAction(bhv_goalie_free_kick=bhv_goalieFreeKick())] #TODO
+                #return BhvSetPlayGoalKick.execute(agent) #TODO GoalieFreeKick
+            else:
+                return BhvStarterSetPlayIndirectFreeKick.execute(agent)
+            return []
+
+        if wm.game_mode_type == GameModeType.KickOff_:
+            if wm.game_mode_side == wm.our_side:
+                return BhvStarterSetPlayKickOff.execute(agent)
+            else:
+                return BhvStarterSetPlay.doBasicTheirSetPlayMove(agent)
+
+
+        if wm.game_mode_type in [GameModeType.KickIn_, GameModeType.CornerKick_]:
+            if wm.game_mode_side == wm.our_side:
+                return BhvStarterSetPlayKickIn.execute(agent)
+            else:
+                return BhvStarterSetPlay.doBasicTheirSetPlayMove(agent)
+
+        if wm.game_mode_type == GameModeType.GoalKick_:
+            if wm.game_mode_side == wm.our_side:
+                return BhvStarterSetPlayGoalKick.execute(agent)
+            else:
+                return BhvStarterTheirGoalKickMove.execute(agent)
+
+        if wm.game_mode_type in [GameModeType.BackPass_, GameModeType.IndFreeKick_]:
+            return BhvStarterSetPlayIndirectFreeKick.execute(agent)
+
+        if wm.game_mode_type in [GameModeType.FoulCharge_, GameModeType.FoulPush_]:
+            if (wm.ball.position.x < agent.server_params.our_penalty_area_line_x + 1.0 and abs(wm.ball.position.y) < agent.server_params.penalty_area_half_width + 1.0):
+                return BhvStarterSetPlayIndirectFreeKick.execute(agent)
+            elif (wm.ball.position.x > agent.server_params.their_penalty_area_line_x - 1.0 and
+                  abs(wm.ball.position.y) < agent.server_params.penalty_area_half_width + 1.0):
+                return BhvStarterSetPlayIndirectFreeKick.execute(agent)
+
+        if wm.is_our_set_play:
+            return BhvStarterSetPlayFreeKick.execute(agent)
+        else:
+            return BhvStarterSetPlay.doBasicTheirSetPlayMove(agent)
+
+        
+
+    def get_set_play_dash_power(agent: IAgent):
+        wm = agent.wm
+        if not wm.is_our_set_play:
+            target_point = StarterStrategy.get_position(agent, wm.self.uniform_number)
+            if target_point.x > wm.self.position.x:
+                if (wm.ball.position.x < -30.0 and
+                        target_point.x < wm.ball.position.x):
+                    return wm.self.get_safety_dash_power
+                rate = 0.0
+                if wm.self.stamina > agent.server_params.stamina_max * 0.8:
+                    rate = 1.5 * wm.self.stamina / agent.server_params.stamina_max
+                else:
+                    rate = 0.9 * (wm.self.stamina -  agent.server_params.recover_dec_thr) /  agent.server_params.stamina_max
+                    rate = max(0.0, rate)
+                return (agent.player_types[wm.self.id].stamina_inc_max * wm.self.recovery * rate)
+        return wm.self.get_safety_dash_power
+
+    def can_go_to(agent: IAgent, count, wm, ball_circle: Circle2D, target_point:Vector2D) -> bool:
+        wm = agent.wm
+        self_position = Vector2D(wm.self.position.x, wm.self.position.y)
+        move_line = Segment2D(self_position, target_point)
+        n_intersection = ball_circle.intersection(move_line)
+
+        num = str(count)
+
+        if n_intersection == 0:
+            return True
+        
+        if n_intersection == 1:
+            angle = Vector2D(target_point - self_position).th()
+            if abs(angle - wm.ball.angle_from_self) > 80.0:
+                return True
+        return False
+
+    def get_avoid_circle_point(wm, target_point,agent:IAgent):
+        SP = agent.server_params
+        wm = agent.wm
+        avoid_radius = SP.center_circle_r + agent.player_types[wm.self.id].player_size
+        ball_position = Vector2D(wm.ball.position.x, wm.ball.position.y)
+        ball_circle = Circle2D(ball_position, avoid_radius)
+        if BhvStarterSetPlay.can_go_to(agent,-1, wm, ball_circle, target_point):
+            return target_point
+        self_position = Vector2D(wm.self.position.x, wm.self.position.y)
+        target_angle = Vector2D(target_point - self_position).th()
+        ball_target_angle = Vector2D(target_point - ball_position).th()
+        ball_ang = AngleDeg(wm.ball.angle_from_self)
+        ball_is_left = ball_ang.is_left_of(target_angle)
+        ANGLE_DIVS = 6
+        subtargets = []
+        angle_step = 1 if ball_is_left else -1
+        count = 0
+        a = angle_step
+        for i in range(1, ANGLE_DIVS):
+            angle = ball_target_angle + (180.0 / ANGLE_DIVS) * a
+            new_target = Vector2D(ball_position + Vector2D.from_polar(avoid_radius + 1.0, angle))
+
+            if abs(new_target.x()) > SP.pitch_half_length + SP.pitch_margin - 1.0 or abs(new_target.y()) > SP.pitch_half_width + SP.pitch_margin - 1.0: #TODO pith_margin
+                break
+            if BhvStarterSetPlay.can_go_to(agent, count, wm, ball_circle, new_target):
+                return new_target
+            a += angle_step
+            count += 1
+        a = -angle_step
+        for i in range(1, ANGLE_DIVS * 2):
+            angle = ball_target_angle + (180.0 / ANGLE_DIVS) * a
+            new_target = Vector2D(ball_position + Vector2D.from_polar(avoid_radius + 1.0, angle))
+
+            if abs(new_target.x()) > SP.pitch_half_length + SP.pitch_margin - 1.0 or abs(new_target.y()) > SP.pitch_half_width + SP.pitch_margin - 1.0:
+                break
+            if BhvStarterSetPlay.can_go_to(agent, count, wm, ball_circle, new_target):
+                return new_target
+            a -= angle_step
+            count += 1
+        return target_point
+
+    def is_kicker(agent: IAgent):
+        wm = agent.wm
+        min_dist = 10000.0
+        unum = 0
+        ball_position = Vector2D(wm.ball.position.x, wm.ball.position.y)
+        for i in range(1, 12):
+            if i == wm.our_goalie_uniform_number and wm.game_mode_type == GameModeType.GoalieCatch_:
+                h_p:RpcVector2D = wm.teammates[wm.our_goalie_uniform_number - 1].position
+            elif i == wm.our_goalie_uniform_number:
+                continue
+            else:
+                h_p:RpcVector2D = StarterStrategy.get_position(agent, i)
+            home_pos = Vector2D(h_p.x, h_p.y)
+            if(home_pos.dist(ball_position) < min_dist):
+                min_dist = home_pos.dist(ball_position)
+                unum = i
+        if wm.self.uniform_number == unum:
+            return True
+        return False   
+        '''teammates_from_ball = Tools.TeammatesFromBall(agent)
+        wm = agent.wm
+        ball_position = Vector2D(wm.ball.position.x, wm.ball.position.y)
+        if wm.game_mode_type == GameModeType.GoalieCatch_ and wm.game_mode_side == wm.our_side and not wm.self.is_goalie:
+            return False
+        kicker_unum = 0
+        min_dist2 = 100000.0
+        second_kicker_unum = 0
+        second_min_dist2 = 100000.0
+        for unum in range(1, 12):
+            if unum == wm.our_goalie_uniform_number:
+                continue
+            h_p:RpcVector2D = Strategy.get_home_pos(agent, unum)
+            home_pos = Vector2D(h_p.x, h_p.y)
+            if not home_pos.is_valid():
+                continue
+            d2 = home_pos.dist2(ball_position)
+            if d2 < second_min_dist2:
+                second_kicker_unum = unum
+                second_min_dist2 = d2
+                if second_min_dist2 < min_dist2:
+                    second_kicker_unum, kicker_unum = kicker_unum, second_kicker_unum
+                    second_min_dist2, min_dist2 = min_dist2, second_min_dist2
+
+        kicker = None
+        second_kicker = None
+        if kicker_unum != 0:
+            print ('unum', kicker_unum)
+            kicker = wm.teammates[kicker_unum]
+        if second_kicker_unum != 0:
+            second_kicker = wm.teammates[second_kicker_unum]
+        if not kicker:
+            if teammates_from_ball and teammates_from_ball[0].dist_from_ball < wm.ball.dist_from_self * 0.9:
+                return False
+
+            return True
+        print('kicker unum: ', kicker.uniform_number)
+        print('second is_kicker', second_kicker.uniform_number)
+        if kicker and second_kicker and (kicker.uniform_number == wm.self.uniform_number or second_kicker.uniform_number == wm.self.uniform_number):
+            if math.sqrt(min_dist2) < math.sqrt(second_min_dist2) * 0.95:
+                return kicker.uniform_number == wm.self.uniform_number
+            elif kicker.dist_from_ball < second_kicker.dist_from_ball * 0.95:
+                return kicker.uniform_number == wm.self.uniform_number
+            elif second_kicker.dist_from_ball < kicker.dist_from_ball * 0.95:
+                return second_kicker.uniform_number == wm.self.uniform_number
+            
+            elif teammates_from_ball and teammates_from_ball[0].dist_from_ball < wm.self.dist_from_ball * 0.95:
+                return False
+            else:
+                return True
+        return kicker.uniform_number == wm.self.uniform_number'''
+
+    def is_delaying_tactics_situation(agent: IAgent):
+        wm = agent.wm
+        real_set_play_count = wm.cycle - wm.last_set_play_start_time
+        wait_buf = 15 if wm.game_mode_type == GameModeType.GoalKick_ else 2
+        if real_set_play_count >= agent.server_params.drop_ball_time - wait_buf:
+            return False
+        our_score = wm.left_team_score if wm.our_side == Side.LEFT else wm.right_team_score
+        opp_score = wm.right_team_score if wm.our_side == Side.LEFT else wm.left_team_score
+        '''if wm.audioMemory().recoveryTime().cycle >= wm.cycle - 10:
+            if our_score > opp_score:
+                return True''' #TODO audio memory
+        cycle_thr = max(0, agent.server_params.nr_normal_halfs * (agent.server_params.half_time * 10) - 500)
+        if wm.cycle < cycle_thr:
+            return False
+        if our_score > opp_score and our_score - opp_score <= 1:
+            return True
+        return False
+
+    def doBasicTheirSetPlayMove(agent: IAgent):
+        wm = agent.wm
+        target = StarterStrategy.get_position(agent, wm.self.uniform_number)
+        target_point = Vector2D(target.x, target.y)
+        ball_position = Vector2D(wm.ball.position.x, wm.ball.position.y)
+        dash_power = BhvStarterSetPlay.get_set_play_dash_power(agent)
+        ball_to_target = Vector2D(target_point - ball_position)
+        if ball_to_target.r() < 11.0:
+            xdiff = math.sqrt(math.pow(11.0, 2) - math.pow(ball_to_target.y(), 2))
+            target_point.set_x(wm.ball.position.x - xdiff)
+            if target_point.x() < -45.0:
+                target_point = ball_position
+                target_point += ball_to_target.set_length_vector(11.0)
+                
+        if wm.game_mode_type == GameModeType.KickOff_ and agent.server_params.kickoff_offside:
+            target_point.set_x(min(-1.0e-5, target_point.x()))
+
+        adjusted_point = BhvStarterSetPlay.get_avoid_circle_point(wm, target_point,agent)
+        dist_thr = wm.ball.dist_from_self * 0.1
+        if dist_thr < 0.7:
+            dist_thr = 0.7
+        self_velocity = Vector2D(wm.self.velocity.x, wm.self.velocity.y)
+        self_position = Vector2D(wm.self.position.x, wm.self.position.y)
+        if adjusted_point != target_point and ball_position.dist(target_point) > 10.0 and Tools.inertia_final_point(agent.player_types[wm.self.id], self_position, self_velocity).dist(adjusted_point) < dist_thr:
+            adjusted_point = target_point
+        actions = []
+        actions.append(PlayerAction(body_go_to_point=Body_GoToPoint(target_point=Convertor.convert_vector2d_to_rpc_vector2d(adjusted_point), distance_threshold=dist_thr, max_dash_power=dash_power)))
+        body_angle = wm.ball.angle_from_self
+        if body_angle < 0.0:
+            body_angle -= 90.0
+        else:
+            body_angle += 90.0
+            actions.append(PlayerAction(body_turn_to_angle=Body_TurnToAngle(angle=body_angle)))
+        return actions

src/behaviors/starter_setplay/bhv_starter_setplay_kickoff.py

@@ -0,0 +1,118 @@
+from src.interfaces.IAgent import IAgent
+#from src.setplay.BhvGoToPlacedBall import BhvGoToPlacedBall
+#from src.setplay.BhvSetPlay import BhvSetPlay
+from service_pb2 import *
+from pyrusgeom.vector_2d import Vector2D
+from src.utils.tools import Tools
+#from src.setplay.BhvGoToPlacedBall import BhvGoToPlacedBall
+from src.strategy.starter_strategy import StarterStrategy
+from src.utils.convertor import Convertor
+
+class BhvStarterSetPlayKickOff:
+    def __init__(self):
+        pass
+    
+    def execute(agent: IAgent):
+        wm = agent.wm
+        teammates = Tools.TeammatesFromBall(agent)
+
+        if not teammates or teammates[0].dist_from_self > wm.self.dist_from_ball:
+            return BhvStarterSetPlayKickOff.do_kick(agent)
+        else:
+            return BhvStarterSetPlayKickOff.do_move(agent)
+
+        return []
+
+    def do_kick(agent: IAgent):
+        # Go to the ball position
+        actions = []
+        from src.behaviors.starter_setplay.bhv_starter_go_to_placed_ball import BhvStarterGoToPlacedBall
+        actions += BhvStarterGoToPlacedBall(0.0).execute(agent)
+        
+        # Wait
+        actions += BhvStarterSetPlayKickOff.do_kick_wait(agent)
+            
+        
+        # Kick
+        wm = agent.wm
+        max_ball_speed = agent.server_params.max_power * wm.self.kick_rate
+
+        target_point = Vector2D()
+        ball_speed = max_ball_speed
+
+        # Teammate not found
+        if not Tools.TeammatesFromSelf(agent):
+            target_point.assign(agent.server_params.pitch_half_length, (-1 + 2 * (wm.cycle % 2)) * 0.8 * agent.server_params.goal_width / 2)
+        else:
+            teammate = Tools.TeammatesFromSelf(agent)[0]
+            dist = teammate.dist_from_self
+
+            if dist > 35.0:
+                # Too far
+                target_point.assign(agent.server_params.pitch_half_length, (-1 + 2 * (wm.cycle % 2)) * 0.8 * agent.server_params.goal_width)
+            else:
+                target_point = Vector2D(teammate.inertia_final_point.x, teammate.inertia_final_point.y)
+                #target_point = teammate.inertia_final_point
+                ball_speed = min(max_ball_speed,
+                                 Tools.calc_first_term_geom_series_last(1.8, dist, agent.server_params.ball_decay))
+
+        ball_position = Vector2D(wm.ball.position.x, wm.ball.position.y)
+        ball_vel = Vector2D.polar2vector(ball_speed, Vector2D(target_point - ball_position).th())
+        ball_next = ball_position + ball_vel
+        self_position = Vector2D(wm.self.position.x, wm.self.position.y)
+        while self_position.dist(ball_next) < agent.player_types[agent.wm.self.id].kickable_area + 0.2:
+            ball_vel.set_length(ball_speed + 0.1)
+            ball_speed += 0.1
+            ball_next = ball_position + ball_vel
+
+        ball_speed = min(ball_speed, max_ball_speed)
+
+
+        # Enforce one step kick
+        actions.append(PlayerAction(body_smart_kick=Body_SmartKick(target_point=Convertor.convert_vector2d_to_rpc_vector2d(target_point), first_speed=ball_speed, first_speed_threshold=ball_speed * 0.96, max_steps=1)))
+        return actions
+        
+    def do_kick_wait(agent: IAgent) -> bool:
+        actions = []
+        wm = agent.wm
+        real_set_play_count = int(wm.cycle - wm.last_set_play_start_time)
+
+        if real_set_play_count >= agent.server_params.drop_ball_time - 5:
+            return []
+        from src.behaviors.starter_setplay.bhv_starter_setplay import BhvStarterSetPlay
+        if BhvStarterSetPlay.is_delaying_tactics_situation(agent):
+            actions.append(PlayerAction(body_turn_to_angle=Body_TurnToAngle(angle=180)))
+            return actions
+            
+        if abs(wm.self.body_direction) < 175.0:
+            actions.append(PlayerAction(body_turn_to_angle=Body_TurnToAngle(angle=180)))
+            return actions
+
+        if not Tools.TeammatesFromBall(agent):
+            actions.append(PlayerAction(body_turn_to_angle=Body_TurnToAngle(angle=180)))
+            return actions
+
+        if len(Tools.TeammatesFromSelf(agent)) < 9:
+            actions.append(PlayerAction(body_turn_to_angle=Body_TurnToAngle(angle=180)))
+            return actions
+
+        if wm.see_time != wm.cycle or wm.self.stamina < agent.server_params.stamina_max * 0.9:
+            actions.append(PlayerAction(body_turn_to_angle=Body_TurnToAngle(angle=180)))
+            return actions
+        return actions
+
+    def do_move(agent: IAgent):
+        wm = agent.wm
+        actions = []
+        target = StarterStrategy.get_position(agent, wm.self.uniform_number)
+        target_point = Vector2D(target.x, target.y)
+        target_point.set_x(min(-0.5, target_point.x()))
+        from src.behaviors.starter_setplay.bhv_starter_setplay import BhvStarterSetPlay
+        dash_power = BhvStarterSetPlay.get_set_play_dash_power(agent)
+        dist_thr = wm.ball.dist_from_self * 0.07
+        if dist_thr < 1.0:
+            dist_thr = 1.0
+        actions.append(PlayerAction(body_go_to_point=Body_GoToPoint(target_point=Convertor.convert_vector2d_to_rpc_vector2d(target_point), distance_threshold=dist_thr, max_dash_power=dash_power)))
+        actions.append(PlayerAction(body_turn_to_ball=Body_TurnToBall(cycle=1)))
+        
+        return actions