Init gym

agent/Agent.py

@@ -0,0 +1,306 @@
+from agent.Base_Agent import Base_Agent
+from math_ops.Math_Ops import Math_Ops as M
+import math
+import numpy as np
+
+
+class Agent(Base_Agent):
+    def __init__(self, host:str, agent_port:int, monitor_port:int, unum:int,
+                 team_name:str, enable_log, enable_draw, wait_for_server=True, is_fat_proxy=False) -> None:
+        
+        # define robot type
+        robot_type = (1,1,1,1,2,3,3,3,4,4,4)[unum-1]
+
+        # Initialize base agent
+        # Args: Server IP, Agent Port, Monitor Port, Uniform No., Robot Type, Team Name, Enable Log, Enable Draw, play mode correction, Wait for Server, Hear Callback
+        super().__init__(host, agent_port, monitor_port, unum, robot_type, team_name, enable_log, enable_draw, True, wait_for_server, None)
+
+        self.enable_draw = enable_draw
+        self.state = 0  # 0-Normal, 1-Getting up, 2-Kicking
+        self.kick_direction = 0
+        self.kick_distance = 0
+        self.fat_proxy_cmd = "" if is_fat_proxy else None
+        self.fat_proxy_walk = np.zeros(3) # filtered walk parameters for fat proxy
+
+        self.init_pos = ([-14,0],[-9,-5],[-9,0],[-9,5],[-5,-5],[-5,0],[-5,5],[-1,-6],[-1,-2.5],[-1,2.5],[-1,6])[unum-1] # initial formation
+
+
+    def beam(self, avoid_center_circle=False):
+        r = self.world.robot
+        pos = self.init_pos[:] # copy position list 
+        self.state = 0
+
+        # Avoid center circle by moving the player back 
+        if avoid_center_circle and np.linalg.norm(self.init_pos) < 2.5:
+            pos[0] = -2.3 
+
+        if np.linalg.norm(pos - r.loc_head_position[:2]) > 0.1 or self.behavior.is_ready("Get_Up"):
+            self.scom.commit_beam(pos, M.vector_angle((-pos[0],-pos[1]))) # beam to initial position, face coordinate (0,0)
+        else:
+            if self.fat_proxy_cmd is None: # normal behavior
+                self.behavior.execute("Zero_Bent_Knees_Auto_Head")
+            else: # fat proxy behavior
+                self.fat_proxy_cmd += "(proxy dash 0 0 0)"
+                self.fat_proxy_walk = np.zeros(3) # reset fat proxy walk
+
+
+    def move(self, target_2d=(0,0), orientation=None, is_orientation_absolute=True,
+             avoid_obstacles=True, priority_unums=[], is_aggressive=False, timeout=3000):
+        '''
+        Walk to target position
+
+        Parameters
+        ----------
+        target_2d : array_like
+            2D target in absolute coordinates
+        orientation : float
+            absolute or relative orientation of torso, in degrees
+            set to None to go towards the target (is_orientation_absolute is ignored)
+        is_orientation_absolute : bool
+            True if orientation is relative to the field, False if relative to the robot's torso
+        avoid_obstacles : bool
+            True to avoid obstacles using path planning (maybe reduce timeout arg if this function is called multiple times per simulation cycle)
+        priority_unums : list
+            list of teammates to avoid (since their role is more important)
+        is_aggressive : bool
+            if True, safety margins are reduced for opponents
+        timeout : float
+            restrict path planning to a maximum duration (in microseconds)    
+        '''
+        r = self.world.robot
+
+        if self.fat_proxy_cmd is not None: # fat proxy behavior
+            self.fat_proxy_move(target_2d, orientation, is_orientation_absolute) # ignore obstacles
+            return
+
+        if avoid_obstacles:
+            target_2d, _, distance_to_final_target = self.path_manager.get_path_to_target(
+                target_2d, priority_unums=priority_unums, is_aggressive=is_aggressive, timeout=timeout)
+        else:
+            distance_to_final_target = np.linalg.norm(target_2d - r.loc_head_position[:2])
+
+        self.behavior.execute("Walk", target_2d, True, orientation, is_orientation_absolute, distance_to_final_target) # Args: target, is_target_abs, ori, is_ori_abs, distance
+    def dribble(self, target_2d=(0,0), orientation=None, is_orientation_absolute=True,
+             avoid_obstacles=True, priority_unums=[], is_aggressive=False, timeout=3000):
+        '''
+        Walk to target position
+
+        Parameters
+        ----------
+        target_2d : array_like
+            2D target in absolute coordinates
+        orientation : float
+            absolute or relative orientation of torso, in degrees
+            set to None to go towards the target (is_orientation_absolute is ignored)
+        is_orientation_absolute : bool
+            True if orientation is relative to the field, False if relative to the robot's torso
+        avoid_obstacles : bool
+            True to avoid obstacles using path planning (maybe reduce timeout arg if this function is called multiple times per simulation cycle)
+        priority_unums : list
+            list of teammates to avoid (since their role is more important)
+        is_aggressive : bool
+            if True, safety margins are reduced for opponents
+        timeout : float
+            restrict path planning to a maximum duration (in microseconds)
+        '''
+        r = self.world.robot
+
+        if self.fat_proxy_cmd is not None: # fat proxy behavior
+            self.fat_proxy_move(target_2d, orientation, is_orientation_absolute) # ignore obstacles
+            return
+
+        if avoid_obstacles:
+            target_2d, _, distance_to_final_target = self.path_manager.get_path_to_target(
+                target_2d, priority_unums=priority_unums, is_aggressive=is_aggressive, timeout=timeout)
+        else:
+            distance_to_final_target = np.linalg.norm(target_2d - r.loc_head_position[:2])
+
+        self.behavior.execute("Dribble", orientation, True) # Args: target, is_target_abs, ori, is_ori_abs, distance
+
+
+
+
+    def kick(self, kick_direction=None, kick_distance=None, abort=False, enable_pass_command=False):
+        '''
+        Walk to ball and kick
+
+        Parameters
+        ----------
+        kick_direction : float
+            kick direction, in degrees, relative to the field
+        kick_distance : float
+            kick distance in meters
+        abort : bool
+            True to abort.
+            The method returns True upon successful abortion, which is immediate while the robot is aligning itself. 
+            However, if the abortion is requested during the kick, it is delayed until the kick is completed.
+        avoid_pass_command : bool
+            When False, the pass command will be used when at least one opponent is near the ball
+            
+        Returns
+        -------
+        finished : bool
+            Returns True if the behavior finished or was successfully aborted.
+        '''
+
+        if self.min_opponent_ball_dist < 1.45 and enable_pass_command:
+            self.scom.commit_pass_command()
+
+        self.kick_direction = self.kick_direction if kick_direction is None else kick_direction
+        self.kick_distance = self.kick_distance if kick_distance is None else kick_distance
+
+        if self.fat_proxy_cmd is None: # normal behavior
+            return self.behavior.execute("Basic_Kick", self.kick_direction, abort) # Basic_Kick has no kick distance control
+        else: # fat proxy behavior
+            return self.fat_proxy_kick()
+
+
+
+
+    def think_and_send(self):
+        w = self.world
+        r = self.world.robot  
+        my_head_pos_2d = r.loc_head_position[:2]
+        my_ori = r.imu_torso_orientation
+        ball_2d = w.ball_abs_pos[:2]
+        ball_vec = ball_2d - my_head_pos_2d
+        ball_dir = M.vector_angle(ball_vec)
+        ball_dist = np.linalg.norm(ball_vec)
+        ball_sq_dist = ball_dist * ball_dist # for faster comparisons
+        ball_speed = np.linalg.norm(w.get_ball_abs_vel(6)[:2])
+        behavior = self.behavior
+        goal_dir = M.target_abs_angle(ball_2d,(15.05,0))
+        path_draw_options = self.path_manager.draw_options
+        PM = w.play_mode
+        PM_GROUP = w.play_mode_group
+
+        #--------------------------------------- 1. Preprocessing
+
+        slow_ball_pos = w.get_predicted_ball_pos(0.5) # predicted future 2D ball position when ball speed <= 0.5 m/s
+
+        # list of squared distances between teammates (including self) and slow ball (sq distance is set to 1000 in some conditions)
+        teammates_ball_sq_dist = [np.sum((p.state_abs_pos[:2] - slow_ball_pos) ** 2)  # squared distance between teammate and ball
+                                  if p.state_last_update != 0 and (w.time_local_ms - p.state_last_update <= 360 or p.is_self) and not p.state_fallen
+                                  else 1000 # force large distance if teammate does not exist, or its state info is not recent (360 ms), or it has fallen
+                                  for p in w.teammates ]
+
+        # list of squared distances between opponents and slow ball (sq distance is set to 1000 in some conditions)
+        opponents_ball_sq_dist = [np.sum((p.state_abs_pos[:2] - slow_ball_pos) ** 2)  # squared distance between teammate and ball
+                                  if p.state_last_update != 0 and w.time_local_ms - p.state_last_update <= 360 and not p.state_fallen
+                                  else 1000 # force large distance if opponent does not exist, or its state info is not recent (360 ms), or it has fallen
+                                  for p in w.opponents ]
+
+        min_teammate_ball_sq_dist = min(teammates_ball_sq_dist)
+        self.min_teammate_ball_dist = math.sqrt(min_teammate_ball_sq_dist)   # distance between ball and closest teammate
+        self.min_opponent_ball_dist = math.sqrt(min(opponents_ball_sq_dist)) # distance between ball and closest opponent
+
+        active_player_unum = teammates_ball_sq_dist.index(min_teammate_ball_sq_dist) + 1
+
+
+        #--------------------------------------- 2. Decide action
+
+
+
+        if PM == w.M_GAME_OVER:
+            pass
+        elif PM_GROUP == w.MG_ACTIVE_BEAM:
+            self.beam()
+        elif PM_GROUP == w.MG_PASSIVE_BEAM:
+            self.beam(True) # avoid center circle
+        elif self.state == 1 or (behavior.is_ready("Get_Up") and self.fat_proxy_cmd is None):
+            self.state = 0 if behavior.execute("Get_Up") else 1 # return to normal state if get up behavior has finished
+        elif PM == w.M_OUR_KICKOFF:
+            if r.unum == 9:
+                self.kick(120,3) # no need to change the state when PM is not Play On
+            else:
+                self.move(self.init_pos, orientation=ball_dir) # walk in place
+        elif PM == w.M_THEIR_KICKOFF:
+            self.move(self.init_pos, orientation=ball_dir) # walk in place
+        elif active_player_unum != r.unum: # I am not the active player
+            if r.unum == 1: # I am the goalkeeper
+                self.move(self.init_pos, orientation=ball_dir) # walk in place 
+            else:
+                # compute basic formation position based on ball position
+                new_x = max(0.5,(ball_2d[0]+15)/15) * (self.init_pos[0]+15) - 15
+                if self.min_teammate_ball_dist < self.min_opponent_ball_dist:
+                    new_x = min(new_x + 3.5, 13) # advance if team has possession
+                self.move((new_x,self.init_pos[1]), orientation=ball_dir, priority_unums=[active_player_unum])
+
+        else: # I am the active player
+            path_draw_options(enable_obstacles=True, enable_path=True, use_team_drawing_channel=True) # enable path drawings for active player (ignored if self.enable_draw is False)
+            enable_pass_command = (PM == w.M_PLAY_ON and ball_2d[0]<6)
+
+            if r.unum == 1 and PM_GROUP == w.MG_THEIR_KICK: # goalkeeper during their kick
+                self.move(self.init_pos, orientation=ball_dir) # walk in place 
+            if PM == w.M_OUR_CORNER_KICK:
+                self.kick( -np.sign(ball_2d[1])*95, 5.5) # kick the ball into the space in front of the opponent's goal
+                # no need to change the state when PM is not Play On
+            elif self.min_opponent_ball_dist + 0.5 < self.min_teammate_ball_dist: # defend if opponent is considerably closer to the ball
+                if self.state == 2: # commit to kick while aborting
+                    self.state = 0 if self.kick(abort=True) else 2
+                else: # move towards ball, but position myself between ball and our goal
+                    self.move(slow_ball_pos + M.normalize_vec((-16,0) - slow_ball_pos) * 0.2, is_aggressive=True)
+            else:
+                self.state = 0 if self.kick(goal_dir, 9, False, enable_pass_command) else 2
+
+            path_draw_options(enable_obstacles=False, enable_path=False) # disable path drawings
+
+        #--------------------------------------- 3. Broadcast
+        self.radio.broadcast()
+
+        #--------------------------------------- 4. Send to server
+        if self.fat_proxy_cmd is None: # normal behavior
+            self.scom.commit_and_send( r.get_command() )
+        else: # fat proxy behavior
+            self.scom.commit_and_send( self.fat_proxy_cmd.encode() ) 
+            self.fat_proxy_cmd = ""
+
+        #---------------------- annotations for debugging
+        if self.enable_draw: 
+            d = w.draw
+            if active_player_unum == r.unum:
+                d.point(slow_ball_pos, 3, d.Color.pink, "status", False) # predicted future 2D ball position when ball speed <= 0.5 m/s
+                d.point(w.ball_2d_pred_pos[-1], 5, d.Color.pink, "status", False) # last ball prediction
+                d.annotation((*my_head_pos_2d, 0.6), "I've got it!" , d.Color.yellow, "status")
+            else:
+                d.clear("status")
+
+
+
+
+    #--------------------------------------- Fat proxy auxiliary methods
+
+
+    def fat_proxy_kick(self):
+        w = self.world
+        r = self.world.robot 
+        ball_2d = w.ball_abs_pos[:2]
+        my_head_pos_2d = r.loc_head_position[:2]
+
+        if np.linalg.norm(ball_2d - my_head_pos_2d) < 0.25:
+            # fat proxy kick arguments: power [0,10]; relative horizontal angle [-180,180]; vertical angle [0,70]
+            self.fat_proxy_cmd += f"(proxy kick 10 {M.normalize_deg( self.kick_direction  - r.imu_torso_orientation ):.2f} 20)" 
+            self.fat_proxy_walk = np.zeros(3) # reset fat proxy walk
+            return True
+        else:
+            self.fat_proxy_move(ball_2d-(-0.1,0), None, True) # ignore obstacles
+            return False
+
+
+    def fat_proxy_move(self, target_2d, orientation, is_orientation_absolute):
+        r = self.world.robot
+
+        target_dist = np.linalg.norm(target_2d - r.loc_head_position[:2])
+        target_dir = M.target_rel_angle(r.loc_head_position[:2], r.imu_torso_orientation, target_2d)
+
+        if target_dist > 0.1 and abs(target_dir) < 8:
+            self.fat_proxy_cmd += (f"(proxy dash {100} {0} {0})")
+            return
+
+        if target_dist < 0.1:
+            if is_orientation_absolute:
+                orientation = M.normalize_deg( orientation - r.imu_torso_orientation )
+            target_dir = np.clip(orientation, -60, 60)
+            self.fat_proxy_cmd += (f"(proxy dash {0} {0} {target_dir:.1f})")
+        else:
+            self.fat_proxy_cmd += (f"(proxy dash {20} {0} {target_dir:.1f})")

agent/Agent_Penalty.py

@@ -0,0 +1,88 @@
+from agent.Base_Agent import Base_Agent
+from math_ops.Math_Ops import Math_Ops as M
+import numpy as np
+import random
+
+
+class Agent(Base_Agent):
+    def __init__(self, host:str, agent_port:int, monitor_port:int, unum:int,
+                 team_name:str, enable_log, enable_draw, wait_for_server=True, is_fat_proxy=False) -> None:
+        
+        # define robot type
+        robot_type = 0 if unum == 1 else 4 # assume the goalkeeper uses uniform number 1 and the kicker uses any other number
+
+        # Initialize base agent
+        # Args: Server IP, Agent Port, Monitor Port, Uniform No., Robot Type, Team Name, Enable Log, Enable Draw, play mode correction, Wait for Server, Hear Callback
+        super().__init__(host, agent_port, monitor_port, unum, robot_type, team_name, enable_log, enable_draw, False, wait_for_server, None)
+
+        self.enable_draw = enable_draw
+        self.state = 0  # 0-Normal, 1-Getting up, 2-Dive Left, 3-Dive Right, 4-Wait
+
+        self.kick_dir = 0 # kick direction
+        self.reset_kick = True # when True, a new random kick direction is generated
+        
+
+    def think_and_send(self):
+        w = self.world
+        r = self.world.robot 
+        my_head_pos_2d = r.loc_head_position[:2]
+        my_ori = r.imu_torso_orientation
+        ball_2d = w.ball_abs_pos[:2]
+        ball_vec = ball_2d - my_head_pos_2d
+        ball_dir = M.vector_angle(ball_vec)
+        ball_dist = np.linalg.norm(ball_vec)
+        ball_speed = np.linalg.norm(w.get_ball_abs_vel(6)[:2])
+        behavior = self.behavior
+        PM = w.play_mode
+
+        #--------------------------------------- 1. Decide action
+
+        if PM in [w.M_BEFORE_KICKOFF, w.M_THEIR_GOAL, w.M_OUR_GOAL]: # beam to initial position and wait
+            self.state = 0
+            self.reset_kick = True
+            pos = (-14,0) if r.unum == 1 else (4.9,0)
+            if np.linalg.norm(pos - r.loc_head_position[:2]) > 0.1 or behavior.is_ready("Get_Up"):
+                self.scom.commit_beam(pos, 0) # beam to initial position
+            else:
+                behavior.execute("Zero_Bent_Knees") # wait
+        elif self.state == 2: # dive left
+            self.state = 4 if behavior.execute("Dive_Left") else 2  # change state to wait after skill has finished
+        elif self.state == 3: # dive right
+            self.state = 4 if behavior.execute("Dive_Right") else 3 # change state to wait after skill has finished
+        elif self.state == 4: # wait (after diving or during opposing kick)
+            pass
+        elif self.state == 1 or behavior.is_ready("Get_Up"): # if getting up or fallen
+            self.state = 0 if behavior.execute("Get_Up") else 1 # return to normal state if get up behavior has finished
+        elif PM == w.M_OUR_KICKOFF and r.unum == 1 or PM == w.M_THEIR_KICKOFF and r.unum != 1:
+            self.state = 4 # wait until next beam
+        elif r.unum == 1: # goalkeeper
+            y_coordinate = np.clip(ball_2d[1], -1.1, 1.1)
+            behavior.execute("Walk", (-14,y_coordinate), True, 0, True, None) # Args: target, is_target_abs, ori, is_ori_abs, distance
+            if ball_2d[0] < -10: 
+                self.state = 2 if ball_2d[1] > 0 else 3 # dive to defend
+        else: # kicker
+            if PM == w.M_OUR_KICKOFF and ball_2d[0] > 5: # check ball position to make sure I see it
+                if self.reset_kick: 
+                    self.kick_dir = random.choice([-7.5,7.5]) 
+                    self.reset_kick = False
+                behavior.execute("Basic_Kick", self.kick_dir)
+            else:
+                behavior.execute("Zero_Bent_Knees") # wait
+
+        #--------------------------------------- 2. Broadcast
+        self.radio.broadcast()
+
+        #--------------------------------------- 3. Send to server
+        self.scom.commit_and_send( r.get_command() )
+
+        #---------------------- annotations for debugging
+        if self.enable_draw: 
+            d = w.draw
+            if r.unum == 1:
+                d.annotation((*my_head_pos_2d, 0.8), "Goalkeeper" , d.Color.yellow, "status")
+            else:
+                d.annotation((*my_head_pos_2d, 0.8), "Kicker" , d.Color.yellow, "status")
+                if PM == w.M_OUR_KICKOFF: # draw arrow to indicate kick direction
+                    d.arrow(ball_2d, ball_2d + 5*M.vector_from_angle(self.kick_dir), 0.4, 3, d.Color.cyan_light, "Target")
+
+

agent/Base_Agent.py

@@ -0,0 +1,47 @@
+from abc import abstractmethod
+from behaviors.Behavior import Behavior
+from communication.Radio import Radio
+from communication.Server_Comm import Server_Comm
+from communication.World_Parser import World_Parser
+from logs.Logger import Logger
+from math_ops.Inverse_Kinematics import Inverse_Kinematics
+from world.commons.Path_Manager import Path_Manager
+from world.World import World
+
+class Base_Agent():
+    all_agents = []
+
+    def __init__(self, host:str, agent_port:int, monitor_port:int, unum:int, robot_type:int, team_name:str, enable_log:bool=True,
+                  enable_draw:bool=True, apply_play_mode_correction:bool=True, wait_for_server:bool=True, hear_callback=None) -> None:
+
+        self.radio = None # hear_message may be called during Server_Comm instantiation
+        self.logger = Logger(enable_log, f"{team_name}_{unum}")
+        self.world = World(robot_type, team_name, unum, apply_play_mode_correction, enable_draw, self.logger, host)
+        self.world_parser = World_Parser(self.world, self.hear_message if hear_callback is None else hear_callback)
+        self.scom = Server_Comm(host,agent_port,monitor_port,unum,robot_type,team_name,self.world_parser,self.world,Base_Agent.all_agents,wait_for_server)
+        self.inv_kinematics = Inverse_Kinematics(self.world.robot)
+        self.behavior = Behavior(self)
+        self.path_manager = Path_Manager(self.world)
+        self.radio = Radio(self.world, self.scom.commit_announcement)
+        self.behavior.create_behaviors()
+        Base_Agent.all_agents.append(self)
+
+    @abstractmethod
+    def think_and_send(self):
+        pass
+
+    def hear_message(self, msg:bytearray, direction, timestamp:float) -> None:
+        if direction != "self" and self.radio is not None:
+            self.radio.receive(msg)
+
+    def terminate(self):
+        # close shared monitor socket if this is the last agent on this thread
+        self.scom.close(close_monitor_socket=(len(Base_Agent.all_agents)==1))
+        Base_Agent.all_agents.remove(self)
+
+    @staticmethod
+    def terminate_all():
+        for o in Base_Agent.all_agents:
+            o.scom.close(True) # close shared monitor socket, if it exists
+        Base_Agent.all_agents = []
+