diff --git a/.DS_Store b/.DS_Store new file mode 100644 index 00000000..1cd2c71b Binary files /dev/null and b/.DS_Store differ diff --git a/.gitignore b/.gitignore index 696fe4a4..a9632f93 100644 --- a/.gitignore +++ b/.gitignore @@ -16,12 +16,13 @@ todos.py *.c build/ binary/ - test.py - player-*.txt player-*.err -log .gitignore test-log main_tmp.py +.DS_Store +keepaway.egg-info/ +logs/ +results/ diff --git a/Dockerfile b/Dockerfile new file mode 100644 index 00000000..d027f70f --- /dev/null +++ b/Dockerfile @@ -0,0 +1,97 @@ +# Start from Ubuntu 20.04 +FROM ubuntu:20.04 + +# Avoid prompts from apt during build +ARG DEBIAN_FRONTEND=noninteractive +ENV DISPLAY docker.for.mac.host.internal:0 +ENV RCSS_CONF_DIR /home/rcsoccersim/.rcssserver +ENV LOG_DIR /home/rcsoccersim/logs +ENV TEAM_DIR /home/rcsoccersim/teams +USER root +RUN apt-get update && apt-get upgrade -y + +# Install build dependencies for rcssserver and soccerwindow2, including common dependencies +RUN apt-get install -y build-essential git autoconf libtool qt5-default libqt5opengl5-dev libaudio-dev libxt-dev libxi-dev libxmu-dev libpng-dev libglib2.0-dev libfontconfig1-dev libxrender-dev libxext-dev + +RUN apt-get update && apt-get install -y flex automake autoconf libtool flex bison libboost-all-dev + +# Clone, build, and install rcssserver +WORKDIR /root +RUN git clone https://github.com/rcsoccersim/rcssserver.git + +WORKDIR /root/rcssserver +RUN autoreconf -i && ./configure && make && make install && ldconfig + +RUN mkdir -p /root/src +WORKDIR /root/src +RUN git clone https://github.com/helios-base/librcsc.git \ + && cd librcsc \ + && ./bootstrap \ + && ./configure --disable-unit-test \ + && make \ + && make install && ldconfig + + +WORKDIR /root/src +RUN git clone https://github.com/helios-base/soccerwindow2.git \ + && cd soccerwindow2 \ + && autoreconf -i \ + && ./configure \ + && make \ + && make install && ldconfig + + +RUN apt-get update && apt-get install -y wget git bzip2 && \ + rm -rf /var/lib/apt/lists/* + + + +RUN wget https://repo.anaconda.com/miniconda/Miniconda3-latest-Linux-aarch64.sh -O /miniconda.sh && \ + bash /miniconda.sh -b -p /miniconda && \ + rm /miniconda.sh + +# Add Conda to PATH +ENV PATH="/miniconda/bin:${PATH}" + +# Create a Conda environment named 'keepaway' with Python 3.11 +RUN conda create -y --name keepaway python=3.11 + + +# Cleanup to reduce image size +RUN apt-get clean && rm -rf /var/lib/apt/lists/* /tmp/* /var/tmp/* + + +RUN useradd -d /home/rcsoccersim -m -s /bin/bash rcsoccersim \ + && echo "rcsoccersim:rcsoccersim" | chpasswd +RUN mkdir -p $RCSS_CONF_DIR $TEAM_DIR $LOG_DIR +RUN chown -R rcsoccersim:rcsoccersim /home/rcsoccersim +USER rcsoccersim + +VOLUME $TEAM_DIR +VOLUME $LOG_DIR + +WORKDIR $TEAM_DIR + +## install keepaway + +RUN git clone https://github.com/aabayomi/Pyrus2D.git \ + && cd Pyrus2D +SHELL ["conda", "run", "-n", "keepaway", "/bin/bash", "-c"] + +WORKDIR $TEAM_DIR/Pyrus2D + +COPY entrypoint.sh /usr/local/bin/entrypoint.sh + +#COPY requirements.txt /tmp/ +#RUN conda install --no-cache-dir -r requirements.txt +# Assume the Conda environment "keepaway" is already created +RUN conda run -n keepaway /bin/bash -c "pip install --no-cache-dir -r requirements.txt" + +RUN conda run -n keepaway /bin/bash -c "pip install -e ." + +WORKDIR $TEAM_DIR/Pyrus2D/keepaway + +ENTRYPOINT ["/usr/local/bin/entrypoint.sh"] + +CMD ["conda", "run", "-n", "keepaway", "python3", "examples/training_agent.py --gc=3v2 --policy=handcoded --num_episodes=1000000"] + diff --git a/Makefile b/Makefile new file mode 100755 index 00000000..994cf1e2 --- /dev/null +++ b/Makefile @@ -0,0 +1,13 @@ +cleanall: + rm -fr keeper_Q*.gz taker_Q*.gz logs/* *.lock core core.* vgcore.* + rm -fr *.lock console.log nohup.out *.dot *.xml + rm -fr keeper_Q*.log + rm -fr taker_Q*.log + rm -fr python_debug.log + rm -fr player* + rm -fr keepers-*.log + rm -fr takers-*.log + rm -fr NA-*.log + rm -fr *.rcg *.rcl + ./kill.sh + killall -q keepaway_player 1>/dev/null 2>&1 diff --git a/README.md b/README.md index de780d4c..7e1b798c 100755 --- a/README.md +++ b/README.md @@ -1,19 +1,31 @@ -# PYRUS2D +# Keepaway-Python -## Robocup Soccer Simmulation 2D Python Base Code +This project offers a python implementation of the RoboCup keepaway environment, designed for deep reinforcement learning research. The environment is based on the [CYRUS](https://arxiv.org/abs/2211.08585) Python based RoboCup 2D soccer simulator, enabling researchers and developers to test and develop multi-agent reinforcement learning algorithms. -PYRUS2D is the first Python base code (sample team) for RoboCup Soccer 2D Simulator. -This project is implemented by members of CYRUS soccer simulation 2D team. -By using this project, a team includes 11 players and one coach can connect to RoboCup Soccer Server and play a game. -Also, researchers can use the trainer to control the server for training proposes. +The published [paper](https://www.cs.utexas.edu/~pstone/Papers/bib2html/b2hd-AB05.html) and [code](https://github.com/tjpalmer/keepaway) outlines the motivation for keepaway for reinforcement learning and some initial research results using the environment. + + +To get started with this project, follow the instructions in the [Quick Start Guide](##Quick-Start-Guide). + +### Using Docker + +This is the recommended way to avoid incompatible package versions. Instructions are available [here](docs/docker.md). + + + + --- -## Dependencies +## Quick Start Guide +### On your computer +#### 1. Install required dependencies +#### Linux -### RoboCup Soccer Simulation Server and Monitor +This code has only been tested on Ubuntu 20.04, which rcssserver and rcssmonitor is supported. Install rcssserver and rcssmonitor (soccer window for debugging proposes) @@ -21,101 +33,90 @@ Install rcssserver and rcssmonitor (soccer window for debugging proposes) - rcssmonitor: [https://github.com/rcsoccersim/rcssmonitor](https://github.com/rcsoccersim/rcssmonitor) - soccer window: [https://github.com/helios-base/soccerwindow2](https://github.com/helios-base/soccerwindow2) -### Python requirements +#### 2. Install Python requirements -- Python v3.9 -- coloredlogs==15.0.1 -- pyrusgeom==0.1.2 -- scipy==1.10.1 +- Python version 3.11 -```bash +``` pip install -r requirements.txt ``` ---- - -## Quick Start +#### 3. Installing keepaway -### Run team (11 players and 1 coach) -To run the team there are some options, +Create a Virtual Environment [virtual environment](https://docs.python.org/3/tutorial/venv.html): -- running agents(players and coach execute separately) - -```bash -cd Pyrus2D -./start.sh +```shell +python3 -m venv keepaway-env +source keepaway-env/bin/activate ``` -- running the agents by Python (or running them separately in PyCharm) +Clone and checkout the release branch -```bash -cd Pyrus2D -python team/main_player.py (11 times) -python team/main_coach.py +```shell +git clone https://github.com/aabayomi/keepaway-python.git +cd keepaway-python +git checkout keepaway-release ``` -- running the agents by using one Python main process +Install keepaway as local package -```bash -cd Pyrus2D -python main.py +```shell +pip install -e . +``` +#### 4. Run baseline policy +```shell +python3 -m examples.test_random_agent ``` -## Start team by arguments - - -To modify team configuration, you can pass the arguments or update ```team_config.py```. - -Configurations are listed bellow: - -```bash -# To change the teamname (default is PYRUS): --t|--teamname TeamName +## Training agents for keepaway -# Determines if the connecting player is goalie or not. (take no parameters) --g|--goalie + -# change the player port connection. (default is 6000) --p|--player-port new_port + -# change the coach port connection. (default is 6002) --P|--coach-port new_port + -# change the trainer port connection. (default is 6001) ---trainer-port new_port + -``` + ---- + -## Useful links + -## Related Papers - -- Zare N, Amini O, Sayareh A, Sarvmaili M, Firouzkouhi A, Rad SR, Matwin S, Soares A. Cyrus2D Base: Source Code Base for RoboCup 2D Soccer Simulation League. InRoboCup 2022: Robot World Cup XXV 2023 Mar 24 (pp. 140-151). Cham: Springer International Publishing. [link](https://arxiv.org/abs/2211.08585) -- Zare N, Sarvmaili M, Sayareh A, Amini O, Matwin S, Soares A. Engineering Features to Improve Pass Prediction in Soccer Simulation 2D Games. InRobot World Cup 2022 (pp. 140-152). Springer, Cham. [link](https://www.researchgate.net/profile/Nader-Zare/publication/352414392_Engineering_Features_to_Improve_Pass_Prediction_in_Soccer_Simulation_2D_Games/links/60c9207fa6fdcc0c5c866520/Engineering-Features-to-Improve-Pass-Prediction-in-Soccer-Simulation-2D-Games.pdf) -- Zare N, Amini O, Sayareh A, Sarvmaili M, Firouzkouhi A, Matwin S, Soares A. Improving Dribbling, Passing, and Marking Actions in Soccer Simulation 2D Games Using Machine Learning. InRobot World Cup 2021 Jun 22 (pp. 340-351). Springer, Cham. [link](https://www.researchgate.net/profile/Nader-Zare/publication/355680673_Improving_Dribbling_Passing_and_Marking_Actions_in_Soccer_Simulation_2D_Games_Using_Machine_Learning/links/617971b0a767a03c14be3e42/Improving-Dribbling-Passing-and-Marking-Actions-in-Soccer-Simulation-2D-Games-Using-Machine-Learning.pdf) -- Akiyama, H., Nakashima, T.: Helios base: An open source package for the robocup soccer 2d simulation. In Robot Soccer World Cup 2013 Jun 24 (pp. 528-535). Springer, Berlin, Heidelberg. + diff --git a/base/bhv_kick.py b/base/bhv_kick.py deleted file mode 100644 index 22c6de4f..00000000 --- a/base/bhv_kick.py +++ /dev/null @@ -1,73 +0,0 @@ -from lib.action.hold_ball import HoldBall -from lib.action.neck_scan_players import NeckScanPlayers -from lib.action.smart_kick import SmartKick -from typing import List -from base.generator_action import KickAction, ShootAction, KickActionType -from base.generator_dribble import BhvDribbleGen -from base.generator_pass import BhvPassGen -from base.generator_shoot import BhvShhotGen -from base.generator_clear import BhvClearGen - -from typing import TYPE_CHECKING - -from lib.debug.debug import log -from lib.messenger.pass_messenger import PassMessenger - -if TYPE_CHECKING: - from lib.player.world_model import WorldModel - from lib.player.player_agent import PlayerAgent - - -class BhvKick: - def __init__(self): - pass - - def execute(self, agent: 'PlayerAgent'): - wm: 'WorldModel' = agent.world() - shoot_candidate: ShootAction = BhvShhotGen().generator(wm) - if shoot_candidate: - log.debug_client().set_target(shoot_candidate.target_point) - log.debug_client().add_message( - 'shoot' + 'to ' + shoot_candidate.target_point.__str__() + ' ' + str(shoot_candidate.first_ball_speed)) - SmartKick(shoot_candidate.target_point, shoot_candidate.first_ball_speed, - shoot_candidate.first_ball_speed - 1, 3).execute(agent) - agent.set_neck_action(NeckScanPlayers()) - return True - else: - action_candidates: List[KickAction] = [] - action_candidates += BhvPassGen().generator(wm) - action_candidates += BhvDribbleGen().generator(wm) - - if len(action_candidates) == 0: - return self.no_candidate_action(agent) - - best_action: KickAction = max(action_candidates) - - target = best_action.target_ball_pos - log.debug_client().set_target(target) - log.debug_client().add_message(best_action.type.value + 'to ' + best_action.target_ball_pos.__str__() + ' ' + str(best_action.start_ball_speed)) - SmartKick(target, best_action.start_ball_speed, best_action.start_ball_speed - 1, 3).execute(agent) - - if best_action.type is KickActionType.Pass: - agent.add_say_message(PassMessenger(best_action.target_unum, - best_action.target_ball_pos, - agent.effector().queued_next_ball_pos(), - agent.effector().queued_next_ball_vel())) - - agent.set_neck_action(NeckScanPlayers()) - return True - - def no_candidate_action(self, agent: 'PlayerAgent'): - wm = agent.world() - opp_min = wm.intercept_table().opponent_reach_cycle() - if opp_min <= 3: - action_candidates = BhvClearGen().generator(wm) - if len(action_candidates) > 0: - best_action: KickAction = max(action_candidates) - target = best_action.target_ball_pos - log.debug_client().set_target(target) - log.debug_client().add_message(best_action.type.value + 'to ' + best_action.target_ball_pos.__str__() + ' ' + str(best_action.start_ball_speed)) - SmartKick(target, best_action.start_ball_speed, best_action.start_ball_speed - 2.0, 2).execute(agent) - - agent.set_neck_action(NeckScanPlayers()) - return HoldBall().execute(agent) \ No newline at end of file diff --git a/base/bhv_move.py b/base/bhv_move.py deleted file mode 100644 index ed9e9c51..00000000 --- a/base/bhv_move.py +++ /dev/null @@ -1,75 +0,0 @@ -from base.basic_tackle import BasicTackle -from lib.action.go_to_point import GoToPoint -from base.strategy_formation import StrategyFormation -from lib.action.intercept import Intercept -from lib.action.neck_turn_to_ball import NeckTurnToBall -from lib.action.neck_turn_to_ball_or_scan import NeckTurnToBallOrScan -from lib.action.turn_to_ball import TurnToBall -from lib.action.turn_to_point import TurnToPoint -from base.tools import Tools -from base.stamina_manager import get_normal_dash_power -from base.bhv_block import Bhv_Block -from pyrusgeom.vector_2d import Vector2D - -from typing import TYPE_CHECKING - -from lib.debug.debug import log - -if TYPE_CHECKING: - from lib.player.player_agent import PlayerAgent - from lib.player.world_model import WorldModel - - -class BhvMove: - def __init__(self): - self._in_recovery_mode = False - pass - - def execute(self, agent: 'PlayerAgent'): - wm: 'WorldModel' = agent.world() - - if BasicTackle(0.8, 80).execute(agent): - return True - - # intercept - self_min = wm.intercept_table().self_reach_cycle() - tm_min = wm.intercept_table().teammate_reach_cycle() - opp_min = wm.intercept_table().opponent_reach_cycle() - log.sw_log().block().add_text( - f"self_min={self_min}") - log.sw_log().block().add_text( - f"tm_min={tm_min}") - log.sw_log().block().add_text( - f"opp_min={opp_min}") - - if (not wm.exist_kickable_teammates() - and (self_min <= 2 - or (self_min <= tm_min - and self_min < opp_min + 5))): - log.sw_log().block().add_text( "INTERCEPTING") - log.debug_client().add_message('intercept') - if Intercept().execute(agent): - agent.set_neck_action(NeckTurnToBall()) - return True - - if opp_min < min(tm_min, self_min): - if Bhv_Block().execute(agent): - agent.set_neck_action(NeckTurnToBall()) - return True - st = StrategyFormation().i() - target = st.get_pos(agent.world().self().unum()) - - log.debug_client().set_target(target) - log.debug_client().add_message('bhv_move') - - dash_power, self._in_recovery_mode = get_normal_dash_power(wm, self._in_recovery_mode) - dist_thr = wm.ball().dist_from_self() * 0.1 - - if dist_thr < 1.0: - dist_thr = 1.0 - - if GoToPoint(target, dist_thr, dash_power).execute(agent): - agent.set_neck_action(NeckTurnToBallOrScan()) - return True - return False - diff --git a/base/decision.py b/base/decision.py deleted file mode 100644 index ede8096f..00000000 --- a/base/decision.py +++ /dev/null @@ -1,47 +0,0 @@ -from base import goalie_decision -from base.strategy_formation import StrategyFormation -from base.set_play.bhv_set_play import Bhv_SetPlay -from base.bhv_kick import BhvKick -from base.bhv_move import BhvMove -from lib.action.neck_scan_field import NeckScanField -from lib.action.neck_scan_players import NeckScanPlayers -from lib.action.neck_turn_to_ball import NeckTurnToBall -from lib.action.neck_turn_to_ball_or_scan import NeckTurnToBallOrScan -from lib.action.scan_field import ScanField -from lib.debug.debug import log -from lib.messenger.ball_pos_vel_messenger import BallPosVelMessenger -from lib.messenger.player_pos_unum_messenger import PlayerPosUnumMessenger -from lib.rcsc.types import GameModeType, ViewWidth, UNUM_UNKNOWN - -from typing import TYPE_CHECKING -if TYPE_CHECKING: - from lib.player.world_model import WorldModel - from lib.player.player_agent import PlayerAgent - - -# TODO TACKLE GEN -# TODO GOAL KICK L/R -# TODO GOAL L/R -def get_decision(agent: 'PlayerAgent'): - wm: 'WorldModel' = agent.world() - st = StrategyFormation().i() - st.update(wm) - - if wm.self().goalie(): - if goalie_decision.decision(agent): - return True - - if wm.game_mode().type() != GameModeType.PlayOn: - if Bhv_SetPlay().execute(agent): - return True - - log.sw_log().team().add_text(f'is kickable? dist {wm.ball().dist_from_self()} ' - f'ka {wm.self().player_type().kickable_area()} ' - f'seen pos count {wm.ball().seen_pos_count()} ' - f'is? {wm.self()._kickable}') - if wm.self().is_kickable(): - return BhvKick().execute(agent) - if BhvMove().execute(agent): - return True - log.os_log().warn("NO ACTION, ScanFIELD") - return ScanField().execute(agent) \ No newline at end of file diff --git a/base/generator_pass.py b/base/generator_pass.py deleted file mode 100644 index ece617c5..00000000 --- a/base/generator_pass.py +++ /dev/null @@ -1,579 +0,0 @@ -from pyrusgeom.geom_2d import * -import pyrusgeom.soccer_math as smath -from lib.debug.color import Color -from lib.debug.debug import log -from lib.rcsc.server_param import ServerParam as SP -from base.tools import Tools -import time -from base.generator_action import KickAction, KickActionType, BhvKickGen - -from typing import TYPE_CHECKING - -from lib.rcsc.types import GameModeType -if TYPE_CHECKING: - from lib.player.world_model import WorldModel - from lib.player.object_player import PlayerObject - -debug_pass = True -max_pass_time = 0 - - -class BhvPassGen(BhvKickGen): - def __init__(self): - super().__init__() - self.best_pass: KickAction = None - self.receivers: list['PlayerObject'] = [] - - def generator(self, wm: 'WorldModel'): - global max_pass_time - start_time = time.time() - self.update_receivers(wm) - - for r in self.receivers: - log.sw_log().pass_().add_text(f'=============== Lead Pass to {r.unum()} pos: {r.pos()}') - # if self.best_pass is not None \ - # and r.pos().x() < self.best_pass.target_ball_pos.x() - 5: - # break - self.generate_direct_pass(wm, r) - self.generate_lead_pass(wm, r) - self.generate_through_pass(wm, r) - - if debug_pass: - for candid in self.debug_list: - if candid[2]: - log.sw_log().pass_().add_message(candid[1].x(), candid[1].y(), '{}'.format(candid[0])) - log.sw_log().pass_().add_circle(circle=Circle2D(candid[1], 0.2), color=Color(string='green')) - else: - log.sw_log().pass_().add_message(candid[1].x(), candid[1].y(), '{}'.format(candid[0])) - log.sw_log().pass_().add_circle(circle=Circle2D(candid[1], 0.2), color=Color(string='red')) - end_time = time.time() - if end_time - start_time > max_pass_time: - max_pass_time = end_time - start_time - log.sw_log().pass_().add_text( 'time:{} max is {}'.format(end_time - start_time, max_pass_time)) - return self.candidates - - def update_receivers(self, wm: 'WorldModel'): - sp = SP.i() - log.sw_log().pass_().add_text('===update receivers'.format()) - for tm in wm.teammates(): - if tm is None: - log.sw_log().pass_().add_text('-----<<< TM is none') - continue - if tm.unum() <= 0: - log.sw_log().pass_().add_text(f'-----<<< TM unum is {tm.unum()}') - continue - if tm.unum() == wm.self().unum(): - log.sw_log().pass_().add_text(f'-----<<< TM unum is {tm.unum()} (self)') - continue - if tm.pos_count() > 10: - log.sw_log().pass_().add_text(f'-----<<< TM unum pos count {tm.pos_count()}') - continue - if tm.is_tackling(): - log.sw_log().pass_().add_text(f'-----<<< TM is tackling') - continue - if tm.pos().x() > wm.offside_line_x(): - log.sw_log().pass_().add_text(f'-----<<< TM is in offside {tm.pos().x()} > {wm.offside_line_x()}') - continue - if tm.goalie() and tm.pos().x() < sp.our_penalty_area_line_x() + 15: - log.sw_log().pass_().add_text(f'-----<<< TM is goalie and danger {tm.pos().x()} < {sp.our_penalty_area_line_x() + 15}') - continue - log.sw_log().pass_().add_text(f'--->>>>> TM {tm.unum()} is added') - self.receivers.append(tm) - self.receivers = sorted(self.receivers, key=lambda p: p.pos().x(), reverse=True) - - def generate_direct_pass(self, wm: 'WorldModel', receiver: 'PlayerObject'): - sp = SP.i() - min_receive_step = 3 - max_direct_pass_dist = 0.8 * smath.inertia_final_distance(sp.ball_speed_max(), sp.ball_decay()) - max_receive_ball_speed = sp.ball_speed_max() * pow(sp.ball_decay(), min_receive_step) - min_direct_pass_dist = receiver.player_type().kickable_area() * 2.2 - if receiver.pos().x() > sp.pitch_half_length() - 1.5 \ - or receiver.pos().x() < -sp.pitch_half_length() + 5.0 \ - or receiver.pos().abs_y() > sp.pitch_half_width() - 1.5: - if debug_pass: - log.sw_log().pass_().add_text( '#DPass to {} {}, out of field'.format(receiver.unum(), receiver.pos())) - return - # TODO sp.ourTeamGoalPos() - if receiver.pos().x() < wm.ball().pos().x() + 1.0 \ - and receiver.pos().dist2(Vector2D(-52.5, 0)) < pow(18.0, 2): - if debug_pass: - log.sw_log().pass_().add_text( '#DPass to {} {}, danger near goal'.format(receiver.unum(), receiver.pos())) - return - - ptype = receiver.player_type() - max_ball_speed = wm.self().kick_rate() * sp.max_power() - if wm.game_mode().type() == GameModeType.PlayOn: - max_ball_speed = sp.ball_speed_max() - - # TODO SP.defaultRealSpeedMax() - min_ball_speed = 1.0 - - receive_point = ptype.inertiaFinalPoint(receiver.pos(), receiver.vel()) - ball_move_dist = wm.ball().pos().dist(receive_point) - - if ball_move_dist < min_direct_pass_dist or max_direct_pass_dist < ball_move_dist: - if debug_pass: - log.sw_log().pass_().add_text( '#DPass to {} {}, far or close'.format(receiver.unum(), receiver.pos())) - return - - if wm.game_mode().type().is_goal_kick() \ - and receive_point.x() < sp.our_penalty_area_line_x() + 1.0 \ - and receive_point.abs_y() < sp.penalty_area_half_width() + 1.0: - if debug_pass: - log.sw_log().pass_().add_text( - '#DPass to {} {}, in penalty area in goal kick mode'.format(receiver.unum(), receiver.pos())) - return - - max_receive_ball_speed = min(max_receive_ball_speed, ptype.kickable_area() + ( - sp.max_dash_power() * ptype.dash_power_rate() * ptype.effort_max()) * 1.8) - min_receive_ball_speed = ptype.real_speed_max() - - ball_move_angle = (receive_point - wm.ball().pos()).th() - - min_ball_step = sp.ball_move_step(sp.ball_speed_max(), ball_move_dist) - # TODO Penalty step - start_step = max(max(min_receive_step, min_ball_step), 0) - max_step = start_step + 2 - log.sw_log().pass_().add_text( '#DPass to {} {}'.format(receiver.unum(), receiver.pos())) - self.create_pass(wm, receiver, receive_point, - start_step, max_step, min_ball_speed, - max_ball_speed, min_receive_ball_speed, - max_receive_ball_speed, ball_move_dist, - ball_move_angle, "D") - - def generate_lead_pass(self, wm: 'WorldModel', receiver): - sp = SP.i() - our_goal_dist_thr2 = pow(16.0, 2) - min_receive_step = 4 - max_receive_step = 20 - min_leading_pass_dist = 3.0 - max_leading_pass_dist = 0.8 * smath.inertia_final_distance(sp.ball_speed_max(), sp.ball_decay()) - max_receive_ball_speed = sp.ball_speed_max() * pow(sp.ball_decay(), min_receive_step) - - max_player_distance = 35 - if receiver.pos().dist(wm.ball().pos()) > max_player_distance: - if debug_pass: - log.sw_log().pass_().add_text( '#####LPass to {} {}, player is far'.format(receiver.unum(), receiver.pos())) - return - - abgle_divs = 8 - angle_step = 360.0 / abgle_divs - dist_divs = 4 - dist_step = 1.1 - - ptype = receiver.player_type() - max_ball_speed = wm.self().kick_rate() * sp.max_power() - if wm.game_mode().type() == GameModeType.PlayOn: - max_ball_speed = sp.ball_speed_max() - min_ball_speed = sp.default_player_speed_max() - - max_receive_ball_speed = min(max_receive_ball_speed, ptype.kickable_area() + ( - sp.max_dash_power() * ptype.dash_power_rate() * ptype.effort_max()) * 1.5) - min_receive_ball_speed = 0.001 - - our_goal = Vector2D(-52.5, 0) - - angle_from_ball = (receiver.pos() - wm.ball().pos()).th() - for d in range(1, dist_divs + 1): - player_move_dist = dist_step * d - a_step = 2 if player_move_dist * 2.0 * math.pi / abgle_divs < 0.6 else 1 - for a in range(abgle_divs + 1): - angle = angle_from_ball + angle_step * a - receive_point = receiver.inertia_point(1) + Vector2D.from_polar(player_move_dist, angle) - - move_dist_penalty_step = 0 - ball_move_line = Line2D(wm.ball().pos(), receive_point) - player_line_dist = ball_move_line.dist(receiver.pos()) - move_dist_penalty_step = int(player_line_dist * 0.3) - if receive_point.x() > sp.pitch_half_length() - 3.0 \ - or receive_point.x() < -sp.pitch_half_length() + 5.0 \ - or receive_point.abs_y() > sp.pitch_half_width() - 3.0: - if debug_pass: - log.sw_log().pass_().add_text( '#####LPass to {} {}, out of field'.format(receiver.unum(), receive_point)) - continue - - if receive_point.x() < wm.ball().pos().x() \ - and receive_point.dist2(our_goal) < our_goal_dist_thr2: - if debug_pass: - log.sw_log().pass_().add_text( '#####LPass to {} {}, pass is danger'.format(receiver.unum(), receive_point)) - continue - - if wm.game_mode().type() in [GameModeType.GoalKick_Right, GameModeType.GoalKick_Left] \ - and receive_point.x() < sp.our_penalty_area_line_x() + 1.0 \ - and receive_point.abs_y() < sp.penalty_area_half_width() + 1.0: - if debug_pass: - log.sw_log().pass_().add_text( '#####LPass to {} {}, in penalty area'.format(receiver.unum(), receive_point)) - return - - ball_move_dist = wm.ball().pos().dist(receive_point) - - if ball_move_dist < min_leading_pass_dist or max_leading_pass_dist < ball_move_dist: - if debug_pass: - log.sw_log().pass_().add_text( '#####LPass to {} {}, so far or so close'.format(receiver.unum(), receive_point)) - continue - - nearest_receiver = Tools.get_nearest_teammate(wm, receive_point, self.receivers) - if nearest_receiver.unum() != receiver.unum(): - if debug_pass: - log.sw_log().pass_().add_text( - '#####LPass to {} {}, {} is closer than receiver '.format(receiver.unum(), receive_point, - nearest_receiver.unum())) - continue - - receiver_step = self.predict_receiver_reach_step(receiver, receive_point, True, - 'L') + move_dist_penalty_step - ball_move_angle = (receive_point - wm.ball().pos()).th() - - min_ball_step = sp.ball_move_step(sp.ball_speed_max(), ball_move_dist) - - start_step = max(max(min_receive_step, min_ball_step), receiver_step) - # ifdef CREATE_SEVERAL_CANDIDATES_ON_SAME_POINT - # max_step = std::max(max_receive_step, start_step + 3); - # else - if debug_pass: - log.sw_log().pass_().add_text( '#####LPass to {} {}'.format(receiver.unum(), receive_point)) - max_step = start_step + 3 - self.create_pass(wm, receiver, receive_point, - start_step, max_step, - min_ball_speed, max_ball_speed, - min_receive_ball_speed, max_receive_ball_speed, - ball_move_dist, ball_move_angle, - 'L') - - def generate_through_pass(self, wm: 'WorldModel', receiver: 'PlayerObject'): - sp = SP.i() - teammate_min_x = 0.0 - target_min_x = 10.0 - min_receive_step = 4 - min_pass_dist = 3.0 - max_pass_dist = 0.8 * smath.inertia_final_distance(sp.ball_speed_max(), sp.ball_decay()) - max_receive_ball_speed = sp.ball_speed_max() * pow(sp.ball_decay(), min_receive_step) - - max_player_distance = 35 - if receiver.pos().dist(wm.ball().pos()) > max_player_distance: - if debug_pass: - log.sw_log().pass_().add_text('#####TPass to {} {}, player is far'.format(receiver.unum(), receiver.pos())) - return - if receiver.pos().x() < teammate_min_x: - if debug_pass: - log.sw_log().pass_().add_text('#####TPass to {} {}, player is far'.format(receiver.unum(), receiver.pos())) - return - if receiver.pos().x() < wm.offside_line_x() - 5.0: - if debug_pass: - log.sw_log().pass_().add_text('#####TPass to {} {}, player is not close to offside line'.format(receiver.unum(), receiver.pos())) - return - if receiver.pos().x() > wm.offside_line_x() - 0.5: - if debug_pass: - log.sw_log().pass_().add_text('#####TPass to {} {}, player is in offside'.format(receiver.unum(), receiver.pos())) - return - if wm.ball().pos().x() < -10.0 or wm.ball().pos().x() > 30.0: - if debug_pass: - log.sw_log().pass_().add_text('#####TPass to {} {}, ball x is low or high'.format(receiver.unum(), receiver.pos())) - return - - min_angle = -30 - max_angle = 30 - angle_step = 10 - dist_divs = 25 - dist_step = 1.0 - - ptype = receiver.player_type() - max_ball_speed = wm.self().kick_rate() * sp.max_power() - if wm.game_mode().type() == GameModeType.PlayOn: - max_ball_speed = sp.ball_speed_max() - min_ball_speed = sp.default_player_speed_max() - - max_receive_ball_speed = min(max_receive_ball_speed, ptype.kickable_area() + ( - sp.max_dash_power() * ptype.dash_power_rate() * ptype.effort_max()) * 1.5) - min_receive_ball_speed = 0.001 - - our_goal = Vector2D(-52.5, 0) - - angle_from_ball = (receiver.pos() - wm.ball().pos()).th() - for d in range(5, dist_divs + 1): - player_move_dist = dist_step * d - for a in range(min_angle, max_angle + 1, angle_step): - receive_point = receiver.inertia_point(1) + Vector2D.from_polar(player_move_dist, a) - - move_dist_penalty_step = 0 - ball_move_line = Line2D(wm.ball().pos(), receive_point) - player_line_dist = ball_move_line.dist(receiver.pos()) - move_dist_penalty_step = int(player_line_dist * 0.3) - if receive_point.x() > sp.pitch_half_length() - 3.0 \ - or receive_point.x() < -sp.pitch_half_length() + 5.0 \ - or receive_point.abs_y() > sp.pitch_half_width() - 3.0: - if debug_pass: - log.sw_log().pass_().add_text('#####TPass to {} {}, out of field'.format(receiver.unum(), receive_point)) - continue - - if receive_point.x() < target_min_x: - if debug_pass: - log.sw_log().pass_().add_text('#####TPass to {} {}, pass is danger'.format(receiver.unum(), receive_point)) - continue - - ball_move_dist = wm.ball().pos().dist(receive_point) - - if ball_move_dist < min_pass_dist or max_pass_dist < ball_move_dist: - if debug_pass: - log.sw_log().pass_().add_text('#####TPass to {} {}, so far or so close'.format(receiver.unum(), receive_point)) - continue - - nearest_receiver = Tools.get_nearest_teammate(wm, receive_point, self.receivers) - if nearest_receiver.unum() != receiver.unum(): - if debug_pass: - log.sw_log().pass_().add_text( - '#####TPass to {} {}, {} is closer than receiver '.format(receiver.unum(), receive_point, - nearest_receiver.unum())) - continue - - receiver_step = self.predict_receiver_reach_step(receiver, receive_point, True, - 'T') + move_dist_penalty_step - ball_move_angle = (receive_point - wm.ball().pos()).th() - - min_ball_step = sp.ball_move_step(sp.ball_speed_max(), ball_move_dist) - - start_step = max(max(min_receive_step, min_ball_step), receiver_step) - # ifdef CREATE_SEVERAL_CANDIDATES_ON_SAME_POINT - # max_step = std::max(max_receive_step, start_step + 3); - # else - if debug_pass: - log.sw_log().pass_().add_text('#####TPass to {} {}'.format(receiver.unum(), receive_point)) - max_step = start_step + 3 - self.create_pass(wm, receiver, receive_point, - start_step, max_step, - min_ball_speed, max_ball_speed, - min_receive_ball_speed, max_receive_ball_speed, - ball_move_dist, ball_move_angle, - 'T') - - def predict_receiver_reach_step(self, receiver, pos: Vector2D, use_penalty, pass_type): - ptype = receiver.player_type() - - target_dist = receiver.inertia_point(1).dist(pos) - n_turn = 1 if receiver.body_count() > 0 else Tools.predict_player_turn_cycle(ptype, receiver.body(), - receiver.vel().r(), target_dist, ( - pos - receiver.inertia_point( - 1)).th(), - ptype.kickable_area(), False) - dash_dist = target_dist - - # if use_penalty: - # dash_dist += receiver.penalty_distance_; - - if pass_type == 'L': - dash_dist *= 1.05 - - dash_angle = (pos - receiver.pos()).th() - - if dash_angle.abs() > 90.0 or receiver.body_count() > 1 or (dash_angle - receiver.body()).abs() > 30.0: - n_turn += 1 - - n_dash = ptype.cycles_to_reach_distance(dash_dist) - - n_step = n_turn + n_dash if n_turn == 0 else n_turn + n_dash + 1 - return n_step - - def create_pass(self, wm: 'WorldModel', receiver, receive_point: Vector2D, - min_step, max_step, min_first_ball_speed, max_first_ball_speed, - min_receive_ball_speed, max_receive_ball_speed, - ball_move_dist, ball_move_angle: AngleDeg, description): - sp = SP.i() - - for step in range(min_step, max_step + 1): - self.index += 1 - first_ball_speed = smath.calc_first_term_geom_series(ball_move_dist, sp.ball_decay(), step) - - if first_ball_speed < min_first_ball_speed: - if debug_pass: - log.sw_log().pass_().add_text( - '##Pass {},to {} {}, step:{}, ball_speed:{}, first ball speed is low'.format( - self.index, - receiver.unum(), - receiver.pos(), - step, - first_ball_speed)) - self.debug_list.append((self.index, receive_point, False)) - break - - if max_first_ball_speed < first_ball_speed: - if debug_pass: - log.sw_log().pass_().add_text( - '##Pass {},to {} {}, step:{}, ball_speed:{}, first ball speed is high'.format( - self.index, - receiver.unum(), - receiver.pos(), - step, - first_ball_speed)) - self.debug_list.append((self.index, receive_point, False)) - continue - - receive_ball_speed = first_ball_speed * pow(sp.ball_decay(), step) - - if receive_ball_speed < min_receive_ball_speed: - if debug_pass: - log.sw_log().pass_().add_text( - '##Pass {},to {} {}, step:{}, ball_speed:{}, rball_speed:{}, receive ball speed is low'.format( - self.index, - receiver.unum(), - receiver.pos(), - step, - first_ball_speed, - receive_ball_speed)) - self.debug_list.append((self.index, receive_point, False)) - break - - if max_receive_ball_speed < receive_ball_speed: - if debug_pass: - log.sw_log().pass_().add_text( - '##Pass {},to {} {}, step:{}, ball_speed:{}, rball_speed:{}, receive ball speed is high'.format( - self.index, - receiver.unum(), - receiver.pos(), - step, - first_ball_speed, - receive_ball_speed)) - self.debug_list.append((self.index, receive_point, False)) - continue - - kick_count = Tools.predict_kick_count(wm, wm.self().unum(), first_ball_speed, ball_move_angle) - - o_step, o_unum, o_intercepted_pos = self.predict_opponents_reach_step(wm, wm.ball().pos(), - first_ball_speed, ball_move_angle, - receive_point, step + (kick_count - 1) + 5, - description) - - failed = False - if description == 'T': - if o_step <= step: - failed = True - else: - if o_step <= step + (kick_count - 1): - failed = True - if failed: - if debug_pass: - log.sw_log().pass_().add_text( - '------<<<<>>>> OK Pass {} to {} {}, opp {} step {} max_step {}'.format(self.index, - receiver.unum(), - receive_point, - o_unum, - o_step, - max_step)) - self.debug_list.append((self.index, receive_point, True)) - candidate = KickAction() - candidate.type = KickActionType.Pass - candidate.start_ball_pos = wm.ball().pos() - candidate.target_ball_pos = receive_point - candidate.target_unum = receiver.unum() - candidate.start_ball_speed = first_ball_speed - candidate.evaluate(wm) - self.candidates.append(candidate) - - if self.best_pass is None or candidate.eval > self.best_pass.eval: - self.best_pass = candidate - - find_another_pass = False - if not find_another_pass: - break - - if o_step <= step + 3: - break - - if min_step + 3 <= step: - break - - def predict_opponents_reach_step(self, wm: 'WorldModel', first_ball_pos: Vector2D, first_ball_speed, - ball_move_angle: AngleDeg, receive_point: Vector2D, max_cycle, description): - first_ball_vel = Vector2D.polar2vector(first_ball_speed, ball_move_angle) - min_step = 1000 - min_opp = 0 - intercepted_pos = None - for opp in wm.opponents(): - if opp is None or opp.unum() == 0: - continue - step, intercepted_pos = self.predict_opponent_reach_step(wm, opp, first_ball_pos, first_ball_vel, ball_move_angle, - receive_point, max_cycle, description) - if step < min_step: - min_step = step - min_opp = opp.unum() - return min_step, min_opp, intercepted_pos - - def predict_opponent_reach_step(self, wm: 'WorldModel', opp: 'PlayerObject', first_ball_pos: Vector2D, first_ball_vel: Vector2D, - ball_move_angle: AngleDeg, receive_point: Vector2D, max_cycle, description): - sp = SP.i() - - penalty_area = Rect2D(Vector2D(sp.their_penalty_area_line_x(), -sp.penalty_area_half_width() ), - Size2D(sp.penalty_area_length(), sp.penalty_area_width())) - CONTROL_AREA_BUF = 0.15 - - - opponent = opp - ptype = opponent.player_type() - min_cycle = Tools.estimate_min_reach_cycle(opponent.pos(), ptype.real_speed_max(), first_ball_pos, - ball_move_angle) - - if min_cycle < 0: - return 1000, None - - for cycle in range(max(1, min_cycle), max_cycle + 1): - ball_pos = smath.inertia_n_step_point(first_ball_pos, first_ball_vel, cycle, sp.ball_decay()) - control_area = sp.catchable_area() if opponent.is_goalie() and penalty_area.contains(ball_pos) else ptype.kickable_area() - - inertia_pos = ptype.inertia_point(opponent.pos(), opponent.vel(), cycle) - target_dist = inertia_pos.dist(ball_pos) - - dash_dist = target_dist - if description == 'T' \ - and first_ball_vel.x() > 2.\ - and ( receive_point.x() > wm.offside_line_x() or receive_point.x() > 30.): - - pass - else: - dash_dist -= Tools.estimate_virtual_dash_distance(opp) - if dash_dist - control_area - CONTROL_AREA_BUF < 0.001: - return cycle, ball_pos - - if description == 'T' \ - and first_ball_vel.x() > 2.\ - and ( receive_point.x() > wm.offside_line_x() or receive_point.x() > 30.): - - dash_dist -= control_area - else: - if receive_point.x() < 25.: - dash_dist -= control_area + 0.5 - else: - dash_dist -= control_area + 0.2 - - if dash_dist > ptype.real_speed_max() * (cycle + min(opponent.pos_count(), 5)): - continue - - n_dash = ptype.cycles_to_reach_distance(dash_dist) - if n_dash > cycle + opponent.pos_count(): - continue - - n_turn = 0 - if opponent.body_count() > 1: - n_turn = Tools.predict_player_turn_cycle(ptype, opponent.body(), opponent.vel().r(), target_dist, - (ball_pos - inertia_pos).th(), control_area, True) - - n_step = n_turn + n_dash if n_turn == 0 else n_turn + n_dash + 1 - - bonus_step = 0 - if opponent.is_tackling(): - bonus_step = -5 - if n_step - bonus_step <= cycle: - return cycle, ball_pos - return 1000, None - diff --git a/base/main_coach.py b/base/main_coach.py deleted file mode 100644 index c19b845f..00000000 --- a/base/main_coach.py +++ /dev/null @@ -1,18 +0,0 @@ -#!/usr/bin/python3 -from lib.player.basic_client import BasicClient -from base.sample_coach import SampleCoach - -import team_config -import sys - - -def main(): - agent = SampleCoach() - if not agent.handle_start(): - agent.handle_exit() - return - agent.run() - - -if __name__ == "__main__": - main() diff --git a/base/sample_communication.py b/base/sample_communication.py deleted file mode 100644 index 91b6c9e0..00000000 --- a/base/sample_communication.py +++ /dev/null @@ -1,641 +0,0 @@ -from math import exp - -from pyrusgeom.soccer_math import bound - -import team_config -from base.strategy import Strategy -from lib.debug.debug import log -from lib.messenger.ball_goalie_messenger import BallGoalieMessenger -from lib.messenger.ball_messenger import BallMessenger -from lib.messenger.ball_player_messenger import BallPlayerMessenger -from lib.messenger.ball_pos_vel_messenger import BallPosVelMessenger -from lib.messenger.goalie_messenger import GoalieMessenger -from lib.messenger.goalie_player_messenger import GoaliePlayerMessenger -from lib.messenger.messenger import Messenger -from lib.messenger.messenger_memory import MessengerMemory -from lib.messenger.one_player_messenger import OnePlayerMessenger -from lib.messenger.recovery_message import RecoveryMessenger -from lib.messenger.stamina_messenger import StaminaMessenger -from lib.messenger.three_player_messenger import ThreePlayerMessenger -from lib.messenger.two_player_messenger import TwoPlayerMessenger -from lib.rcsc.game_time import GameTime -from lib.rcsc.server_param import ServerParam -from lib.rcsc.types import UNUM_UNKNOWN, GameModeType, SideID - -from typing import TYPE_CHECKING - -if TYPE_CHECKING: - from lib.player.player_agent import PlayerAgent - from lib.player.object_player import PlayerObject - from lib.player.world_model import WorldModel - - -class ObjectScore: - def __init__(self, n=UNUM_UNKNOWN, score=-1000, player=None): - self.number = n - self.score = score - self.player: PlayerObject = player - - -class SampleCommunication: - def __init__(self): - self._current_sender_unum: int = UNUM_UNKNOWN - self._next_sender_unum: int = UNUM_UNKNOWN - self._ball_send_time: GameTime = GameTime(0, 0) - self._teammate_send_time: list[GameTime] = [GameTime(0, 0) for i in range(12)] - self._opponent_send_time: list[GameTime] = [GameTime(0, 0) for i in range(12)] - - def should_say_ball(self, agent: 'PlayerAgent'): - wm = agent.world() - ef = agent.effector() - - if wm.ball().seen_pos_count() > 0 or wm.ball().seen_vel_count() > 2: - return False - if wm.game_mode().type() != GameModeType.PlayOn and ef.queued_next_ball_vel().r2() < 0.5 ** 2: - return False - if wm.kickable_teammate(): - return False - - ball_vel_changed = False - current_ball_speed = wm.ball().vel().r() - - if wm.prev_ball().vel_valid(): - prev_ball_speed = wm.prev_ball().vel().r() - angle_diff = (wm.ball().vel().th() - wm.prev_ball().vel().th()).abs() - - log.sw_log().communication().add_text(f'(sample communication)' - f'prev vel={wm.prev_ball().vel()}, r={prev_ball_speed}' - f'current_vel={wm.ball().vel()}, r={wm.ball().vel()}') - - if current_ball_speed > prev_ball_speed + 0.1 \ - or ( - prev_ball_speed > 0.5 and current_ball_speed < prev_ball_speed * ServerParam.i().ball_decay() / 2) \ - or (prev_ball_speed > 0.5 and angle_diff > 20.): - log.sw_log().communication().add_text(f'(sample communication) ball vel changed') - ball_vel_changed = True - - if wm.self().is_kickable(): - if ball_vel_changed and wm.last_kicker_side() != wm.our_side() and not wm.kickable_opponent(): - log.sw_log().communication().add_text( - "(sample communication) ball vel changed. opponent kicked. no opponent kicker") - return True - if ef.queued_next_ball_kickable() and current_ball_speed < 1.: - return False - if ball_vel_changed and ef.queued_next_ball_vel().r() > 1.: - log.sw_log().communication().add_text('(sample communication) kickable. ball vel changed') - return True - log.sw_log().communication().add_text('(sample communication) kickable. but no say') - return False - - ball_nearest_teammate: PlayerObject = None - second_ball_nearest_teammate: PlayerObject = None - - for p in wm.teammates_from_ball(): - if p is None: - continue - if p.is_ghost() or p.pos_count() >= 10: - continue - - if ball_nearest_teammate is None: - ball_nearest_teammate = p - elif second_ball_nearest_teammate is None: - second_ball_nearest_teammate = p - break - - our_min = min(wm.intercept_table().self_reach_cycle(), wm.intercept_table().teammate_reach_cycle()) - opp_min = wm.intercept_table().opponent_reach_cycle() - - if ball_nearest_teammate is None \ - or ball_nearest_teammate.dist_from_ball() > wm.ball().dist_from_self() - 3.: - log.sw_log().communication().add_text('(sample communication) maybe nearest to ball') - - if ball_vel_changed or (opp_min <= 1 and current_ball_speed > 1.): - log.sw_log().communication().add_text('(sample communication) nearest to ball. ball vel changed?') - return True - - if ball_nearest_teammate is not None \ - and wm.ball().dist_from_self() < 20. \ - and 1. < ball_nearest_teammate.dist_from_ball() < 6. \ - and (opp_min <= our_min + 1 or ball_vel_changed): - log.sw_log().communication().add_text('(sample communication) support nearset player') - return True - return False - - def should_say_opponent_goalie(self, agent: 'PlayerAgent'): - wm = agent.world() - goalie: PlayerObject = wm.get_their_goalie() - - if goalie is None: - return False - - if goalie.seen_pos_count() == 0 \ - and goalie.body_count() == 0 \ - and goalie.unum() != UNUM_UNKNOWN \ - and goalie.unum_count() == 0 \ - and goalie.dist_from_self() < 25. \ - and 51. - 16. < goalie.pos().x() < 52.5 \ - and goalie.pos().abs_y() < 20.: - - goal_pos = ServerParam.i().their_team_goal_pos() - ball_next = wm.ball().pos() + wm.ball().vel() - - if ball_next.dist2(goal_pos) < 18 ** 2: - return True - return False - - def update_player_send_time(self, wm: 'WorldModel', side: SideID, unum: int): - if not (1 <= unum <= 11): - log.os_log().error(f'(sample communication) illegal player number. unum={unum}') - return - - if side == wm.our_side(): - self._teammate_send_time[unum] = wm.time().copy() - else: - self._opponent_send_time[unum] = wm.time().copy() - - - def say_ball_and_players(self, agent: 'PlayerAgent'): - SP = ServerParam.i() - wm = agent.world() - ef = agent.effector() - - current_len = ef.get_say_message_length() - - should_say_ball = self.should_say_ball(agent) - should_say_goalie = self.should_say_opponent_goalie(agent) - goalie_say_situation = False # self.goalie_say_situation # TODO IMP FUNC - - if not should_say_ball and not should_say_goalie and not goalie_say_situation \ - and self._current_sender_unum != wm.self().unum() \ - and current_len == 0: - log.sw_log().communication().add_text('(sample communication) say ball and players: no send situation') - return False - - available_len = SP.player_say_msg_size() - current_len - mm = wm.messenger_memory() - - objects = [ObjectScore(i, 1000) for i in range(23)] - objects[0].score = wm.time().cycle() - mm.ball_time().cycle() - - for p in mm.player_record(): - n = round(p[1].unum_) - if not (1 <= n <= 22): - continue - - objects[n].score = wm.time().cycle() - p[0].cycle() - - if 1 <= wm.their_goalie_unum() <= 11: - n = wm.their_goalie_unum() + 11 - diff = wm.time().cycle() - mm.goalie_time().cycle() - objects[n].score = min(objects[n].score, diff) - - if wm.self().is_kickable(): - if ef.queued_next_ball_kickable(): - objects[0].score = -1000 - else: - objects[0].score = 1000 - elif wm.ball().seen_pos_count() > 0 or wm.ball().seen_vel_count() > 1 or wm.kickable_teammate(): - objects[0].score = -1000 - elif should_say_ball: - objects[0].score = 1000 - elif objects[0].score > 0: - if wm.prev_ball().vel_valid(): - angle_diff = (wm.ball().vel().th() - wm.prev_ball().vel().th()).abs() - prev_speed = wm.prev_ball().vel().r() - current_speed = wm.ball().vel().r() - - if (current_speed > prev_speed + 0.1 - or (prev_speed > 0.5 and current_speed < prev_speed * SP.ball_decay() * 0.5) - or (prev_speed > 0.5 and angle_diff > 20.)): - objects[0].score = 1000 - else: - objects[0].score /= 2 - - variance = 30 - x_rate = 1 - y_rate = 0.5 - - min_step = min(wm.intercept_table().opponent_reach_cycle(), wm.intercept_table().self_reach_cycle(), - wm.intercept_table().teammate_reach_cycle()) - ball_pos = wm.ball().inertia_point(min_step) - - for i in range(1, 12): - p = wm.our_player(i) - if p is None or p.unum_count() >= 2: - objects[i].score = -1000 - else: - d = (((p.pos().x() - ball_pos.x()) * x_rate) ** 2 + ((p.pos().y() - ball_pos.y()) * y_rate) ** 2) ** 0.5 - objects[i].score *= exp(-d ** 2 / (2 * variance ** 2)) - objects[i].score *= 0.3 ** p.unum_count() - objects[i].player = p - - p = wm.their_player(i) - if p is None or p.unum_count() >= 2: - objects[i + 11].score = -1000 - else: - d = (((p.pos().x() - ball_pos.x()) * x_rate) ** 2 + ((p.pos().y() - ball_pos.y()) * y_rate) ** 2) ** 0.5 - objects[i + 11].score *= exp(-d ** 2 / (2 * variance ** 2)) - objects[i + 11].score *= 0.3 ** p.unum_count() - objects[i + 11].player = p - - objects = list(filter(lambda x: x.score > 0.1, objects)) - objects.sort(key=lambda x: x.score, reverse=True) - - can_send_ball = False - send_ball_and_player = False - send_players: list[ObjectScore] = [] - - for o in objects: - if o.number == 0: - can_send_ball = True - if len(send_players) == 1: - send_ball_and_player = True - break - else: - send_players.append(o) - if can_send_ball: - send_ball_and_player = True - break - - if len(send_players) >= 3: - break - - if should_say_ball: - can_send_ball = True - - ball_vel = ef.queued_next_ball_vel() - if wm.self().is_kickable() and ef.queued_next_ball_kickable(): - ball_vel.assign(0, 0) - log.sw_log().communication().add_text('(sample communication) next cycle is kickable') - - if wm.kickable_opponent() or wm.kickable_teammate(): - ball_vel.assign(0, 0) - - if can_send_ball and not send_ball_and_player and available_len >= Messenger.SIZES[ - Messenger.Types.BALL]: - if available_len >= Messenger.SIZES[Messenger.Types.BALL_PLAYER]: - agent.add_say_message(BallPlayerMessenger(ef.queued_next_ball_pos(), - ball_vel, - wm.self().unum(), - ef.queued_next_self_pos(), - ef.queued_next_self_body())) - self.update_player_send_time(wm, wm.our_side(), wm.self().unum()) - else: - agent.add_say_message(BallMessenger(ef.queued_next_ball_pos(), ball_vel)) - - self._ball_send_time = wm.time().copy() - log.sw_log().communication().add_text('(sample communication) only ball') - return True - - if send_ball_and_player: - if should_say_goalie and available_len >= Messenger.SIZES[Messenger.Types.BALL_GOALIE]: - goalie = wm.get_their_goalie() - agent.add_say_message(BallGoalieMessenger(ef.queued_next_ball_pos(), - ball_vel, - goalie.pos() + goalie.vel(), - goalie.body())) - self._ball_send_time = wm.time().copy() - self.update_player_send_time(wm, goalie.side(), goalie.unum()) - - log.sw_log().communication().add_text('(sample communication) ball and goalie') - return True - - if available_len >= Messenger.SIZES[Messenger.Types.BALL_PLAYER]: - p = send_players[0].player - if p.unum() == wm.self().unum(): - agent.add_say_message(BallPlayerMessenger(ef.queued_next_ball_pos(), - ball_vel, - wm.self().unum(), - ef.queued_next_self_pos(), - ef.queued_next_self_body())) - else: - agent.add_say_message(BallPlayerMessenger(ef.queued_next_ball_pos(), - ball_vel, - send_players[0].number, - p.pos() + p.vel(), - p.body())) - - self._ball_send_time = wm.time().copy() - self.update_player_send_time(wm, p.side(), p.unum()) - - log.sw_log().communication().add_text(f'(sample communication) ball and player {p.side()}{p.unum()}') - return True - - if wm.ball().pos().x() > 34 and wm.ball().pos().abs_y() < 20: - goalie: PlayerObject = wm.get_their_goalie() - - if goalie is not None \ - and goalie.seen_pos_count() == 0 \ - and goalie.body_count() == 0 \ - and goalie.pos().x() > 53. - 16 \ - and goalie.pos().abs_y() < 20. \ - and goalie.unum() != UNUM_UNKNOWN \ - and goalie.dist_from_self() < 25: - if available_len >= Messenger.SIZES[Messenger.Types.GOALIE_PLAYER]: - player: PlayerObject = None - for p in send_players: - if p.player.unum() != goalie.unum() and p.player.side() != goalie.side(): - player = p.player - break - - if player is not None: - goalie_pos = goalie.pos() + goalie.vel() - goalie_pos.assign( - bound(53. - 16., goalie_pos.x(), 52.9), - bound(-20, goalie_pos.y(), 20), - ) - agent.add_say_message(GoaliePlayerMessenger(goalie.unum(), - goalie_pos, - goalie.body(), - ( - player.unum() if player.side() == wm.our_side() else player.unum() + 11), - player.pos() + player.vel())) - self.update_player_send_time(wm, goalie.side(), goalie.unum()) - self.update_player_send_time(wm, player.side(), player.unum()) - - log.sw_log().communication().add_text(f'(sample communication) say goalie and player: ' - f'goalie({goalie.unum()}): p={goalie.pos()} b={goalie.body()}' - f'player({player.side()}{player.unum()}: {player.pos()})') - return True - - if available_len >= Messenger.SIZES[Messenger.Types.GOALIE]: - goalie_pos = goalie.pos() + goalie.vel() - goalie_pos.assign( - bound(53. - 16., goalie_pos.x(), 52.9), - bound(-20, goalie_pos.y(), 20), - ) - agent.add_say_message(GoalieMessenger(goalie.unum(), - goalie_pos, - goalie.body())) - self._ball_send_time = wm.time().copy() - self._opponent_send_time[goalie.unum()] = wm.time().copy() - - log.sw_log().communication().add_text(f'(sample communication) say goalie info:' - f'{goalie.unum()} {goalie.pos()} {goalie.body()}') - return True - - if len(send_players) >= 3 and available_len >= Messenger.SIZES[Messenger.Types.THREE_PLAYER]: - for o in send_players: - log.os_log().debug(o.player) - log.os_log().debug(o.player.pos()) - p0 = send_players[0].player - p1 = send_players[1].player - p2 = send_players[2].player - - agent.add_say_message(ThreePlayerMessenger(send_players[0].number, - p0.pos() + p0.vel(), - send_players[1].number, - p1.pos() + p1.vel(), - send_players[2].number, - p2.pos() + p2.vel())) - self.update_player_send_time(wm, p0.side(), p0.unum()) - self.update_player_send_time(wm, p1.side(), p1.unum()) - self.update_player_send_time(wm, p2.side(), p2.unum()) - - log.sw_log().communication().add_text(f'(sample communication) three players:' - f'{p0.side()}{p0.unum()}' - f'{p1.side()}{p1.unum()}' - f'{p2.side()}{p2.unum()}') - return True - - if len(send_players) >= 2 and available_len >= Messenger.SIZES[Messenger.Types.TWO_PLAYER]: - p0 = send_players[0].player - p1 = send_players[1].player - - agent.add_say_message(TwoPlayerMessenger(send_players[0].number, - p0.pos() + p0.vel(), - send_players[1].number, - p1.pos() + p1.vel())) - self.update_player_send_time(wm, p0.side(), p0.unum()) - self.update_player_send_time(wm, p1.side(), p1.unum()) - - log.sw_log().communication().add_text(f'(sample communication) two players:' - f'{p0.side()}{p0.unum()}' - f'{p1.side()}{p1.unum()}') - return True - - if len(send_players) >= 1 and available_len >= Messenger.SIZES[Messenger.Types.GOALIE]: - p0 = send_players[0].player - if p0.side() == wm.their_side() \ - and p0.goalie() \ - and p0.pos().x() > 53. - 16. \ - and p0.pos().abs_y() < 20 \ - and p0.dist_from_self() < 25: - goalie_pos = p0.pos() + p0.vel() - goalie_pos.assign( - bound(53. - 16., goalie_pos.x(), 52.9), - bound(-20, goalie_pos.y(), 20), - ) - agent.add_say_message(GoalieMessenger(p0.unum(), - goalie_pos, - p0.body())) - - self.update_player_send_time(wm, p0.side(), p0.unum()) - - log.sw_log().communication().add_text(f'(sample communication) goalie:' - f'{p0.side()}{p0.unum()}') - return True - - if len(send_players) >= 1 and available_len >= Messenger.SIZES[Messenger.Types.ONE_PLAYER]: - p0 = send_players[0].player - - agent.add_say_message(OnePlayerMessenger(send_players[0].number, - p0.pos() + p0.vel())) - - self.update_player_send_time(wm, p0.side(), p0.unum()) - - log.sw_log().communication().add_text(f'(sample communication) one player:' - f'{p0.side()}{p0.unum()}') - return True - - return False - - def update_current_sender(self, agent: 'PlayerAgent'): - wm = agent.world() - if agent.effector().get_say_message_length() > 0: - self._current_sender_unum = wm.self().unum() - return - - self._current_sender_unum = UNUM_UNKNOWN - candidate_unum: list[int] = [] - - if wm.ball().pos().x() < -10. or wm.game_mode().type() != GameModeType.PlayOn: - for unum in range(1, 12): - candidate_unum.append(unum) - else: - goalie_unum = wm.our_goalie_unum() if wm.our_goalie_unum() != UNUM_UNKNOWN else 1 # TODO STRATEGY.GOALIE_UNUM() - for unum in range(1, 12): - if unum != goalie_unum: - candidate_unum.append(unum) - val = wm.time().cycle() + wm.time().stopped_cycle() if wm.time().stopped_cycle() > 0 else wm.time().cycle() - current = val % len(candidate_unum) - next = (val+1)%len(candidate_unum) - - self._current_sender_unum = candidate_unum[current] - self._next_sender_unum = candidate_unum[next] - - def say_recovery(self, agent: 'PlayerAgent'): - current_len = agent.effector().get_say_message_length() - available_len = ServerParam.i().player_say_msg_size() - current_len - if available_len < Messenger.SIZES[Messenger.Types.RECOVERY]: - return False - - agent.add_say_message(RecoveryMessenger(agent.world().self().recovery())) - log.sw_log().communication().add_text('(sample communication) say self recovery') - return True - - - def say_stamina(self, agent: 'PlayerAgent'): - current_len = agent.effector().get_say_message_length() - if current_len == 0: - return False - available_len = ServerParam.i().player_say_msg_size() - current_len - if available_len < Messenger.SIZES[Messenger.Types.STAMINA]: - return False - agent.add_say_message(StaminaMessenger(agent.world().self().stamina())) - log.sw_log().communication().add_text('(sample communication) say self stamina') - return True - - def attention_to_someone(self, agent: 'PlayerAgent'): # TODO IMP FUNC - wm = agent.world() - ef = agent.effector() - - if wm.self().pos().x() > wm.offside_line_x() - 15. \ - and wm.intercept_table().self_reach_cycle() <= 3: - if self._current_sender_unum != wm.self().unum() and self._current_sender_unum != UNUM_UNKNOWN: - agent.do_attentionto(wm.our_side(), self._current_sender_unum) - player = wm.our_player(self._current_sender_unum) - if player is not None: - log.debug_client().add_circle(player.pos(), 3., color='#000088') - log.debug_client().add_line(player.pos(), wm.self().pos(), '#000088') - log.debug_client().add_message(f'AttCurSender{self._current_sender_unum}') - else: - candidates: list[PlayerObject] = [] - for p in wm.teammates_from_self(): - if p.goalie() or p.unum() == UNUM_UNKNOWN or p.pos().x() > wm.offside_line_x() + 1.: - continue - if p.dist_from_self() > 20.: - break - - candidates.append(p) - - self_next = ef.queued_next_self_pos() - - target_teammate: PlayerObject = None - max_x = -100000 - for p in candidates: - diff = p.pos() + p.vel() - self_next - x = diff.x() * (1. - diff.abs_y() / 40) - if x > max_x: - max_x = x - target_teammate = p - - if target_teammate is not None: - log.sw_log().communication().add_text(f'(attentionto someone) most front teammate') - log.debug_client().add_message(f'AttFrontMate{target_teammate.unum()}') - log.debug_client().add_circle(target_teammate.pos(), 3., color='#000088') - log.debug_client().add_line(target_teammate.pos(), wm.self().pos(), '#000088') - agent.do_attentionto(wm.our_side(), target_teammate.unum()) - return - - if wm.self().attentionto_unum() > 0: - log.sw_log().communication().add_text('(attentionto someone) attentionto off. maybe ball owner') - log.debug_client().add_message('AttOffBOwner') - agent.do_attentionto_off() - return - - fastest_teammate = wm.intercept_table().fastest_teammate() - self_min = wm.intercept_table().self_reach_cycle() - mate_min = wm.intercept_table().teammate_reach_cycle() - opp_min = wm.intercept_table().opponent_reach_cycle() - - if fastest_teammate is not None \ - and fastest_teammate.unum() != UNUM_UNKNOWN \ - and mate_min <= 1 \ - and mate_min < self_min \ - and mate_min <= opp_min + 1 \ - and mate_min <= 5 + min(4, fastest_teammate.pos_count()) \ - and wm.ball().inertia_point(mate_min).dist2(ef.queued_next_self_pos()) < 35.**2: - log.debug_client().add_message(f'AttBallOwner{fastest_teammate.unum()}') - log.debug_client().add_circle(fastest_teammate.pos(), 3., color='#000088') - log.debug_client().add_line(fastest_teammate.pos(), wm.self().pos(), '#000088') - agent.do_attentionto(wm.our_side(), fastest_teammate.unum()) - return - - nearest_teammate = wm.get_teammate_nearest_to_ball(5) - if nearest_teammate is not None \ - and nearest_teammate.unum() != UNUM_UNKNOWN \ - and opp_min <= 3 \ - and opp_min <= mate_min \ - and opp_min <= self_min \ - and nearest_teammate.dist_from_self() < 45. \ - and nearest_teammate.dist_from_ball() < 20.: - log.debug_client().add_message(f'AttBallNearest(1){nearest_teammate.unum()}') - log.debug_client().add_circle(nearest_teammate.pos(), 3., color='#000088') - log.debug_client().add_line(nearest_teammate.pos(), wm.self().pos(), '#000088') - agent.do_attentionto(wm.our_side(), nearest_teammate.unum()) - return - - if nearest_teammate is not None \ - and nearest_teammate.unum() != UNUM_UNKNOWN \ - and wm.ball().pos_count() >= 3 \ - and nearest_teammate.dist_from_ball() < 20.: - log.debug_client().add_message(f'AttBallNearest(2){nearest_teammate.unum()}') - log.debug_client().add_circle(nearest_teammate.pos(), 3., color='#000088') - log.debug_client().add_line(nearest_teammate.pos(), wm.self().pos(), '#000088') - agent.do_attentionto(wm.our_side(), nearest_teammate.unum()) - return - - if nearest_teammate is not None \ - and nearest_teammate.unum() != 45. \ - and nearest_teammate.dist_from_self() < 45. \ - and nearest_teammate.dist_from_ball() < 3.5: - log.debug_client().add_message(f'AttBallNearest(3){nearest_teammate.unum()}') - log.debug_client().add_circle(nearest_teammate.pos(), 3., color='#000088') - log.debug_client().add_line(nearest_teammate.pos(), wm.self().pos(), '#000088') - agent.do_attentionto(wm.our_side(), nearest_teammate.unum()) - return - - if self._current_sender_unum != wm.self().unum() and self._current_sender_unum != UNUM_UNKNOWN: - log.debug_client().add_message(f'AttCurSender{self._current_sender_unum}') - player = wm.our_player(self._current_sender_unum) - if player is not None: - log.debug_client().add_circle(player.pos(), 3., color='#000088') - log.debug_client().add_line(player.pos(), wm.self().pos(), '#000088') - agent.do_attentionto(wm.our_side(), self._current_sender_unum) - else: - log.debug_client().add_message(f'AttOff') - agent.do_attentionto_off() - - - - - def execute(self, agent: 'PlayerAgent'): - if not team_config.USE_COMMUNICATION: - return False - - self.update_current_sender(agent) - - wm = agent.world() - penalty_shootout = wm.game_mode().is_penalty_kick_mode() - - say_recovery = False - if wm.game_mode().type() == GameModeType.PlayOn \ - and not penalty_shootout \ - and self._current_sender_unum == wm.self().unum() \ - and wm.self().recovery() < ServerParam.i().recover_init() - 0.002: - say_recovery = True - self.say_recovery(agent) - - if wm.game_mode().type() == GameModeType.BeforeKickOff \ - or wm.game_mode().type().is_after_goal() \ - or penalty_shootout: - return say_recovery - - if not(wm.game_mode().type() == GameModeType.BeforeKickOff or wm.game_mode().type().is_kick_off()): - self.say_ball_and_players(agent) - self.say_stamina(agent) - - self.attention_to_someone(agent) # TODO IMP FUNC - - return True diff --git a/base/sample_player.py b/base/sample_player.py deleted file mode 100644 index 17657264..00000000 --- a/base/sample_player.py +++ /dev/null @@ -1,111 +0,0 @@ -from base.decision import get_decision -from base.sample_communication import SampleCommunication -from base.view_tactical import ViewTactical -from lib.action.go_to_point import GoToPoint -from lib.action.intercept import Intercept -from lib.action.neck_body_to_ball import NeckBodyToBall -from lib.action.neck_turn_to_ball import NeckTurnToBall -from lib.action.neck_turn_to_ball_or_scan import NeckTurnToBallOrScan -from lib.action.scan_field import ScanField -from lib.debug.debug import log -from lib.debug.level import Level -from lib.player.player_agent import PlayerAgent -from lib.rcsc.server_param import ServerParam -from lib.rcsc.types import GameModeType - - -class SamplePlayer(PlayerAgent): - def __init__(self): - super().__init__() - - self._communication = SampleCommunication() - - def action_impl(self): - wm = self.world() - if self.do_preprocess(): - return - - get_decision(self) - - def do_preprocess(self): - wm = self.world() - - if wm.self().is_frozen(): - self.set_view_action(ViewTactical()) - self.set_neck_action(NeckTurnToBallOrScan()) - return True - - if not wm.self().pos_valid(): - self.set_view_action(ViewTactical()) - ScanField().execute(self) - return True - - count_thr = 10 if wm.self().goalie() else 5 - if wm.ball().pos_count() > count_thr or ( wm.game_mode().type() is not GameModeType.PlayOn and wm.ball().seen_pos_count() > count_thr + 10): - self.set_view_action(ViewTactical()) - NeckBodyToBall().execute(self) - return True - - self.set_view_action(ViewTactical()) - - if self.do_heard_pass_receive(): - return True - - return False - - def do_heard_pass_receive(self): - wm = self.world() - - if wm.messenger_memory().pass_time() != wm.time() \ - or len(wm.messenger_memory().pass_()) == 0 \ - or wm.messenger_memory().pass_()[0]._receiver != wm.self().unum(): - - return False - - self_min = wm.intercept_table().self_reach_cycle() - intercept_pos = wm.ball().inertia_point(self_min) - heard_pos = wm.messenger_memory().pass_()[0]._pos - - log.sw_log().team().add_text( f"(sample palyer do heard pass) heard_pos={heard_pos}, intercept_pos={intercept_pos}") - - if not wm.kickable_teammate() \ - and wm.ball().pos_count() <= 1 \ - and wm.ball().vel_count() <= 1 \ - and self_min < 20: - log.sw_log().team().add_text( f"(sample palyer do heard pass) intercepting!, self_min={self_min}") - log.debug_client().add_message("Comm:Receive:Intercept") - Intercept().execute(self) - self.set_neck_action(NeckTurnToBall()) - else: - log.sw_log().team().add_text( f"(sample palyer do heard pass) go to point!, cycle={self_min}") - log.debug_client().set_target(heard_pos) - log.debug_client().add_message("Comm:Receive:GoTo") - - GoToPoint(heard_pos, 0.5, ServerParam.i().max_dash_power()).execute(self) - self.set_neck_action(NeckTurnToBall()) - - # TODO INTENTION?!? - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/base/tools.py b/base/tools.py deleted file mode 100644 index 75b268f0..00000000 --- a/base/tools.py +++ /dev/null @@ -1,83 +0,0 @@ -import math - -from pyrusgeom.geom_2d import * -from lib.rcsc.server_param import ServerParam -from lib.rcsc.player_type import PlayerType -from lib.rcsc.game_mode import GameModeType -from lib.action.kick_table import calc_max_velocity - -from typing import TYPE_CHECKING -if TYPE_CHECKING: - from lib.player.world_model import WorldModel - from lib.player.object_player import PlayerObject - -class Tools: - @staticmethod - def predict_player_turn_cycle(ptype: PlayerType, player_body: AngleDeg, player_speed, target_dist, - target_angle: AngleDeg, dist_thr, use_back_dash): - sp = ServerParam.i() - n_turn = 0 - angle_diff = (target_angle - player_body).abs() - - if use_back_dash and target_dist < 5.0 and angle_diff > 90.0 and sp.min_dash_power() < -sp.max_dash_power() + 1.0: - angle_diff = abs( angle_diff - 180.0 ) - - turn_margin = 180.0 - if dist_thr < target_dist: - turn_margin = max( 15.0, AngleDeg.asin_deg( dist_thr / target_dist ) ) - - speed = float(player_speed) - while angle_diff > turn_margin: - angle_diff -= ptype.effective_turn( sp.max_moment(), speed ) - speed *= ptype.player_decay() - n_turn += 1 - - return n_turn - - @staticmethod - def predict_kick_count(wm: 'WorldModel', kicker, first_ball_speed, ball_move_angle: AngleDeg): - if wm.game_mode().type() != GameModeType.PlayOn and not wm.game_mode().is_penalty_kick_mode(): - return 1 - - if kicker == wm.self().unum() and wm.self().is_kickable(): - max_vel = calc_max_velocity(ball_move_angle, wm.self().kick_rate(), wm.ball().vel()) - if max_vel.r2() >= pow( first_ball_speed, 2): - return 1 - if first_ball_speed > 2.5: - return 3 - elif first_ball_speed > 1.5: - return 2 - return 1 - - @staticmethod - def estimate_min_reach_cycle(player_pos: Vector2D, player_speed_max, target_first_point: Vector2D, target_move_angle: AngleDeg): - target_to_player: Vector2D = (player_pos - target_first_point).rotated_vector(-target_move_angle) - if target_to_player.x() < -1.0: - return -1 - else: - return max( 1, int(target_to_player.abs_y() / player_speed_max)) - - @staticmethod - def get_nearest_teammate(wm: 'WorldModel', position: Vector2D, players: list['PlayerObject'] =None): - if players is None: - players = wm.teammates() - best_player: 'PlayerObject' = None - min_dist2 = 1000 - for player in players: - d2 = player.pos().dist2( position ) - if d2 < min_dist2: - min_dist2 = d2 - best_player = player - - return best_player - - @staticmethod - def estimate_virtual_dash_distance(player: 'PlayerObject'): - pos_count = min(10, player.pos_count(), player.seen_pos_count()) - max_speed = player.player_type().real_speed_max() * 0.8 - - d = 0. - for i in range(pos_count): - d += max_speed * math.exp(-(i**2)/15) - - return d diff --git a/build_image.sh b/build_image.sh new file mode 100755 index 00000000..d359b0f8 --- /dev/null +++ b/build_image.sh @@ -0,0 +1,11 @@ +#!/bin/bash + +# Path to the Dockerfile directory +DOCKERFILE_PATH="$(dirname "$0")" + +# Image name +IMAGE_NAME="keepaway" + +# Build the Docker image +docker build -t "$IMAGE_NAME" "$DOCKERFILE_PATH" + diff --git a/docs/docker.md b/docs/docker.md new file mode 100644 index 00000000..23e75b1a --- /dev/null +++ b/docs/docker.md @@ -0,0 +1,13 @@ +# Running in Docker # + +Build the image and run the shell script. Pass the directory to the Dockerfile as an argument + +``` +./build_image.sh {$DIR} +``` + +Run the docker image. + +``` +docker run -v /tmp/.X11-unix:/tmp/.X11-unix:rw -it keepaway +``` diff --git a/entrypoint.sh b/entrypoint.sh new file mode 100755 index 00000000..2d89ceac --- /dev/null +++ b/entrypoint.sh @@ -0,0 +1,5 @@ +#!/bin/bash +# Activate the Conda environment +source activate keepaway +# Execute the command passed to the Docker container +exec "$@" diff --git a/keepaway/.DS_Store b/keepaway/.DS_Store new file mode 100644 index 00000000..36b2589b Binary files /dev/null and b/keepaway/.DS_Store differ diff --git a/keepaway/Makefile b/keepaway/Makefile new file mode 100755 index 00000000..994cf1e2 --- /dev/null +++ b/keepaway/Makefile @@ -0,0 +1,13 @@ +cleanall: + rm -fr keeper_Q*.gz taker_Q*.gz logs/* *.lock core core.* vgcore.* + rm -fr *.lock console.log nohup.out *.dot *.xml + rm -fr keeper_Q*.log + rm -fr taker_Q*.log + rm -fr python_debug.log + rm -fr player* + rm -fr keepers-*.log + rm -fr takers-*.log + rm -fr NA-*.log + rm -fr *.rcg *.rcl + ./kill.sh + killall -q keepaway_player 1>/dev/null 2>&1 diff --git a/keepaway/__init__.py b/keepaway/__init__.py new file mode 100644 index 00000000..7a64b7e5 --- /dev/null +++ b/keepaway/__init__.py @@ -0,0 +1,5 @@ +from keepaway.envs.multiagentenv import MultiAgentEnv +from keepaway.envs.keepaway_env import KeepawayEnv + +__all__ = ["MultiAgentEnv", "KeepawayEnv"] + diff --git a/keepaway/base/.DS_Store b/keepaway/base/.DS_Store new file mode 100644 index 00000000..57adcf51 Binary files /dev/null and b/keepaway/base/.DS_Store differ diff --git a/keepaway/base/__init__.py b/keepaway/base/__init__.py new file mode 100755 index 00000000..4bd694ed --- /dev/null +++ b/keepaway/base/__init__.py @@ -0,0 +1,2 @@ +from keepaway.base import * +# from keepaway.base.set_play import * \ No newline at end of file diff --git a/base/basic_tackle.py b/keepaway/base/basic_tackle.py old mode 100644 new mode 100755 similarity index 86% rename from base/basic_tackle.py rename to keepaway/base/basic_tackle.py index 886fd4ff..93034775 --- a/base/basic_tackle.py +++ b/keepaway/base/basic_tackle.py @@ -4,14 +4,14 @@ from pyrusgeom.ray_2d import Ray2D from pyrusgeom.vector_2d import Vector2D -from base.tackle_generator import TackleGenerator -from lib.action.neck_turn_to_point import NeckTurnToPoint -from lib.debug.debug import log -from lib.rcsc.server_param import ServerParam -from lib.rcsc.types import Card +from keepaway.base.tackle_generator import TackleGenerator +from keepaway.lib.action.neck_turn_to_point import NeckTurnToPoint +from keepaway.lib.debug.debug import log +from keepaway.lib.rcsc.server_param import ServerParam +from keepaway.lib.rcsc.types import Card if TYPE_CHECKING: - from lib.player.player_agent import PlayerAgent + from keepaway.lib.player.player_agent import PlayerAgent class BasicTackle: diff --git a/base/bhv_block.py b/keepaway/base/bhv_block.py old mode 100644 new mode 100755 similarity index 81% rename from base/bhv_block.py rename to keepaway/base/bhv_block.py index e1e1a02a..65c42874 --- a/base/bhv_block.py +++ b/keepaway/base/bhv_block.py @@ -1,16 +1,16 @@ -from lib.action.go_to_point import GoToPoint -from base.strategy_formation import StrategyFormation -from lib.action.intercept import Intercept -from base.tools import Tools -from base.stamina_manager import get_normal_dash_power +from keepaway.lib.action.go_to_point import GoToPoint +from keepaway.base.strategy_formation import StrategyFormation +from keepaway.lib.action.intercept import Intercept +from keepaway.base.tools import Tools +from keepaway.base.stamina_manager import get_normal_dash_power from pyrusgeom.soccer_math import * from typing import TYPE_CHECKING -from lib.debug.debug import log +from keepaway.lib.debug.debug import log if TYPE_CHECKING: - from lib.player.player_agent import PlayerAgent + from keepaway.lib.player.player_agent import PlayerAgent class Bhv_Block: def execute(self, agent: 'PlayerAgent'): diff --git a/keepaway/base/bhv_kick.py b/keepaway/base/bhv_kick.py new file mode 100755 index 00000000..f79d9a1a --- /dev/null +++ b/keepaway/base/bhv_kick.py @@ -0,0 +1,192 @@ +from keepaway.lib.action.hold_ball import HoldBall +from keepaway.lib.action.neck_scan_players import NeckScanPlayers +from keepaway.lib.action.smart_kick import SmartKick +from typing import List +from keepaway.base.generator_action import KickAction, ShootAction, KickActionType +from keepaway.base.generator_dribble import BhvDribbleGen +from keepaway.base.generator_pass import BhvPassGen +from keepaway.base.generator_shoot import BhvShhotGen +from keepaway.base.generator_clear import BhvClearGen +from keepaway.lib.rcsc.server_param import ServerParam +from pyrusgeom.line_2d import Line2D +from pyrusgeom.vector_2d import Vector2D +from keepaway.base.tools import Tools + +from typing import TYPE_CHECKING + +from keepaway.lib.debug.debug import log +from keepaway.lib.messenger.pass_messenger import PassMessenger + +if TYPE_CHECKING: + from keepaway.lib.player.world_model import WorldModel + from keepaway.lib.player.player_agent import PlayerAgent + + +class BhvKick: + def __init__(self): + pass + + def execute(self, agent: "PlayerAgent"): + wm: "WorldModel" = agent.world() + shoot_candidate: ShootAction = BhvShhotGen().generator(wm) + if shoot_candidate: + log.debug_client().set_target(shoot_candidate.target_point) + log.debug_client().add_message( + "shoot" + + "to " + + shoot_candidate.target_point.__str__() + + " " + + str(shoot_candidate.first_ball_speed) + ) + SmartKick( + shoot_candidate.target_point, + shoot_candidate.first_ball_speed, + shoot_candidate.first_ball_speed - 1, + 3, + ).execute(agent) + agent.set_neck_action(NeckScanPlayers()) + return True + else: + action_candidates: List[KickAction] = [] + action_candidates += BhvPassGen().generator(wm) + # action_candidates += BhvDribbleGen().generator(wm) # TODO + + if len(action_candidates) == 0: + return self.no_candidate_action(agent) + + best_action: KickAction = max(action_candidates)# + + target = best_action.target_ball_pos + log.debug_client().set_target(target) + log.debug_client().add_message( + best_action.type.value + + "to " + + best_action.target_ball_pos.__str__() + + " " + + str(best_action.start_ball_speed) + ) + SmartKick( + target, + best_action.start_ball_speed, + best_action.start_ball_speed - 1, + 3, + ).execute(agent) + + if best_action.type is KickActionType.Pass: + agent.add_say_message( + PassMessenger( + best_action.target_unum, + best_action.target_ball_pos, + agent.effector().queued_next_ball_pos(), + agent.effector().queued_next_ball_vel(), + ) + ) + + agent.set_neck_action(NeckScanPlayers()) + return True + + def no_candidate_action(self, agent: "PlayerAgent"): + wm = agent.world() + opp_min = wm.intercept_table().opponent_reach_cycle() + if opp_min <= 3: + action_candidates = BhvClearGen().generator(wm) + if len(action_candidates) > 0: + best_action: KickAction = max(action_candidates) + target = best_action.target_ball_pos + log.debug_client().set_target(target) + log.debug_client().add_message( + best_action.type.value + + "to " + + best_action.target_ball_pos.__str__() + + " " + + str(best_action.start_ball_speed) + ) + SmartKick( + target, + best_action.start_ball_speed, + best_action.start_ball_speed - 2.0, + 2, + ).execute(agent) + + agent.set_neck_action(NeckScanPlayers()) + return HoldBall().execute(agent) + + # def kick_to(self,agent: "PlayerAgent",tar_pos,speed): + # SP = ServerParam.i() + # wm = agent.world() + + # ball_pos = wm.ball().pos() + # ball_vel = wm.ball().vel() + # travel_dist = tar_pos - ball_pos + # curr_pos = wm.self().pos() + + # # set polar + # cal_travel_speed = Tools.get_kick_travel(travel_dist.r(), speed) + # vel_des = Vector2D.polar2vector(cal_travel_speed, travel_dist.dir()) + + # # vel_des = (tar_pos - curr_pos).normalized() * speed + + # if (wm.predict_pos().dist(ball_pos + vel_des) < SP.ball_size() + SP.player_size() ): + # line_segment = Line2D(ball_pos,ball_pos + travel_dist) + # body_proj = line_segment.projection(curr_pos) + # dist = ball_pos.dist(body_proj) + # if (travel_dist.r() < dist): + # dist -= SP.ball_size() + SP.player_size() + # else: + # dist += SP.ball_size() + SP.player_size() + # # Log.log(102, "kick results in collision, change velDes from (%f,%f)", + # # velDes.getX(), velDes.getY()); + # travel_dist.set_polar(dist, travel_dist.th()) + + # dDistOpp = std::numeric_limits::max(); + # objOpp = WM->getClosestInSetTo(OBJECT_SET_OPPONENTS, + # OBJECT_BALL, &dDistOpp); + + # # // can never reach point + # if (travel_dist.r() > SP.ball_speed_max()): + # pow = SP.getMaxPower() + # speed = wm.self().kick_rate() * pow + # tmp = ball_vel.rotate(-travel_dist.th()).get_y() + # ang = travel_dist.th() - AngleDeg.asin_deg(tmp / speed) + # speedpred = (ball_vel + Vector2D.polar2vector(speed, ang)).r() + # # but ball acceleration in right direction is very high + # # player kick prop 0.85 - handcoded + # if (speedpred > 0.85 * SP.ball_accel_max()): + # # Log.log(102, "pos (%f,%f) too far, but can acc ball good to %f k=%f,%f", + # # velDes.getX(), velDes.getY(), dSpeedPred, dSpeed, tmp); + # # // shoot nevertheless + + # return accelerateBallToVelocity(vel_des) + # elif (wm.kick_power_rate() > 0.85 * SP.kick_power_rate()): + # # { + # # Log.log(102, "point too far, freeze ball"); // ball well-positioned + # # // freeze ball + # return freezeBall(); + # else: + # # Log.log(102, "point too far, reposition ball (k_r = %f)", + # # WM->getActualKickPowerRate() / (SS->getKickPowerRate())); + # # // else position ball better + + # return kickBallCloseToBody(0); + # # // can reach point + # else : + # Vector2D accBallDes = vel_des - vel_ball + # dPower = wm.get_kick_power_speed(accBallDes.getMagnitude()) + + # # // with current ball speed + # if (dPower <= 1.05 * SS->getMaxPower() or (dDistOpp < 2.0 && dPower <= 1.30 * + # SP.getMaxPower())): // 1.05 since cannot get ball fully perfect + # # Log.log(102, "point good and can reach point %f", dPower); + # # // perform shooting action + # return accelerateBallToVelocity(velDes); + # else: + # # Log.log(102, "point good, but reposition ball since need %f", dPower); + # SoccerCommand soc = kickBallCloseToBody(0); + # VecPosition posPredBall; + + # WM->predictBallInfoAfterCommand(soc, &posPredBall); + # dDistOpp = posPredBall.getDistanceTo(WM->getGlobalPosition(objOpp)); + # # // position ball better + # return soc; + + diff --git a/keepaway/base/bhv_move.py b/keepaway/base/bhv_move.py new file mode 100755 index 00000000..ddb74bf7 --- /dev/null +++ b/keepaway/base/bhv_move.py @@ -0,0 +1,73 @@ +from keepaway.base.basic_tackle import BasicTackle +from keepaway.lib.action.go_to_point import GoToPoint +from keepaway.base.strategy_formation import StrategyFormation +from keepaway.lib.action.intercept import Intercept +from keepaway.lib.action.neck_turn_to_ball import NeckTurnToBall +from keepaway.lib.action.neck_turn_to_ball_or_scan import NeckTurnToBallOrScan +from keepaway.lib.action.turn_to_ball import TurnToBall +from keepaway.lib.action.turn_to_point import TurnToPoint +from keepaway.base.tools import Tools +from keepaway.base.stamina_manager import get_normal_dash_power +from keepaway.base.bhv_block import Bhv_Block +from pyrusgeom.vector_2d import Vector2D + +from typing import TYPE_CHECKING + +from keepaway.lib.debug.debug import log + +if TYPE_CHECKING: + from keepaway.lib.player.player_agent import PlayerAgent + from keepaway.lib.player.world_model import WorldModel + + +class BhvMove: + def __init__(self): + self._in_recovery_mode = False + pass + + def execute(self, agent: "PlayerAgent"): + wm: "WorldModel" = agent.world() + + if BasicTackle(0.8, 80).execute(agent): + return True + + # intercept + self_min = wm.intercept_table().self_reach_cycle() + tm_min = wm.intercept_table().teammate_reach_cycle() + opp_min = wm.intercept_table().opponent_reach_cycle() + log.sw_log().block().add_text(f"self_min={self_min}") + log.sw_log().block().add_text(f"tm_min={tm_min}") + log.sw_log().block().add_text(f"opp_min={opp_min}") + + if not wm.exist_kickable_teammates() and ( + self_min <= 2 or (self_min <= tm_min and self_min < opp_min + 5) + ): + log.sw_log().block().add_text("INTERCEPTING") + log.debug_client().add_message("intercept") + if Intercept().execute(agent): + agent.set_neck_action(NeckTurnToBall()) + return True + + if opp_min < min(tm_min, self_min): + if Bhv_Block().execute(agent): + agent.set_neck_action(NeckTurnToBall()) + return True + st = StrategyFormation().i() + target = st.get_pos(agent.world().self().unum()) + + log.debug_client().set_target(target) + log.debug_client().add_message("bhv_move") + + dash_power, self._in_recovery_mode = get_normal_dash_power( + wm, self._in_recovery_mode + ) + dist_thr = wm.ball().dist_from_self() * 0.1 + + if dist_thr < 1.0: + dist_thr = 1.0 + + print("target", target) + if GoToPoint(target, dist_thr, dash_power).execute(agent): + agent.set_neck_action(NeckTurnToBallOrScan()) + return True + return False diff --git a/keepaway/base/decision.py b/keepaway/base/decision.py new file mode 100755 index 00000000..b1cff6e5 --- /dev/null +++ b/keepaway/base/decision.py @@ -0,0 +1,575 @@ +from keepaway.base import goalie_decision +from keepaway.base.strategy_formation import StrategyFormation +from keepaway.base.set_play.bhv_set_play import Bhv_SetPlay +from keepaway.base.bhv_kick import BhvKick +from keepaway.base.bhv_move import BhvMove +from keepaway.lib.action.neck_scan_field import NeckScanField +from keepaway.lib.action.neck_scan_players import NeckScanPlayers +from keepaway.lib.action.neck_turn_to_ball import NeckTurnToBall +from keepaway.lib.action.scan_field import ScanField +from keepaway.lib.debug.debug import log +from keepaway.lib.action.hold_ball import HoldBall + +from keepaway.utils.keepaway_actions import SmartKick, GoToPoint, NeckTurnToBallOrScan,NeckBodyToPoint + +from keepaway.lib.action.turn_to_ball import TurnToBall +from keepaway.lib.action.neck_body_to_ball import NeckBodyToBall +from keepaway.lib.rcsc.server_param import ServerParam + +from keepaway.base.generator_pass import BhvPassGen +from keepaway.lib.action.intercept import Intercept + + +from pyrusgeom.soccer_math import * +from pyrusgeom.rect_2d import Rect2D +from pyrusgeom.vector_2d import Vector2D +from pyrusgeom.line_2d import Line2D +from pyrusgeom.segment_2d import Segment2D +from pyrusgeom.geom_2d import * + +from keepaway.base.tools import Tools +from typing import TYPE_CHECKING + +import math as Math + +if TYPE_CHECKING: + from keepaway.lib.player.world_model import WorldModel + from keepaway.lib.player.player_agent import PlayerAgent + +# TODO TACKLE GEN +# TODO GOAL KICK L/R +# TODO GOAL L/R +DEBUG = True + + +def get_decision(agent: "PlayerAgent"): + wm: "WorldModel" = agent.world() + + st = StrategyFormation().i() + st.update(wm) + + if wm.self().goalie(): + if goalie_decision.decision(agent): + return True + + if wm.game_mode().type() != GameModeType.PlayOn: + if Bhv_SetPlay().execute(agent): + return True + + log.sw_log().team().add_text( + f"is kickable? dist {wm.ball().dist_from_self()} " + f"ka {wm.self().player_type().kickable_area()} " + f"seen pos count {wm.ball().seen_pos_count()} " + f"is? {wm.self()._kickable}" + ) + if wm.self().is_kickable(): + ## TODO: check this + return BhvKick().execute(agent) + if BhvMove().execute(agent): + return True + log.os_log().warn("NO ACTION, ScanFIELD") + return ScanField().execute(agent) + + +# working code - should + + +def get_decision_keepaway( + agent: "PlayerAgent", + count_list, + barrier, + event_to_set, + event_to_wait, + obs, + last_action_time, + reward, + terminated, + full_world, +): + wm: "WorldModel" = agent.world() + + # void GetOpen::run() { + # int status = WM->isTmControllBall(); + # while (running() && status == WM->isTmControllBall()) { + # SoccerCommand soc; + # ObjectT fastest = WM->getFastestInSetTo(OBJECT_SET_TEAMMATES, OBJECT_BALL); + # int iCycles = WM->predictNrCyclesToObject(fastest, OBJECT_BALL); + # VecPosition posPassFrom = WM->predictPosAfterNrCycles(OBJECT_BALL, iCycles); + # posPassFrom = refineTarget(posPassFrom, WM->getBallPos()); + # ACT->putCommandInQueue(soc = player->getOpenForPassFromInRectangle(WM->getKeepawayRect(), posPassFrom)); + # ACT->putCommandInQueue(player->turnNeckToObject(OBJECT_BALL, soc)); + # Log.log(101, "GetOpen::run action"); + # Action(this)(); + # if (WM->isBallKickable()) break; + # } + + # bool WorldModel::isTmControllBall() { + # ObjectT K0 = getClosestInSetTo(OBJECT_SET_TEAMMATES, OBJECT_BALL); + # VecPosition B = getBallPos(); + # double WK0_dist_to_B = getGlobalPosition(K0).getDistanceTo(B); + # bool tmControllBall = WK0_dist_to_B < getMaximalKickDist(K0); + # return tmControllBall || isBallKickable(); + # } + + def get_marking_position(wm, pos, dDist): + """Returns the marking position.""" + ball_pos = wm.ball().pos() + ball_angle = (ball_pos - pos._pos).dir() + return pos._pos + Vector2D.polar2vector(dDist, ball_angle) + + def mark_opponent(wm, p, dDist, obj): # TODO: check this. + """Mark the given opponent.""" + + pos_mark = get_marking_position(wm, p, dDist) + pos_agent = p.pos() + pos_ball = wm.ball().pos() + if obj == "ball": + if pos_mark.dist(pos_agent) < 1.5: + TurnToBall().execute(agent) + else: + GoToPoint(pos_mark, 0.2, 100).execute(agent) + if pos_agent.dist(pos_mark) < 2.0: + ang_opp = (p.pos() - pos_agent).th() + ang_ball = (pos_ball - pos_agent).th() + normalized_ang_opp = AngleDeg.normalize_angle(ang_opp + 180) + + if ang_ball.is_within(ang_opp, normalized_ang_opp): + ang_opp += 80 + else: + ang_opp -= 80 + + ang_opp = AngleDeg.normalize_angle(ang_opp) + target = pos_agent + Vector2D(1.0, ang_opp, POLAR) + + return NeckBodyToPoint(target).execute(agent) + + return GoToPoint(pos_mark, 0.2, 100).execute(agent) + + def mark_most_open_opponent(wm): + """Mark the most open opponent.""" + keepers = wm.opponents() + if len(keepers) == 0: + return + else: + pos_from = keepers[0].pos() + min_player = None + min = 1000 + for p in keepers: + if p.pos_valid(): + point = p.pos() + if point.abs_y() == 37: + continue + num = get_in_set_in_cone(wm, 0.3, pos_from, point) + if num < min: + min = num + min_player = p + return mark_opponent(wm, min_player, 4.0, "ball") + + def do_heard_pass_receive(wm, agent): + """ + check on pass . + + """ + # print(" checking on pass ") + # print("pass ball changed .. ", len(wm.messenger_memory().balls())) + # print("pass time ", wm.messenger_memory().pass_time()) + # print("world model time ", wm.time()) + # print("length of pass ", wm.messenger_memory().pass_()) + # if len(wm.messenger_memory().pass_()) > 0: + # print("pass receiver ", wm.messenger_memory().pass_()[0]._receiver) + # print("memory players ", wm.messenger_memory().players()) + + if ( + wm.messenger_memory().pass_time() != wm.time() + or len(wm.messenger_memory().pass_()) == 0 + or wm.messenger_memory().pass_()[0]._receiver != wm.self().unum() + ): + # print("False") + return False + + self_min = wm.intercept_table().self_reach_cycle() + intercept_pos = wm.ball().inertia_point(self_min) + heard_pos = wm.messenger_memory().pass_()[0]._pos + + # print("heard pos ", heard_pos) + + log.sw_log().team().add_text( + f"(sample player do heard pass) heard_pos={heard_pos}, intercept_pos={intercept_pos}" + ) + + if ( + not wm.kickable_teammate() + and wm.ball().pos_count() <= 1 + and wm.ball().vel_count() <= 1 + and self_min < 20 + ): + print( + "sample player do heard pass) intercepting!", "i am ", wm.self().unum() + ) + + log.sw_log().team().add_text( + f"(sample player do heard pass) intercepting!, self_min={self_min}" + ) + log.debug_client().add_message("Comm:Receive:Intercept") + Intercept().execute(agent) + agent.set_neck_action(NeckTurnToBall()) + + else: + print("(sample player do heard pass) go to point!, cycle ", self_min, " i am ", wm.self().unum()) + + log.sw_log().team().add_text( + f"(sample player do heard pass) go to point!, cycle={self_min}" + ) + log.debug_client().set_target(heard_pos) + log.debug_client().add_message("Comm:Receive:GoTo") + + GoToPoint(heard_pos, 1.0, ServerParam.i().max_dash_power()).execute(agent) + agent.set_neck_action(NeckTurnToBall()) + + def get_in_set_in_cone(wm, radius, pos_from, pos_to): + """Returns the number of players in the given set in the cone from posFrom to posTo with the given radius.""" + conf_threshold = 0.88 + line_segments = Segment2D(pos_from, pos_to) + count = 0 + for p in wm._all_players: + if p.pos_valid(): + pos = p.pos() + pos_on_line = line_segments.nearest_point(pos) + ## projection not right yet . line.isInBetween(posOnLine, start, end) + if ( + (pos_on_line.dist(pos) < radius * pos_on_line.dist(pos_from)) + and (line_segments.projection(pos_on_line)) + and (pos_from.dist(pos) < pos_from.dist(pos_to)) + ): + count += 1 + return count + + def congestion(wm, point, consider_me): + """Returns the congestion at the given position.""" + congest = 0 + for p in wm._teammates: + if p.pos_valid() and p.pos() != point: + congest += 1 / p.pos().dist(point) + + for p in wm._opponents: + if p.pos_valid(): + congest += 1 / p.pos().dist(point) + return congest + + def least_congested_point_for_pass_in_rectangle(rect: Rect2D, pos_from): + """Returns the least congested point for a pass in the given rectangle.""" + + x_granularity = 5 # 5 samples by 5 samples + y_granularity = 5 + x_buffer = 0.15 # 15% border on each side + y_buffer = 0.15 + size = rect.size() + length = size.length() + width = size.width() + x_mesh = length * (1 - 2 * x_buffer) / (x_granularity - 1) + y_mesh = width * (1 - 2 * y_buffer) / (y_granularity - 1) + + start_x = rect.bottom_left().x() + x_buffer * length + start_y = rect.top_left().y() + y_buffer * width + + x = start_x + y = start_y + + # print("X and Y ", x, y) + + best_congestion = 1000 + point = Vector2D(x, y) + tmp = None + + for i in range(x_granularity): + for j in range(y_granularity): + point = Vector2D(x, y) + # print("point", point) + tmp = congestion(wm, point, True) + if ( + tmp < best_congestion + and get_in_set_in_cone(wm, 0.3, pos_from, point) == 0 + ): + best_congestion = tmp + best_point = point + y += y_mesh + x += x_mesh + y = start_y + + if best_congestion == 1000: + # take the point out of the rectangle -- meaning no point was valid. + best_point = rect.center() + return best_point + + def get_open(wm,best_point): + if wm.self().pos().dist(best_point) < 1.5: + NeckBodyToPoint(best_point).execute(agent) + else: + GoToPoint(best_point, 0.2, 100).execute(agent) + + + def keeper_support(wm, fastest, agent): + """Keeper support.""" + from keepaway.lib.messenger.one_player_messenger import OnePlayerMessenger + + sp = ServerParam.i() + first_ball_pos = wm.ball().pos() + min_reach_cycle = Tools.estimate_min_reach_cycle( + fastest._pos, + 1.0, + first_ball_pos, + first_ball_pos.th(), + ) + # print("min_reach_cycle ", min_reach_cycle) + ball_vel = wm.ball().vel() + ball_pos = wm.ball().pos() + first_ball_speed = ball_vel.r() + + dist_ball = ( + first_ball_speed + * (1 - pow(sp.ball_decay(), min_reach_cycle)) + / (1 - sp.ball_decay()) + ) + ball_angle = ball_vel.th() + ball_pos += Vector2D.polar2vector(dist_ball, ball_angle) + ball_vel += ball_vel * pow(sp.ball_decay(), min_reach_cycle) + pos_pass_from = ball_pos + + rect = wm.keepaway_rect() + best_point = least_congested_point_for_pass_in_rectangle(rect, pos_pass_from) + + + if do_heard_pass_receive(wm, agent) == False: + # print("i am ", wm.self()._unum,"no pass heard") + # print("i am ", wm.self()._unum, "going to ", best_point) + GoToPoint(best_point, 0.2, 100).execute(agent) + return + else: + print("pass was heard ") + # i am waiting for the pass. + agent.set_neck_action(NeckScanField()) + return + + # if wm.self().pos().dist(best_point) < 1.5: + # print("NeckScanField") + # return NeckBodyToPoint(wm.ball().pos()).execute(agent) + # # agent.set_neck_action(NeckScanPlayers()) + # else: + # print("i am ", wm.self()._unum, "going to ", best_point) + + # # agent.add_say_message(OnePlayerMessenger(wm.self().unum(), + # # best_point)) + # GoToPoint(best_point, 0.2, 100).execute(agent) + # return + + # # # # ObjectT lookObject = self._choose_look_object( 0.97 ) + + + + def search_ball(wm, agent): + return ScanField().execute(agent) + + def hold(wm, agent): + """ """ + from keepaway.lib.action.neck_scan_players import NeckScanPlayers + agent.set_neck_action(NeckScanPlayers()) + HoldBall().execute(agent) + return + + def keeper(wm: "WorldModel", agent: "PlayerAgent"): + # if wm.self()._is_new_episode() == True: + # wm.self().end_episode(wm._reward()) # + # wm.set_new_episode() + # self.world().set_last_action(-1) # + # time_start_episode = self.world().time() # + if DEBUG: + log.sw_log().world().add_text(f"ball pos = {wm.ball().pos()}") + + if wm.get_confidence("ball") < 0.90: + search_ball(wm, agent) + return + # move(wm, agent) + return + + def test_kick(wm, agent, t): + """Test implementation for kick + target + speed + speed threshold + max step + similar to the goalie kick + """ + + from keepaway.lib.action.smart_kick import SmartKick + + action_candidates = BhvPassGen().generator(wm) + + print("action candidates ", action_candidates) + + if len(action_candidates) == 0: + print("Holding the ball") + agent.set_neck_action(NeckScanPlayers()) + return HoldBall().execute(agent) + + best_action: KickAction = max(action_candidates) + target = best_action.target_ball_pos + print("Target : ", target) + log.debug_client().set_target(target) + log.debug_client().add_message( + best_action.type.value + + "to " + + best_action.target_ball_pos.__str__() + + " " + + str(best_action.start_ball_speed) + ) + + # print(" best action speed ", best_action.start_ball_speed) + + # SmartKick( + # target, best_action.start_ball_speed, best_action.start_ball_speed - 1, 3 + # ).execute(agent) + agent.set_neck_action(NeckScanPlayers()) + + return + + def interpret_keeper_action(wm, agent, action): + print("interpret actions , ", action) + + if action == 0: + print("Holding Ball ") + hold(wm, agent) + else: + print("Passing ") + k = wm.teammates_from_ball() + if len(k) > 0: + for tm in k: + if tm.unum() == action: + temp_pos = tm.pos() + # print("passing to player ", tm.unum(), "at pos ", temp_pos) + print( + "i am ", + wm.self().unum(),"at pos ",wm.self().pos(), + "passing to player ", + tm.unum(), + "at pos ", + temp_pos, + ) + # ball_to_player.rotate(-wm.ball().vel().th()) + agent.do_kick_to(tm, 1.5) + # agent.do_kick_2(tm, 1.5) + ## test pass logic + # agent.test_pass(tm, 1.5) + # test_kick(wm,agent,temp_pos) + agent.set_neck_action(NeckScanPlayers()) + # agent.do_kick(temp_pos, 0.8) + # k = wm.messenger_memory().players() + # if len(k) > 0: + # for tm in k: + # print("player num ", tm.unum_) + # if tm.unum_ == action: + # temp_pos = tm.pos_ + # print("i am ",wm.self().unum(), "passing to player ", tm.unum_, "at pos ", temp_pos) + # agent.do_kick_to(tm, 2.0) + # agent.set_neck_action(NeckScanPlayers()) + + else: + print("no teammates") + pass + return + + def keeper_with_ball_2( + wm: "WorldModel", agent: "PlayerAgent", actions, last_action_time + ): + # print("keeper with ball") + # action = actions[wm.self().unum()] + action = actions[wm.self().unum() - 1] + # print( + # "agent ", + # wm.self().unum(), + # " action ", + # actions, + # "my act ", + # actions[wm.self().unum() - 1], + # ) + interpret_keeper_action(wm, agent, action) + + if wm.our_team_name() == "keepers": + barrier.wait() + if wm.get_confidence("ball") < 0.90: + search_ball(wm, agent) + + # teammates_from_ball = wm.teammates_from_ball() + ball_pos = wm.ball().pos() + # GoToPoint(ball_pos, 0.2, 100).execute(agent) + + closest_keeper_from_ball = wm.all_teammates_from_ball() + if len(closest_keeper_from_ball) > 0: + if wm.self().unum() == closest_keeper_from_ball[0].unum(): + GoToPoint(ball_pos, 0.2, 100).execute(agent) + + if wm.self().pos().dist(ball_pos) < 5.0: + obs[wm.self().unum()] = wm._retrieve_observation() + if wm.self().is_kickable(): + wm._available_actions[wm.self().unum()] = 2 + # count_list[wm.self().unum()] = 2 + with count_list.get_lock(): + # print("action received ", list(count_list)) + pass + # print("ball is kickable, ", wm.self().unum()) + keeper_with_ball_2(wm, agent, count_list, last_action_time) + # return + else: + # fastest = wm.get_teammate_nearest_to_ball(1) + # print("i should be intercepting ") + fastest = wm.intercept_table().fastest_teammate() + # print("fastest player intercept table ", f) + # print("fastest player ", fastest) + ## TODO:: re-implement interception. this is not working properly. + ## . check with + if fastest is not None: + # print("Get Open") + keeper_support(wm, fastest, agent) + + ## old implementation + # if fastest is not None: + # # Intercept().execute(agent) + # if fastest.unum == wm.self()._unum: + # print("intercepting") + # # print("fastest player ", fastest._pos) + # Intercept().execute(agent) + # # agent.set_neck_action(NeckTurnToBall()) + # # # print("fastest") + # # # print("fastest player ", fastest._pos) + # keeper_support(wm, wm.self(), fastest, agent) + + ## Update State and Environment + ## calculate reward . + # with last_action_time.get_lock(): + # last_action = last_action_time.value + # with reward.get_lock(): + # reward.value = wm.reward(wm.get_current_cycle(), last_action) + + if wm.our_team_name() == "takers": + # pass + # barrier.wait() + if wm.get_confidence("ball") < 0.90: + search_ball(wm, agent) + + ball_pos = wm.ball().pos() + GoToPoint(ball_pos, 0.2, 100).execute(agent) + + # Maintain possession if you have the ball. + if wm.self().is_kickable() and (len(wm.teammates_from_ball()) == 0): + return HoldBall().execute(agent) + + closest_taker_from_ball = wm.teammates_from_ball() + if wm.self() not in closest_taker_from_ball: + mark_most_open_opponent(wm) + return NeckTurnToBall().execute(agent) + + d = closest_taker_from_ball.dist_to_ball() + if d < 0.3: + NeckTurnToBall().execute(agent) + NeckBodyToBall().execute(agent) + return + return Intercept().execute(agent) diff --git a/base/formation_dt/before_kick_off.conf b/keepaway/base/formation_dt/before_kick_off.conf old mode 100644 new mode 100755 similarity index 100% rename from base/formation_dt/before_kick_off.conf rename to keepaway/base/formation_dt/before_kick_off.conf diff --git a/base/formation_dt/defense_formation.conf b/keepaway/base/formation_dt/defense_formation.conf old mode 100644 new mode 100755 similarity index 100% rename from base/formation_dt/defense_formation.conf rename to keepaway/base/formation_dt/defense_formation.conf diff --git a/base/formation_dt/goalie_kick_opp_formation.conf b/keepaway/base/formation_dt/goalie_kick_opp_formation.conf old mode 100644 new mode 100755 similarity index 100% rename from base/formation_dt/goalie_kick_opp_formation.conf rename to keepaway/base/formation_dt/goalie_kick_opp_formation.conf diff --git a/base/formation_dt/goalie_kick_our_formation.conf b/keepaway/base/formation_dt/goalie_kick_our_formation.conf old mode 100644 new mode 100755 similarity index 100% rename from base/formation_dt/goalie_kick_our_formation.conf rename to keepaway/base/formation_dt/goalie_kick_our_formation.conf diff --git a/base/formation_dt/kickin_our_formation.conf b/keepaway/base/formation_dt/kickin_our_formation.conf old mode 100644 new mode 100755 similarity index 100% rename from base/formation_dt/kickin_our_formation.conf rename to keepaway/base/formation_dt/kickin_our_formation.conf diff --git a/base/formation_dt/offense_formation.conf b/keepaway/base/formation_dt/offense_formation.conf old mode 100644 new mode 100755 similarity index 100% rename from base/formation_dt/offense_formation.conf rename to keepaway/base/formation_dt/offense_formation.conf diff --git a/base/formation_dt/setplay_opp_formation.conf b/keepaway/base/formation_dt/setplay_opp_formation.conf old mode 100644 new mode 100755 similarity index 100% rename from base/formation_dt/setplay_opp_formation.conf rename to keepaway/base/formation_dt/setplay_opp_formation.conf diff --git a/base/formation_dt/setplay_our_formation.conf b/keepaway/base/formation_dt/setplay_our_formation.conf old mode 100644 new mode 100755 similarity index 100% rename from base/formation_dt/setplay_our_formation.conf rename to keepaway/base/formation_dt/setplay_our_formation.conf diff --git a/base/generator_action.py b/keepaway/base/generator_action.py old mode 100644 new mode 100755 similarity index 55% rename from base/generator_action.py rename to keepaway/base/generator_action.py index 964aed07..df723f93 --- a/base/generator_action.py +++ b/keepaway/base/generator_action.py @@ -2,16 +2,17 @@ from pyrusgeom.geom_2d import * from typing import TYPE_CHECKING + if TYPE_CHECKING: - from lib.player.world_model import WorldModel - from lib.player.object_player import PlayerObject + from keepaway.lib.player.world_model import WorldModel + from keepaway.lib.player.object_player import PlayerObject class KickActionType(Enum): - No = '0' - Pass = 'Pass' - Dribble = 'Dribble' - Clear = 'Clear' + No = "0" + Pass = "Pass" + Dribble = "Dribble" + Clear = "Clear" class KickAction: @@ -26,14 +27,27 @@ def __init__(self): self.index = 0 self.min_opp_dist = 0.0 - def calculate_min_opp_dist(self, wm: 'WorldModel' = None): + def calculate_min_opp_dist(self, wm: "WorldModel" = None): if wm is None: return 0.0 - return min([opp.pos().dist(self.target_ball_pos) for opp in wm.opponents() if opp is not None and opp.unum() > 0]) - - def evaluate(self, wm: 'WorldModel' = None): + for opp in wm.opponents(): + pass + # print(opp) + # if opp is not None and opp.unum() > 0 + return min( + [ + opp.pos().dist(self.target_ball_pos) + for opp in wm.opponents() + if opp is not None and opp.unum() > 0 + ] + ) + + def evaluate(self, wm: "WorldModel" = None): + ## changed 52 to 10 self.min_opp_dist = self.calculate_min_opp_dist(wm) - self.eval = self.target_ball_pos.x() + max(0.0, 40.0 - self.target_ball_pos.dist(Vector2D(52, 0))) + self.eval = self.target_ball_pos.x() + max( + 0.0, 40.0 - self.target_ball_pos.dist(Vector2D(10, 0)) + ) if self.min_opp_dist < 5.0: self.eval -= list([30, 20, 10, 5, 2])[int(self.min_opp_dist)] @@ -41,12 +55,14 @@ def __gt__(self, other): return self.eval > other.eval def __repr__(self): - return '{} Action {} to {} in ({}, {}) eval:{}'.format(self.type.value, - self.start_unum, - self.target_unum, - round(self.target_ball_pos.x(), 2), - round(self.target_ball_pos.y(), 2), - self.eval) + return "{} Action {} to {} in ({}, {}) eval:{}".format( + self.type.value, + self.start_unum, + self.target_unum, + round(self.target_ball_pos.x(), 2), + round(self.target_ball_pos.y(), 2), + self.eval, + ) def __str__(self): return self.__repr__() @@ -59,27 +75,37 @@ def __init__(self, angle: AngleDeg = None, vel: Vector2D = None): self._ball_vel: Vector2D = vel.copy() self._ball_speed: float = vel.r() self._ball_move_angle: AngleDeg = vel.th() - self._score: float = 0. + self._score: float = 0.0 return self._tackle_angle: AngleDeg = AngleDeg(0) - self._ball_vel: Vector2D = Vector2D(0,0) - self._ball_speed: float = 0. + self._ball_vel: Vector2D = Vector2D(0, 0) + self._ball_speed: float = 0.0 self._ball_move_angle: AngleDeg = AngleDeg(0) - self._score: float = -float('inf') + self._score: float = -float("inf") def clear(self): self._tackle_angle: AngleDeg = AngleDeg(0) - self._ball_vel: Vector2D = Vector2D(0,0) - self._ball_speed: float = 0. + self._ball_vel: Vector2D = Vector2D(0, 0) + self._ball_speed: float = 0.0 self._ball_move_angle: AngleDeg = AngleDeg(0) - self._score: float = -float('inf') + self._score: float = -float("inf") class ShootAction: - def __init__(self, index, target_point, first_ball_speed, ball_move_angle, ball_move_dist, ball_reach_step): + def __init__( + self, + index, + target_point, + first_ball_speed, + ball_move_angle, + ball_move_dist, + ball_reach_step, + ): self.index = index self.target_point: Vector2D = target_point - self.first_ball_vel: Vector2D = Vector2D.polar2vector(first_ball_speed, ball_move_angle) + self.first_ball_vel: Vector2D = Vector2D.polar2vector( + first_ball_speed, ball_move_angle + ) self.first_ball_speed = first_ball_speed self.ball_move_angle: AngleDeg = ball_move_angle self.ball_move_dist = ball_move_dist @@ -90,7 +116,9 @@ def __init__(self, index, target_point, first_ball_speed, ball_move_angle, ball_ self.score = 0 def __repr__(self): - return '{} target {} first_vel {}'.format(self.index, self.target_point, self.first_ball_vel) + return "{} target {} first_vel {}".format( + self.index, self.target_point, self.first_ball_vel + ) def __str__(self): return self.__repr__() @@ -102,9 +130,9 @@ def __init__(self): self.index = 0 self.debug_list = [] - def can_opponent_cut_ball(self, wm: 'WorldModel', ball_pos, cycle): + def can_opponent_cut_ball(self, wm: "WorldModel", ball_pos, cycle): for unum in range(1, 12): - opp: 'PlayerObject' = wm.their_player(unum) + opp: "PlayerObject" = wm.their_player(unum) if opp.unum() == 0: continue opp_cycle = opp.pos().dist(ball_pos) - opp.player_type().kickable_area() diff --git a/base/generator_clear.py b/keepaway/base/generator_clear.py old mode 100644 new mode 100755 similarity index 92% rename from base/generator_clear.py rename to keepaway/base/generator_clear.py index 06c85e8d..557542e7 --- a/base/generator_clear.py +++ b/keepaway/base/generator_clear.py @@ -1,11 +1,11 @@ from pyrusgeom.geom_2d import * -from lib.debug.color import Color -from lib.debug.debug import log -from lib.rcsc.server_param import ServerParam as SP -from base.generator_action import KickAction, KickActionType, BhvKickGen +from keepaway.lib.debug.color import Color +from keepaway.lib.debug.debug import log +from keepaway.lib.rcsc.server_param import ServerParam as SP +from keepaway.base.generator_action import KickAction, KickActionType, BhvKickGen from typing import TYPE_CHECKING if TYPE_CHECKING: - from lib.player.world_model import WorldModel + from keepaway.lib.player.world_model import WorldModel debug_clear_ball = True diff --git a/base/generator_dribble.py b/keepaway/base/generator_dribble.py old mode 100644 new mode 100755 similarity index 97% rename from base/generator_dribble.py rename to keepaway/base/generator_dribble.py index 71527811..89df033c --- a/base/generator_dribble.py +++ b/keepaway/base/generator_dribble.py @@ -1,16 +1,16 @@ from pyrusgeom.geom_2d import * import pyrusgeom.soccer_math as smath -from lib.debug.debug import log -from lib.rcsc.server_param import ServerParam as SP -from base.tools import Tools +from keepaway.lib.debug.debug import log +from keepaway.lib.rcsc.server_param import ServerParam as SP +from keepaway.base.tools import Tools import time -from base.generator_action import BhvKickGen, KickActionType, KickAction +from keepaway.base.generator_action import BhvKickGen, KickActionType, KickAction from typing import TYPE_CHECKING if TYPE_CHECKING: - from lib.player.world_model import WorldModel - from lib.player.object_player import PlayerObject + from keepaway.lib.player.world_model import WorldModel + from keepaway.lib.player.object_player import PlayerObject debug_dribble = False max_dribble_time = 0 diff --git a/keepaway/base/generator_pass.py b/keepaway/base/generator_pass.py new file mode 100755 index 00000000..125393c4 --- /dev/null +++ b/keepaway/base/generator_pass.py @@ -0,0 +1,858 @@ +from pyrusgeom.geom_2d import * +import pyrusgeom.soccer_math as smath +from keepaway.lib.debug.color import Color +from keepaway.lib.debug.debug import log +from keepaway.lib.rcsc.server_param import ServerParam as SP +from keepaway.base.tools import Tools +import time +from keepaway.base.generator_action import KickAction, KickActionType, BhvKickGen + +from typing import TYPE_CHECKING + +from keepaway.lib.rcsc.types import GameModeType + +if TYPE_CHECKING: + from keepaway.lib.player.world_model import WorldModel + from keepaway.lib.player.object_player import PlayerObject + +debug_pass = True +max_pass_time = 0 + + +class BhvPassGen(BhvKickGen): + def __init__(self): + super().__init__() + self.best_pass: KickAction = None + self.receivers: list["PlayerObject"] = [] + + def generator(self, wm: "WorldModel"): + + print("Generating ..") + global max_pass_time + start_time = time.time() + self.update_receivers(wm) + + for r in self.receivers: + log.sw_log().pass_().add_text( + f"=============== Lead Pass to {r.unum()} pos: {r.pos()}" + ) + # if self.best_pass is not None \ + # and r.pos().x() < self.best_pass.target_ball_pos.x() - 5: + # break + self.generate_direct_pass(wm, r) + # self.generate_lead_pass(wm, r) + # self.generate_through_pass(wm, r) + + if debug_pass: + for candid in self.debug_list: + if candid[2]: + log.sw_log().pass_().add_message( + candid[1].x(), candid[1].y(), "{}".format(candid[0]) + ) + log.sw_log().pass_().add_circle( + circle=Circle2D(candid[1], 0.2), color=Color(string="green") + ) + else: + log.sw_log().pass_().add_message( + candid[1].x(), candid[1].y(), "{}".format(candid[0]) + ) + log.sw_log().pass_().add_circle( + circle=Circle2D(candid[1], 0.2), color=Color(string="red") + ) + end_time = time.time() + if end_time - start_time > max_pass_time: + max_pass_time = end_time - start_time + log.sw_log().pass_().add_text( + "time:{} max is {}".format(end_time - start_time, max_pass_time) + ) + return self.candidates + + def update_receivers(self, wm: "WorldModel"): + sp = SP.i() + log.sw_log().pass_().add_text("===update receivers".format()) + for tm in wm.teammates(): + if tm is None: + log.sw_log().pass_().add_text("-----<<< TM is none") + continue + if tm.unum() <= 0: + log.sw_log().pass_().add_text(f"-----<<< TM unum is {tm.unum()}") + continue + if tm.unum() == wm.self().unum(): + log.sw_log().pass_().add_text(f"-----<<< TM unum is {tm.unum()} (self)") + continue + if tm.pos_count() > 10: + log.sw_log().pass_().add_text( + f"-----<<< TM unum pos count {tm.pos_count()}" + ) + continue + if tm.is_tackling(): + log.sw_log().pass_().add_text(f"-----<<< TM is tackling") + continue + if tm.pos().x() > wm.offside_line_x(): + log.sw_log().pass_().add_text( + f"-----<<< TM is in offside {tm.pos().x()} > {wm.offside_line_x()}" + ) + continue + if tm.goalie() and tm.pos().x() < sp.our_penalty_area_line_x() + 15: + log.sw_log().pass_().add_text( + f"-----<<< TM is goalie and danger {tm.pos().x()} < {sp.our_penalty_area_line_x() + 15}" + ) + continue + log.sw_log().pass_().add_text(f"--->>>>> TM {tm.unum()} is added") + self.receivers.append(tm) + self.receivers = sorted(self.receivers, key=lambda p: p.pos().x(), reverse=True) + + def generate_direct_pass(self, wm: "WorldModel", receiver: "PlayerObject"): + sp = SP.i() + min_receive_step = 3 + max_direct_pass_dist = 0.8 * smath.inertia_final_distance( + sp.ball_speed_max(), sp.ball_decay() + ) + max_receive_ball_speed = sp.ball_speed_max() * pow( + sp.ball_decay(), min_receive_step + ) + min_direct_pass_dist = receiver.player_type().kickable_area() * 2.2 + if ( + receiver.pos().x() > sp.pitch_half_length() - 1.5 + or receiver.pos().x() < -sp.pitch_half_length() + 5.0 + or receiver.pos().abs_y() > sp.pitch_half_width() - 1.5 + ): + if debug_pass: + log.sw_log().pass_().add_text( + "#DPass to {} {}, out of field".format( + receiver.unum(), receiver.pos() + ) + ) + return + # TODO sp.ourTeamGoalPos() + print("ball pos in gen pass: ", wm.ball().pos()) + if receiver.pos().x() < wm.ball().pos().x() + 1.0 and receiver.pos().dist2( + Vector2D(-52.5, 0) + ) < pow(18.0, 2): + if debug_pass: + log.sw_log().pass_().add_text( + "#DPass to {} {}, danger near goal".format( + receiver.unum(), receiver.pos() + ) + ) + return + + ptype = receiver.player_type() + max_ball_speed = wm.self().kick_rate() * sp.max_power() + if wm.game_mode().type() == GameModeType.PlayOn: + max_ball_speed = sp.ball_speed_max() + + # TODO SP.defaultRealSpeedMax() + min_ball_speed = 1.0 + + receive_point = ptype.inertiaFinalPoint(receiver.pos(), receiver.vel()) + ball_move_dist = wm.ball().pos().dist(receive_point) + + if ( + ball_move_dist < min_direct_pass_dist + or max_direct_pass_dist < ball_move_dist + ): + if debug_pass: + log.sw_log().pass_().add_text( + "#DPass to {} {}, far or close".format( + receiver.unum(), receiver.pos() + ) + ) + return + + if ( + wm.game_mode().type().is_goal_kick() + and receive_point.x() < sp.our_penalty_area_line_x() + 1.0 + and receive_point.abs_y() < sp.penalty_area_half_width() + 1.0 + ): + if debug_pass: + log.sw_log().pass_().add_text( + "#DPass to {} {}, in penalty area in goal kick mode".format( + receiver.unum(), receiver.pos() + ) + ) + return + + max_receive_ball_speed = min( + max_receive_ball_speed, + ptype.kickable_area() + + (sp.max_dash_power() * ptype.dash_power_rate() * ptype.effort_max()) + * 1.8, + ) + min_receive_ball_speed = ptype.real_speed_max() + + ball_move_angle = (receive_point - wm.ball().pos()).th() + + min_ball_step = sp.ball_move_step(sp.ball_speed_max(), ball_move_dist) + # TODO Penalty step + start_step = max(max(min_receive_step, min_ball_step), 0) + max_step = start_step + 2 + log.sw_log().pass_().add_text( + "#DPass to {} {}".format(receiver.unum(), receiver.pos()) + ) + self.create_pass( + wm, + receiver, + receive_point, + start_step, + max_step, + min_ball_speed, + max_ball_speed, + min_receive_ball_speed, + max_receive_ball_speed, + ball_move_dist, + ball_move_angle, + "D", + ) + + def generate_lead_pass(self, wm: "WorldModel", receiver): + sp = SP.i() + our_goal_dist_thr2 = pow(16.0, 2) + min_receive_step = 4 + max_receive_step = 20 + min_leading_pass_dist = 3.0 + max_leading_pass_dist = 0.8 * smath.inertia_final_distance( + sp.ball_speed_max(), sp.ball_decay() + ) + max_receive_ball_speed = sp.ball_speed_max() * pow( + sp.ball_decay(), min_receive_step + ) + + max_player_distance = 35 + if receiver.pos().dist(wm.ball().pos()) > max_player_distance: + if debug_pass: + log.sw_log().pass_().add_text( + "#####LPass to {} {}, player is far".format( + receiver.unum(), receiver.pos() + ) + ) + return + + abgle_divs = 8 + angle_step = 360.0 / abgle_divs + dist_divs = 4 + dist_step = 1.1 + + ptype = receiver.player_type() + max_ball_speed = wm.self().kick_rate() * sp.max_power() + if wm.game_mode().type() == GameModeType.PlayOn: + max_ball_speed = sp.ball_speed_max() + min_ball_speed = sp.default_player_speed_max() + + max_receive_ball_speed = min( + max_receive_ball_speed, + ptype.kickable_area() + + (sp.max_dash_power() * ptype.dash_power_rate() * ptype.effort_max()) + * 1.5, + ) + min_receive_ball_speed = 0.001 + + our_goal = Vector2D(-52.5, 0) + + angle_from_ball = (receiver.pos() - wm.ball().pos()).th() + for d in range(1, dist_divs + 1): + player_move_dist = dist_step * d + a_step = 2 if player_move_dist * 2.0 * math.pi / abgle_divs < 0.6 else 1 + for a in range(abgle_divs + 1): + angle = angle_from_ball + angle_step * a + receive_point = receiver.inertia_point(1) + Vector2D.from_polar( + player_move_dist, angle + ) + + move_dist_penalty_step = 0 + ball_move_line = Line2D(wm.ball().pos(), receive_point) + player_line_dist = ball_move_line.dist(receiver.pos()) + move_dist_penalty_step = int(player_line_dist * 0.3) + if ( + receive_point.x() > sp.pitch_half_length() - 3.0 + or receive_point.x() < -sp.pitch_half_length() + 5.0 + or receive_point.abs_y() > sp.pitch_half_width() - 3.0 + ): + if debug_pass: + log.sw_log().pass_().add_text( + "#####LPass to {} {}, out of field".format( + receiver.unum(), receive_point + ) + ) + continue + + if ( + receive_point.x() < wm.ball().pos().x() + and receive_point.dist2(our_goal) < our_goal_dist_thr2 + ): + if debug_pass: + log.sw_log().pass_().add_text( + "#####LPass to {} {}, pass is danger".format( + receiver.unum(), receive_point + ) + ) + continue + + if ( + wm.game_mode().type() + in [GameModeType.GoalKick_Right, GameModeType.GoalKick_Left] + and receive_point.x() < sp.our_penalty_area_line_x() + 1.0 + and receive_point.abs_y() < sp.penalty_area_half_width() + 1.0 + ): + if debug_pass: + log.sw_log().pass_().add_text( + "#####LPass to {} {}, in penalty area".format( + receiver.unum(), receive_point + ) + ) + return + + ball_move_dist = wm.ball().pos().dist(receive_point) + + if ( + ball_move_dist < min_leading_pass_dist + or max_leading_pass_dist < ball_move_dist + ): + if debug_pass: + log.sw_log().pass_().add_text( + "#####LPass to {} {}, so far or so close".format( + receiver.unum(), receive_point + ) + ) + continue + + nearest_receiver = Tools.get_nearest_teammate( + wm, receive_point, self.receivers + ) + if nearest_receiver.unum() != receiver.unum(): + if debug_pass: + log.sw_log().pass_().add_text( + "#####LPass to {} {}, {} is closer than receiver ".format( + receiver.unum(), receive_point, nearest_receiver.unum() + ) + ) + continue + + receiver_step = ( + self.predict_receiver_reach_step(receiver, receive_point, True, "L") + + move_dist_penalty_step + ) + ball_move_angle = (receive_point - wm.ball().pos()).th() + + min_ball_step = sp.ball_move_step(sp.ball_speed_max(), ball_move_dist) + + start_step = max(max(min_receive_step, min_ball_step), receiver_step) + # ifdef CREATE_SEVERAL_CANDIDATES_ON_SAME_POINT + # max_step = std::max(max_receive_step, start_step + 3); + # else + if debug_pass: + log.sw_log().pass_().add_text( + "#####LPass to {} {}".format(receiver.unum(), receive_point) + ) + max_step = start_step + 3 + self.create_pass( + wm, + receiver, + receive_point, + start_step, + max_step, + min_ball_speed, + max_ball_speed, + min_receive_ball_speed, + max_receive_ball_speed, + ball_move_dist, + ball_move_angle, + "L", + ) + + def generate_through_pass(self, wm: "WorldModel", receiver: "PlayerObject"): + sp = SP.i() + teammate_min_x = 0.0 + target_min_x = 10.0 + min_receive_step = 4 + min_pass_dist = 3.0 + max_pass_dist = 0.8 * smath.inertia_final_distance( + sp.ball_speed_max(), sp.ball_decay() + ) + max_receive_ball_speed = sp.ball_speed_max() * pow( + sp.ball_decay(), min_receive_step + ) + + max_player_distance = 35 + if receiver.pos().dist(wm.ball().pos()) > max_player_distance: + if debug_pass: + log.sw_log().pass_().add_text( + "#####TPass to {} {}, player is far".format( + receiver.unum(), receiver.pos() + ) + ) + return + if receiver.pos().x() < teammate_min_x: + if debug_pass: + log.sw_log().pass_().add_text( + "#####TPass to {} {}, player is far".format( + receiver.unum(), receiver.pos() + ) + ) + return + if receiver.pos().x() < wm.offside_line_x() - 5.0: + if debug_pass: + log.sw_log().pass_().add_text( + "#####TPass to {} {}, player is not close to offside line".format( + receiver.unum(), receiver.pos() + ) + ) + return + if receiver.pos().x() > wm.offside_line_x() - 0.5: + if debug_pass: + log.sw_log().pass_().add_text( + "#####TPass to {} {}, player is in offside".format( + receiver.unum(), receiver.pos() + ) + ) + return + if wm.ball().pos().x() < -10.0 or wm.ball().pos().x() > 30.0: + if debug_pass: + log.sw_log().pass_().add_text( + "#####TPass to {} {}, ball x is low or high".format( + receiver.unum(), receiver.pos() + ) + ) + return + + min_angle = -30 + max_angle = 30 + angle_step = 10 + dist_divs = 25 + dist_step = 1.0 + + ptype = receiver.player_type() + max_ball_speed = wm.self().kick_rate() * sp.max_power() + if wm.game_mode().type() == GameModeType.PlayOn: + max_ball_speed = sp.ball_speed_max() + min_ball_speed = sp.default_player_speed_max() + + max_receive_ball_speed = min( + max_receive_ball_speed, + ptype.kickable_area() + + (sp.max_dash_power() * ptype.dash_power_rate() * ptype.effort_max()) + * 1.5, + ) + min_receive_ball_speed = 0.001 + + our_goal = Vector2D(-52.5, 0) + + angle_from_ball = (receiver.pos() - wm.ball().pos()).th() + for d in range(5, dist_divs + 1): + player_move_dist = dist_step * d + for a in range(min_angle, max_angle + 1, angle_step): + receive_point = receiver.inertia_point(1) + Vector2D.from_polar( + player_move_dist, a + ) + + move_dist_penalty_step = 0 + ball_move_line = Line2D(wm.ball().pos(), receive_point) + player_line_dist = ball_move_line.dist(receiver.pos()) + move_dist_penalty_step = int(player_line_dist * 0.3) + if ( + receive_point.x() > sp.pitch_half_length() - 3.0 + or receive_point.x() < -sp.pitch_half_length() + 5.0 + or receive_point.abs_y() > sp.pitch_half_width() - 3.0 + ): + if debug_pass: + log.sw_log().pass_().add_text( + "#####TPass to {} {}, out of field".format( + receiver.unum(), receive_point + ) + ) + continue + + if receive_point.x() < target_min_x: + if debug_pass: + log.sw_log().pass_().add_text( + "#####TPass to {} {}, pass is danger".format( + receiver.unum(), receive_point + ) + ) + continue + + ball_move_dist = wm.ball().pos().dist(receive_point) + + if ball_move_dist < min_pass_dist or max_pass_dist < ball_move_dist: + if debug_pass: + log.sw_log().pass_().add_text( + "#####TPass to {} {}, so far or so close".format( + receiver.unum(), receive_point + ) + ) + continue + + nearest_receiver = Tools.get_nearest_teammate( + wm, receive_point, self.receivers + ) + if nearest_receiver.unum() != receiver.unum(): + if debug_pass: + log.sw_log().pass_().add_text( + "#####TPass to {} {}, {} is closer than receiver ".format( + receiver.unum(), receive_point, nearest_receiver.unum() + ) + ) + continue + + receiver_step = ( + self.predict_receiver_reach_step(receiver, receive_point, True, "T") + + move_dist_penalty_step + ) + ball_move_angle = (receive_point - wm.ball().pos()).th() + + min_ball_step = sp.ball_move_step(sp.ball_speed_max(), ball_move_dist) + + start_step = max(max(min_receive_step, min_ball_step), receiver_step) + # ifdef CREATE_SEVERAL_CANDIDATES_ON_SAME_POINT + # max_step = std::max(max_receive_step, start_step + 3); + # else + if debug_pass: + log.sw_log().pass_().add_text( + "#####TPass to {} {}".format(receiver.unum(), receive_point) + ) + max_step = start_step + 3 + self.create_pass( + wm, + receiver, + receive_point, + start_step, + max_step, + min_ball_speed, + max_ball_speed, + min_receive_ball_speed, + max_receive_ball_speed, + ball_move_dist, + ball_move_angle, + "T", + ) + + def predict_receiver_reach_step( + self, receiver, pos: Vector2D, use_penalty, pass_type + ): + ptype = receiver.player_type() + + target_dist = receiver.inertia_point(1).dist(pos) + n_turn = ( + 1 + if receiver.body_count() > 0 + else Tools.predict_player_turn_cycle( + ptype, + receiver.body(), + receiver.vel().r(), + target_dist, + (pos - receiver.inertia_point(1)).th(), + ptype.kickable_area(), + False, + ) + ) + dash_dist = target_dist + + # if use_penalty: + # dash_dist += receiver.penalty_distance_; + + if pass_type == "L": + dash_dist *= 1.05 + + dash_angle = (pos - receiver.pos()).th() + + if ( + dash_angle.abs() > 90.0 + or receiver.body_count() > 1 + or (dash_angle - receiver.body()).abs() > 30.0 + ): + n_turn += 1 + + n_dash = ptype.cycles_to_reach_distance(dash_dist) + + n_step = n_turn + n_dash if n_turn == 0 else n_turn + n_dash + 1 + return n_step + + def create_pass( + self, + wm: "WorldModel", + receiver, + receive_point: Vector2D, + min_step, + max_step, + min_first_ball_speed, + max_first_ball_speed, + min_receive_ball_speed, + max_receive_ball_speed, + ball_move_dist, + ball_move_angle: AngleDeg, + description, + ): + sp = SP.i() + + for step in range(min_step, max_step + 1): + self.index += 1 + first_ball_speed = smath.calc_first_term_geom_series( + ball_move_dist, sp.ball_decay(), step + ) + + if first_ball_speed < min_first_ball_speed: + if debug_pass: + log.sw_log().pass_().add_text( + "##Pass {},to {} {}, step:{}, ball_speed:{}, first ball speed is low".format( + self.index, + receiver.unum(), + receiver.pos(), + step, + first_ball_speed, + ) + ) + self.debug_list.append((self.index, receive_point, False)) + break + + if max_first_ball_speed < first_ball_speed: + if debug_pass: + log.sw_log().pass_().add_text( + "##Pass {},to {} {}, step:{}, ball_speed:{}, first ball speed is high".format( + self.index, + receiver.unum(), + receiver.pos(), + step, + first_ball_speed, + ) + ) + self.debug_list.append((self.index, receive_point, False)) + continue + + receive_ball_speed = first_ball_speed * pow(sp.ball_decay(), step) + + if receive_ball_speed < min_receive_ball_speed: + if debug_pass: + log.sw_log().pass_().add_text( + "##Pass {},to {} {}, step:{}, ball_speed:{}, rball_speed:{}, receive ball speed is low".format( + self.index, + receiver.unum(), + receiver.pos(), + step, + first_ball_speed, + receive_ball_speed, + ) + ) + self.debug_list.append((self.index, receive_point, False)) + break + + if max_receive_ball_speed < receive_ball_speed: + if debug_pass: + log.sw_log().pass_().add_text( + "##Pass {},to {} {}, step:{}, ball_speed:{}, rball_speed:{}, receive ball speed is high".format( + self.index, + receiver.unum(), + receiver.pos(), + step, + first_ball_speed, + receive_ball_speed, + ) + ) + self.debug_list.append((self.index, receive_point, False)) + continue + + + + kick_count = Tools.predict_kick_count( + wm, wm.self().unum(), first_ball_speed, ball_move_angle + ) + + o_step, o_unum, o_intercepted_pos = self.predict_opponents_reach_step( + wm, + wm.ball().pos(), + first_ball_speed, + ball_move_angle, + receive_point, + step + (kick_count - 1) + 5, + description, + ) + + failed = False + if description == "T": + if o_step <= step: + failed = True + else: + if o_step <= step + (kick_count - 1): + failed = True + if failed: + if debug_pass: + log.sw_log().pass_().add_text( + "------<<<<>>>> OK Pass {} to {} {}, opp {} step {} max_step {}".format( + self.index, + receiver.unum(), + receive_point, + o_unum, + o_step, + max_step, + ) + ) + self.debug_list.append((self.index, receive_point, True)) + candidate = KickAction() + candidate.type = KickActionType.Pass + candidate.start_ball_pos = wm.ball().pos() + candidate.target_ball_pos = receive_point + candidate.target_unum = receiver.unum() + candidate.start_ball_speed = first_ball_speed + candidate.evaluate(wm) + + + self.candidates.append(candidate) + + if self.best_pass is None or candidate.eval > self.best_pass.eval: + self.best_pass = candidate + + find_another_pass = False + if not find_another_pass: + break + + if o_step <= step + 3: + break + + if min_step + 3 <= step: + break + + def predict_opponents_reach_step( + self, + wm: "WorldModel", + first_ball_pos: Vector2D, + first_ball_speed, + ball_move_angle: AngleDeg, + receive_point: Vector2D, + max_cycle, + description, + ): + first_ball_vel = Vector2D.polar2vector(first_ball_speed, ball_move_angle) + min_step = 1000 + min_opp = 0 + intercepted_pos = None + for opp in wm.opponents(): + if opp is None or opp.unum() == 0: + continue + step, intercepted_pos = self.predict_opponent_reach_step( + wm, + opp, + first_ball_pos, + first_ball_vel, + ball_move_angle, + receive_point, + max_cycle, + description, + ) + if step < min_step: + min_step = step + min_opp = opp.unum() + return min_step, min_opp, intercepted_pos + + def predict_opponent_reach_step( + self, + wm: "WorldModel", + opp: "PlayerObject", + first_ball_pos: Vector2D, + first_ball_vel: Vector2D, + ball_move_angle: AngleDeg, + receive_point: Vector2D, + max_cycle, + description, + ): + sp = SP.i() + + penalty_area = Rect2D( + Vector2D(sp.their_penalty_area_line_x(), -sp.penalty_area_half_width()), + Size2D(sp.penalty_area_length(), sp.penalty_area_width()), + ) + CONTROL_AREA_BUF = 0.15 + + opponent = opp + ptype = opponent.player_type() + min_cycle = Tools.estimate_min_reach_cycle( + opponent.pos(), ptype.real_speed_max(), first_ball_pos, ball_move_angle + ) + + if min_cycle < 0: + return 1000, None + + for cycle in range(max(1, min_cycle), max_cycle + 1): + ball_pos = smath.inertia_n_step_point( + first_ball_pos, first_ball_vel, cycle, sp.ball_decay() + ) + control_area = ( + sp.catchable_area() + if opponent.is_goalie() and penalty_area.contains(ball_pos) + else ptype.kickable_area() + ) + + inertia_pos = ptype.inertia_point(opponent.pos(), opponent.vel(), cycle) + target_dist = inertia_pos.dist(ball_pos) + + dash_dist = target_dist + if ( + description == "T" + and first_ball_vel.x() > 2.0 + and ( + receive_point.x() > wm.offside_line_x() or receive_point.x() > 30.0 + ) + ): + pass + else: + dash_dist -= Tools.estimate_virtual_dash_distance(opp) + if dash_dist - control_area - CONTROL_AREA_BUF < 0.001: + return cycle, ball_pos + + if ( + description == "T" + and first_ball_vel.x() > 2.0 + and ( + receive_point.x() > wm.offside_line_x() or receive_point.x() > 30.0 + ) + ): + dash_dist -= control_area + else: + if receive_point.x() < 25.0: + dash_dist -= control_area + 0.5 + else: + dash_dist -= control_area + 0.2 + + if dash_dist > ptype.real_speed_max() * ( + cycle + min(opponent.pos_count(), 5) + ): + continue + + n_dash = ptype.cycles_to_reach_distance(dash_dist) + if n_dash > cycle + opponent.pos_count(): + continue + + n_turn = 0 + if opponent.body_count() > 1: + n_turn = Tools.predict_player_turn_cycle( + ptype, + opponent.body(), + opponent.vel().r(), + target_dist, + (ball_pos - inertia_pos).th(), + control_area, + True, + ) + + n_step = n_turn + n_dash if n_turn == 0 else n_turn + n_dash + 1 + + bonus_step = 0 + if opponent.is_tackling(): + bonus_step = -5 + if n_step - bonus_step <= cycle: + return cycle, ball_pos + return 1000, None diff --git a/base/generator_shoot.py b/keepaway/base/generator_shoot.py old mode 100644 new mode 100755 similarity index 96% rename from base/generator_shoot.py rename to keepaway/base/generator_shoot.py index 04247981..ad68bc28 --- a/base/generator_shoot.py +++ b/keepaway/base/generator_shoot.py @@ -1,18 +1,18 @@ from pyrusgeom.geom_2d import * import pyrusgeom.soccer_math as smath -from lib.debug.debug import log -from lib.rcsc.server_param import ServerParam as SP -from base.tools import Tools +from keepaway.lib.debug.debug import log +from keepaway.lib.rcsc.server_param import ServerParam as SP +from keepaway.base.tools import Tools import time -from base.generator_action import ShootAction, BhvKickGen -from lib.action.kick_table import calc_max_velocity -from lib.rcsc.types import GameModeType +from keepaway.base.generator_action import ShootAction, BhvKickGen +from keepaway.lib.action.kick_table import calc_max_velocity +from keepaway.lib.rcsc.types import GameModeType from typing import TYPE_CHECKING if TYPE_CHECKING: - from lib.player.world_model import WorldModel - from lib.player.object_player import PlayerObject + from keepaway.lib.player.world_model import WorldModel + from keepaway.lib.player.object_player import PlayerObject debug_shoot = False diff --git a/base/goalie_decision.py b/keepaway/base/goalie_decision.py old mode 100644 new mode 100755 similarity index 82% rename from base/goalie_decision.py rename to keepaway/base/goalie_decision.py index 16c234a9..28cb57cc --- a/base/goalie_decision.py +++ b/keepaway/base/goalie_decision.py @@ -7,24 +7,24 @@ from pyrusgeom.soccer_math import bound from pyrusgeom.vector_2d import Vector2D -from base.basic_tackle import BasicTackle -from base.generator_action import KickAction -from base.generator_pass import BhvPassGen -from base.set_play.bhv_goalie_set_play import Bhv_GoalieSetPlay -from lib.action.go_to_point import GoToPoint -from lib.action.hold_ball import HoldBall -from lib.action.intercept import Intercept -from lib.action.neck_scan_players import NeckScanPlayers -from lib.action.neck_turn_to_ball import NeckTurnToBall -from lib.action.smart_kick import SmartKick -from lib.debug.color import Color -from lib.debug.debug import log -from lib.debug.level import Level -from lib.rcsc.server_param import ServerParam -from lib.rcsc.types import GameModeType +from keepaway.base.basic_tackle import BasicTackle +from keepaway.base.generator_action import KickAction +from keepaway.base.generator_pass import BhvPassGen +from keepaway.keepaway.base.set_play.bhv_goalie_set_play import Bhv_GoalieSetPlay +from keepaway.lib.action.go_to_point import GoToPoint +from keepaway.lib.action.hold_ball import HoldBall +from keepaway.lib.action.intercept import Intercept +from keepaway.lib.action.neck_scan_players import NeckScanPlayers +from keepaway.lib.action.neck_turn_to_ball import NeckTurnToBall +from keepaway.lib.action.smart_kick import SmartKick +from keepaway.lib.debug.color import Color +from keepaway.lib.debug.debug import log +from keepaway.lib.debug.level import Level +from keepaway.lib.rcsc.server_param import ServerParam +from keepaway.lib.rcsc.types import GameModeType if TYPE_CHECKING: - from lib.player.player_agent import PlayerAgent + from keepaway.lib.player.player_agent import PlayerAgent DEBUG = True diff --git a/keepaway/base/main_coach.py b/keepaway/base/main_coach.py new file mode 100755 index 00000000..24a76954 --- /dev/null +++ b/keepaway/base/main_coach.py @@ -0,0 +1,21 @@ +#!/usr/bin/python3 +from keepaway.lib.player.basic_client import BasicClient +from keepaway.base.sample_coach import SampleCoach + +from keepaway.config import team_config +import sys + + +def main(): + agent = SampleCoach() + print(agent.handle_start()) + if not agent.handle_start(): + # print("Failed to start") + agent.handle_exit() + return + print("Starting Coach") + agent.run() + + +if __name__ == "__main__": + main() diff --git a/base/main_player.py b/keepaway/base/main_player.py similarity index 52% rename from base/main_player.py rename to keepaway/base/main_player.py index d56df4f2..58c44937 100755 --- a/base/main_player.py +++ b/keepaway/base/main_player.py @@ -1,10 +1,5 @@ #!/usr/bin/python3 -from base.sample_player import SamplePlayer -from lib.player.basic_client import BasicClient -from lib.player.player_agent import PlayerAgent -import sys -import team_config - +from keepaway.base.sample_player import SamplePlayer def main(): agent = SamplePlayer() diff --git a/base/main_trainer.py b/keepaway/base/main_trainer.py similarity index 71% rename from base/main_trainer.py rename to keepaway/base/main_trainer.py index 8c38ece4..5cbee293 100755 --- a/base/main_trainer.py +++ b/keepaway/base/main_trainer.py @@ -1,6 +1,6 @@ #!/usr/bin/python3 -from base.sample_trainer import SampleTrainer -from lib.player.basic_client import BasicClient +from keepaway.base.sample_trainer import SampleTrainer +from keepaway.lib.player.basic_client import BasicClient import sys diff --git a/base/sample_coach.py b/keepaway/base/sample_coach.py old mode 100644 new mode 100755 similarity index 93% rename from base/sample_coach.py rename to keepaway/base/sample_coach.py index 6477070e..80091b52 --- a/base/sample_coach.py +++ b/keepaway/base/sample_coach.py @@ -1,10 +1,10 @@ import functools -from lib.coach.coach_agent import CoachAgent -from lib.debug.debug import log -from lib.rcsc.player_type import PlayerType -from lib.rcsc.types import HETERO_DEFAULT, HETERO_UNKNOWN +from keepaway.lib.coach.coach_agent import CoachAgent +from keepaway.lib.debug.debug import log +from keepaway.lib.rcsc.player_type import PlayerType +from keepaway.lib.rcsc.types import HETERO_DEFAULT, HETERO_UNKNOWN -import team_config +from keepaway.config import team_config def real_speed_max(lhs: PlayerType, rhs: PlayerType) -> bool: if abs(lhs.real_speed_max() - rhs.real_speed_max()) < 0.005: diff --git a/keepaway/base/sample_communication.py b/keepaway/base/sample_communication.py new file mode 100755 index 00000000..d44500ec --- /dev/null +++ b/keepaway/base/sample_communication.py @@ -0,0 +1,842 @@ +from math import exp + +from pyrusgeom.soccer_math import bound + +from keepaway.config import team_config +from keepaway.base.strategy import Strategy +from keepaway.lib.debug.debug import log +from keepaway.lib.messenger.ball_goalie_messenger import BallGoalieMessenger +from keepaway.lib.messenger.ball_messenger import BallMessenger +from keepaway.lib.messenger.ball_player_messenger import BallPlayerMessenger +from keepaway.lib.messenger.ball_pos_vel_messenger import BallPosVelMessenger +from keepaway.lib.messenger.goalie_messenger import GoalieMessenger +from keepaway.lib.messenger.goalie_player_messenger import GoaliePlayerMessenger +from keepaway.lib.messenger.messenger import Messenger +from keepaway.lib.messenger.messenger_memory import MessengerMemory +from keepaway.lib.messenger.one_player_messenger import OnePlayerMessenger +from keepaway.lib.messenger.recovery_message import RecoveryMessenger +from keepaway.lib.messenger.stamina_messenger import StaminaMessenger +from keepaway.lib.messenger.three_player_messenger import ThreePlayerMessenger +from keepaway.lib.messenger.two_player_messenger import TwoPlayerMessenger +from keepaway.lib.rcsc.game_time import GameTime +from keepaway.lib.rcsc.server_param import ServerParam +from keepaway.lib.rcsc.types import UNUM_UNKNOWN, GameModeType, SideID + +from typing import TYPE_CHECKING + +if TYPE_CHECKING: + from keepaway.lib.player.player_agent import PlayerAgent + from keepaway.lib.player.object_player import PlayerObject + from keepaway.lib.player.world_model import WorldModel + + +class ObjectScore: + def __init__(self, n=UNUM_UNKNOWN, score=-1000, player=None): + self.number = n + self.score = score + self.player: PlayerObject = player + + +class SampleCommunication: + def __init__(self): + self._current_sender_unum: int = UNUM_UNKNOWN + self._next_sender_unum: int = UNUM_UNKNOWN + self._ball_send_time: GameTime = GameTime(0, 0) + self._teammate_send_time: list[GameTime] = [GameTime(0, 0) for i in range(12)] + self._opponent_send_time: list[GameTime] = [GameTime(0, 0) for i in range(12)] + + def should_say_ball(self, agent: "PlayerAgent"): + wm = agent.world() + ef = agent.effector() + + if wm.ball().seen_pos_count() > 0 or wm.ball().seen_vel_count() > 2: + return False + if ( + wm.game_mode().type() != GameModeType.PlayOn + and ef.queued_next_ball_vel().r2() < 0.5**2 + ): + return False + if wm.kickable_teammate(): + return False + + ball_vel_changed = False + current_ball_speed = wm.ball().vel().r() + + if wm.prev_ball().vel_valid(): + prev_ball_speed = wm.prev_ball().vel().r() + angle_diff = (wm.ball().vel().th() - wm.prev_ball().vel().th()).abs() + + log.sw_log().communication().add_text( + f"(sample communication)" + f"prev vel={wm.prev_ball().vel()}, r={prev_ball_speed}" + f"current_vel={wm.ball().vel()}, r={wm.ball().vel()}" + ) + + if ( + current_ball_speed > prev_ball_speed + 0.1 + or ( + prev_ball_speed > 0.5 + and current_ball_speed + < prev_ball_speed * ServerParam.i().ball_decay() / 2 + ) + or (prev_ball_speed > 0.5 and angle_diff > 20.0) + ): + log.sw_log().communication().add_text( + f"(sample communication) ball vel changed" + ) + ball_vel_changed = True + + if wm.self().is_kickable(): + if ( + ball_vel_changed + and wm.last_kicker_side() != wm.our_side() + and not wm.kickable_opponent() + ): + log.sw_log().communication().add_text( + "(sample communication) ball vel changed. opponent kicked. no opponent kicker" + ) + return True + if ef.queued_next_ball_kickable() and current_ball_speed < 1.0: + return False + if ball_vel_changed and ef.queued_next_ball_vel().r() > 1.0: + log.sw_log().communication().add_text( + "(sample communication) kickable. ball vel changed" + ) + return True + log.sw_log().communication().add_text( + "(sample communication) kickable. but no say" + ) + return False + + ball_nearest_teammate: PlayerObject = None + second_ball_nearest_teammate: PlayerObject = None + + for p in wm.teammates_from_ball(): + if p is None: + continue + if p.is_ghost() or p.pos_count() >= 10: + continue + + if ball_nearest_teammate is None: + ball_nearest_teammate = p + elif second_ball_nearest_teammate is None: + second_ball_nearest_teammate = p + break + + our_min = min( + wm.intercept_table().self_reach_cycle(), + wm.intercept_table().teammate_reach_cycle(), + ) + opp_min = wm.intercept_table().opponent_reach_cycle() + + if ( + ball_nearest_teammate is None + or ball_nearest_teammate.dist_from_ball() > wm.ball().dist_from_self() - 3.0 + ): + log.sw_log().communication().add_text( + "(sample communication) maybe nearest to ball" + ) + + if ball_vel_changed or (opp_min <= 1 and current_ball_speed > 1.0): + log.sw_log().communication().add_text( + "(sample communication) nearest to ball. ball vel changed?" + ) + return True + + if ( + ball_nearest_teammate is not None + and wm.ball().dist_from_self() < 20.0 + and 1.0 < ball_nearest_teammate.dist_from_ball() < 6.0 + and (opp_min <= our_min + 1 or ball_vel_changed) + ): + log.sw_log().communication().add_text( + "(sample communication) support nearset player" + ) + return True + return False + + def should_say_opponent_goalie(self, agent: "PlayerAgent"): + wm = agent.world() + goalie: PlayerObject = wm.get_their_goalie() + + if goalie is None: + return False + + if ( + goalie.seen_pos_count() == 0 + and goalie.body_count() == 0 + and goalie.unum() != UNUM_UNKNOWN + and goalie.unum_count() == 0 + and goalie.dist_from_self() < 25.0 + and 51.0 - 16.0 < goalie.pos().x() < 52.5 + and goalie.pos().abs_y() < 20.0 + ): + goal_pos = ServerParam.i().their_team_goal_pos() + ball_next = wm.ball().pos() + wm.ball().vel() + + if ball_next.dist2(goal_pos) < 18**2: + return True + return False + + def update_player_send_time(self, wm: "WorldModel", side: SideID, unum: int): + if not (1 <= unum <= 11): + log.os_log().error( + f"(sample communication) illegal player number. unum={unum}" + ) + return + + if side == wm.our_side(): + self._teammate_send_time[unum] = wm.time().copy() + else: + self._opponent_send_time[unum] = wm.time().copy() + + def say_ball_and_players(self, agent: "PlayerAgent"): + # print("communication starting .. ") + SP = ServerParam.i() + wm = agent.world() + ef = agent.effector() + + current_len = ef.get_say_message_length() + + should_say_ball = self.should_say_ball(agent) + should_say_goalie = self.should_say_opponent_goalie(agent) + goalie_say_situation = False # self.goalie_say_situation # TODO IMP FUNC + + if ( + not should_say_ball + and not should_say_goalie + and not goalie_say_situation + and self._current_sender_unum != wm.self().unum() + and current_len == 0 + ): + log.sw_log().communication().add_text( + "(sample communication) say ball and players: no send situation" + ) + return False + + available_len = SP.player_say_msg_size() - current_len + mm = wm.messenger_memory() + + objects = [ObjectScore(i, 1000) for i in range(23)] + objects[0].score = wm.time().cycle() - mm.ball_time().cycle() + + for p in mm.player_record(): + n = round(p[1].unum_) + if not (1 <= n <= 22): + continue + + objects[n].score = wm.time().cycle() - p[0].cycle() + + if 1 <= wm.their_goalie_unum() <= 11: + n = wm.their_goalie_unum() + 11 + diff = wm.time().cycle() - mm.goalie_time().cycle() + objects[n].score = min(objects[n].score, diff) + + if wm.self().is_kickable(): + if ef.queued_next_ball_kickable(): + objects[0].score = -1000 + else: + objects[0].score = 1000 + elif ( + wm.ball().seen_pos_count() > 0 + or wm.ball().seen_vel_count() > 1 + or wm.kickable_teammate() + ): + objects[0].score = -1000 + elif should_say_ball: + objects[0].score = 1000 + elif objects[0].score > 0: + if wm.prev_ball().vel_valid(): + angle_diff = (wm.ball().vel().th() - wm.prev_ball().vel().th()).abs() + prev_speed = wm.prev_ball().vel().r() + current_speed = wm.ball().vel().r() + + if ( + current_speed > prev_speed + 0.1 + or ( + prev_speed > 0.5 + and current_speed < prev_speed * SP.ball_decay() * 0.5 + ) + or (prev_speed > 0.5 and angle_diff > 20.0) + ): + objects[0].score = 1000 + else: + objects[0].score /= 2 + + variance = 30 + x_rate = 1 + y_rate = 0.5 + + min_step = min( + wm.intercept_table().opponent_reach_cycle(), + wm.intercept_table().self_reach_cycle(), + wm.intercept_table().teammate_reach_cycle(), + ) + ball_pos = wm.ball().inertia_point(min_step) + + for i in range(1, 12): + p = wm.our_player(i) + if p is None or p.unum_count() >= 2: + objects[i].score = -1000 + else: + d = ( + ((p.pos().x() - ball_pos.x()) * x_rate) ** 2 + + ((p.pos().y() - ball_pos.y()) * y_rate) ** 2 + ) ** 0.5 + objects[i].score *= exp(-(d**2) / (2 * variance**2)) + objects[i].score *= 0.3 ** p.unum_count() + objects[i].player = p + + p = wm.their_player(i) + if p is None or p.unum_count() >= 2: + objects[i + 11].score = -1000 + else: + d = ( + ((p.pos().x() - ball_pos.x()) * x_rate) ** 2 + + ((p.pos().y() - ball_pos.y()) * y_rate) ** 2 + ) ** 0.5 + objects[i + 11].score *= exp(-(d**2) / (2 * variance**2)) + objects[i + 11].score *= 0.3 ** p.unum_count() + objects[i + 11].player = p + + objects = list(filter(lambda x: x.score > 0.1, objects)) + objects.sort(key=lambda x: x.score, reverse=True) + + can_send_ball = False + send_ball_and_player = False + send_players: list[ObjectScore] = [] + + for o in objects: + if o.number == 0: + can_send_ball = True + if len(send_players) == 1: + send_ball_and_player = True + break + else: + send_players.append(o) + if can_send_ball: + send_ball_and_player = True + break + + if len(send_players) >= 3: + break + + if should_say_ball: + can_send_ball = True + + ball_vel = ef.queued_next_ball_vel() + if wm.self().is_kickable() and ef.queued_next_ball_kickable(): + ball_vel.assign(0, 0) + log.sw_log().communication().add_text( + "(sample communication) next cycle is kickable" + ) + + if wm.kickable_opponent() or wm.kickable_teammate(): + ball_vel.assign(0, 0) + + if ( + can_send_ball + and not send_ball_and_player + and available_len >= Messenger.SIZES[Messenger.Types.BALL] + ): + if available_len >= Messenger.SIZES[Messenger.Types.BALL_PLAYER]: + agent.add_say_message( + BallPlayerMessenger( + ef.queued_next_ball_pos(), + ball_vel, + wm.self().unum(), + ef.queued_next_self_pos(), + ef.queued_next_self_body(), + ) + ) + self.update_player_send_time(wm, wm.our_side(), wm.self().unum()) + else: + agent.add_say_message( + BallMessenger(ef.queued_next_ball_pos(), ball_vel) + ) + + self._ball_send_time = wm.time().copy() + log.sw_log().communication().add_text("(sample communication) only ball") + return True + + if send_ball_and_player: + if ( + should_say_goalie + and available_len >= Messenger.SIZES[Messenger.Types.BALL_GOALIE] + ): + goalie = wm.get_their_goalie() + agent.add_say_message( + BallGoalieMessenger( + ef.queued_next_ball_pos(), + ball_vel, + goalie.pos() + goalie.vel(), + goalie.body(), + ) + ) + self._ball_send_time = wm.time().copy() + self.update_player_send_time(wm, goalie.side(), goalie.unum()) + + log.sw_log().communication().add_text( + "(sample communication) ball and goalie" + ) + return True + + if available_len >= Messenger.SIZES[Messenger.Types.BALL_PLAYER]: + p = send_players[0].player + if p.unum() == wm.self().unum(): + agent.add_say_message( + BallPlayerMessenger( + ef.queued_next_ball_pos(), + ball_vel, + wm.self().unum(), + ef.queued_next_self_pos(), + ef.queued_next_self_body(), + ) + ) + else: + agent.add_say_message( + BallPlayerMessenger( + ef.queued_next_ball_pos(), + ball_vel, + send_players[0].number, + p.pos() + p.vel(), + p.body(), + ) + ) + + self._ball_send_time = wm.time().copy() + self.update_player_send_time(wm, p.side(), p.unum()) + + log.sw_log().communication().add_text( + f"(sample communication) ball and player {p.side()}{p.unum()}" + ) + return True + + if wm.ball().pos().x() > 34 and wm.ball().pos().abs_y() < 20: + goalie: PlayerObject = wm.get_their_goalie() + + if ( + goalie is not None + and goalie.seen_pos_count() == 0 + and goalie.body_count() == 0 + and goalie.pos().x() > 53.0 - 16 + and goalie.pos().abs_y() < 20.0 + and goalie.unum() != UNUM_UNKNOWN + and goalie.dist_from_self() < 25 + ): + if available_len >= Messenger.SIZES[Messenger.Types.GOALIE_PLAYER]: + player: PlayerObject = None + for p in send_players: + if ( + p.player.unum() != goalie.unum() + and p.player.side() != goalie.side() + ): + player = p.player + break + + if player is not None: + goalie_pos = goalie.pos() + goalie.vel() + goalie_pos.assign( + bound(53.0 - 16.0, goalie_pos.x(), 52.9), + bound(-20, goalie_pos.y(), 20), + ) + agent.add_say_message( + GoaliePlayerMessenger( + goalie.unum(), + goalie_pos, + goalie.body(), + ( + player.unum() + if player.side() == wm.our_side() + else player.unum() + 11 + ), + player.pos() + player.vel(), + ) + ) + self.update_player_send_time(wm, goalie.side(), goalie.unum()) + self.update_player_send_time(wm, player.side(), player.unum()) + + log.sw_log().communication().add_text( + f"(sample communication) say goalie and player: " + f"goalie({goalie.unum()}): p={goalie.pos()} b={goalie.body()}" + f"player({player.side()}{player.unum()}: {player.pos()})" + ) + return True + + if available_len >= Messenger.SIZES[Messenger.Types.GOALIE]: + goalie_pos = goalie.pos() + goalie.vel() + goalie_pos.assign( + bound(53.0 - 16.0, goalie_pos.x(), 52.9), + bound(-20, goalie_pos.y(), 20), + ) + agent.add_say_message( + GoalieMessenger(goalie.unum(), goalie_pos, goalie.body()) + ) + self._ball_send_time = wm.time().copy() + self._opponent_send_time[goalie.unum()] = wm.time().copy() + + log.sw_log().communication().add_text( + f"(sample communication) say goalie info:" + f"{goalie.unum()} {goalie.pos()} {goalie.body()}" + ) + return True + + if ( + len(send_players) >= 3 + and available_len >= Messenger.SIZES[Messenger.Types.THREE_PLAYER] + ): + for o in send_players: + log.os_log().debug(o.player) + log.os_log().debug(o.player.pos()) + p0 = send_players[0].player + p1 = send_players[1].player + p2 = send_players[2].player + + agent.add_say_message( + ThreePlayerMessenger( + send_players[0].number, + p0.pos() + p0.vel(), + send_players[1].number, + p1.pos() + p1.vel(), + send_players[2].number, + p2.pos() + p2.vel(), + ) + ) + self.update_player_send_time(wm, p0.side(), p0.unum()) + self.update_player_send_time(wm, p1.side(), p1.unum()) + self.update_player_send_time(wm, p2.side(), p2.unum()) + + log.sw_log().communication().add_text( + f"(sample communication) three players:" + f"{p0.side()}{p0.unum()}" + f"{p1.side()}{p1.unum()}" + f"{p2.side()}{p2.unum()}" + ) + return True + + if ( + len(send_players) >= 2 + and available_len >= Messenger.SIZES[Messenger.Types.TWO_PLAYER] + ): + p0 = send_players[0].player + p1 = send_players[1].player + + agent.add_say_message( + TwoPlayerMessenger( + send_players[0].number, + p0.pos() + p0.vel(), + send_players[1].number, + p1.pos() + p1.vel(), + ) + ) + self.update_player_send_time(wm, p0.side(), p0.unum()) + self.update_player_send_time(wm, p1.side(), p1.unum()) + + log.sw_log().communication().add_text( + f"(sample communication) two players:" + f"{p0.side()}{p0.unum()}" + f"{p1.side()}{p1.unum()}" + ) + return True + + if ( + len(send_players) >= 1 + and available_len >= Messenger.SIZES[Messenger.Types.GOALIE] + ): + p0 = send_players[0].player + if ( + p0.side() == wm.their_side() + and p0.goalie() + and p0.pos().x() > 53.0 - 16.0 + and p0.pos().abs_y() < 20 + and p0.dist_from_self() < 25 + ): + goalie_pos = p0.pos() + p0.vel() + goalie_pos.assign( + bound(53.0 - 16.0, goalie_pos.x(), 52.9), + bound(-20, goalie_pos.y(), 20), + ) + agent.add_say_message(GoalieMessenger(p0.unum(), goalie_pos, p0.body())) + + self.update_player_send_time(wm, p0.side(), p0.unum()) + + log.sw_log().communication().add_text( + f"(sample communication) goalie:" f"{p0.side()}{p0.unum()}" + ) + return True + + if ( + len(send_players) >= 1 + and available_len >= Messenger.SIZES[Messenger.Types.ONE_PLAYER] + ): + p0 = send_players[0].player + + agent.add_say_message( + OnePlayerMessenger(send_players[0].number, p0.pos() + p0.vel()) + ) + + self.update_player_send_time(wm, p0.side(), p0.unum()) + + log.sw_log().communication().add_text( + f"(sample communication) one player:" f"{p0.side()}{p0.unum()}" + ) + return True + + return False + + def update_current_sender(self, agent: "PlayerAgent"): + wm = agent.world() + if agent.effector().get_say_message_length() > 0: + self._current_sender_unum = wm.self().unum() + return + + self._current_sender_unum = UNUM_UNKNOWN + candidate_unum: list[int] = [] + + if wm.ball().pos().x() < -10.0 or wm.game_mode().type() != GameModeType.PlayOn: + for unum in range(1, 12): + candidate_unum.append(unum) + else: + goalie_unum = ( + wm.our_goalie_unum() if wm.our_goalie_unum() != UNUM_UNKNOWN else 1 + ) # TODO STRATEGY.GOALIE_UNUM() + for unum in range(1, 12): + if unum != goalie_unum: + candidate_unum.append(unum) + val = ( + wm.time().cycle() + wm.time().stopped_cycle() + if wm.time().stopped_cycle() > 0 + else wm.time().cycle() + ) + current = val % len(candidate_unum) + next = (val + 1) % len(candidate_unum) + + self._current_sender_unum = candidate_unum[current] + self._next_sender_unum = candidate_unum[next] + + def say_recovery(self, agent: "PlayerAgent"): + current_len = agent.effector().get_say_message_length() + available_len = ServerParam.i().player_say_msg_size() - current_len + if available_len < Messenger.SIZES[Messenger.Types.RECOVERY]: + return False + + agent.add_say_message(RecoveryMessenger(agent.world().self().recovery())) + log.sw_log().communication().add_text( + "(sample communication) say self recovery" + ) + return True + + def say_stamina(self, agent: "PlayerAgent"): + current_len = agent.effector().get_say_message_length() + if current_len == 0: + return False + available_len = ServerParam.i().player_say_msg_size() - current_len + if available_len < Messenger.SIZES[Messenger.Types.STAMINA]: + return False + agent.add_say_message(StaminaMessenger(agent.world().self().stamina())) + log.sw_log().communication().add_text("(sample communication) say self stamina") + return True + + def attention_to_someone(self, agent: "PlayerAgent"): # TODO IMP FUNC + wm = agent.world() + ef = agent.effector() + + if ( + wm.self().pos().x() > wm.offside_line_x() - 15.0 + and wm.intercept_table().self_reach_cycle() <= 3 + ): + if ( + self._current_sender_unum != wm.self().unum() + and self._current_sender_unum != UNUM_UNKNOWN + ): + agent.do_attentionto(wm.our_side(), self._current_sender_unum) + player = wm.our_player(self._current_sender_unum) + if player is not None: + log.debug_client().add_circle(player.pos(), 3.0, color="#000088") + log.debug_client().add_line( + player.pos(), wm.self().pos(), "#000088" + ) + log.debug_client().add_message( + f"AttCurSender{self._current_sender_unum}" + ) + else: + candidates: list[PlayerObject] = [] + for p in wm.teammates_from_self(): + if ( + p.goalie() + or p.unum() == UNUM_UNKNOWN + or p.pos().x() > wm.offside_line_x() + 1.0 + ): + continue + if p.dist_from_self() > 20.0: + break + + candidates.append(p) + + self_next = ef.queued_next_self_pos() + + target_teammate: PlayerObject = None + max_x = -100000 + for p in candidates: + diff = p.pos() + p.vel() - self_next + x = diff.x() * (1.0 - diff.abs_y() / 40) + if x > max_x: + max_x = x + target_teammate = p + + if target_teammate is not None: + log.sw_log().communication().add_text( + f"(attentionto someone) most front teammate" + ) + log.debug_client().add_message( + f"AttFrontMate{target_teammate.unum()}" + ) + log.debug_client().add_circle( + target_teammate.pos(), 3.0, color="#000088" + ) + log.debug_client().add_line( + target_teammate.pos(), wm.self().pos(), "#000088" + ) + agent.do_attentionto(wm.our_side(), target_teammate.unum()) + return + + if wm.self().attentionto_unum() > 0: + log.sw_log().communication().add_text( + "(attentionto someone) attentionto off. maybe ball owner" + ) + log.debug_client().add_message("AttOffBOwner") + agent.do_attentionto_off() + return + + fastest_teammate = wm.intercept_table().fastest_teammate() + self_min = wm.intercept_table().self_reach_cycle() + mate_min = wm.intercept_table().teammate_reach_cycle() + opp_min = wm.intercept_table().opponent_reach_cycle() + + if ( + fastest_teammate is not None + and fastest_teammate.unum() != UNUM_UNKNOWN + and mate_min <= 1 + and mate_min < self_min + and mate_min <= opp_min + 1 + and mate_min <= 5 + min(4, fastest_teammate.pos_count()) + and wm.ball().inertia_point(mate_min).dist2(ef.queued_next_self_pos()) + < 35.0**2 + ): + log.debug_client().add_message(f"AttBallOwner{fastest_teammate.unum()}") + log.debug_client().add_circle(fastest_teammate.pos(), 3.0, color="#000088") + log.debug_client().add_line( + fastest_teammate.pos(), wm.self().pos(), "#000088" + ) + agent.do_attentionto(wm.our_side(), fastest_teammate.unum()) + return + + nearest_teammate = wm.get_teammate_nearest_to_ball(5) + if ( + nearest_teammate is not None + and nearest_teammate.unum() != UNUM_UNKNOWN + and opp_min <= 3 + and opp_min <= mate_min + and opp_min <= self_min + and nearest_teammate.dist_from_self() < 45.0 + and nearest_teammate.dist_from_ball() < 20.0 + ): + log.debug_client().add_message( + f"AttBallNearest(1){nearest_teammate.unum()}" + ) + log.debug_client().add_circle(nearest_teammate.pos(), 3.0, color="#000088") + log.debug_client().add_line( + nearest_teammate.pos(), wm.self().pos(), "#000088" + ) + agent.do_attentionto(wm.our_side(), nearest_teammate.unum()) + return + + if ( + nearest_teammate is not None + and nearest_teammate.unum() != UNUM_UNKNOWN + and wm.ball().pos_count() >= 3 + and nearest_teammate.dist_from_ball() < 20.0 + ): + log.debug_client().add_message( + f"AttBallNearest(2){nearest_teammate.unum()}" + ) + log.debug_client().add_circle(nearest_teammate.pos(), 3.0, color="#000088") + log.debug_client().add_line( + nearest_teammate.pos(), wm.self().pos(), "#000088" + ) + agent.do_attentionto(wm.our_side(), nearest_teammate.unum()) + return + + if ( + nearest_teammate is not None + and nearest_teammate.unum() != 45.0 + and nearest_teammate.dist_from_self() < 45.0 + and nearest_teammate.dist_from_ball() < 3.5 + ): + log.debug_client().add_message( + f"AttBallNearest(3){nearest_teammate.unum()}" + ) + log.debug_client().add_circle(nearest_teammate.pos(), 3.0, color="#000088") + log.debug_client().add_line( + nearest_teammate.pos(), wm.self().pos(), "#000088" + ) + agent.do_attentionto(wm.our_side(), nearest_teammate.unum()) + return + + if ( + self._current_sender_unum != wm.self().unum() + and self._current_sender_unum != UNUM_UNKNOWN + ): + log.debug_client().add_message(f"AttCurSender{self._current_sender_unum}") + player = wm.our_player(self._current_sender_unum) + if player is not None: + log.debug_client().add_circle(player.pos(), 3.0, color="#000088") + log.debug_client().add_line(player.pos(), wm.self().pos(), "#000088") + agent.do_attentionto(wm.our_side(), self._current_sender_unum) + else: + log.debug_client().add_message(f"AttOff") + agent.do_attentionto_off() + + # print("fastest teammate", fastest_teammate) + + def execute(self, agent: "PlayerAgent"): + # if not team_config.USE_COMMUNICATION: + # # print("False") + # return False + + self.update_current_sender(agent) + + wm = agent.world() + penalty_shootout = wm.game_mode().is_penalty_kick_mode() + + say_recovery = False + + # if ( + # wm.game_mode().type() == GameModeType.PlayOn + # and not penalty_shootout + # and self._current_sender_unum == wm.self().unum() + # and wm.self().recovery() < ServerParam.i().recover_init() - 0.002 + # ): + # say_recovery = True + # self.say_recovery(agent) + + # if ( + # wm.game_mode().type() == GameModeType.BeforeKickOff + # or wm.game_mode().type().is_after_goal() + # or penalty_shootout + # ): + # return say_recovery + + # if not ( + # wm.game_mode().type() == GameModeType.BeforeKickOff + # or wm.game_mode().type().is_kick_off() + # ): + + self.say_ball_and_players(agent) + + # print("said ball and players") + self.say_stamina(agent) + + self.attention_to_someone(agent) # TODO IMP FUNC + + return True diff --git a/keepaway/base/sample_player.py b/keepaway/base/sample_player.py new file mode 100755 index 00000000..f80b5454 --- /dev/null +++ b/keepaway/base/sample_player.py @@ -0,0 +1,101 @@ +from keepaway.base.decision import get_decision +from keepaway.base.sample_communication import SampleCommunication +from keepaway.base.view_tactical import ViewTactical +from keepaway.lib.action.go_to_point import GoToPoint +from keepaway.lib.action.intercept import Intercept +from keepaway.lib.action.neck_body_to_ball import NeckBodyToBall +from keepaway.lib.action.neck_turn_to_ball import NeckTurnToBall +from keepaway.lib.action.neck_turn_to_ball_or_scan import NeckTurnToBallOrScan +from keepaway.lib.action.scan_field import ScanField +from keepaway.lib.debug.debug import log +from keepaway.lib.debug.level import Level +from keepaway.lib.player.player_agent import PlayerAgent +from keepaway.lib.rcsc.server_param import ServerParam +from keepaway.lib.rcsc.types import GameModeType + + +class SamplePlayer(PlayerAgent): + def __init__(self): + super().__init__() + + self._communication = SampleCommunication() + + def action_impl(self): + wm = self.world() + if self.do_preprocess(): + return + + get_decision(self) + + def do_preprocess(self): + wm = self.world() + + print("do_preprocess ball position: ", wm.ball().pos()) + + if wm.self().is_frozen(): + self.set_view_action(ViewTactical()) + self.set_neck_action(NeckTurnToBallOrScan()) + return True + + if not wm.self().pos_valid(): + self.set_view_action(ViewTactical()) + ScanField().execute(self) + return True + + count_thr = 10 if wm.self().goalie() else 5 + if wm.ball().pos_count() > count_thr or ( + wm.game_mode().type() is not GameModeType.PlayOn + and wm.ball().seen_pos_count() > count_thr + 10 + ): + self.set_view_action(ViewTactical()) + NeckBodyToBall().execute(self) + return True + + self.set_view_action(ViewTactical()) + + if self.do_heard_pass_receive(): + return True + + return False + + def do_heard_pass_receive(self): + wm = self.world() + + if ( + wm.messenger_memory().pass_time() != wm.time() + or len(wm.messenger_memory().pass_()) == 0 + or wm.messenger_memory().pass_()[0]._receiver != wm.self().unum() + ): + return False + + self_min = wm.intercept_table().self_reach_cycle() + intercept_pos = wm.ball().inertia_point(self_min) + heard_pos = wm.messenger_memory().pass_()[0]._pos + + log.sw_log().team().add_text( + f"(sample palyer do heard pass) heard_pos={heard_pos}, intercept_pos={intercept_pos}" + ) + + if ( + not wm.kickable_teammate() + and wm.ball().pos_count() <= 1 + and wm.ball().vel_count() <= 1 + and self_min < 20 + ): + log.sw_log().team().add_text( + f"(sample palyer do heard pass) intercepting!, self_min={self_min}" + ) + log.debug_client().add_message("Comm:Receive:Intercept") + Intercept().execute(self) + self.set_neck_action(NeckTurnToBall()) + else: + log.sw_log().team().add_text( + f"(sample palyer do heard pass) go to point!, cycle={self_min}" + ) + log.debug_client().set_target(heard_pos) + log.debug_client().add_message("Comm:Receive:GoTo") + + GoToPoint(heard_pos, 0.5, ServerParam.i().max_dash_power()).execute(self) + self.set_neck_action(NeckTurnToBall()) + + # TODO INTENTION?!? diff --git a/base/sample_trainer.py b/keepaway/base/sample_trainer.py old mode 100644 new mode 100755 similarity index 80% rename from base/sample_trainer.py rename to keepaway/base/sample_trainer.py index 2eb5c3c1..9ff1bc7d --- a/base/sample_trainer.py +++ b/keepaway/base/sample_trainer.py @@ -1,8 +1,8 @@ -from lib.debug.debug import log -from lib.debug.level import Level +from keepaway.lib.debug.debug import log +from keepaway.lib.debug.level import Level from pyrusgeom.vector_2d import Vector2D -from lib.player.trainer_agent import TrainerAgent -from lib.rcsc.types import GameModeType +from keepaway.lib.player.trainer_agent import TrainerAgent +from keepaway.lib.rcsc.types import GameModeType class SampleTrainer(TrainerAgent): diff --git a/keepaway/base/set_play/__init__.py b/keepaway/base/set_play/__init__.py new file mode 100755 index 00000000..e69de29b diff --git a/base/set_play/bhv_goalie_set_play.py b/keepaway/base/set_play/bhv_goalie_set_play.py old mode 100644 new mode 100755 similarity index 87% rename from base/set_play/bhv_goalie_set_play.py rename to keepaway/base/set_play/bhv_goalie_set_play.py index 46afcb4f..512a6c17 --- a/base/set_play/bhv_goalie_set_play.py +++ b/keepaway/base/set_play/bhv_goalie_set_play.py @@ -1,19 +1,19 @@ from pyrusgeom.rect_2d import Rect2D from pyrusgeom.vector_2d import Vector2D -from base.generator_action import KickAction, KickActionType -from base.generator_pass import BhvPassGen -from lib.action.hold_ball import HoldBall -from lib.action.neck_scan_field import NeckScanField -from lib.action.neck_scan_players import NeckScanPlayers -from lib.action.smart_kick import SmartKick -from lib.debug.debug import log -from lib.messenger.pass_messenger import PassMessenger -from lib.rcsc.server_param import ServerParam +from keepaway.base.generator_action import KickAction, KickActionType +from keepaway.base.generator_pass import BhvPassGen +from keepaway.lib.action.hold_ball import HoldBall +from keepaway.lib.action.neck_scan_field import NeckScanField +from keepaway.lib.action.neck_scan_players import NeckScanPlayers +from keepaway.lib.action.smart_kick import SmartKick +from keepaway.lib.debug.debug import log +from keepaway.lib.messenger.pass_messenger import PassMessenger +from keepaway.lib.rcsc.server_param import ServerParam from typing import TYPE_CHECKING if TYPE_CHECKING: - from lib.player.player_agent import PlayerAgent + from keepaway.lib.player.player_agent import PlayerAgent class Bhv_GoalieSetPlay: @@ -94,14 +94,14 @@ def execute(self, agent: 'PlayerAgent'): return True def get_kick_point(self, agent: 'PlayerAgent'): - base_x = -43. + keepaway.base_x = -43. basy_y = 10. candids: list[tuple[Vector2D, float]] = [] points = [ - Vector2D(base_x, basy_y), - Vector2D(base_x, -basy_y), - Vector2D(base_x, 0), + Vector2D(keepaway.base_x, basy_y), + Vector2D(keepaway.base_x, -basy_y), + Vector2D(keepaway.base_x, 0), ] for p in points: diff --git a/base/set_play/bhv_set_play.py b/keepaway/base/set_play/bhv_set_play.py old mode 100644 new mode 100755 similarity index 88% rename from base/set_play/bhv_set_play.py rename to keepaway/base/set_play/bhv_set_play.py index 0293171f..10d6ef02 --- a/base/set_play/bhv_set_play.py +++ b/keepaway/base/set_play/bhv_set_play.py @@ -1,21 +1,21 @@ -from base.set_play.bhv_set_play_before_kick_off import Bhv_BeforeKickOff -from base.strategy_formation import * -from lib.action.neck_scan_players import NeckScanPlayers -from lib.action.neck_turn_to_ball_or_scan import NeckTurnToBallOrScan -from lib.action.scan_field import ScanField -from lib.debug.debug import log -from lib.debug.level import Level -from lib.action.go_to_point import * -from lib.messenger.pass_messenger import PassMessenger -from lib.rcsc.types import GameModeType -from base.generator_action import KickAction -from base.generator_pass import BhvPassGen -from lib.action.smart_kick import SmartKick +from keepaway.base.set_play.bhv_set_play_before_kick_off import Bhv_BeforeKickOff +from keepaway.base.strategy_formation import * +from keepaway.lib.action.neck_scan_players import NeckScanPlayers +from keepaway.lib.action.neck_turn_to_ball_or_scan import NeckTurnToBallOrScan +from keepaway.lib.action.scan_field import ScanField +from keepaway.lib.debug.debug import log +from keepaway.lib.debug.level import Level +from keepaway.lib.action.go_to_point import * +from keepaway.lib.messenger.pass_messenger import PassMessenger +from keepaway.lib.rcsc.types import GameModeType +from keepaway.base.generator_action import KickAction +from keepaway.base.generator_pass import BhvPassGen +from keepaway.lib.action.smart_kick import SmartKick from typing import TYPE_CHECKING if TYPE_CHECKING: - from lib.player.object_player import PlayerObject - from lib.player.player_agent import PlayerAgent + from keepaway.lib.player.object_player import PlayerObject + from keepaway.lib.player.player_agent import PlayerAgent class Bhv_SetPlay: diff --git a/base/set_play/bhv_set_play_before_kick_off.py b/keepaway/base/set_play/bhv_set_play_before_kick_off.py old mode 100644 new mode 100755 similarity index 66% rename from base/set_play/bhv_set_play_before_kick_off.py rename to keepaway/base/set_play/bhv_set_play_before_kick_off.py index dca191e9..a7a9241e --- a/base/set_play/bhv_set_play_before_kick_off.py +++ b/keepaway/base/set_play/bhv_set_play_before_kick_off.py @@ -1,12 +1,12 @@ -from lib.action.neck_scan_field import NeckScanField -from lib.action.scan_field import ScanField -from lib.debug.level import Level +from keepaway.lib.action.neck_scan_field import NeckScanField +from keepaway.lib.action.scan_field import ScanField +from keepaway.lib.debug.level import Level from pyrusgeom.angle_deg import AngleDeg -from base.strategy_formation import StrategyFormation +from keepaway.base.strategy_formation import StrategyFormation from typing import TYPE_CHECKING if TYPE_CHECKING: - from lib.player.player_agent import PlayerAgent + from keepaway.lib.player.player_agent import PlayerAgent class Bhv_BeforeKickOff: def __init__(self): pass diff --git a/base/stamina_manager.py b/keepaway/base/stamina_manager.py old mode 100644 new mode 100755 similarity index 94% rename from base/stamina_manager.py rename to keepaway/base/stamina_manager.py index cd6a0360..d96a44cc --- a/base/stamina_manager.py +++ b/keepaway/base/stamina_manager.py @@ -1,9 +1,9 @@ -from lib.rcsc.server_param import ServerParam as SP +from keepaway.lib.rcsc.server_param import ServerParam as SP import pyrusgeom.soccer_math as smath from typing import TYPE_CHECKING if TYPE_CHECKING: - from lib.player.world_model import WorldModel + from keepaway.lib.player.world_model import WorldModel def get_normal_dash_power(wm: 'WorldModel', s_recover_mode: bool): diff --git a/base/strategy.py b/keepaway/base/strategy.py old mode 100644 new mode 100755 similarity index 100% rename from base/strategy.py rename to keepaway/base/strategy.py diff --git a/base/strategy_formation.py b/keepaway/base/strategy_formation.py old mode 100644 new mode 100755 similarity index 94% rename from base/strategy_formation.py rename to keepaway/base/strategy_formation.py index 2467bc4b..72fdfa51 --- a/base/strategy_formation.py +++ b/keepaway/base/strategy_formation.py @@ -1,12 +1,12 @@ -from lib.formation.delaunay_triangulation import * +from keepaway.formation.delaunay_triangulation import * import os from enum import Enum -from lib.debug.debug import log -from lib.rcsc.types import GameModeType +from keepaway.lib.debug.debug import log +from keepaway.lib.rcsc.types import GameModeType from typing import TYPE_CHECKING if TYPE_CHECKING: - from lib.player.world_model import WorldModel + from keepaway.lib.player.world_model import WorldModel class Situation(Enum): OurSetPlay_Situation = 0, @@ -19,8 +19,8 @@ class Situation(Enum): class _StrategyFormation: def __init__(self): pwd = '.' - if "base" in os.listdir('.'): - pwd = 'base' + if "keepaway.base" in os.listdir('.'): + pwd = 'keepaway.base' self.before_kick_off_formation: Formation = Formation(f'{pwd}/formation_dt/before_kick_off.conf') self.defense_formation: Formation = Formation(f'{pwd}/formation_dt/defense_formation.conf') self.offense_formation: Formation = Formation(f'{pwd}/formation_dt/offense_formation.conf') diff --git a/base/tackle_generator.py b/keepaway/base/tackle_generator.py old mode 100644 new mode 100755 similarity index 96% rename from base/tackle_generator.py rename to keepaway/base/tackle_generator.py index 71aa923b..9eefb15a --- a/base/tackle_generator.py +++ b/keepaway/base/tackle_generator.py @@ -8,20 +8,20 @@ from pyrusgeom.soccer_math import inertia_final_point, inertia_n_step_point from pyrusgeom.vector_2d import Vector2D -from base.generator_action import TackleAction -from base.tools import Tools -from lib.rcsc.game_time import GameTime +from keepaway.base.generator_action import TackleAction +from keepaway.base.tools import Tools +from keepaway.lib.rcsc.game_time import GameTime from typing import TYPE_CHECKING -from lib.rcsc.server_param import ServerParam -from lib.rcsc.types import GameModeType +from keepaway.lib.rcsc.server_param import ServerParam +from keepaway.lib.rcsc.types import GameModeType if TYPE_CHECKING: - from lib.player.world_model import WorldModel - from lib.player.object_player import PlayerObject + from keepaway.lib.player.world_model import WorldModel + from keepaway.lib.player.object_player import PlayerObject class TackleGenerator: diff --git a/keepaway/base/tools.py b/keepaway/base/tools.py new file mode 100755 index 00000000..e801b7a6 --- /dev/null +++ b/keepaway/base/tools.py @@ -0,0 +1,357 @@ +import math + +from pyrusgeom.geom_2d import * +from keepaway.lib.rcsc.server_param import ServerParam +from keepaway.lib.rcsc.player_type import PlayerType +from keepaway.lib.rcsc.game_mode import GameModeType +from keepaway.lib.action.kick_table import calc_max_velocity +import pyrusgeom.soccer_math as sm + +# from keepaway.lib.rcsc.types import CommandType +from keepaway.lib.player_command.player_command import CommandType +from pyrusgeom.vector_2d import Vector2D +from pyrusgeom.angle_deg import AngleDeg + +from typing import TYPE_CHECKING + +if TYPE_CHECKING: + from keepaway.lib.player.world_model import WorldModel + from keepaway.lib.player.object_player import PlayerObject + + +class Tools: + @staticmethod + def predict_player_turn_cycle( + ptype: PlayerType, + player_body: AngleDeg, + player_speed, + target_dist, + target_angle: AngleDeg, + dist_thr, + use_back_dash, + ): + sp = ServerParam.i() + n_turn = 0 + angle_diff = (target_angle - player_body).abs() + + if ( + use_back_dash + and target_dist < 5.0 + and angle_diff > 90.0 + and sp.min_dash_power() < -sp.max_dash_power() + 1.0 + ): + angle_diff = abs(angle_diff - 180.0) + + turn_margin = 180.0 + if dist_thr < target_dist: + turn_margin = max(15.0, AngleDeg.asin_deg(dist_thr / target_dist)) + + speed = float(player_speed) + while angle_diff > turn_margin: + angle_diff -= ptype.effective_turn(sp.max_moment(), speed) + speed *= ptype.player_decay() + n_turn += 1 + + return n_turn + + @staticmethod + def predict_kick_count( + wm: "WorldModel", kicker, first_ball_speed, ball_move_angle: AngleDeg + ): + if ( + wm.game_mode().type() != GameModeType.PlayOn + and not wm.game_mode().is_penalty_kick_mode() + ): + return 1 + + if kicker == wm.self().unum() and wm.self().is_kickable(): + max_vel = calc_max_velocity( + ball_move_angle, wm.self().kick_rate(), wm.ball().vel() + ) + if max_vel.r2() >= pow(first_ball_speed, 2): + return 1 + if first_ball_speed > 2.5: + return 3 + elif first_ball_speed > 1.5: + return 2 + return 1 + + @staticmethod + def estimate_min_reach_cycle( + player_pos: Vector2D, + player_speed_max, + target_first_point: Vector2D, + target_move_angle: AngleDeg, + ): + target_to_player: Vector2D = (player_pos - target_first_point).rotated_vector( + -target_move_angle + ) + if target_to_player.x() < -1.0: + return -1 + else: + return max(1, int(target_to_player.abs_y() / player_speed_max)) + + @staticmethod + def get_nearest_teammate( + wm: "WorldModel", position: Vector2D, players: list["PlayerObject"] = None + ): + if players is None: + players = wm.teammates() + best_player: "PlayerObject" = None + min_dist2 = 1000 + for player in players: + d2 = player.pos().dist2(position) + if d2 < min_dist2: + min_dist2 = d2 + best_player = player + + return best_player + + @staticmethod + def estimate_virtual_dash_distance(player: "PlayerObject"): + pos_count = min(10, player.pos_count(), player.seen_pos_count()) + max_speed = player.player_type().real_speed_max() * 0.8 + + d = 0.0 + for i in range(pos_count): + d += max_speed * math.exp(-(i**2) / 15) + + return d + + @staticmethod + def get_end_speed_from_first_speed(end_speed, cycles, decay): + SS = ServerParam.i() + if decay < 0.0: + decay = SS.ball_decay() + return end_speed * math.pow(1.0 / SS.ball_decay(), cycles) + + @staticmethod + def get_kick_travel(distance, target_speed): + SS = ServerParam.i() + if target_speed < 0.0001: + return sm.calc_first_term_geom_series(distance, SS.ball_decay()) + steps = sm.calc_length_geom_series( + target_speed, 1.0 / SS.ball_decay(), distance + ) + + sp = Tools.get_end_speed_from_first_speed(target_speed, steps, SS.ball_decay()) + # print("speed first : ", sp) + return sp + + @staticmethod + def predict_stamina_after_dash(dash_power, stamina): + sta = stamina.stamina() + eff = stamina.effort() + rec = stamina.recovery() + # // double negative value when dashed backwards + sta -= dash_power if dash_power > 0.0 else -2 * dash_power + if sta < 0: + sta = 0 + # // stamina below recovery threshold, lower recovery + if ( + sta <= ServerParam.i().recover_dec_thr() * ServerParam.i().stamina_max() + and rec > ServerParam.i().recover_min() + ): + rec -= ServerParam.i().recover_dec() + # // stamina below effort decrease threshold, lower effort + if ( + sta <= ServerParam.i().effort_dec_thr() * ServerParam.i().stamina_max() + and eff > ServerParam.i().effort_min() + ): + eff -= ServerParam.i().effort_dec() + # // stamina higher than effort incr threshold, raise effort and check maximum + if ( + sta >= ServerParam.i().effort_inc_thr() * ServerParam.i().stamina_max() + and eff < 1.0 + ): + eff += ServerParam.i().effort_inc() + if eff > 1.0: + eff = 1.0 + # // increase stamina with (new) recovery value and check for maximum + sta += rec * ServerParam.i().stamina_inc_max() + if sta > ServerParam.i().stamina_max(): + sta = ServerParam.i().stamina_max() + stamina.set_stamina(sta) + stamina.set_effort(eff) + stamina.set_recovery(rec) + + @staticmethod + def predict_state_after_dash(p, dash_power, pos, vel, stamina, direction): + """Predict the state of the object after dash.""" + + SP = ServerParam.i() + # get acceleration associated with actual power + effort = stamina.effort() if stamina else p.effort_max() + acc = dash_power * p.player_type().dash_power_rate() * effort + + # add it to the velocity; negative acceleration in backward direction + if acc > 0: + vel += Vector2D.polar2vector(acc, direction) + else: + vel += Vector2D.polar2vector(abs(acc), direction + 180) + + # check if velocity doesn't exceed maximum speed + if vel.r() > SP.default_player_speed_max(): + vel.set_r(SP.player_speed_max()) + + # add velocity to current global position and decrease velocity + pos += vel + vel *= p.player_type().player_decay() + # print("pos: ", pos) + # print("vel: ", vel) + + if stamina: + stamina.simulate_dash(p.player_type(), dash_power) + # Tools.predict_stamina_after_dash(dash_power, stamina) + + @staticmethod + def predict_pos_after_n_cycles(p, cycles, dash_power): + """Returns the position of player object after n cycles.""" + + SP = ServerParam.i() + # if p == BallObject: + # d = Geometry.get_sum_geom_series( + # vel.r(), + # SP.ball_decay(), + # cycles, + # ) + # pos += Vector2D(d, vel.th(), POLAR) + # vel *= math.pow(SP.ball_decay(),cycles) + + # if p == ptype.player_type: + update = True + ptype = p.player_type() + direction = p.body() + # print("direction: ", direction) + stamina = p.stamina_model() + + for i in range(cycles): + Tools.predict_state_after_dash( + p, dash_power, p.pos(), p.vel(), stamina, direction + ) + if update: + pos = p.pos() + vel = p.vel() + return pos + + @staticmethod + def predict_ball_after_command(wm: "WorldModel", command, pos, vel): + ball_pos = wm.ball().pos() + ball_vel = wm.ball().vel() + if command.type() == CommandType.KICK: + kick_angle = command.kick_dir() + power = command.kick_power() + angle = AngleDeg.normalize_angle(kick_angle + wm.self().body().degree()) + ball_vel += Vector2D.polar2vector( + wm.self().player_type().kick_power_rate() * power, angle + ) + if ball_vel.r() > ServerParam.i().ball_speed_max(): + ball_vel.set_length(ServerParam.i().ball_speed_max()) + # print("ang: ", angle, "kick_rate: ", wm.self().player_type().kick_power_rate()) + # print("update for kick: ", power, angle) + ball_pos += ball_vel + ball_vel *= ServerParam.i().ball_decay() + return ball_pos, ball_vel + + # @staticmethod + # def predict_pos_after_command(wm: "WorldModel", command): + + # @staticmethod + # def predict_state_after_dash( + # dash_power, + # pos: Vector2D, + # vel: Vector2D, + # stamina, + # d_direction, + # p : "PlayerObject", + # ): + # SS = ServerParam.i() + + # d_effort = stamina.get_effort() if stamina else p.stamina_model().get_effort() + # d_acc = dash_power * SS.dash_power_rate() * d_effort + + # # add it to the velocity; negative acceleration in backward direction + # if d_acc > 0: + # vel += Vector2D.polar2vector(d_acc, d_direction) + # else: + # vel += Vector2D.polar2vector(abs(d_acc), Vector2D.normalize_angle(d_direction + 180)) + + # # check if velocity doesn't exceed maximum speed + # if vel.r() > SS.default_player_speed_max(): + # vel.set_length(SS.default_player_speed_max()) + + # # add velocity to current global position and decrease velocity + # pos += vel + # vel *= p.player_type().player_decay() + + # # if stamina is provided, predict its value after dash + # if stamina: + # predict_stamina_after_dash(d_actual_power, stamina) + + # @staticmethod + # def predict_stamina_after_dash(dash_power): + # SS = ServerParam.i() + # sta = me.stamina_model() + # eff = sta.effort() + # rec = True + + # # double negative value when dashed backwards + # sta -= dash_power if dash_power > 0.0 else -2 * dash_power + # if sta < 0: + # sta = 0 + + # # stamina below recovery threshold, lower recovery + # if sta <= SS.recover_dec_thr * SS.stamina_max and rec > SS.recover_min: + # rec -= SS.recover_dec + + # # stamina below effort decrease threshold, lower effort + # if sta <= SS.effort_dec_thr * SS.stamina_max and eff > SS.effort_min: + # eff -= SS.effort_dec + + # # stamina higher than effort increase threshold, raise effort and check maximum + # if sta >= SS.effort_inc_thr * SS.stamina_max and eff < 1.0: + # eff += SS.effort_inc + # if eff > 1.0: + # eff = 1.0 + + # # increase stamina with (new) recovery value and check for maximum + # sta += rec * SS.stamina_inc_max + # if sta > SS.stamina_max: + # sta = SS.stamina_max + + # stamina.set_stamina(sta) + # stamina.set_effort(eff) + # stamina.set_recovery(rec) + + # @staticmethod + # def estimate_position( + # player: "PlayerObject", + # cycles, + # dash_power, + # pos: Vector2D, + # vel: Vector2D, + # ): + + # vel = wm.self()._vel if vel is None else vel + # pos = wm.self()._pos if pos is None else pos + + # d_direction = 0.0 + # stamina = wm.self().stamina_model() + + # if player: + # d_direction = player.body() + # stamina = player.stamina_model() + # elif wm.self().time() > wm.self().last_dash_time() + 2: + # d_direction = wm.self().body() + + # for i in range(int(cycles)): + # predict_state_after_dash( + # dash_power, pos, vel, stamina, d_direction, obj + # ) + + # if pos not None: + # pos = pos + vel * cycles: + # if vel not None: + # vel = vel * player.player_type().player_decay() ** cycles + + # return pos diff --git a/base/view_tactical.py b/keepaway/base/view_tactical.py old mode 100644 new mode 100755 similarity index 94% rename from base/view_tactical.py rename to keepaway/base/view_tactical.py index 897cd590..6d14fb36 --- a/base/view_tactical.py +++ b/keepaway/base/view_tactical.py @@ -3,12 +3,12 @@ from pyrusgeom.vector_2d import Vector2D -from lib.player.soccer_action import ViewAction -from lib.rcsc.server_param import ServerParam -from lib.rcsc.types import GameModeType, ViewWidth +from keepaway.lib.player.soccer_action import ViewAction +from keepaway.lib.rcsc.server_param import ServerParam +from keepaway.lib.rcsc.types import GameModeType, ViewWidth if TYPE_CHECKING: - from lib.player.player_agent import PlayerAgent + from keepaway.lib.player.player_agent import PlayerAgent class ViewTactical(ViewAction): diff --git a/keepaway/config/.DS_Store b/keepaway/config/.DS_Store new file mode 100644 index 00000000..797fd239 Binary files /dev/null and b/keepaway/config/.DS_Store differ diff --git a/keepaway/config/__init__.py b/keepaway/config/__init__.py new file mode 100644 index 00000000..200f1d41 --- /dev/null +++ b/keepaway/config/__init__.py @@ -0,0 +1,5 @@ +from keepaway.config.game_config import get_config + +def config(name): + c = get_config()[name] + return c \ No newline at end of file diff --git a/keepaway/config/game_config.py b/keepaway/config/game_config.py new file mode 100644 index 00000000..50791dc9 --- /dev/null +++ b/keepaway/config/game_config.py @@ -0,0 +1,33 @@ +""" +This configuration file describes the keep-away environment. +It is used to define the number of keepers, takers, limit and pitch size for each keepaway game. + +""" + +game_play = { + "3v2":{ + "num_keepers": 3, + "num_takers": 2, + "limit": 10, + "pitch_size": 20, + "sparse_reward": False, + }, + "4v3":{ + "num_keepers": 4, + "num_takers": 3, + "limit": 10, + "pitch_size": 30, + "sparse_reward": False, + }, + "5v4":{ + "num_keepers": 5, + "num_takers": 4, + "limit": 10, + "pitch_size": 40, + "sparse_reward": False, + }, +} + +def get_config(): + return game_play + diff --git a/keepaway/config/sample_agent_config.yml b/keepaway/config/sample_agent_config.yml new file mode 100644 index 00000000..b4604e61 --- /dev/null +++ b/keepaway/config/sample_agent_config.yml @@ -0,0 +1,15 @@ +always-hold: + policy: always-hold + strategy: greedy + learning_rate: 0.01 + +handcoded: + policy: handcoded + strategy: fixed + actions: + - action1 + - action2 + +random: + policy: random + strategy: random diff --git a/keepaway/config/server-config.yml b/keepaway/config/server-config.yml new file mode 100644 index 00000000..29f6aca2 --- /dev/null +++ b/keepaway/config/server-config.yml @@ -0,0 +1,41 @@ +# server configuration file - server-config.yml +# This file is used to configure the server. +## server parameters for keepaway passed + +coach: 0 +coach_port: 6001 +field_length: 20 +field_width: 20 +file_per_player: false +game_start: 8 +keeper_count: 3 +keeper_input: null +keeper_learn: 0 +keeper_output: null +keeper_policy: rand +label: "" +log_dir: "./../logs" +game_log_dir: "./../logs" +log_game: true +log_text: true +monitor: false +log_keepaway: true +online_coach_port: 6002 +port: 6000 +restricted_vision: false +synch_mode: false +taker_count: 2 +taker_input: null +taker_learn: false +taker_output: null +taker_policy: hand +log_level: 1 +fullstate: false +hierarchical_fsm: false +crossEntropy: false +qlearning: false +gamma: 1.0 +lambd: 0.0 +alpha: 0.125 +memory_check: false +initial_weight: 0.0 diff --git a/team_config.py b/keepaway/config/team_config.py similarity index 85% rename from team_config.py rename to keepaway/config/team_config.py index 989c55a6..d373e405 100644 --- a/team_config.py +++ b/keepaway/config/team_config.py @@ -2,13 +2,13 @@ class OUT_OPTION(Enum): - STDOUT = 'std' - TEXTFILE = 'textfile' + STDOUT = "std" + TEXTFILE = "textfile" TEAM_NAME = "PYRUS" OUT = OUT_OPTION.TEXTFILE -HOST = 'localhost' +HOST = "localhost" PLAYER_PORT = 6000 TRAINER_PORT = 6001 COACH_PORT = 6002 diff --git a/keepaway/envs/.DS_Store b/keepaway/envs/.DS_Store new file mode 100644 index 00000000..a807f536 Binary files /dev/null and b/keepaway/envs/.DS_Store differ diff --git a/keepaway/envs/__init__.py b/keepaway/envs/__init__.py new file mode 100644 index 00000000..79d4c3de --- /dev/null +++ b/keepaway/envs/__init__.py @@ -0,0 +1,5 @@ +# from keepaway.envs.keepaway_wrapper import KeepawayEnv +# __all__ = [ +# "KeepawayEnv", +# ] + diff --git a/keepaway/envs/keepaway_env.py b/keepaway/envs/keepaway_env.py new file mode 100644 index 00000000..c1229c2b --- /dev/null +++ b/keepaway/envs/keepaway_env.py @@ -0,0 +1,416 @@ +import atexit +from warnings import warn +from operator import attrgetter +import copy +import numpy as np +from absl import logging +from subprocess import Popen +import yaml +import time +from keepaway.lib.player.world_model import WorldModel +import multiprocessing +import keepaway.utils.main_keepaway_player as kp +import atexit +from keepaway.envs.multiagentenv import MultiAgentEnv +import os + +config_dir = os.getcwd() + "/config" + + +class KeepawayEnv(MultiAgentEnv): + """Keepaway environment for multi-agent reinforcement learning scenarios version 0.1.0.""" + + def __init__(self, config): + """ + Initialize a keep-away environment. + --------------------------------- + Parameters: + config: dict + Configuration dictionary containing the following + parameters: + num_keepers: int + Number of keepers in the environment. + num_takers: int + Number of takers in the environment. + pitch_size: int + Size of the pitch. + sparse_reward: bool + Whether to use a sparse reward signal. + + """ + + self.num_keepers = config["num_keepers"] + self.num_takers = config["num_takers"] + self.pitch_size = config["pitch_size"] + self.sparse_reward = config["sparse_reward"] + self.actions = self.num_keepers # 0: hold, 1: pass + self._episode_count = 0 + self._episode_steps = 0 + self._total_steps = 0 + self.force_restarts = 0 + self.episode_limit = 1000 + self.timeouts = 0 + self.continuing_episode = False + self.num_agents = self.num_keepers + self.num_takers + + self._last_action = None + manager = multiprocessing.Manager() + self._world = WorldModel("real", self.num_keepers, manager) # for all agents + + self._lock = self._world + self._event = multiprocessing.Event() + self._barrier = multiprocessing.Barrier(self.num_keepers) + + ## Event implementation + self._event_from_subprocess = multiprocessing.Event() + self._main_process_event = ( + multiprocessing.Event() + ) + + self._actions = [0] * self.num_keepers + self._shared_values = multiprocessing.Array("i", self._actions) + + self._obs = self._world._obs + # Use a joint value instead + self.last_action_time = 0 + self._last_action_time = multiprocessing.Value("i", self.last_action_time) + self._proximity_adj_mat = None + + ## reward + self._reward = self._world._reward + ## episode + self._terminated = self._world._terminated + self._episode_reward = [] + self._proximity_threshold = 2 + + self._terminal_time = None + + self.renderer = None + + self._keepers = [ + multiprocessing.Process( + target=kp.main, + args=( + "keepers", + i, + False, + self._shared_values, + self._barrier, + self._lock, + self._event, + self._event_from_subprocess, + self._main_process_event, + self._world, + self._obs, + self._last_action_time, + self._reward, + self._terminated, + self._proximity_adj_mat, + self._proximity_threshold, + ), + name="keeper", + ) + for i in range(self.num_keepers) + ] + + self._takers = [ + multiprocessing.Process( + target=kp.main, + args=( + "takers", + i, + False, + self._shared_values, + self._barrier, + self._lock, + self._event, + self._event_from_subprocess, + self._main_process_event, + self._world, + self._obs, + self._last_action_time, + self._reward, + self._terminated, + self._proximity_adj_mat, + self._proximity_threshold, + ), + name="takers", + ) + for i in range(self.num_takers) + ] + + ## uncomment to include some coaching + + # self._coach = [multiprocessing.Process(target=main_c.main)] + # coach = mp.Process(target=main_c.main) + # coach.start() + + self._server = [] + self._render = [] + self._sleep = time + + def _agents(self) -> list: + """Utility to return all agents in the environment.""" + return self._keepers + self._takers + + def _launch_monitor(self) -> int: + """Launches the monitor.""" + + logging.debug("Built the command to connect to the server") + monitor_cmd = f"soccerwindow2 &" + popen = Popen(monitor_cmd, shell=True) + return popen + + def load_agent_config(self, file_path): + with open(file_path) as f: + return yaml.safe_load(f) + + def _parse_options(self) -> object: + """ + Parses the given list of args, defaulting to sys.argv[1:]. + Retrieve other options from the YAML config file. + """ + + with open(f"{config_dir}/server-config.yml", "r") as yml: + config = yaml.safe_load(yml) + + class ConfigOptions: + pass + + options = ConfigOptions() + for key, value in config.items(): + if not getattr(options, key, None): + setattr(options, key, value) + return options + + def _launch_server(self, options) -> int: + """Launch the RCSS Server and Monitor""" + + options.field_length = self.pitch_size + options.field_width = self.pitch_size + + log_name = f"{time.strftime('%Y%m%d%H%M%S')}" + + server_options = [ + f"server::{opt}={val}" + for opt, val in { + "coach": int(options.coach), + "coach_port": int(options.coach_port), + "forbid_kick_off_offside": 1, + "half_time": -1, + "keepaway": int(not options.coach), + "keepaway_start": options.game_start, + "keepaway_length": int(options.field_length), + "keepaway_width": int(options.field_width), + "keepaway_logging": 1 if options.log_keepaway else 0, + "keepaway_log_dir": options.log_dir if options.log_keepaway else None, + "keepaway_log_fixed": 1 if options.log_keepaway else 0, + "keepaway_log_fixed_name": log_name if options.log_keepaway else None, + "game_log_compression": 0, + "game_log_dir": options.game_log_dir if options.log_game else None, + "game_log_fixed": 1 if options.log_game else 0, + "game_log_fixed_name": log_name if options.log_game else None, + "game_log_version": 5 if options.log_game else 0, + "game_logging": 1, + "olcoach_port": options.online_coach_port, + "port": options.port, + "stamina_inc_max": 3500, + "fullstate_l": int(options.fullstate), + "fullstate_r": int(options.fullstate), + "synch_mode": int(options.synch_mode), + "synch_offset": 60, + "synch_see_offset": 0, + "text_log_compression": 0, + "text_log_dir": options.game_log_dir if options.log_text else None, + "text_log_fixed": 1 if options.log_text else 0, + "text_log_fixed_name": log_name if options.log_text else None, + "text_logging": 1, + "use_offside": 0, + "visible_angle": 360 if not options.restricted_vision else None, + }.items() + if val is not None + ] + + + ## + command = ["rcssserver"] + server_options + popen = Popen(command) + + return popen + + def launch_game(self)-> None: + """Launch a keepaway game instance.""" + options = self._parse_options() + self._server.append(self._launch_server(options)) + + def reset( + self, + )-> tuple: + """Reset the environment. Required after each full episode.""" + + self._episode_steps = 0 + self._total_steps = 0 + self.last_action = None + self._episode_reward = [] + self.info = {} + if self._world._terminated.get_lock(): + self._world._terminated.value = False + + return (self._obs) + + def reward(self): + """ returns the reward for the current state """ + r = self._world.time().cycle() - self._terminal_time.cycle() + return r + + def _restart(self)-> None: + self.full_restart() + + def full_restart(self)-> None: + """Restart the environment. Required after each full episode.""" + + self.close() + self.start() + self.force_restarts += 1 + + def start(self)-> None: + """Start the players. Required before any other method calls.""" + + if self._episode_count == 0: + for i in range(self.num_keepers): + self._keepers[i].start() + + self._sleep.sleep(0.5) + + for i in range(self.num_takers): + self._takers[i].start() + + self._sleep.sleep(2.0) + + ## uncomment to include some coaching + # self._coach[0].start() + + atexit.register(self.close) + self._episode_count += 1 + else: + pass + + + def close(self)-> None: + """Close the environment. No other method calls possible afterwards.""" + + for p in self._keepers: + p.terminate() + + for s in self._server: + s.terminate() + + for t in self._takers: + t.terminate() + + for r in self._render: + r.terminate() + self.renderer = None + # self._coach[0].terminate() + + def render(self, mode="human")-> None: + """Render the environment using the monitor.""" + self._render.append(self._launch_monitor()) + self.renderer = mode + + def get_avail_agent_actions(self, agent_id)-> list: + """Returns the available actions for agent_id.""" + + l = [3] * self.num_agents + # return self._shared_values[agent_id] + return l + + def get_obs(self)-> dict: + """Returns the observation for all agents.""" + return self._obs + + def get_reward(self)-> int: + """ Returns the rewards for all agents.""" + return self._reward + + def get_obs_agent(self, agent_id)-> dict: + """Returns the observation for agent_id.""" + return self._obs[agent_id] + + def get_total_actions(self)-> int: + """Returns the total number of actions an agent could ever take.""" + return self.actions + + def get_obs_size(self): + """Returns the shape of the observation.""" + ## this should change based on the number of agents + return 13 + + def get_state_size(self): + """Returns the shape of the state.""" + obs_size = self.get_obs_size() + return self.num_agents + + def get_proximity_adj_mat(self): + adjacent_matrix = np.frombuffer(self._proximity_adj_mat, dtype=np.float64) + return adjacent_matrix + + def get_state(self): + """Returns the global state.""" + + obs = self.get_obs().values()["state_vars"] + obs_concat = np.concatenate(obs, axis=0) + return obs_concat + + def _check_process(self): + """Check if the process is still running.""" + for p in self._keepers + self._takers: + if not p.is_alive(): + return False + return True + + def step(self, actions): + """A single environment step. Returns reward, terminated, info. + + Args: + actions: list of actions for each agent + + Returns: + Tuple of time-step data for each agent + + + applies all actions to the + send an action signal and return observation. + """ + + if not actions: + self.agents = [] + return {}, {}, {}, {}, {} + + actions_int = actions + self._total_steps += 1 + self._episode_steps += 1 + total_reward = 0 + info = {} + terminated = False + + self._actions = copy.deepcopy(actions_int) + self._shared_values = multiprocessing.Array("i", self._actions) + self._observation = self._world._obs + game_state = self._world._terminated.value + if game_state == 1: + terminated = True + if not self.sparse_reward: + pass + # total_reward = self._reward.value + else: + total_reward += copy.deepcopy(self._reward.value) + + self._terminal_time = self._world.time() + if terminated: + self._episode_count += 1 + + ## check if processes are still running + if not self._check_process(): + self._restart() + return total_reward, terminated, info diff --git a/keepaway/envs/keepaway_wrapper.py b/keepaway/envs/keepaway_wrapper.py new file mode 100644 index 00000000..45aa0993 --- /dev/null +++ b/keepaway/envs/keepaway_wrapper.py @@ -0,0 +1,73 @@ +""" + Sample wrapper for the Keep-away environment. + This wrapper is used to make the environment compatible with the OpenAI Gym API. +""" + + + +import akro +import gym +import numpy as np +from gym import spaces +from gymnasium.utils import EzPickle +from gymnasium.utils import seeding +from keepaway.envs.keepaway_env import KeepawayEnv +from keepaway.config.game_config import get_config + +class KeepawayWrapper(KeepawayEnv): + + def __init__(self,centralized,config, *args, **kwargs): + super().__init__(config,*args, **kwargs) + + self.reward_range = (-np.inf, np.inf) + self.viewer = None + self.action_space = gym.spaces.Discrete(self.actions) + + observation_size = self.obs_size() + + self._obs_low = np.array([-1] * observation_size) + self._obs_high = np.array([1] * observation_size) + + + self.observation_space = gym.spaces.Box( + low=np.array(self._obs_low * self.num_agents), + high=np.array(self._obs_high * self.num_agents), + ) + + + self.pickleable = False + + self.state_size = observation_size + self.state_space = spaces.Box( + low=-1, high=1, shape=(self.state_size,), dtype="float32" + ) + self.episode_limit = 100000 + self.metric_name = "EvalAverageReturn" + self.run_flag = False + + def obs_size(self): + state = super().get_obs_size() + return state + + def render(self, mode="human"): + super().render() + + def close(self): + super().close() + + def launch_game(self): + launch = super().launch_game() + return launch + + def reset(self): + import copy + obses = copy.deepcopy(super().reset()) + return obses + + def step(self, actions): + reward, terminated, info = super().step(actions) + return super().get_obs(),super().get_reward().value, terminated, info + + def __del__(self): + super().close() + diff --git a/keepaway/envs/multiagentenv.py b/keepaway/envs/multiagentenv.py new file mode 100644 index 00000000..e4d1e4dd --- /dev/null +++ b/keepaway/envs/multiagentenv.py @@ -0,0 +1,56 @@ +class MultiAgentEnv(object): + def step(self, actions): + """Returns reward, terminated, info.""" + raise NotImplementedError + + def get_obs(self): + """Returns all agent observations in a list.""" + raise NotImplementedError + + def get_obs_agent(self, agent_id): + """Returns observation for agent_id.""" + raise NotImplementedError + + def get_obs_size(self): + """Returns the size of the observation.""" + raise NotImplementedError + + def get_state(self): + """Returns the global state.""" + raise NotImplementedError + + def get_state_size(self): + """Returns the size of the global state.""" + raise NotImplementedError + + + def get_avail_actions(self): + """Returns the available actions of all agents in a list.""" + raise NotImplementedError + + def get_avail_agent_actions(self, agent_id): + """Returns the available actions for agent_id.""" + raise NotImplementedError + + def get_total_actions(self): + """Returns the total number of actions an agent could ever take.""" + raise NotImplementedError + + def reset(self): + """Returns initial observations and states.""" + raise NotImplementedError + + def close(self): + raise NotImplementedError + + + def get_env_info(self): + env_info = { + "state_shape": self.get_state_size(), + "obs_shape": self.get_obs_size(), + "cap_shape": self.get_cap_size(), + "n_actions": self.get_total_actions(), + "n_agents": self.n_agents, + "episode_limit": self.episode_limit, + } + return env_info diff --git a/keepaway/envs/policies/.DS_Store b/keepaway/envs/policies/.DS_Store new file mode 100644 index 00000000..131d72d5 Binary files /dev/null and b/keepaway/envs/policies/.DS_Store differ diff --git a/keepaway/envs/policies/__init__.py b/keepaway/envs/policies/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/keepaway/envs/policies/always_hold.py b/keepaway/envs/policies/always_hold.py new file mode 100644 index 00000000..4a24ac65 --- /dev/null +++ b/keepaway/envs/policies/always_hold.py @@ -0,0 +1,24 @@ +""" +Implementation for always-hold policy for keepaway. + +Always Hold policy keepaway adapted from Adaptive Behavior '05 article +* Stone, Sutton, and Kuhlmann. + +""" + + +class AlwaysHoldPolicy(object): + def __init__(self, config=None): + pass + + def get_actions(self, obs, greedy=False): + agent_ids = obs.keys() # {1, 2, 3} + actions = [None] * len(agent_ids) + for id, agent_obs in obs.items(): + if agent_obs is None: + actions[id - 1] = 0 + else: + a = 0 + actions[id - 1] = a + + return actions, {} diff --git a/keepaway/envs/policies/handcoded_agent.py b/keepaway/envs/policies/handcoded_agent.py new file mode 100644 index 00000000..bf4610ce --- /dev/null +++ b/keepaway/envs/policies/handcoded_agent.py @@ -0,0 +1,95 @@ +""" +Implementation for handcoded policy for keepaway. + +Handcoded policy keepaway adapted from Adaptive Behavior '05 article +* Stone, Sutton, and Kuhlmann. + +""" + +import numpy as np + +class HandcodedPolicy(): + def __init__(self, config=None): + """Initializes the policy.""" + if config is not None: + self.num_keepers = config["num_keepers"] + self.num_takers = config["num_takers"] + else: + self.num_keepers = 3 + self.num_takers = 2 + + ## this is same as beta in the paper stone et al. 2005 + self.hold_distance = 90.0 + ## this is same as alpha in the paper stone et al. 2005. + self.dist_weight = 4.0 + + self.distance_threshold = 10.0 + + + def select_agent_action(self, obs, agent_id): + """ + Returns the ac in the keep-away domain. + """ + + scores = [0] * self.num_keepers + + + if isinstance(obs, dict): + obs = obs["state_vars"] + else: + obs = obs + + if self.num_keepers == 3: + start_idx = 7 + elif self.num_keepers == 4: + start_idx = 10 + elif self.num_keepers == 5: + start_idx = 13 + + + last_index = (len(obs) - 1) - self.num_takers + + # set current agent index to a very small value + scores[agent_id - 1] = -1000000.0 + my_distance_to_taker = obs[start_idx : start_idx + self.num_takers] + if my_distance_to_taker[0] > self.distance_threshold: + return 0 + + for i in range(self.num_keepers): + if i == agent_id - 1: + pass + else: + a = self.dist_weight * obs[start_idx + i] + obs[last_index + i] + scores[i] = a + + best = np.argmax(scores) + if scores[best] < self.hold_distance: + return best + else: + return 0 + + def get_actions(self, obs): + + """ Returns the actions for the agents in the keep-away domain. + Hold threshold (alpha) + beta : Dist/Ang ratio (beta) + Args: + obs: dict of observations for each agent. + Returns: + actions: list of actions for each agent. + agent_infos: dict of agent information. + """ + + agent_ids = obs.keys() + + + ## Check if no observations are available. + actions = [0] * len(agent_ids) + + for id, agent_obs in obs.items(): + if agent_obs is None: + actions[id - 1] = 0 + else: + a = self.select_agent_action(agent_obs, id) + actions[id - 1] = a + return actions, {} diff --git a/keepaway/envs/policies/random_agent.py b/keepaway/envs/policies/random_agent.py new file mode 100644 index 00000000..3f8f01b9 --- /dev/null +++ b/keepaway/envs/policies/random_agent.py @@ -0,0 +1,33 @@ +""" +Implementation for random policy for keepaway. + +Random policy keepaway.baseline adapted from Adaptive Behavior '05 article +* Stone, Sutton, and Kuhlmann. + +""" + +import random + + +class RandomPolicy(): + def __init__(self, config=None): + """Initializes the policy.""" + if config is not None: + self.num_keepers = config["num_keepers"] + self.num_takers = config["num_takers"] + + + def get_actions(self, obs): + """ + Returns a random action for each agent. + """ + agents_ids = obs.keys() + actions = [] + for idx in agents_ids: + a = random.randint(0, self.num_keepers) + if a != idx: + actions.append(a) + else: + l = [num for num in range(0, self.num_keepers + 1) if num != idx] + actions.append(random.choice(l)) + return actions, {} diff --git a/keepaway/envs/reward_wrapper.py b/keepaway/envs/reward_wrapper.py new file mode 100644 index 00000000..d8c3a69e --- /dev/null +++ b/keepaway/envs/reward_wrapper.py @@ -0,0 +1,39 @@ + +""" + Reward Shaping Wrapper to learn progressive passing behavior among keepers. + +""" + +import gym + +class CheckpointReward(gym.RewardWrapper): + """ + Reward shaping for the keepaway environment. + + """ + + def __init__(self, env): + gym.RewardWrapper.__init__(self, env) + self._collected_checkpoints = {} + self._num_checkpoints = self.num_keepers + self._checkpoint_reward = 1 + + def reset(self): + self._collected_checkpoints = {} + return self.env.reset() + + ## Test the reward function + def reward(self, reward): + """ Reward is given for a successful pass""" + observation = self.env.observation() + if observation is None: + return reward + + for i in range(self.num_keepers): + keeper = observation[i] + if keeper[0] == 1: + keeper_id = keeper[1] + if keeper_id not in self._collected_checkpoints: + self._collected_checkpoints[keeper_id] = 1 + reward += self._checkpoint_reward + return reward \ No newline at end of file diff --git a/keepaway/examples/.DS_Store b/keepaway/examples/.DS_Store new file mode 100644 index 00000000..e96c06f1 Binary files /dev/null and b/keepaway/examples/.DS_Store differ diff --git a/keepaway/examples/test_agent.py b/keepaway/examples/test_agent.py new file mode 100644 index 00000000..5c6bd14b --- /dev/null +++ b/keepaway/examples/test_agent.py @@ -0,0 +1,95 @@ +import time +import yaml +import argparse +import os +from absl import logging +from keepaway.envs.keepaway_env import KeepawayEnv +from keepaway.envs.policies.random_agent import RandomPolicy +from keepaway.envs.policies.always_hold import AlwaysHoldPolicy +from keepaway.envs.policies.handcoded_agent import HandcodedPolicy +from keepaway.config.game_config import get_config + +from absl import flags, app + + +# Edit the configuration path if needed(this is the default path) +agent_config_path = os.getcwd() + "/config/sample_agent_config.yml" + + +def load_agent_config(file_path): + with open(file_path) as f: + return yaml.safe_load(f) + + +def setup_environment(config): + # print(f"Environment setup with configuration: {config}") + config = get_config()[config] + return config + +def setup_agent(policy): + # print(f"Agent setup with policy: {policy}") + agent_config = policy + return agent_config + + +def test_agent(env_configs, agent_config, nepisode, nsteps): + print(f"nepisode: {nepisode}, nsteps: {nsteps}") + + if agent_config.get('policy') == 'random': + policy = RandomPolicy(env_configs) + elif agent_config.get('policy') == 'always-hold': + policy = AlwaysHoldPolicy(env_configs) + elif agent_config.get('policy') == 'handcoded': + policy = HandcodedPolicy(env_configs) + else: + raise ValueError(f"Unknown policy: {agent_config.get('policy')}") + + env = KeepawayEnv(env_configs) + env.launch_game() + env.render() + time.sleep(1) + for e in range(nepisode): + env.reset() + terminated = False + episode_reward = 0 + env.start() + while not terminated: + obs = env.get_obs() + actions, agent_infos = policy.get_actions(obs) + reward, terminated, info = env.step(actions) + episode_reward += reward + env.close() + +def run(args): + agent_configs = load_agent_config(agent_config_path) + env_config = args.gc + agent_config = agent_configs.get(args.policy, {}) + + if not env_config: + raise ValueError(f"Unknown environment configuration: {args.gc}") + if not agent_config: + raise ValueError(f"Unknown agent configuration: {args.policy}") + + environment = setup_environment(env_config) + agent = setup_agent(agent_config) + + # print(f"Training agent with configuration: {agent}") + # print(f"Training agent with environment configuration: {environment}") + + test_agent(environment, agent, args.nepisode, args.nsteps) + +if __name__ == '__main__': + parser = argparse.ArgumentParser(description='Train an agent with specific configurations.') + parser.add_argument('--gc', type=str, default='3v2', + help='Configuration to use, defines the number of keepers e.g., "3v2", "4v3", or "5v4"') + parser.add_argument('--num_timesteps', type=int, default=int(2e6), + help='Number of timesteps to run for.') + parser.add_argument('--nepisode', type=int, default=5, + help='Number of episodes') + parser.add_argument('--nsteps', type=int, default=128, + help='Number of environment steps per epoch; batch size is nsteps * nenv') + parser.add_argument('--policy', type=str, default='handcoded', + help='Policy to use, e.g., "handcoded", "always-hold", "random"') + + args = parser.parse_args() + run(args) \ No newline at end of file diff --git a/keepaway/examples/test_alwayshold_agent.py b/keepaway/examples/test_alwayshold_agent.py new file mode 100644 index 00000000..b1a8ff45 --- /dev/null +++ b/keepaway/examples/test_alwayshold_agent.py @@ -0,0 +1,43 @@ +from __future__ import absolute_import, division, print_function + +import time +from absl import logging + +logging.set_verbosity(logging.DEBUG) +from keepaway.envs.keepaway_env import KeepawayEnv +from keepaway.envs.policies.always_hold import AlwaysHoldPolicy + +from keepaway.config.game_config import get_config +config = get_config()["3v2"] + +def main(): + env = KeepawayEnv(config) + episodes = 1000000 + env.launch_game() + agents = env.num_keepers + policy = AlwaysHoldPolicy(config) + env.render() + for e in range(episodes): + print(f"Episode {e}") + env.reset() + terminated = False + episode_reward = 0 + env.start() + + while not terminated: + obs = env.get_obs() + actions, agent_infos = policy.get_actions(obs) + # print("actions ", actions) + + reward, terminated, info = env.step(actions) + # print("reward ", reward, "terminated ", terminated, "info ", info) + # print("matrix jfjfj ", env.get_proximity_adj_mat()) + time.sleep(0.15) + episode_reward += reward + + print("closing game") + env.close() + + +if __name__ == "__main__": + main() diff --git a/keepaway/examples/test_handcoded_agent.py b/keepaway/examples/test_handcoded_agent.py new file mode 100644 index 00000000..8175f705 --- /dev/null +++ b/keepaway/examples/test_handcoded_agent.py @@ -0,0 +1,33 @@ +import time +from absl import logging +logging.set_verbosity(logging.DEBUG) +from keepaway.envs.keepaway_env import KeepawayEnv +from keepaway.envs.policies.handcoded_agent import HandcodedPolicy +from keepaway.config.game_config import get_config +config = get_config()["3v2"] + + +def main(): + env = KeepawayEnv(config) + episodes = 20 + env.launch_game() + policy = HandcodedPolicy(config) + env.render() + for e in range(episodes): + print(f"Episode {e}") + env.reset() + terminated = False + episode_reward = 0 + env.start() + while not terminated: + obs = env.get_obs() + actions, agent_infos = policy.get_actions(obs) + reward, terminated, info = env.step(actions) + time.sleep(0.15) + episode_reward += reward + + print("closing game") + env.close() + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/keepaway/examples/test_random_agent.py b/keepaway/examples/test_random_agent.py new file mode 100644 index 00000000..a86ad381 --- /dev/null +++ b/keepaway/examples/test_random_agent.py @@ -0,0 +1,38 @@ +import time +from absl import logging +from keepaway.envs.keepaway_env import KeepawayEnv +from keepaway.envs.policies.random_agent import RandomPolicy +from keepaway.config.game_config import get_config +logging.set_verbosity(logging.DEBUG) + +config = get_config()["3v2"] + +def main(): + env = KeepawayEnv(config) + episodes = 10 + print("Training episodes") + print("launching game") + env.launch_game() + policy = RandomPolicy(config) + env.render() + for e in range(episodes): + print("Episode: {}".format(e)) + env.reset() + terminated = False + episode_reward = 0 + env.start() + + while not terminated: + obs = env.get_obs() + actions, agent_infos = policy.get_actions(obs) + # print(actions) + reward, terminated, info = env.step(actions) + time.sleep(0.15) + episode_reward += reward + + print("closing game") + env.close() + + +if __name__ == "__main__": + main() diff --git a/keepaway/examples/test_wrapper.py b/keepaway/examples/test_wrapper.py new file mode 100644 index 00000000..e6db9366 --- /dev/null +++ b/keepaway/examples/test_wrapper.py @@ -0,0 +1,36 @@ +import time +from absl import logging +from keepaway.envs.keepaway_wrapper import KeepawayWrapper +from keepaway.envs.policies.random_agent import RandomPolicy +from keepaway.config.game_config import get_config +logging.set_verbosity(logging.DEBUG) + + +config = get_config()["3v2"] + + +def main(): + env = KeepawayWrapper(False,config) + episodes = 3 + env.launch_game() + policy = RandomPolicy(config) + env.render() + for e in range(episodes): + print(f"Episode {e}") + env.reset() + terminated = False + episode_reward = 0 + env.start() + while not terminated: + obs = env.get_obs() + actions, agent_infos = policy.get_actions(obs) + obs, reward, terminated, info = env.step(actions) + # print(f"Obs: {obs}" ,f"Reward: {reward}", f"Terminated: {terminated}", f"Info: {info}", f"Actions: {actions}") + time.sleep(0.15) + episode_reward += reward + + print("closing game") + env.close() + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/keepaway/lib/.DS_Store b/keepaway/lib/.DS_Store new file mode 100644 index 00000000..5383488a Binary files /dev/null and b/keepaway/lib/.DS_Store differ diff --git a/keepaway/lib/__init__.py b/keepaway/lib/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/keepaway/lib/action/__init__.py b/keepaway/lib/action/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/lib/action/basic_actions.py b/keepaway/lib/action/basic_actions.py similarity index 97% rename from lib/action/basic_actions.py rename to keepaway/lib/action/basic_actions.py index 31324ac0..47ad1e29 100644 --- a/lib/action/basic_actions.py +++ b/keepaway/lib/action/basic_actions.py @@ -2,16 +2,16 @@ \ file basic_actions.py \ brief basic player actions """ -from lib.debug.debug import log -from lib.player.soccer_action import * +from keepaway.lib.debug.debug import log +from keepaway.lib.player.soccer_action import * from pyrusgeom.soccer_math import * -from lib.rcsc.server_param import ServerParam +from keepaway.lib.rcsc.server_param import ServerParam from pyrusgeom.angle_deg import AngleDeg from typing import TYPE_CHECKING if TYPE_CHECKING: - from lib.player.player_agent import PlayerAgent + from keepaway.lib.player.player_agent import PlayerAgent diff --git a/lib/action/go_to_point.py b/keepaway/lib/action/go_to_point.py similarity index 97% rename from lib/action/go_to_point.py rename to keepaway/lib/action/go_to_point.py index f9c30b1c..5e79ffad 100644 --- a/lib/action/go_to_point.py +++ b/keepaway/lib/action/go_to_point.py @@ -1,11 +1,10 @@ -# from lib.player.player_agent import * -from lib.rcsc.server_param import ServerParam as SP +from keepaway.lib.rcsc.server_param import ServerParam as SP import pyrusgeom.soccer_math as smath from pyrusgeom.geom_2d import * from typing import TYPE_CHECKING if TYPE_CHECKING: - from lib.player.world_model import WorldModel + from keepaway.lib.player.world_model import WorldModel class GoToPoint: _dir_thr: float diff --git a/lib/action/hold_ball.py b/keepaway/lib/action/hold_ball.py similarity index 97% rename from lib/action/hold_ball.py rename to keepaway/lib/action/hold_ball.py index 1ff1850a..663e643b 100644 --- a/lib/action/hold_ball.py +++ b/keepaway/lib/action/hold_ball.py @@ -4,23 +4,23 @@ """ import functools -from lib.debug.debug import log -from lib.debug.level import Level +from keepaway.lib.debug.debug import log +from keepaway.lib.debug.level import Level from pyrusgeom.soccer_math import * from pyrusgeom.angle_deg import AngleDeg -from lib.action.stop_ball import StopBall -from lib.action.basic_actions import TurnToPoint -from lib.player.soccer_action import BodyAction -from lib.rcsc.game_time import GameTime -from lib.rcsc.server_param import ServerParam +from keepaway.lib.action.stop_ball import StopBall +from keepaway.lib.action.basic_actions import TurnToPoint +from keepaway.lib.player.soccer_action import BodyAction +from keepaway.lib.rcsc.game_time import GameTime +from keepaway.lib.rcsc.server_param import ServerParam from pyrusgeom.rect_2d import Rect2D from pyrusgeom.size_2d import Size2D from pyrusgeom.line_2d import Line2D from typing import TYPE_CHECKING if TYPE_CHECKING: - from lib.player.world_model import WorldModel - from lib.player.player_agent import PlayerAgent + from keepaway.lib.player.world_model import WorldModel + from keepaway.lib.player.player_agent import PlayerAgent """ \ struct KeepPoint diff --git a/lib/action/intercept.py b/keepaway/lib/action/intercept.py similarity index 93% rename from lib/action/intercept.py rename to keepaway/lib/action/intercept.py index 4b79342b..cf385b5e 100644 --- a/lib/action/intercept.py +++ b/keepaway/lib/action/intercept.py @@ -1,21 +1,21 @@ import math -from lib.action.basic_actions import TurnToPoint -from lib.action.go_to_point import GoToPoint -from lib.action.intercept_info import InterceptInfo -from lib.action.intercept_table import InterceptTable -from lib.debug.color import Color -from lib.debug.debug import log -from lib.debug.level import Level +from keepaway.lib.action.basic_actions import TurnToPoint +from keepaway.lib.action.go_to_point import GoToPoint +from keepaway.lib.action.intercept_info import InterceptInfo +from keepaway.lib.action.intercept_table import InterceptTable +from keepaway.lib.debug.color import Color +from keepaway.lib.debug.debug import log +from keepaway.lib.debug.level import Level from pyrusgeom.soccer_math import inertia_n_step_distance, bound, calc_first_term_geom_series, min_max from pyrusgeom.vector_2d import Vector2D -from lib.player.object_player import PlayerObject -from lib.rcsc.server_param import ServerParam +from keepaway.lib.player.object_player import PlayerObject +from keepaway.lib.rcsc.server_param import ServerParam from typing import TYPE_CHECKING if TYPE_CHECKING: - from lib.player.player_agent import PlayerAgent - from lib.player.world_model import WorldModel + from keepaway.lib.player.player_agent import PlayerAgent + from keepaway.lib.player.world_model import WorldModel class Intercept: @@ -30,10 +30,11 @@ def execute(self, agent: 'PlayerAgent'): if not wm.ball().pos_valid(): return False - if self.do_kickable_opponent_check(agent): - return True + # if self.do_kickable_opponent_check(agent): + # return True - table = wm.intercept_table() + table = wm.intercept_table() # what is here? + # print (" intercept : ", table.self_reach_cycle()) if table.self_reach_cycle() > 100: final_point = wm.ball().inertia_final_point() log.sw_log().intercept().add_text( @@ -42,14 +43,19 @@ def execute(self, agent: 'PlayerAgent'): center=final_point, r=0.5, color=Color(string='red')) + print ("GoToPoint in intercept ", final_point) GoToPoint(final_point, 2, ServerParam.i().max_dash_power()).execute(agent) return True best_intercept: InterceptInfo = self.get_best_intercept(wm, table) + + print ("Best Intercept : ", best_intercept) + target_point = wm.ball().inertia_point(best_intercept.reach_cycle()) if best_intercept.dash_cycle() == 0: + print("best_intercept.dash_cycle() == 0") face_point = self._face_point.copy() if not face_point.is_valid(): face_point.assign(50.5, wm.self().pos().y() * 0.75) @@ -60,11 +66,13 @@ def execute(self, agent: 'PlayerAgent'): center=face_point, r=0.5, color=Color(string='red')) + print ("Turning to Point in intercept ", final_point) TurnToPoint(face_point, best_intercept.reach_cycle()).execute(agent) return True if best_intercept.turn_cycle() > 0: + print("Turning to Point in intercept") my_inertia = wm.self().inertia_point(best_intercept.reach_cycle()) target_angle = (target_point - my_inertia).th() if best_intercept.dash_power() < 0: @@ -78,6 +86,7 @@ def execute(self, agent: 'PlayerAgent'): return True if self._save_recovery and not wm.self().stamina_model().capacity_is_empty(): + print("save recovery") consumed_stamina = best_intercept.dash_power() if best_intercept.dash_power() < 0: consumed_stamina *= -2 @@ -95,6 +104,8 @@ def execute(self, agent: 'PlayerAgent'): center=target_point, r=0.5, color=Color(string='red')) + + print ("Do inertia dash in intercept ", target_point) return self.do_inertia_dash(agent, target_point, best_intercept) @@ -136,8 +147,12 @@ def get_best_intercept(self, wm: 'WorldModel', goal_pos = Vector2D(65, 0) our_goal_pos = Vector2D(-SP.pitch_half_length(), 0) - max_pitch_x = SP.pitch_half_length() - 1 - max_pitch_y = SP.pitch_half_width() - 1 + # max_pitch_x = SP.pitch_half_length() - 1 + # max_pitch_y = SP.pitch_half_width() - 1 + + max_pitch_x = SP.keepaway_length()/2 - 1 + max_pitch_y = SP.keepaway_width()/2 - 1 + penalty_x = SP.our_penalty_area_line_x() penalty_y = SP.penalty_area_half_width() speed_max = wm.self().player_type().real_speed_max() * 0.9 diff --git a/lib/action/intercept_info.py b/keepaway/lib/action/intercept_info.py similarity index 100% rename from lib/action/intercept_info.py rename to keepaway/lib/action/intercept_info.py diff --git a/lib/action/intercept_player.py b/keepaway/lib/action/intercept_player.py similarity index 97% rename from lib/action/intercept_player.py rename to keepaway/lib/action/intercept_player.py index d52d1664..0576c4c8 100644 --- a/lib/action/intercept_player.py +++ b/keepaway/lib/action/intercept_player.py @@ -3,13 +3,13 @@ from pyrusgeom.angle_deg import AngleDeg from pyrusgeom.soccer_math import bound, inertia_n_step_point from pyrusgeom.vector_2d import Vector2D -from lib.player.object_player import PlayerObject -from lib.rcsc.player_type import PlayerType -from lib.rcsc.server_param import ServerParam +from keepaway.lib.player.object_player import PlayerObject +from keepaway.lib.rcsc.player_type import PlayerType +from keepaway.lib.rcsc.server_param import ServerParam from typing import TYPE_CHECKING if TYPE_CHECKING: - from lib.player.world_model import WorldModel + from keepaway.lib.player.world_model import WorldModel class PlayerIntercept: diff --git a/lib/action/intercept_self.py b/keepaway/lib/action/intercept_self.py similarity index 99% rename from lib/action/intercept_self.py rename to keepaway/lib/action/intercept_self.py index 9eaadc6a..c518e2a1 100644 --- a/lib/action/intercept_self.py +++ b/keepaway/lib/action/intercept_self.py @@ -1,21 +1,21 @@ from math import ceil -from lib.action.intercept_info import InterceptInfo -from lib.debug.debug import log +from keepaway.lib.action.intercept_info import InterceptInfo +from keepaway.lib.debug.debug import log from pyrusgeom.angle_deg import AngleDeg from pyrusgeom.line_2d import Line2D from pyrusgeom.segment_2d import Segment2D from pyrusgeom.soccer_math import bound, calc_first_term_geom_series, min_max from pyrusgeom.vector_2d import Vector2D -from lib.player.object_ball import BallObject -from lib.player.object_player import PlayerObject -from lib.player.stamina_model import StaminaModel -from lib.rcsc.player_type import PlayerType -from lib.rcsc.server_param import ServerParam +from keepaway.lib.player.object_ball import BallObject +from keepaway.lib.player.object_player import PlayerObject +from keepaway.lib.player.stamina_model import StaminaModel +from keepaway.lib.rcsc.player_type import PlayerType +from keepaway.lib.rcsc.server_param import ServerParam from typing import TYPE_CHECKING if TYPE_CHECKING: - from lib.player.world_model import WorldModel + from keepaway.lib.player.world_model import WorldModel control_area_buf = 0.15 diff --git a/lib/action/intercept_table.py b/keepaway/lib/action/intercept_table.py similarity index 93% rename from lib/action/intercept_table.py rename to keepaway/lib/action/intercept_table.py index f8c90fa7..4e5fe679 100644 --- a/lib/action/intercept_table.py +++ b/keepaway/lib/action/intercept_table.py @@ -1,19 +1,19 @@ from typing import Union -from lib.action.intercept_info import InterceptInfo -from lib.action.intercept_player import PlayerIntercept -from lib.action.intercept_self import SelfIntercept -from lib.debug.color import Color -from lib.debug.debug import log -from lib.debug.level import Level +from keepaway.lib.action.intercept_info import InterceptInfo +from keepaway.lib.action.intercept_player import PlayerIntercept +from keepaway.lib.action.intercept_self import SelfIntercept +from keepaway.lib.debug.color import Color +from keepaway.lib.debug.debug import log +from keepaway.lib.debug.level import Level from pyrusgeom.vector_2d import Vector2D -from lib.player.object_player import PlayerObject -from lib.rcsc.game_time import GameTime -from lib.rcsc.server_param import ServerParam -from lib.rcsc.types import GameModeType +from keepaway.lib.player.object_player import PlayerObject +from keepaway.lib.rcsc.game_time import GameTime +from keepaway.lib.rcsc.server_param import ServerParam +from keepaway.lib.rcsc.types import GameModeType from typing import TYPE_CHECKING if TYPE_CHECKING: - from lib.player.world_model import WorldModel + from keepaway.lib.player.world_model import WorldModel class InterceptTable: @@ -127,8 +127,12 @@ def clear(self): def create_ball_cache(self, wm): SP = ServerParam.i() - pitch_max_x = SP.pitch_half_length() + 5 - pitch_max_y = SP.pitch_half_width() + 5 + # pitch_max_x = SP.pitch_half_length() + 5 + # pitch_max_y = SP.pitch_half_width() + 5 + + pitch_max_x = SP.keepaway_length()/2 + 5 + pitch_max_y = SP.keepaway_width()/2 + 5 + ball_decay = SP.ball_decay() ball_pos: Vector2D = wm.ball().pos() diff --git a/lib/action/kick_table.py b/keepaway/lib/action/kick_table.py similarity index 98% rename from lib/action/kick_table.py rename to keepaway/lib/action/kick_table.py index e7059e75..be6d5204 100644 --- a/lib/action/kick_table.py +++ b/keepaway/lib/action/kick_table.py @@ -4,13 +4,13 @@ """ import functools -from lib.debug.debug import log +from keepaway.lib.debug.debug import log # from typing import List # from enum import Enum -from lib.debug.level import Level -from lib.rcsc.player_type import PlayerType -from lib.rcsc.server_param import ServerParam +from keepaway.lib.debug.level import Level +from keepaway.lib.rcsc.player_type import PlayerType +from keepaway.lib.rcsc.server_param import ServerParam from pyrusgeom.soccer_math import * from pyrusgeom.ray_2d import Ray2D from pyrusgeom.circle_2d import Circle2D @@ -18,12 +18,12 @@ from pyrusgeom.rect_2d import Rect2D from pyrusgeom.size_2d import Size2D from pyrusgeom.math_values import EPSILON -from lib.rcsc.game_time import * +from keepaway.lib.rcsc.game_time import * from typing import TYPE_CHECKING if TYPE_CHECKING: - from lib.player.world_model import WorldModel + from keepaway.lib.player.world_model import WorldModel """ @@ -466,8 +466,11 @@ def update_state(self, world: 'WorldModel'): def create_state_cache(self, world: 'WorldModel'): param = ServerParam.i() - pitch = Rect2D(Vector2D(- param.pitch_half_length(), - param.pitch_half_width()), Size2D(param.pitch_length(), - param.pitch_width())) + # pitch = Rect2D(Vector2D(- param.pitch_half_length(), - param.pitch_half_width()), Size2D(param.pitch_length(), + # param.pitch_width())) + ## TODO - changed to keepaway sized pitch + pitch = world.keepaway_rect() + self_type = world.self().player_type() near_dist = calc_near_dist(self_type) mid_dist = calc_mid_dist(self_type) diff --git a/lib/action/neck_body_to_ball.py b/keepaway/lib/action/neck_body_to_ball.py similarity index 67% rename from lib/action/neck_body_to_ball.py rename to keepaway/lib/action/neck_body_to_ball.py index 7b53cd3f..98d9a719 100644 --- a/lib/action/neck_body_to_ball.py +++ b/keepaway/lib/action/neck_body_to_ball.py @@ -1,14 +1,14 @@ from pyrusgeom.angle_deg import AngleDeg -from lib.action.neck_body_to_point import NeckBodyToPoint -from lib.action.scan_field import ScanField -from lib.debug.debug import log -from lib.player.soccer_action import NeckAction +from keepaway.lib.action.neck_body_to_point import NeckBodyToPoint +from keepaway.lib.action.scan_field import ScanField +from keepaway.lib.debug.debug import log +from keepaway.lib.player.soccer_action import NeckAction from typing import TYPE_CHECKING, Union if TYPE_CHECKING: - from lib.player.player_agent import PlayerAgent + from keepaway.lib.player.player_agent import PlayerAgent class NeckBodyToBall(NeckAction): diff --git a/lib/action/neck_body_to_point.py b/keepaway/lib/action/neck_body_to_point.py similarity index 84% rename from lib/action/neck_body_to_point.py rename to keepaway/lib/action/neck_body_to_point.py index 89b26e3c..32fe37af 100644 --- a/lib/action/neck_body_to_point.py +++ b/keepaway/lib/action/neck_body_to_point.py @@ -2,16 +2,16 @@ from pyrusgeom.soccer_math import bound from pyrusgeom.vector_2d import Vector2D -from lib.action.neck_turn_to_relative import NeckTurnToRelative -from lib.debug.debug import log -from lib.player.soccer_action import NeckAction +from keepaway.lib.action.neck_turn_to_relative import NeckTurnToRelative +from keepaway.lib.debug.debug import log +from keepaway.lib.player.soccer_action import NeckAction from typing import TYPE_CHECKING, Union -from lib.rcsc.server_param import ServerParam +from keepaway.lib.rcsc.server_param import ServerParam if TYPE_CHECKING: - from lib.player.player_agent import PlayerAgent + from keepaway.lib.player.player_agent import PlayerAgent class NeckBodyToPoint(NeckAction): diff --git a/lib/action/neck_scan_field.py b/keepaway/lib/action/neck_scan_field.py similarity index 93% rename from lib/action/neck_scan_field.py rename to keepaway/lib/action/neck_scan_field.py index 4e0b92e5..8906758a 100644 --- a/lib/action/neck_scan_field.py +++ b/keepaway/lib/action/neck_scan_field.py @@ -1,16 +1,16 @@ -from lib.debug.debug import log -from lib.debug.level import Level -from lib.player.soccer_action import NeckAction -from lib.rcsc.game_time import GameTime -from lib.rcsc.server_param import ServerParam -from lib.rcsc.types import GameModeType, ViewWidth -from lib.player.world_model import WorldModel +from keepaway.lib.debug.debug import log +from keepaway.lib.debug.level import Level +from keepaway.lib.player.soccer_action import NeckAction +from keepaway.lib.rcsc.game_time import GameTime +from keepaway.lib.rcsc.server_param import ServerParam +from keepaway.lib.rcsc.types import GameModeType, ViewWidth +from keepaway.lib.player.world_model import WorldModel from pyrusgeom.geom_2d import AngleDeg, Rect2D, Size2D, Vector2D from typing import TYPE_CHECKING if TYPE_CHECKING: - from lib.player.player_agent import PlayerAgent + from keepaway.lib.player.player_agent import PlayerAgent @@ -27,7 +27,7 @@ def __init__(self): super().__init__() def execute(self, agent: 'PlayerAgent'): - from lib.action.neck_scan_players import NeckScanPlayers + from keepaway.lib.action.neck_scan_players import NeckScanPlayers log.debug_client().add_message('NeckScanField/') wm = agent.world() ef = agent.effector() diff --git a/lib/action/neck_scan_players.py b/keepaway/lib/action/neck_scan_players.py similarity index 91% rename from lib/action/neck_scan_players.py rename to keepaway/lib/action/neck_scan_players.py index 866a6dfb..bd151a26 100644 --- a/lib/action/neck_scan_players.py +++ b/keepaway/lib/action/neck_scan_players.py @@ -1,20 +1,20 @@ from math import exp -from lib.action.neck_scan_field import NeckScanField -from lib.debug.debug import log -from lib.debug.level import Level -from lib.player.soccer_action import NeckAction -from lib.rcsc.game_time import GameTime -from lib.rcsc.server_param import ServerParam -from lib.player.world_model import WorldModel +from keepaway.lib.action.neck_scan_field import NeckScanField +from keepaway.lib.debug.debug import log +from keepaway.lib.debug.level import Level +from keepaway.lib.player.soccer_action import NeckAction +from keepaway.lib.rcsc.game_time import GameTime +from keepaway.lib.rcsc.server_param import ServerParam +from keepaway.lib.player.world_model import WorldModel from pyrusgeom.soccer_math import bound from pyrusgeom.geom_2d import Vector2D, AngleDeg from typing import TYPE_CHECKING -from lib.rcsc.types import ViewWidth +from keepaway.lib.rcsc.types import ViewWidth if TYPE_CHECKING: - from lib.player.player_agent import PlayerAgent + from keepaway.lib.player.player_agent import PlayerAgent class NeckScanPlayers(NeckAction): DEBUG = True @@ -141,10 +141,10 @@ def calculate_score(wm: WorldModel, next_self_pos: Vector2D, left_angle: AngleDe if p.side() == wm.our_side() and (p.pos().x() > wm.ball().pos().x() - 10 or p.pos().x() > 30): pos_count *=2 - base_val = pos_count**2 + keepaway.base_val = pos_count**2 rate = exp(-(p.dist_from_self() ** 2) / (2*(20**2))) - score += base_val * rate + score += keepaway.base_val * rate buf = min((angle-left_angle).abs(), (angle-right_angle).abs()) if buf < view_buffer: diff --git a/lib/action/neck_turn_to_ball.py b/keepaway/lib/action/neck_turn_to_ball.py similarity index 91% rename from lib/action/neck_turn_to_ball.py rename to keepaway/lib/action/neck_turn_to_ball.py index 94d188c0..b36dea81 100644 --- a/lib/action/neck_turn_to_ball.py +++ b/keepaway/lib/action/neck_turn_to_ball.py @@ -1,17 +1,17 @@ from pyrusgeom.angle_deg import AngleDeg from pyrusgeom.soccer_math import bound -from lib.action.basic_actions import NeckAction +from keepaway.lib.action.basic_actions import NeckAction from typing import TYPE_CHECKING -from lib.action.neck_scan_field import NeckScanField -from lib.action.neck_scan_players import NeckScanPlayers -from lib.debug.debug import log -from lib.rcsc.server_param import ServerParam +from keepaway.lib.action.neck_scan_field import NeckScanField +from keepaway.lib.action.neck_scan_players import NeckScanPlayers +from keepaway.lib.debug.debug import log +from keepaway.lib.rcsc.server_param import ServerParam if TYPE_CHECKING: - from lib.player.player_agent import PlayerAgent + from keepaway.lib.player.player_agent import PlayerAgent class NeckTurnToBall(NeckAction): def __init__(self): diff --git a/lib/action/neck_turn_to_ball_or_scan.py b/keepaway/lib/action/neck_turn_to_ball_or_scan.py similarity index 77% rename from lib/action/neck_turn_to_ball_or_scan.py rename to keepaway/lib/action/neck_turn_to_ball_or_scan.py index 48bb6228..126d7b42 100644 --- a/lib/action/neck_turn_to_ball_or_scan.py +++ b/keepaway/lib/action/neck_turn_to_ball_or_scan.py @@ -1,14 +1,14 @@ -from lib.action.neck_scan_field import NeckScanField -from lib.action.neck_turn_to_ball import NeckTurnToBall -from lib.debug.debug import log -from lib.player.soccer_action import NeckAction +from keepaway.lib.action.neck_scan_field import NeckScanField +from keepaway.lib.action.neck_turn_to_ball import NeckTurnToBall +from keepaway.lib.debug.debug import log +from keepaway.lib.player.soccer_action import NeckAction from typing import TYPE_CHECKING -from lib.rcsc.server_param import ServerParam +from keepaway.lib.rcsc.server_param import ServerParam if TYPE_CHECKING: - from lib.player.player_agent import PlayerAgent + from keepaway.lib.player.player_agent import PlayerAgent class NeckTurnToBallOrScan(NeckAction): diff --git a/lib/action/neck_turn_to_point.py b/keepaway/lib/action/neck_turn_to_point.py similarity index 76% rename from lib/action/neck_turn_to_point.py rename to keepaway/lib/action/neck_turn_to_point.py index 2e3ea01b..f6fcf309 100644 --- a/lib/action/neck_turn_to_point.py +++ b/keepaway/lib/action/neck_turn_to_point.py @@ -1,15 +1,15 @@ from pyrusgeom.vector_2d import Vector2D -from lib.action.neck_scan_field import NeckScanField -from lib.debug.debug import log -from lib.player.soccer_action import NeckAction +from keepaway.lib.action.neck_scan_field import NeckScanField +from keepaway.lib.debug.debug import log +from keepaway.lib.player.soccer_action import NeckAction from typing import TYPE_CHECKING -from lib.rcsc.server_param import ServerParam +from keepaway.lib.rcsc.server_param import ServerParam if TYPE_CHECKING: - from lib.player.player_agent import PlayerAgent + from keepaway.lib.player.player_agent import PlayerAgent class NeckTurnToPoint(NeckAction): diff --git a/lib/action/neck_turn_to_relative.py b/keepaway/lib/action/neck_turn_to_relative.py similarity index 77% rename from lib/action/neck_turn_to_relative.py rename to keepaway/lib/action/neck_turn_to_relative.py index 567d9b7d..6d88a8e2 100644 --- a/lib/action/neck_turn_to_relative.py +++ b/keepaway/lib/action/neck_turn_to_relative.py @@ -1,11 +1,11 @@ from pyrusgeom.angle_deg import AngleDeg -from lib.player.soccer_action import NeckAction +from keepaway.lib.player.soccer_action import NeckAction from typing import TYPE_CHECKING, Union if TYPE_CHECKING: - from lib.player.player_agent import PlayerAgent + from keepaway.lib.player.player_agent import PlayerAgent class NeckTurnToRelative(NeckAction): diff --git a/keepaway/lib/action/scan_field.py b/keepaway/lib/action/scan_field.py new file mode 100644 index 00000000..0f4e2e45 --- /dev/null +++ b/keepaway/lib/action/scan_field.py @@ -0,0 +1,72 @@ +from pyrusgeom.vector_2d import Vector2D + +from keepaway.lib.action.neck_body_to_point import NeckBodyToPoint +from keepaway.lib.action.neck_turn_to_relative import NeckTurnToRelative +from keepaway.lib.action.view_wide import ViewWide +from keepaway.lib.debug.debug import log +from keepaway.lib.player.soccer_action import BodyAction + +from typing import TYPE_CHECKING + +from keepaway.lib.rcsc.types import ViewWidth + +if TYPE_CHECKING: + from keepaway.lib.player.player_agent import PlayerAgent + + +class ScanField(BodyAction): + def __init__(self): + pass + + def execute(self, agent: "PlayerAgent"): + log.debug_client().add_message("ScanField/") + wm = agent.world() + if not wm.self().pos_valid(): + agent.do_turn(60.0) + agent.set_neck_action(NeckTurnToRelative(0)) + return True + + if wm.ball().pos_valid(): + self.scan_all_field(agent) + return True + + self.find_ball(agent) + return True + + def find_ball(self, agent: "PlayerAgent"): + wm = agent.world() + # print(wm.ball().pos()) + # print("find ball/") + + if agent.effector().queued_next_view_width() is not ViewWidth.WIDE: + agent.set_view_action(ViewWide()) + + my_next = wm.self().pos() + wm.self().vel() + face_angle = ( + (wm.ball().seen_pos() - my_next).th() + if wm.ball().seen_pos().is_valid() + else (my_next * -1).th() + ) + + search_flag = wm.ball().lost_count() // 3 + if search_flag % 2 == 1: + face_angle += 180.0 + + face_point = my_next + Vector2D(r=10, a=face_angle) + NeckBodyToPoint(face_point).execute(agent) + + def scan_all_field(self, agent: "PlayerAgent"): + wm = agent.world() + # print(wm.ball().pos()) + # print("find ball/") + + if agent.effector().queued_next_view_width() is not ViewWidth.WIDE: + agent.set_view_action(ViewWide()) + + turn_moment = ( + wm.self().view_width().width() + + agent.effector().queued_next_view_width().width() + ) + turn_moment /= 2 + agent.do_turn(turn_moment) + agent.set_neck_action(NeckTurnToRelative(wm.self().neck())) diff --git a/lib/action/smart_kick.py b/keepaway/lib/action/smart_kick.py similarity index 88% rename from lib/action/smart_kick.py rename to keepaway/lib/action/smart_kick.py index 35895ad7..c9195537 100644 --- a/lib/action/smart_kick.py +++ b/keepaway/lib/action/smart_kick.py @@ -2,21 +2,21 @@ \ file smart_kick.py \ brief smart kick action class file. """ -from lib.debug.debug import log -from lib.player.soccer_action import * -from lib.action.kick_table import KickTable, Sequence -from lib.action.stop_ball import StopBall -from lib.action.hold_ball import HoldBall -from lib.debug.level import Level -from lib.rcsc.server_param import ServerParam +from keepaway.lib.debug.debug import log +from keepaway.lib.player.soccer_action import * +from keepaway.lib.action.kick_table import KickTable, Sequence +from keepaway.lib.action.stop_ball import StopBall +from keepaway.lib.action.hold_ball import HoldBall +from keepaway.lib.debug.level import Level +from keepaway.lib.rcsc.server_param import ServerParam from pyrusgeom.soccer_math import * from typing import TYPE_CHECKING if TYPE_CHECKING: - from lib.player.player_agent import PlayerAgent + from keepaway.lib.player.player_agent import PlayerAgent -# from lib.player.player_agent import * +# from keepaway.lib.player.player_agent import * # from pyrusgeom.soccer_math import * @@ -51,6 +51,7 @@ def execute(self, agent: 'PlayerAgent'): log.os_log().info("-- NonValidBall -> StopBall --") log.sw_log().kick().add_text("unknown ball vel") return StopBall().execute(agent) + first_speed = min(self._first_speed, ServerParam.i().ball_speed_max()) first_speed_thr = max(0.0, self._first_speed_thr) max_step = max(1, self._max_step) @@ -60,6 +61,9 @@ def execute(self, agent: 'PlayerAgent'): first_speed_thr, max_step, self._sequence) + + print("sequence , ", ans) + if ans[0] and SmartKick.debug_print_DEBUG: log.os_log().info(f"Smart kick : {ans[0]} seq -> speed : {ans[1].speed_} power : {ans[1].power_} score : {ans[1].score_} flag : {ans[1].flag_} next_pos : {ans[1].pos_list_[0]} {len(ans[1].pos_list_)} step {ans[1].pos_list_}") log.sw_log().kick().add_text(f"Smart kick : {ans[0]} seq -> speed : {ans[1].speed_} power : {ans[1].power_} score : {ans[1].score_} flag : {ans[1].flag_} next_pos : {ans[1].pos_list_[0]} {len(ans[1].pos_list_)} step {ans[1].pos_list_}") @@ -73,6 +77,8 @@ def execute(self, agent: 'PlayerAgent'): log.os_log().debug(f"Kick Vel : {vel}, Kick Power : {kick_accel.r() / wm.self().kick_rate()}, Kick Angle : {kick_accel.th() - wm.self().body()}") log.sw_log().kick().add_text(f"Kick Vel : {vel}, Kick Power : {kick_accel.r() / wm.self().kick_rate()}, Kick Angle : {kick_accel.th() - wm.self().body()}") + # print("Performing agent kick ") + agent.do_kick(kick_accel.r() / wm.self().kick_rate(), kick_accel.th() - wm.self().body()) if SmartKick.debug_print_DEBUG: diff --git a/lib/action/stop_ball.py b/keepaway/lib/action/stop_ball.py similarity index 94% rename from lib/action/stop_ball.py rename to keepaway/lib/action/stop_ball.py index 303a2468..e1a57ecb 100644 --- a/lib/action/stop_ball.py +++ b/keepaway/lib/action/stop_ball.py @@ -3,15 +3,15 @@ \ brief kick the ball to keep a current positional relation. """ -from lib.player.soccer_action import * +from keepaway.lib.player.soccer_action import * from pyrusgeom.soccer_math import * from pyrusgeom.angle_deg import AngleDeg -from lib.rcsc.server_param import ServerParam +from keepaway.lib.rcsc.server_param import ServerParam from typing import TYPE_CHECKING if TYPE_CHECKING: - from lib.player.world_model import WorldModel - from lib.player.player_agent import PlayerAgent + from keepaway.lib.player.world_model import WorldModel + from keepaway.lib.player.player_agent import PlayerAgent """ \ class Body_StopBall \ brief stop the ball, possible as. diff --git a/lib/action/turn_to_ball.py b/keepaway/lib/action/turn_to_ball.py similarity index 67% rename from lib/action/turn_to_ball.py rename to keepaway/lib/action/turn_to_ball.py index 2167da8a..3334eac7 100644 --- a/lib/action/turn_to_ball.py +++ b/keepaway/lib/action/turn_to_ball.py @@ -1,16 +1,18 @@ -from lib.action.turn_to_point import TurnToPoint +from keepaway.lib.action.turn_to_point import TurnToPoint from typing import TYPE_CHECKING + if TYPE_CHECKING: - from lib.player.player_agent import PlayerAgent + from keepaway.lib.player.player_agent import PlayerAgent + class TurnToBall: def __init__(self, cycle: int = 1) -> None: self._cycle: int = cycle - - def execute(self, agent: 'PlayerAgent'): + + def execute(self, agent: "PlayerAgent"): if not agent.world().ball().pos_valid(): return False - + ball_point = agent.world().ball().inertia_point(self._cycle) - return TurnToPoint(ball_point, self._cycle).execute(agent) \ No newline at end of file + return TurnToPoint(ball_point, self._cycle).execute(agent) diff --git a/lib/action/turn_to_point.py b/keepaway/lib/action/turn_to_point.py similarity index 92% rename from lib/action/turn_to_point.py rename to keepaway/lib/action/turn_to_point.py index 10623e57..1ac5fc76 100644 --- a/lib/action/turn_to_point.py +++ b/keepaway/lib/action/turn_to_point.py @@ -2,7 +2,7 @@ from typing import TYPE_CHECKING if TYPE_CHECKING: - from lib.player.player_agent import PlayerAgent + from keepaway.lib.player.player_agent import PlayerAgent class TurnToPoint: def __init__(self, point: Vector2D, cycle: int = 1) -> None: diff --git a/lib/action/view_wide.py b/keepaway/lib/action/view_wide.py similarity index 52% rename from lib/action/view_wide.py rename to keepaway/lib/action/view_wide.py index ae3bbf4d..dafab8dd 100644 --- a/lib/action/view_wide.py +++ b/keepaway/lib/action/view_wide.py @@ -1,9 +1,9 @@ -from lib.player.soccer_action import ViewAction -from lib.rcsc.types import ViewWidth +from keepaway.lib.player.soccer_action import ViewAction +from keepaway.lib.rcsc.types import ViewWidth from typing import TYPE_CHECKING if TYPE_CHECKING: - from lib.player.player_agent import PlayerAgent + from keepaway.lib.player.player_agent import PlayerAgent class ViewWide(ViewAction): diff --git a/keepaway/lib/coach/__init__.py b/keepaway/lib/coach/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/lib/coach/coach_agent.py b/keepaway/lib/coach/coach_agent.py similarity index 66% rename from lib/coach/coach_agent.py rename to keepaway/lib/coach/coach_agent.py index a5cf41a4..0b946fde 100644 --- a/lib/coach/coach_agent.py +++ b/keepaway/lib/coach/coach_agent.py @@ -1,25 +1,37 @@ import logging import time -from lib.debug.debug import log -from lib.messenger.free_form_messenger import FreeFormMessenger -from lib.messenger.messenger import Messenger -from lib.network.udp_socket import IPAddress -from lib.coach.gloabl_world_model import GlobalWorldModel -from lib.player.soccer_agent import SoccerAgent -from lib.player_command.coach_command import CoachChangePlayerTypeCommand, CoachCommand, CoachDoneCommand, \ - CoachEyeCommand, CoachLookCommand, \ - CoachFreeFormMessageCommand, CoachInitCommand, CoachSendCommands, CoachTeamnameCommand -from lib.rcsc.game_mode import GameMode -from lib.rcsc.game_time import GameTime -from lib.rcsc.server_param import ServerParam -from lib.rcsc.types import HETERO_DEFAULT, HETERO_UNKNOWN, GameModeType - -import team_config +from keepaway.lib.debug.debug import log +from keepaway.lib.messenger.free_form_messenger import FreeFormMessenger +from keepaway.lib.messenger.messenger import Messenger +from keepaway.lib.network.udp_socket import IPAddress +from keepaway.lib.coach.gloabl_world_model import GlobalWorldModel +from keepaway.lib.player.world_model import WorldModel +from keepaway.lib.player.soccer_agent import SoccerAgent +from keepaway.lib.player_command.coach_command import ( + CoachChangePlayerTypeCommand, + CoachCommand, + CoachDoneCommand, + CoachEyeCommand, + CoachLookCommand, + CoachFreeFormMessageCommand, + CoachInitCommand, + CoachSendCommands, + CoachTeamnameCommand, + CoachCheckBallCommand, +) +from keepaway.lib.rcsc.game_mode import GameMode +from keepaway.lib.rcsc.game_time import GameTime +from keepaway.lib.rcsc.server_param import ServerParam +from keepaway.lib.rcsc.types import HETERO_DEFAULT, HETERO_UNKNOWN, GameModeType + +# from keepaway.config import team_config +from keepaway.config from keepaway.config import team_config # TODO PLAYER PARAMS? + class CoachAgent(SoccerAgent): def __init__(self): super().__init__() @@ -28,20 +40,27 @@ def __init__(self): self._current_time: GameTime = GameTime(-1, 0) self._game_mode: GameMode = GameMode() self._world: GlobalWorldModel = GlobalWorldModel() + # self._world: WorldModel = WorldModel("full") self._is_synch_mode: bool = True self._last_body_command: list[CoachCommand] = [] self._free_from_messages: list[FreeFormMessenger] = [] def send_init_command(self): # TODO check reconnection - # TODO make config class for these data - com = CoachInitCommand(team_config.TEAM_NAME, team_config.COACH_VERSION) + # com = CoachInitCommand(team_config.TEAM_NAME, team_config.COACH_VERSION) + + com = CoachInitCommand("keepers", team_config.COACH_VERSION) + + print(com.str()) if self._client.send_message(com.str()) <= 0: + print("ERROR failed to connect to server") + log.os_log().error("ERROR failed to connect to server") self._client.set_server_alive(False) return False + return True def send_bye_command(self): if self._client.is_server_alive() is True: @@ -66,7 +85,7 @@ def see_parser(self, message: str): self.world().update_after_see(self._current_time) def parse_cycle_info(self, message: str, by_see_global: bool): - cycle = int(message.split(' ')[1]) + cycle = int(message.split(" ")[1]) self.update_current_time(cycle, by_see_global) def update_current_time(self, new_time: int, by_see_global: bool): @@ -75,20 +94,20 @@ def update_current_time(self, new_time: int, by_see_global: bool): self._current_time.assign(new_time, 0) else: if by_see_global: - self._current_time.assign(self._current_time.cycle(), - self._current_time.stopped_cycle() + 1) + self._current_time.assign( + self._current_time.cycle(), + self._current_time.stopped_cycle() + 1, + ) else: self._current_time.assign(new_time, 0) def hear_parser(self, message: str): self.parse_cycle_info(message, False) - _, cycle, sender = tuple( - message.split(" ")[:3] - ) + _, cycle, sender = tuple(message.split(" ")[:3]) cycle = int(cycle) - if sender[0].isnumeric() or sender[0] == '-': # PLAYER MESSAGE + if sender[0].isnumeric() or sender[0] == "-": # PLAYER MESSAGE self.hear_player_parser(message) elif sender == "referee": self.hear_referee_parser(message) @@ -118,43 +137,79 @@ def hear_referee_parser(self, message: str): # TODO FULL STATE WORLD update def analyze_change_player_type(self, msg: str): - data = msg.strip('()').split(' ') + data = msg.strip("()").split(" ") n = len(data) if n == 4: pass elif n == 3: - unum, type = int(data[1]), int(data[2].removesuffix(')\x00')) + unum, type = int(data[1]), int(data[2].removesuffix(")\x00")) self.world().change_player_type(self.world().our_side(), unum, type) elif n == 2: - unum = int(data[1].removesuffix(')\x00')) - self.world().change_player_type(self.world().their_side(), unum, HETERO_UNKNOWN) + unum = int(data[1].removesuffix(")\x00")) + self.world().change_player_type( + self.world().their_side(), unum, HETERO_UNKNOWN + ) def handle_start(self): + # print(self._client) + # print(team_config.COACH_PORT) + if self._client is None: return False if team_config.COACH_VERSION < 18: - log.os_log().error("PYRUS2D base code does not support coach version less than 18.") + log.os_log().error( + "PYRUS2D keepaway.base code does not support coach version less than 18." + ) + print("PYRUS2D keepaway.base code does not support coach version less than 18.") self._client.set_server_alive(False) return False # TODO check for config.host not empty - if not self._client.connect_to(IPAddress(team_config.HOST, team_config.COACH_PORT)): + # print(self._client.connect_to(IPAddress(team_config.HOST, 6002))) + + if not self._client.connect_to( + IPAddress(team_config.HOST, team_config.COACH_PORT) + ): log.os_log().error("ERROR failed to connect to server") + print("ERROR failed to connect to server") self._client.set_server_alive(False) return False + # print("coach agent handle start 2") + # print(self.send_init_command()) + if not self.send_init_command(): return False return True + def handle_exit(self): + if self._client.is_server_alive(): + self.send_bye_command() + # log.os_log().info(f"player( {self._real_world.self_unum()} ): finished") + + # def run(self): + # while self._client.is_server_alive(): + # # self.do_check_ball() + # length, message, server_address = self._client.recv_message() + # if len(message) != 0: + # print("coach ", message) + # self.parse_message(message.decode()) + # else: + # self._client.set_server_alive(False) + # break + def run(self): + # print("coach agent run") last_time_rec = time.time() while True: while True: + self.do_check_ball() length, message, server_address = self._client.recv_message() if len(message) != 0: + print("coach ", message) + self.parse_message(message.decode()) last_time_rec = time.time() break @@ -172,7 +227,13 @@ def run(self): if self.think_received: self.action() self._think_received = False - # TODO elif for not sync mode + # TODO elif for not sync mode + + def do_check_ball(self): + # command = CoachCheckBallCommand() + # print(command.str()) + command = CoachTeamnameCommand() + self._client.send_message(command.str()) def parse_message(self, message): if message.find("(init") != -1: # TODO Use startwith instead of find @@ -192,9 +253,11 @@ def parse_message(self, message): self._think_received = True elif message.find("(ok") != -1: self._client.send_message(CoachDoneCommand().str()) + else: + self._client.send_message(CoachTeamnameCommand().str()) def init_dlog(self, message): - log.setup(self.world().team_name_l(), 'coach', self._current_time) + log.setup(self.world().team_name_l(), "coach", self._current_time) def world(self) -> GlobalWorldModel: return self._world @@ -232,11 +295,15 @@ def do_look(self): def do_change_player_type(self, unum: int, type: int): if not 1 <= unum <= 11: - log.os_log().error(f"(coach agent do change player type) illegal unum! unum={unum}") + log.os_log().error( + f"(coach agent do change player type) illegal unum! unum={unum}" + ) return False if not HETERO_DEFAULT <= type < 18: # TODO Player PARAM - log.os_log().error(f"(coach agent do change player type) illegal player type! type={type}") + log.os_log().error( + f"(coach agent do change player type) illegal player type! type={type}" + ) return False self._last_body_command.append(CoachChangePlayerTypeCommand(unum, type)) diff --git a/lib/coach/gloabl_world_model.py b/keepaway/lib/coach/gloabl_world_model.py similarity index 94% rename from lib/coach/gloabl_world_model.py rename to keepaway/lib/coach/gloabl_world_model.py index cea117f2..134bc69a 100644 --- a/lib/coach/gloabl_world_model.py +++ b/keepaway/lib/coach/gloabl_world_model.py @@ -1,11 +1,11 @@ -from lib.coach.global_object import GlobalPlayerObject, GlobalBallObject -from lib.debug.debug import log -from lib.parser.global_message_parser import GlobalFullStateWorldMessageParser -from lib.player.object_player import * -from lib.player.object_ball import * -from lib.rcsc.game_mode import GameMode -from lib.rcsc.game_time import GameTime -from lib.rcsc.types import HETERO_DEFAULT, HETERO_UNKNOWN, GameModeType +from keepaway.lib.coach.global_object import GlobalPlayerObject, GlobalBallObject +from keepaway.lib.debug.debug import log +from keepaway.lib.parser.global_message_parser import GlobalFullStateWorldMessageParser +from keepaway.lib.player.object_player import * +from keepaway.lib.player.object_ball import * +from keepaway.lib.rcsc.game_mode import GameMode +from keepaway.lib.rcsc.game_time import GameTime +from keepaway.lib.rcsc.types import HETERO_DEFAULT, HETERO_UNKNOWN, GameModeType class GlobalWorldModel: diff --git a/lib/coach/global_object.py b/keepaway/lib/coach/global_object.py similarity index 97% rename from lib/coach/global_object.py rename to keepaway/lib/coach/global_object.py index d26acaad..3c2e3af3 100644 --- a/lib/coach/global_object.py +++ b/keepaway/lib/coach/global_object.py @@ -1,8 +1,8 @@ from pyrusgeom.angle_deg import AngleDeg from pyrusgeom.vector_2d import Vector2D -from lib.rcsc.player_type import PlayerType -from lib.rcsc.server_param import ServerParam -from lib.rcsc.types import SideID, Card +from keepaway.lib.rcsc.player_type import PlayerType +from keepaway.lib.rcsc.server_param import ServerParam +from keepaway.lib.rcsc.types import SideID, Card class GlobalBallObject: diff --git a/keepaway/lib/debug/__init__.py b/keepaway/lib/debug/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/lib/debug/color.py b/keepaway/lib/debug/color.py similarity index 100% rename from lib/debug/color.py rename to keepaway/lib/debug/color.py diff --git a/lib/debug/debug.py b/keepaway/lib/debug/debug.py similarity index 64% rename from lib/debug/debug.py rename to keepaway/lib/debug/debug.py index 48f79880..cd533622 100644 --- a/lib/debug/debug.py +++ b/keepaway/lib/debug/debug.py @@ -1,12 +1,16 @@ import sys from logging import Logger -import team_config -from lib.debug.debug_client import DebugClient -from lib.debug.os_logger import get_logger -from lib.debug.sw_logger import SoccerWindow_Logger -from lib.rcsc.game_time import GameTime +# from keepaway.config import team_config +from keepaway.config import team_config +from keepaway.lib.debug.debug_client import DebugClient +from keepaway.lib.debug.os_logger import get_logger +from keepaway.lib.debug.sw_logger import SoccerWindow_Logger +from keepaway.lib.rcsc.game_time import GameTime +import os +up_one_dir = os.getcwd() +logs_dir = os.path.join(up_one_dir, "logs") class DebugLogger: def __init__(self): @@ -15,7 +19,7 @@ def __init__(self): self._debug_client: DebugClient = None def setup(self, team_name, unum, time): - sys.stderr = open(f'player-{unum}.err', 'w') + sys.stderr = open(f'{logs_dir}/player-{unum}.err', 'w') self._sw_log = SoccerWindow_Logger(team_name, unum, time) self._os_log = get_logger(unum, team_config.OUT == team_config.OUT_OPTION.TEXTFILE) self._debug_client = DebugClient() diff --git a/lib/debug/debug_client.py b/keepaway/lib/debug/debug_client.py similarity index 96% rename from lib/debug/debug_client.py rename to keepaway/lib/debug/debug_client.py index 97534c72..c05a918b 100644 --- a/lib/debug/debug_client.py +++ b/keepaway/lib/debug/debug_client.py @@ -1,15 +1,16 @@ from pyrusgeom.geom_2d import * import socket -from lib.rcsc.types import Card, SideID, GameModeType, UNUM_UNKNOWN +from keepaway.lib.rcsc.types import Card, SideID, GameModeType, UNUM_UNKNOWN -import team_config +# from keepaway.config import team_config +from keepaway.config import team_config from typing import TYPE_CHECKING if TYPE_CHECKING: - from lib.player.world_model import WorldModel - from lib.player.object_player import PlayerObject + from keepaway.lib.player.world_model import WorldModel + from keepaway.lib.player.object_player import PlayerObject def player_printer(p: 'PlayerObject', our_side: SideID): diff --git a/lib/debug/level.py b/keepaway/lib/debug/level.py similarity index 100% rename from lib/debug/level.py rename to keepaway/lib/debug/level.py diff --git a/lib/debug/os_logger.py b/keepaway/lib/debug/os_logger.py similarity index 87% rename from lib/debug/os_logger.py rename to keepaway/lib/debug/os_logger.py index cfb049ea..29f8584b 100644 --- a/lib/debug/os_logger.py +++ b/keepaway/lib/debug/os_logger.py @@ -1,8 +1,12 @@ import logging from typing import Union - import coloredlogs import sys +import os + +up_one_dir = os.path.dirname(os.getcwd()) +logs_dir = os.path.join(up_one_dir, "logs") + def get_logger(unum: Union[int, str] = None, on_file=False): @@ -29,11 +33,11 @@ def get_logger(unum: Union[int, str] = None, on_file=False): ) if on_file: if unum == 'coach': - file_name = 'coach.txt' + file_name = f'{logs_dir}/coach.txt' elif unum > 0: - file_name = f'player-{unum}.txt' + file_name = f'{logs_dir}/player-{unum}.txt' else: - file_name = f'coach-log.txt' + file_name = f'{logs_dir}/coach-log.txt' ch = logging.StreamHandler(stream=open(file_name, 'w')) ch.setFormatter(logging.Formatter('%(asctime)s %(filename)s %(lineno)-3d %(message)s', '%H:%M:%S:%s')) diff --git a/lib/debug/sw_logger.py b/keepaway/lib/debug/sw_logger.py similarity index 92% rename from lib/debug/sw_logger.py rename to keepaway/lib/debug/sw_logger.py index f1d1c410..eca472fd 100644 --- a/lib/debug/sw_logger.py +++ b/keepaway/lib/debug/sw_logger.py @@ -1,9 +1,14 @@ from pyrusgeom.circle_2d import Circle2D from pyrusgeom.vector_2d import Vector2D -from lib.debug.color import Color -from lib.debug.level import Level -from lib.rcsc.game_time import GameTime +from keepaway.lib.debug.color import Color +from keepaway.lib.debug.level import Level +from keepaway.lib.rcsc.game_time import GameTime + +import os +# logs_dir = "/logs" +up_one_dir = os.getcwd() +logs_dir = os.path.join(up_one_dir, "logs") class SoccerWindow_Logger: @@ -62,7 +67,11 @@ def add_message(self, self._commands += f"{self._time.cycle()},{self._time.stopped_cycle()} {self.level.value} m {round(x, 4)} {round(y, 4)} {msg}\n" def __init__(self, team_name: str, unum: int, time: GameTime): - self._file = open(f"/tmp/{team_name}-{unum}.log", 'w') + ## modified to be saved in the current working directory + # self._file = open(f"/tmp/{team_name}-{unum}.log", 'w') - original code + # print(f"logs_dir: {logs_dir}") + self._file = open(f"{logs_dir}/{team_name}-{unum}.log", 'w') + # self._file = open(f"logs/{team_name}-{unum}.log", 'w') self._time: GameTime = time self._system = SoccerWindow_Logger.LoggerLevel(Level.SYSTEM, self._time) diff --git a/lib/debug/timer.py b/keepaway/lib/debug/timer.py similarity index 100% rename from lib/debug/timer.py rename to keepaway/lib/debug/timer.py diff --git a/keepaway/lib/formation/__init__.py b/keepaway/lib/formation/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/lib/formation/delaunay_triangulation.py b/keepaway/lib/formation/delaunay_triangulation.py similarity index 97% rename from lib/formation/delaunay_triangulation.py rename to keepaway/lib/formation/delaunay_triangulation.py index b8994674..4fdb3c0c 100644 --- a/lib/formation/delaunay_triangulation.py +++ b/keepaway/lib/formation/delaunay_triangulation.py @@ -1,7 +1,7 @@ from scipy.spatial import Delaunay from pyrusgeom.geom_2d import * from enum import Enum -from lib.rcsc.server_param import ServerParam +from keepaway.lib.rcsc.server_param import ServerParam from pyrusgeom.soccer_math import min_max class FormationType(Enum): @@ -113,7 +113,7 @@ def get_poses(self): def __repr__(self): return self._path -# f = Formation('base/formations-dt/before-kick-off.conf') +# f = Formation('keepaway.base/formations-dt/before-kick-off.conf') # debug_print(len(f._balls)) # debug_print(len(f._players)) # debug_print(f._formation_type) diff --git a/keepaway/lib/messenger/__init__.py b/keepaway/lib/messenger/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/lib/messenger/ball_goalie_messenger.py b/keepaway/lib/messenger/ball_goalie_messenger.py similarity index 89% rename from lib/messenger/ball_goalie_messenger.py rename to keepaway/lib/messenger/ball_goalie_messenger.py index d2287911..e133fe79 100644 --- a/lib/messenger/ball_goalie_messenger.py +++ b/keepaway/lib/messenger/ball_goalie_messenger.py @@ -2,17 +2,17 @@ from pyrusgeom.soccer_math import bound from pyrusgeom.vector_2d import Vector2D -from lib.debug.debug import log -from lib.messenger.converters import MessengerConverter -from lib.messenger.messenger import Messenger +from keepaway.lib.debug.debug import log +from keepaway.lib.messenger.converters import MessengerConverter +from keepaway.lib.messenger.messenger import Messenger -from lib.messenger.messenger_memory import MessengerMemory -from lib.rcsc.game_time import GameTime -from lib.rcsc.server_param import ServerParam +from keepaway.lib.messenger.messenger_memory import MessengerMemory +from keepaway.lib.rcsc.game_time import GameTime +from keepaway.lib.rcsc.server_param import ServerParam from typing import TYPE_CHECKING if TYPE_CHECKING: - from lib.player.world_model import WorldModel + from keepaway.lib.player.world_model import WorldModel class BallGoalieMessenger(Messenger): diff --git a/lib/messenger/ball_messenger.py b/keepaway/lib/messenger/ball_messenger.py similarity index 85% rename from lib/messenger/ball_messenger.py rename to keepaway/lib/messenger/ball_messenger.py index 014f48ba..d4f3f9bc 100644 --- a/lib/messenger/ball_messenger.py +++ b/keepaway/lib/messenger/ball_messenger.py @@ -2,17 +2,17 @@ from pyrusgeom.soccer_math import bound from pyrusgeom.vector_2d import Vector2D -from lib.debug.debug import log -from lib.messenger.converters import MessengerConverter -from lib.messenger.messenger import Messenger +from keepaway.lib.debug.debug import log +from keepaway.lib.messenger.converters import MessengerConverter +from keepaway.lib.messenger.messenger import Messenger -from lib.messenger.messenger_memory import MessengerMemory -from lib.rcsc.game_time import GameTime -from lib.rcsc.server_param import ServerParam +from keepaway.lib.messenger.messenger_memory import MessengerMemory +from keepaway.lib.rcsc.game_time import GameTime +from keepaway.lib.rcsc.server_param import ServerParam from typing import TYPE_CHECKING if TYPE_CHECKING: - from lib.player.world_model import WorldModel + from keepaway.lib.player.world_model import WorldModel class BallMessenger(Messenger): diff --git a/lib/messenger/ball_player_messenger.py b/keepaway/lib/messenger/ball_player_messenger.py similarity index 90% rename from lib/messenger/ball_player_messenger.py rename to keepaway/lib/messenger/ball_player_messenger.py index a224755f..718fff83 100644 --- a/lib/messenger/ball_player_messenger.py +++ b/keepaway/lib/messenger/ball_player_messenger.py @@ -2,18 +2,18 @@ from pyrusgeom.soccer_math import bound from pyrusgeom.vector_2d import Vector2D -from lib.debug.debug import log -from lib.messenger.converters import MessengerConverter -from lib.messenger.messenger import Messenger +from keepaway.lib.debug.debug import log +from keepaway.lib.messenger.converters import MessengerConverter +from keepaway.lib.messenger.messenger import Messenger -from lib.messenger.messenger_memory import MessengerMemory -from lib.rcsc.game_time import GameTime -from lib.rcsc.server_param import ServerParam +from keepaway.lib.messenger.messenger_memory import MessengerMemory +from keepaway.lib.rcsc.game_time import GameTime +from keepaway.lib.rcsc.server_param import ServerParam from typing import TYPE_CHECKING if TYPE_CHECKING: - from lib.player.world_model import WorldModel + from keepaway.lib.player.world_model import WorldModel class BallPlayerMessenger(Messenger): diff --git a/lib/messenger/ball_pos_vel_messenger.py b/keepaway/lib/messenger/ball_pos_vel_messenger.py similarity index 87% rename from lib/messenger/ball_pos_vel_messenger.py rename to keepaway/lib/messenger/ball_pos_vel_messenger.py index b43f4b7d..46591b9d 100644 --- a/lib/messenger/ball_pos_vel_messenger.py +++ b/keepaway/lib/messenger/ball_pos_vel_messenger.py @@ -1,16 +1,16 @@ from pyrusgeom.soccer_math import min_max from pyrusgeom.vector_2d import Vector2D -from lib.messenger.messenger import Messenger -from lib.messenger.messenger_memory import MessengerMemory -from lib.rcsc.game_time import GameTime -from lib.rcsc.server_param import ServerParam +from keepaway.lib.messenger.messenger import Messenger +from keepaway.lib.messenger.messenger_memory import MessengerMemory +from keepaway.lib.rcsc.game_time import GameTime +from keepaway.lib.rcsc.server_param import ServerParam -import lib.messenger.converters as converters +import keepaway.lib.messenger.converters as converters from typing import TYPE_CHECKING if TYPE_CHECKING: - from lib.player.world_model import WorldModel + from keepaway.lib.player.world_model import WorldModel class BallPosVelMessenger(Messenger): def __init__(self, message:str=None) -> None: diff --git a/lib/messenger/converters.py b/keepaway/lib/messenger/converters.py similarity index 98% rename from lib/messenger/converters.py rename to keepaway/lib/messenger/converters.py index 906af48c..3da17316 100644 --- a/lib/messenger/converters.py +++ b/keepaway/lib/messenger/converters.py @@ -3,7 +3,7 @@ from pyrusgeom.soccer_math import bound -from lib.debug.debug import log +from keepaway.lib.debug.debug import log chars = "abcdefghijklmnopqrstuvwxyzABCDEFGHIJKLMNOPQRSTUVWXYZ ().+-*/?<>_0123456789" diff --git a/lib/messenger/free_form_messenger.py b/keepaway/lib/messenger/free_form_messenger.py similarity index 63% rename from lib/messenger/free_form_messenger.py rename to keepaway/lib/messenger/free_form_messenger.py index 30a23cfa..83eca1a5 100644 --- a/lib/messenger/free_form_messenger.py +++ b/keepaway/lib/messenger/free_form_messenger.py @@ -1,10 +1,10 @@ -from lib.messenger.messenger import Messenger -from lib.messenger.messenger_memory import MessengerMemory -from lib.rcsc.game_time import GameTime +from keepaway.lib.messenger.messenger import Messenger +from keepaway.lib.messenger.messenger_memory import MessengerMemory +from keepaway.lib.rcsc.game_time import GameTime from typing import TYPE_CHECKING if TYPE_CHECKING: - from lib.coach.gloabl_world_model import GlobalWorldModel + from keepaway.lib.coach.gloabl_world_model import GlobalWorldModel class FreeFormMessenger(Messenger): def __init__(self) -> None: diff --git a/lib/messenger/goalie_messenger.py b/keepaway/lib/messenger/goalie_messenger.py similarity index 85% rename from lib/messenger/goalie_messenger.py rename to keepaway/lib/messenger/goalie_messenger.py index 53319507..5e02718a 100644 --- a/lib/messenger/goalie_messenger.py +++ b/keepaway/lib/messenger/goalie_messenger.py @@ -2,17 +2,17 @@ from pyrusgeom.soccer_math import bound from pyrusgeom.vector_2d import Vector2D -from lib.debug.debug import log -from lib.messenger.converters import MessengerConverter -from lib.messenger.messenger import Messenger +from keepaway.lib.debug.debug import log +from keepaway.lib.messenger.converters import MessengerConverter +from keepaway.lib.messenger.messenger import Messenger -from lib.messenger.messenger_memory import MessengerMemory -from lib.rcsc.game_time import GameTime -from lib.rcsc.server_param import ServerParam +from keepaway.lib.messenger.messenger_memory import MessengerMemory +from keepaway.lib.rcsc.game_time import GameTime +from keepaway.lib.rcsc.server_param import ServerParam from typing import TYPE_CHECKING if TYPE_CHECKING: - from lib.player.world_model import WorldModel + from keepaway.lib.player.world_model import WorldModel class GoalieMessenger(Messenger): diff --git a/lib/messenger/goalie_player_messenger.py b/keepaway/lib/messenger/goalie_player_messenger.py similarity index 89% rename from lib/messenger/goalie_player_messenger.py rename to keepaway/lib/messenger/goalie_player_messenger.py index c4c81716..f253ff4f 100644 --- a/lib/messenger/goalie_player_messenger.py +++ b/keepaway/lib/messenger/goalie_player_messenger.py @@ -2,17 +2,17 @@ from pyrusgeom.soccer_math import bound from pyrusgeom.vector_2d import Vector2D -from lib.debug.debug import log -from lib.messenger.converters import MessengerConverter -from lib.messenger.messenger import Messenger +from keepaway.lib.debug.debug import log +from keepaway.lib.messenger.converters import MessengerConverter +from keepaway.lib.messenger.messenger import Messenger -from lib.messenger.messenger_memory import MessengerMemory -from lib.rcsc.game_time import GameTime -from lib.rcsc.server_param import ServerParam +from keepaway.lib.messenger.messenger_memory import MessengerMemory +from keepaway.lib.rcsc.game_time import GameTime +from keepaway.lib.rcsc.server_param import ServerParam from typing import TYPE_CHECKING if TYPE_CHECKING: - from lib.player.world_model import WorldModel + from keepaway.lib.player.world_model import WorldModel class GoaliePlayerMessenger(Messenger): diff --git a/lib/messenger/messenger.py b/keepaway/lib/messenger/messenger.py similarity index 75% rename from lib/messenger/messenger.py rename to keepaway/lib/messenger/messenger.py index 8a6be96a..499d08b8 100644 --- a/lib/messenger/messenger.py +++ b/keepaway/lib/messenger/messenger.py @@ -1,16 +1,16 @@ from enum import Enum, unique from typing import TYPE_CHECKING -from lib.debug.debug import log -from lib.debug.level import Level +from keepaway.lib.debug.debug import log +from keepaway.lib.debug.level import Level from pyrusgeom.vector_2d import Vector2D -from lib.messenger.messenger_memory import MessengerMemory +from keepaway.lib.messenger.messenger_memory import MessengerMemory -from lib.rcsc.game_time import GameTime +from keepaway.lib.rcsc.game_time import GameTime -from lib.rcsc.server_param import ServerParam +from keepaway.lib.rcsc.server_param import ServerParam if TYPE_CHECKING: - from lib.player.world_model import WorldModel + from keepaway.lib.player.world_model import WorldModel class Messenger: @@ -94,17 +94,17 @@ def encode_all(messages: list['Messenger']): @staticmethod def decode_all(messenger_memory: MessengerMemory, messages: str, sender: int, current_time: GameTime): - from lib.messenger.pass_messenger import PassMessenger - from lib.messenger.ball_goalie_messenger import BallGoalieMessenger - from lib.messenger.ball_messenger import BallMessenger - from lib.messenger.ball_player_messenger import BallPlayerMessenger - from lib.messenger.goalie_messenger import GoalieMessenger - from lib.messenger.goalie_player_messenger import GoaliePlayerMessenger - from lib.messenger.one_player_messenger import OnePlayerMessenger - from lib.messenger.recovery_message import RecoveryMessenger - from lib.messenger.stamina_messenger import StaminaMessenger - from lib.messenger.three_player_messenger import ThreePlayerMessenger - from lib.messenger.two_player_messenger import TwoPlayerMessenger + from keepaway.lib.messenger.pass_messenger import PassMessenger + from keepaway.lib.messenger.ball_goalie_messenger import BallGoalieMessenger + from keepaway.lib.messenger.ball_messenger import BallMessenger + from keepaway.lib.messenger.ball_player_messenger import BallPlayerMessenger + from keepaway.lib.messenger.goalie_messenger import GoalieMessenger + from keepaway.lib.messenger.goalie_player_messenger import GoaliePlayerMessenger + from keepaway.lib.messenger.one_player_messenger import OnePlayerMessenger + from keepaway.lib.messenger.recovery_message import RecoveryMessenger + from keepaway.lib.messenger.stamina_messenger import StaminaMessenger + from keepaway.lib.messenger.three_player_messenger import ThreePlayerMessenger + from keepaway.lib.messenger.two_player_messenger import TwoPlayerMessenger messenger_classes: dict[Messenger.Types, type['Messenger']] = { Messenger.Types.BALL: BallMessenger, diff --git a/lib/messenger/messenger_memory.py b/keepaway/lib/messenger/messenger_memory.py similarity index 99% rename from lib/messenger/messenger_memory.py rename to keepaway/lib/messenger/messenger_memory.py index 6aeef8cc..21552d45 100644 --- a/lib/messenger/messenger_memory.py +++ b/keepaway/lib/messenger/messenger_memory.py @@ -2,7 +2,7 @@ from pyrusgeom.angle_deg import AngleDeg from pyrusgeom.vector_2d import Vector2D -from lib.rcsc.game_time import GameTime +from keepaway.lib.rcsc.game_time import GameTime class MessengerMemory: diff --git a/lib/messenger/one_player_messenger.py b/keepaway/lib/messenger/one_player_messenger.py similarity index 85% rename from lib/messenger/one_player_messenger.py rename to keepaway/lib/messenger/one_player_messenger.py index c3d17964..9db10e26 100644 --- a/lib/messenger/one_player_messenger.py +++ b/keepaway/lib/messenger/one_player_messenger.py @@ -2,18 +2,18 @@ from pyrusgeom.soccer_math import bound from pyrusgeom.vector_2d import Vector2D -from lib.debug.debug import log -from lib.messenger.converters import MessengerConverter -from lib.messenger.messenger import Messenger +from keepaway.lib.debug.debug import log +from keepaway.lib.messenger.converters import MessengerConverter +from keepaway.lib.messenger.messenger import Messenger -from lib.messenger.messenger_memory import MessengerMemory -from lib.rcsc.game_time import GameTime -from lib.rcsc.server_param import ServerParam +from keepaway.lib.messenger.messenger_memory import MessengerMemory +from keepaway.lib.rcsc.game_time import GameTime +from keepaway.lib.rcsc.server_param import ServerParam from typing import TYPE_CHECKING if TYPE_CHECKING: - from lib.player.world_model import WorldModel + from keepaway.lib.player.world_model import WorldModel class OnePlayerMessenger(Messenger): diff --git a/keepaway/lib/messenger/pass_messenger.py b/keepaway/lib/messenger/pass_messenger.py new file mode 100644 index 00000000..44b154dd --- /dev/null +++ b/keepaway/lib/messenger/pass_messenger.py @@ -0,0 +1,90 @@ +from pyrusgeom.soccer_math import min_max, bound +from pyrusgeom.vector_2d import Vector2D + +from keepaway.lib.messenger.converters import MessengerConverter +from keepaway.lib.messenger.messenger import Messenger +from keepaway.lib.messenger.messenger_memory import MessengerMemory +from keepaway.lib.rcsc.game_time import GameTime +from keepaway.lib.rcsc.server_param import ServerParam + +import keepaway.lib.messenger.converters as converters + +from typing import TYPE_CHECKING + +if TYPE_CHECKING: + from keepaway.lib.player.world_model import WorldModel + + +class PassMessenger(Messenger): + # CONVERTER = MessengerConverter(Messenger.SIZES[Messenger.Types.PASS], [ + # (-ServerParam.i().pitch_half_length(), ServerParam.i().pitch_half_length(), 2 ** 10), + # (-ServerParam.i().pitch_half_width(), ServerParam.i().pitch_half_width(), 2 ** 9), + # (1, 12, 11), + # (-ServerParam.i().pitch_half_length(), ServerParam.i().pitch_half_length(), 2 ** 10), + # (-ServerParam.i().pitch_half_width(), ServerParam.i().pitch_half_width(), 2 ** 9), + # (-ServerParam.i().ball_speed_max(), ServerParam.i().ball_speed_max(), 2 ** 6), + # (-ServerParam.i().ball_speed_max(), ServerParam.i().ball_speed_max(), 2 ** 6), + # ]) + + ## TODO - keepway custom i dont expect this to change from the orignal pitch size + CONVERTER = MessengerConverter(Messenger.SIZES[Messenger.Types.PASS], [ + (-ServerParam.i().keepaway_length()/2, ServerParam.i().keepaway_length()/2, 2 ** 10), + (-ServerParam.i().keepaway_width()/2, ServerParam.i().keepaway_width()/2, 2 ** 9), + (1, 12, 11), + (-ServerParam.i().keepaway_length()/2, ServerParam.i().keepaway_length()/2, 2 ** 10), + (-ServerParam.i().keepaway_width()/2, ServerParam.i().keepaway_width()/2, 2 ** 9), + (-ServerParam.i().ball_speed_max(), ServerParam.i().ball_speed_max(), 2 ** 6), + (-ServerParam.i().ball_speed_max(), ServerParam.i().ball_speed_max(), 2 ** 6), + ]) + def __init__(self, + receiver_unum: int = None, + receive_point: Vector2D = None, + ball_pos: Vector2D = None, + ball_vel: Vector2D = None, + message: str = None) -> None: + super().__init__() + self._size = Messenger.SIZES[Messenger.Types.PASS] + self._header = Messenger.Types.PASS.value + + if message: + self._message = message + return + self._receiver_unum = receiver_unum + self._receive_point = receive_point.copy() + self._ball_pos = ball_pos.copy() + self._ball_vel = ball_vel.copy() + + def encode(self) -> str: + SP = ServerParam.i() + ep = 0.001 + # msg = PassMessenger.CONVERTER.convert_to_word([ + # bound(-SP.pitch_half_length() + ep, self._receive_point.x(), SP.pitch_half_length() - ep), + # bound(-SP.pitch_half_width() + ep, self._receive_point.y(), SP.pitch_half_width() - ep), + # self._receiver_unum, + # bound(-SP.pitch_half_length() + ep, self._ball_pos.x(), SP.pitch_half_length() - ep), + # bound(-SP.pitch_half_width() + ep, self._ball_pos.y(), SP.pitch_half_width() - ep), + # bound(ep, self._ball_vel.x(), SP.ball_speed_max() - ep), + # bound(ep, self._ball_vel.y(), SP.ball_speed_max() - ep), + # ]) + + msg = PassMessenger.CONVERTER.convert_to_word([ + bound(-SP.keepaway_length()/2 + ep, self._receive_point.x(), SP.keepaway_length()/2 - ep), + bound(-SP.keepaway_width()/2 + ep, self._receive_point.y(), SP.keepaway_width()/2 - ep), + self._receiver_unum, + bound(-SP.keepaway_length()/2 + ep, self._ball_pos.x(), SP.keepaway_length()/2 - ep), + bound(-SP.keepaway_width()/2 + ep, self._ball_pos.y(), SP.keepaway_width()/2 - ep), + bound(ep, self._ball_vel.x(), SP.ball_speed_max() - ep), + bound(ep, self._ball_vel.y(), SP.ball_speed_max() - ep), + ]) + + return f'{self._header}{msg}' + + def decode(self, messenger_memory: MessengerMemory, sender: int, current_time: GameTime) -> None: + rpx, rpy, ru, bpx, bpy, bvx, bvy = PassMessenger.CONVERTER.convert_to_values(self._message) + + messenger_memory.add_pass(sender, ru, Vector2D(rpx, rpy), current_time) + messenger_memory.add_ball(sender, Vector2D(bpx, bpy), Vector2D(bvx, bvy), current_time) + + def __repr__(self) -> str: + return "ball pos vel msg" + diff --git a/lib/messenger/player_pos_unum_messenger.py b/keepaway/lib/messenger/player_pos_unum_messenger.py similarity index 84% rename from lib/messenger/player_pos_unum_messenger.py rename to keepaway/lib/messenger/player_pos_unum_messenger.py index 9b3c2c09..e2ad5297 100644 --- a/lib/messenger/player_pos_unum_messenger.py +++ b/keepaway/lib/messenger/player_pos_unum_messenger.py @@ -1,18 +1,18 @@ -from lib.debug.debug import log -from lib.debug.level import Level +from keepaway.lib.debug.debug import log +from keepaway.lib.debug.level import Level from pyrusgeom.soccer_math import min_max from pyrusgeom.vector_2d import Vector2D -from lib.messenger.messenger import Messenger -from lib.messenger.messenger_memory import MessengerMemory -from lib.rcsc.game_time import GameTime -from lib.rcsc.server_param import ServerParam +from keepaway.lib.messenger.messenger import Messenger +from keepaway.lib.messenger.messenger_memory import MessengerMemory +from keepaway.lib.rcsc.game_time import GameTime +from keepaway.lib.rcsc.server_param import ServerParam -import lib.messenger.converters as converters +import keepaway.lib.messenger.converters as converters from typing import TYPE_CHECKING if TYPE_CHECKING: - from lib.player.world_model import WorldModel + from keepaway.lib.player.world_model import WorldModel class PlayerPosUnumMessenger(Messenger): def __init__(self, unum: int = None, message:str=None) -> None: diff --git a/lib/messenger/recovery_message.py b/keepaway/lib/messenger/recovery_message.py similarity index 77% rename from lib/messenger/recovery_message.py rename to keepaway/lib/messenger/recovery_message.py index e64f6d4b..6756b1ee 100644 --- a/lib/messenger/recovery_message.py +++ b/keepaway/lib/messenger/recovery_message.py @@ -1,8 +1,8 @@ -from lib.messenger.converters import MessengerConverter -from lib.messenger.messenger import Messenger -from lib.messenger.messenger_memory import MessengerMemory -from lib.rcsc.game_time import GameTime -from lib.rcsc.server_param import ServerParam +from keepaway.lib.messenger.converters import MessengerConverter +from keepaway.lib.messenger.messenger import Messenger +from keepaway.lib.messenger.messenger_memory import MessengerMemory +from keepaway.lib.rcsc.game_time import GameTime +from keepaway.lib.rcsc.server_param import ServerParam class RecoveryMessenger(Messenger): diff --git a/lib/messenger/stamina_messenger.py b/keepaway/lib/messenger/stamina_messenger.py similarity index 76% rename from lib/messenger/stamina_messenger.py rename to keepaway/lib/messenger/stamina_messenger.py index 22fbf2c7..a813dc4e 100644 --- a/lib/messenger/stamina_messenger.py +++ b/keepaway/lib/messenger/stamina_messenger.py @@ -1,8 +1,8 @@ -from lib.messenger.converters import MessengerConverter -from lib.messenger.messenger import Messenger -from lib.messenger.messenger_memory import MessengerMemory -from lib.rcsc.game_time import GameTime -from lib.rcsc.server_param import ServerParam +from keepaway.lib.messenger.converters import MessengerConverter +from keepaway.lib.messenger.messenger import Messenger +from keepaway.lib.messenger.messenger_memory import MessengerMemory +from keepaway.lib.rcsc.game_time import GameTime +from keepaway.lib.rcsc.server_param import ServerParam class StaminaMessenger(Messenger): diff --git a/lib/messenger/three_player_messenger.py b/keepaway/lib/messenger/three_player_messenger.py similarity index 87% rename from lib/messenger/three_player_messenger.py rename to keepaway/lib/messenger/three_player_messenger.py index 4366a295..1b8d1233 100644 --- a/lib/messenger/three_player_messenger.py +++ b/keepaway/lib/messenger/three_player_messenger.py @@ -2,17 +2,17 @@ from pyrusgeom.soccer_math import bound from pyrusgeom.vector_2d import Vector2D -from lib.debug.debug import log -from lib.messenger.converters import MessengerConverter -from lib.messenger.messenger import Messenger +from keepaway.lib.debug.debug import log +from keepaway.lib.messenger.converters import MessengerConverter +from keepaway.lib.messenger.messenger import Messenger -from lib.messenger.messenger_memory import MessengerMemory -from lib.rcsc.game_time import GameTime -from lib.rcsc.server_param import ServerParam +from keepaway.lib.messenger.messenger_memory import MessengerMemory +from keepaway.lib.rcsc.game_time import GameTime +from keepaway.lib.rcsc.server_param import ServerParam from typing import TYPE_CHECKING if TYPE_CHECKING: - from lib.player.world_model import WorldModel + from keepaway.lib.player.world_model import WorldModel class ThreePlayerMessenger(Messenger): diff --git a/lib/messenger/two_player_messenger.py b/keepaway/lib/messenger/two_player_messenger.py similarity index 86% rename from lib/messenger/two_player_messenger.py rename to keepaway/lib/messenger/two_player_messenger.py index d5bb901b..85d7cf53 100644 --- a/lib/messenger/two_player_messenger.py +++ b/keepaway/lib/messenger/two_player_messenger.py @@ -2,18 +2,18 @@ from pyrusgeom.soccer_math import bound from pyrusgeom.vector_2d import Vector2D -from lib.debug.debug import log -from lib.messenger.converters import MessengerConverter -from lib.messenger.messenger import Messenger +from keepaway.lib.debug.debug import log +from keepaway.lib.messenger.converters import MessengerConverter +from keepaway.lib.messenger.messenger import Messenger -from lib.messenger.messenger_memory import MessengerMemory -from lib.rcsc.game_time import GameTime -from lib.rcsc.server_param import ServerParam +from keepaway.lib.messenger.messenger_memory import MessengerMemory +from keepaway.lib.rcsc.game_time import GameTime +from keepaway.lib.rcsc.server_param import ServerParam from typing import TYPE_CHECKING if TYPE_CHECKING: - from lib.player.world_model import WorldModel + from keepaway.lib.player.world_model import WorldModel class TwoPlayerMessenger(Messenger): diff --git a/keepaway/lib/network/__init__.py b/keepaway/lib/network/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/lib/network/udp_socket.py b/keepaway/lib/network/udp_socket.py similarity index 97% rename from lib/network/udp_socket.py rename to keepaway/lib/network/udp_socket.py index 62091682..a970cb4e 100644 --- a/lib/network/udp_socket.py +++ b/keepaway/lib/network/udp_socket.py @@ -1,6 +1,6 @@ import socket -import team_config +from keepaway.config import team_config MAX_BUFF_SIZE = 8192 diff --git a/keepaway/lib/parser/__init__.py b/keepaway/lib/parser/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/lib/parser/global_message_parser.py b/keepaway/lib/parser/global_message_parser.py similarity index 98% rename from lib/parser/global_message_parser.py rename to keepaway/lib/parser/global_message_parser.py index 77b9fc8a..3f5ad0c6 100644 --- a/lib/parser/global_message_parser.py +++ b/keepaway/lib/parser/global_message_parser.py @@ -1,4 +1,4 @@ -from lib.parser.parser_message_params import MessageParamsParser +from keepaway.lib.parser.parser_message_params import MessageParamsParser """ sample version >= 7.0 diff --git a/lib/parser/message_params_parser_see.py b/keepaway/lib/parser/message_params_parser_see.py similarity index 100% rename from lib/parser/message_params_parser_see.py rename to keepaway/lib/parser/message_params_parser_see.py diff --git a/lib/parser/parser_message_fullstate_world.py b/keepaway/lib/parser/parser_message_fullstate_world.py similarity index 98% rename from lib/parser/parser_message_fullstate_world.py rename to keepaway/lib/parser/parser_message_fullstate_world.py index e24c80a4..7684b47c 100644 --- a/lib/parser/parser_message_fullstate_world.py +++ b/keepaway/lib/parser/parser_message_fullstate_world.py @@ -1,4 +1,4 @@ -from lib.parser.parser_message_params import MessageParamsParser +from keepaway.lib.parser.parser_message_params import MessageParamsParser """" (fullstate