From 4c9fc3ec10452936274858db666ebc4d73056f8c Mon Sep 17 00:00:00 2001 From: MagDish <2717360869@qq.com> Date: Mon, 6 May 2024 21:57:43 +0800 Subject: [PATCH] dribble --- .idea/FCPCodebase.iml | 2 +- .idea/misc.xml | 2 +- agent/Agent.py | 26 +++++++++++++++++++++----- world/Robot.py | 2 +- world/World.py | 1 + 5 files changed, 25 insertions(+), 8 deletions(-) diff --git a/.idea/FCPCodebase.iml b/.idea/FCPCodebase.iml index 8b8c395..4b9218a 100644 --- a/.idea/FCPCodebase.iml +++ b/.idea/FCPCodebase.iml @@ -2,7 +2,7 @@ - + diff --git a/.idea/misc.xml b/.idea/misc.xml index a33389a..d9b1e57 100644 --- a/.idea/misc.xml +++ b/.idea/misc.xml @@ -3,5 +3,5 @@ - + \ No newline at end of file diff --git a/agent/Agent.py b/agent/Agent.py index c5500e2..4232022 100644 --- a/agent/Agent.py +++ b/agent/Agent.py @@ -24,6 +24,14 @@ class Agent(Base_Agent): 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 dribble(self, target_2d=(15, 0), avoid_obstacles=True): + + if avoid_obstacles: + next_pos, next_rel_ori = self.path_manager.get_dribble_path(optional_2d_target=target_2d) + else: + distance_to_final_target = np.linalg.norm(target_2d - self.loc_head_position[:2]) + return self.behavior.execute("Dribble", next_rel_ori, + False) # Args: target, is_target_abs, ori, is_ori_abs, distance def beam(self, avoid_center_circle=False): r = self.world.robot @@ -124,7 +132,7 @@ class Agent(Base_Agent): def think_and_send(self): w = self.world - r = self.world.robot + 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] @@ -158,8 +166,10 @@ class Agent(Base_Agent): 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 + if r.dribbling_state == "In": + active_player_unum = r.unum + else: + active_player_unum = teammates_ball_sq_dist.index(min_teammate_ball_sq_dist) + 1 #--------------------------------------- 2. Decide action @@ -173,6 +183,7 @@ class Agent(Base_Agent): 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): + r.dribbling_state = "Out" 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: @@ -192,6 +203,7 @@ class Agent(Base_Agent): 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) @@ -202,11 +214,15 @@ class Agent(Base_Agent): # 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 + self.dribble() + r.dribbling_state = "In" 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) + r.dribbling_state = "Out" + else: - self.state = 0 if self.kick(goal_dir,9,False,enable_pass_command) else 2 + self.dribble() + r.dribbling_state = "In" path_draw_options(enable_obstacles=False, enable_path=False) # disable path drawings diff --git a/world/Robot.py b/world/Robot.py index bdeb901..ff92f3f 100644 --- a/world/Robot.py +++ b/world/Robot.py @@ -41,7 +41,7 @@ class Robot(): #Fix symmetry issues 1b/4 (identification) self.FIX_EFFECTOR_MASK = np.ones(self.no_of_joints) self.FIX_EFFECTOR_MASK[Robot.FIX_INDICES_LIST] = -1 - + self.dribbling_state = "Out" self.body_parts = dict() # keys='body part names' (given by the robot's XML), values='Body_Part objects' self.unum = unum # Robot's uniform number self.gyro = np.zeros(3) # Angular velocity along the three axes of freedom of the robot's torso (deg/s) diff --git a/world/World.py b/world/World.py index 9c1f549..6405254 100644 --- a/world/World.py +++ b/world/World.py @@ -101,6 +101,7 @@ class World(): self.robot = Robot(unum, robot_type) + def log(self, msg:str): ''' Shortcut for: