├── protocols ├── __init__.py ├── vssref_placement_pb2.py ├── vssref_command_pb2.py ├── command_pb2.py └── packet_pb2.py ├── fields ├── __init__.py └── field.py ├── strategy ├── utils │ └── __init__.py ├── commons │ ├── __init__.py │ └── Replacer.py ├── __init__.py ├── rsm2025 │ ├── __init__.py │ ├── SecondAttacker.py │ └── GoalkeeperArch.py ├── tests │ ├── goFoward.py │ ├── Idle.py │ ├── FIRATest.py │ ├── dwaScratch.py │ ├── asScratch2.py │ ├── __init__.py │ ├── pathfinderTest.py │ ├── pfMidFielder.py │ ├── PID_test.py │ ├── PID_attacker.py │ ├── uvf_test.py │ ├── potential_fields_test.py │ ├── limit_cycle_test.py │ ├── pfScratch.py │ └── PID_tuner.py ├── DebugTools.py └── BaseStrategy.py ├── enviroment.config ├── enviroment.dev.config ├── commons ├── __init__.py ├── utils.py └── math.py ├── live_plot ├── __init__.py ├── plot_astar_map.py ├── visualizer.py ├── circuit_plot.py ├── logger.py ├── plot_circuit.ipynb ├── live_plot_script.py └── plot_djisktra.py ├── algorithms ├── potential_fields │ ├── __init__.py │ └── plotter.py ├── RRT │ └── __init__.py ├── dijkstra_waypoint │ ├── __init__.py │ ├── waypoint.py │ └── dijkstra.py ├── astar │ ├── __init__.py │ ├── fieldGraph.py │ ├── astar.py │ └── pathAstar.py ├── __init__.py └── dwa │ └── dynamicWindowApproach.py ├── match ├── __init__.py ├── match.py └── match_real_life.py ├── scripts └── run_firasim.sh ├── readme └── FiraSIM-comtab.png ├── Makefile ├── vision ├── __init__.py └── vision.py ├── comm ├── __init__.py ├── rl_comm.py └── comm.py ├── Dockerfile ├── entities ├── __init__.py ├── trainer │ ├── __init__.py │ ├── circuit_run.py │ └── base_trainer.py ├── coach │ ├── coach.py │ ├── __init__.py │ ├── guideCoach.py │ ├── test_coach.py │ ├── Backup.py │ ├── rsm2025_defend.py │ └── rsm2025_attack.py ├── plays │ └── __init__.py ├── Ball.py └── Robot.py ├── api ├── __init__.py ├── parameters.py ├── api_recv.py ├── new_api.py ├── info_api.py └── udp_sender.py ├── requirements.txt ├── controller ├── noController.py ├── __init__.py ├── visualizers │ ├── pid_tuner.py │ ├── plot_script.py │ └── run_visualizers.py ├── PID.py ├── simple_LQR.py └── uni_controller.py ├── pyproject.toml ├── blue_team.json ├── yellow_team.json ├── config.json ├── config_real_life.json ├── fields.json ├── README.md ├── .gitignore ├── main.py ├── main_real_life.py └── foul_placements3v3.json /protocols/__init__.py: -------------------------------------------------------------------------------- 1 | import protocols -------------------------------------------------------------------------------- /fields/__init__.py: -------------------------------------------------------------------------------- 1 | from fields.field import Field 2 | -------------------------------------------------------------------------------- /strategy/utils/__init__.py: -------------------------------------------------------------------------------- 1 | from strategy.utils import player_playbook -------------------------------------------------------------------------------- /enviroment.config: -------------------------------------------------------------------------------- 1 | firasim_folder="COLOQUE AQUI O CAMINHO DO SEU FIRA SIM" -------------------------------------------------------------------------------- /enviroment.dev.config: -------------------------------------------------------------------------------- 1 | firasim_folder=~/Desktop/UFABC/NEON/FIRASim 2 | -------------------------------------------------------------------------------- /commons/__init__.py: -------------------------------------------------------------------------------- 1 | from commons import math 2 | from commons import utils 3 | -------------------------------------------------------------------------------- /strategy/commons/__init__.py: -------------------------------------------------------------------------------- 1 | from strategy.commons.Replacer import Replacer -------------------------------------------------------------------------------- /live_plot/__init__.py: -------------------------------------------------------------------------------- 1 | from live_plot.logger import Writer, Reader, Parameter 2 | -------------------------------------------------------------------------------- /algorithms/potential_fields/__init__.py: -------------------------------------------------------------------------------- 1 | import algorithms.potential_fields.plotter 2 | -------------------------------------------------------------------------------- /algorithms/RRT/__init__.py: -------------------------------------------------------------------------------- 1 | import algorithms.RRT.rrt 2 | import algorithms.RRT.rrtstar 3 | -------------------------------------------------------------------------------- /match/__init__.py: -------------------------------------------------------------------------------- 1 | from match.match import Match 2 | from match.match_real_life import MatchRealLife 3 | -------------------------------------------------------------------------------- /scripts/run_firasim.sh: -------------------------------------------------------------------------------- 1 | . ../enviroment.dev.config 2 | 3 | cd "$firasim_folder" 4 | 5 | ./bin/FIRASim -------------------------------------------------------------------------------- /readme/FiraSIM-comtab.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/project-neon/NeonFC/HEAD/readme/FiraSIM-comtab.png -------------------------------------------------------------------------------- /Makefile: -------------------------------------------------------------------------------- 1 | build: 2 | docker build --no-cache -t neon-fc . 3 | run: 4 | docker run --net=host $(params) neon-fc -------------------------------------------------------------------------------- /vision/__init__.py: -------------------------------------------------------------------------------- 1 | from vision.vision import FiraVision 2 | 3 | from vision.vision import assign_empty_values 4 | -------------------------------------------------------------------------------- /comm/__init__.py: -------------------------------------------------------------------------------- 1 | from comm.comm import FiraComm 2 | from comm.rl_comm import RLComm 3 | from comm.comm import FiraFullComm 4 | -------------------------------------------------------------------------------- /algorithms/dijkstra_waypoint/__init__.py: -------------------------------------------------------------------------------- 1 | from algorithms.dijkstra_waypoint.waypoint import WaypointSystem 2 | from algorithms.dijkstra_waypoint.dijkstra import Vertex, Graph 3 | -------------------------------------------------------------------------------- /algorithms/astar/__init__.py: -------------------------------------------------------------------------------- 1 | from algorithms.astar.astar import AStar, Node 2 | from algorithms.astar.pathAstar import PathAstar 3 | from algorithms.astar.fieldGraph import FieldGraph 4 | -------------------------------------------------------------------------------- /Dockerfile: -------------------------------------------------------------------------------- 1 | FROM python:3.5 2 | WORKDIR / 3 | COPY requirements.txt ./ 4 | EXPOSE 5000 5 | RUN pip install --no-cache-dir -r requirements.txt 6 | COPY . . 7 | CMD ["python3", "/main.py"] 8 | -------------------------------------------------------------------------------- /entities/__init__.py: -------------------------------------------------------------------------------- 1 | from entities.Robot import Robot 2 | from entities.Ball import Ball 3 | from entities import coach 4 | from entities import plays 5 | 6 | from entities import trainer 7 | -------------------------------------------------------------------------------- /algorithms/__init__.py: -------------------------------------------------------------------------------- 1 | # Astar and suplementar tools 2 | from algorithms import astar 3 | 4 | 5 | # Dijkstra + waypoint planning 6 | from algorithms import dijkstra_waypoint 7 | 8 | from algorithms.RRT import rrt 9 | -------------------------------------------------------------------------------- /strategy/__init__.py: -------------------------------------------------------------------------------- 1 | from strategy import tests 2 | from strategy import rsm2025 3 | from strategy.BaseStrategy import Strategy 4 | from strategy import commons 5 | 6 | # debug tools 7 | from strategy import DebugTools 8 | -------------------------------------------------------------------------------- /api/__init__.py: -------------------------------------------------------------------------------- 1 | from api.udp_sender import DataSender 2 | from api.udp_sender import DataNode 3 | 4 | from api.new_api import Api 5 | from api.api_recv import Api_recv 6 | from api.info_api import InfoApi 7 | 8 | from api.parameters import Parameter -------------------------------------------------------------------------------- /entities/trainer/__init__.py: -------------------------------------------------------------------------------- 1 | from entities.trainer.circuit_run import CircuitRunTrainer 2 | 3 | _trainer_list = [ 4 | # Tournament coaches 5 | CircuitRunTrainer 6 | ] 7 | 8 | TRAINERS = {c.NAME: c for c in _trainer_list} 9 | 10 | -------------------------------------------------------------------------------- /commons/utils.py: -------------------------------------------------------------------------------- 1 | import json 2 | 3 | def get_config(config_file=None): 4 | if config_file: 5 | config = json.loads(open(config_file, 'r').read()) 6 | else: 7 | config = json.loads(open('config.json', 'r').read()) 8 | 9 | return config 10 | -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | protobuf == 3.20.3 2 | numpy 3 | scipy 4 | matplotlib 5 | 6 | # live plot view stuff 7 | pyqtgraph 8 | PyQt5 9 | 10 | # Referee custom library 11 | #pyVsssReferee 12 | 13 | # Vision custom library 14 | #pySSLVision 15 | 16 | # Path Planning 17 | NeonPathPlanning 18 | 19 | # For real life execution 20 | pyserial 21 | -------------------------------------------------------------------------------- /controller/noController.py: -------------------------------------------------------------------------------- 1 | class NoController(object): 2 | def __init__(self, robot): 3 | self.robot = robot 4 | self.environment = robot.game.environment 5 | self.match = robot.game.match 6 | 7 | def set_desired(self, desired): 8 | self.v = desired[0] 9 | self.w = desired[1] 10 | 11 | def update(self): 12 | return self.v, self.w -------------------------------------------------------------------------------- /controller/__init__.py: -------------------------------------------------------------------------------- 1 | from controller.simple_LQR import SimpleLQR 2 | from controller.simple_LQR import TwoSidesLQR 3 | 4 | from controller.PID import Robot_PID 5 | 6 | from controller.uni_controller import UniController 7 | 8 | from controller.PID_control import PID_control 9 | from controller.PID_control import PID_W_control 10 | 11 | from controller.noController import NoController 12 | -------------------------------------------------------------------------------- /strategy/rsm2025/__init__.py: -------------------------------------------------------------------------------- 1 | # Baseline de estrategias 2 | from strategy.rsm2025.Goalkeeper import Goalkeeper 3 | from strategy.rsm2025.Attacker import MainStriker 4 | from strategy.rsm2025.SecondAttacker import ShadowAttacker 5 | from strategy.rsm2025.Defender import Defender 6 | from strategy.rsm2025.SecondDefender import ShadowDefender 7 | # from strategy.rcx2023.Unstuck import Unstuck 8 | from strategy.rsm2025.Goalkeeper import PushPlay -------------------------------------------------------------------------------- /entities/coach/coach.py: -------------------------------------------------------------------------------- 1 | from abc import ABC, abstractmethod 2 | from os import replace 3 | 4 | class BaseCoach(ABC): 5 | NAME = "BASE_COACH" 6 | def __init__(self, match): 7 | self.match = match 8 | 9 | @abstractmethod 10 | def decide (self): 11 | raise NotImplementedError("Coach needs decide implementation!") 12 | 13 | def get_positions(self, foul, team_color, foul_color, quadrant): 14 | return None -------------------------------------------------------------------------------- /strategy/tests/goFoward.py: -------------------------------------------------------------------------------- 1 | from strategy.BaseStrategy import Strategy 2 | from controller import NoController 3 | 4 | class Foward(Strategy): 5 | def __init__(self, match): 6 | super().__init__(match, 'Foward', controller=NoController) 7 | 8 | 9 | def start(self, robot=None): 10 | super().start(robot=robot) 11 | 12 | def reset(self, robot=None): 13 | super().reset() 14 | if robot: 15 | self.robot = robot 16 | 17 | 18 | def decide(self): 19 | return 30, 30 20 | -------------------------------------------------------------------------------- /strategy/tests/Idle.py: -------------------------------------------------------------------------------- 1 | import math 2 | import algorithms 3 | from strategy.BaseStrategy import Strategy 4 | from commons.math import unit_vector 5 | 6 | import json 7 | import numpy as np 8 | 9 | class Idle(Strategy): 10 | def __init__(self, match): 11 | super().__init__(match, 'Idle') 12 | 13 | 14 | def start(self, robot=None): 15 | super().start(robot=robot) 16 | 17 | def reset(self, robot=None): 18 | super().reset() 19 | if robot: 20 | self.robot = robot 21 | 22 | 23 | def decide(self): 24 | return 0, 0 25 | -------------------------------------------------------------------------------- /entities/plays/__init__.py: -------------------------------------------------------------------------------- 1 | from entities.plays.playbook import Playbook 2 | 3 | from entities.plays.playbook import OneOneOnePlay 4 | from entities.plays.playbook import UnstuckPlay 5 | 6 | from entities.plays.playbook import StuckRobots as StuckRobotsTrigger 7 | from entities.plays.playbook import WaitFor as WaitForTrigger 8 | 9 | from entities.plays.playbook import OnFreeBall 10 | from entities.plays.playbook import OnFreeKick 11 | from entities.plays.playbook import OnKickOff 12 | from entities.plays.playbook import OnGoalKick 13 | from entities.plays.playbook import OnPenaltyKick 14 | -------------------------------------------------------------------------------- /strategy/tests/FIRATest.py: -------------------------------------------------------------------------------- 1 | import time 2 | 3 | from controller.noController import NoController 4 | from strategy.BaseStrategy import Strategy 5 | 6 | 7 | class FiraTest(Strategy): 8 | def __init__(self, match): 9 | self.lt = time.time() 10 | super().__init__(match, "FiraTest", controller=NoController) 11 | 12 | def start(self, robot=None): 13 | super().start(robot=robot) 14 | 15 | def reset(self, robot=None): 16 | super().reset() 17 | if robot: 18 | self.start(robot) 19 | 20 | def decide(self): 21 | return 20, 20 # 0, 10 22 | -------------------------------------------------------------------------------- /entities/coach/__init__.py: -------------------------------------------------------------------------------- 1 | from entities.coach.coach import BaseCoach 2 | 3 | from entities.coach.guideCoach import Coach as GuideCoach 4 | 5 | from entities.coach.test_coach import Coach as TestCoach 6 | 7 | from entities.coach.Backup import Coach as IRON_2025 8 | 9 | from entities.coach.rsm2025_attack import Coach as RSM_2025_Attack 10 | 11 | from entities.coach.rsm2025_defend import Coach as RSM_2025_Defend 12 | 13 | _coach_list = [ 14 | # Tournament coaches 15 | GuideCoach, 16 | TestCoach, 17 | IRON_2025, 18 | RSM_2025_Attack, 19 | RSM_2025_Defend 20 | ] 21 | 22 | COACHES = {c.NAME: c for c in _coach_list} 23 | -------------------------------------------------------------------------------- /pyproject.toml: -------------------------------------------------------------------------------- 1 | [tool.poetry] 2 | name = "neonfc" 3 | version = "0.1.0" 4 | description = "" 5 | authors = ["Raphael-AugustoG "] 6 | readme = "README.md" 7 | 8 | [tool.poetry.dependencies] 9 | python = "^3.12" 10 | numpy = "^1.26.4" 11 | scipy = "^1.13.1" 12 | matplotlib = "^3.9.0" 13 | pyqtgraph = "^0.13.7" 14 | pyserial = "^3.5" 15 | neonpathplanning = "^1.0.1" 16 | protobuf = "3.20.3" 17 | pysslvision = "^2.0.0" 18 | pyvsssreferee = "^2.0.0" 19 | pyqt5-qt5 = "5.15.2" 20 | pyqt5 = "^5.15.10" 21 | 22 | 23 | [build-system] 24 | requires = ["poetry-core"] 25 | build-backend = "poetry.core.masonry.api" 26 | -------------------------------------------------------------------------------- /controller/visualizers/pid_tuner.py: -------------------------------------------------------------------------------- 1 | from live_plot.logger import Writer 2 | 3 | kp = -100 4 | ki = 0 5 | kd = -7.5 6 | 7 | pid_tunner = Writer('pid_tuner', 8 | {'kp': 'FLOAT', 9 | 'ki': 'FLOAT', 10 | 'kd': 'FLOAT'}) 11 | 12 | pid_tunner.write([kp, ki, kd]) 13 | 14 | while True: 15 | 16 | gain = input(f"change in kp({kp}), ki({ki}) or kd({kd}): ") 17 | if gain == 'kp': 18 | kp = float(input('kp: ')) 19 | if gain == 'ki': 20 | ki = float(input('ki: ')) 21 | if gain == 'kd': 22 | kd = float(input('kd: ')) 23 | 24 | pid_tunner.write([kp, ki, kd]) 25 | -------------------------------------------------------------------------------- /live_plot/plot_astar_map.py: -------------------------------------------------------------------------------- 1 | import json 2 | 3 | import socket 4 | import struct 5 | 6 | MCAST_GRP = '224.1.2.3' 7 | MCAST_PORT = 5007 8 | 9 | sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP) 10 | sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) 11 | 12 | sock.bind((MCAST_GRP, MCAST_PORT)) 13 | 14 | mreq = struct.pack("4sl", socket.inet_aton(MCAST_GRP), socket.INADDR_ANY) 15 | 16 | sock.setsockopt(socket.IPPROTO_IP, socket.IP_ADD_MEMBERSHIP, mreq) 17 | 18 | while True: 19 | data = sock.recv(10240) 20 | data = json.loads(data.decode('utf8').replace("'", '"')) 21 | if [a['name'] == 'Astar' for a in data]: 22 | print(data) 23 | -------------------------------------------------------------------------------- /blue_team.json: -------------------------------------------------------------------------------- 1 | { 2 | "network" : { 3 | "multicast_ip": "224.0.0.1", 4 | "referee_ip": "224.5.23.2", 5 | "host_ip": "localhost", 6 | "blue_port": 30011, 7 | "yellow_port": 30012, 8 | "vision_port": 10002, 9 | "command_port": 20011, 10 | "replacer_port": 10004, 11 | "referee_port": 10003, 12 | "api_address": "127.0.0.1", 13 | "api_port" : 43210 14 | }, 15 | "match" : { 16 | "team_side": "left", 17 | "team_color": "blue", 18 | "coach_name": "CBFRS_2022_5V5", 19 | "category": "5v5" 20 | }, 21 | "referee": true, 22 | "data_sender": false, 23 | "api": false 24 | } 25 | -------------------------------------------------------------------------------- /yellow_team.json: -------------------------------------------------------------------------------- 1 | { 2 | "network" : { 3 | "multicast_ip": "224.0.0.1", 4 | "referee_ip": "224.5.23.2", 5 | "host_ip": "localhost", 6 | "blue_port": 30011, 7 | "yellow_port": 30012, 8 | "vision_port": 10002, 9 | "command_port": 20011, 10 | "replacer_port": 10004, 11 | "referee_port": 10003, 12 | "api_address": "127.0.0.1", 13 | "api_port" : 43210 14 | }, 15 | "match" : { 16 | "team_side": "right", 17 | "team_color": "yellow", 18 | "coach_name": "CBFRS_2022_5V5", 19 | "category": "5v5" 20 | }, 21 | "referee": true, 22 | "data_sender": false, 23 | "api": false 24 | } 25 | -------------------------------------------------------------------------------- /config.json: -------------------------------------------------------------------------------- 1 | { 2 | "network" : { 3 | "multicast_ip": "224.0.0.1", 4 | "referee_ip": "224.5.23.2", 5 | "host_ip": "localhost", 6 | "blue_port": 30011, 7 | "yellow_port": 30012, 8 | "vision_port": 10002, 9 | "command_port": 20011, 10 | "replacer_port": 10004, 11 | "referee_port": 10003, 12 | "api_address": "127.0.0.1", 13 | "api_port" : 43210, 14 | "api_recv_port": 43211 15 | }, 16 | "match" : { 17 | "team_side": "left", 18 | "team_color": "blue", 19 | "coach_name": "TEST", 20 | "category": "3v3" 21 | }, 22 | "referee": false, 23 | "data_sender": true, 24 | "api": false 25 | } 26 | -------------------------------------------------------------------------------- /strategy/DebugTools.py: -------------------------------------------------------------------------------- 1 | from strategy.BaseStrategy import Strategy 2 | 3 | from algorithms.potential_fields.plotter import PotentialDataExporter 4 | 5 | import controller 6 | 7 | class DebugPotentialFieldStrategy(Strategy): 8 | def __init__(self, match, name, controller=controller.SimpleLQR, controller_kwargs={}): 9 | super().__init__(match, name, controller, controller_kwargs) 10 | self.analyzed = None 11 | 12 | def start(self, robot=None): 13 | super().start(robot) 14 | self.exporter = PotentialDataExporter(self.robot.get_name()) 15 | 16 | def decide(self, behaviour): 17 | ''' 18 | recebe o Potential Field para que a analise seja feita e retorna 0, 0 19 | ''' 20 | self.exporter.export(behaviour, self.robot, self.match.ball) 21 | 22 | return [0, 0] 23 | -------------------------------------------------------------------------------- /strategy/tests/dwaScratch.py: -------------------------------------------------------------------------------- 1 | import math 2 | import algorithms 3 | from strategy.BaseStrategy import Strategy 4 | from commons.math import unit_vector 5 | 6 | import json 7 | import numpy as np 8 | 9 | class DwaScratch(Strategy): 10 | def __init__(self, match): 11 | self.match = match 12 | self.game = self.match.game 13 | super().__init__(match) 14 | 15 | def start(self, robot=None): 16 | self.dwa = algorithms.dwa.DynamicWindowApproach(robot, self.game) 17 | self.controller = self.dwa 18 | if (robot): 19 | self.robot = robot 20 | 21 | def reset(self, robot=None): 22 | super().reset() 23 | if robot: 24 | self.robot = robot 25 | 26 | def set_desired(self, desired): 27 | return 28 | 29 | def decide(self): 30 | return 0, 0 31 | 32 | -------------------------------------------------------------------------------- /controller/visualizers/plot_script.py: -------------------------------------------------------------------------------- 1 | import sys 2 | import random 3 | import subprocess 4 | 5 | from itertools import count 6 | from collections import deque 7 | 8 | import matplotlib.pyplot as plt 9 | from matplotlib.animation import FuncAnimation 10 | 11 | y_vals_a = [] 12 | y_vals_l = [] 13 | 14 | fig, axs = plt.subplots(2) 15 | 16 | fig.suptitle('PID Controller') 17 | 18 | with open("pid.log", "r") as a_file: 19 | for line in a_file: 20 | parts_l = [float(y) for y in line.split(',')][:2] 21 | parts_a = [float(y) for y in line.split(',')][3:-1] 22 | 23 | y_vals_l.append(parts_l) 24 | y_vals_a.append(parts_a) 25 | 26 | 27 | line_0bjects = axs[0].plot(y_vals_l) 28 | axs[0].legend(iter(line_0bjects), ('Desired', 'Actual')) 29 | 30 | line_0bjects = axs[1].plot(y_vals_a) 31 | axs[1].legend(iter(line_0bjects), ('Desired', 'Actual')) 32 | 33 | plt.tight_layout() 34 | plt.show() -------------------------------------------------------------------------------- /api/parameters.py: -------------------------------------------------------------------------------- 1 | from api import InfoApi 2 | 3 | class Parameter: 4 | def __init__(self, default, address): 5 | InfoApi.parameter_list.append(self) 6 | self.value = default 7 | self.addr = address 8 | 9 | def update(self, info): 10 | self.value = info.get(self.addr.upper(), self.value) 11 | 12 | def __add__(self, value): 13 | return self.value + value 14 | 15 | def __mul__(self, value): 16 | return self.value * value 17 | 18 | def __rmul__(self, value): 19 | return self.value * value 20 | 21 | def __pow__(self, power): 22 | return self.value ** power 23 | 24 | def __lt__(self, value): 25 | return self.value < value 26 | 27 | def __gt__(self, value): 28 | return self.value > value 29 | 30 | def __le__(self, value): 31 | return self.value <= value 32 | 33 | def __ge__(self, value): 34 | return self.value >= value -------------------------------------------------------------------------------- /strategy/tests/asScratch2.py: -------------------------------------------------------------------------------- 1 | from algorithms.astar import PathAstar 2 | from strategy.BaseStrategy import Strategy 3 | 4 | class AsScratch2(Strategy): 5 | def __init__(self, match): 6 | super().__init__(match, "AstarVoronoi") 7 | self.last_update = 0 8 | 9 | def start(self, robot=None): 10 | super().start(robot=robot) 11 | self.astar = PathAstar(self.match) 12 | 13 | def reset(self, robot=None): 14 | super().reset() 15 | if robot: 16 | self.start(robot) 17 | 18 | def decide(self): 19 | robot_pos = [self.robot.x, self.robot.y] 20 | ball_pos = [self.match.ball.x, self.match.ball.y] 21 | 22 | obstacles = [ 23 | [r.x, r.y] for r in self.match.opposites] + [ 24 | [r.x, r.y] for r in self.match.robots 25 | if r.robot_id != self.robot.robot_id 26 | ] 27 | 28 | r_v = self.astar.calculate(robot_pos, ball_pos, obstacles) 29 | 30 | return r_v 31 | -------------------------------------------------------------------------------- /algorithms/astar/fieldGraph.py: -------------------------------------------------------------------------------- 1 | from algorithms.astar.astar import Node 2 | 3 | class FieldGraph(): 4 | def __init__(self): 5 | self.start = None 6 | self.nodes = None 7 | 8 | def set_start(self, initial_node): 9 | self.start = initial_node 10 | 11 | def set_nodes(self, nodes = []): 12 | self.nodes = nodes 13 | 14 | def update_neighbours(self, edges): 15 | for edge in edges: 16 | self.add_edge(edge) 17 | 18 | def add_edge(self, edge = []): 19 | edge[0].neighbours.append(edge[1]) 20 | edge[1].neighbours.append(edge[0]) 21 | 22 | def remove_edge(self, edge = []): 23 | edge[0].neighbours.remove(edge[1]) 24 | edge[1].neighbours.remove(edge[0]) 25 | 26 | def generate_graph(self, nodes = [], edges = []): 27 | """ 28 | generate_graph is an abstract method that can be used by robots to create 29 | the nodes on the field and their edges. 30 | """ 31 | return self.start 32 | -------------------------------------------------------------------------------- /strategy/BaseStrategy.py: -------------------------------------------------------------------------------- 1 | from abc import ABC 2 | import controller 3 | 4 | class Strategy(ABC): 5 | def __init__(self, match, name, controller=controller.SimpleLQR, controller_kwargs={}): 6 | self.match = match 7 | self._controller = controller 8 | self._ctr_kwargs = controller_kwargs 9 | self.name = name 10 | 11 | def start(self, robot=None): 12 | ''' 13 | Inicializa a estrategia 14 | ''' 15 | self.controller = self._controller(robot, **self._ctr_kwargs) 16 | 17 | if robot: 18 | self.robot = robot 19 | 20 | def reset(self): 21 | pass 22 | 23 | def update(self): 24 | return self.controller.update() 25 | 26 | def set_desired(self, desired): 27 | self.controller.set_desired(desired) 28 | 29 | def decide(self): 30 | ''' 31 | Calcula o proximo comando para o robo 32 | retorna: vetor (x, y) objetivo 33 | ''' 34 | 35 | return self.robot.x, self.robot.y -------------------------------------------------------------------------------- /config_real_life.json: -------------------------------------------------------------------------------- 1 | { 2 | "network" : { 3 | "multicast_ip": "224.5.23.2", 4 | "referee_ip": "224.5.23.2", 5 | "host_ip": "localhost", 6 | "blue_port": 30011, 7 | "yellow_port": 30012, 8 | "vision_port": 10015, 9 | "command_port": 20011, 10 | "replacer_port": 10004, 11 | "referee_port": 10312, 12 | "api_address": "127.0.0.1", 13 | "api_port" : 43210, 14 | "api_recv_port": 43211 15 | }, 16 | "match" : { 17 | "team_side": "left", 18 | "team_color": "blue", 19 | "coach_name": "RSM_2025_Attack", 20 | "category": "3v3", 21 | "robot_ids": [ 22 | 5, 23 | 7, 24 | 8 25 | ], 26 | "opposite_ids": [ 27 | 5, 28 | 7, 29 | 8 30 | ] 31 | }, 32 | "comm": { 33 | "comm_port": "/dev/ttyACM0", 34 | "baud_rate": 115200 35 | }, 36 | "referee": false, 37 | "data_sender": true, 38 | "api": false 39 | } 40 | -------------------------------------------------------------------------------- /live_plot/visualizer.py: -------------------------------------------------------------------------------- 1 | from abc import ABC, abstractmethod 2 | from live_plot import logger 3 | import matplotlib.pyplot as plt 4 | import matplotlib.animation as animation 5 | 6 | 7 | class Visualizer(ABC): 8 | def __init__(self): 9 | self.readers = {table: logger.Reader(table) for table in self.tables} 10 | self.values = {table: None for table in self.tables} 11 | self.__animation_object = None 12 | 13 | def update(self): 14 | self.values = {table: reader.read() for table, reader in self.readers.items()} 15 | 16 | 17 | class MPLVisualizer(Visualizer): 18 | def __init__(self): 19 | super().__init__() 20 | 21 | self.fig, self.ax = plt.subplots() 22 | 23 | @abstractmethod 24 | def _draw(self, frame, **kwargs): 25 | pass 26 | 27 | def animation(self, interval=1): 28 | self.__animation_object = animation.FuncAnimation(self.fig, self._draw, interval=interval, blit=True) 29 | return self.__animation_object 30 | 31 | def __call__(self, interval=10): 32 | return self.animation(interval=interval) 33 | -------------------------------------------------------------------------------- /fields.json: -------------------------------------------------------------------------------- 1 | { 2 | "3v3": { 3 | "field_size": [1.500, 1.300], 4 | "small_area": { 5 | "offensive": [1.350, 0.300, 0.150, 0.700], 6 | "defensive": [0, 0.300, 0.150, 0.700] 7 | }, 8 | "quadrant_positions": { 9 | "q1": [1.125, 1.050], 10 | "q2": [0.375, 1.050], 11 | "q3": [0.375, 0.250], 12 | "q4": [1.125, 0.250] 13 | }, 14 | "free_kick": { 15 | "left": [0.375, 0.650], 16 | "right": [1.125, 0.650] 17 | } 18 | }, 19 | "5v5": { 20 | "field_size": [2.200, 1.800], 21 | "small_area": { 22 | "offensive": [2.050, 0.650, 0.150, 0.500], 23 | "defensive": [0, 0.650, 0.150, 0.500] 24 | }, 25 | "quadrant_positions": { 26 | "q1": [1.650, 1.500], 27 | "q2": [0.550, 1.500], 28 | "q3": [0.550, 0.300], 29 | "q4": [1.650, 0.300] 30 | }, 31 | "free_kick": { 32 | "left": [0.375, 0.900], 33 | "right": [1.825, 0.900] 34 | } 35 | } 36 | } 37 | -------------------------------------------------------------------------------- /live_plot/circuit_plot.py: -------------------------------------------------------------------------------- 1 | from bokeh.plotting import curdoc, figure 2 | import json 3 | import socket 4 | 5 | UDP_IP_ADDRESS = 'localhost' 6 | UDP_PORT_NO = 43215 7 | 8 | serverSock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) 9 | serverSock.bind((UDP_IP_ADDRESS, UDP_PORT_NO)) 10 | 11 | def update(): 12 | raw_data, addr = serverSock.recvfrom(4096) 13 | data = json.loads(raw_data) 14 | x = data['robot']['x'] 15 | y = data['robot']['y'] 16 | print(data) 17 | circuit_x = [p[0] for p in data['circuit']] 18 | circuit_y = [p[1] for p in data['circuit']] 19 | 20 | obstacles_x = [p['x'] for p in data['obstacles']] 21 | obstacles_y = [p['y'] for p in data['obstacles']] 22 | 23 | r.data_source.stream({'x': [x], 'y': [y]}) 24 | 25 | c.data_source.stream({'x': circuit_x, 'y': circuit_y}) 26 | o.data_source.stream({'x': obstacles_x, 'y': obstacles_y}) 27 | 28 | 29 | 30 | p = figure() 31 | r = p.circle([], []) 32 | c = p.circle([], [], color="olive") 33 | o = p.circle([], [], color="red") 34 | 35 | curdoc().add_root(p) 36 | curdoc().add_root(c) 37 | 38 | curdoc().add_periodic_callback(update, 1) -------------------------------------------------------------------------------- /api/api_recv.py: -------------------------------------------------------------------------------- 1 | from socket import * 2 | import json 3 | import threading 4 | import struct 5 | import time, collections 6 | 7 | class Api_recv(threading.Thread): 8 | def __init__(self, match, address, port): 9 | super(Api_recv, self).__init__() 10 | 11 | BUFFER_SIZE = 4096 12 | 13 | self.match = match 14 | self.address = address 15 | self.port = port 16 | self.buffer_size = BUFFER_SIZE 17 | self.decod_data = None 18 | 19 | self.kill_recieved = False 20 | 21 | def connect_info(self,info_api): 22 | self.info_api = info_api 23 | 24 | # Receives data 25 | def run(self): 26 | self.obj_socket = socket(AF_INET, SOCK_DGRAM) 27 | self.obj_socket.bind((self.address, self.port)) 28 | print("Starting api_recv...") 29 | 30 | while not self.kill_recieved: 31 | data, origem = self.obj_socket.recvfrom(self.buffer_size) 32 | decoded_data = json.loads(data.decode()) 33 | # Feedback commands from socket (e.g. an interface) 34 | 35 | self.info_api.update_recv(decoded_data) 36 | 37 | self.decod_data = decoded_data -------------------------------------------------------------------------------- /strategy/tests/__init__.py: -------------------------------------------------------------------------------- 1 | # Baseline de estrategias 2 | from strategy.tests.pfGoalKeeper import GoalKeeper 3 | from strategy.tests.pfAttacker import Attacker 4 | from strategy.tests.pfMidFielder import MidFielder 5 | 6 | # Para proposito de criação de novas estrategias 7 | from strategy.tests.Idle import Idle 8 | from strategy.tests.pfScratch import Scratch 9 | from strategy.tests.dwaScratch import DwaScratch 10 | 11 | from strategy.tests.midfieldercx import MidFielderSupporter 12 | from strategy.tests.goalkeeper_rcx import GoalKeeperRCX 13 | from strategy.tests.attacker_rcx import SpinnerAttacker 14 | 15 | from strategy.tests.asScratch2 import AsScratch2 16 | from strategy.tests.newAttacker import newAttacker 17 | from strategy.tests.uvf_attacker import Attacker as UVFAttacker 18 | 19 | from strategy.tests.PID_attacker import Attacker as PID_Attacker 20 | from strategy.tests.PID_test import PID_Test 21 | from strategy.tests.PID_tuner import PIDTuner 22 | 23 | from strategy.tests.uvf_test import UVF_Test 24 | from strategy.tests.potential_fields_test import Fields_Test 25 | from strategy.tests.limit_cycle_test import LimitCycle_Test 26 | 27 | from strategy.tests.goFoward import Foward -------------------------------------------------------------------------------- /algorithms/potential_fields/plotter.py: -------------------------------------------------------------------------------- 1 | import json 2 | 3 | class PotentialDataExporter(object): 4 | def __init__(self, name): 5 | self.file = open( 6 | '{}|potential_field.log'.format(name), 7 | 'w' 8 | ) 9 | 10 | def export(self, behaviour, robot, ball): 11 | X = [] 12 | Y = [] 13 | U = [] 14 | V = [] 15 | 16 | for x in range(-10, 150 + 10, 2): 17 | x = x/100.0 18 | for y in range(-10, 130 + 10, 2): 19 | y = y/100.0 20 | robot.x = x 21 | robot.y = y 22 | res = behaviour.compute([x, y]) 23 | X.append(x) 24 | Y.append(y) 25 | U.append(res[0]) 26 | V.append(res[1]) 27 | 28 | plot_file = { 29 | "x": X, 30 | "y": Y, 31 | "u": U, 32 | "v": V, 33 | "robot_x": robot.x, 34 | "robot_y": robot.y, 35 | "ball_x": ball.x, 36 | "ball_y": ball.y, 37 | "behaviour": behaviour.name 38 | } 39 | 40 | self.file.write(json.dumps(plot_file) + "||") 41 | -------------------------------------------------------------------------------- /strategy/tests/pathfinderTest.py: -------------------------------------------------------------------------------- 1 | from typing import Tuple 2 | 3 | from controller import PID_control 4 | from strategy.BaseStrategy import Strategy 5 | import time 6 | 7 | class PathfinderTest(Strategy): 8 | 9 | time_init = 0 10 | 11 | def __init__(self, match): 12 | super().__init__( 13 | match, 'pathfinderTest', 14 | controller=PID_control 15 | ) 16 | self.time_init = time.time() 17 | self.field_limits = [0.75*2, 0.65*2] 18 | 19 | def get_play(self): return None 20 | 21 | def decide(self) -> tuple[float, float]: 22 | phaseTime = self.time_init % 60 #segundos pq Python é avançado demais pra usar milis 23 | #ret = tuple[0.0,0.0] 24 | if phaseTime < 10: return self.field_limits[0] * .25 , self.field_limits[1] * .75 25 | elif phaseTime < 20: return self.field_limits[0] * .25 , self.field_limits[1] * .25 26 | elif phaseTime < 30: return self.field_limits[0] * .75 , self.field_limits[1] * .25 27 | elif phaseTime < 40: return self.field_limits[0] * .75 , self.field_limits[1] * .75 28 | elif phaseTime < 50: return self.field_limits[0] * .5 , self.field_limits[1] * .5 29 | return 0.0, 0.0 30 | -------------------------------------------------------------------------------- /entities/coach/guideCoach.py: -------------------------------------------------------------------------------- 1 | from entities.coach.coach import BaseCoach 2 | 3 | import strategy 4 | 5 | class Coach(BaseCoach): # heranca da classe abstrata 6 | NAME = "CoachTeste" 7 | def __init__(self, match): 8 | super().__init__(match) # chamada do metodo da classe mae 9 | 10 | # vamos usar strategies de teste por enquanto, essa deixa o robo parado 11 | self.attacker_strategy = strategy.tests.newAttacker(self.match) 12 | self.midfielder_strategy = strategy.tests.Idle(self.match) 13 | self.goalkeeper_strategy = strategy.tests.Idle(self.match) 14 | 15 | def decide(self): 16 | # esta lista eh ordenada em [robot_0, ..., robot_n] 17 | robots = [r.robot_id for r in self.match.robots] 18 | strategies = [ 19 | self.goalkeeper_strategy, 20 | self.attacker_strategy, self.midfielder_strategy 21 | 22 | ] 23 | for robot, strategy in zip(robots, strategies): 24 | if self.match.robots[robot].strategy is not None: 25 | # vamos evitar chamar o start todo frame 26 | # pode ser que essa strategia seja pesada de carregar 27 | continue 28 | self.match.robots[robot].strategy = strategy 29 | self.match.robots[robot].start() 30 | -------------------------------------------------------------------------------- /strategy/tests/pfMidFielder.py: -------------------------------------------------------------------------------- 1 | import math 2 | import algorithms 3 | from strategy.BaseStrategy import Strategy 4 | from commons.math import unit_vector 5 | 6 | import json 7 | import numpy as np 8 | 9 | class MidFielder(Strategy): 10 | def __init__(self, match, plot_field=False): 11 | super().__init__(match) 12 | 13 | """ 14 | Ainda precisa ser feito 15 | """ 16 | 17 | self.plot_field = plot_field 18 | self.exporter = None 19 | 20 | self.base_rules = algorithms.fields.PotentialField( 21 | self.match, 22 | name="{}|BaseRulesBehaviour".format(self.__class__) 23 | ) 24 | 25 | 26 | 27 | def start(self, robot=None): 28 | super().start(robot=robot) 29 | 30 | if self.plot_field: 31 | self.exporter = algorithms.fields.PotentialDataExporter(self.robot.get_name()) 32 | 33 | 34 | def reset(self, robot=None): 35 | super().reset() 36 | if robot: 37 | self.robot = robot 38 | 39 | 40 | def decide(self): 41 | dist_to_ball = np.linalg.norm( 42 | np.array([self.robot.x, self.robot.y]) - 43 | np.array([self.match.ball.x, self.match.ball.y]) 44 | ) 45 | 46 | behaviour = self.base_rules 47 | 48 | return behaviour.compute([self.robot.x, self.robot.y]) 49 | 50 | -------------------------------------------------------------------------------- /strategy/tests/PID_test.py: -------------------------------------------------------------------------------- 1 | from NeonPathPlanning import LimitCycle 2 | import math 3 | from controller.PID_control import PID_control, PID_W_control 4 | from controller.simple_LQR import TwoSidesLQR, SimpleLQR 5 | from strategy.BaseStrategy import Strategy 6 | from collections import deque 7 | from controller.uni_controller import UniController 8 | import algorithms 9 | import time 10 | 11 | class PID_Test(Strategy): 12 | def __init__(self, match, plot_field=False): 13 | self.lt = time.time() 14 | super().__init__(match, "PID_Test", controller=PID_W_control) 15 | 16 | self.circuit = [(1.1, .40), (1.1, .90), (.35, .65)]#, (.4, .90), (.4, .40)] 17 | self.circuit = deque(self.circuit) 18 | self.dl = 0.000001 19 | 20 | def next_point(self): 21 | point = self.circuit[0] 22 | dx = point[0] - self.robot.x 23 | dy = point[1] - self.robot.y 24 | 25 | if math.sqrt(dx**2 + dy**2) < 0.05: 26 | self.circuit.rotate(-1) 27 | print("Change point! ", self.circuit[0]) 28 | 29 | return self.circuit[0] 30 | 31 | def start(self, robot=None): 32 | super().start(robot=robot) 33 | 34 | def reset(self, robot=None): 35 | super().reset() 36 | if robot: 37 | self.start(robot) 38 | 39 | def decide(self): 40 | 41 | desired = self.next_point() 42 | 43 | return desired 44 | -------------------------------------------------------------------------------- /entities/coach/test_coach.py: -------------------------------------------------------------------------------- 1 | from entities.coach.coach import BaseCoach 2 | 3 | # from strategy.tests.FIRATest import FiraTest 4 | # from strategy.tests.goFoward import Foward 5 | 6 | import strategy 7 | import strategy.tests.FIRATest 8 | import strategy.tests.goFoward 9 | 10 | class Coach(BaseCoach): # heranca da classe abstrata 11 | NAME = "TEST" 12 | 13 | def __init__(self, match): 14 | super().__init__(match) # chamada do metodo da classe mae 15 | 16 | # vamos usar strategies de teste por enquanto, essa deixa o robo parado 17 | self._1 = strategy.tests.FIRATest.FiraTest(self.match) 18 | self._2 = strategy.tests.goFoward.Foward(self.match) 19 | self._3 = strategy.tests.goFoward.Foward(self.match) # 20 | 21 | def decide(self): 22 | # esta lista eh ordenada em [robot_0, ..., robot_n] 23 | robots = [(i, r.robot_id) for i, r in enumerate(self.match.robots)] 24 | strategies = [ 25 | self._1, 26 | self._2, 27 | self._3 28 | ] 29 | 30 | for robot, strategy in zip(robots, strategies): 31 | if self.match.robots[robot[0]].strategy is not None: 32 | # vamos evitar chamar o start todo frame 33 | # pode ser que essa strategia seja pesada de carregar 34 | continue 35 | self.match.robots[robot[0]].strategy = strategy 36 | self.match.robots[robot[0]].start() -------------------------------------------------------------------------------- /strategy/tests/PID_attacker.py: -------------------------------------------------------------------------------- 1 | import math 2 | import api 3 | from collections import deque 4 | 5 | from strategy.BaseStrategy import Strategy 6 | from controller import PID_control 7 | 8 | 9 | class Attacker(Strategy): 10 | def __init__(self, match, name = 'PID-Attacker'): 11 | super().__init__(match, 12 | name=name, 13 | controller=PID_control 14 | ) 15 | 16 | self.circuit = [(0, 0)] 17 | 18 | def start(self, robot=None): 19 | super().start(robot=robot) 20 | 21 | 22 | def reset(self, robot=None): 23 | super().reset() 24 | if robot: 25 | self.start(robot) 26 | 27 | def set_circuit(self, circuit): 28 | self.circuit = deque(circuit) 29 | 30 | def next_point(self): 31 | point = self.circuit[0] 32 | dx = point[0] - self.robot.x 33 | dy = point[1] - self.robot.y 34 | 35 | if math.sqrt(dx**2 + dy**2) < 0.025: 36 | self.circuit.rotate(-1) 37 | print("Change point! ", self.circuit[0]) 38 | 39 | return self.circuit[0] 40 | 41 | 42 | def decide(self): 43 | api.Api().send_custom_data( 44 | { 45 | 'circuit': list(self.circuit), 46 | 'robot': {'x': self.robot.x, 'y': self.robot.y, 'theta': self.robot.theta}, 47 | 'speed': (self.robot.vx**2 + self.robot.vy**2)**.5 48 | } 49 | ) 50 | return self.next_point() 51 | -------------------------------------------------------------------------------- /fields/field.py: -------------------------------------------------------------------------------- 1 | import json 2 | 3 | class Field(): 4 | def __init__(self, category="3v3"): 5 | self.game_mode = category 6 | self.source = json.loads(open('fields.json', 'r').read()) 7 | self.field = self.source.get(self.game_mode) 8 | 9 | #the dimensios are defined in fields.json and all of them are in meters 10 | def get_dimensions(self): 11 | #get field dimensions and return width and height 12 | field_size = self.field.get('field_size', 0) 13 | return field_size 14 | 15 | def get_small_area(self, team): 16 | #this function return the coordinates (x1, y1, width, height) of the small area based on arg team, 17 | #that recieves team color 18 | areas = self.field.get('small_area', 0) 19 | small_area = areas.get(team) 20 | return small_area 21 | 22 | def get_quadrant_position(self, quad): 23 | #quadrants are definied as the quadrants from trigonometry, therefore, q1 is in upper right corner 24 | #and they are in counterclockwise. 25 | #return quadrant positions (x, y) based on arg quad, that recieves an integer number. 26 | quadrants = self.field.get('quadrant_positions') 27 | quad_dimen = quadrants.get(f'q{quad}') 28 | return quad_dimen 29 | 30 | def get_free_kick_position(self, side): 31 | #return free kick position (x, y) based on arg side, that recieves the field side 32 | free_kicks = self.field.get("free_kick") 33 | fk_pos = free_kicks.get(side) 34 | return fk_pos 35 | -------------------------------------------------------------------------------- /entities/trainer/circuit_run.py: -------------------------------------------------------------------------------- 1 | from entities.trainer.base_trainer import Trainer 2 | import strategy 3 | 4 | 5 | class CircuitRunTrainer(Trainer): 6 | NAME = 'TEST' 7 | def __init__(self, lab_match, n_iterations = 1, train_name="CircuitRunning"): 8 | super().__init__(lab_match, n_iterations, train_name) 9 | 10 | self.robot = self.match.essay_robot 11 | 12 | fx, fy = self.match.game.field.get_dimensions()[0]/2, self.match.game.field.get_dimensions()[1]/2 13 | 14 | self.circuit = [ 15 | (0.370 + fx, 0.430 + fy), 16 | (0.370 + fx, 0 + fy), 17 | (0.370 + fx, -0.430 + fy), 18 | 19 | (0 + fx, -0.430 + fy), 20 | 21 | (-0.370 + fx, -0.430 + fy), 22 | (-0.370 + fx, 0 + fy), 23 | (-0.370 + fx, 0.430 + fy), 24 | 25 | (0 + fx, 0.430 + fy), 26 | ] 27 | 28 | self.train_strategy = strategy.tests.UVF_Test(self.match) 29 | 30 | # self.train_strategy.set_circuit(self.circuit) 31 | 32 | self.idle_strategy = strategy.tests.Idle(self.match) 33 | 34 | 35 | def execute(self): 36 | robots = self.match.robots 37 | 38 | for robot in robots: 39 | if robot.strategy is not None: 40 | continue 41 | 42 | if robot.robot_id == 0: 43 | robot.strategy = self.train_strategy 44 | else: 45 | robot.strategy = self.idle_strategy 46 | 47 | robot.start() 48 | 49 | def setup(self): 50 | super().setup() 51 | 52 | self.repositioning_robot(self.robot, [0, 0, 0]) 53 | self.execute() 54 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # NeonFC - Neon Futebol Clube 2 | 3 | ### Software de estratégia 4 | 5 | Software desenvolvido para RSM 2025 para o Futebol Mini. 6 | 7 | ### Instalação 8 | 9 | #### Para usar o NeonFC você irá precisar ter instalado em sua máquina: 10 | 11 | - Python == 3.12.1 12 | - [Poetry](https://python-poetry.org/) 13 | 14 | Além de uma interface que irá enviar os dados do campo. Atualmente o [FIRASim](https://github.com/VSSSLeague/FIRASim) é suportado. 15 | 16 | #### passo-a-passo da instalação: 17 | 18 | 1. Clonar o repositório 19 | ```bash 20 | git clone https://github.com/project-neon/NeonFC.git 21 | ``` 22 | 23 | 2. Instalar dependências usando poetry: 24 | 25 | ```bash 26 | cd NeonFC 27 | poetry install 28 | ``` 29 | 30 | 2.1. Antes de executar, é importante checar o arquivo de configuração para verificar se as portas e endereços de rede estão compativeis com o software de simulação. 31 | 32 | No FiraSIM: 33 | 34 | ![Aba de Comunicação do FIRASim](readme/FiraSIM-comtab.png "Aba de Comunicação do FIRASim") 35 | 36 | No ```config.json```: 37 | 38 | ```json 39 | { 40 | "network" : { 41 | "multicast_ip": "224.0.0.1", 42 | "host_ip": "localhost", 43 | "blue_port": 30011, 44 | "yellow_port": 30012, 45 | "vision_port": 10020, 46 | "command_port": 20011 47 | }, 48 | "match" : { 49 | "robots": 5 50 | } 51 | } 52 | ``` 53 | 54 | 3. Agora é só rodar ```Python main.py``` 55 | 56 | ### Desenvolvimento 57 | 58 | Esse repositório é desenvolvido pela equipe Project Neon, mas sinta-se livre para Forkar! 59 | 60 | Manual de desenvolvimento ainda esta WIP mas as documentação irá ser construida [aqui](https://github.com/project-neon/NeonFC/wiki). 61 | -------------------------------------------------------------------------------- /api/new_api.py: -------------------------------------------------------------------------------- 1 | from socket import * 2 | import json 3 | import time 4 | import threading 5 | 6 | 7 | class SingletonMeta(type): 8 | """ 9 | The Singleton class can be implemented in different ways in Python. Some 10 | possible methods include: base class, decorator, metaclass. We will use the 11 | metaclass because it is best suited for this purpose. 12 | source: https://refactoring.guru/design-patterns/singleton/python/example 13 | """ 14 | 15 | _instances = {} 16 | 17 | def __call__(cls, *args, **kwargs): 18 | """ 19 | Possible changes to the value of the `__init__` argument do not affect 20 | the returned instance. 21 | """ 22 | if cls not in cls._instances: 23 | instance = super().__call__(*args, **kwargs) 24 | cls._instances[cls] = instance 25 | return cls._instances[cls] 26 | 27 | class Api(metaclass=SingletonMeta): 28 | 29 | #sender_thread: threading.Thread 30 | 31 | def __init__(self, address, port): 32 | self.address = address 33 | self.port = port 34 | self.kill_recieved = False 35 | 36 | # Initiate socket connection 37 | def start(self): 38 | self.obj_socket = socket(AF_INET, SOCK_DGRAM) 39 | 40 | # Sends dict game data to socket listener 41 | def send_data(self, info_api): 42 | while not self.kill_recieved: 43 | data_dict = info_api.organize_send() 44 | msg = json.dumps(data_dict) 45 | self.obj_socket.sendto(msg.encode(), (self.address, self.port)) 46 | 47 | 48 | def send_custom_data(self, data): 49 | msg = json.dumps(data) 50 | self.obj_socket.sendto(msg.encode(), (self.address, self.port)) -------------------------------------------------------------------------------- /api/info_api.py: -------------------------------------------------------------------------------- 1 | import entities 2 | 3 | class InfoApi(): 4 | parameter_list = [] 5 | 6 | def __init__(self, match, robots, opposites, coach, ball, parameters) : 7 | 8 | self.match = match 9 | self.robots = robots 10 | self.opposites = opposites 11 | self.coach = coach 12 | self.ball = ball 13 | self.parameters = parameters 14 | 15 | self.coach_list = [] 16 | for coach_name, entitie in entities.coach.COACHES.items(): 17 | self.coach_list.append(coach_name) 18 | 19 | self.data = {} 20 | 21 | def organize_send(self): 22 | data_send = { 23 | 'MATCH': 24 | {'COACH_NAME' : self.match.coach_name, 25 | 'COACH_LIST' : self.coach_list 26 | }, 27 | 'TEAM_ROBOTS': 28 | {'ROBOT_POS' : {f"{robot.robot_id}": (robot.x, robot.y, robot.theta) for robot in self.robots}, 29 | 'STRATEGY' : {f"{robot.robot_id}": robot.strategy.name for robot in self.robots} 30 | }, 31 | 'OPPOSITE_ROBOTS': 32 | {'ROBOT_POS' : {f"{robot.robot_id}": (robot.x, robot.y, robot.theta) for robot in self.opposites} 33 | }, 34 | 'BALL': 35 | {'BALL_POS' : (self.ball.x, self.ball.y) 36 | } 37 | } 38 | 39 | self.save_data(data_send) 40 | 41 | return data_send 42 | 43 | 44 | def update_recv(self,info_recv): 45 | self.match.update_information(info_recv['MATCH']) 46 | 47 | 48 | for parameter in self.parameter_list: 49 | parameter.update(info_recv.get('PARAMETERS', {})) 50 | 51 | self.save_data(info_recv) 52 | 53 | 54 | def save_data(self,info_save): 55 | 56 | for i in info_save: 57 | self.data.update({i: info_save[i]}) 58 | 59 | -------------------------------------------------------------------------------- /strategy/tests/uvf_test.py: -------------------------------------------------------------------------------- 1 | import math 2 | import time 3 | from collections import deque 4 | from strategy.BaseStrategy import Strategy 5 | from controller import PID_control, SimpleLQR, TwoSidesLQR, UniController 6 | from NeonPathPlanning import UnivectorField 7 | 8 | class UVF_Test(Strategy): 9 | def __init__(self, match, plot_field=False): 10 | super().__init__(match, name="UVF_Test", controller=UniController) 11 | 12 | self.circuit = [(0.375, 0.25), (1.125, 0.25), (0.75, 0.65), (1.125, 1.05), (0.375, 1.05)] 13 | self.circuit = deque(self.circuit) 14 | self.dl = 0.000001 15 | 16 | def next_point(self): 17 | point = self.circuit[0] 18 | dx = point[0] - self.robot.x 19 | dy = point[1] - self.robot.y 20 | 21 | if math.sqrt(dx**2 + dy**2) < 0.05: 22 | self.circuit.rotate(-1) 23 | print("Change point! ", self.circuit[0]) 24 | 25 | return self.circuit[0] 26 | 27 | def start(self, robot=None): 28 | super().start(robot=robot) 29 | 30 | self.Univector = UnivectorField(n=6, rect_size=.375) 31 | 32 | def reset(self, robot=None): 33 | super().reset() 34 | if robot: 35 | self.start(robot) 36 | 37 | def decide(self): 38 | 39 | ball_x, ball_y = self.match.ball.x, self.match.ball.y 40 | theta_ball = math.atan2(0.65 - ball_y,1.6 - ball_x) 41 | ball_rx, ball_ry = ball_x + .05*math.cos(theta_ball), ball_y + .05*math.sin(theta_ball) 42 | 43 | self.Univector.set_target(g=(ball_x, ball_y), r=(ball_rx, ball_ry)) 44 | 45 | x, y = self.robot.x, self.robot.y 46 | 47 | theta_d = self.Univector.compute((x, y)) 48 | theta_f = self.Univector.compute((x + self.dl*math.cos(self.robot.theta), y + self.dl*math.sin(self.robot.theta))) 49 | 50 | return theta_d, theta_f 51 | -------------------------------------------------------------------------------- /entities/Ball.py: -------------------------------------------------------------------------------- 1 | from collections import deque 2 | import copy 3 | 4 | def speed(_list, _fps): 5 | if len(_list) <= 1: 6 | return 0 7 | 8 | speed_fbf = [ 9 | (v - i) for i, v 10 | in zip( 11 | _list, 12 | list(_list)[1:] 13 | ) 14 | ] 15 | 16 | return _fps * (sum(speed_fbf)/len(speed_fbf)) 17 | 18 | 19 | class Ball(object): 20 | 21 | def __init__(self, game): 22 | self.game = game 23 | self.current_data = [] 24 | 25 | self._frames = { 26 | 'x': deque(maxlen=10), 27 | 'y': deque(maxlen=10) 28 | } 29 | 30 | self.vx, self.vy = 0, 0 31 | self.x, self.y = 0, 0 32 | 33 | def get_name(self): 34 | return 'BALL' 35 | 36 | def get_speed(self): 37 | return (self.vx**2 + self.vy**2)**.5 38 | 39 | def update(self, frame): 40 | self.current_data = frame.get('ball') 41 | if self.current_data is not None: 42 | self._update_speeds() 43 | 44 | def pos_next(self, fps=10): 45 | ball_next = copy.copy(self) 46 | ball_next.x += ball_next.vx * 10 * self.game.vision._fps 47 | ball_next.y += ball_next.vy * 10 * self.game.vision._fps 48 | return ball_next 49 | 50 | def _update_speeds(self): 51 | self._frames['x'].append(self.current_data['x']) 52 | self._frames['y'].append(self.current_data['y']) 53 | 54 | self.x = self.current_data['x'] 55 | self.y = self.current_data['y'] 56 | 57 | self.vx = speed(self._frames['x'], self.game.vision._fps) 58 | self.vy = speed(self._frames['y'], self.game.vision._fps) 59 | 60 | def __getitem__(self, item): 61 | if item == 0: 62 | return self.x 63 | 64 | if item == 1: 65 | return self.y 66 | 67 | raise IndexError("Ball only has 2 coordinates") 68 | -------------------------------------------------------------------------------- /strategy/tests/potential_fields_test.py: -------------------------------------------------------------------------------- 1 | import math 2 | from collections import deque 3 | from strategy.BaseStrategy import Strategy 4 | from controller import PID_control, SimpleLQR, TwoSidesLQR, UniController 5 | from NeonPathPlanning.potential_fields import PotentialField, PointField, LineField, TangentialField 6 | 7 | class Fields_Test(Strategy): 8 | def __init__(self, match, plot_field=False): 9 | super().__init__( 10 | match, 11 | name="Potential_Fields_Test", 12 | controller=TwoSidesLQR, 13 | controller_kwargs={'l': 0.0135} 14 | ) 15 | 16 | self.circuit = [(0.375, 0.25), (1.125, 0.25), (0.75, 0.65), (1.125, 1.05), (0.375, 1.05)] 17 | self.circuit = deque(self.circuit) 18 | self.dl = 0.000001 19 | 20 | def next_point(self): 21 | point = self.circuit[0] 22 | dx = point[0] - self.robot.x 23 | dy = point[1] - self.robot.y 24 | 25 | if math.sqrt(dx**2 + dy**2) < 0.05: 26 | self.circuit.rotate(-1) 27 | print("Change point! ", self.circuit[0]) 28 | 29 | return self.circuit[0] 30 | 31 | def start(self, robot=None): 32 | super().start(robot=robot) 33 | 34 | self.test_field = PotentialField( 35 | self.match, 36 | name=f'{self.__class__}|TestFieldBehaviour' 37 | ) 38 | 39 | # self.test_field.add_field( 40 | # PointField( 41 | # self.match, 42 | # target = (.75, .65), 43 | # radius = .3, 44 | # decay = lambda x: x, 45 | # multiplier = .8, 46 | # ) 47 | # ) 48 | 49 | self.test_field.add_field( 50 | LineField() 51 | ) 52 | 53 | def reset(self, robot=None): 54 | super().reset() 55 | if robot: 56 | self.start(robot) 57 | 58 | def decide(self): 59 | behaviour = self.test_field 60 | 61 | return behaviour.compute([self.robot.x, self.robot.y]) 62 | -------------------------------------------------------------------------------- /comm/rl_comm.py: -------------------------------------------------------------------------------- 1 | import os 2 | import serial 3 | import json 4 | 5 | def get_config(config_file=None): 6 | if config_file: 7 | config = json.loads(open(config_file, 'r').read()) 8 | else: 9 | config = json.loads(open('config_real_life.json', 'r').read()) 10 | 11 | return config 12 | 13 | class RLComm(object): 14 | def __init__(self): 15 | super(RLComm, self).__init__() 16 | self.config = get_config() 17 | self.commands = [] 18 | self.command_port = os.environ.get('COMM_PORT', self.config['comm']['comm_port']) 19 | self.baud_rate = int(os.environ.get('BAUD_RATE', self.config['comm']['baud_rate'])) 20 | 21 | def start(self): 22 | print("Starting communication...") 23 | self.comm = serial.Serial(self.command_port, self.baud_rate) 24 | print(f"Communication port created on {self.command_port}!") 25 | 26 | def send(self, robot_commands = []): 27 | ''' 28 | Send commands to ESP-32 29 | 30 | robot_commands follows: 31 | [ 32 | { 33 | robot_id: int, 34 | color: 'yellow|blue', 35 | wheel_left: float, 36 | wheel_right: float, 37 | } 38 | ] 39 | ''' 40 | message = "<" 41 | robot_commands = sorted(robot_commands, key = lambda i: i['robot_id']) 42 | for rb in robot_commands: 43 | message += f"{rb['robot_id']},{round(rb['wheel_left'], 4)},{round(rb['wheel_right'], 4)}," 44 | 45 | message = message[:-1] + '>' 46 | 47 | self.comm.write(message.encode()) 48 | 49 | def _get_robot_color(self, robot): 50 | return True if robot['color'] == 'yellow' else False 51 | 52 | if __name__ == "__main__": 53 | import time 54 | c = RLComm() 55 | 56 | c.start() 57 | 58 | while True: 59 | time.sleep(1) 60 | c.send( 61 | [ 62 | { 63 | 'robot_id': 0, 64 | 'wheel_left': 20, 65 | 'wheel_right': -20, 66 | 'color': 'blue' 67 | } 68 | ] 69 | ) 70 | -------------------------------------------------------------------------------- /live_plot/logger.py: -------------------------------------------------------------------------------- 1 | import sqlite3 2 | 3 | DB = "log.db" 4 | 5 | 6 | class Writer: 7 | def __init__(self, table, columns): 8 | # initialize parameters 9 | self.db = DB 10 | self.table = table 11 | self.columns = columns 12 | 13 | # create table 14 | conn = sqlite3.connect(self.db) 15 | cursor = conn.cursor() 16 | columns_text = ', '.join((f'{column_name} {datatype}' for column_name, datatype in self.columns.items())) 17 | cursor.execute(f"DROP TABLE IF EXISTS {self.table}") 18 | cursor.execute(f"CREATE TABLE {self.table} ({columns_text}, " 19 | f"time TIMESTAMP DEFAULT ((julianday('now') - 2440587.5) * 86400.0), PRIMARY KEY (time))") 20 | conn.close() 21 | 22 | def write(self, row): 23 | conn = sqlite3.connect(self.db) 24 | cursor = conn.cursor() 25 | cursor.execute(f"INSERT INTO {self.table} ({', '.join(self.columns.keys())}) " 26 | f"VALUES ({','.join(['?'] * len(self.columns))})", row) 27 | conn.commit() 28 | conn.close() 29 | 30 | 31 | class Reader: 32 | def __init__(self, table): 33 | # initialize parameters 34 | self.db = DB 35 | self.table = table 36 | 37 | def read(self): 38 | conn = sqlite3.connect(self.db) 39 | conn.row_factory = sqlite3.Row 40 | cursor = conn.cursor() 41 | print(f"SELECT * FROM {self.table} ORDER BY time DESC LIMIT 1") 42 | cursor.execute(f"SELECT * FROM {self.table} ORDER BY time DESC LIMIT 1") 43 | out = cursor.fetchone() 44 | conn.close() 45 | 46 | return out 47 | 48 | class Parameter: 49 | def __init__(self, base_value, table, column): 50 | self.base_value = base_value 51 | self.column = column 52 | 53 | self.reader = Reader(table) 54 | 55 | def __call__(self, *args, **kwargs): 56 | try: 57 | return self.reader.read()[self.column] 58 | except sqlite3.OperationalError as e: 59 | print(e) 60 | return self.base_value 61 | 62 | def __mul__(self, other): 63 | return self() * other 64 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | *$py.class 5 | 6 | # C extensions 7 | *.so 8 | 9 | # PyInstaller 10 | # Usually these files are written by a python script from a template 11 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 12 | *.manifest 13 | *.spec 14 | 15 | # Translations 16 | *.mo 17 | *.pot 18 | 19 | # Django stuff: 20 | *.log 21 | local_settings.py 22 | db.sqlite3 23 | db.sqlite3-journal 24 | 25 | # Flask stuff: 26 | instance/ 27 | .webassets-cache 28 | 29 | # Scrapy stuff: 30 | .scrapy 31 | 32 | # Sphinx documentation 33 | docs/_build/ 34 | 35 | # PyBuilder 36 | .pybuilder/ 37 | target/ 38 | 39 | # Jupyter Notebook 40 | .ipynb_checkpoints 41 | 42 | # IPython 43 | profile_default/ 44 | ipython_config.py 45 | 46 | # pyenv 47 | # For a library or package, you might want to ignore these files since the code is 48 | # intended to run in multiple environments; otherwise, check them in: 49 | # .python-version 50 | 51 | # pipenv 52 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 53 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 54 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 55 | # install all needed dependencies. 56 | #Pipfile.lock 57 | 58 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow 59 | __pypackages__/ 60 | 61 | # Celery stuff 62 | celerybeat-schedule 63 | celerybeat.pid 64 | 65 | # SageMath parsed files 66 | *.sage.py 67 | 68 | # Environments 69 | .env 70 | .venv 71 | env/ 72 | venv/ 73 | ENV/ 74 | env.bak/ 75 | venv.bak/ 76 | 77 | # Spyder project settings 78 | .spyderproject 79 | .spyproject 80 | 81 | # Rope project settings 82 | .ropeproject 83 | 84 | # mkdocs documentation 85 | /site 86 | 87 | # mypy 88 | .mypy_cache/ 89 | .dmypy.json 90 | dmypy.json 91 | 92 | # Pyre type checker 93 | .pyre/ 94 | 95 | # pytype static type analyzer 96 | .pytype/ 97 | 98 | # Cython debug symbols 99 | cython_debug/ 100 | 101 | 102 | # Visual Code Stuff 103 | .vscode 104 | 105 | # SQLite database files 106 | *.db 107 | 108 | .idea/ 109 | 110 | # Poetry cache 111 | poetry.lock 112 | -------------------------------------------------------------------------------- /protocols/vssref_placement_pb2.py: -------------------------------------------------------------------------------- 1 | # Generated by the protocol buffer compiler. DO NOT EDIT! 2 | # source: vssref_placement.proto 3 | 4 | import sys 5 | _b=sys.version_info[0]<3 and (lambda x:x) or (lambda x:x.encode('latin1')) 6 | from google.protobuf import descriptor as _descriptor 7 | from google.protobuf import message as _message 8 | from google.protobuf import reflection as _reflection 9 | from google.protobuf import symbol_database as _symbol_database 10 | # @@protoc_insertion_point(imports) 11 | 12 | _sym_db = _symbol_database.Default() 13 | 14 | 15 | import vssref_common_pb2 as vssref__common__pb2 16 | 17 | 18 | DESCRIPTOR = _descriptor.FileDescriptor( 19 | name='vssref_placement.proto', 20 | package='VSSRef.team_to_ref', 21 | syntax='proto3', 22 | serialized_options=None, 23 | serialized_pb=_b('\n\x16vssref_placement.proto\x12\x12VSSRef.team_to_ref\x1a\x13vssref_common.proto\"0\n\x10VSSRef_Placement\x12\x1c\n\x05world\x18\x01 \x01(\x0b\x32\r.VSSRef.Frameb\x06proto3') 24 | , 25 | dependencies=[vssref__common__pb2.DESCRIPTOR,]) 26 | 27 | 28 | 29 | 30 | _VSSREF_PLACEMENT = _descriptor.Descriptor( 31 | name='VSSRef_Placement', 32 | full_name='VSSRef.team_to_ref.VSSRef_Placement', 33 | filename=None, 34 | file=DESCRIPTOR, 35 | containing_type=None, 36 | fields=[ 37 | _descriptor.FieldDescriptor( 38 | name='world', full_name='VSSRef.team_to_ref.VSSRef_Placement.world', index=0, 39 | number=1, type=11, cpp_type=10, label=1, 40 | has_default_value=False, default_value=None, 41 | message_type=None, enum_type=None, containing_type=None, 42 | is_extension=False, extension_scope=None, 43 | serialized_options=None, file=DESCRIPTOR), 44 | ], 45 | extensions=[ 46 | ], 47 | nested_types=[], 48 | enum_types=[ 49 | ], 50 | serialized_options=None, 51 | is_extendable=False, 52 | syntax='proto3', 53 | extension_ranges=[], 54 | oneofs=[ 55 | ], 56 | serialized_start=67, 57 | serialized_end=115, 58 | ) 59 | 60 | _VSSREF_PLACEMENT.fields_by_name['world'].message_type = vssref__common__pb2._FRAME 61 | DESCRIPTOR.message_types_by_name['VSSRef_Placement'] = _VSSREF_PLACEMENT 62 | _sym_db.RegisterFileDescriptor(DESCRIPTOR) 63 | 64 | VSSRef_Placement = _reflection.GeneratedProtocolMessageType('VSSRef_Placement', (_message.Message,), dict( 65 | DESCRIPTOR = _VSSREF_PLACEMENT, 66 | __module__ = 'vssref_placement_pb2' 67 | # @@protoc_insertion_point(class_scope:VSSRef.team_to_ref.VSSRef_Placement) 68 | )) 69 | _sym_db.RegisterMessage(VSSRef_Placement) 70 | 71 | 72 | # @@protoc_insertion_point(module_scope) 73 | -------------------------------------------------------------------------------- /live_plot/plot_circuit.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": 11, 6 | "id": "759750d5", 7 | "metadata": {}, 8 | "outputs": [ 9 | { 10 | "ename": "ModuleNotFoundError", 11 | "evalue": "No module named 'bokeh'", 12 | "output_type": "error", 13 | "traceback": [ 14 | "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m", 15 | "\u001b[0;31mModuleNotFoundError\u001b[0m Traceback (most recent call last)", 16 | "Input \u001b[0;32mIn [11]\u001b[0m, in \u001b[0;36m\u001b[0;34m()\u001b[0m\n\u001b[0;32m----> 1\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m \u001b[38;5;21;01mbokeh\u001b[39;00m\u001b[38;5;21;01m.\u001b[39;00m\u001b[38;5;21;01mplotting\u001b[39;00m \u001b[38;5;28;01mimport\u001b[39;00m curdoc, figure\n\u001b[1;32m 2\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m \u001b[38;5;21;01mrandom\u001b[39;00m\n\u001b[1;32m 3\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m \u001b[38;5;21;01mtime\u001b[39;00m\n", 17 | "\u001b[0;31mModuleNotFoundError\u001b[0m: No module named 'bokeh'" 18 | ] 19 | } 20 | ], 21 | "source": [ 22 | "from bokeh.plotting import curdoc, figure\n", 23 | "import json\n", 24 | "import socket\n", 25 | "\n", 26 | "UDP_IP_ADDRESS = 'localhost'\n", 27 | "UDP_PORT_NO = 43215\n", 28 | "\n", 29 | "serverSock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)\n", 30 | "serverSock.bind((UDP_IP_ADDRESS, UDP_PORT_NO))\n", 31 | "\n", 32 | "def update():\n", 33 | " raw_data, addr = serverSock.recvfrom(4096)\n", 34 | " data = json.loads(raw_data)\n", 35 | " x = data['robot']['x']\n", 36 | " y = data['robot']['y']\n", 37 | " r.data_source.stream({'x': [x], 'y': [y]})\n", 38 | "\n", 39 | "p = figure()\n", 40 | "r = p.circle([], [])\n", 41 | "curdoc().add_root(p)\n", 42 | "curdoc().add_periodic_callback(update, 100)" 43 | ] 44 | }, 45 | { 46 | "cell_type": "code", 47 | "execution_count": null, 48 | "id": "2803a53e", 49 | "metadata": {}, 50 | "outputs": [], 51 | "source": [] 52 | } 53 | ], 54 | "metadata": { 55 | "kernelspec": { 56 | "display_name": "Python 3 (ipykernel)", 57 | "language": "python", 58 | "name": "python3" 59 | }, 60 | "language_info": { 61 | "codemirror_mode": { 62 | "name": "ipython", 63 | "version": 3 64 | }, 65 | "file_extension": ".py", 66 | "mimetype": "text/x-python", 67 | "name": "python", 68 | "nbconvert_exporter": "python", 69 | "pygments_lexer": "ipython3", 70 | "version": "3.8.10" 71 | } 72 | }, 73 | "nbformat": 4, 74 | "nbformat_minor": 5 75 | } 76 | -------------------------------------------------------------------------------- /algorithms/dijkstra_waypoint/waypoint.py: -------------------------------------------------------------------------------- 1 | from algorithms.dijkstra_waypoint import dijkstra 2 | from commons.math import dist_point_line 3 | 4 | class WaypointSystem(): 5 | def __init__(self, 6 | min_range=0.30, max_range=1.50, 7 | robot_radius=0.10,wall_anchors = 4 8 | ): 9 | 10 | self.min_range = min_range 11 | self.max_range = max_range 12 | self.robot_radius = robot_radius 13 | 14 | self.dijkstra = None 15 | self.graph = None 16 | 17 | def start(self): 18 | pass 19 | 20 | def update(self, me, objective=None, obstacles=[], field_size = [200, 180]): 21 | self.graph = dijkstra.Graph() 22 | 23 | self.graph.add_vertex(f'{objective.get_name()}', objective.x, objective.y) 24 | self.graph.add_vertex(f'{me.get_name()}', me.x, me.y) 25 | 26 | for obs1 in obstacles: 27 | for obs2 in obstacles: 28 | if obs1.get_name() == obs2.get_name(): 29 | continue 30 | x = (obs1.x + obs2.x)/2 31 | y = (obs1.y + obs2.y)/2 32 | 33 | dist = ( (obs1.x - obs2.x)**2 + (obs1.y - obs2.y)**2 )**(1/2) 34 | 35 | if dist > self.min_range and not self.graph.vert_dict.get(f'{obs2.get_name()}_{obs1.get_name()}'): 36 | self.graph.add_vertex(f'{obs1.get_name()}_{obs2.get_name()}', x, y) 37 | 38 | for vtx1 in self.graph.vert_dict.values(): 39 | for vtx2 in self.graph.vert_dict.values(): 40 | if (vtx1.get_id() == vtx2.get_id()) or (vtx1.x == vtx2.x and vtx1.y == vtx2.y): 41 | continue 42 | if any( 43 | [ 44 | dist_point_line(vtx1.x, vtx1.y, vtx2.x, vtx2.y, o.x, o.y) < self.robot_radius 45 | for o in obstacles 46 | ] 47 | ): 48 | continue 49 | self.graph.add_edge(vtx1, vtx2) 50 | 51 | def dir_vector(self, path, robot): 52 | target = self.graph.vert_dict[path[1]] if len(path) > 1 else self.graph.vert_dict[path[0]] 53 | dist = ( (target.x - robot.x)**2 + (target.y - robot.y)**2 )**.5 54 | return [ 55 | (target.x - robot.x)/dist, 56 | (target.y - robot.y)/dist 57 | ] 58 | 59 | def decide(self, robot, target, speed): 60 | robot_name = robot.get_name() 61 | target_name = target.get_name() 62 | 63 | dijkstra.dijkstra(self.graph, robot_name, target_name) 64 | 65 | target = self.graph.get_vertex(target_name) 66 | 67 | path = [target.get_id()] 68 | 69 | dijkstra.shortest(target, path) 70 | 71 | return [v * speed for v in self.dir_vector(path[::-1], robot)] 72 | -------------------------------------------------------------------------------- /entities/trainer/base_trainer.py: -------------------------------------------------------------------------------- 1 | import time 2 | import logging 3 | 4 | class CustomFormatter(logging.Formatter): 5 | 6 | grey = "\x1b[38;20m" 7 | yellow = "\x1b[33;20m" 8 | red = "\x1b[31;20m" 9 | bold_red = "\x1b[31;1m" 10 | reset = "\x1b[0m" 11 | format = "[%(asctime)s | %(name)s | %(levelname)s] %(message)s" 12 | 13 | FORMATS = { 14 | logging.DEBUG: grey + format + reset, 15 | logging.INFO: grey + format + reset, 16 | logging.WARNING: yellow + format + reset, 17 | logging.ERROR: red + format + reset, 18 | logging.CRITICAL: bold_red + format + reset 19 | } 20 | 21 | def format(self, record): 22 | log_fmt = self.FORMATS.get(record.levelno) 23 | formatter = logging.Formatter(log_fmt) 24 | return formatter.format(record) 25 | 26 | class Trainer(object): 27 | def __init__(self, lab_match, n_iterations, train_name): 28 | self.n_iterations = n_iterations 29 | self.train_name = train_name 30 | self.match = lab_match 31 | 32 | self.results = [] 33 | 34 | self.logger = self.setup_logger(train_name) 35 | 36 | def setup_logger(self, train_name): 37 | logger = logging.getLogger(train_name) 38 | logger.setLevel(logging.DEBUG) 39 | 40 | ch = logging.StreamHandler() 41 | ch.setLevel(logging.DEBUG) 42 | 43 | ch.setFormatter(CustomFormatter()) 44 | 45 | logger.addHandler(ch) 46 | return logger 47 | 48 | 49 | def repositioning_robot(self, r, pose): 50 | """ 51 | pose as [x, y, theta] 52 | """ 53 | command = {'x': pose[0], 'y': pose[1], 'theta': pose[2], 'color': r.team_color, 'robot_id': r.robot_id} 54 | 55 | self.logger.debug("reposicionando robo: " + str(command)) 56 | 57 | self.match.game.comm.replace([command], None) 58 | time.sleep(2) 59 | self.logger.debug("posicionamento completo!") 60 | 61 | def clean_field(self): 62 | self.logger.debug("colocando robos e bola fora do campo...") 63 | commands = [] 64 | ball_commands = {'x': -10, 'y': -10} 65 | for i, r in enumerate(self.match.robots + self.match.opposites): 66 | commands.append( 67 | {'x': 2, 'y': 2 + i * 0.1, 'theta': 0, 'color': r.team_color, 'robot_id': r.robot_id} 68 | ) 69 | 70 | self.match.game.comm.replace(commands, ball_commands) 71 | time.sleep(2) 72 | self.logger.debug("posicionamento completo!") 73 | 74 | def decide(self): 75 | self.execute() 76 | 77 | def execute(self, iteration): 78 | pass 79 | 80 | def teardown(self, iteration): 81 | pass 82 | 83 | def setup(self): 84 | self.clean_field() 85 | 86 | def compile(self): 87 | pass -------------------------------------------------------------------------------- /strategy/tests/limit_cycle_test.py: -------------------------------------------------------------------------------- 1 | import math 2 | from collections import deque 3 | from strategy.BaseStrategy import Strategy 4 | from controller import PID_control, PID_W_control, SimpleLQR, TwoSidesLQR, UniController 5 | from NeonPathPlanning import LimitCycle 6 | 7 | class LimitCycle_Test(Strategy): 8 | def __init__(self, match, plot_field=False): 9 | super().__init__(match, name='LimitCycle_Test', controller=PID_W_control) 10 | 11 | self.circuit = [(0.375, 0.25), (1.125, 0.25), (0.75, 0.65), (1.125, 1.05), (0.375, 1.05)] 12 | self.circuit = deque(self.circuit) 13 | self.dl = 0.000001 14 | 15 | self.field_w, self.field_h = self.match.game.field.get_dimensions() 16 | 17 | def next_point(self): 18 | point = self.circuit[0] 19 | dx = point[0] - self.robot.x 20 | dy = point[1] - self.robot.y 21 | 22 | if math.sqrt(dx**2 + dy**2) < 0.05: 23 | self.circuit.rotate(-1) 24 | print("Change point! ", self.circuit[0]) 25 | 26 | return self.circuit[0] 27 | 28 | def get_virtual_obstacle(self, target): 29 | ''' 30 | - m: angle of the line perpendicular to the line between the ball and 31 | the center of the goal 32 | - p: distance of the virtual obstacles to the ball / radius of the virtual obstacle 33 | - vo: virtual obstacle 34 | - j: this is the angle between the ball and the center of the goal 35 | - m: the normal angle perpendicular to j 36 | - r: radius of the ball 37 | ''' 38 | aim_point = [self.field_h/2, self.field_w] 39 | 40 | j = math.atan2(aim_point[0] - target[1], aim_point[1] - target[0]) 41 | m = j + math.pi/2 42 | p = 0.1 43 | 44 | r = .02 #(.0427)/2 45 | 46 | ''' 47 | the terms r*cos(j) and r*sin(j) are subtracted to move 48 | the center of the obstacles behind the ball instead of its center 49 | ''' 50 | if self.robot.y < math.tan(j)*(self.robot.x - target[0]) + target[1]: 51 | virtual_obstacle = (target[0] - p*math.cos(m) - r*math.cos(j), target[1] - p*math.sin(m) - r*math.sin(j), p, 1) 52 | else: 53 | virtual_obstacle = (target[0] + p*math.cos(m) - r*math.cos(j), target[1] + p*math.sin(m) - r*math.sin(j), p, -1) 54 | 55 | return virtual_obstacle 56 | 57 | def start(self, robot=None): 58 | super().start(robot=robot) 59 | 60 | self.limit_cycle = LimitCycle(self.match) 61 | 62 | def reset(self, robot=None): 63 | super().reset() 64 | if robot: 65 | self.start(robot) 66 | 67 | def decide(self): 68 | target = (self.match.ball.x, self.match.ball.y) 69 | virtual_obstacle = self.get_virtual_obstacle(target) 70 | 71 | self.limit_cycle.set_target(target) 72 | self.limit_cycle.add_obstacle(virtual_obstacle) 73 | 74 | return self.limit_cycle.compute(self.robot) 75 | -------------------------------------------------------------------------------- /algorithms/astar/astar.py: -------------------------------------------------------------------------------- 1 | import heapq 2 | from commons.math import distance_between_points as distance 3 | 4 | class Node: 5 | def __init__(self, position): 6 | self.previous = None 7 | self.position = position 8 | self.g = 0 # distance so far 9 | self.h = 0 # heuristic/distance to target 10 | self.f = 0 # g plus h 11 | self.neighbours = [] 12 | 13 | def set_position(self, position = []): 14 | self.position = position 15 | 16 | def __lt__(self, other): 17 | if (self.f == other.f): 18 | return self.h < other.h 19 | return self.f < other.f 20 | 21 | def __gt__(self, other): 22 | return self.f > other.f 23 | 24 | def __eq__(self, o): 25 | if not hasattr(o, "position"): 26 | return False 27 | return o.position == self.position 28 | 29 | def __hash__(self): 30 | return hash(str(self.position)) 31 | 32 | class AStar(): 33 | def __init__(self, initial_node, target): 34 | self.start = initial_node 35 | self.target = target 36 | self.open_list = [] 37 | self.closed_list = [] 38 | 39 | def create_path(self, current): 40 | backtracking = [] 41 | while current != None: 42 | backtracking.append(current.position) 43 | current = current.previous 44 | return backtracking[::-1] 45 | 46 | def calculate(self): 47 | self.open_list = [self.start] 48 | heapq.heapify(self.open_list) 49 | while len(self.open_list) > 0: 50 | # open_list nodes are ordered by lowest f value to highest 51 | # in case of a tie, lowest h is prioritized 52 | # current is the node with the smallest f value and is the first in the open_list 53 | current = self.open_list[0] 54 | if current == self.target: 55 | path_to_target = self.create_path(current) 56 | return path_to_target 57 | self.closed_list.append(heapq.heappop(self.open_list)) 58 | for neighbor in current.neighbours: 59 | if neighbor not in self.closed_list: 60 | if neighbor not in self.open_list: 61 | neighbor.h = distance(neighbor.position, self.target.position) 62 | neighbor.g = distance(neighbor.position, current.position) 63 | neighbor.f = neighbor.g + neighbor.h 64 | neighbor.previous = current 65 | else: 66 | dist_from_previous = current.g + distance(neighbor.position, current.position) 67 | if dist_from_previous < neighbor.g: 68 | neighbor.g = dist_from_previous 69 | neighbor.f = neighbor.g + neighbor.h 70 | neighbor.previous = current 71 | self.open_list.remove(neighbor) 72 | heapq.heappush(self.open_list, neighbor) 73 | return [] 74 | -------------------------------------------------------------------------------- /live_plot/live_plot_script.py: -------------------------------------------------------------------------------- 1 | import sys 2 | import json 3 | 4 | import socket 5 | import struct 6 | 7 | import pyqtgraph as pg 8 | from pyqtgraph.Qt import QtCore, QtGui 9 | import _thread as thread 10 | 11 | MCAST_GRP = '224.1.2.3' 12 | MCAST_PORT = 5007 13 | 14 | sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP) 15 | sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) 16 | 17 | sock.bind((MCAST_GRP, MCAST_PORT)) 18 | 19 | mreq = struct.pack("4sl", socket.inet_aton(MCAST_GRP), socket.INADDR_ANY) 20 | 21 | sock.setsockopt(socket.IPPROTO_IP, socket.IP_ADD_MEMBERSHIP, mreq) 22 | 23 | def socketparse(): 24 | data = sock.recv(1024) 25 | data = json.loads(data.decode('utf8').replace("'", '"')) 26 | if not data: 27 | return 28 | 29 | data_point = data[0]['data'] 30 | 31 | return data_point 32 | 33 | 34 | desired_linear = [] 35 | now_linear = [] 36 | new_linear = [] 37 | desired_angular = [] 38 | now_angular = [] 39 | new_angular = [] 40 | 41 | for x in range(50): 42 | while True: 43 | data_point = socketparse() 44 | if data_point: 45 | break 46 | 47 | desired_linear.append(data_point['desired_linear']) 48 | now_linear.append(data_point['now_linear']) 49 | new_linear.append(data_point['new_linear']) 50 | 51 | desired_angular.append(data_point['desired_angular']) 52 | now_angular.append(data_point['now_angular']) 53 | new_angular.append(data_point['new_angular']) 54 | 55 | win = pg.GraphicsWindow() 56 | win.setWindowTitle('PID fine-tuning data - Live plot') 57 | p1 = win.addPlot() 58 | 59 | curve1 = p1.plot(desired_angular, pen=pg.mkPen('r', width=3)) 60 | curve2 = p1.plot(now_angular, pen=pg.mkPen('g', width=3)) 61 | # curve3 = p1.plot(new_linear, pen=pg.mkPen('b', width=3)) 62 | 63 | ptr1 = 0 64 | 65 | def updatelist(): 66 | global desired_linear, now_linear, new_linear, curve1, curve2, ptr1 67 | while True: 68 | data_point = socketparse() 69 | if data_point: 70 | break 71 | 72 | 73 | temp = data_point['desired_angular'] 74 | 75 | #temperature plot 76 | desired_angular[:-1] = desired_angular[1:] 77 | desired_angular[-1] = temp 78 | curve1.setData(desired_angular) 79 | 80 | temp = data_point['now_angular'] 81 | 82 | #temperature plot 83 | now_angular[:-1] = now_angular[1:] 84 | now_angular[-1] = temp 85 | curve2.setData(now_angular) 86 | 87 | # temp = data_point['new_linear'] 88 | 89 | # #temperature plot 90 | # new_linear[:-1] = new_linear[1:] 91 | # new_linear[-1] = temp 92 | # curve3.setData(new_linear) 93 | 94 | ptr1 += 1 95 | 96 | def updateplot(): 97 | updatelist() 98 | 99 | timer = pg.QtCore.QTimer() 100 | timer.timeout.connect(updateplot) 101 | timer.start(50) 102 | 103 | if __name__ == '__main__': 104 | import sys 105 | 106 | if (sys.flags.interactive != 1) or not hasattr(QtCore, 'PYQT_VERSION'): 107 | QtGui.QApplication.instance().exec_() 108 | -------------------------------------------------------------------------------- /live_plot/plot_djisktra.py: -------------------------------------------------------------------------------- 1 | import sys 2 | import json 3 | 4 | import socket 5 | import struct 6 | 7 | import pyqtgraph as pg 8 | from pyqtgraph.Qt import QtCore, QtGui 9 | import pyqtgraph.opengl as gl 10 | import _thread as thread 11 | 12 | MCAST_GRP = '224.1.2.3' 13 | MCAST_PORT = 5007 14 | 15 | sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP) 16 | sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) 17 | 18 | sock.bind((MCAST_GRP, MCAST_PORT)) 19 | 20 | mreq = struct.pack("4sl", socket.inet_aton(MCAST_GRP), socket.INADDR_ANY) 21 | 22 | sock.setsockopt(socket.IPPROTO_IP, socket.IP_ADD_MEMBERSHIP, mreq) 23 | 24 | class RectItem(pg.GraphicsObject): 25 | def __init__(self, rect, parent=None): 26 | super().__init__(parent) 27 | self._rect = rect 28 | self.picture = QtGui.QPicture() 29 | self._generate_picture() 30 | 31 | @property 32 | def rect(self): 33 | return self._rect 34 | 35 | def _generate_picture(self): 36 | painter = QtGui.QPainter(self.picture) 37 | painter.setPen(pg.mkPen("w")) 38 | painter.setBrush(pg.mkBrush("g")) 39 | painter.drawRect(self.rect) 40 | painter.end() 41 | 42 | def paint(self, painter, option, widget=None): 43 | painter.drawPicture(0, 0, self.picture) 44 | 45 | def boundingRect(self): 46 | return QtCore.QRectF(self.picture.boundingRect()) 47 | 48 | def update_points(): 49 | data = socketparse() 50 | node_spots = [{'pos': [n['x'], n['y']], 'data': 1, 'brush': pg.intColor(100 if n['name'] == 'BALL' else 300)} for n in data['nodes']] 51 | obs_spots = [{'pos': [n['x'], n['y']], 'data': 1, 'brush': pg.intColor(800)} for n in data['obstacles']] 52 | rect_item = RectItem(QtCore.QRectF(0, 0, 1.5, 1.3)) 53 | pf.addItem(rect_item) 54 | pf.addItem(scatter) 55 | 56 | scatter.clear() 57 | scatter.addPoints(node_spots) 58 | scatter.addPoints(obs_spots) 59 | 60 | pf.clear() 61 | lines = [pg.LineSegmentROI(a, pen=(1,1)) for a in data['edges']] 62 | for line in lines: 63 | pf.addItem(line) 64 | 65 | 66 | def socketparse(): 67 | data = sock.recv(40023) 68 | data = json.loads(data.decode('utf8').replace("'", '"')) 69 | if not data: 70 | return 71 | 72 | data_point = data[0]['data'] 73 | 74 | return data_point 75 | 76 | points = [] 77 | 78 | win = pg.GraphicsWindow() 79 | win.setWindowTitle('Waypoint pathfinding - live plot') 80 | pf = win.addPlot() 81 | 82 | 83 | data = socketparse() 84 | 85 | node_spots = [{'pos': [n['x'], n['y']], 'data': 1, 'brush': pg.intColor(300)} for n in data['nodes']] 86 | obs_spots = [{'pos': [n['x'], n['y']], 'data': 1, 'brush': pg.intColor(100)} for n in data['obstacles']] 87 | 88 | edges = [] 89 | 90 | 91 | scatter = pg.ScatterPlotItem(size=10) 92 | scatter.addPoints(node_spots) 93 | scatter.addPoints(obs_spots) 94 | 95 | 96 | def updateplot(): 97 | update_points() 98 | 99 | timer = pg.QtCore.QTimer() 100 | timer.timeout.connect(updateplot) 101 | timer.start( (1000/60) * 5) 102 | 103 | if __name__ == '__main__': 104 | import sys 105 | 106 | if (sys.flags.interactive != 1) or not hasattr(QtCore, 'PYQT_VERSION'): 107 | QtGui.QApplication.instance().exec_() 108 | -------------------------------------------------------------------------------- /algorithms/astar/pathAstar.py: -------------------------------------------------------------------------------- 1 | from algorithms.astar import AStar, Node 2 | from scipy.spatial import Voronoi 3 | import algorithms 4 | 5 | class PathAstar: 6 | def __init__(self, match): 7 | self.graph = algorithms.astar.FieldGraph() 8 | self.match = match 9 | self.road = [] 10 | self.corners = [ 11 | [self.match.game.field.get_dimensions()[0], 0], 12 | [0, 0], 13 | [0, self.match.game.field.get_dimensions()[1]], 14 | [self.match.game.field.get_dimensions()[0], self.match.game.field.get_dimensions()[1]] 15 | ] 16 | 17 | def voronoi_graph(self, start = [], target = [], obstacles = []): 18 | """ 19 | Uses Voronoi in order to create a graph to be used by the astar. 20 | Returns an array of positions of the nodes of the graph that leads to the target. 21 | 22 | Receives the starting position [x, y] 23 | Receives the target position [x, y] 24 | Receives a list of positions of the obstacles [[x1, y1], [x2, y2]] 25 | """ 26 | self.graph = algorithms.astar.FieldGraph() 27 | 28 | start_node = Node(start) 29 | self.graph.set_start(start_node) 30 | 31 | target_node = Node(target) 32 | self.graph.set_start(target_node) 33 | 34 | obstacle_list = self.corners + obstacles + [target] + [start] 35 | 36 | vor = Voronoi(obstacle_list) 37 | 38 | nodes = [ 39 | Node([a[0], a[1]]) for a in vor.vertices 40 | ] + [ 41 | target_node, start_node 42 | ] 43 | 44 | objective_index = len(obstacle_list) - 2 45 | start_index = len(obstacle_list) - 1 46 | 47 | self.graph.set_nodes(nodes) 48 | 49 | polygon_objective_edges = [] 50 | polygon_start_edges = [] 51 | 52 | for edge, ridge_vertice in zip(vor.ridge_vertices, vor.ridge_points): 53 | if edge[0] == -1: continue 54 | self.graph.add_edge([nodes[edge[0]], nodes[edge[1]]]) 55 | 56 | if objective_index in ridge_vertice: 57 | polygon_objective_edges.append(nodes[edge[0]]) 58 | polygon_objective_edges.append(nodes[edge[1]]) 59 | 60 | if start_index in ridge_vertice: 61 | polygon_start_edges.append(nodes[edge[0]]) 62 | polygon_start_edges.append(nodes[edge[1]]) 63 | 64 | if objective_index in ridge_vertice and start_index in ridge_vertice: 65 | self.graph.add_edge([start_node, target_node]) 66 | 67 | for edge_to_target in set(polygon_objective_edges): 68 | self.graph.add_edge([edge_to_target, target_node]) 69 | 70 | for edge_to_start in set(polygon_start_edges): 71 | self.graph.add_edge([edge_to_start, start_node]) 72 | 73 | road = AStar(start_node, target_node).calculate() 74 | 75 | return road 76 | 77 | def calculate(self, start = [], target = [], obstacles = []): 78 | self.road = self.voronoi_graph(start, target, obstacles) 79 | if self.road == []: 80 | self.road = [start, target] 81 | dist = ( (self.road[0][0] - self.road[1][0])**2 + (self.road[0][1] - self.road[1][1])**2 )**.5 82 | r_v = [ 83 | 0.9 * (self.road[1][0] - self.road[0][0])/dist, 84 | 0.9 * (self.road[1][1] - self.road[0][1])/dist 85 | ] 86 | return r_v 87 | -------------------------------------------------------------------------------- /api/udp_sender.py: -------------------------------------------------------------------------------- 1 | import json 2 | import socket 3 | import struct 4 | 5 | from time import sleep 6 | 7 | import threading 8 | 9 | class SingletonMeta(type): 10 | """ 11 | The Singleton class can be implemented in different ways in Python. Some 12 | possible methods include: base class, decorator, metaclass. We will use the 13 | metaclass because it is best suited for this purpose. 14 | 15 | source: https://refactoring.guru/design-patterns/singleton/python/example 16 | """ 17 | 18 | _instances = {} 19 | 20 | def __call__(cls, *args, **kwargs): 21 | """ 22 | Possible changes to the value of the `__init__` argument do not affect 23 | the returned instance. 24 | """ 25 | if cls not in cls._instances: 26 | instance = super().__call__(*args, **kwargs) 27 | cls._instances[cls] = instance 28 | return cls._instances[cls] 29 | 30 | class DataSender(metaclass=SingletonMeta): 31 | def __init__(self): 32 | super(DataSender, self).__init__() 33 | self.nodes = {} 34 | # talvez arquivo de congf? 35 | self.multicast_group = '224.1.2.3' 36 | self.multicast_port = 5007 37 | self.multicast_ttl = 1 38 | 39 | self.sock = None 40 | 41 | def append_datanode(self, datanode): 42 | self.nodes[datanode.node_name] = datanode 43 | 44 | 45 | def get_node(self, name): 46 | if not self.nodes.get(name): 47 | print(f'Data node [{name}] not registered! registering now!') 48 | 49 | new_node = DataNode(name) 50 | self.append_datanode(new_node) 51 | 52 | return self.nodes.get(name) 53 | 54 | def start(self): 55 | print("Starting Data Sender...") 56 | self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP) 57 | self.sock.setsockopt(socket.IPPROTO_IP, socket.IP_MULTICAST_TTL, self.multicast_ttl) 58 | print(f"Data Sender started at {self.multicast_group}:{self.multicast_port}!") 59 | 60 | # while True: 61 | # self.send_data() 62 | 63 | def send_data(self): 64 | data_pack = [] 65 | 66 | if not self.nodes.keys(): 67 | return 68 | 69 | for node in self.nodes.values(): 70 | d_ = node.retrieve() 71 | if d_: 72 | data_pack.append({'name': node.node_name, 'data': d_}) 73 | 74 | data_bytes = bytes(json.dumps(data_pack), 'utf-8') 75 | 76 | self.sock.sendto(data_bytes, (self.multicast_group, self.multicast_port)) 77 | 78 | 79 | class DataNode(object): 80 | def __init__(self, node_name): 81 | self.node_name = node_name 82 | self._buffer_data = None 83 | 84 | def capture(self, **data): 85 | self._buffer_data = data 86 | 87 | def retrieve(self): 88 | b_data = self._buffer_data 89 | self._buffer_data = None 90 | return b_data 91 | 92 | if __name__ == '__main__': 93 | import random 94 | s1 = DataSender() 95 | 96 | dn = DataNode('PID_ROBOT_1') 97 | dn2 = DataNode('PID_ROBOT_2') 98 | 99 | s1.start() 100 | sleep(1) 101 | 102 | s1.append_datanode(dn) 103 | s1.append_datanode(dn2) 104 | while True: 105 | sleep(1/1000) 106 | s1.get_node('PID_ROBOT_1').capture(error=random.random()) 107 | s1.get_node('PID_ROBOT_2').capture(error=random.random()) -------------------------------------------------------------------------------- /controller/PID.py: -------------------------------------------------------------------------------- 1 | import api 2 | import math 3 | import numpy as np 4 | 5 | class PID(object): 6 | def __init__(self, kp, kd ,ki, _ilimit=1000): 7 | self.desired_PID = 0.0 8 | 9 | self.kp = kp 10 | self.kd = kd 11 | self.ki = ki 12 | 13 | self._ilimit = _ilimit 14 | 15 | self.last_error = 0 16 | self.integral = 0 17 | 18 | def set_desired_PID(self, speed): 19 | self.desired_PID = speed 20 | 21 | def update_PID(self, now, fps): 22 | dt = 1/fps 23 | 24 | error = self.desired_PID - now 25 | 26 | self.integral = min(self._ilimit, max(-self._ilimit, self.integral)) 27 | 28 | derivative = (error - self.last_error)/dt 29 | 30 | self.integral = self.integral + error * dt 31 | 32 | output = self.kp * error + self.kd * derivative + self.ki * self.integral 33 | 34 | self.last_error = error 35 | 36 | return output 37 | 38 | class Robot_PID(object): 39 | def __init__(self, robot, send_data=False): 40 | self.robot = robot 41 | self.game = self.robot.game 42 | 43 | self.desired = np.array([0, 0]) 44 | self.linear_pid = PID(2, 1.2, 0) 45 | self.angular_pid = PID(12, 4, 0) 46 | self.power_left , self.power_right = 0, 0 47 | 48 | # self.pid_file = open("pid.log", "a") 49 | 50 | def update(self): 51 | linear_speed, angular_speed = self.robot._get_differential_robot_speeds(self.robot.vx, self.robot.vy, self.robot.theta) 52 | linear_speed, angular_speed = linear_speed * 100, angular_speed 53 | 54 | linear_desired, angular_desired = self.robot._get_desired_differential_robot_speeds(self.desired[0],self.desired[1], self.robot.theta) 55 | 56 | linear_desired, angular_desired = linear_desired * 100, angular_desired 57 | 58 | vl, va = self.update_Speed(linear_desired,angular_desired,linear_speed, angular_speed) 59 | 60 | acc_left = vl - va 61 | acc_right = vl + va 62 | 63 | if self.game.vision._fps != 0: 64 | self.power_left = self.power_left + acc_left * (1/self.game.vision._fps) 65 | self.power_right = self.power_right + acc_right * (1/self.game.vision._fps) 66 | 67 | self.power_left = min(500, max(self.power_left, -500)) 68 | self.power_right = min(500, max(self.power_right, -500)) 69 | 70 | return self.power_left , self.power_right 71 | 72 | return 0, 0 73 | 74 | 75 | def set_desired(self, vector): 76 | self.desired = vector 77 | 78 | 79 | def update_Speed(self, linear_desired, angular_desired, now_linear, now_angular): 80 | self.linear_desired = linear_desired 81 | self.angular_desired = angular_desired 82 | 83 | self.linear_pid.set_desired_PID(linear_desired) 84 | self.angular_pid.set_desired_PID(angular_desired) 85 | 86 | vl, va = 0, 0 87 | if self.game.vision._fps != 0: 88 | vl = self.linear_pid.update_PID(now_linear, self.game.vision._fps) 89 | va = self.angular_pid.update_PID(now_angular, self.game.vision._fps) 90 | 91 | api.DataSender().get_node(f'PID_{self.robot.get_name()}').capture( 92 | desired_linear=self.linear_desired, 93 | now_linear=now_linear, 94 | new_linear=vl, 95 | desired_angular=self.angular_desired, 96 | now_angular=now_angular, 97 | new_angular=va, 98 | ) 99 | return vl, va 100 | -------------------------------------------------------------------------------- /match/match.py: -------------------------------------------------------------------------------- 1 | import os 2 | import entities 3 | from api import Parameter 4 | from concurrent import futures 5 | 6 | CATEGORIES = { 7 | '3v3': 3, '5v5': 5 8 | } 9 | 10 | class Match(object): 11 | def __init__(self, game, team_side, team_color, coach_name=None, category="3v3"): 12 | super().__init__() 13 | self.game = game 14 | 15 | self.coach_name = os.environ.get('COACH_NAME', coach_name) 16 | self.team_side = os.environ.get('TEAM_SIDE', team_side) 17 | self.team_color = os.environ.get('TEAM_COLOR', team_color) 18 | self.category = os.environ.get('CATEGORY', category) 19 | self.n_robots = CATEGORIES.get(self.category) 20 | 21 | self.opposite_team_color = 'yellow' if self.team_color == 'blue' else 'blue' 22 | 23 | pid_kp, ki, kd = Parameter(0, 'pid_kp'), Parameter(0, 'ki'), Parameter(0, 'kd') 24 | kw, rm, vm, uni_kp = Parameter(0, 'kw'), Parameter(0, 'rm'), Parameter(0, 'vm'), Parameter(0, 'uni_kp') 25 | 26 | self.parameters = {"pid_kp":pid_kp, "ki":ki, "kd":kd, "kw":kw, "rm":rm, "vm":vm, "uni_kp":uni_kp} 27 | 28 | self.game_status = 'GAME_ON' 29 | self.match_event = {'event': 'PLAYING', 'quadrant': 1, 'mine': True} 30 | 31 | 32 | def start(self): 33 | print("Starting match module starting ...") 34 | self.ball = entities.Ball(self.game) 35 | 36 | self.opposites = [ 37 | entities.Robot(self.game, i, self.opposite_team_color) for i in range(self.n_robots) 38 | ] 39 | 40 | self.robots = [ 41 | entities.Robot(self.game, i, self.team_color) for i in range(self.n_robots) 42 | ] 43 | 44 | self.coach = entities.coach.COACHES[self.coach_name](self) 45 | print(f"Match started! coach is [{self.coach.NAME}]") 46 | self.coach.decide() 47 | 48 | for robot in self.robots: 49 | robot.start() 50 | 51 | def restart(self, team_color): 52 | self.team_color = team_color 53 | self.opposite_team_color = 'yellow' if self.team_color == 'blue' else 'blue' 54 | 55 | self.opposites = [ 56 | entities.Robot(self.game, i, self.opposite_team_color) for i in range(self.n_robots) 57 | ] 58 | 59 | self.robots = [ 60 | entities.Robot(self.game, i, self.team_color) for i in range(self.n_robots) 61 | ] 62 | 63 | self.coach.decide() 64 | 65 | for robot in self.robots: 66 | robot.start() 67 | 68 | 69 | def update(self, frame): 70 | self.ball.update(frame) 71 | 72 | for entity in self.opposites: 73 | entity.update(frame) 74 | 75 | for entity in self.robots: 76 | entity.update(frame) 77 | 78 | 79 | def update_information(self, **kwargs): #Function to update values recieved in api 80 | for key, value in kwargs.items(): 81 | if hasattr(self, key): 82 | setattr(self, key, value) 83 | 84 | def decide(self): 85 | commands = [] 86 | commands_futures = [] 87 | ''' 88 | https://docs.python.org/3/library/concurrent.futures.html 89 | ''' 90 | self.coach.decide() 91 | 92 | with futures.ThreadPoolExecutor(max_workers=self.n_robots) as executor: 93 | commands_futures = [ 94 | executor.submit(robot.decide) for robot in self.robots 95 | ] 96 | 97 | for future in futures.as_completed(commands_futures): 98 | commands.append(future.result()) 99 | 100 | return commands 101 | -------------------------------------------------------------------------------- /controller/simple_LQR.py: -------------------------------------------------------------------------------- 1 | import math 2 | import numpy as np 3 | import numpy.linalg as la 4 | from commons.math import speed_to_power 5 | 6 | def py_ang(v1, v2): 7 | """ Returns the angle in radians between vectors 'v1' and 'v2' """ 8 | cosang = np.dot(v1, v2) 9 | sinang = la.norm(np.cross(v1, v2)) 10 | return np.arctan2(sinang, cosang) 11 | 12 | """ 13 | Essa variavel experimental serve para converter o resultado do LQR 14 | para um valor coerente a velocidade desejada em m/s 15 | """ 16 | EXPERIMENTAL_SPEED_CONSTANT = 7000 17 | 18 | class SimpleLQR(object): 19 | def __init__(self, robot, l=0.185): 20 | self.desired = np.array([0, 0]) 21 | self.robot = robot 22 | self.environment = robot.game.environment 23 | 24 | self.l = l 25 | self.L = self.robot.dimensions.get('L') 26 | self.R = self.robot.dimensions.get('R') 27 | self.inverted = False 28 | 29 | def change_orientation(self): 30 | self.inverted = not self.inverted 31 | 32 | def set_desired(self, vector): 33 | 34 | self.desired = (self.robot.x + vector[0] * EXPERIMENTAL_SPEED_CONSTANT, self.robot.y + vector[1] * EXPERIMENTAL_SPEED_CONSTANT) 35 | 36 | def update(self): 37 | n = (1/self.l) 38 | 39 | theta = self.robot.theta 40 | if self.inverted: 41 | theta = self.robot.theta - math.pi 42 | 43 | v = self.desired[0] * math.cos(-theta) - self.desired[1] * math.sin(-theta) 44 | w = n * (self.desired[0] * math.sin(-theta) + self.desired[1] * math.cos(-theta)) 45 | 46 | if self.environment == 'simulation': 47 | powers = speed_to_power(v, w, self.L, self.R) 48 | if self.inverted: 49 | return tuple(np.dot(-1, powers)) 50 | else: 51 | return powers 52 | 53 | linear = v*self.R 54 | angular = self.R*(w*self.L)/2 55 | 56 | if self.inverted: 57 | linear = -linear 58 | 59 | return angular, linear 60 | 61 | class TwoSidesLQR(object): 62 | def __init__(self, robot, l=0.195): 63 | self.desired = np.array([0, 0]) 64 | self.robot = robot 65 | self.environment = robot.game.environment 66 | 67 | self.l = l 68 | self.L = self.robot.dimensions.get('L') 69 | self.R = self.robot.dimensions.get('R') 70 | 71 | def set_desired(self, vector): 72 | self.desired = (self.robot.x + vector[0] * EXPERIMENTAL_SPEED_CONSTANT, self.robot.y + vector[1] * EXPERIMENTAL_SPEED_CONSTANT) 73 | 74 | def update(self): 75 | n = (1/self.l) 76 | 77 | A = np.array(self.desired) 78 | B = np.array([math.cos(self.robot.theta), math.sin(self.robot.theta)]) 79 | between = py_ang(A, B) 80 | 81 | if (between > math.pi/2): 82 | theta = self.robot.theta - math.pi 83 | else: 84 | theta = self.robot.theta 85 | 86 | v = self.desired[0] * math.cos(-theta) - self.desired[1] * math.sin(-theta) 87 | w = n * (self.desired[0] * math.sin(-theta) + self.desired[1] * math.cos(-theta)) 88 | 89 | if self.environment == 'simulation': 90 | power_left, power_right = speed_to_power(v, w, self.L, self.R) 91 | 92 | if (between > math.pi/2): 93 | # this multiplies -1 to both elements of 'powers' and return it as a tuple 94 | return -power_right, -power_left 95 | else: 96 | return power_left, power_right 97 | 98 | linear = v*self.R 99 | angular = self.R*(w*self.L)/2 100 | 101 | if (between > math.pi/2): 102 | return -linear, -angular 103 | return linear, -angular 104 | -------------------------------------------------------------------------------- /strategy/tests/pfScratch.py: -------------------------------------------------------------------------------- 1 | import math 2 | import algorithms 3 | from strategy.BaseStrategy import Strategy 4 | from commons.math import unit_vector 5 | 6 | import json 7 | import numpy as np 8 | 9 | class Scratch(Strategy): 10 | def __init__(self, match, plot_field=False): 11 | super().__init__(match) 12 | 13 | """ 14 | Ambiente para rascunhar novas estrategias com 15 | campos potencias: 16 | 17 | Para criar estrategias, você vai precisar: 18 | 19 | 1) definir quantos comportamentos diferentes haverão na sua estrategia 20 | e quais serão as transições entre eles (usando posição da bola, angulo do robo, etc...) 21 | 22 | 2) para cada comportamento, criar e adicionar os campos que você ira precisar para que 23 | ele ocorra como desejado, caso seja necessario visualizar, use o plot_field=True no começo 24 | dessa função para ter o arquivo de campo (deixe rodar alguns segundos apenas com a simulação ligada) 25 | 26 | 3) Teste tanto os campos quanto as transições, analise se o campo não tem nenhum 27 | ponto morto (onde os campos se anulam). 28 | 29 | 4) :opcional: rode com outros robos, para verificar se ele atrapalha outras estrategias, cogite adicionar 30 | comportamentos que evitem que ele cometa faltas ou fique travado na parede, atrapalhe o goleiro e principalmente 31 | que evite-o de fazer gol contra :P 32 | """ 33 | 34 | self.plot_field = plot_field 35 | self.exporter = None 36 | 37 | """ 38 | Essa é uma definição basica de campo, você sempre irá usar o objeto 39 | PotentialField, sera passado para ele a referencia da partida (self.match) e um nome, 40 | nesse caso convencionamos que sera 'NOME_CLASSE|NOME_COMPORTAMENTO', o nome da classe 41 | já é dado pelo cósigo e o nome do comportamento você decide, nesse caso é FieldBehaviour 42 | """ 43 | self.field = algorithms.fields.PotentialField( 44 | self.match, 45 | name="{}|FieldBehaviour".format(self.__class__) 46 | ) 47 | """ 48 | Crie quantos você quiser, cada um irá representar um comportamento que você definiu no passo (1) 49 | """ 50 | 51 | 52 | 53 | def start(self, robot=None): 54 | super().start(robot=robot) 55 | 56 | if self.plot_field: 57 | self.exporter = algorithms.fields.PotentialDataExporter(self.robot.get_name()) 58 | 59 | """ 60 | No Start você ira adicionar campos potenciais aos comportamentos criados no metodo __init__ 61 | de uma olhada na documentação de como se cria cada campo e como eles se comportam. Aqui, por exemplo 62 | tem um campo que leva a bola, note que elementos dinamicos podem ser passados como uma função lambda 63 | referencia util para funções lambdas: https://realpython.com/python-lambda/. 64 | """ 65 | self.field.add_field( 66 | algorithms.fields.PointField( 67 | self.match, 68 | target = (0.75, 0.65), 69 | radius = 0.05, # 30cm 70 | decay = lambda x: 1, 71 | field_limits = [0.75*2 , 0.65*2], 72 | multiplier = 1.2 # 50 cm/s 73 | ) 74 | ) 75 | 76 | 77 | def reset(self, robot=None): 78 | super().reset() 79 | if robot: 80 | self.start(robot) 81 | 82 | 83 | def decide(self): 84 | """ 85 | No decide iremos programar as regras que irão decidir qual 86 | comportamento sera execuetado nesse momento. crie o conjunto de regras 87 | que preferir e no final atribua algum dos comportamentos a variavel behaviour 88 | """ 89 | behaviour = self.field 90 | 91 | if self.exporter: 92 | self.exporter.export(behaviour, self.robot, self.match.ball) 93 | return (0, 0) 94 | 95 | return behaviour.compute([self.robot.x, self.robot.y]) 96 | -------------------------------------------------------------------------------- /main.py: -------------------------------------------------------------------------------- 1 | import os 2 | from api.new_api import Api 3 | from api.api_recv import Api_recv 4 | import comm 5 | import vision 6 | import match 7 | import argparse 8 | import fields as pitch 9 | from pyVSSSReferee.RefereeComm import RefereeComm 10 | from commons.utils import get_config 11 | import time 12 | 13 | parser = argparse.ArgumentParser(description='NeonFC') 14 | parser.add_argument('--config_file', default='config.json') 15 | parser.add_argument('--env', default='simulation') 16 | 17 | args = parser.parse_args() 18 | 19 | class Game(): 20 | def __init__(self, config_file=None, env='simulation'): 21 | self.config = get_config(config_file) 22 | self.match = match.Match(self, 23 | **self.config.get('match') 24 | ) 25 | self.vision = vision.FiraVision() 26 | self.comm = comm.FiraComm() 27 | self.field = pitch.Field(self.match.category) 28 | self.referee = RefereeComm(config_file) 29 | self.environment = env 30 | 31 | self.use_api = self.config.get("api") 32 | self.api_address = self.config.get("network").get("api_address") 33 | self.api_port = self.config.get("network").get("api_port") 34 | self.api_recv_port = self.config.get("network").get("api_recv_port") 35 | 36 | self.api = Api(self.api_address, self.api_port) 37 | 38 | self.api_recv = Api_recv(self.match, self.api_address, self.api_recv_port) 39 | 40 | if os.environ.get('USE_REFEREE'): 41 | self.use_referee = bool(int(os.environ.get('USE_REFEREE'))) 42 | else: 43 | self.use_referee = self.config.get('referee') 44 | 45 | self.start() 46 | 47 | def start(self): 48 | self.vision.assign_vision(self) 49 | self.referee.start() 50 | self.match.start() 51 | 52 | self.vision.start() 53 | self.comm.start() 54 | 55 | if self.use_api: 56 | self.api.start() 57 | self.api_recv.start() 58 | 59 | 60 | def update(self): 61 | frame = vision.assign_empty_values( 62 | self.vision.frame, 63 | field_size=self.field.get_dimensions(), 64 | team_side=self.match.team_side 65 | ) 66 | self.match.update(frame) 67 | commands = self.match.decide() 68 | 69 | if self.use_api and self.match.game_status == 'stop': 70 | commands = [ 71 | { 72 | 'robot_id': r['robot_id'], 73 | 'color': r['color'], 74 | 'wheel_left': 0, 75 | 'wheel_right': 0 76 | } for r in commands 77 | ] 78 | self.comm.send(commands) 79 | 80 | if self.referee.can_play() or (not self.use_referee): 81 | self.comm.send(commands) 82 | else: 83 | commands = [ 84 | { 85 | 'robot_id': r['robot_id'], 86 | 'color': r['color'], 87 | 'wheel_left': 0, 88 | 'wheel_right': 0 89 | } for r in commands 90 | ] 91 | self.comm.send(commands) 92 | 93 | if self.referee.get_foul() != "STOP" and self.referee.get_foul() != None and self.referee.get_foul() != "HALT": 94 | if self.match.coach.get_positions( self.referee.get_foul(), self.match.team_color.upper(), self.referee.get_color(), self.referee.get_quadrant() ): 95 | self.referee.send_replacement( 96 | self.match.coach.get_positions( self.referee.get_foul(), self.match.team_color.upper(), self.referee.get_color(), self.referee.get_quadrant() ), 97 | self.match.team_color.upper() 98 | ) 99 | 100 | if self.use_api: 101 | self.api.send_data(self.match) 102 | 103 | g = Game(config_file=args.config_file, env=args.env) 104 | 105 | 106 | try: 107 | while True: 108 | time.sleep(0.01) 109 | except KeyboardInterrupt: 110 | g.stop() 111 | -------------------------------------------------------------------------------- /main_real_life.py: -------------------------------------------------------------------------------- 1 | from api import Api, Api_recv, InfoApi 2 | import comm 3 | import match 4 | import argparse 5 | import fields as pitch 6 | from commons.utils import get_config 7 | from pyVSSSReferee.RefereeComm import RefereeComm 8 | from pySSLVision.VisionComm import SSLVision, assign_empty_values 9 | import os 10 | import threading 11 | import time, collections 12 | 13 | parser = argparse.ArgumentParser(description='NeonFC') 14 | parser.add_argument('--config_file', default='config_real_life.json') 15 | parser.add_argument('--env', default='real_life') 16 | 17 | args = parser.parse_args() 18 | 19 | class Game(): 20 | def __init__(self, config_file=None, env='real_life'): 21 | self.config = get_config(config_file) 22 | self.match = match.MatchRealLife(self, 23 | **self.config.get('match') 24 | ) 25 | self.vision = SSLVision() 26 | self.comm = comm.RLComm() 27 | self.field = pitch.Field(self.match.category) 28 | self.environment = env 29 | 30 | self.t1 = time.time() 31 | self.list = collections.deque(maxlen=25) 32 | 33 | self.use_api = self.config.get("api") 34 | self.api_address = self.config.get("network").get("api_address") 35 | self.api_port = self.config.get("network").get("api_port") 36 | self.api_recv_port = self.config.get("network").get("api_recv_port") 37 | 38 | self.referee = RefereeComm(config_file) 39 | 40 | if self.use_api: 41 | self.api = Api(self.api_address, self.api_port) 42 | self.api_recv = Api_recv(self.match, self.api_address, self.api_recv_port) 43 | 44 | if os.environ.get('USE_REFEREE'): 45 | self.use_referee = bool(int(os.environ.get('USE_REFEREE'))) 46 | else: 47 | self.use_referee = self.config.get('referee') 48 | self.start() 49 | 50 | def start(self): 51 | self.vision.assign_vision(self.update) 52 | if self.use_referee: 53 | self.referee.start() 54 | self.match.start() 55 | 56 | self.vision.start() 57 | self.comm.start() 58 | 59 | if self.use_api: 60 | self.info_api = InfoApi(self.match, self.match.robots, self.match.opposites, self.match.coach, self.match.ball, self.match.parameters) 61 | self.api.start() 62 | self.api_recv.connect_info(self.info_api) 63 | self.api_recv.start() 64 | self.sender_thread = threading.Thread(target = self.api.send_data, args=[self.info_api]) 65 | self.sender_thread.start() 66 | 67 | def update(self): 68 | frame = assign_empty_values( 69 | self.vision.frame, 70 | field_size=self.field.get_dimensions(), 71 | team_side=self.match.team_side, 72 | last_frame=self.vision.last_frame 73 | ) 74 | self.vision.last_frame = frame 75 | 76 | self.match.update(frame) 77 | if self.use_referee: 78 | self.match.check_foul(self.referee) 79 | commands = self.match.decide() 80 | 81 | if (self.use_api or self.use_referee) and (self.match.game_status == 'STOP' or self.match.game_status is None): 82 | commands = [ 83 | { 84 | 'robot_id': r['robot_id'], 85 | 'color': r['color'], 86 | 'wheel_left': 0, 87 | 'wheel_right': 0 88 | } for r in commands 89 | ] 90 | 91 | self.comm.send(commands) 92 | delta_t = float(time.time() - self.t1) 93 | self.list.append(delta_t) 94 | self.t1 = time.time() 95 | 96 | print(len(self.list)/sum(self.list), 'hz') 97 | 98 | def stop(self): 99 | for t in threading.enumerate(): 100 | t.kill_recieved = True 101 | 102 | 103 | 104 | g = Game(config_file=args.config_file, env=args.env) 105 | 106 | try: 107 | while True: 108 | time.sleep(0.01) 109 | except KeyboardInterrupt: 110 | g.stop() 111 | 112 | 113 | -------------------------------------------------------------------------------- /entities/coach/Backup.py: -------------------------------------------------------------------------------- 1 | from commons.math import distance_between_points 2 | from entities.coach.coach import BaseCoach 3 | import strategy 4 | import json 5 | 6 | import strategy.rsm2025 7 | 8 | 9 | class Coach(BaseCoach): 10 | NAME = "IRON_2025" 11 | 12 | def __init__(self, match): 13 | super().__init__(match) 14 | 15 | self.SS_strategy = strategy.rsm2025.ShadowAttacker(self.match) 16 | self.ST_strategy = strategy.rsm2025.MainStriker(self.match) 17 | self.GK_strategy = strategy.rsm2025.Goalkeeper(self.match) 18 | self.CB_strategy = strategy.rsm2025.Defender(self.match) 19 | self.SD_strategy = strategy.rsm2025.ShadowDefender(self.match) 20 | 21 | self.GK_id = 0 # Goalkeeper fixed ID 22 | # todo volta isso pra 5 23 | self.defending = False 24 | 25 | # self.unstucks = {r.robot_id: strategy.rsm2023.Unstuck(self.match) for r in self.match.robots if r.robot_id != self.GK_id} 26 | 27 | def decide(self): 28 | if self.match.match_event['event'] == 'PLAYING': 29 | GK = next(filter(lambda r: r.robot_id == self.GK_id, self.match.robots)) 30 | self.set_strategy(GK, self.GK_strategy) 31 | 32 | 33 | if self.match.ball.x < .6: 34 | self.defend() 35 | # print("Defend") 36 | else: 37 | self.attack() 38 | # print("Attadck") 39 | 40 | else: 41 | self.not_playing() 42 | 43 | def attack(self): 44 | strikers = [r for i, r in enumerate(self.match.robots) if r.robot_id != self.GK_id] 45 | ST, SS = self.choose_main_striker(*strikers) 46 | 47 | self.set_strategy(ST, self.ST_strategy) 48 | self.set_strategy(SS, self.SS_strategy) 49 | 50 | def defend(self): 51 | defenders = [r for i, r in enumerate(self.match.robots) if r.robot_id != self.GK_id] 52 | CB, SD = self.choose_main_defender(*defenders) 53 | 54 | self.set_strategy(CB, self.CB_strategy) 55 | self.set_strategy(SD, self.SD_strategy) 56 | 57 | def not_playing(self): 58 | robots = [(i, r.robot_id) for i, r in enumerate(self.match.robots)] 59 | for robot, strategy in zip(robots, self._position): 60 | if self.match.robots[robot[0]].strategy == strategy: 61 | continue 62 | self.match.robots[robot[0]].strategy = strategy 63 | self.match.robots[robot[0]].start() 64 | 65 | def set_strategy(self, robot, strat): 66 | if robot != strat: 67 | robot.strategy = strat 68 | robot.start() 69 | 70 | def choose_main_striker(self, r1, r2): 71 | b = self.match.ball 72 | 73 | a1 = distance_between_points((b.x, b.y), (r1.x, r1.y)) 74 | a2 = distance_between_points((b.x, b.y), (r2.x, r2.y)) 75 | 76 | b1, b2 = b.x - r1.x - 0.05, b.x - r2.x - 0.05 77 | 78 | if b1 * b2 > 0: 79 | if a1 < a2: 80 | return r1, r2 81 | return r2, r1 82 | if b1 > 0: 83 | return r1, r2 84 | return r2, r1 85 | 86 | def choose_main_defender(self, r1, r2): 87 | b = self.match.ball 88 | 89 | a1 = distance_between_points((b.x, b.y), (r1.x, r1.y)) 90 | a2 = distance_between_points((b.x, b.y), (r2.x, r2.y)) 91 | 92 | 93 | if a1 < a2: 94 | return r1, r2 95 | return r2, r1 96 | 97 | def handle_stuck(self, ST, SS): 98 | game_runing = not (self.match.game_status == 'STOP' or self.match.game_status == None) 99 | stuck_st = ST.is_stuck() and game_runing 100 | stuck_ss = SS.is_stuck() and game_runing 101 | 102 | # if stuck_st and stuck_ss: 103 | # return self.unstucks[ST.robot_id], self.unstucks[SS.robot_id] 104 | # 105 | # if stuck_st and not stuck_ss: 106 | # return self.unstucks[ST.robot_id], self.ST_strategy 107 | # 108 | # if not stuck_st and stuck_ss: 109 | # return self.ST_strategy, self.unstucks[SS.robot_id] 110 | 111 | return self.ST_strategy, self.SS_strategy 112 | -------------------------------------------------------------------------------- /vision/vision.py: -------------------------------------------------------------------------------- 1 | import socket 2 | import struct 3 | 4 | import json 5 | import time 6 | import math 7 | 8 | import threading 9 | 10 | import os 11 | 12 | from collections import deque 13 | from commons.utils import get_config 14 | 15 | from google.protobuf.json_format import MessageToJson 16 | 17 | from protocols import packet_pb2 18 | 19 | 20 | class FiraVision(threading.Thread): 21 | def __init__(self): 22 | super(FiraVision, self).__init__() 23 | self.config = get_config() 24 | 25 | self.frame = {} 26 | 27 | self.vision_port = int(os.environ.get('VISION_PORT', self.config['network']['vision_port'])) 28 | self.host = os.environ.get('MULTICAST_IP', self.config['network']['multicast_ip']) 29 | 30 | self._fps = 0 31 | self._frame_times = deque(maxlen=60) 32 | 33 | def assign_vision(self, game): 34 | self.game = game 35 | 36 | def set_fps(self): 37 | self._frame_times.append(time.time()) 38 | if len(self._frame_times) <= 3: 39 | return 40 | fps_frame_by_frame = [ 41 | (v - i) for i, v in zip(self._frame_times, list(self._frame_times)[1:]) 42 | ] 43 | self._fps = len(fps_frame_by_frame)/sum(fps_frame_by_frame) 44 | 45 | def run(self): 46 | print("Starting vision...") 47 | self.vision_sock = self._create_socket() 48 | self._wait_to_connect() 49 | print("Vision completed!") 50 | 51 | while True: 52 | env = packet_pb2.Environment() 53 | data = self.vision_sock.recv(1024) 54 | self.set_fps() 55 | env.ParseFromString(data) 56 | self.frame = json.loads(MessageToJson(env)) 57 | self.game.update() 58 | 59 | 60 | def _wait_to_connect(self): 61 | self.vision_sock.recv(1024) 62 | 63 | def _create_socket(self): 64 | print(f"Creating socket with address: {self.host} and port: {self.vision_port}") 65 | sock = socket.socket( 66 | socket.AF_INET, 67 | socket.SOCK_DGRAM, 68 | socket.IPPROTO_UDP 69 | ) 70 | 71 | sock.setsockopt( 72 | socket.SOL_SOCKET, 73 | socket.SO_REUSEADDR, 1 74 | ) 75 | 76 | sock.bind((self.host, self.vision_port)) 77 | 78 | mreq = struct.pack( 79 | "4sl", 80 | socket.inet_aton(self.host), 81 | socket.INADDR_ANY 82 | ) 83 | 84 | sock.setsockopt( 85 | socket.IPPROTO_IP, 86 | socket.IP_ADD_MEMBERSHIP, 87 | mreq 88 | ) 89 | 90 | return sock 91 | 92 | 93 | def assign_empty_values(raw_frame, field_size, team_side): 94 | frame = raw_frame.get('frame') 95 | w, h = field_size 96 | if frame.get('ball'): 97 | if team_side == 'right': 98 | frame['ball']['x'] = -frame['ball'].get('x', 0) 99 | frame['ball']['y'] = -frame['ball'].get('y', 0) 100 | 101 | frame['ball']['x'] = frame['ball'].get('x', 0) + w/2 102 | frame['ball']['y'] = frame['ball'].get('y', 0) + h/2 103 | 104 | for robot in frame.get("robotsYellow"): 105 | if team_side == 'right': 106 | robot['x'] = - robot.get('x', 0) 107 | robot['y'] = - robot.get('y', 0) 108 | robot['orientation'] = robot.get('orientation', 0) + math.pi 109 | 110 | robot['x'] = robot.get('x', 0) + w/2 111 | robot['y'] = robot.get('y', 0) + h/2 112 | robot['robotId'] = robot.get('robotId', 0) 113 | robot['orientation'] = robot.get('orientation', 0) 114 | 115 | for robot in frame.get("robotsBlue"): 116 | if team_side == 'right': 117 | robot['x'] = - robot.get('x', 0) 118 | robot['y'] = - robot.get('y', 0) 119 | robot['orientation'] = robot.get('orientation', 0) + math.pi 120 | 121 | robot['x'] = robot.get('x', 0) + w/2 122 | robot['y'] = robot.get('y', 0) + h/2 123 | robot['robotId'] = robot.get('robotId', 0) 124 | robot['orientation'] = robot.get('orientation', 0) 125 | 126 | return frame 127 | 128 | if __name__ == "__main__": 129 | import time 130 | v = FiraVision() 131 | 132 | v.start() 133 | 134 | while True: 135 | time.sleep(1) 136 | print(v.frame) -------------------------------------------------------------------------------- /entities/coach/rsm2025_defend.py: -------------------------------------------------------------------------------- 1 | from commons.math import distance_between_points 2 | from entities.coach.coach import BaseCoach 3 | import strategy 4 | import json 5 | 6 | import strategy.rsm2025 7 | 8 | 9 | class Coach(BaseCoach): 10 | NAME = "RSM_2025_Defend" 11 | 12 | def __init__(self, match): 13 | super().__init__(match) 14 | #print("AAAAAA");print(match); print(match.coach) 15 | 16 | self.SS_strategy = strategy.rsm2025.ShadowAttacker(self.match) 17 | self.ST_strategy = strategy.rsm2025.MainStriker(self.match) 18 | self.GK_strategy = strategy.rsm2025.Goalkeeper(self.match) 19 | self.CB_strategy = strategy.rsm2025.Defender(self.match) 20 | self.SD_strategy = strategy.rsm2025.ShadowDefender(self.match) 21 | 22 | self.GK_id = 5 # Goalkeeper fixed ID 23 | self.defending = False 24 | 25 | positions = json.loads(open('foul_placements3v3.json', 'r').read()) 26 | self._position = {r.robot_id: strategy.commons.Replacer(self.match, positions[str(r.robot_id)]) for r in self.match.robots} 27 | 28 | # self.unstucks = {r.robot_id: strategy.rsm2023.Unstuck(self.match) for r in self.match.robots if r.robot_id != self.GK_id} 29 | 30 | def decide(self): 31 | if self.match.match_event['event'] == 'PLAYING': 32 | GK = next(filter(lambda r: r.robot_id == self.GK_id, self.match.robots)) 33 | self.set_strategy(GK, self.GK_strategy) 34 | 35 | if self.match.ball.x < .5: 36 | self.defend() 37 | # print("Defend") 38 | else: 39 | self.attack() 40 | # print("Attack") 41 | 42 | else: 43 | self.not_playing() 44 | 45 | def attack(self): 46 | strikers = [r for i, r in enumerate(self.match.robots) if r.robot_id != self.GK_id] 47 | ST, SS = self.choose_main_striker(*strikers) 48 | 49 | 50 | self.set_strategy(ST, self.ST_strategy) 51 | self.set_strategy(SS, self.SS_strategy) 52 | 53 | def defend(self): 54 | defenders = [r for i, r in enumerate(self.match.robots) if r.robot_id != self.GK_id] 55 | CB, SD = self.choose_main_defender(*defenders) 56 | 57 | 58 | self.set_strategy(CB, self.CB_strategy) 59 | self.set_strategy(SD, self.SD_strategy) 60 | 61 | def not_playing(self): 62 | robots = [(i, r.robot_id) for i, r in enumerate(self.match.robots)] 63 | for robot, strategy in zip(robots, self._position): 64 | if self.match.robots[robot[0]].strategy == strategy: 65 | continue 66 | self.match.robots[robot[0]].strategy = strategy 67 | self.match.robots[robot[0]].start() 68 | 69 | def set_strategy(self, robot, strat): 70 | if robot != strat: 71 | robot.strategy = strat 72 | robot.start() 73 | 74 | def choose_main_striker(self, r1, r2): 75 | b = self.match.ball 76 | 77 | a1 = distance_between_points((b.x, b.y), (r1.x, r1.y)) 78 | a2 = distance_between_points((b.x, b.y), (r2.x, r2.y)) 79 | 80 | b1, b2 = b.x - r1.x - 0.05, b.x - r2.x - 0.05 81 | 82 | if b1 * b2 > 0: 83 | if a1 < a2: 84 | return r1, r2 85 | return r2, r1 86 | if b1 > 0: 87 | return r1, r2 88 | return r2, r1 89 | 90 | def choose_main_defender(self, r1, r2): 91 | b = self.match.ball 92 | 93 | a1 = distance_between_points((b.x, b.y), (r1.x, r1.y)) 94 | a2 = distance_between_points((b.x, b.y), (r2.x, r2.y)) 95 | 96 | 97 | if a1 < a2: 98 | return r1, r2 99 | return r2, r1 100 | 101 | def handle_stuck(self, ST, SS): 102 | game_runing = not (self.match.game_status == 'STOP' or self.match.game_status == None) 103 | stuck_st = ST.is_stuck() and game_runing 104 | stuck_ss = SS.is_stuck() and game_runing 105 | 106 | # if stuck_st and stuck_ss: 107 | # return self.unstucks[ST.robot_id], self.unstucks[SS.robot_id] 108 | # 109 | # if stuck_st and not stuck_ss: 110 | # return self.unstucks[ST.robot_id], self.ST_strategy 111 | # 112 | # if not stuck_st and stuck_ss: 113 | # return self.ST_strategy, self.unstucks[SS.robot_id] 114 | 115 | return self.ST_strategy, self.SS_strategy 116 | -------------------------------------------------------------------------------- /match/match_real_life.py: -------------------------------------------------------------------------------- 1 | import os 2 | import entities 3 | from concurrent.futures import ThreadPoolExecutor, as_completed 4 | from api import Parameter 5 | import entities.coach.coach 6 | 7 | CATEGORIES = { 8 | '3v3': 3, '5v5': 5 9 | } 10 | 11 | class MatchRealLife(object): 12 | def __init__(self, game, team_side, team_color, coach_name=None, category="3v3", robot_ids=[0,1,2], opposite_ids=[0,1,2]): 13 | super().__init__() 14 | self.game = game 15 | 16 | self.coach_name = os.environ.get('COACH_NAME', coach_name) 17 | self.team_side = os.environ.get('TEAM_SIDE', team_side) 18 | self.team_color = os.environ.get('TEAM_COLOR', team_color) 19 | self.category = os.environ.get('CATEGORY', category) 20 | self.n_robots = CATEGORIES.get(self.category) 21 | self.robot_ids = os.environ.get('robot_ids', robot_ids) 22 | self.opposite_ids = os.environ.get('opposite_ids', opposite_ids) 23 | 24 | 25 | pid_kp, ki, kd = Parameter(0.1, 'pid_kp'), Parameter(0, 'ki'), Parameter(0, 'kd') 26 | kw, rm, vm, uni_kp = Parameter(3.5, 'kw'), Parameter(0.44, 'rm'), Parameter(0.5, 'vm'), Parameter(1, 'uni_kp') 27 | 28 | self.parameters = {"pid_kp":pid_kp, "ki":ki, "kd":kd, "kw":kw, "rm":rm, "vm":vm, "uni_kp":uni_kp} 29 | 30 | self.opposite_team_color = 'yellow' if self.team_color == 'blue' else 'blue' 31 | 32 | self.game_status = 'GAME_ON' 33 | self.match_event = {'event': 'PLAYING', 'quadrant': 1, 'mine': True} 34 | 35 | 36 | def start(self): 37 | print("Starting match module starting ...") 38 | self.ball = entities.Ball(self.game) 39 | 40 | self.opposites = [ 41 | entities.Robot(self.game, i, self.opposite_team_color) for i in self.opposite_ids 42 | ] 43 | 44 | self.robots = [ 45 | entities.Robot(self.game, i, self.team_color) for i in self.robot_ids 46 | ] 47 | 48 | self.coach = entities.coach.COACHES[self.coach_name](self) 49 | print(f"Match started! coach is [{self.coach.NAME}]") 50 | self.coach.decide() 51 | 52 | for robot in self.robots: 53 | robot.start() 54 | 55 | def restart(self, team_color): 56 | self.team_color = team_color 57 | self.opposite_team_color = 'yellow' if self.team_color == 'blue' else 'blue' 58 | 59 | self.opposites = [ 60 | entities.Robot(self.game, i, self.opposite_team_color) for i in self.opposite_ids 61 | ] 62 | 63 | self.robots = [ 64 | entities.Robot(self.game, i, self.team_color) for i in self.robot_ids 65 | ] 66 | 67 | self.coach.decide() 68 | 69 | for robot in self.robots: 70 | robot.start() 71 | 72 | 73 | def update(self, frame): 74 | self.ball.update(frame) 75 | 76 | for entity in self.opposites: 77 | entity.update(frame) 78 | 79 | for entity in self.robots: 80 | entity.update(frame) 81 | 82 | def check_foul(self, ref): 83 | if ref.can_play(): 84 | self.match_event['event'] = 'PLAYING' 85 | self.game_status = 'GAME_ON' 86 | else: 87 | if ref.get_foul() == 'STOP': 88 | self.game_status = 'STOP' 89 | return 90 | self.game_status = 'GAME_ON' 91 | 92 | self.match_event['event'] = 'KICKOFF' if ref.get_foul() == None else ref.get_foul() 93 | self.match_event['quadrant'] = ref.get_quadrant() 94 | self.match_event['mine'] = ref.get_color() == self.team_color.upper() 95 | 96 | 97 | def update_information(self, info): #Function to update values recieved in api 98 | for key, value in info.items(): 99 | setattr(self, key.lower(), value) 100 | 101 | 102 | def decide(self): 103 | commands = [] 104 | commands_futures = [] 105 | ''' 106 | https://docs.python.org/3/library/concurrent.futures.html 107 | ''' 108 | self.coach.decide() 109 | 110 | with ThreadPoolExecutor(max_workers=self.n_robots) as executor: 111 | commands_futures = [ 112 | executor.submit(robot.decide) for robot in self.robots 113 | ] 114 | 115 | for future in as_completed(commands_futures): 116 | commands.append(future.result()) 117 | 118 | return commands -------------------------------------------------------------------------------- /commons/math.py: -------------------------------------------------------------------------------- 1 | import math 2 | import numpy as np 3 | from numpy import arccos, array, dot, pi, cross 4 | from numpy.linalg import det, norm 5 | 6 | def dist_point_line(x1, y1, x2, y2, x3, y3): 7 | """ 8 | verify the distance between a point and a line 9 | x1,y1: definition of the first point that makes the line 10 | x2,y2: definition of the second point that makes the line 11 | x3,y3: definition of the point that will be used to calculate the distance 12 | """ 13 | px = x2-x1 14 | py = y2-y1 15 | 16 | norm = px*px + py*py 17 | 18 | u = ((x3 - x1) * px + (y3 - y1) * py) / float(norm) 19 | 20 | if u > 1: 21 | u = 1 22 | elif u < 0: 23 | u = 0 24 | 25 | x = x1 + u * px 26 | y = y1 + u * py 27 | 28 | dx = x - x3 29 | dy = y - y3 30 | 31 | dist = (dx*dx + dy*dy)**.5 32 | 33 | return dist 34 | 35 | def _fix_angle(theta_1, theta_2): 36 | rate_theta = (theta_2 - theta_1) 37 | 38 | if (rate_theta > math.pi ): 39 | rate_theta -= 2 * math.pi 40 | elif (rate_theta < -math.pi): 41 | rate_theta += 2 * math.pi 42 | 43 | return rate_theta 44 | 45 | def angular_speed(_list, _fps): 46 | if len(_list) <= 1: 47 | return 0 48 | 49 | speed_fbf = [ 50 | _fix_angle(t0, t1) for t0, t1 51 | in zip( 52 | _list, 53 | list(_list)[1:] 54 | ) 55 | ] 56 | if not speed_fbf: 57 | return 0 58 | return _fps * (sum(speed_fbf)/len(speed_fbf)) 59 | 60 | def speed(_list, _fps): 61 | if len(_list) <= 1: 62 | return 0 63 | 64 | speed_fbf = [ 65 | (t1 - t0) for t0, t1 66 | in zip( 67 | _list, 68 | list(_list)[1:] 69 | ) if abs((t1 - t0)) < 0.1 70 | # considering the game runs at 60 fps 71 | # to limit 0.1 m/f here is to say that is impossible 72 | # for the robot to run at 6 m/s (0.1 [m][f⁻¹] * 60 [f][s⁻¹] = 6[m][s⁻¹]) 73 | ] 74 | if not speed_fbf: 75 | return 0 76 | return _fps * (sum(speed_fbf)/len(speed_fbf)) 77 | 78 | def unit_vector(vector): 79 | """ Returns the unit vector of the vector.""" 80 | if np.linalg.norm(vector) == 0: 81 | return np.array([0, 0]) 82 | return vector / np.linalg.norm(vector) 83 | 84 | def rotate_via_numpy(xy, radians): 85 | """Use numpy to build a rotation matrix and take the dot product.""" 86 | x, y = xy 87 | c, s = np.cos(radians), np.sin(radians) 88 | j = np.matrix([[c, s], [-s, c]]) 89 | m = np.dot(j, [x, y]) 90 | 91 | return float(m.T[0]), float(m.T[1]) 92 | 93 | def dot_product(v1, v2): 94 | return sum((a*b) for a, b in zip(v1, v2)) 95 | 96 | def length(v): 97 | return math.sqrt(dot_product(v, v)) 98 | 99 | def angle_between(v1, v2): 100 | return math.acos(dot_product(v1, v2) / (length(v1) * length(v2))) 101 | 102 | def distance(A, B, P): 103 | """ segment line AB, point P, where each one is an array([x, y]) """ 104 | A = np.array(A) 105 | B = np.array(B) 106 | P = np.array(P) 107 | if all(A == P) or all(B == P) or all(A == B): 108 | return 0 109 | if arccos(dot((P - A) / norm(P - A), (B - A) / norm(B - A))) > pi / 2: 110 | return norm(P - A) 111 | if arccos(dot((P - B) / norm(P - B), (A - B) / norm(A - B))) > pi / 2: 112 | return norm(P - B) 113 | return norm(cross(A-B, A-P))/norm(B-A) 114 | 115 | def distance_to_line(x, y, l1x, l1y, l2x, l2y): 116 | x_diff = l2x - l1x 117 | y_diff = l2y - l1y 118 | num = y_diff*x - x_diff*y + l2x*l1y - l2y*l1x 119 | den = math.sqrt(y_diff**2 + x_diff**2) 120 | return num / den 121 | 122 | def point_in_rect(point, rect): 123 | x1, y1, w, h = rect 124 | x2, y2 = x1+w, y1+h 125 | x, y = point 126 | if (x1 < x and x < x2): 127 | if (y1 < y and y < y2): 128 | return True 129 | return False 130 | 131 | def distance_between_points(p1, p2): 132 | ''' 133 | Calculates the distance between 2 points, p1 and p2. 134 | Arguments: 135 | p1: an array([x, y]) 136 | p2: an array([x, y]) 137 | Returns: 138 | Distance between p1 and p2 139 | ''' 140 | dx = p1[0] - p2[0] 141 | dy = p1[1] - p2[1] 142 | 143 | return np.sqrt(dx**2 + dy**2) 144 | 145 | def speed_to_power(linear_speed, angular_speed, L, R): 146 | 147 | power_left = (2*linear_speed - angular_speed*L)/2 * R 148 | power_right = (2*linear_speed + angular_speed*L)/2 * R 149 | 150 | return power_left, power_right 151 | -------------------------------------------------------------------------------- /algorithms/dijkstra_waypoint/dijkstra.py: -------------------------------------------------------------------------------- 1 | """ 2 | Code adapted from https://www.bogotobogo.com/python/python_Dijkstras_Shortest_Path_Algorithm.php 3 | """ 4 | 5 | import math 6 | import heapq 7 | 8 | class Vertex: 9 | def __init__(self, node, x, y): 10 | self.id = node 11 | 12 | self.x = x 13 | self.y = y 14 | 15 | self.adjacent = {} 16 | # Set distance to infinity for all nodes 17 | self.distance = 999999 18 | # Mark all nodes unvisited 19 | self.visited = False 20 | # Predecessor 21 | self.previous = None 22 | 23 | def __lt__(self, other): 24 | return self.distance < other.distance 25 | 26 | def add_neighbor(self, neighbor, weight=0): 27 | self.adjacent[neighbor] = weight 28 | 29 | def get_connections(self): 30 | return self.adjacent.keys() 31 | 32 | def get_id(self): 33 | return self.id 34 | 35 | def get_weight(self, neighbor): 36 | return self.adjacent[neighbor] 37 | 38 | def set_distance(self, dist): 39 | self.distance = dist 40 | 41 | def get_distance(self): 42 | return self.distance 43 | 44 | def set_previous(self, prev): 45 | self.previous = prev 46 | 47 | def set_visited(self): 48 | self.visited = True 49 | 50 | def __str__(self): 51 | return str(self.id) + ' adjacent: ' + str([x.id for x in self.adjacent]) 52 | 53 | class Graph: 54 | def __init__(self): 55 | self.vert_dict = {} 56 | self.num_vertices = 0 57 | 58 | self.edges = [] 59 | 60 | def __iter__(self): 61 | return iter(self.vert_dict.values()) 62 | 63 | def add_vertex(self, node, x, y): 64 | self.num_vertices = self.num_vertices + 1 65 | new_vertex = Vertex(node, x, y) 66 | self.vert_dict[node] = new_vertex 67 | return new_vertex 68 | 69 | def get_vertex(self, n): 70 | if n in self.vert_dict: 71 | return self.vert_dict[n] 72 | else: 73 | return None 74 | 75 | def add_edge(self, frm, to, cost = None): 76 | if frm.get_id() not in self.vert_dict: 77 | self.add_vertex(frm) 78 | if to.get_id() not in self.vert_dict: 79 | self.add_vertex(to) 80 | 81 | self.edges.append([[frm.x, frm.y], [to.x, to.y]]) 82 | 83 | if cost == None: 84 | cost = math.sqrt( (frm.x - to.x)**2 + (frm.y - to.y)**2 ) 85 | 86 | 87 | 88 | frm.add_neighbor(to, cost) 89 | to.add_neighbor(frm, cost) 90 | 91 | def get_vertices(self): 92 | return self.vert_dict.keys() 93 | 94 | def set_previous(self, current): 95 | self.previous = current 96 | 97 | def get_previous(self, current): 98 | return self.previous 99 | 100 | def shortest(v, path): 101 | ''' make shortest path from v.previous''' 102 | if v.previous: 103 | path.append(v.previous.get_id()) 104 | shortest(v.previous, path) 105 | return 106 | 107 | def dijkstra(aGraph, start_name, target_name): 108 | # print("Dijkstra's shortest path") 109 | # Set the distance for the start node to zero 110 | # print(start_name, target_name, aGraph.vert_dict.keys()) 111 | start = aGraph.vert_dict[start_name] 112 | target = aGraph.vert_dict[target_name] 113 | 114 | start.set_distance(0) 115 | 116 | # Put tuple pair into the priority queue 117 | unvisited_queue = [(v.get_distance(), v) for v in aGraph] 118 | heapq.heapify(unvisited_queue) 119 | 120 | while len(unvisited_queue): 121 | # Pops a vertex with the smallest distance 122 | uv = heapq.heappop(unvisited_queue) 123 | current = uv[1] 124 | current.set_visited() 125 | 126 | #for next in v.adjacent: 127 | for next in current.adjacent: 128 | # if visited, skip 129 | if next.visited: 130 | continue 131 | new_dist = current.get_distance() + current.get_weight(next) 132 | 133 | if new_dist < next.get_distance(): 134 | next.set_distance(new_dist) 135 | next.set_previous(current) 136 | else: 137 | pass 138 | 139 | # Rebuild heap 140 | # 1. Pop every item 141 | while len(unvisited_queue): 142 | heapq.heappop(unvisited_queue) 143 | # 2. Put all vertices not visited into the queue 144 | unvisited_queue = [(v.get_distance(),v) for v in aGraph if not v.visited] 145 | heapq.heapify(unvisited_queue) 146 | -------------------------------------------------------------------------------- /protocols/vssref_command_pb2.py: -------------------------------------------------------------------------------- 1 | # Generated by the protocol buffer compiler. DO NOT EDIT! 2 | # source: vssref_command.proto 3 | 4 | import sys 5 | _b=sys.version_info[0]<3 and (lambda x:x) or (lambda x:x.encode('latin1')) 6 | from google.protobuf import descriptor as _descriptor 7 | from google.protobuf import message as _message 8 | from google.protobuf import reflection as _reflection 9 | from google.protobuf import symbol_database as _symbol_database 10 | # @@protoc_insertion_point(imports) 11 | 12 | _sym_db = _symbol_database.Default() 13 | 14 | 15 | from protocols import vssref_common_pb2 as vssref__common__pb2 16 | 17 | 18 | DESCRIPTOR = _descriptor.FileDescriptor( 19 | name='vssref_command.proto', 20 | package='VSSRef.ref_to_team', 21 | syntax='proto3', 22 | serialized_options=None, 23 | serialized_pb=_b('\n\x14vssref_command.proto\x12\x12VSSRef.ref_to_team\x1a\x13vssref_common.proto\"\xa9\x01\n\x0eVSSRef_Command\x12\x1a\n\x04\x66oul\x18\x01 \x01(\x0e\x32\x0c.VSSRef.Foul\x12 \n\tteamcolor\x18\x02 \x01(\x0e\x32\r.VSSRef.Color\x12&\n\x0c\x66oulQuadrant\x18\x03 \x01(\x0e\x32\x10.VSSRef.Quadrant\x12\x11\n\ttimestamp\x18\x04 \x01(\x01\x12\x1e\n\x08gameHalf\x18\x05 \x01(\x0e\x32\x0c.VSSRef.Halfb\x06proto3') 24 | , 25 | dependencies=[vssref__common__pb2.DESCRIPTOR,]) 26 | 27 | 28 | 29 | 30 | _VSSREF_COMMAND = _descriptor.Descriptor( 31 | name='VSSRef_Command', 32 | full_name='VSSRef.ref_to_team.VSSRef_Command', 33 | filename=None, 34 | file=DESCRIPTOR, 35 | containing_type=None, 36 | fields=[ 37 | _descriptor.FieldDescriptor( 38 | name='foul', full_name='VSSRef.ref_to_team.VSSRef_Command.foul', index=0, 39 | number=1, type=14, cpp_type=8, label=1, 40 | has_default_value=False, default_value=0, 41 | message_type=None, enum_type=None, containing_type=None, 42 | is_extension=False, extension_scope=None, 43 | serialized_options=None, file=DESCRIPTOR), 44 | _descriptor.FieldDescriptor( 45 | name='teamcolor', full_name='VSSRef.ref_to_team.VSSRef_Command.teamcolor', index=1, 46 | number=2, type=14, cpp_type=8, label=1, 47 | has_default_value=False, default_value=0, 48 | message_type=None, enum_type=None, containing_type=None, 49 | is_extension=False, extension_scope=None, 50 | serialized_options=None, file=DESCRIPTOR), 51 | _descriptor.FieldDescriptor( 52 | name='foulQuadrant', full_name='VSSRef.ref_to_team.VSSRef_Command.foulQuadrant', index=2, 53 | number=3, type=14, cpp_type=8, label=1, 54 | has_default_value=False, default_value=0, 55 | message_type=None, enum_type=None, containing_type=None, 56 | is_extension=False, extension_scope=None, 57 | serialized_options=None, file=DESCRIPTOR), 58 | _descriptor.FieldDescriptor( 59 | name='timestamp', full_name='VSSRef.ref_to_team.VSSRef_Command.timestamp', index=3, 60 | number=4, type=1, cpp_type=5, label=1, 61 | has_default_value=False, default_value=float(0), 62 | message_type=None, enum_type=None, containing_type=None, 63 | is_extension=False, extension_scope=None, 64 | serialized_options=None, file=DESCRIPTOR), 65 | _descriptor.FieldDescriptor( 66 | name='gameHalf', full_name='VSSRef.ref_to_team.VSSRef_Command.gameHalf', index=4, 67 | number=5, type=14, cpp_type=8, label=1, 68 | has_default_value=False, default_value=0, 69 | message_type=None, enum_type=None, containing_type=None, 70 | is_extension=False, extension_scope=None, 71 | serialized_options=None, file=DESCRIPTOR), 72 | ], 73 | extensions=[ 74 | ], 75 | nested_types=[], 76 | enum_types=[ 77 | ], 78 | serialized_options=None, 79 | is_extendable=False, 80 | syntax='proto3', 81 | extension_ranges=[], 82 | oneofs=[ 83 | ], 84 | serialized_start=66, 85 | serialized_end=235, 86 | ) 87 | 88 | _VSSREF_COMMAND.fields_by_name['foul'].enum_type = vssref__common__pb2._FOUL 89 | _VSSREF_COMMAND.fields_by_name['teamcolor'].enum_type = vssref__common__pb2._COLOR 90 | _VSSREF_COMMAND.fields_by_name['foulQuadrant'].enum_type = vssref__common__pb2._QUADRANT 91 | _VSSREF_COMMAND.fields_by_name['gameHalf'].enum_type = vssref__common__pb2._HALF 92 | DESCRIPTOR.message_types_by_name['VSSRef_Command'] = _VSSREF_COMMAND 93 | _sym_db.RegisterFileDescriptor(DESCRIPTOR) 94 | 95 | VSSRef_Command = _reflection.GeneratedProtocolMessageType('VSSRef_Command', (_message.Message,), dict( 96 | DESCRIPTOR = _VSSREF_COMMAND, 97 | __module__ = 'vssref_command_pb2' 98 | # @@protoc_insertion_point(class_scope:VSSRef.ref_to_team.VSSRef_Command) 99 | )) 100 | _sym_db.RegisterMessage(VSSRef_Command) 101 | 102 | 103 | # @@protoc_insertion_point(module_scope) 104 | -------------------------------------------------------------------------------- /foul_placements3v3.json: -------------------------------------------------------------------------------- 1 | { 2 | "5": { 3 | "KICKOFF": { 4 | "True": { 5 | "robot" : [0.05, 0.65], "guide": [0.05, 0] 6 | }, 7 | "False": { 8 | "robot" : [0.05, 0.65], "guide": [0.05, 0] 9 | } 10 | }, 11 | "FREE_BALL": { 12 | "QUADRANT_3": { 13 | "robot" : [0.05, 0.65], "guide": [0.05, 0] 14 | }, 15 | "QUADRANT_4": { 16 | "robot" : [0.05, 0.65], "guide": [0.05, 0] 17 | }, 18 | "QUADRANT_1": { 19 | "robot" : [0.05, 0.65], "guide": [0.05, 0] 20 | }, 21 | "QUADRANT_2": { 22 | "robot" : [0.05, 0.65], "guide": [0.05, 0] 23 | } 24 | }, 25 | "GOAL_KICK": { 26 | "False": { 27 | "robot" : [0.05, 0.65], "guide": [0.05, 0] 28 | }, 29 | "True": { 30 | "robot" : [0.2, 0.34], "guide": [0, 0.65] 31 | } 32 | }, 33 | "PENALTY_KICK": { 34 | "True": { 35 | "robot" : [0.05, 0.65], "guide": [0.05, 0] 36 | }, 37 | "False": { 38 | "robot" : [0.8625, 0.35], "guide": [0.9125, 0.35] 39 | } 40 | }, 41 | "FREE_KICK": { 42 | "True": { 43 | "robot" : [0.05, 0.65], "guide": [0.05, 0] 44 | }, 45 | "False": { 46 | "robot" : [0.05, 0.65], "guide": [0.05, 0] 47 | } 48 | } 49 | 50 | }, 51 | 52 | "7": { 53 | "KICKOFF": { 54 | "True": { 55 | "robot" : [0.69, 0.65], "guide": [0.85, 0.65] 56 | }, 57 | "False": { 58 | "robot" : [0.45, 0.65], "guide": [0.6, 0.65] 59 | } 60 | }, 61 | "FREE_BALL": { 62 | "QUADRANT_3": { 63 | "robot" : [0.135, 0.25], "guide": [0.375, 0.25] 64 | }, 65 | "QUADRANT_4": { 66 | "robot" : [0.885, 0.25], "guide": [1.125, 0.25] 67 | }, 68 | "QUADRANT_1": { 69 | "robot" : [0.885, 1.05], "guide": [1.125, 1.05] 70 | }, 71 | "QUADRANT_2": { 72 | "robot" : [0.135, 1.05], "guide": [0.375, 1.05] 73 | } 74 | }, 75 | "GOAL_KICK": { 76 | "False":{ 77 | "robot" : [0.35, 1.075], "guide": [0.40, 1.075] 78 | }, 79 | "True": { 80 | "robot" : [0.25, 1], "guide": [1.5, 0.65] 81 | } 82 | }, 83 | "PENALTY_KICK": { 84 | "True": { 85 | "robot" : [1.07, 0.65], "guide": [1.5, 0.65] 86 | }, 87 | "False": { 88 | "robot" : [0.03, 0.65], "guide": [0.5, 0.65] 89 | } 90 | }, 91 | "FREE_KICK": { 92 | "True": { 93 | "robot" : [0.975, 0.75], "guide": [1.025, 0.75] 94 | }, 95 | "False": { 96 | "robot" : [0.05, 0.65], "guide": [1, 0.65] 97 | } 98 | } 99 | }, 100 | "8": { 101 | "KICKOFF": { 102 | "True": { 103 | "robot" : [0.45, 0.65], "guide": [0.85, 0.65] 104 | }, 105 | "False": { 106 | "robot" : [0.35, 0.65], "guide": [0.6, 0.65] 107 | } 108 | }, 109 | "FREE_BALL": { 110 | "QUADRANT_3": { 111 | "robot" : [0.3, 0.8], "guide": [0.375, 0.25] 112 | }, 113 | "QUADRANT_4": { 114 | "robot" : [0.68, 0.65], "guide": [1.125, 0.25] 115 | }, 116 | "QUADRANT_1": { 117 | "robot" : [0.68, 0.65], "guide": [1.125, 1.05] 118 | }, 119 | "QUADRANT_2": { 120 | "robot" : [0.3, 0.5], "guide": [0.375, 1.05] 121 | } 122 | }, 123 | "GOAL_KICK": { 124 | "False":{ 125 | "robot" : [0.50, 0.65], "guide": [0.55, 0.275] 126 | }, 127 | "True": { 128 | "robot" : [0.07, 0.65], "guide": [1.5, 0.65] 129 | } 130 | }, 131 | "PENALTY_KICK": { 132 | "True": { 133 | "robot" : [0.5, 0.9], "guide": [1.5, 0.65] 134 | }, 135 | "False": { 136 | "robot" : [0.8, 1], "guide": [0.9, 1] 137 | } 138 | }, 139 | "FREE_KICK": { 140 | "True": { 141 | "robot" : [0.637, 1.15], "guide": [0.6837, 1.15] 142 | }, 143 | "False": { 144 | "robot" : [0.9125, 1.10], "guide": [0.8625, 1.10] 145 | } 146 | } 147 | } 148 | } -------------------------------------------------------------------------------- /protocols/command_pb2.py: -------------------------------------------------------------------------------- 1 | # Generated by the protocol buffer compiler. DO NOT EDIT! 2 | # source: command.proto 3 | 4 | import sys 5 | _b=sys.version_info[0]<3 and (lambda x:x) or (lambda x:x.encode('latin1')) 6 | from google.protobuf import descriptor as _descriptor 7 | from google.protobuf import message as _message 8 | from google.protobuf import reflection as _reflection 9 | from google.protobuf import symbol_database as _symbol_database 10 | # @@protoc_insertion_point(imports) 11 | 12 | _sym_db = _symbol_database.Default() 13 | 14 | 15 | 16 | DESCRIPTOR = _descriptor.FileDescriptor( 17 | name='command.proto', 18 | package='fira_message.sim_to_ref', 19 | syntax='proto3', 20 | serialized_options=None, 21 | serialized_pb=_b('\n\rcommand.proto\x12\x17\x66ira_message.sim_to_ref\"R\n\x07\x43ommand\x12\n\n\x02id\x18\x01 \x01(\r\x12\x12\n\nyellowteam\x18\x02 \x01(\x08\x12\x12\n\nwheel_left\x18\x06 \x01(\x01\x12\x13\n\x0bwheel_right\x18\x07 \x01(\x01\"D\n\x08\x43ommands\x12\x38\n\x0erobot_commands\x18\x01 \x03(\x0b\x32 .fira_message.sim_to_ref.Commandb\x06proto3') 22 | ) 23 | 24 | 25 | 26 | 27 | _COMMAND = _descriptor.Descriptor( 28 | name='Command', 29 | full_name='fira_message.sim_to_ref.Command', 30 | filename=None, 31 | file=DESCRIPTOR, 32 | containing_type=None, 33 | fields=[ 34 | _descriptor.FieldDescriptor( 35 | name='id', full_name='fira_message.sim_to_ref.Command.id', index=0, 36 | number=1, type=13, cpp_type=3, label=1, 37 | has_default_value=False, default_value=0, 38 | message_type=None, enum_type=None, containing_type=None, 39 | is_extension=False, extension_scope=None, 40 | serialized_options=None, file=DESCRIPTOR), 41 | _descriptor.FieldDescriptor( 42 | name='yellowteam', full_name='fira_message.sim_to_ref.Command.yellowteam', index=1, 43 | number=2, type=8, cpp_type=7, label=1, 44 | has_default_value=False, default_value=False, 45 | message_type=None, enum_type=None, containing_type=None, 46 | is_extension=False, extension_scope=None, 47 | serialized_options=None, file=DESCRIPTOR), 48 | _descriptor.FieldDescriptor( 49 | name='wheel_left', full_name='fira_message.sim_to_ref.Command.wheel_left', index=2, 50 | number=6, type=1, cpp_type=5, label=1, 51 | has_default_value=False, default_value=float(0), 52 | message_type=None, enum_type=None, containing_type=None, 53 | is_extension=False, extension_scope=None, 54 | serialized_options=None, file=DESCRIPTOR), 55 | _descriptor.FieldDescriptor( 56 | name='wheel_right', full_name='fira_message.sim_to_ref.Command.wheel_right', index=3, 57 | number=7, type=1, cpp_type=5, label=1, 58 | has_default_value=False, default_value=float(0), 59 | message_type=None, enum_type=None, containing_type=None, 60 | is_extension=False, extension_scope=None, 61 | serialized_options=None, file=DESCRIPTOR), 62 | ], 63 | extensions=[ 64 | ], 65 | nested_types=[], 66 | enum_types=[ 67 | ], 68 | serialized_options=None, 69 | is_extendable=False, 70 | syntax='proto3', 71 | extension_ranges=[], 72 | oneofs=[ 73 | ], 74 | serialized_start=42, 75 | serialized_end=124, 76 | ) 77 | 78 | 79 | _COMMANDS = _descriptor.Descriptor( 80 | name='Commands', 81 | full_name='fira_message.sim_to_ref.Commands', 82 | filename=None, 83 | file=DESCRIPTOR, 84 | containing_type=None, 85 | fields=[ 86 | _descriptor.FieldDescriptor( 87 | name='robot_commands', full_name='fira_message.sim_to_ref.Commands.robot_commands', index=0, 88 | number=1, type=11, cpp_type=10, label=3, 89 | has_default_value=False, default_value=[], 90 | message_type=None, enum_type=None, containing_type=None, 91 | is_extension=False, extension_scope=None, 92 | serialized_options=None, file=DESCRIPTOR), 93 | ], 94 | extensions=[ 95 | ], 96 | nested_types=[], 97 | enum_types=[ 98 | ], 99 | serialized_options=None, 100 | is_extendable=False, 101 | syntax='proto3', 102 | extension_ranges=[], 103 | oneofs=[ 104 | ], 105 | serialized_start=126, 106 | serialized_end=194, 107 | ) 108 | 109 | _COMMANDS.fields_by_name['robot_commands'].message_type = _COMMAND 110 | DESCRIPTOR.message_types_by_name['Command'] = _COMMAND 111 | DESCRIPTOR.message_types_by_name['Commands'] = _COMMANDS 112 | _sym_db.RegisterFileDescriptor(DESCRIPTOR) 113 | 114 | Command = _reflection.GeneratedProtocolMessageType('Command', (_message.Message,), dict( 115 | DESCRIPTOR = _COMMAND, 116 | __module__ = 'command_pb2' 117 | # @@protoc_insertion_point(class_scope:fira_message.sim_to_ref.Command) 118 | )) 119 | _sym_db.RegisterMessage(Command) 120 | 121 | Commands = _reflection.GeneratedProtocolMessageType('Commands', (_message.Message,), dict( 122 | DESCRIPTOR = _COMMANDS, 123 | __module__ = 'command_pb2' 124 | # @@protoc_insertion_point(class_scope:fira_message.sim_to_ref.Commands) 125 | )) 126 | _sym_db.RegisterMessage(Commands) 127 | 128 | 129 | # @@protoc_insertion_point(module_scope) 130 | -------------------------------------------------------------------------------- /controller/uni_controller.py: -------------------------------------------------------------------------------- 1 | import math 2 | import numpy as np 3 | from commons.math import speed_to_power 4 | 5 | """ 6 | Angle based controller 7 | reffering to soccer robotics 8 | """ 9 | 10 | 11 | class UniController(object): 12 | """ 13 | An implementation of the Uni controller specified on the soccer robotics article 14 | 15 | The UniController will receive a desired angle for the robot current position, the desired angle for the position in 16 | front of the robot and along with the current robot angle it will calculate the robot angular and linear speed. it 17 | can also be set through the control_speed parameter to slow down when near the target position. 18 | 19 | Attributes 20 | ---------- 21 | K_P : float 22 | the linear Kp value 23 | control_speed : bool 24 | whether the controller will use the P control for linear speed 25 | V_M : int 26 | the maximum linear speed 27 | K_W : float 28 | a positive constant 29 | R_M : int 30 | the maximum turn (v*w) 31 | target: tuple[float, float] 32 | the target position used for calculating the linear speed P 33 | """ 34 | 35 | CONSTANTS = { 36 | 'simulation': { 37 | 'V_M': 100, 38 | 'R_M': 3 * 100, # 3 * V_M 39 | 'K_W': 90, 40 | 'K_P': 10 41 | }, 42 | 'real_life': { 43 | 'V_M': 0.5, 44 | 'R_M': 0.44, # 20 * V_M 45 | 'K_W': 3.5, # 313, 46 | 'K_P': 1 47 | } 48 | } 49 | 50 | def __init__(self, robot, control_speed=False): 51 | self.robot = robot 52 | self.environment = robot.game.environment 53 | self.match = robot.game.match 54 | self.L = self.robot.dimensions.get("L") # m 55 | self.R = self.robot.dimensions.get("R") # m 56 | 57 | self.__dict__.update(self.CONSTANTS.get(self.environment)) 58 | 59 | self.control_speed = control_speed 60 | self.v1 = 0 # speed limit 1 61 | self.v2 = 0 # speed limit 2 62 | self.theta_d = 0 63 | self.theta_f = 0 64 | self.dl = 0.000001 # approximate phi_v in m 65 | self.phi_v = 0 66 | self.a_phi_v = 0 # absolute value of phi_v 67 | self.theta_e = 0 68 | self.a_theta_e = 0 # absolute value of theta_e 69 | 70 | self.parameters = self.match.parameters 71 | self.K_W = self.parameters['kw'].value 72 | self.R_M = self.parameters['rm'].value 73 | self.V_M = self.parameters['vm'].value 74 | self.K_P = self.parameters['uni_kp'].value 75 | 76 | self.target = [1.5, 0.65] 77 | 78 | def control(self): 79 | """ 80 | x, y, q: robot's posture (x position, t position, robot's angle) 81 | returns: v and w, linear and angular speed, respectively 82 | """ 83 | 84 | self.phi_v = self.theta_f - self.theta_d 85 | 86 | while self.phi_v > math.pi: 87 | self.phi_v -= 2 * math.pi 88 | while self.phi_v < -math.pi: 89 | self.phi_v += 2 * math.pi 90 | 91 | self.phi_v = self.phi_v / self.dl 92 | self.a_phi_v = abs(self.phi_v) 93 | 94 | # calculate theta_e 95 | self.theta_e = self.theta_d - self.robot.theta 96 | 97 | while self.theta_e > math.pi: 98 | self.theta_e -= 2 * math.pi 99 | while self.theta_e < -math.pi: 100 | self.theta_e += 2 * math.pi 101 | 102 | self.a_theta_e = abs(self.theta_e) 103 | 104 | # calculate v 105 | self.v1 = (2 * self.V_M - self.L * self.K_W * math.sqrt(self.a_theta_e)) / (2 + self.L * self.a_phi_v) 106 | 107 | self.v2 = (math.sqrt(self.a_theta_e * self.K_W ** 2 + 4 * self.R_M * self.a_phi_v) - self.K_W * math.sqrt( 108 | self.a_theta_e)) \ 109 | / (2 * self.a_phi_v) if self.a_phi_v > 0 else self.V_M 110 | 111 | self.v3 = self.K_P * ((self.robot.x - self.target[0]) ** 2 + (self.robot.y - self.target[1]) ** 2) ** .5 112 | 113 | if self.control_speed: 114 | v = min(self.v1, self.v2, self.v3) 115 | else: 116 | v = min(self.v1, self.v2) 117 | 118 | # calculate w 119 | if self.theta_e > 0: 120 | w = v * self.phi_v + self.K_W * math.sqrt(self.a_theta_e) 121 | else: 122 | w = v * self.phi_v - self.K_W * math.sqrt(self.a_theta_e) 123 | 124 | return v, w 125 | 126 | def set_desired(self, desired): 127 | """ 128 | Defines the target angles 129 | 130 | Parameters 131 | ---------- 132 | desired (tuple[float, float]): the desired angle in the current position and in the desired angle in 133 | front of the robot 134 | """ 135 | self.theta_d = desired[0] 136 | self.theta_f = desired[1] 137 | 138 | def update(self): 139 | v, w = self.control() 140 | 141 | if self.environment == 'simulation': 142 | return tuple(np.dot(250, speed_to_power(v, w, self.L, self.R))) 143 | 144 | # w = w if abs(w) < 4 else 4 * w / abs(w) 145 | return -v, -w -------------------------------------------------------------------------------- /controller/visualizers/run_visualizers.py: -------------------------------------------------------------------------------- 1 | from live_plot.visualizer import MPLVisualizer 2 | from algorithms import UnivectorField 3 | from collections import deque 4 | import math 5 | import numpy as np 6 | import matplotlib.pyplot as plt 7 | import time 8 | 9 | 10 | def main(): 11 | #uvf_visualizer = UVFVisualizer() 12 | #uvf_visualizer() 13 | 14 | # pid_visualizer = PIDVisualizer(500) 15 | # pid_visualizer() 16 | 17 | position_visualizer = PositionVisualizer(1000) 18 | position_visualizer(1) 19 | 20 | circuit = [(1.1, .40), (1.1, .90), (.75, .65), (.4, .90), (.4, .40)] 21 | x, y = list(map(list, zip(*circuit))) 22 | 23 | ax = plt.gca() 24 | 25 | # ax.add_patch(plt.Circle((.75, .65 + .08), .08, color="blue", fill=None)) 26 | # ax.add_patch(plt.Circle((.75, .65 - .08), .08, color="blue", fill=None)) 27 | 28 | plt.scatter(x, y, c='r') 29 | plt.show() 30 | 31 | class PositionVisualizer(MPLVisualizer): 32 | def __init__(self, n): 33 | self.lt = time.time() 34 | self.tables = ['robot'] 35 | super().__init__() 36 | 37 | self.xs = deque(maxlen=n) 38 | self.ys = deque(maxlen=n) 39 | 40 | def __update(self): 41 | t = time.time() 42 | print(1/(t-self.lt)) 43 | self.lt=t 44 | 45 | self.update() 46 | 47 | try: 48 | self.xs.append(self.values['robot']['x']) 49 | self.ys.append(self.values['robot']['y']) 50 | except TypeError: 51 | pass 52 | 53 | 54 | def _draw(self, frame, **kwargs): 55 | self.__update() 56 | 57 | plt.xlim([0, 1.5]) 58 | plt.ylim([0, 1.3]) 59 | 60 | return [self.ax.scatter(self.xs, self.ys, s=1, c='black')] 61 | 62 | 63 | 64 | class PIDVisualizer(MPLVisualizer): 65 | def __init__(self, n): 66 | self.tables = ['pid'] 67 | super().__init__() 68 | 69 | self.set_points = deque(maxlen=n) 70 | self.errors = deque(maxlen=n) 71 | self.times = deque(maxlen=n) 72 | self.inputs = deque(maxlen=n) 73 | self.ws = deque(maxlen=n) 74 | 75 | self.t0 = None 76 | self.__update(True) 77 | 78 | def __update(self, set_t0=False): 79 | self.update() 80 | 81 | if set_t0: 82 | self.t0 = self.values['pid']['time'] 83 | 84 | try: 85 | self.set_points.append(self.values['pid']['set_point']) 86 | self.errors.append(self.values['pid']['error']) 87 | self.inputs.append(self.values['pid']['set_point'] - self.values['pid']['error']) 88 | self.times.append(self.values['pid']['time'] - self.t0) 89 | self.ws.append(self.values['pid']['w']/50) 90 | except TypeError: 91 | pass 92 | 93 | print(self.times[-1], self.set_points[-1]) 94 | 95 | 96 | def _draw(self, frame, **kwargs): 97 | self.__update() 98 | 99 | plt.xlim([self.times[0], self.times[-1]]) 100 | 101 | return [self.ax.plot(self.times, self.set_points, 'r--')[0], 102 | self.ax.plot(self.times, self.inputs, 'black')[0]]#, 103 | #self.ax.plot(self.times, self.ws, 'g:')[0]] 104 | 105 | 106 | 107 | class UVFVisualizer(MPLVisualizer): 108 | def __init__(self): 109 | self.tables = ['ball', 'robot', 'uvf'] 110 | super().__init__() 111 | 112 | def _draw(self, frame, **kwargs): 113 | self.update() 114 | 115 | uvf = UnivectorField(self.values['uvf']['n'], 0) 116 | g = (self.values['uvf']['gx'], self.values['uvf']['gy']) 117 | r = (self.values['uvf']['rx'], self.values['uvf']['ry']) 118 | uvf.set_target(g, r) 119 | xs, ys = np.meshgrid(np.linspace(0, 1.5, 53), np.linspace(0, 1.3, 53)) 120 | us, vs = [], [] 121 | 122 | for x, y in zip(np.nditer(xs), np.nditer(ys)): 123 | ang = uvf((x, y)) 124 | 125 | u = math.cos(ang) 126 | v = math.sin(ang) 127 | 128 | us.append(u) 129 | vs.append(v) 130 | 131 | us = np.array(us).reshape(xs.shape) 132 | vs = np.array(vs).reshape(ys.shape) 133 | 134 | artists = [self.ax.quiver(xs, ys, us, vs, color='#A23BEC'), 135 | self.ax.add_patch(plt.Rectangle((0, 0), 1.5, 1.3, color='black', fill=False)), 136 | self.ax.add_patch(plt.Rectangle((1.350, 0.300), 0.150, 0.700, color='black', fill=False)), 137 | self.ax.add_patch(plt.Rectangle((0, 0.300), 0.150, 0.700, color='black', fill=False))] 138 | 139 | for obstacle in uvf.obstacles: 140 | artists.append( 141 | self.ax.add_patch(plt.Rectangle((obstacle.center[0] - 0.075 * 0.5, obstacle.center[1] - 0.075 * 0.5), 142 | 0.075, 143 | 0.075, 144 | color='blue', 145 | fill=True))) 146 | artists.append(self.ax.add_patch(plt.Circle(uvf.g, 21.5e-3, color="orange", fill=True))) 147 | artists.append(self.ax.add_patch(plt.Circle(uvf.r, 1.e-3, color="red", fill=True))) 148 | 149 | return artists 150 | 151 | 152 | if __name__ == "__main__": 153 | main() 154 | -------------------------------------------------------------------------------- /entities/coach/rsm2025_attack.py: -------------------------------------------------------------------------------- 1 | from commons.math import distance_between_points 2 | from entities.coach.coach import BaseCoach 3 | import strategy 4 | import json 5 | 6 | import strategy.rsm2025 7 | import strategy.rsm2025 8 | 9 | 10 | class Coach(BaseCoach): 11 | NAME = "RSM_2025_Attack" 12 | 13 | def __init__(self, match): 14 | super().__init__(match) 15 | self.SS_strategy = strategy.rsm2025.ShadowAttacker(self.match) 16 | self.ST_strategy = strategy.rsm2025.MainStriker(self.match) 17 | self.GK_strategy = strategy.rsm2025.Goalkeeper(self.match) 18 | 19 | self.GK_id = 5 # Goalkeeper fixed ID 20 | self.defending = False 21 | 22 | self.ball = self.match.ball 23 | positions = json.loads(open('foul_placements3v3.json', 'r').read()) 24 | self._position = {r.robot_id: strategy.commons.Replacer(self.match, positions[str(r.robot_id)]) for r in self.match.robots} 25 | 26 | # self.unstucks = {r.robot_id: strategy.rsm2023.Unstuck(self.match) for r in self.match.robots if r.robot_id != self.GK_id} 27 | 28 | self.GK = next(filter(lambda r: r.robot_id == self.GK_id, self.match.robots)) 29 | strikers = [r for i, r in enumerate(self.match.robots) if r.robot_id != self.GK_id] 30 | self.ST, self.SS = self.choose_main_striker(*strikers) 31 | started = False 32 | 33 | def start(self): 34 | self.set_strategy(self.GK, self.GK_strategy) 35 | self.set_strategy(self.ST, self.ST_strategy) 36 | self.set_strategy(self.SS, self.SS_strategy) 37 | self.started = True 38 | self.ball.x, self.ball.y = 1.5 , 1.3 39 | 40 | def decide(self): 41 | if not self.started: 42 | self.start() 43 | print(self.ST.robot_id, self.SS.robot_id) 44 | 45 | if self.match.match_event['event'] == 'PLAYING': 46 | cond_strikers = self.strikers_behind([self.ST, self.SS]) 47 | 48 | 49 | if self.ball.y < 0.3 and self.ball.x < 0.4 and cond_strikers == True: 50 | self.ST, self.GK, self.SS = self.choose_goalkeeper(self.GK, self.ST, self.SS) 51 | 52 | self.set_strategy(self.ST, self.ST_strategy) 53 | self.set_strategy(self.GK, self.GK_strategy) 54 | self.set_strategy(self.SS, self.SS_strategy) 55 | 56 | if self.ball.y > 1 and self.ball.x < 0.4 and cond_strikers == True: 57 | self.ST, self.GK, self.SS = self.choose_goalkeeper(self.GK, self.ST, self.SS) 58 | 59 | self.set_strategy(self.ST, self.ST_strategy) 60 | self.set_strategy(self.GK, self.GK_strategy) 61 | self.set_strategy(self.SS, self.SS_strategy) 62 | 63 | else: 64 | self.not_playing() 65 | 66 | def attack(self): 67 | strikers = [r for i, r in enumerate(self.match.robots) if r.robot_id != self.GK_id] 68 | self.ST, self.SS = self.choose_main_striker(*strikers) 69 | 70 | self.set_strategy(self.ST, self.ST_strategy) 71 | self.set_strategy(self.SS, self.SS_strategy) 72 | 73 | def not_playing(self): 74 | robots = [(i, r.robot_id) for i, r in enumerate(self.match.robots)] 75 | for robot, strategy in zip(robots, self._position): 76 | if self.match.robots[robot[0]].strategy == strategy: 77 | continue 78 | self.match.robots[robot[0]].strategy = strategy 79 | self.match.robots[robot[0]].start() 80 | 81 | def set_strategy(self, robot, strat): 82 | if robot != strat: 83 | robot.strategy = strat 84 | robot.start() 85 | 86 | def choose_main_striker(self, r1, r2): 87 | b = self.ball 88 | 89 | a1 = distance_between_points((b.x, b.y), (r1.x, r1.y)) 90 | a2 = distance_between_points((b.x, b.y), (r2.x, r2.y)) 91 | 92 | b1, b2 = b.x - r1.x - 0.05, b.x - r2.x - 0.05 93 | 94 | if b1 * b2 > 0: 95 | if a1 < a2: 96 | return r1, r2 97 | return r2, r1 98 | if b1 > 0: 99 | return r1, r2 100 | return r2, r1 101 | 102 | def choose_goalkeeper(self, gk, r1, r2): 103 | b = self.ball 104 | 105 | a1 = distance_between_points((b.x, b.y), (r1.x, r1.y)) 106 | a2 = distance_between_points((b.x, b.y), (r2.x, r2.y)) 107 | 108 | b1, b2 = b.x - r1.x - 0.05, b.x - r2.x - 0.05 109 | 110 | if b1 * b2 > 0: 111 | if a1 < a2: 112 | return gk, r1, r2 113 | return gk, r2, r1 114 | if b1 > 0: 115 | return gk, r1, r2 116 | return gk, r2, r1 117 | 118 | def strikers_behind(self, strikers): 119 | b = self.ball 120 | r1,r2 = strikers[0], strikers[1] 121 | 122 | b1, b2 = b.x - r1.x - 0.1, b.x - r2.x - 0.1 123 | 124 | if b1 < 0 and b2 < 0: 125 | return True 126 | return False 127 | 128 | 129 | 130 | def handle_stuck(self, ST, SS): 131 | game_runing = not (self.match.game_status == 'STOP' or self.match.game_status == None) 132 | stuck_st = ST.is_stuck() and game_runing 133 | stuck_ss = SS.is_stuck() and game_runing 134 | 135 | # if stuck_st and stuck_ss: 136 | # return self.unstucks[ST.robot_id], self.unstucks[SS.robot_id] 137 | # 138 | # if stuck_st and not stuck_ss: 139 | # return self.unstucks[ST.robot_id], self.ST_strategy 140 | # 141 | # if not stuck_st and stuck_ss: 142 | # return self.ST_strategy, self.unstucks[SS.robot_id] 143 | 144 | return self.ST_strategy, self.SS_strategy 145 | -------------------------------------------------------------------------------- /strategy/commons/Replacer.py: -------------------------------------------------------------------------------- 1 | from controller.PID_control import PID_control, PID_W_control 2 | from strategy.BaseStrategy import Strategy 3 | from strategy.utils.player_playbook import PlayerPlaybook, PlayerPlay, OnNextTo 4 | from NeonPathPlanning import LimitCycle 5 | from itertools import chain 6 | 7 | 8 | class Target: 9 | def __init__(self, target_dict): 10 | self.target_dict = target_dict 11 | self.guide_x = 0 12 | self.guide_y = 0 13 | self.target = [] 14 | 15 | def update(self, foul, quadrant, mine): 16 | if foul == 'KICKOFF': 17 | target = self.target_dict[foul][mine]['robot'] 18 | guide = self.target_dict[foul][mine]['guide'] 19 | elif foul == 'FREE_BALL': 20 | target = self.target_dict[foul][quadrant]['robot'] 21 | guide = self.target_dict[foul][quadrant]['guide'] 22 | elif foul == 'PENALTY_KICK': 23 | target = self.target_dict[foul][mine]['robot'] 24 | guide = self.target_dict[foul][mine]['guide'] 25 | elif foul == 'GOAL_KICK': 26 | target = self.target_dict[foul][mine]['robot'] 27 | guide = self.target_dict[foul][mine]['guide'] 28 | else: 29 | target = [0, 0] 30 | guide = [0, 0] 31 | 32 | self.target = target 33 | 34 | self.guide_x = guide[0] 35 | self.guide_y = guide[1] 36 | 37 | @property 38 | def x(self): 39 | return self.target[0] 40 | 41 | @property 42 | def y(self): 43 | return self.target[1] 44 | 45 | def __getitem__(self, item): 46 | return self.target[item] 47 | 48 | 49 | class Position(PlayerPlay): 50 | def __init__(self, match, robot, target): 51 | super().__init__(match, robot) 52 | self.target = target 53 | 54 | def get_name(self): 55 | return f"<{self.robot.get_name()} Position Planning>" 56 | 57 | def start_up(self): 58 | super().start_up() 59 | controller = PID_control 60 | controller_kwargs={'V_MIN': 0, 'V_MAX':0.1 , 'K_RHO': 6} 61 | self.robot.strategy.controller = controller(self.robot, **controller_kwargs) 62 | 63 | def update(self): 64 | return self.target.x, self.target.y 65 | 66 | def start(self): 67 | pass 68 | 69 | 70 | class Far(PlayerPlay): 71 | def __init__(self, match, robot, target): 72 | super().__init__(match, robot) 73 | self.target = target 74 | 75 | def get_name(self): 76 | return f"<{self.robot.get_name()} Far Planning>" 77 | 78 | def start_up(self): 79 | super().start_up() 80 | controller = PID_W_control 81 | controller_kwargs={'V_MIN': 0, 'V_MAX':0.1} 82 | self.robot.strategy.controller = controller(self.robot, **controller_kwargs) 83 | self.limit_cycle = LimitCycle() 84 | self.limit_cycle.add_obstacle(self.match.ball, 0.13) 85 | 86 | def update(self): 87 | self.limit_cycle.set_target(self.target) 88 | 89 | for robot in chain(self.match.robots, self.match.opposites): 90 | if robot == self.robot: 91 | continue 92 | if robot.is_visible(): 93 | self.limit_cycle.add_obstacle(robot, 0.11) 94 | else: 95 | try: 96 | self.limit_cycle.del_obstacle(robot) 97 | except KeyError: 98 | pass 99 | 100 | return self.limit_cycle.compute(self.robot) 101 | 102 | def start(self): 103 | pass 104 | 105 | 106 | class Angle(PlayerPlay): 107 | def __init__(self, match, robot, target): 108 | super().__init__(match, robot) 109 | self.target = target 110 | 111 | def get_name(self): 112 | return f"<{self.robot.get_name()} Angle Planning>" 113 | 114 | def start_up(self): 115 | super().start_up() 116 | controller = PID_control 117 | controller_kwargs = {'V_MIN': 0, 'V_MAX': 0, 'TWO_SIDES': False, 'K_P': -5} 118 | self.robot.strategy.controller = controller(self.robot, **controller_kwargs) 119 | 120 | def update(self): 121 | return self.target.guide_x, self.target.guide_y 122 | 123 | def start(self): 124 | pass 125 | 126 | 127 | class Replacer(Strategy): 128 | def __init__(self, match,target_dict, name="MainAttacker"): 129 | super().__init__(match, name, controller=PID_control) 130 | self.target = Target(target_dict) 131 | self.playerbook = None 132 | 133 | def start(self, robot=None): 134 | super().start(robot=robot) 135 | 136 | # Criando Player Playbook: A maquina de estados do jogador 137 | self.playerbook = PlayerPlaybook(self.match.coach, self.robot) 138 | 139 | # Criando Path Planning baseado em Astar 140 | pos = Position(self.match, self.robot, self.target) 141 | pos.start() 142 | angle = Angle(self.match, self.robot, self.target) 143 | angle.start() 144 | far = Far(self.match, self.robot, self.target) 145 | 146 | self.playerbook.add_play(pos) 147 | self.playerbook.add_play(angle) 148 | self.playerbook.add_play(far) 149 | 150 | on_point = OnNextTo(self.target, self.robot, 0.02) 151 | off_point = OnNextTo(self.target, self.robot, 0.02, True) 152 | on_near = OnNextTo(self.target, self.robot, 0.05) 153 | 154 | pos.add_transition(on_point, angle) 155 | angle.add_transition(off_point, pos) 156 | far.add_transition(on_near, pos) 157 | 158 | # Estado inicial é o astar 159 | self.playerbook.set_play(far) 160 | 161 | def decide(self): 162 | self.target.update(self.match.match_event['event'], 163 | self.match.match_event['quadrant'], 164 | str(self.match.match_event['mine'])) 165 | res = self.playerbook.update() 166 | return res 167 | -------------------------------------------------------------------------------- /strategy/tests/PID_tuner.py: -------------------------------------------------------------------------------- 1 | from collections import deque 2 | from controller.PID_control import PID_W_control 3 | from strategy.BaseStrategy import Strategy 4 | from api import Api, Api_recv 5 | import time 6 | import math 7 | 8 | 9 | class PIDTuner(Strategy): 10 | def __init__(self, match): 11 | super().__init__(match, "PID_Tune", controller=PID_W_control, 12 | controller_kwargs={'V_MAX': 0, 'V_MIN': 50, 'KP': -3, 'KI': 0, 'KD': 0}) 13 | 14 | self.sender = Api("127.0.0.1", 43210) 15 | self.sender.start() 16 | self.reciver = Api_recv(match, "127.0.0.1", 43221) 17 | self.reciver.start() 18 | self.circuit = [] 19 | self.t0 = time.time() 20 | self.state = "countdown" 21 | self.still = True 22 | print("on init") 23 | 24 | def start(self, robot=None): 25 | super().start(robot=robot) 26 | self.y = self.robot.y 27 | 28 | def reset(self, robot=None): 29 | super().reset() 30 | if robot: 31 | self.start(robot) 32 | 33 | def next_point(self): 34 | point = self.circuit[0] 35 | dx = point[0] - self.robot.x 36 | dy = point[1] - self.robot.y 37 | 38 | if math.sqrt(dx**2 + dy**2) < 0.05: 39 | self.circuit.rotate(-1) 40 | print("Change point! ", self.circuit[0]) 41 | 42 | return self.circuit[0] 43 | 44 | def make_hexagonal_path(self): 45 | if .15 <= self.robot.x <= .3 and .575 <= self.robot.y <= .725: 46 | 47 | # set points on the hexagonal path that the robot will perform 48 | A = (self.robot.x, self.robot.y) 49 | B = (self.robot.x + .425, self.robot.y + .425) 50 | C = (self.robot.x + .625, self.robot.y + .425) 51 | D = (self.robot.x + 1.05, self.robot.y) 52 | E = (self.robot.x + .625, self.robot.y - .425) 53 | F = (self.robot.x + .425, self.robot.y - .425) 54 | 55 | # set the circuit 56 | self.circuit = [B, C, D, E, F, A] 57 | 58 | def make_line_path(self): 59 | size = .5 60 | 61 | start = self.robot.x, self.robot.y 62 | mid = self.robot.x + size*math.cos(self.robot.theta), self.robot.y + size*math.sin(self.robot.theta) 63 | 64 | if .15 < mid[0] < 1.35 and .15 < mid[1] < 1.15: 65 | self.circuit = [mid, start] 66 | else: 67 | mid = self.robot.x + size * math.cos(self.robot.theta - math.pi), self.robot.y + size * math.sin(self.robot.theta - math.pi) 68 | if .15 < mid[0] < 1.35 and .15 < mid[1] < 1.15: 69 | self.circuit = [mid, start] 70 | 71 | def make_inplace(self): 72 | self.controller.V_MAX = 0 73 | self.controller.V_MIN = 0 74 | self.still = True 75 | 76 | self.circuit = [(self.robot.x + 1, self.robot.y), (self.robot.x, self.robot.y + 1)] 77 | 78 | def decide(self): 79 | target = self.robot 80 | data = {'pid': { 81 | 'set_point': 0, 82 | 'error': abs(self.controller.error), 83 | 'time': time.time(), 84 | 'w': self.robot.vtheta, 85 | 'running': self.state == "running"}, 86 | 'pos':{ 87 | 'x': self.robot.x, 88 | 'y': self.robot.y, 89 | 'theta': self.robot.theta 90 | } 91 | } 92 | 93 | self.sender.send_custom_data(data) 94 | 95 | data = self.reciver.decod_data 96 | 97 | if data: 98 | if self.controller.KP != data['kp']: 99 | print(self.controller.KP, data['kp']) 100 | self.controller.KP = data['kp'] 101 | self.state = "position" 102 | if self.controller.KI != data['ki']: 103 | print(self.controller.KI, data['ki']) 104 | self.controller.KI = data['ki'] 105 | self.state = "position" 106 | if self.controller.KD != data['kd']: 107 | print(self.controller.KI, data['ki']) 108 | self.controller.KD = data['kd'] 109 | self.state = "position" 110 | 111 | if self.state == "position": 112 | self.controller.alpha_old = 0 113 | self.controller.int_alpha = 0 114 | 115 | self.make_inplace() 116 | print("position") 117 | if len(self.circuit) > 0: 118 | print(self.circuit) 119 | self.state = "countdown" 120 | self.t0 = time.time() 121 | 122 | if self.state == "countdown": 123 | if time.time() - self.t0 <= 0: # 3s 124 | print("countdown") 125 | target = self.robot 126 | else: 127 | self.make_inplace() 128 | self.state = "running" 129 | self.t0 = time.time() 130 | 131 | if self.state == "running": 132 | print(self.circuit) 133 | if not self.circuit: 134 | self.state = "wait" 135 | 136 | elif self.still: 137 | target = self.circuit[0] 138 | 139 | if time.time() - self.t0 > 1: 140 | print("reached") 141 | self.t0 = time.time() 142 | self.circuit.pop(0) 143 | else: 144 | target = self.circuit[0] 145 | 146 | dx = self.circuit[0][0] - self.robot.x 147 | dy = self.circuit[0][1] - self.robot.y 148 | 149 | if math.sqrt(dx ** 2 + dy ** 2) < 0.05: 150 | print("reached") 151 | self.circuit.pop(0) 152 | 153 | self.controller.V_MAX = 50 154 | 155 | if self.state == "wait": 156 | target = self.robot 157 | self.controller.V_MAX = 0 158 | 159 | return target 160 | -------------------------------------------------------------------------------- /strategy/rsm2025/SecondAttacker.py: -------------------------------------------------------------------------------- 1 | from NeonPathPlanning import UnivectorField, Point 2 | import math 3 | from controller.uni_controller import UniController 4 | from strategy.BaseStrategy import Strategy 5 | from strategy.utils.player_playbook import PlayerPlaybook, PlayerPlay, OnInsideBox, OnNextTo, AndTransition 6 | from commons.math import distance_between_points 7 | from controller.PID_control import PID_control, PID_W_control 8 | 9 | 10 | class MainPlay(PlayerPlay): 11 | def __init__(self, match, robot): 12 | super().__init__(match, robot) 13 | self.dl = 0.000001 14 | 15 | def get_name(self): 16 | return f"<{self.robot.get_name()} Shadow Position Planning>" 17 | 18 | def start_up(self): 19 | super().start_up() 20 | controller = UniController 21 | controller_kwargs = {'control_speed': True} 22 | self.robot.strategy.controller = controller(self.robot, **controller_kwargs) 23 | self.univector = UnivectorField(n=.3, rect_size=.1) 24 | 25 | def update(self): 26 | ball = self.match.ball 27 | robot = self.robot 28 | main_st = next(filter(lambda r:r.strategy.name == "Main_Attacker", self.match.robots)) 29 | obs_radius = distance_between_points(main_st, ball) 30 | gk = next(filter(lambda r:r.strategy.name == "Goalkeeper", self.match.robots)) 31 | 32 | # for r in self.match.robots: 33 | # if r != robot: 34 | # self.univector.add_obstacle(r, obs_radius) 35 | 36 | 37 | # second attacker offset on x based on the distance of the main attacker to the ball 38 | # second attacker offset on y based on the distance of the ball to the center 39 | target = Point(max(main_st.x*.8, 0.2), main_st.y + .7*(.65-ball.y)) 40 | 41 | self.univector.set_target(target, ball) 42 | self.univector.add_obstacle(main_st, obs_radius) 43 | self.univector.add_obstacle(gk, 0.075*1.4 + 0.1) 44 | 45 | 46 | theta_d = self.univector.compute(robot) 47 | theta_f = self.univector.compute(Point( 48 | robot.x + self.dl * math.cos(robot.theta), 49 | robot.y + self.dl * math.sin(robot.theta) 50 | )) 51 | 52 | self.robot.strategy.controller.target = target 53 | 54 | return theta_d, theta_f 55 | 56 | 57 | class Wait(PlayerPlay): 58 | def __init__(self, match, robot): 59 | super().__init__(match, robot) 60 | 61 | def get_name(self): 62 | return f"<{self.robot.get_name()} Position Planning>" 63 | 64 | def start_up(self): 65 | super().start_up() 66 | controller = PID_control 67 | controller_kwargs={'V_MIN': 0, 'K_RHO': 1.5} 68 | self.robot.strategy.controller = controller(self.robot, **controller_kwargs) 69 | 70 | def update(self): 71 | return self.position() 72 | 73 | def position(self): 74 | a = (.35, 1.1) 75 | b = (.35, 0.2) 76 | 77 | c = (self.robot.x, self.robot.y) 78 | d = [(r.x, r.y) for r in self.match.robots if r.strategy.name == "Main_Attacker"][0] 79 | 80 | # Calculate the distances between each robot and each fixed point 81 | distance_c_a = math.sqrt((c[0] - a[0]) ** 2 + (c[1] - a[1]) ** 2) 82 | distance_c_b = math.sqrt((c[0] - b[0]) ** 2 + (c[1] - b[1]) ** 2) 83 | distance_d_a = math.sqrt((d[0] - a[0]) ** 2 + (d[1] - a[1]) ** 2) 84 | distance_d_b = math.sqrt((d[0] - b[0]) ** 2 + (d[1] - b[1]) ** 2) 85 | 86 | # Assign the robots to the closer fixed point 87 | if distance_c_a + distance_d_b < distance_c_b + distance_d_a: 88 | return a 89 | else: 90 | return b 91 | 92 | 93 | class LookAtBall(PlayerPlay): 94 | def __init__(self, match, robot): 95 | super().__init__(match, robot) 96 | 97 | def get_name(self): 98 | return f"<{self.robot.get_name()} Looking at the ball>" 99 | 100 | def start_up(self): 101 | super().start_up() 102 | controller = PID_W_control 103 | controller_kwargs = {'V_MIN': 0, 'V_MAX': 0} 104 | self.robot.strategy.controller = controller(self.robot, **controller_kwargs) 105 | 106 | def update(self): 107 | return self.match.ball.x, self.match.ball.y 108 | 109 | class ShadowAttacker(Strategy): 110 | def __init__(self, match, name="Shadow_Attacker"): 111 | super().__init__(match, name, controller=UniController, controller_kwargs={'control_speed': True}) 112 | 113 | def start(self, robot=None): 114 | super().start(robot=robot) 115 | 116 | # Criando Player Playbook: A maquina de estados do jogador 117 | self.playerbook = PlayerPlaybook(self.match.coach, self.robot) 118 | 119 | main = MainPlay(self.match, self.robot) 120 | defensive = Wait(self.match, self.robot) 121 | angle = LookAtBall(self.match, self.robot) 122 | 123 | self.playerbook.add_play(main) 124 | self.playerbook.add_play(defensive) 125 | self.playerbook.add_play(angle) 126 | 127 | on_defensive_box = OnInsideBox(self.match, [-.2, .3, .35, .7], False) 128 | off_defensive_box = OnInsideBox(self.match, [-.2, .3, .35, .7], True) 129 | on_position_1 = OnNextTo([.35, 1.1], self.robot, 0.1, False) 130 | off_position_1 = OnNextTo([.35, 1.1], self.robot, 0.1, True) 131 | on_position_2 = OnNextTo([.35, .2], self.robot, 0.1, False) 132 | off_position_2 = OnNextTo([.35, .2], self.robot, 0.1, True) 133 | 134 | main.add_transition(on_defensive_box, defensive) 135 | defensive.add_transition(off_defensive_box, main) 136 | 137 | defensive.add_transition(on_position_1, angle) 138 | defensive.add_transition(on_position_2, angle) 139 | angle.add_transition(AndTransition([off_position_1, off_position_2]), defensive) 140 | angle.add_transition(off_defensive_box, main) 141 | 142 | # Estado inicial 143 | self.playerbook.set_play(main) 144 | 145 | def decide(self): 146 | res = self.playerbook.update() 147 | return res 148 | -------------------------------------------------------------------------------- /comm/comm.py: -------------------------------------------------------------------------------- 1 | import os 2 | import json 3 | import struct 4 | import socket 5 | import threading 6 | 7 | from commons.utils import get_config 8 | 9 | from google.protobuf.json_format import MessageToJson 10 | from protocols import command_pb2, common_pb2, packet_pb2, vssref_command_pb2, replacement_pb2 11 | 12 | class FiraComm(object): 13 | def __init__(self): 14 | super(FiraComm, self).__init__() 15 | self.config = get_config() 16 | 17 | self.commands = [] 18 | 19 | self.command_port = int(os.environ.get('COMMAND_PORT', self.config['network']['command_port'])) 20 | self.host = os.environ.get('HOST_IP', self.config['network']['host_ip']) 21 | 22 | def start(self): 23 | print("Starting communication...") 24 | self.command_sock = self._create_socket() 25 | print("Communication socket created!") 26 | 27 | def send(self, robot_commands = []): 28 | ''' 29 | Send commands to FIRASim 30 | 31 | robot_commands follows: 32 | [ 33 | { 34 | robot_id: NUM, 35 | color: 'yellow|blue', 36 | wheel_left: float, 37 | wheel_right: float, 38 | } 39 | ] 40 | ''' 41 | commands = command_pb2.Commands() 42 | 43 | for robot in robot_commands: 44 | command = commands.robot_commands.add() 45 | command.yellowteam = self._get_robot_color(robot) 46 | command.wheel_right = robot['wheel_right'] 47 | command.wheel_left = robot['wheel_left'] 48 | command.id = robot['robot_id'] 49 | 50 | packet = packet_pb2.Packet() 51 | packet.cmd.CopyFrom(commands) 52 | 53 | self.command_sock.sendto( 54 | packet.SerializeToString(), 55 | (self.host, self.command_port) 56 | ) 57 | 58 | def _get_robot_color(self, robot): 59 | return True if robot['color'] == 'yellow' else False 60 | 61 | def _create_socket(self): 62 | return socket.socket(socket.AF_INET, socket.SOCK_DGRAM) 63 | 64 | class FiraFullComm(FiraComm): 65 | def __init__(self): 66 | super().__init__() 67 | self.replacement_port = int(os.environ.get('REPLACER_PORT', self.config['network']['replacer_port'])) 68 | 69 | 70 | def start(self): 71 | super().start() 72 | print("Starting replacer communication...") 73 | self.replacer_sock = self._create_socket() 74 | print("Replacer Communication socket created!") 75 | 76 | def replace(self, robots=None, ball=None): 77 | """ 78 | replace comm: 79 | [ 80 | {'robot_id': X, 'color': '', 'x': 0, 'y': 0, 'theta'}, 81 | ... 82 | ] 83 | ball comm: 84 | {'x', 'y', 'theta'} 85 | """ 86 | replacements = replacement_pb2.Replacement() 87 | 88 | for robot in robots: 89 | replacement = replacements.robots.add() 90 | 91 | replacement.yellowteam = self._get_robot_color(robot) 92 | replacement.turnon = True 93 | 94 | replacement.position.x = robot['x'] 95 | replacement.position.y = robot['y'] 96 | replacement.position.orientation = robot['theta'] 97 | replacement.position.robot_id = robot['robot_id'] 98 | 99 | if ball: 100 | replacements.ball.x = ball['x'] 101 | replacements.ball.y = ball['y'] 102 | 103 | 104 | packet = packet_pb2.Packet() 105 | packet.replace.CopyFrom(replacements) 106 | 107 | self.replacer_sock.sendto( 108 | packet.SerializeToString(), 109 | (self.host, self.command_port) 110 | ) 111 | 112 | 113 | class RefereeComm(threading.Thread): 114 | def __init__(self): 115 | super(RefereeComm, self).__init__() 116 | self.config = get_config() 117 | self.commands = [] 118 | 119 | self.status = None 120 | 121 | self.referee_port = int(os.environ.get('REFEREE_PORT', self.config['network']['referee_port'])) 122 | self.host = os.environ.get('REFEREE_IP', self.config['network']['referee_ip']) 123 | 124 | self.can_play = False 125 | 126 | def run(self): 127 | print("Starting referee...") 128 | self.referee_sock = self._create_socket() 129 | print("Referee completed!") 130 | while True: 131 | c = vssref_command_pb2.VSSRef_Command() 132 | data = self.referee_sock.recv(1024) 133 | c.ParseFromString(data) 134 | self.status = json.loads(MessageToJson(c)) 135 | 136 | self.can_play = self.status.get('foul') == 'GAME_ON' 137 | 138 | def _create_socket(self): 139 | sock = socket.socket( 140 | socket.AF_INET, 141 | socket.SOCK_DGRAM, 142 | socket.IPPROTO_UDP 143 | ) 144 | 145 | sock.setsockopt( 146 | socket.SOL_SOCKET, 147 | socket.SO_REUSEADDR, 1 148 | ) 149 | 150 | sock.bind((self.host, self.referee_port)) 151 | 152 | mreq = struct.pack( 153 | "4sl", 154 | socket.inet_aton(self.host), 155 | socket.INADDR_ANY 156 | ) 157 | 158 | sock.setsockopt( 159 | socket.IPPROTO_IP, 160 | socket.IP_ADD_MEMBERSHIP, 161 | mreq 162 | ) 163 | 164 | return sock 165 | 166 | 167 | if __name__ == "__main__": 168 | import time 169 | c = FiraComm() 170 | 171 | c.start() 172 | 173 | while True: 174 | time.sleep(1) 175 | c.send( 176 | [ 177 | { 178 | 'robot_id': 0, 179 | 'wheel_left': 20, 180 | 'wheel_right': -20, 181 | 'color': 'blue' 182 | } 183 | ] 184 | ) -------------------------------------------------------------------------------- /strategy/rsm2025/GoalkeeperArch.py: -------------------------------------------------------------------------------- 1 | import math 2 | 3 | from entities.plays.playbook import Trigger 4 | from strategy.BaseStrategy import Strategy 5 | from strategy.utils.player_playbook import PlayerPlay, PlayerPlaybook, OnInsideBox, OnNextTo, CheckAngle, AndTransition, RobotOnInsideBox, NotTransition, OnStuckTrigger, RobotLookBall 6 | from controller import PID_control, PID_W_control, UniController, NoController 7 | from NeonPathPlanning import UnivectorField, Point 8 | 9 | import time 10 | 11 | class IsInLine(Trigger): 12 | """ 13 | Verifica se o robô está arbitrariamente perto de uma linha 14 | reta entre a bola e alguma determinada coordenada (que em tese é pra ser o centro do gol) 15 | A ideia é que caso o robô consiga se inserir entre o gol (nosso) e a bola, ele vai atacar 16 | pra cima da bola. 17 | """ 18 | 19 | def calc_dist(self): 20 | # equação distância ponto-hiperplano 21 | # d=|w.p + b| / ||w|| 22 | b = self.point 23 | #TODO 24 | pass 25 | 26 | def __init__(self, robot, ball, point, dist): 27 | super().__init__() 28 | self.robot = robot 29 | self.ball = ball 30 | self.point = point 31 | self.dist = dist 32 | 33 | def evaluate(self, *args, **kwargs): 34 | if (self.robot.theta > 0.9 and self.robot.theta < 2.3 ) and self.ball.y > .65: 35 | return True 36 | if (self.robot.theta > -2.3 and self.robot.theta < -0.9) and self.ball.y <= .65: 37 | return True 38 | return False 39 | 40 | class DefendPlay(PlayerPlay): 41 | radius = None 42 | dist = None 43 | def __init__(self,match,robot,dist,radius): 44 | super().__init__(match,robot) 45 | 46 | # dist é a coordenada x do círculo de defesa 47 | # radius é o raio do círculo 48 | 49 | self.dist = dist 50 | self.radius = radius 51 | self.ball = self.match.ball 52 | self.goal_left = .3 53 | self.goal_right = 1.1 54 | 55 | def update(self): 56 | """Retorna o ponto mais próximo da bola 57 | no círculo com centro em (dist,0) de raio radius""" 58 | ball = self.ball 59 | dist = self.dist 60 | 61 | cx = ball.x - dist; cy = ball.y - .75 # vetor que aponta do centro até a bola 62 | 63 | mag = (cx ** 2 + cy ** 2) ** .5 64 | 65 | cx /= mag; cy /= mag # normaliza o vetor c 66 | 67 | return cx * dist, cy * dist # TODO minimiza o raio caso a bola esteja dentro da área 68 | 69 | # eu quero testar se isso ^^^ dá certo, senão só descomenta tudo isso aqui embaixo 70 | 71 | # ball = self.ball 72 | # 73 | # if ball.y > 1.1: 74 | # return .1, 1.1 75 | # 76 | # if ball.y < .3: 77 | # return .1, .3 78 | # 79 | # 80 | # projection_rate = 0.3 #(ball.x - .15) / (1 - .15) 81 | # projection_point = ball.y + projection_rate * ball.vy 82 | # 83 | # y = min(max(projection_point, self.goal_right), self.goal_left) 84 | # 85 | # x = (0.0256 - y**2) **.2 86 | # 87 | # self.x = x, self.y = y 88 | # 89 | # return x, y 90 | 91 | # olha se isso é o controller certo 92 | def start_up(self): 93 | super().start_up() 94 | controller = PID_control 95 | controller_kwargs = { 96 | 'K_RHO': 1, 97 | 'V_MIN': 0, 98 | } 99 | self.robot.strategy.controller = controller(self.robot, **controller_kwargs) 100 | 101 | class FollowBallPlay(PlayerPlay): 102 | def __init__(self, match, robot): 103 | super().__init__(match, robot) 104 | 105 | def start_up(self): 106 | super().start_up() 107 | controller = PID_control 108 | controller_kwargs = { 109 | 'K_RHO': 1, 110 | 'V_MIN': 0, 111 | } 112 | self.robot.strategy.controller = controller(self.robot, **controller_kwargs) 113 | 114 | def update(self): 115 | ball = self.match.ball 116 | 117 | if ball.y > self.robot.y: 118 | return .05, ball.y 119 | else: 120 | return .05, ball.y 121 | 122 | class InterceptPlay(PlayerPlay): 123 | """Tenta colocar o robô entre a bola e o centro do gol""" 124 | def __init__(self, match, robot): 125 | super().__init__(match, robot) 126 | 127 | def start_up(self): 128 | super().start_up() 129 | controller = PID_control 130 | controller_kwargs = { 131 | 'K_RHO': 1, 132 | 'V_MIN': 0, 133 | } 134 | self.robot.strategy.controller = controller(self.robot, **controller_kwargs) 135 | 136 | def update(self): 137 | ball = self.match.ball 138 | goal = [0,0.65] 139 | 140 | v = [goal[0] - ball.x, goal[1] - ball.y] 141 | # mag = (v[0]**2+v[1]**2)**.5 142 | # v[0]/= mag; v[1]/= mag 143 | v /= 2 144 | return v[0], v[1] 145 | 146 | class Goalkeeper(Strategy): 147 | def __init__(self, match): 148 | super().__init__(match, "Goalkeeper", controller=PID_control) 149 | self.old_play = None 150 | 151 | def start(self, robot=None): 152 | super().start(robot=robot) 153 | 154 | self.playerbook = PlayerPlaybook(self.match.coach, self.robot) 155 | 156 | defend_play = DefendPlay(self.match, self.robot,.2,.1) # TODO muda os parametros aqui 157 | follow_ball = FollowBallPlay(self.match, self.robot) 158 | 159 | self.playerbook.add_play(follow_ball) 160 | 161 | if self.playerbook.actual_play is None: 162 | self.playerbook.set_play(follow_ball) 163 | 164 | def reset(self, robot=None): 165 | super().reset() 166 | if robot: 167 | self.start(robot) 168 | 169 | def get_play(self): 170 | return self.playerbook.actual_play 171 | 172 | def decide(self): 173 | res = self.playerbook.update() 174 | # if self.old_play != self.playerbook.actual_play: 175 | # print(self.playerbook.actual_play) 176 | print('GK_id:', self.robot.robot_id) 177 | # self.old_play = self.playerbook.actual_play 178 | return res 179 | -------------------------------------------------------------------------------- /algorithms/dwa/dynamicWindowApproach.py: -------------------------------------------------------------------------------- 1 | import os, math, time, random, copy 2 | 3 | ROBOT_WIDTH = 0.08 4 | ROBOT_RADIUS = 0.04 5 | SAFE_DIST = 0.0375 6 | BARRIER_RADIUS = 0.08 7 | MAX_VELOCITY = 50.0 8 | ACCELERATION_MULTIPLIER = 3 9 | MAX_ACCELERATION = 10.0 * ACCELERATION_MULTIPLIER 10 | STEPS_AHEAD_TO_PLAN = 2 11 | 12 | class DynamicWindowApproach: 13 | 14 | def __init__(self, robot, game): 15 | self.robot = robot 16 | self.game = game 17 | self.vL = 0.00 18 | self.vR = 0.00 19 | 20 | def get_obstacles(self): 21 | return self.game.match.opposites 22 | 23 | def predict_position(self, vL, vR, x, y, theta, deltat): 24 | # Simple special cases 25 | # Straight line motion 26 | if (round (vL,3) == round(vR,3)): 27 | xnew = x + vL * deltat * math.cos(theta) 28 | ynew = y + vL * deltat * math.sin(theta) 29 | thetanew = theta 30 | # Pure rotation motion 31 | elif (round(vL,3) == -round(vR,3)): 32 | xnew = x 33 | ynew = y 34 | thetanew = theta + ((vR - vL) * deltat / ROBOT_WIDTH) 35 | else: 36 | # Rotation and arc angle of general circular motion 37 | # Using equations given in Lecture 2 38 | R = ROBOT_WIDTH / 2.0 * (vR + vL) / (vR - vL) 39 | deltatheta = (vR - vL) * deltat / ROBOT_WIDTH 40 | xnew = x + R * (math.sin(deltatheta + theta) - math.sin(theta)) 41 | ynew = y - R * (math.cos(deltatheta + theta) - math.cos(theta)) 42 | thetanew = theta + deltatheta 43 | 44 | return (xnew, ynew, thetanew) 45 | 46 | def calculate_closest_obstacles_distance(self, x, y): 47 | closestdist = 100000.0 48 | # Calculate distance to closest obstacle 49 | opposites = self.get_obstacles() 50 | for (i,opposites) in enumerate(opposites): 51 | dx = (opposites.x + ROBOT_RADIUS) - (x + ROBOT_RADIUS) 52 | dy = (opposites.y + ROBOT_RADIUS) - (y + ROBOT_RADIUS) 53 | dist = math.sqrt(dx**2 + dy**2) 54 | if (dist < closestdist): 55 | closestdist = dist 56 | return closestdist 57 | 58 | def get_target(self): 59 | return self.game.match.ball 60 | 61 | def set_desired(self, desired): 62 | pass 63 | 64 | def update(self): 65 | return self.get_best_path() 66 | 67 | def decide(self): 68 | return [0, 0] 69 | 70 | def start(self, robot): 71 | pass 72 | 73 | def get_best_path(self): 74 | 75 | if (self.game.vision._fps > 0): 76 | dt = 1.0 / self.game.vision._fps 77 | else: 78 | dt = 1.0/60 79 | 80 | BEST_BENEFIT = -100000 81 | FORWARD_WEIGHT = 12 82 | OBSTACLE_WEIGHT = 6666 83 | TAU = dt * STEPS_AHEAD_TO_PLAN 84 | 85 | vLpossiblearray = ( 86 | self.vL - MAX_ACCELERATION * dt, 87 | -self.vL - MAX_ACCELERATION * dt, 88 | self.vL, 89 | -self.vL, 90 | self.vL + MAX_ACCELERATION * dt, 91 | -self.vL + MAX_ACCELERATION * dt, 92 | self.vL - (MAX_ACCELERATION / (2 * ACCELERATION_MULTIPLIER)) * dt, 93 | -self.vL - (MAX_ACCELERATION / 2) * dt, 94 | self.vL + (MAX_ACCELERATION / (2 * ACCELERATION_MULTIPLIER)) * dt, 95 | -self.vL + (MAX_ACCELERATION / 2) * dt, 96 | ) 97 | vRpossiblearray = ( 98 | self.vR - MAX_ACCELERATION * dt, 99 | -self.vR - MAX_ACCELERATION * dt, 100 | self.vR, 101 | -self.vR, 102 | self.vR + MAX_ACCELERATION * dt, 103 | -self.vR + MAX_ACCELERATION * dt, 104 | self.vR - (MAX_ACCELERATION / (2 * ACCELERATION_MULTIPLIER)) * dt, 105 | -self.vR - (MAX_ACCELERATION / 2) * dt, 106 | self.vR + (MAX_ACCELERATION / (2 * ACCELERATION_MULTIPLIER)) * dt, 107 | -self.vR + (MAX_ACCELERATION / 2) * dt, 108 | ) 109 | 110 | for vLpossible in vLpossiblearray: 111 | for vRpossible in vRpossiblearray: 112 | # We can only choose an action if it's within velocity limits 113 | if (vLpossible <= MAX_VELOCITY and vRpossible <= MAX_VELOCITY and vLpossible >= -MAX_VELOCITY and vRpossible >= -MAX_VELOCITY): 114 | # Predict new position in TAU seconds 115 | (xpredict, ypredict, thetapredict) = self.predict_position(vLpossible, vRpossible, self.robot.x, self.robot.y, self.robot.theta, TAU) 116 | # What is the distance to the closest obstacle from this possible position? 117 | distanceToObstacle = self.calculate_closest_obstacles_distance(xpredict, ypredict) 118 | # Calculate how much close we've moved to target location 119 | previousTargetDistance = math.sqrt((self.robot.x - self.get_target().x)** 2 + (self.robot.y - self.get_target().y) ** 2) 120 | newTargetDistance = math.sqrt((xpredict - self.get_target().x) ** 2 + (ypredict - self.get_target().y) ** 2) 121 | distanceForward = previousTargetDistance - newTargetDistance 122 | # Alternative: how far have I moved forwards? 123 | # distanceForward = xpredict - x 124 | # Positive benefit 125 | distanceBenefit = FORWARD_WEIGHT * distanceForward 126 | # Negative cost: once we are less than SAFE_DIST from collision, linearly increasing cost 127 | if (distanceToObstacle < SAFE_DIST): 128 | obstacleCost = OBSTACLE_WEIGHT * 1000 * (SAFE_DIST - distanceToObstacle) 129 | else: 130 | obstacleCost = 0.0 131 | # Total benefit function to optimise 132 | benefit = distanceBenefit - obstacleCost 133 | if (benefit > BEST_BENEFIT): 134 | vLchosen = vLpossible 135 | vRchosen = vRpossible 136 | BEST_BENEFIT = benefit 137 | 138 | self.vL = vLchosen 139 | self.vR = vRchosen 140 | 141 | return self.vL, self.vR 142 | 143 | -------------------------------------------------------------------------------- /entities/Robot.py: -------------------------------------------------------------------------------- 1 | import math 2 | import logging 3 | import numpy as np 4 | from collections import deque 5 | from commons.math import angular_speed, rotate_via_numpy 6 | from commons.math import speed as avg_speed 7 | 8 | 9 | class Robot(object): 10 | 11 | def __init__(self, game, robot_id, team_color): 12 | self.game = game 13 | self.robot_id = robot_id 14 | self.team_color = team_color 15 | self.current_data = {} 16 | 17 | self.strategy = None 18 | 19 | self.stuck_time = 0 20 | 21 | """ 22 | Essas atribuições serão feitas no Coach quando ele existir 23 | """ 24 | 25 | self.log = logging.getLogger(self.get_name()) 26 | ch = logging.StreamHandler() 27 | ch.setLevel(logging.INFO) 28 | 29 | formatter = logging.Formatter('\033[1m|%(levelname)s|%(name)s|%(message)s\033[1m') 30 | ch.setFormatter(formatter) 31 | self.log.addHandler(ch) 32 | 33 | self.dimensions = { 34 | 'L': 0.075, 35 | 'R': 0.035 36 | } 37 | 38 | self.power_left, self.power_right = 0, 0 39 | 40 | self._frames = { 41 | 'x': deque(maxlen=10), 42 | 'y': deque(maxlen=10), 43 | 'theta': deque(maxlen=10) 44 | } 45 | 46 | self.vx, self.vy, self.vtheta = 0, 0, 0 47 | self.x, self.y, self.theta = 0, 0, 0 48 | self.speed = 0 49 | self.lost_frames = 0 50 | self.visible = False 51 | 52 | def start(self): 53 | self.strategy.start(self) 54 | 55 | def get_name(self): 56 | return 'ROBOT_{}_{}'.format(self.robot_id, self.team_color) 57 | 58 | def is_visible(self): 59 | return self.visible 60 | 61 | def update(self, frame): 62 | team_color_key = 'robotsBlue' if self.team_color == 'blue' else 'robotsYellow' 63 | 64 | robot_data = [i for i in frame.get(team_color_key, []) if i.get('robotId') == self.robot_id] 65 | 66 | if len(robot_data) >= 1: 67 | self.current_data = robot_data[0] 68 | self.lost_frames = 0 69 | self.visible = True 70 | else: 71 | self.lost_frames += 1 72 | if self.lost_frames > 45: 73 | self.visible = False 74 | return 75 | 76 | self._update_speeds() 77 | self.update_stuckness() 78 | 79 | def get_speed(self): 80 | return (self.vx ** 2 + self.vy ** 2) ** .5 81 | 82 | def _update_speeds(self): 83 | self._frames['x'].append(self.current_data['x']) 84 | self._frames['y'].append(self.current_data['y']) 85 | self._frames['theta'].append(self.current_data['orientation']) 86 | 87 | self.theta = self.current_data['orientation'] 88 | 89 | self.x = self.current_data['x'] 90 | self.y = self.current_data['y'] 91 | 92 | self.vx = avg_speed(self._frames['x'], self.game.vision._fps) 93 | self.vy = avg_speed(self._frames['y'], self.game.vision._fps) 94 | self.vtheta = angular_speed(self._frames['theta'], self.game.vision._fps) 95 | 96 | self.speed = math.sqrt(self.vx ** 2 + self.vy ** 2) 97 | 98 | def update_stuckness(self): 99 | MIN_STUCK_SPEED = 0.5 100 | MIN_STUCK_ANG_SPEED = 1 101 | 102 | if self.game.use_referee and not self.game.referee.can_play: 103 | self.stuck_time = 0 104 | 105 | if self.speed <= MIN_STUCK_SPEED and abs(self.vtheta) <= MIN_STUCK_ANG_SPEED: 106 | self.stuck_time += 1 107 | else: 108 | self.stuck_time = 0 109 | #print(self.stuck_time) 110 | 111 | def is_stuck(self): 112 | MIN_STUCK_TIME = 1 # in seconds 113 | if self.game.vision._fps > 0: 114 | time_in_seconds = self.stuck_time / self.game.vision._fps 115 | if time_in_seconds > MIN_STUCK_TIME: 116 | return True 117 | return False 118 | 119 | def _get_desired_differential_robot_speeds(self, vx, vy, theta): 120 | ''' 121 | Entradas: velocidades no eixo X, Y 122 | Saidas: velocidades linear, angular 123 | ''' 124 | 125 | speed_vector = np.array([vx, vy]) 126 | speed_norm = np.linalg.norm(speed_vector) 127 | robot_world_speed = list(rotate_via_numpy(speed_vector, theta)) 128 | vl = robot_world_speed[0] * speed_norm 129 | 130 | # # code to make the robot move to both directions 131 | if robot_world_speed[0] > 0.0: 132 | robot_world_speed[1] = -robot_world_speed[1] 133 | robot_world_speed[0] = -robot_world_speed[0] 134 | 135 | robot_angle_speed = -math.atan2(robot_world_speed[1], robot_world_speed[0]) 136 | 137 | # va = signal of robot_angle_speed {-1, 1} * robot_world_speed.y [0, 1] * math.pi (max_speed = PI rad/s ) 138 | va = (robot_angle_speed / abs(robot_angle_speed)) * robot_world_speed[1] * math.pi 139 | return vl, va 140 | 141 | def _get_differential_robot_speeds(self, vx, vy, theta): 142 | ''' 143 | Entradas: velocidades no eixo X, Y 144 | Saidas: velocidades linear, angular 145 | ''' 146 | speed_vector = np.array([vx, vy]) 147 | speed_norm = np.linalg.norm(speed_vector) 148 | robot_world_speed = rotate_via_numpy(speed_vector, theta) 149 | 150 | vl = robot_world_speed[0] * speed_norm 151 | 152 | va = self.vtheta 153 | 154 | return vl, va 155 | 156 | def decide(self): 157 | desired = self.strategy.decide() 158 | self.strategy.set_desired(desired) 159 | self.power_left, self.power_right = self.strategy.update() 160 | 161 | return self._get_command(self.power_left, self.power_right) 162 | 163 | def _get_command(self, power_left, power_right): 164 | return { 165 | 'robot_id': self.robot_id, 166 | 'wheel_left': power_left, 167 | 'wheel_right': power_right, 168 | 'color': self.team_color 169 | } 170 | 171 | def __getitem__(self, item): 172 | if item == 0: 173 | return self.x 174 | 175 | if item == 1: 176 | return self.y 177 | 178 | if item == 2: 179 | return self.theta 180 | 181 | raise IndexError("Robot only has 3 coordinates") 182 | -------------------------------------------------------------------------------- /protocols/packet_pb2.py: -------------------------------------------------------------------------------- 1 | # Generated by the protocol buffer compiler. DO NOT EDIT! 2 | # source: packet.proto 3 | 4 | import sys 5 | _b=sys.version_info[0]<3 and (lambda x:x) or (lambda x:x.encode('latin1')) 6 | from google.protobuf import descriptor as _descriptor 7 | from google.protobuf import message as _message 8 | from google.protobuf import reflection as _reflection 9 | from google.protobuf import symbol_database as _symbol_database 10 | # @@protoc_insertion_point(imports) 11 | 12 | _sym_db = _symbol_database.Default() 13 | 14 | 15 | import protocols.command_pb2 as command__pb2 16 | import protocols.replacement_pb2 as replacement__pb2 17 | import protocols.common_pb2 as common__pb2 18 | 19 | 20 | DESCRIPTOR = _descriptor.FileDescriptor( 21 | name='packet.proto', 22 | package='fira_message.sim_to_ref', 23 | syntax='proto3', 24 | serialized_options=None, 25 | serialized_pb=_b('\n\x0cpacket.proto\x12\x17\x66ira_message.sim_to_ref\x1a\rcommand.proto\x1a\x11replacement.proto\x1a\x0c\x63ommon.proto\"o\n\x06Packet\x12.\n\x03\x63md\x18\x01 \x01(\x0b\x32!.fira_message.sim_to_ref.Commands\x12\x35\n\x07replace\x18\x02 \x01(\x0b\x32$.fira_message.sim_to_ref.Replacement\"c\n\x0b\x45nvironment\x12\x0c\n\x04step\x18\x01 \x01(\r\x12\"\n\x05\x66rame\x18\x02 \x01(\x0b\x32\x13.fira_message.Frame\x12\"\n\x05\x66ield\x18\x03 \x01(\x0b\x32\x13.fira_message.Field2_\n\x08Simulate\x12S\n\x08Simulate\x12\x1f.fira_message.sim_to_ref.Packet\x1a$.fira_message.sim_to_ref.Environment\"\x00\x62\x06proto3') 26 | , 27 | dependencies=[command__pb2.DESCRIPTOR,replacement__pb2.DESCRIPTOR,common__pb2.DESCRIPTOR,]) 28 | 29 | 30 | 31 | 32 | _PACKET = _descriptor.Descriptor( 33 | name='Packet', 34 | full_name='fira_message.sim_to_ref.Packet', 35 | filename=None, 36 | file=DESCRIPTOR, 37 | containing_type=None, 38 | fields=[ 39 | _descriptor.FieldDescriptor( 40 | name='cmd', full_name='fira_message.sim_to_ref.Packet.cmd', index=0, 41 | number=1, type=11, cpp_type=10, label=1, 42 | has_default_value=False, default_value=None, 43 | message_type=None, enum_type=None, containing_type=None, 44 | is_extension=False, extension_scope=None, 45 | serialized_options=None, file=DESCRIPTOR), 46 | _descriptor.FieldDescriptor( 47 | name='replace', full_name='fira_message.sim_to_ref.Packet.replace', index=1, 48 | number=2, type=11, cpp_type=10, label=1, 49 | has_default_value=False, default_value=None, 50 | message_type=None, enum_type=None, containing_type=None, 51 | is_extension=False, extension_scope=None, 52 | serialized_options=None, file=DESCRIPTOR), 53 | ], 54 | extensions=[ 55 | ], 56 | nested_types=[], 57 | enum_types=[ 58 | ], 59 | serialized_options=None, 60 | is_extendable=False, 61 | syntax='proto3', 62 | extension_ranges=[], 63 | oneofs=[ 64 | ], 65 | serialized_start=89, 66 | serialized_end=200, 67 | ) 68 | 69 | 70 | _ENVIRONMENT = _descriptor.Descriptor( 71 | name='Environment', 72 | full_name='fira_message.sim_to_ref.Environment', 73 | filename=None, 74 | file=DESCRIPTOR, 75 | containing_type=None, 76 | fields=[ 77 | _descriptor.FieldDescriptor( 78 | name='step', full_name='fira_message.sim_to_ref.Environment.step', index=0, 79 | number=1, type=13, cpp_type=3, label=1, 80 | has_default_value=False, default_value=0, 81 | message_type=None, enum_type=None, containing_type=None, 82 | is_extension=False, extension_scope=None, 83 | serialized_options=None, file=DESCRIPTOR), 84 | _descriptor.FieldDescriptor( 85 | name='frame', full_name='fira_message.sim_to_ref.Environment.frame', index=1, 86 | number=2, type=11, cpp_type=10, label=1, 87 | has_default_value=False, default_value=None, 88 | message_type=None, enum_type=None, containing_type=None, 89 | is_extension=False, extension_scope=None, 90 | serialized_options=None, file=DESCRIPTOR), 91 | _descriptor.FieldDescriptor( 92 | name='field', full_name='fira_message.sim_to_ref.Environment.field', index=2, 93 | number=3, type=11, cpp_type=10, label=1, 94 | has_default_value=False, default_value=None, 95 | message_type=None, enum_type=None, containing_type=None, 96 | is_extension=False, extension_scope=None, 97 | serialized_options=None, file=DESCRIPTOR), 98 | ], 99 | extensions=[ 100 | ], 101 | nested_types=[], 102 | enum_types=[ 103 | ], 104 | serialized_options=None, 105 | is_extendable=False, 106 | syntax='proto3', 107 | extension_ranges=[], 108 | oneofs=[ 109 | ], 110 | serialized_start=202, 111 | serialized_end=301, 112 | ) 113 | 114 | _PACKET.fields_by_name['cmd'].message_type = command__pb2._COMMANDS 115 | _PACKET.fields_by_name['replace'].message_type = replacement__pb2._REPLACEMENT 116 | _ENVIRONMENT.fields_by_name['frame'].message_type = common__pb2._FRAME 117 | _ENVIRONMENT.fields_by_name['field'].message_type = common__pb2._FIELD 118 | DESCRIPTOR.message_types_by_name['Packet'] = _PACKET 119 | DESCRIPTOR.message_types_by_name['Environment'] = _ENVIRONMENT 120 | _sym_db.RegisterFileDescriptor(DESCRIPTOR) 121 | 122 | Packet = _reflection.GeneratedProtocolMessageType('Packet', (_message.Message,), dict( 123 | DESCRIPTOR = _PACKET, 124 | __module__ = 'packet_pb2' 125 | # @@protoc_insertion_point(class_scope:fira_message.sim_to_ref.Packet) 126 | )) 127 | _sym_db.RegisterMessage(Packet) 128 | 129 | Environment = _reflection.GeneratedProtocolMessageType('Environment', (_message.Message,), dict( 130 | DESCRIPTOR = _ENVIRONMENT, 131 | __module__ = 'packet_pb2' 132 | # @@protoc_insertion_point(class_scope:fira_message.sim_to_ref.Environment) 133 | )) 134 | _sym_db.RegisterMessage(Environment) 135 | 136 | 137 | 138 | _SIMULATE = _descriptor.ServiceDescriptor( 139 | name='Simulate', 140 | full_name='fira_message.sim_to_ref.Simulate', 141 | file=DESCRIPTOR, 142 | index=0, 143 | serialized_options=None, 144 | serialized_start=303, 145 | serialized_end=398, 146 | methods=[ 147 | _descriptor.MethodDescriptor( 148 | name='Simulate', 149 | full_name='fira_message.sim_to_ref.Simulate.Simulate', 150 | index=0, 151 | containing_service=None, 152 | input_type=_PACKET, 153 | output_type=_ENVIRONMENT, 154 | serialized_options=None, 155 | ), 156 | ]) 157 | _sym_db.RegisterServiceDescriptor(_SIMULATE) 158 | 159 | DESCRIPTOR.services_by_name['Simulate'] = _SIMULATE 160 | 161 | # @@protoc_insertion_point(module_scope) 162 | --------------------------------------------------------------------------------