From f0d861675461767ce81942c5fae6848ab1ed8dd2 Mon Sep 17 00:00:00 2001 From: Joao Pedro Leal Date: Fri, 16 Jun 2023 22:17:48 -0300 Subject: [PATCH 01/16] feat(twiddle): implement twiddle support inside NeonFC --- config_real_life.json | 2 +- entities/coach/test_coach.py | 4 +- main_real_life.py | 22 +++++------ strategy/tests/PID_tuner.py | 74 +++++++++++++++++++++++++++--------- 4 files changed, 70 insertions(+), 32 deletions(-) diff --git a/config_real_life.json b/config_real_life.json index 8fa54248..f58cfa62 100644 --- a/config_real_life.json +++ b/config_real_life.json @@ -16,7 +16,7 @@ "match" : { "team_side": "left", "team_color": "yellow", - "coach_name": "RSM_2023", + "coach_name": "TEST", "category": "3v3", "robot_ids": [ 1, diff --git a/entities/coach/test_coach.py b/entities/coach/test_coach.py index 2a88f404..6fd144fa 100644 --- a/entities/coach/test_coach.py +++ b/entities/coach/test_coach.py @@ -8,8 +8,8 @@ def __init__(self, match): super().__init__(match) # chamada do metodo da classe mae # vamos usar strategies de teste por enquanto, essa deixa o robo parado - self._1 = strategy.tests.Idle(self.match) - self._2 = strategy.tests.Foward(self.match) + self._1 = strategy.tests.PIDTuner(self.match) + self._2 = strategy.tests.Idle(self.match) self._3 = strategy.tests.Idle(self.match) def decide(self): diff --git a/main_real_life.py b/main_real_life.py index 91635b13..5c79ba29 100644 --- a/main_real_life.py +++ b/main_real_life.py @@ -33,7 +33,7 @@ def __init__(self, config_file=None, env='real_life'): self.referee = RefereeComm(config_file) self.api = Api(self.api_address, self.api_port) - self.api_recv = Api_recv(self.match, self.api_address, self.api_recv_port) + # self.api_recv = Api_recv(self.match, self.api_address, self.api_recv_port) self.use_referee = False @@ -48,7 +48,7 @@ def start(self): if self.use_api: self.api.start() - self.api_recv.start() + # self.api_recv.start() def update(self): frame = assign_empty_values( @@ -62,15 +62,15 @@ def update(self): self.match.update(frame) commands = self.match.decide() - if self.use_api and (self.match.game_status == 'STOP' or self.match.game_status == None): - commands = [ - { - 'robot_id': r['robot_id'], - 'color': r['color'], - 'wheel_left': 0, - 'wheel_right': 0 - } for r in commands - ] + # if self.use_api and (self.match.game_status == 'STOP' or self.match.game_status == None): + # commands = [ + # { + # 'robot_id': r['robot_id'], + # 'color': r['color'], + # 'wheel_left': 0, + # 'wheel_right': 0 + # } for r in commands + # ] self.comm.send(commands) diff --git a/strategy/tests/PID_tuner.py b/strategy/tests/PID_tuner.py index bcdbe68b..7e7f0018 100644 --- a/strategy/tests/PID_tuner.py +++ b/strategy/tests/PID_tuner.py @@ -8,14 +8,16 @@ class PIDTuner(Strategy): def __init__(self, match): - self.lt = time.time() - super().__init__(match, "PID_Tune", controller=PID_W_control) + super().__init__(match, "PID_Tune", controller=PID_W_control, + controller_kwargs={'V_MAX': 0, 'V_MIN': 0, 'KP': 0, 'KI': 0, 'KD': 0}) self.sender = Api("127.0.0.1", 43210) self.sender.start() - self.reciver = Api_recv(match, "127.0.0.1", 43211) + self.reciver = Api_recv(match, "127.0.0.1", 43221) self.reciver.start() self.circuit = [(.2, .65), (1.1, .65)] # , (.4, .90), (.4, .40)] self.circuit = deque(self.circuit) + self.t0 = time.time() + print("on init") def start(self, robot=None): super().start(robot=robot) @@ -39,23 +41,59 @@ def next_point(self): def decide(self): data = {'pid': { - 'set_point': 0, - 'error': self.controller.error, - 'time': time.time(), - 'w': self.robot.vtheta - }} + 'set_point': 0, + 'error': self.controller.error, + 'time': time.time(), + 'w': self.robot.vtheta}, + 'pos':{ + 'x': self.robot.x, + 'y': self.robot.y, + 'theta': self.robot.theta + } + } + self.sender.send_custom_data(data) data = self.reciver.decod_data + new_ks = False + if data: - self.controller.KP = data['kp'] - self.controller.KI = data['ki'] - self.controller.KD = data['kd'] - - if self.y == 0: - self.y = self.robot.y - target = [.1,self.y] - print(target) - return target + if self.controller.KP != data['kp']: + print(self.controller.KP, data['kp']) + self.controller.KP = data['kp'] + new_ks = True + if self.controller.KI != data['ki']: + print(self.controller.KI, data['ki']) + self.controller.KI = data['ki'] + new_ks = True + if self.controller.KD != data['kd']: + print(self.controller.KI, data['ki']) + self.controller.KD = data['kd'] + new_ks = True + + if new_ks: + print("new ks") + self.controller.alpha_old = 0 + self.controller.int_alpha = 0 + new_ks = False + + self.t0 = time.time() + + if time.time() - self.t0 <= 1: + target = [.5 * self.robot.x, .5 * self.robot.y] - return self.next_point() + elif time.time() - self.t0 <= 2: + target = [self.robot.x + math.cos(self.robot.theta + .5 * math.pi), + self.robot.y + math.sin(self.robot.theta + .5 * math.pi)] + self.lp = [self.robot.x, self.robot.y, self.robot.theta] + + elif time.time() - self.t0 <= 3: + target = [self.lp[0] + math.cos(self.lp[2] + .5 * math.pi), + self.lp[1] + math.sin(self.lp[2] + .5 * math.pi)] + + else: + # self.controller.KP, self.controller.KI, self.controller.KD = 0, 0, 0 + target = self.robot + + + return target From b48a5e57cd6415310d780c9bf4e39e031c87e320 Mon Sep 17 00:00:00 2001 From: Joao Pedro Leal Date: Fri, 16 Jun 2023 22:18:35 -0300 Subject: [PATCH 02/16] fix(PID): initial twiddle tune --- controller/PID_control.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/controller/PID_control.py b/controller/PID_control.py index 935b113e..242d2e7c 100644 --- a/controller/PID_control.py +++ b/controller/PID_control.py @@ -59,8 +59,8 @@ class PID_control(object): # Control params 'K_RHO': 500, # Linear speed gain # PID of angular speed - 'KP': -1000, # -700, # Proportional gain of w (angular speed), respecting the stability condition: K_RHO > 0 and KP > K_RHO - 'KD': 0, # -180, # Derivative gain of w + 'KP': -3.9699999999999993, # -700, # Proportional gain of w (angular speed), respecting the stability condition: K_RHO > 0 and KP > K_RHO + 'KD': 0.014201915, # -180, # Derivative gain of w 'KI': 0, # Integral gain of w # Max speeds for the robot 'V_MAX': 150, # linear speed @@ -190,5 +190,6 @@ def update(self): if self.environment == 'simulation': powers = speed_to_power(v, w, self.l, self.R) return tuple(np.dot(1000, powers)) + print(v, w) return v, w From 3aa7f12dd3063ec0492339cabb27cdf2e9804ae7 Mon Sep 17 00:00:00 2001 From: Joao Pedro Leal Date: Sat, 22 Jul 2023 11:39:24 -0300 Subject: [PATCH 03/16] fix(rsm 2023 strategy): remove unnecessary print --- strategy/rsm2023/SecondAttacker.py | 1 - 1 file changed, 1 deletion(-) diff --git a/strategy/rsm2023/SecondAttacker.py b/strategy/rsm2023/SecondAttacker.py index c41ffd99..1ce81a35 100644 --- a/strategy/rsm2023/SecondAttacker.py +++ b/strategy/rsm2023/SecondAttacker.py @@ -64,7 +64,6 @@ def start_up(self): self.robot.strategy.controller = controller(self.robot, **controller_kwargs) def update(self): - print(self.position()) return self.position() def start(self): From f6941d8e97e3c7f978586036224da586118d1b68 Mon Sep 17 00:00:00 2001 From: Joao Pedro Leal Date: Sat, 22 Jul 2023 11:43:47 -0300 Subject: [PATCH 04/16] feat(twiddle): update pid tunner to make it more versatile --- strategy/tests/PID_tuner.py | 118 +++++++++++++++++++++++++++--------- 1 file changed, 89 insertions(+), 29 deletions(-) diff --git a/strategy/tests/PID_tuner.py b/strategy/tests/PID_tuner.py index 7e7f0018..40ffce72 100644 --- a/strategy/tests/PID_tuner.py +++ b/strategy/tests/PID_tuner.py @@ -9,14 +9,16 @@ class PIDTuner(Strategy): def __init__(self, match): super().__init__(match, "PID_Tune", controller=PID_W_control, - controller_kwargs={'V_MAX': 0, 'V_MIN': 0, 'KP': 0, 'KI': 0, 'KD': 0}) + controller_kwargs={'V_MAX': 0, 'V_MIN': 50, 'KP': -3, 'KI': 0, 'KD': 0}) + self.sender = Api("127.0.0.1", 43210) self.sender.start() self.reciver = Api_recv(match, "127.0.0.1", 43221) self.reciver.start() - self.circuit = [(.2, .65), (1.1, .65)] # , (.4, .90), (.4, .40)] - self.circuit = deque(self.circuit) + self.circuit = [] self.t0 = time.time() + self.state = "countdown" + self.still = True print("on init") def start(self, robot=None): @@ -39,12 +41,48 @@ def next_point(self): return self.circuit[0] + def make_hexagonal_path(self): + if .15 <= self.robot.x <= .3 and .575 <= self.robot.y <= .725: + + # set points on the hexagonal path that the robot will perform + A = (self.robot.x, self.robot.y) + B = (self.robot.x + .425, self.robot.y + .425) + C = (self.robot.x + .625, self.robot.y + .425) + D = (self.robot.x + 1.05, self.robot.y) + E = (self.robot.x + .625, self.robot.y - .425) + F = (self.robot.x + .425, self.robot.y - .425) + + # set the circuit + self.circuit = [B, C, D, E, F, A] + + def make_line_path(self): + size = .5 + + start = self.robot.x, self.robot.y + mid = self.robot.x + size*math.cos(self.robot.theta), self.robot.y + size*math.sin(self.robot.theta) + + if .15 < mid[0] < 1.35 and .15 < mid[1] < 1.15: + self.circuit = [mid, start] + else: + mid = self.robot.x + size * math.cos(self.robot.theta - math.pi), self.robot.y + size * math.sin(self.robot.theta - math.pi) + if .15 < mid[0] < 1.35 and .15 < mid[1] < 1.15: + self.circuit = [mid, start] + + def make_inplace(self): + self.controller.V_MAX = 0 + self.controller.V_MIN = 0 + self.still = True + + self.circuit = [(self.robot.x + 1, self.robot.y), (self.robot.x, self.robot.y + 1)] + def decide(self): + target = self.robot data = {'pid': { 'set_point': 0, - 'error': self.controller.error, + 'error': abs(self.controller.error), 'time': time.time(), - 'w': self.robot.vtheta}, + 'w': self.robot.vtheta, + 'running': self.state == "running"}, 'pos':{ 'x': self.robot.x, 'y': self.robot.y, @@ -55,45 +93,67 @@ def decide(self): self.sender.send_custom_data(data) data = self.reciver.decod_data - new_ks = False if data: if self.controller.KP != data['kp']: print(self.controller.KP, data['kp']) self.controller.KP = data['kp'] - new_ks = True + self.state = "position" if self.controller.KI != data['ki']: print(self.controller.KI, data['ki']) self.controller.KI = data['ki'] - new_ks = True + self.state = "position" if self.controller.KD != data['kd']: print(self.controller.KI, data['ki']) self.controller.KD = data['kd'] - new_ks = True + self.state = "position" - if new_ks: - print("new ks") + if self.state == "position": self.controller.alpha_old = 0 self.controller.int_alpha = 0 - new_ks = False - - self.t0 = time.time() - - if time.time() - self.t0 <= 1: - target = [.5 * self.robot.x, .5 * self.robot.y] - elif time.time() - self.t0 <= 2: - target = [self.robot.x + math.cos(self.robot.theta + .5 * math.pi), - self.robot.y + math.sin(self.robot.theta + .5 * math.pi)] - self.lp = [self.robot.x, self.robot.y, self.robot.theta] - - elif time.time() - self.t0 <= 3: - target = [self.lp[0] + math.cos(self.lp[2] + .5 * math.pi), - self.lp[1] + math.sin(self.lp[2] + .5 * math.pi)] - - else: - # self.controller.KP, self.controller.KI, self.controller.KD = 0, 0, 0 + self.make_inplace() + print("position") + if len(self.circuit) > 0: + print(self.circuit) + self.state = "countdown" + self.t0 = time.time() + + if self.state == "countdown": + if time.time() - self.t0 <= 0: # 3s + print("countdown") + target = self.robot + else: + self.make_inplace() + self.state = "running" + self.t0 = time.time() + + if self.state == "running": + print(self.circuit) + if not self.circuit: + self.state = "wait" + + elif self.still: + target = self.circuit[0] + + if time.time() - self.t0 > 1: + print("reached") + self.t0 = time.time() + self.circuit.pop(0) + else: + target = self.circuit[0] + + dx = self.circuit[0][0] - self.robot.x + dy = self.circuit[0][1] - self.robot.y + + if math.sqrt(dx ** 2 + dy ** 2) < 0.05: + print("reached") + self.circuit.pop(0) + + self.controller.V_MAX = 50 + + if self.state == "wait": target = self.robot - + self.controller.V_MAX = 0 return target From 4ae9972db8cba71f8799417518bc842445a6482c Mon Sep 17 00:00:00 2001 From: Joao Pedro Leal Date: Sat, 22 Jul 2023 11:44:23 -0300 Subject: [PATCH 05/16] feat(comm): make comm more precise --- comm/rl_comm.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/comm/rl_comm.py b/comm/rl_comm.py index cbbc59ec..78ca4d79 100644 --- a/comm/rl_comm.py +++ b/comm/rl_comm.py @@ -40,7 +40,7 @@ def send(self, robot_commands = []): message = "<" robot_commands = sorted(robot_commands, key = lambda i: i['robot_id']) for rb in robot_commands: - message += f"{rb['robot_id']},{round(rb['wheel_left'], 2)},{round(rb['wheel_right'], 2)}," + message += f"{rb['robot_id']},{round(rb['wheel_left'], 4)},{round(rb['wheel_right'], 4)}," message = message[:-1] + '>' From 81b69ca11cc6ec9f839b0b0d356b98b064fed520 Mon Sep 17 00:00:00 2001 From: Joao Pedro Leal Date: Sat, 22 Jul 2023 11:56:58 -0300 Subject: [PATCH 06/16] feat(unicontroller): update unicontroller parameters to new robot --- controller/uni_controller.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/controller/uni_controller.py b/controller/uni_controller.py index 7e113f4b..27136a42 100644 --- a/controller/uni_controller.py +++ b/controller/uni_controller.py @@ -39,10 +39,10 @@ class UniController(object): 'K_P': 10 }, 'real_life': { - 'V_M': 10000, - 'R_M': 3 * 10000, # 20 * V_M - 'K_W': 270, # 313, - 'K_P': 100 + 'V_M': 0.5, + 'R_M': 0.44, # 20 * V_M + 'K_W': 4, # 313, + 'K_P': 1 } } From f530433826a6560eb4b22868b1c60ef3689ea204 Mon Sep 17 00:00:00 2001 From: Joao Pedro Leal Date: Sat, 22 Jul 2023 11:57:49 -0300 Subject: [PATCH 07/16] feat(pid): update PID controller parameters to new robot --- controller/PID_control.py | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/controller/PID_control.py b/controller/PID_control.py index 242d2e7c..9cf93527 100644 --- a/controller/PID_control.py +++ b/controller/PID_control.py @@ -59,13 +59,13 @@ class PID_control(object): # Control params 'K_RHO': 500, # Linear speed gain # PID of angular speed - 'KP': -3.9699999999999993, # -700, # Proportional gain of w (angular speed), respecting the stability condition: K_RHO > 0 and KP > K_RHO - 'KD': 0.014201915, # -180, # Derivative gain of w - 'KI': 0, # Integral gain of w + 'KP': -3.5, # -700, # Proportional gain of w (angular speed), respecting the stability condition: K_RHO > 0 and KP > K_RHO + 'KD': 0, # -180, # Derivative gain of w + 'KI': 0 , # Integral gain of w # Max speeds for the robot - 'V_MAX': 150, # linear speed - 'W_MAX': -1, # angular speed rad/s - 'V_MIN': 20, + 'V_MAX': 0.3, # linear speed + 'W_MAX': -0.5, # angular speed rad/s + 'V_MIN': 0.1, 'TWO_SIDES': True } @@ -190,6 +190,5 @@ def update(self): if self.environment == 'simulation': powers = speed_to_power(v, w, self.l, self.R) return tuple(np.dot(1000, powers)) - print(v, w) return v, w From 013bad29223f832597dd9ae1b309edb829b1aa29 Mon Sep 17 00:00:00 2001 From: Joao Pedro Leal Date: Sat, 22 Jul 2023 14:47:31 -0300 Subject: [PATCH 08/16] feat(rcx_2023): create rcx 2023 files --- config_real_life.json | 2 +- entities/coach/__init__.py | 3 + entities/coach/rcx2023.py | 70 +++++ strategy/__init__.py | 1 + strategy/rcx2023/Attacker.py | 347 ++++++++++++++++++++++++ strategy/rcx2023/Goalkeeper.py | 471 +++++++++++++++++++++++++++++++++ strategy/rcx2023/__init__.py | 5 + 7 files changed, 898 insertions(+), 1 deletion(-) create mode 100644 entities/coach/rcx2023.py create mode 100644 strategy/rcx2023/Attacker.py create mode 100644 strategy/rcx2023/Goalkeeper.py create mode 100644 strategy/rcx2023/__init__.py diff --git a/config_real_life.json b/config_real_life.json index f58cfa62..6af1b096 100644 --- a/config_real_life.json +++ b/config_real_life.json @@ -16,7 +16,7 @@ "match" : { "team_side": "left", "team_color": "yellow", - "coach_name": "TEST", + "coach_name": "RCX_2023", "category": "3v3", "robot_ids": [ 1, diff --git a/entities/coach/__init__.py b/entities/coach/__init__.py index c8ec888a..5742d3bb 100644 --- a/entities/coach/__init__.py +++ b/entities/coach/__init__.py @@ -12,6 +12,8 @@ from entities.coach.rsm2023 import Coach as RSM_2023 +from entities.coach.rcx2023 import Coach as RCX_2023 + _coach_list = [ # Tournament coaches GuideCoach, @@ -19,6 +21,7 @@ IRON_2022, IRON_2023, RSM_2023, + RCX_2023, TestCoach ] diff --git a/entities/coach/rcx2023.py b/entities/coach/rcx2023.py new file mode 100644 index 00000000..844179ac --- /dev/null +++ b/entities/coach/rcx2023.py @@ -0,0 +1,70 @@ +from commons.math import distance_between_points +from entities.coach.coach import BaseCoach +import strategy + + +class Coach(BaseCoach): + NAME = "RCX_2023" + + def __init__(self, match): + super().__init__(match) + + self.SS_strategy = strategy.rcx2023.ShadowAttacker(self.match) + self.ST_strategy = strategy.rcx2023.MainStriker(self.match) + self.GK_strategy = strategy.rcx2023.Goalkeeper(self.match) + self.GK_id = 3 # Goalkeeper fixed ID + + self.unstucks = {r.robot_id: strategy.rsm2023.Unstuck(self.match) for r in self.match.robots if r.robot_id != self.GK_id} + + def decide(self): + GK = [i for i, r in enumerate(self.match.robots) if r.robot_id is self.GK_id][0] + strikers = [r for i, r in enumerate(self.match.robots) if r.robot_id is not self.GK_id] + ST, SS = self.choose_main_striker(*strikers) + + st_strat, ss_start = self.handle_stuck(ST, SS) + + if self.match.robots[GK].strategy is None: + self.match.robots[GK].strategy = self.GK_strategy + self.match.robots[GK].start() + else: + if self.match.robots[GK].strategy.name != self.GK_strategy: + self.match.robots[GK].strategy = self.GK_strategy + self.match.robots[GK].start() + + ST.strategy = st_strat + ST.start() + + SS.strategy = ss_start + SS.start() + + def choose_main_striker(self, r1, r2): + b = self.match.ball + + a1 = distance_between_points((b.x, b.y), (r1.x, r1.y)) + a2 = distance_between_points((b.x, b.y), (r2.x, r2.y)) + + b1, b2 = b.x - r1.x, b.x - r2.x + + if b1 * b2 > 0: + if a1 < a2: + return r1, r2 + return r2, r1 + if b1 > 0: + return r1, r2 + return r2, r1 + + def handle_stuck(self, ST, SS): + game_runing = not (self.match.game_status == 'STOP' or self.match.game_status == None) + stuck_st = ST.is_stuck() and game_runing + stuck_ss = SS.is_stuck() and game_runing + + if stuck_st and stuck_ss: + return self.unstucks[ST.robot_id], self.unstucks[SS.robot_id] + + if stuck_st and not stuck_ss: + return self.unstucks[ST.robot_id], self.ST_strategy + + if not stuck_st and stuck_ss: + return self.ST_strategy, self.unstucks[SS.robot_id] + + return self.ST_strategy, self.SS_strategy diff --git a/strategy/__init__.py b/strategy/__init__.py index bc02955c..17e92f65 100644 --- a/strategy/__init__.py +++ b/strategy/__init__.py @@ -5,6 +5,7 @@ from strategy import cbfr2022 from strategy import iron2023 from strategy import rsm2023 +from strategy import rcx2023 from strategy.BaseStrategy import Strategy # debug tools diff --git a/strategy/rcx2023/Attacker.py b/strategy/rcx2023/Attacker.py new file mode 100644 index 00000000..d5080fc5 --- /dev/null +++ b/strategy/rcx2023/Attacker.py @@ -0,0 +1,347 @@ +import numpy as np +from algorithms.limit_cycle import LimitCycle +import math +from controller.PID_control import PID_W_control, PID_control +from strategy.BaseStrategy import Strategy +from strategy.utils.player_playbook import PlayerPlay, PlayerPlaybook, OnInsideBox, OnNextTo, AndTransition +from commons.math import distance_between_points + + +class MainPlay(PlayerPlay): + def __init__(self, match, robot): + # super().__init__(match, "Main_Attacker", controller=PID_W_control) + super().__init__(match, robot) + + def get_name(self): + return f"<{self.robot.get_name()} Main Attacker Planning>" + + def start_up(self): + super().start_up() + controller = PID_W_control + self.robot.strategy.controller = controller(self.robot) + + def start(self): + self.limit_cycle = LimitCycle(self.match) + self.field_w, self.field_h = self.match.game.field.get_dimensions() + + def update(self): + ball = (self.match.ball.x, self.match.ball.y) + robot = (self.robot.x, self.robot.y) + + if distance_between_points(ball, robot) < 0.1 and not self.behind_ball(): + desired = (1.5, 0.65) + else: + self.limit_cycle.set_target(ball) + self.limit_cycle.add_obstacle(self.get_virtual_obstacle(ball)) + try: + desired = self.limit_cycle.compute(self.robot, fitness=20) + except ZeroDivisionError: + print("ZeroDivisionError on attacker") + desired = list(robot) + + return desired + + def get_virtual_obstacle(self, target): + ''' + - m: angle of the line perpendicular to the line between the ball and + the center of the goal + - p: distance of the virtual obstacles to the ball / radius of the virtual obstacle + - vo: virtual obstacle + - j: this is the angle between the ball and the center of the goal + - m: the normal angle perpendicular to j + - r: radius of the ball + ''' + aim_point = [self.field_w, self.field_h / 2] + + j = math.atan2(aim_point[1] - target[1], aim_point[0] - target[0]) + m = j + math.pi / 2 + p = 0.1 + + r = .0427 / 2 + + ''' + the terms r*cos(j) and r*sin(j) are subtracted to move + the center of the obstacles behind the ball instead of its center + ''' + if self.robot.y < math.tan(j) * (self.robot.x - target[0]) + target[1]: + virtual_obstacle = ( + target[0] - p * math.cos(m) - r * math.cos(j), + target[1] - p * math.sin(m) - r * math.sin(j), + p, + 1 + ) + else: + virtual_obstacle = ( + target[0] + p * math.cos(m) - r * math.cos(j), + target[1] + p * math.sin(m) - r * math.sin(j), + p, + -1 + ) + + if self.behind_ball(): + virtual_obstacle = ( + target[0] - p * math.cos(m) - r * math.cos(j), + target[1] - p * math.sin(m) - r * math.sin(j), + 2 * p, + np.sign(target[1] - p * math.sin(m) - r * math.sin(j) - 0.65) + ) + + return virtual_obstacle + + def behind_ball(self): + # Convert input to numpy arrays for easy calculation + point_on_line = np.array((self.match.ball.x, self.match.ball.y)) + point_on_normal = np.array((1.5, .65)) + point_to_check = np.array((self.robot.x, self.robot.y)) + + # Calculate the normal vector of the line + normal = point_on_normal - point_on_line + + # Calculate the vector from the point on the line to the point to check + vector_to_check = point_to_check - point_on_line + + # Calculate the dot product of the normal vector and the vector to check + dot_product = np.dot(normal, vector_to_check) + + # Check the sign of the dot product to determine if the point is to the right or left of the line + return dot_product > 0 + + +class WingPlay(PlayerPlay): + def __init__(self, match, robot): + super().__init__(match, robot) + + self.BALL_Y_MIN = 0.05 + self.BALL_Y_MAX = 1.25 + + def get_name(self): + return f"<{self.robot.get_name()} Wing Attacker Planning>" + + def start_up(self): + super().start_up() + controller = PID_W_control + self.robot.strategy.controller = controller(self.robot) + + def start(self): + self.limit_cycle = LimitCycle(self.match) + self.field_w, self.field_h = self.match.game.field.get_dimensions() + + def update(self): + target = [self.match.ball.x, self.match.ball.y] + robot = (self.robot.x, self.robot.y) + + target[1] = min(self.BALL_Y_MAX, target[1]) + target[1] = max(self.BALL_Y_MIN, target[1]) + + aim_point = [self.field_w, self.BALL_Y_MAX] if target[1] > .65 else [self.field_w, self.BALL_Y_MIN] + + if distance_between_points(target, robot) < 0.1 and not self.behind_ball(aim_point): + desired = aim_point + else: + self.limit_cycle.set_target(target) + self.limit_cycle.add_obstacle(self.get_virtual_obstacle(target)) + try: + desired = self.limit_cycle.compute(self.robot, fitness=20) + except ZeroDivisionError: + print("ZeroDivisionError on attacker") + desired = list(robot) + + return desired + + def get_virtual_obstacle(self, target): + """ + - m: angle of the line perpendicular to the line between the ball and + the center of the goal + - p: distance of the virtual obstacles to the ball / radius of the virtual obstacle + - vo: virtual obstacle + - j: this is the angle between the ball and the center of the goal + - m: the normal angle perpendicular to j + - r: radius of the ball + """ + aim_point = [self.field_w, self.BALL_Y_MAX] if target[1] > .65 else [self.field_w, self.BALL_Y_MIN] + + j = math.atan2(aim_point[1] - target[1], aim_point[0] - target[0]) + m = j + math.pi / 2 + p = 0.1 + + r = .0427 / 2 + + ''' + the terms r*cos(j) and r*sin(j) are subtracted to move + the center of the obstacles behind the ball instead of its center + ''' + if self.robot.y < math.tan(j) * (self.robot.x - target[0]) + target[1]: + virtual_obstacle = ( + target[0] - p * math.cos(m) - r * math.cos(j), + target[1] - p * math.sin(m) - r * math.sin(j), + p, + 1 + ) + else: + virtual_obstacle = ( + target[0] + p * math.cos(m) - r * math.cos(j), + target[1] + p * math.sin(m) - r * math.sin(j), + p, + -1 + ) + + return virtual_obstacle + + def behind_ball(self, aim_point): + # Convert input to numpy arrays for easy calculation + point_on_line = np.array((self.match.ball.x, self.match.ball.y)) + point_on_normal = np.array(aim_point) + point_to_check = np.array((self.robot.x, self.robot.y)) + + # Calculate the normal vector of the line + normal = point_on_normal - point_on_line + + # Calculate the vector from the point on the line to the point to check + vector_to_check = point_to_check - point_on_line + + # Calculate the dot product of the normal vector and the vector to check + dot_product = np.dot(normal, vector_to_check) + + # Check the sign of the dot product to determine if the point is to the right or left of the line + return dot_product > 0 + + +class CrossPlay(PlayerPlay): + def __init__(self, match, robot): + super().__init__(match, robot) + + def get_name(self): + return f"<{self.robot.get_name()} Cross Planning>" + + def start_up(self): + super().start_up() + controller = PID_W_control + controller_kwargs = {'V_MAX': 0, 'V_MIN': 0, 'W_MAX': 100000000000} + self.robot.strategy.controller = controller(self.robot, **controller_kwargs) + + def update(self): + if self.robot.y > .65: + ang_diff = self.robot.theta - math.pi/2.1 + else: + ang_diff = self.robot.theta + math.pi/2.1 + + x = self.robot.x + 0.5*math.cos(ang_diff) + y = self.robot.y + 0.5*math.sin(ang_diff) + + return x, y + + def start(self): + pass + + +class Wait(PlayerPlay): + def __init__(self, match, robot): + super().__init__(match, robot) + + def get_name(self): + return f"<{self.robot.get_name()} Position Planning>" + + def start_up(self): + super().start_up() + controller = PID_control + controller_kwargs={'V_MIN': 0, 'K_RHO': 75} + self.robot.strategy.controller = controller(self.robot, **controller_kwargs) + + def update(self): + # print(self.position()) + return self.position() + + def start(self): + pass + + def position(self): + a = (.35, 1.1) + b = (.35, 0.2) + + c = (self.robot.x, self.robot.y) + + d = [(r.x, r.y) for r in self.match.robots if r.strategy.name not in ["Main_Attacker", "Goalkeeper_RSM2023"]][0] + + # Calculate the distances between each robot and each fixed point + distance_c_a = math.sqrt((c[0] - a[0]) ** 2 + (c[1] - a[1]) ** 2) + distance_c_b = math.sqrt((c[0] - b[0]) ** 2 + (c[1] - b[1]) ** 2) + distance_d_a = math.sqrt((d[0] - a[0]) ** 2 + (d[1] - a[1]) ** 2) + distance_d_b = math.sqrt((d[0] - b[0]) ** 2 + (d[1] - b[1]) ** 2) + + # Assign the robots to the closer fixed point + if distance_c_a + distance_d_b < distance_c_b + distance_d_a: + return a + else: + return b + + +class LookAtBall(PlayerPlay): + def __init__(self, match, robot): + super().__init__(match, robot) + + def get_name(self): + return f"<{self.robot.get_name()} Looking at the ball>" + + def start_up(self): + super().start_up() + controller = PID_W_control + controller_kwargs = {'V_MIN': 0, 'V_MAX': 0} + self.robot.strategy.controller = controller(self.robot, **controller_kwargs) + + def update(self): + return self.match.ball.x, self.match.ball.y + + def start(self): + pass + + +class MainStriker(Strategy): + def __init__(self, match, name="Main_Attacker"): + super().__init__(match, name, controller=PID_W_control) + + self.playerbook = None + + def start(self, robot=None): + super().start(robot=robot) + + field_dim = self.match.game.field.get_dimensions() + + # Criando Player Playbook: A maquina de estados do jogador + self.playerbook = PlayerPlaybook(self.match.coach, self.robot) + + main = MainPlay(self.match, self.robot) + main.start() + wing = WingPlay(self.match, self.robot) + wing.start() + cross = CrossPlay(self.match, self.robot) + cross.start() + defensive = Wait(self.match, self.robot) + defensive.start() + angle = LookAtBall(self.match, self.robot) + + self.playerbook.add_play(main) + self.playerbook.add_play(wing) + self.playerbook.add_play(cross) + self.playerbook.add_play(defensive) + self.playerbook.add_play(angle) + + on_wing = OnInsideBox(self.match, [0, 0.3, 1.5, 0.7], True) + on_cross_region = OnInsideBox(self.match, [1.4, 0, .15, 1.3]) + on_near_ball = OnNextTo(self.match.ball, self.robot, 0.1) + on_defensive_box = OnInsideBox(self.match, [-.2, .3, .35, .7]) + on_positon_1 = OnNextTo([.35, 1.1], self.robot, 0.1) + on_positon_2 = OnNextTo([.35, .2], self.robot, 0.1) + + main.add_transition(on_wing, wing) + main.add_transition(AndTransition([on_near_ball, on_cross_region, on_wing]), cross) + main.add_transition(on_defensive_box, defensive) + + main.add_transition(AndTransition([on_positon_1, on_defensive_box]), angle) + main.add_transition(AndTransition([on_positon_2, on_defensive_box]), angle) + + # Estado inicial + self.playerbook.set_play(main) + + def decide(self): + res = self.playerbook.update() + return res diff --git a/strategy/rcx2023/Goalkeeper.py b/strategy/rcx2023/Goalkeeper.py new file mode 100644 index 00000000..ca79b6cb --- /dev/null +++ b/strategy/rcx2023/Goalkeeper.py @@ -0,0 +1,471 @@ +import math +import numpy as np +from strategy.BaseStrategy import Strategy +from strategy.utils.player_playbook import PlayerPlay, PlayerPlaybook, OnInsideBox, AndTransition, OrTransition, \ + NotTransition, OnNextTo +from entities.plays.playbook import Trigger +from controller import PID_control, PID_W_control +from commons.math import point_in_rect + + +class Returning_to_Goal(PlayerPlay): + def __init__(self, match, robot): + super().__init__(match, robot) + + def get_name(self): + return f"<{self.robot.get_name()} Return to Goal>" + + def start_up(self): + super().start_up() + controller = PID_W_control + # controller_kwargs = { + # 'V_MAX': 100, + # 'V_MIN': 100 + # } + self.robot.strategy.controller = controller(self.robot) # , **controller_kwargs) + + def update(self): + return super().update() + + def start(self): + pass + + def update(self): + ball = self.match.ball + + y_pos = max(.45, min(ball.y, .85)) + + return (.08, y_pos) + + +class Aligning_angle(PlayerPlay): + def __init__(self, match, robot): + super().__init__(match, robot) + + def get_name(self): + return f"<{self.robot.get_name()} Align angle>" + + def start_up(self): + super().start_up() + controller = PID_control + controller_kwargs = { + 'V_MAX': 0, + 'V_MIN': 0 + } + self.robot.strategy.controller = controller(self.robot, **controller_kwargs) + + def update(self): + return super().update() + + def start(self): + pass + + def update(self): + return self.robot.x, self.robot.y + .05 + + +class Spinning(PlayerPlay): + def __init__(self, match, robot): + super().__init__(match, robot) + + self.field_w, self.field_h = self.match.game.field.get_dimensions() + + def get_name(self): + return f"<{self.robot.get_name()} Spin>" + + def start_up(self): + super().start_up() + controller = PID_control + controller_kwargs = { + 'V_MAX': 0, + 'V_MIN': 0, + 'W_MAX': 50000 + } + self.robot.strategy.controller = controller(self.robot, **controller_kwargs) + + def update(self): + return super().update() + + def start(self): + pass + + def update(self): + ball = self.match.ball + side = np.sign(self.robot.y - ball.y) + x = self.robot.x + 0.5 * math.cos(self.robot.theta + side * math.pi / 2) + y = self.robot.y + 0.5 * math.sin(self.robot.theta + side * math.pi / 2) + return x, y + + +class PushBall(PlayerPlay): + def __init__(self, match, robot): + super().__init__(match, robot) + + def get_name(self): + return f"<{self.robot.get_name()} Push Ball>" + + def start_up(self): + super().start_up() + controller = PID_control + controller_kwargs = { + 'V_MAX': 200, + 'V_MIN': 200 + } + self.robot.strategy.controller = controller(self.robot, **controller_kwargs) + + def update(self): + return super().update() + + def start(self): + pass + + def update(self): + ball = self.match.ball + return ball.x, ball.y + + +class BallInCorner(PlayerPlay): + def __init__(self, match, robot): + super().__init__(match, robot) + + self.field_w, self.field_h = self.match.game.field.get_dimensions() + + def get_name(self): + return f"<{self.robot.get_name()} Defend Ball in Corner>" + + def start_up(self): + super().start_up() + controller = PID_control + controller_kwargs = { + 'K_RHO': 300, + 'V_MAX': 200, + 'V_MIN': 75, + 'W_MAX': 0 + } + self.robot.strategy.controller = controller(self.robot, **controller_kwargs) + + def update(self): + return super().update() + + def start(self): + pass + + def update(self): + ball = self.match.ball + side = np.sign(ball.y - self.robot.y) + return self.robot.x, self.field_h / 2 + side * 0.2 + + +class FollowBallPlay(PlayerPlay): + def __init__(self, match, robot): + super().__init__(match, robot) + + self.field_w, self.field_h = self.match.game.field.get_dimensions() + + self.goal_vertical_line = .15 + + self.goal_left = self.field_h / 2 + .2 + self.goal_right = self.field_h / 2 - .2 + + def get_name(self): + return f"<{self.robot.get_name()} Follow Ball>" + + def start_up(self): + super().start_up() + controller = PID_control + controller_kwargs = { + 'K_RHO': 300, + 'V_MAX': 200, + 'V_MIN': 75, + 'W_MAX': 0 + } + self.robot.strategy.controller = controller(self.robot, **controller_kwargs) + + def update(self): + return super().update() + + def start(self): + pass + + def update(self): + ball = self.match.ball + + projection_rate = (ball.x - .15) / (1 - .15) + + # projection_limit = 0.15*projection_rate + + projection_point = ball.y + projection_rate * ball.vy + + # bounded_projection = min( max( projection_point, ball.y - projection_limit), ball.y + projection_limit ) + + y = min(max(projection_point, self.goal_right), self.goal_left) + + return self.robot.x, y + + +class ForwardCornerPlay(PlayerPlay): + def __init__(self, match, robot): + super().__init__(match, robot) + + self.field_w, self.field_h = self.match.game.field.get_dimensions() + + self.goal_vertical_line = .15 + + self.goal_left = self.field_h / 2 + .2 + self.goal_right = self.field_h / 2 - .2 + + self.goal_area_left = self.field_h / 2 + .7 / 2 + self.goal_area_right = self.field_h / 2 - .7 / 2 + + def get_name(self): + return f"<{self.robot.get_name()} Forward Corners Play>" + + def start_up(self): + super().start_up() + controller = PID_control + controller_kwargs = { + 'K_RHO': 300, + 'V_MAX': 200, + 'V_MIN': 75, + 'W_MAX': 0 + } + self.robot.strategy.controller = controller(self.robot, **controller_kwargs) + + def update(self): + return super().update() + + def start(self): + pass + + def update(self): + ball = self.match.ball + + side = np.sign(ball.y - self.field_h / 2) + + y = self.field_h / 2 + 0.2 * side + + return self.robot.x, y + + +class AttackerAreaPlay(PlayerPlay): + def __init__(self, match, robot): + super().__init__(match, robot) + + def get_name(self): + return f"<{self.robot.get_name()} Attacker Area>" + + def start_up(self): + super().start_up() + controller = PID_control + controller_kwargs = { + 'K_RHO': 300, + 'V_MAX': 200, + 'V_MIN': 75 + } + self.robot.strategy.controller = controller(self.robot, **controller_kwargs) + + def update(self): + return super().update() + + def start(self): + pass + + def update(self): + ball = self.match.ball + + y_pos = max(.45, min(ball.y, .85)) + + return (.08, y_pos) + + +class StationaryPlay(PlayerPlay): + def __init__(self, match, robot): + super().__init__(match, robot) + + def get_name(self): + return f"<{self.robot.get_name()} Stationary>" + + def start_up(self): + super().start_up() + controller = PID_control + controller_kwargs = { + 'V_MAX': 0, + 'V_MIN': 0 + } + self.robot.strategy.controller = controller(self.robot, **controller_kwargs) + + def update(self): + return super().update() + + def start(self): + pass + + def update(self): + return self.robot.x, self.robot.y + + +class Goalkeeper(Strategy): + def __init__(self, match): + super().__init__(match, "Goalkeeper_RSM2023", controller=PID_control) + + def start(self, robot=None): + super().start(robot=robot) + + self.playerbook = PlayerPlaybook(self.match.coach, self.robot) + + aligning_angle = Aligning_angle(self.match, self.robot) # 1 + aligning_angle.start() + + returning_to_goal = Returning_to_Goal(self.match, self.robot) # 2 + returning_to_goal.start() + + follow_ball = FollowBallPlay(self.match, self.robot) # 3 + follow_ball.start() + + defend_corner = BallInCorner(self.match, self.robot) # 4 + defend_corner.start() + + forward_corner = ForwardCornerPlay(self.match, self.robot) # 5 + forward_corner.start() + + pushing_ball = PushBall(self.match, self.robot) # 6 + pushing_ball.start() + + attacker_area = AttackerAreaPlay(self.match, self.robot) # ? + attacker_area.start() + + stationary = StationaryPlay(self.match, self.robot) # 7 + stationary.start() + + self.playerbook.add_play(returning_to_goal) + self.playerbook.add_play(aligning_angle) + self.playerbook.add_play(pushing_ball) + self.playerbook.add_play(defend_corner) + self.playerbook.add_play(follow_ball) + self.playerbook.add_play(forward_corner) + self.playerbook.add_play(attacker_area) + self.playerbook.add_play(stationary) + + not_aligned = OnAlignment(self.robot, .4 * math.pi / 2, True) # 1 + aligned = OnAlignment(self.robot, .1 * math.pi / 2) # !1 + outside_goal_area = OnReposition(self.robot) # 2 + on_follow_ball = OnInsideBox(self.match, [.15, .4, .6, .7]) # 3 + on_corner = OrTransition([ # 4 + OnInsideBox(self.match, [0, 1, .15, .3]), + OnInsideBox(self.match, [0, 0, .15, .3]) + ]) + on_forward_corner = OrTransition([ # 5 + OnInsideBox(self.match, [.15, 1, .6, .3]), + OnInsideBox(self.match, [.15, 0, .6, .3]) + ]) + on_goal_area = OnInsideBox(self.match, [0, .3, .15, .7]) # 6 + + on_attack_area = OnInsideBox(self.match, [.75, 0, .75, 1.3]) # 7 + + on_spot = OnNextTo(self.robot, [.075, .65], 0.1) # ? + + aligning_angle.add_transition(outside_goal_area, returning_to_goal) # 1 -> 2 + aligning_angle.add_transition(AndTransition( # 1 -> 3 + [aligned, on_follow_ball] + ), follow_ball) + aligning_angle.add_transition(AndTransition( # 1 -> 4 + [aligned, on_corner] + ), defend_corner) + aligning_angle.add_transition(AndTransition( # 1 -> 5 + [aligned, on_forward_corner] + ), forward_corner) + aligning_angle.add_transition(on_goal_area, pushing_ball) # 1 -> 6 + aligning_angle.add_transition(on_attack_area, attacker_area) # 1 -> 6 + + returning_to_goal.add_transition(AndTransition( # 2 -> 1 + [NotTransition(outside_goal_area), not_aligned] + ), aligning_angle) + returning_to_goal.add_transition(NotTransition(outside_goal_area), aligning_angle) + + follow_ball.add_transition(not_aligned, aligning_angle) # 3 -> 1 + follow_ball.add_transition(AndTransition([on_attack_area, on_spot]), aligning_angle) + follow_ball.add_transition(outside_goal_area, attacker_area) # 3 -> 2 + follow_ball.add_transition(on_corner, defend_corner) # 3 -> 4 + follow_ball.add_transition(on_forward_corner, forward_corner) # 3 -> 5 + follow_ball.add_transition(on_goal_area, pushing_ball) # 3 -> 6 + follow_ball.add_transition(AndTransition([NotTransition(on_spot), on_attack_area]), attacker_area) # 3 -> ? + + defend_corner.add_transition(not_aligned, aligning_angle) # 4 -> 1 + defend_corner.add_transition(outside_goal_area, returning_to_goal) # 4 -> 2 + defend_corner.add_transition(on_follow_ball, follow_ball) # 4 -> 3 + defend_corner.add_transition(on_forward_corner, forward_corner) # 4 -> 5 + defend_corner.add_transition(on_goal_area, pushing_ball) # 4 -> 6 + + forward_corner.add_transition(not_aligned, aligning_angle) # 5 -> 1 + forward_corner.add_transition(outside_goal_area, returning_to_goal) # 5 -> 2 + forward_corner.add_transition(on_follow_ball, follow_ball) # 5 -> 3 + forward_corner.add_transition(on_corner, defend_corner) # 5 -> 4 + forward_corner.add_transition(on_goal_area, pushing_ball) # 5 -> 6 + forward_corner.add_transition(on_attack_area, attacker_area) # 5 -> ? + + pushing_ball.add_transition(AndTransition( # 6 -> 2 + [NotTransition(on_goal_area), outside_goal_area] + ), returning_to_goal) + + attacker_area.add_transition(on_spot, aligning_angle) # ? -> 7 + + stationary.add_transition(not_aligned, aligning_angle) # 7 -> 1 + stationary.add_transition(outside_goal_area, returning_to_goal) # 7 -> 2 + stationary.add_transition(on_follow_ball, follow_ball) # 7 -> 3 + stationary.add_transition(on_forward_corner, forward_corner) # 7 -> 5 + stationary.add_transition(NotTransition(on_spot), attacker_area) # 7 -> ? + + if self.playerbook.actual_play == None: + self.playerbook.set_play(follow_ball) + + def reset(self, robot=None): + super().reset() + if robot: + self.start(robot) + + def decide(self): + res = self.playerbook.update() + # print(self.playerbook.actual_play) + return res + + +class OnAlignment(Trigger): + def __init__(self, robot, tolerance, outside=False): + super().__init__() + self.robot = robot + self.outside = outside + self.tolerance = tolerance + + def evaluate(self, *args, **kwargs): + robot_theta = self.robot.theta + robot_v_theta = self.robot.vtheta + + if self.outside: + in_between = (math.pi + self.tolerance < robot_theta < -math.pi + self.tolerance or + -self.tolerance < robot_theta < self.tolerance) + + else: + in_between = (math.pi / 2 - self.tolerance < robot_theta < math.pi / 2 + self.tolerance or + -math.pi / 2 - self.tolerance < robot_theta < -math.pi / 2 + self.tolerance) + + return in_between and robot_v_theta < .1 + + +class OnReposition(Trigger): + def __init__(self, robot): + super().__init__() + self.robot = robot + self.box = [0, .3, .15, .7] # x1, y1, w, h + + def evaluate(self, *args, **kwargs): + return not point_in_rect([self.robot.x, self.robot.y], self.box) + + +class RobotOnSpot(Trigger): + def __init__(self, robot): + super().__init__() + self.robot = robot + self.box = [0, .6, .15, .1] # x1, y1, w, h + + def evaluate(self, *args, **kwargs): + return point_in_rect([self.robot.x, self.robot.y], self.box) diff --git a/strategy/rcx2023/__init__.py b/strategy/rcx2023/__init__.py new file mode 100644 index 00000000..31844f7a --- /dev/null +++ b/strategy/rcx2023/__init__.py @@ -0,0 +1,5 @@ +# Baseline de estrategias +from strategy.rcx2023.Goalkeeper import Goalkeeper +from strategy.rcx2023.Attacker import MainStriker +from strategy.rcx2023.SecondAttacker import ShadowAttacker +# from strategy.rcx2023.Unstuck import Unstuck From 2b27d2461f945df3076bcdebe4c957a284ef4dae Mon Sep 17 00:00:00 2001 From: Joao Pedro Leal Date: Sat, 22 Jul 2023 18:13:37 -0300 Subject: [PATCH 09/16] fix(api): undo changes to make interface work again --- main_real_life.py | 22 +++++++++++----------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/main_real_life.py b/main_real_life.py index 5c79ba29..91635b13 100644 --- a/main_real_life.py +++ b/main_real_life.py @@ -33,7 +33,7 @@ def __init__(self, config_file=None, env='real_life'): self.referee = RefereeComm(config_file) self.api = Api(self.api_address, self.api_port) - # self.api_recv = Api_recv(self.match, self.api_address, self.api_recv_port) + self.api_recv = Api_recv(self.match, self.api_address, self.api_recv_port) self.use_referee = False @@ -48,7 +48,7 @@ def start(self): if self.use_api: self.api.start() - # self.api_recv.start() + self.api_recv.start() def update(self): frame = assign_empty_values( @@ -62,15 +62,15 @@ def update(self): self.match.update(frame) commands = self.match.decide() - # if self.use_api and (self.match.game_status == 'STOP' or self.match.game_status == None): - # commands = [ - # { - # 'robot_id': r['robot_id'], - # 'color': r['color'], - # 'wheel_left': 0, - # 'wheel_right': 0 - # } for r in commands - # ] + if self.use_api and (self.match.game_status == 'STOP' or self.match.game_status == None): + commands = [ + { + 'robot_id': r['robot_id'], + 'color': r['color'], + 'wheel_left': 0, + 'wheel_right': 0 + } for r in commands + ] self.comm.send(commands) From 5799a42593ca5ddb839cb2ddebfb42e793637a53 Mon Sep 17 00:00:00 2001 From: Joao Pedro Leal Date: Sat, 22 Jul 2023 18:17:04 -0300 Subject: [PATCH 10/16] fix(control): change moving directions --- controller/PID_control.py | 2 +- controller/uni_controller.py | 9 ++++++--- 2 files changed, 7 insertions(+), 4 deletions(-) diff --git a/controller/PID_control.py b/controller/PID_control.py index 9cf93527..f8f9d730 100644 --- a/controller/PID_control.py +++ b/controller/PID_control.py @@ -146,7 +146,7 @@ def _update(self): self.alpha_old = alpha - return v, w + return -v, w def update(self): v, w = self._update() diff --git a/controller/uni_controller.py b/controller/uni_controller.py index 27136a42..0b0c8996 100644 --- a/controller/uni_controller.py +++ b/controller/uni_controller.py @@ -41,7 +41,7 @@ class UniController(object): 'real_life': { 'V_M': 0.5, 'R_M': 0.44, # 20 * V_M - 'K_W': 4, # 313, + 'K_W': 3.5, # 313, 'K_P': 1 } } @@ -133,5 +133,8 @@ def update(self): if self.environment == 'simulation': return tuple(np.dot(250, speed_to_power(v, w, self.L, self.R))) - - return v, -w + + + # w = w if abs(w) < 4 else 4 * w / abs(w) + print(w) + return -v, -w From 8d82d7bee155c09a5d09a63c99e583c95ca3aa8f Mon Sep 17 00:00:00 2001 From: Joao Pedro Leal Date: Sat, 22 Jul 2023 18:19:59 -0300 Subject: [PATCH 11/16] feat(rsm_2023): make attacker univector with unicontroller --- strategy/rcx2023/Attacker.py | 128 ++++++++++------------------------- 1 file changed, 34 insertions(+), 94 deletions(-) diff --git a/strategy/rcx2023/Attacker.py b/strategy/rcx2023/Attacker.py index d5080fc5..066181c0 100644 --- a/strategy/rcx2023/Attacker.py +++ b/strategy/rcx2023/Attacker.py @@ -1,7 +1,9 @@ import numpy as np from algorithms.limit_cycle import LimitCycle +from algorithms.univector_field import UnivectorField import math from controller.PID_control import PID_W_control, PID_control +from controller.uni_controller import UniController from strategy.BaseStrategy import Strategy from strategy.utils.player_playbook import PlayerPlay, PlayerPlaybook, OnInsideBox, OnNextTo, AndTransition from commons.math import distance_between_points @@ -11,105 +13,42 @@ class MainPlay(PlayerPlay): def __init__(self, match, robot): # super().__init__(match, "Main_Attacker", controller=PID_W_control) super().__init__(match, robot) + self.dl = 0.000001 def get_name(self): return f"<{self.robot.get_name()} Main Attacker Planning>" def start_up(self): super().start_up() - controller = PID_W_control + controller = UniController self.robot.strategy.controller = controller(self.robot) def start(self): - self.limit_cycle = LimitCycle(self.match) + self.univector = UnivectorField(n=6, rect_size=.1) self.field_w, self.field_h = self.match.game.field.get_dimensions() def update(self): - ball = (self.match.ball.x, self.match.ball.y) - robot = (self.robot.x, self.robot.y) - - if distance_between_points(ball, robot) < 0.1 and not self.behind_ball(): - desired = (1.5, 0.65) - else: - self.limit_cycle.set_target(ball) - self.limit_cycle.add_obstacle(self.get_virtual_obstacle(ball)) - try: - desired = self.limit_cycle.compute(self.robot, fitness=20) - except ZeroDivisionError: - print("ZeroDivisionError on attacker") - desired = list(robot) - - return desired - - def get_virtual_obstacle(self, target): - ''' - - m: angle of the line perpendicular to the line between the ball and - the center of the goal - - p: distance of the virtual obstacles to the ball / radius of the virtual obstacle - - vo: virtual obstacle - - j: this is the angle between the ball and the center of the goal - - m: the normal angle perpendicular to j - - r: radius of the ball - ''' - aim_point = [self.field_w, self.field_h / 2] - - j = math.atan2(aim_point[1] - target[1], aim_point[0] - target[0]) - m = j + math.pi / 2 - p = 0.1 - - r = .0427 / 2 - - ''' - the terms r*cos(j) and r*sin(j) are subtracted to move - the center of the obstacles behind the ball instead of its center - ''' - if self.robot.y < math.tan(j) * (self.robot.x - target[0]) + target[1]: - virtual_obstacle = ( - target[0] - p * math.cos(m) - r * math.cos(j), - target[1] - p * math.sin(m) - r * math.sin(j), - p, - 1 - ) - else: - virtual_obstacle = ( - target[0] + p * math.cos(m) - r * math.cos(j), - target[1] + p * math.sin(m) - r * math.sin(j), - p, - -1 - ) - - if self.behind_ball(): - virtual_obstacle = ( - target[0] - p * math.cos(m) - r * math.cos(j), - target[1] - p * math.sin(m) - r * math.sin(j), - 2 * p, - np.sign(target[1] - p * math.sin(m) - r * math.sin(j) - 0.65) - ) - - return virtual_obstacle - - def behind_ball(self): - # Convert input to numpy arrays for easy calculation - point_on_line = np.array((self.match.ball.x, self.match.ball.y)) - point_on_normal = np.array((1.5, .65)) - point_to_check = np.array((self.robot.x, self.robot.y)) + ball_x, ball_y = self.match.ball.x, self.match.ball.y + theta_ball = math.atan2(0.65 - ball_y, 1.6 - ball_x) + ball_rx, ball_ry = ball_x + .05 * math.cos(theta_ball), ball_y + .05 * math.sin(theta_ball) - # Calculate the normal vector of the line - normal = point_on_normal - point_on_line + self.univector.set_target(g=(ball_x, ball_y), r=(ball_rx, ball_ry)) - # Calculate the vector from the point on the line to the point to check - vector_to_check = point_to_check - point_on_line + x, y = self.robot.x, self.robot.y - # Calculate the dot product of the normal vector and the vector to check - dot_product = np.dot(normal, vector_to_check) + theta_d = self.univector.compute((x, y)) + theta_f = self.univector.compute( + (x + self.dl * math.cos(self.robot.theta), + y + self.dl * math.sin(self.robot.theta)) + ) - # Check the sign of the dot product to determine if the point is to the right or left of the line - return dot_product > 0 + return theta_d, theta_f class WingPlay(PlayerPlay): def __init__(self, match, robot): super().__init__(match, robot) + self.dl = 0.000001 self.BALL_Y_MIN = 0.05 self.BALL_Y_MAX = 1.25 @@ -119,34 +58,34 @@ def get_name(self): def start_up(self): super().start_up() - controller = PID_W_control + controller = UniController self.robot.strategy.controller = controller(self.robot) def start(self): - self.limit_cycle = LimitCycle(self.match) + self.univector = UnivectorField(n=6, rect_size=.1) self.field_w, self.field_h = self.match.game.field.get_dimensions() def update(self): target = [self.match.ball.x, self.match.ball.y] - robot = (self.robot.x, self.robot.y) target[1] = min(self.BALL_Y_MAX, target[1]) target[1] = max(self.BALL_Y_MIN, target[1]) - aim_point = [self.field_w, self.BALL_Y_MAX] if target[1] > .65 else [self.field_w, self.BALL_Y_MIN] + ball_x, ball_y = target[0], target[1] + theta_ball = math.atan2(target[1] - ball_y, 1.6 - ball_x) + ball_rx, ball_ry = ball_x + .05 * math.cos(theta_ball), ball_y + .05 * math.sin(theta_ball) - if distance_between_points(target, robot) < 0.1 and not self.behind_ball(aim_point): - desired = aim_point - else: - self.limit_cycle.set_target(target) - self.limit_cycle.add_obstacle(self.get_virtual_obstacle(target)) - try: - desired = self.limit_cycle.compute(self.robot, fitness=20) - except ZeroDivisionError: - print("ZeroDivisionError on attacker") - desired = list(robot) + self.univector.set_target(g=(ball_x, ball_y), r=(ball_rx, ball_ry)) + + x, y = self.robot.x, self.robot.y + + theta_d = self.univector.compute((x, y)) + theta_f = self.univector.compute( + (x + self.dl * math.cos(self.robot.theta), + y + self.dl * math.sin(self.robot.theta)) + ) - return desired + return theta_d, theta_f def get_virtual_obstacle(self, target): """ @@ -244,7 +183,7 @@ def get_name(self): def start_up(self): super().start_up() controller = PID_control - controller_kwargs={'V_MIN': 0, 'K_RHO': 75} + controller_kwargs={'V_MIN': 0, 'K_RHO': 1.5} self.robot.strategy.controller = controller(self.robot, **controller_kwargs) def update(self): @@ -344,4 +283,5 @@ def start(self, robot=None): def decide(self): res = self.playerbook.update() + print(self.playerbook.actual_play) return res From 25266161010858197f25f7db0dd43f4d138d5082 Mon Sep 17 00:00:00 2001 From: Joao Pedro Leal Date: Sat, 22 Jul 2023 18:20:15 -0300 Subject: [PATCH 12/16] feat(rsm_2023): add second attacker --- strategy/rcx2023/SecondAttacker.py | 149 +++++++++++++++++++++++++++++ 1 file changed, 149 insertions(+) create mode 100644 strategy/rcx2023/SecondAttacker.py diff --git a/strategy/rcx2023/SecondAttacker.py b/strategy/rcx2023/SecondAttacker.py new file mode 100644 index 00000000..b5e60b87 --- /dev/null +++ b/strategy/rcx2023/SecondAttacker.py @@ -0,0 +1,149 @@ +from algorithms.univector_field import UnivectorField +import math +from controller.uni_controller import UniController +from strategy.BaseStrategy import Strategy +from strategy.utils.player_playbook import PlayerPlaybook, PlayerPlay, OnInsideBox, OnNextTo, AndTransition +from commons.math import distance_between_points +from controller.PID_control import PID_control, PID_W_control + + +class MainPlay(PlayerPlay): + def __init__(self, match, robot): + super().__init__(match, robot) + + def get_name(self): + return f"<{self.robot.get_name()} Shadow Position Planning>" + + def start_up(self): + super().start_up() + controller = UniController + controller_kwargs = {'control_speed': True} + self.robot.strategy.controller = controller(self.robot, **controller_kwargs) + + def update(self): + ball = (self.match.ball.x, self.match.ball.y) + main_st = [[i.x, i.y] for i in self.match.robots if i.strategy.name == "Main_Attacker"][0] + obs_radius = distance_between_points(main_st, ball) + target = main_st[:] + gk = [[i.x, i.y] for i in self.match.robots if i.strategy.name == "Goalkeeper_RSM2023"][0] + + # second attacker offset on x based on the distance of the main attacker to the ball + target[0] -= max(4*0.075, obs_radius) + # second attacker offset on y based on the distance of the ball to the center + target[1] += .5*(.65-ball[1]) + + self.univector_field.set_target(target, ball) + self.univector_field.del_obstacle(all=True) + self.univector_field.add_obstacle(main_st, 0.075*1.4, obs_radius-0.075*1.4) + self.univector_field.add_obstacle(gk, 0.075*1.4, 0.1) + + x, y = self.robot.x, self.robot.y + theta_d = self.univector_field.compute((x, y)) + theta_f = self.univector_field.compute((x + self.dl * math.cos(self.robot.theta), y + self.dl * math.sin(self.robot.theta))) + + self.robot.strategy.controller.target = target + + return theta_d, theta_f + + def start(self): + self.univector_field = UnivectorField(n=.3, rect_size=.1) + self.dl = 0.000001 + + +class Wait(PlayerPlay): + def __init__(self, match, robot): + super().__init__(match, robot) + + def get_name(self): + return f"<{self.robot.get_name()} Position Planning>" + + def start_up(self): + super().start_up() + controller = PID_control + controller_kwargs={'V_MIN': 0, 'K_RHO': 1.5} + self.robot.strategy.controller = controller(self.robot, **controller_kwargs) + + def update(self): + return self.position() + + def start(self): + pass + + def position(self): + a = (.35, 1.1) + b = (.35, 0.2) + + c = (self.robot.x, self.robot.y) + d = [(r.x, r.y) for r in self.match.robots if r.strategy.name == "Main_Attacker"][0] + + # Calculate the distances between each robot and each fixed point + distance_c_a = math.sqrt((c[0] - a[0]) ** 2 + (c[1] - a[1]) ** 2) + distance_c_b = math.sqrt((c[0] - b[0]) ** 2 + (c[1] - b[1]) ** 2) + distance_d_a = math.sqrt((d[0] - a[0]) ** 2 + (d[1] - a[1]) ** 2) + distance_d_b = math.sqrt((d[0] - b[0]) ** 2 + (d[1] - b[1]) ** 2) + + # Assign the robots to the closer fixed point + if distance_c_a + distance_d_b < distance_c_b + distance_d_a: + return a + else: + return b + + def start(self): + pass + + +class LookAtBall(PlayerPlay): + def __init__(self, match, robot): + super().__init__(match, robot) + + def get_name(self): + return f"<{self.robot.get_name()} Looking at the ball>" + + def start_up(self): + super().start_up() + controller = PID_W_control + controller_kwargs = {'V_MIN': 0, 'V_MAX': 0} + self.robot.strategy.controller = controller(self.robot, **controller_kwargs) + + def update(self): + return self.match.ball.x, self.match.ball.y + + def start(self): + pass + +class ShadowAttacker(Strategy): + def __init__(self, match, name="Shadow_Attacker"): + super().__init__(match, name, controller=UniController, controller_kwargs={'control_speed': True}) + + def start(self, robot=None): + super().start(robot=robot) + + # Criando Player Playbook: A maquina de estados do jogador + self.playerbook = PlayerPlaybook(self.match.coach, self.robot) + + main = MainPlay(self.match, self.robot) + main.start() + defensive = Wait(self.match, self.robot) + defensive.start() + angle = LookAtBall(self.match, self.robot) + angle.start() + + self.playerbook.add_play(main) + self.playerbook.add_play(defensive) + self.playerbook.add_play(angle) + + on_defensive_box = OnInsideBox(self.match, [-.2, .3, .35, .7]) + on_positon_1 = OnNextTo([.35, 1.1], self.robot, 0.1) + on_positon_2 = OnNextTo([.35, .2], self.robot, 0.1) + + main.add_transition(on_defensive_box, defensive) + + main.add_transition(AndTransition([on_positon_1, on_defensive_box]), angle) + main.add_transition(AndTransition([on_positon_2, on_defensive_box]), angle) + + # Estado inicial + self.playerbook.set_play(main) + + def decide(self): + res = self.playerbook.update() + return res From c8f8177007ac2a58e352261be92c398b912808b3 Mon Sep 17 00:00:00 2001 From: Joao Pedro Leal Date: Mon, 24 Jul 2023 16:23:06 -0300 Subject: [PATCH 13/16] feat(rsm_2023): create goalkeeper --- strategy/rcx2023/Goalkeeper.py | 415 +++++---------------------------- 1 file changed, 53 insertions(+), 362 deletions(-) diff --git a/strategy/rcx2023/Goalkeeper.py b/strategy/rcx2023/Goalkeeper.py index ca79b6cb..9d7a1346 100644 --- a/strategy/rcx2023/Goalkeeper.py +++ b/strategy/rcx2023/Goalkeeper.py @@ -4,156 +4,9 @@ from strategy.utils.player_playbook import PlayerPlay, PlayerPlaybook, OnInsideBox, AndTransition, OrTransition, \ NotTransition, OnNextTo from entities.plays.playbook import Trigger -from controller import PID_control, PID_W_control +from controller import PID_control, PID_W_control, UniController from commons.math import point_in_rect - - -class Returning_to_Goal(PlayerPlay): - def __init__(self, match, robot): - super().__init__(match, robot) - - def get_name(self): - return f"<{self.robot.get_name()} Return to Goal>" - - def start_up(self): - super().start_up() - controller = PID_W_control - # controller_kwargs = { - # 'V_MAX': 100, - # 'V_MIN': 100 - # } - self.robot.strategy.controller = controller(self.robot) # , **controller_kwargs) - - def update(self): - return super().update() - - def start(self): - pass - - def update(self): - ball = self.match.ball - - y_pos = max(.45, min(ball.y, .85)) - - return (.08, y_pos) - - -class Aligning_angle(PlayerPlay): - def __init__(self, match, robot): - super().__init__(match, robot) - - def get_name(self): - return f"<{self.robot.get_name()} Align angle>" - - def start_up(self): - super().start_up() - controller = PID_control - controller_kwargs = { - 'V_MAX': 0, - 'V_MIN': 0 - } - self.robot.strategy.controller = controller(self.robot, **controller_kwargs) - - def update(self): - return super().update() - - def start(self): - pass - - def update(self): - return self.robot.x, self.robot.y + .05 - - -class Spinning(PlayerPlay): - def __init__(self, match, robot): - super().__init__(match, robot) - - self.field_w, self.field_h = self.match.game.field.get_dimensions() - - def get_name(self): - return f"<{self.robot.get_name()} Spin>" - - def start_up(self): - super().start_up() - controller = PID_control - controller_kwargs = { - 'V_MAX': 0, - 'V_MIN': 0, - 'W_MAX': 50000 - } - self.robot.strategy.controller = controller(self.robot, **controller_kwargs) - - def update(self): - return super().update() - - def start(self): - pass - - def update(self): - ball = self.match.ball - side = np.sign(self.robot.y - ball.y) - x = self.robot.x + 0.5 * math.cos(self.robot.theta + side * math.pi / 2) - y = self.robot.y + 0.5 * math.sin(self.robot.theta + side * math.pi / 2) - return x, y - - -class PushBall(PlayerPlay): - def __init__(self, match, robot): - super().__init__(match, robot) - - def get_name(self): - return f"<{self.robot.get_name()} Push Ball>" - - def start_up(self): - super().start_up() - controller = PID_control - controller_kwargs = { - 'V_MAX': 200, - 'V_MIN': 200 - } - self.robot.strategy.controller = controller(self.robot, **controller_kwargs) - - def update(self): - return super().update() - - def start(self): - pass - - def update(self): - ball = self.match.ball - return ball.x, ball.y - - -class BallInCorner(PlayerPlay): - def __init__(self, match, robot): - super().__init__(match, robot) - - self.field_w, self.field_h = self.match.game.field.get_dimensions() - - def get_name(self): - return f"<{self.robot.get_name()} Defend Ball in Corner>" - - def start_up(self): - super().start_up() - controller = PID_control - controller_kwargs = { - 'K_RHO': 300, - 'V_MAX': 200, - 'V_MIN': 75, - 'W_MAX': 0 - } - self.robot.strategy.controller = controller(self.robot, **controller_kwargs) - - def update(self): - return super().update() - - def start(self): - pass - - def update(self): - ball = self.match.ball - side = np.sign(ball.y - self.robot.y) - return self.robot.x, self.field_h / 2 + side * 0.2 +from algorithms import UnivectorField class FollowBallPlay(PlayerPlay): @@ -174,23 +27,18 @@ def start_up(self): super().start_up() controller = PID_control controller_kwargs = { - 'K_RHO': 300, - 'V_MAX': 200, - 'V_MIN': 75, - 'W_MAX': 0 + 'K_RHO': 1, + 'V_MIN': 0, } self.robot.strategy.controller = controller(self.robot, **controller_kwargs) - def update(self): - return super().update() - def start(self): pass def update(self): ball = self.match.ball - projection_rate = (ball.x - .15) / (1 - .15) + projection_rate = 0.5 #(ball.x - .15) / (1 - .15) # projection_limit = 0.15*projection_rate @@ -200,110 +48,73 @@ def update(self): y = min(max(projection_point, self.goal_right), self.goal_left) - return self.robot.x, y + return 0.04, y -class ForwardCornerPlay(PlayerPlay): +class InsideArea(PlayerPlay): def __init__(self, match, robot): + # super().__init__(match, "Main_Attacker", controller=PID_W_control) super().__init__(match, robot) - - self.field_w, self.field_h = self.match.game.field.get_dimensions() - - self.goal_vertical_line = .15 - - self.goal_left = self.field_h / 2 + .2 - self.goal_right = self.field_h / 2 - .2 - - self.goal_area_left = self.field_h / 2 + .7 / 2 - self.goal_area_right = self.field_h / 2 - .7 / 2 + self.dl = 0.000001 def get_name(self): - return f"<{self.robot.get_name()} Forward Corners Play>" + return f"<{self.robot.get_name()} Inside Area Planning>" def start_up(self): super().start_up() - controller = PID_control - controller_kwargs = { - 'K_RHO': 300, - 'V_MAX': 200, - 'V_MIN': 75, - 'W_MAX': 0 - } - self.robot.strategy.controller = controller(self.robot, **controller_kwargs) - - def update(self): - return super().update() + controller = PID_W_control + self.robot.strategy.controller = controller(self.robot, W_MAX=10) def start(self): - pass + self.univector = UnivectorField(n=6, rect_size=.1) + self.field_w, self.field_h = self.match.game.field.get_dimensions() def update(self): ball = self.match.ball + theta_ball = math.atan2(0.3 - ball.y, 0.15 - ball.x) if ball.y < self.robot.y else math.atan2(1 - ball.y, 0.15 - ball.x) + ball_rx, ball_ry = ball.x + .05 * math.cos(theta_ball), ball.y + .05 * math.sin(theta_ball) - side = np.sign(ball.y - self.field_h / 2) + self.univector.set_target(g=(ball.x, ball.y), r=(ball_rx, ball_ry)) - y = self.field_h / 2 + 0.2 * side + x, y = self.robot.x, self.robot.y - return self.robot.x, y + theta_d = self.univector.compute((x, y)) + theta_f = self.univector.compute( + (x + self.dl * math.cos(self.robot.theta), + y + self.dl * math.sin(self.robot.theta)) + ) + + return ball.x, ball.y + return x+0.01*math.cos(theta_d), y+0.01*math.sin(theta_d) -class AttackerAreaPlay(PlayerPlay): +class Spin(PlayerPlay): def __init__(self, match, robot): super().__init__(match, robot) def get_name(self): - return f"<{self.robot.get_name()} Attacker Area>" + return f"<{self.robot.get_name()} Spin Planning>" def start_up(self): super().start_up() - controller = PID_control - controller_kwargs = { - 'K_RHO': 300, - 'V_MAX': 200, - 'V_MIN': 75 - } + controller = PID_W_control + controller_kwargs = {'V_MAX': 0, 'V_MIN': 0, 'W_MAX': 100000000000, 'KP':-10000000} self.robot.strategy.controller = controller(self.robot, **controller_kwargs) def update(self): - return super().update() - - def start(self): - pass - - def update(self): - ball = self.match.ball - - y_pos = max(.45, min(ball.y, .85)) - - return (.08, y_pos) - - -class StationaryPlay(PlayerPlay): - def __init__(self, match, robot): - super().__init__(match, robot) + if self.robot.y > .65: + ang_diff = self.robot.theta - math.pi/2.1 + else: + ang_diff = self.robot.theta + math.pi/2.1 - def get_name(self): - return f"<{self.robot.get_name()} Stationary>" + x = self.robot.x + 0.5*math.cos(ang_diff) + y = self.robot.y + 0.5*math.sin(ang_diff) - def start_up(self): - super().start_up() - controller = PID_control - controller_kwargs = { - 'V_MAX': 0, - 'V_MIN': 0 - } - self.robot.strategy.controller = controller(self.robot, **controller_kwargs) - - def update(self): - return super().update() + return x, y def start(self): pass - def update(self): - return self.robot.x, self.robot.y - - class Goalkeeper(Strategy): def __init__(self, match): super().__init__(match, "Goalkeeper_RSM2023", controller=PID_control) @@ -313,107 +124,29 @@ def start(self, robot=None): self.playerbook = PlayerPlaybook(self.match.coach, self.robot) - aligning_angle = Aligning_angle(self.match, self.robot) # 1 - aligning_angle.start() - - returning_to_goal = Returning_to_Goal(self.match, self.robot) # 2 - returning_to_goal.start() - follow_ball = FollowBallPlay(self.match, self.robot) # 3 follow_ball.start() - defend_corner = BallInCorner(self.match, self.robot) # 4 - defend_corner.start() + inside_area = InsideArea(self.match, self.robot) + inside_area.start() - forward_corner = ForwardCornerPlay(self.match, self.robot) # 5 - forward_corner.start() + spin = Spin(self.match, self.robot) + spin.start() - pushing_ball = PushBall(self.match, self.robot) # 6 - pushing_ball.start() + self.playerbook.add_play(follow_ball) + self.playerbook.add_play(inside_area) + self.playerbook.add_play(spin) - attacker_area = AttackerAreaPlay(self.match, self.robot) # ? - attacker_area.start() + on_near_ball = OnNextTo(self.match.ball, self.robot, 0.09) + off_near_ball = OnNextTo(self.match.ball, self.robot, 0.12, True) - stationary = StationaryPlay(self.match, self.robot) # 7 - stationary.start() + follow_ball.add_transition(OnInsideBox(self.match, [-.5, .3, .65, .7]), inside_area) + follow_ball.add_transition(on_near_ball, spin) - self.playerbook.add_play(returning_to_goal) - self.playerbook.add_play(aligning_angle) - self.playerbook.add_play(pushing_ball) - self.playerbook.add_play(defend_corner) - self.playerbook.add_play(follow_ball) - self.playerbook.add_play(forward_corner) - self.playerbook.add_play(attacker_area) - self.playerbook.add_play(stationary) - - not_aligned = OnAlignment(self.robot, .4 * math.pi / 2, True) # 1 - aligned = OnAlignment(self.robot, .1 * math.pi / 2) # !1 - outside_goal_area = OnReposition(self.robot) # 2 - on_follow_ball = OnInsideBox(self.match, [.15, .4, .6, .7]) # 3 - on_corner = OrTransition([ # 4 - OnInsideBox(self.match, [0, 1, .15, .3]), - OnInsideBox(self.match, [0, 0, .15, .3]) - ]) - on_forward_corner = OrTransition([ # 5 - OnInsideBox(self.match, [.15, 1, .6, .3]), - OnInsideBox(self.match, [.15, 0, .6, .3]) - ]) - on_goal_area = OnInsideBox(self.match, [0, .3, .15, .7]) # 6 - - on_attack_area = OnInsideBox(self.match, [.75, 0, .75, 1.3]) # 7 - - on_spot = OnNextTo(self.robot, [.075, .65], 0.1) # ? - - aligning_angle.add_transition(outside_goal_area, returning_to_goal) # 1 -> 2 - aligning_angle.add_transition(AndTransition( # 1 -> 3 - [aligned, on_follow_ball] - ), follow_ball) - aligning_angle.add_transition(AndTransition( # 1 -> 4 - [aligned, on_corner] - ), defend_corner) - aligning_angle.add_transition(AndTransition( # 1 -> 5 - [aligned, on_forward_corner] - ), forward_corner) - aligning_angle.add_transition(on_goal_area, pushing_ball) # 1 -> 6 - aligning_angle.add_transition(on_attack_area, attacker_area) # 1 -> 6 - - returning_to_goal.add_transition(AndTransition( # 2 -> 1 - [NotTransition(outside_goal_area), not_aligned] - ), aligning_angle) - returning_to_goal.add_transition(NotTransition(outside_goal_area), aligning_angle) - - follow_ball.add_transition(not_aligned, aligning_angle) # 3 -> 1 - follow_ball.add_transition(AndTransition([on_attack_area, on_spot]), aligning_angle) - follow_ball.add_transition(outside_goal_area, attacker_area) # 3 -> 2 - follow_ball.add_transition(on_corner, defend_corner) # 3 -> 4 - follow_ball.add_transition(on_forward_corner, forward_corner) # 3 -> 5 - follow_ball.add_transition(on_goal_area, pushing_ball) # 3 -> 6 - follow_ball.add_transition(AndTransition([NotTransition(on_spot), on_attack_area]), attacker_area) # 3 -> ? - - defend_corner.add_transition(not_aligned, aligning_angle) # 4 -> 1 - defend_corner.add_transition(outside_goal_area, returning_to_goal) # 4 -> 2 - defend_corner.add_transition(on_follow_ball, follow_ball) # 4 -> 3 - defend_corner.add_transition(on_forward_corner, forward_corner) # 4 -> 5 - defend_corner.add_transition(on_goal_area, pushing_ball) # 4 -> 6 - - forward_corner.add_transition(not_aligned, aligning_angle) # 5 -> 1 - forward_corner.add_transition(outside_goal_area, returning_to_goal) # 5 -> 2 - forward_corner.add_transition(on_follow_ball, follow_ball) # 5 -> 3 - forward_corner.add_transition(on_corner, defend_corner) # 5 -> 4 - forward_corner.add_transition(on_goal_area, pushing_ball) # 5 -> 6 - forward_corner.add_transition(on_attack_area, attacker_area) # 5 -> ? - - pushing_ball.add_transition(AndTransition( # 6 -> 2 - [NotTransition(on_goal_area), outside_goal_area] - ), returning_to_goal) - - attacker_area.add_transition(on_spot, aligning_angle) # ? -> 7 - - stationary.add_transition(not_aligned, aligning_angle) # 7 -> 1 - stationary.add_transition(outside_goal_area, returning_to_goal) # 7 -> 2 - stationary.add_transition(on_follow_ball, follow_ball) # 7 -> 3 - stationary.add_transition(on_forward_corner, forward_corner) # 7 -> 5 - stationary.add_transition(NotTransition(on_spot), attacker_area) # 7 -> ? + inside_area.add_transition(OnInsideBox(self.match, [-.5, .3, .75, .8], True), follow_ball) + inside_area.add_transition(on_near_ball, spin) + + spin.add_transition(off_near_ball, follow_ball) if self.playerbook.actual_play == None: self.playerbook.set_play(follow_ball) @@ -425,47 +158,5 @@ def reset(self, robot=None): def decide(self): res = self.playerbook.update() - # print(self.playerbook.actual_play) + print(self.playerbook.actual_play) return res - - -class OnAlignment(Trigger): - def __init__(self, robot, tolerance, outside=False): - super().__init__() - self.robot = robot - self.outside = outside - self.tolerance = tolerance - - def evaluate(self, *args, **kwargs): - robot_theta = self.robot.theta - robot_v_theta = self.robot.vtheta - - if self.outside: - in_between = (math.pi + self.tolerance < robot_theta < -math.pi + self.tolerance or - -self.tolerance < robot_theta < self.tolerance) - - else: - in_between = (math.pi / 2 - self.tolerance < robot_theta < math.pi / 2 + self.tolerance or - -math.pi / 2 - self.tolerance < robot_theta < -math.pi / 2 + self.tolerance) - - return in_between and robot_v_theta < .1 - - -class OnReposition(Trigger): - def __init__(self, robot): - super().__init__() - self.robot = robot - self.box = [0, .3, .15, .7] # x1, y1, w, h - - def evaluate(self, *args, **kwargs): - return not point_in_rect([self.robot.x, self.robot.y], self.box) - - -class RobotOnSpot(Trigger): - def __init__(self, robot): - super().__init__() - self.robot = robot - self.box = [0, .6, .15, .1] # x1, y1, w, h - - def evaluate(self, *args, **kwargs): - return point_in_rect([self.robot.x, self.robot.y], self.box) From ea58e3be25da53c82a5a7164a7e8fc371ea9c1d7 Mon Sep 17 00:00:00 2001 From: Joao Pedro Leal Date: Tue, 25 Jul 2023 13:03:53 -0300 Subject: [PATCH 14/16] fix(Univector field): fix import for univector field --- algorithms/__init__.py | 2 ++ controller/uni_controller.py | 1 - strategy/rcx2023/Attacker.py | 3 +-- strategy/rcx2023/SecondAttacker.py | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/algorithms/__init__.py b/algorithms/__init__.py index 9933ff56..fa276900 100644 --- a/algorithms/__init__.py +++ b/algorithms/__init__.py @@ -12,4 +12,6 @@ from algorithms import univector_field +from algorithms.univector_field import UnivectorField + from algorithms.RRT import rrt diff --git a/controller/uni_controller.py b/controller/uni_controller.py index 0b0c8996..d849add7 100644 --- a/controller/uni_controller.py +++ b/controller/uni_controller.py @@ -136,5 +136,4 @@ def update(self): # w = w if abs(w) < 4 else 4 * w / abs(w) - print(w) return -v, -w diff --git a/strategy/rcx2023/Attacker.py b/strategy/rcx2023/Attacker.py index 066181c0..5dac3e95 100644 --- a/strategy/rcx2023/Attacker.py +++ b/strategy/rcx2023/Attacker.py @@ -1,6 +1,6 @@ import numpy as np from algorithms.limit_cycle import LimitCycle -from algorithms.univector_field import UnivectorField +from algorithms import UnivectorField import math from controller.PID_control import PID_W_control, PID_control from controller.uni_controller import UniController @@ -283,5 +283,4 @@ def start(self, robot=None): def decide(self): res = self.playerbook.update() - print(self.playerbook.actual_play) return res diff --git a/strategy/rcx2023/SecondAttacker.py b/strategy/rcx2023/SecondAttacker.py index b5e60b87..67f09830 100644 --- a/strategy/rcx2023/SecondAttacker.py +++ b/strategy/rcx2023/SecondAttacker.py @@ -1,4 +1,4 @@ -from algorithms.univector_field import UnivectorField +from algorithms import UnivectorField import math from controller.uni_controller import UniController from strategy.BaseStrategy import Strategy From fd9acd160db76aecff8ee7c69f9da4170eb96f10 Mon Sep 17 00:00:00 2001 From: Joao Pedro Leal Date: Tue, 25 Jul 2023 16:10:08 -0300 Subject: [PATCH 15/16] feat(rcx 2023): add rest play for goalkeeper --- strategy/rcx2023/Goalkeeper.py | 33 ++++++++++++++++++++++++++++++++- 1 file changed, 32 insertions(+), 1 deletion(-) diff --git a/strategy/rcx2023/Goalkeeper.py b/strategy/rcx2023/Goalkeeper.py index 9d7a1346..34600f43 100644 --- a/strategy/rcx2023/Goalkeeper.py +++ b/strategy/rcx2023/Goalkeeper.py @@ -85,7 +85,6 @@ def update(self): ) return ball.x, ball.y - return x+0.01*math.cos(theta_d), y+0.01*math.sin(theta_d) class Spin(PlayerPlay): @@ -115,6 +114,32 @@ def update(self): def start(self): pass + +class Rest(PlayerPlay): + def __init__(self, match, robot): + super().__init__(match, robot) + + self.target = (0.04, .65) + def get_name(self): + return f"<{self.robot.get_name()} Rest>" + + def start_up(self): + super().start_up() + controller = PID_control + controller_kwargs = { + 'K_RHO': 1, + 'V_MIN': 0, + } + self.robot.strategy.controller = controller(self.robot, **controller_kwargs) + + def start(self): + pass + + def update(self): + + return self.target + + class Goalkeeper(Strategy): def __init__(self, match): super().__init__(match, "Goalkeeper_RSM2023", controller=PID_control) @@ -133,20 +158,26 @@ def start(self, robot=None): spin = Spin(self.match, self.robot) spin.start() + rest = Rest(self.match, self.robot) + rest.start() + self.playerbook.add_play(follow_ball) self.playerbook.add_play(inside_area) self.playerbook.add_play(spin) + self.playerbook.add_play(rest) on_near_ball = OnNextTo(self.match.ball, self.robot, 0.09) off_near_ball = OnNextTo(self.match.ball, self.robot, 0.12, True) follow_ball.add_transition(OnInsideBox(self.match, [-.5, .3, .65, .7]), inside_area) follow_ball.add_transition(on_near_ball, spin) + follow_ball.add_transition(OnInsideBox(self.match, [.75, -.3, 7, 1.9]), rest) inside_area.add_transition(OnInsideBox(self.match, [-.5, .3, .75, .8], True), follow_ball) inside_area.add_transition(on_near_ball, spin) spin.add_transition(off_near_ball, follow_ball) + rest.add_transition(OnInsideBox(self.match, [.75, -.3, 7, 1.9], True), follow_ball) if self.playerbook.actual_play == None: self.playerbook.set_play(follow_ball) From ebbd10c8a764187f498ae2d131add073a8e34d89 Mon Sep 17 00:00:00 2001 From: Joao Pedro Leal Date: Tue, 25 Jul 2023 16:10:38 -0300 Subject: [PATCH 16/16] feat(rcx 2023): remove stuck in coach --- entities/coach/rcx2023.py | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/entities/coach/rcx2023.py b/entities/coach/rcx2023.py index 844179ac..78308286 100644 --- a/entities/coach/rcx2023.py +++ b/entities/coach/rcx2023.py @@ -12,9 +12,9 @@ def __init__(self, match): self.SS_strategy = strategy.rcx2023.ShadowAttacker(self.match) self.ST_strategy = strategy.rcx2023.MainStriker(self.match) self.GK_strategy = strategy.rcx2023.Goalkeeper(self.match) - self.GK_id = 3 # Goalkeeper fixed ID + self.GK_id = 1 # Goalkeeper fixed ID - self.unstucks = {r.robot_id: strategy.rsm2023.Unstuck(self.match) for r in self.match.robots if r.robot_id != self.GK_id} + # self.unstucks = {r.robot_id: strategy.rsm2023.Unstuck(self.match) for r in self.match.robots if r.robot_id != self.GK_id} def decide(self): GK = [i for i, r in enumerate(self.match.robots) if r.robot_id is self.GK_id][0] @@ -58,13 +58,13 @@ def handle_stuck(self, ST, SS): stuck_st = ST.is_stuck() and game_runing stuck_ss = SS.is_stuck() and game_runing - if stuck_st and stuck_ss: - return self.unstucks[ST.robot_id], self.unstucks[SS.robot_id] - - if stuck_st and not stuck_ss: - return self.unstucks[ST.robot_id], self.ST_strategy - - if not stuck_st and stuck_ss: - return self.ST_strategy, self.unstucks[SS.robot_id] + # if stuck_st and stuck_ss: + # return self.unstucks[ST.robot_id], self.unstucks[SS.robot_id] + # + # if stuck_st and not stuck_ss: + # return self.unstucks[ST.robot_id], self.ST_strategy + # + # if not stuck_st and stuck_ss: + # return self.ST_strategy, self.unstucks[SS.robot_id] return self.ST_strategy, self.SS_strategy