From 5f59d0de1b679e0eef990cb6687c16ac5653a2e0 Mon Sep 17 00:00:00 2001 From: Tyler Arehart Date: Wed, 12 Feb 2020 21:10:23 -0800 Subject: [PATCH 01/11] Adding a sequence system to help with dodges. --- src/bot.py | 38 +++++++++++++++------------------- src/util/sequence.py | 49 ++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 66 insertions(+), 21 deletions(-) create mode 100644 src/util/sequence.py diff --git a/src/bot.py b/src/bot.py index 58511a2..ea3ea31 100644 --- a/src/bot.py +++ b/src/bot.py @@ -5,6 +5,7 @@ from util.orientation import Orientation from util.vec import Vec3 +from util.sequence import Sequence, ControlStep class MyBot(BaseAgent): @@ -12,12 +13,26 @@ class MyBot(BaseAgent): def initialize_agent(self): # This runs once before the bot starts up self.controller_state = SimpleControllerState() + self.active_sequence: Sequence = None def get_output(self, packet: GameTickPacket) -> SimpleControllerState: - ball_location = Vec3(packet.game_ball.physics.location) + if self.active_sequence and not self.active_sequence.done: + return self.active_sequence.tick(packet) + + ball_location = Vec3(packet.game_ball.physics.location) my_car = packet.game_cars[self.index] car_location = Vec3(my_car.physics.location) + car_velocity = Vec3(my_car.physics.velocity) + + if 550 < car_velocity.length() < 600: + self.active_sequence = Sequence([ + ControlStep(0.05, SimpleControllerState(jump=True)), + ControlStep(0.05, SimpleControllerState(jump=False)), + ControlStep(0.2, SimpleControllerState(jump=True, pitch=-1)), + ControlStep(0.8, SimpleControllerState()), + ]) + return self.active_sequence.tick(packet) car_to_ball = ball_location - car_location @@ -27,18 +42,8 @@ def get_output(self, packet: GameTickPacket) -> SimpleControllerState: steer_correction_radians = find_correction(car_direction, car_to_ball) - if steer_correction_radians > 0: - # Positive radians in the unit circle is a turn to the left. - turn = -1.0 # Negative value for a turn to the left. - action_display = "turn left" - else: - turn = 1.0 - action_display = "turn right" - self.controller_state.throttle = 1.0 - self.controller_state.steer = turn - - draw_debug(self.renderer, my_car, packet.game_ball, action_display) + self.controller_state.steer = -1 if steer_correction_radians > 0 else 1.0 return self.controller_state @@ -60,12 +65,3 @@ def find_correction(current: Vec3, ideal: Vec3) -> float: diff -= 2 * math.pi return diff - - -def draw_debug(renderer, car, ball, action_display): - renderer.begin_rendering() - # draw a line from the car to the ball - renderer.draw_line_3d(car.physics.location, ball.physics.location, renderer.white()) - # print the action that the bot is taking - renderer.draw_string_3d(car.physics.location, 2, 2, action_display, renderer.white()) - renderer.end_rendering() diff --git a/src/util/sequence.py b/src/util/sequence.py new file mode 100644 index 0000000..5ebae2d --- /dev/null +++ b/src/util/sequence.py @@ -0,0 +1,49 @@ +from dataclasses import dataclass +from typing import List + +from rlbot.agents.base_agent import SimpleControllerState +from rlbot.utils.structures.game_data_struct import GameTickPacket + + +@dataclass +class StepResult: + controls: SimpleControllerState + done: bool + + +class Step: + def tick(self, packet: GameTickPacket) -> StepResult: + raise NotImplementedError + + +class ControlStep(Step): + def __init__(self, duration: float, controls: SimpleControllerState): + self.duration = duration + self.controls = controls + self.start_time: float = None + + def tick(self, packet: GameTickPacket) -> StepResult: + if self.start_time is None: + self.start_time = packet.game_info.seconds_elapsed + elapsed_time = packet.game_info.seconds_elapsed - self.start_time + return StepResult(controls=self.controls, done=elapsed_time > self.duration) + + +class Sequence: + def __init__(self, steps: List[Step]): + self.steps = steps + self.index = 0 + self.done = False + + def tick(self, packet: GameTickPacket): + while True: + if self.index >= len(self.steps): + self.done = True + return SimpleControllerState() + step = self.steps[self.index] + result = step.tick(packet) + if result.done: + self.index += 1 + if self.index >= len(self.steps): + self.done = True + return result.controls From 7d969f300bf6f9e74bbb4980fffa802da5031823 Mon Sep 17 00:00:00 2001 From: Tyler Arehart Date: Wed, 12 Feb 2020 21:45:16 -0800 Subject: [PATCH 02/11] Adding a spike watcher utility so bots know who has spiked the ball. --- src/bot.cfg | 2 +- src/bot.py | 21 +++++++++++++++++++-- src/util/spikes.py | 30 ++++++++++++++++++++++++++++++ 3 files changed, 50 insertions(+), 3 deletions(-) create mode 100644 src/util/spikes.py diff --git a/src/bot.cfg b/src/bot.cfg index 3aa2023..247ba37 100644 --- a/src/bot.cfg +++ b/src/bot.cfg @@ -6,7 +6,7 @@ looks_config = ./appearance.cfg python_file = ./bot.py # Name of the bot in-game -name = PythonExampleBot +name = SpikeRushExampleBot # The maximum number of ticks per second that your bot wishes to receive. maximum_tick_rate_preference = 120 diff --git a/src/bot.py b/src/bot.py index ea3ea31..28c90e4 100644 --- a/src/bot.py +++ b/src/bot.py @@ -2,8 +2,10 @@ from rlbot.agents.base_agent import BaseAgent, SimpleControllerState from rlbot.utils.structures.game_data_struct import GameTickPacket +from rlbot.utils.structures.quick_chats import QuickChats from util.orientation import Orientation +from util.spikes import SpikeWatcher from util.vec import Vec3 from util.sequence import Sequence, ControlStep @@ -14,17 +16,22 @@ def initialize_agent(self): # This runs once before the bot starts up self.controller_state = SimpleControllerState() self.active_sequence: Sequence = None + self.spike_watcher = SpikeWatcher() def get_output(self, packet: GameTickPacket) -> SimpleControllerState: if self.active_sequence and not self.active_sequence.done: return self.active_sequence.tick(packet) + self.spike_watcher.read_packet(packet) + ball_location = Vec3(packet.game_ball.physics.location) my_car = packet.game_cars[self.index] car_location = Vec3(my_car.physics.location) car_velocity = Vec3(my_car.physics.velocity) + # Example of using a sequence + # This will do a front flip if the car's velocity is between 550 and 600 if 550 < car_velocity.length() < 600: self.active_sequence = Sequence([ ControlStep(0.05, SimpleControllerState(jump=True)), @@ -34,13 +41,23 @@ def get_output(self, packet: GameTickPacket) -> SimpleControllerState: ]) return self.active_sequence.tick(packet) - car_to_ball = ball_location - car_location + # Example of using the spike watcher. + # This will make the bot say I got it! when it spikes the ball, + # then release it 3 seconds later. + if self.spike_watcher.carrying_car == my_car: + if self.spike_watcher.carry_duration == 0: + self.send_quick_chat(QuickChats.CHAT_EVERYONE, QuickChats.Information_IGotIt) + elif self.spike_watcher.carry_duration > 3: + return SimpleControllerState(use_item=True) + # The rest of this code just ball chases. # Find the direction of our car using the Orientation class car_orientation = Orientation(my_car.physics.rotation) car_direction = car_orientation.forward - steer_correction_radians = find_correction(car_direction, car_to_ball) + target = ball_location + car_to_target = target - car_location + steer_correction_radians = find_correction(car_direction, car_to_target) self.controller_state.throttle = 1.0 self.controller_state.steer = -1 if steer_correction_radians > 0 else 1.0 diff --git a/src/util/spikes.py b/src/util/spikes.py new file mode 100644 index 0000000..44334e9 --- /dev/null +++ b/src/util/spikes.py @@ -0,0 +1,30 @@ +from rlbot.utils.structures.game_data_struct import PlayerInfo, GameTickPacket + +from util.vec import Vec3 + + +class SpikeWatcher: + def __init__(self): + self.carrying_car: PlayerInfo = None + self.spike_moment = 0 + self.carry_duration = 0 + + def read_packet(self, packet: GameTickPacket): + ball_location = Vec3(packet.game_ball.physics.location) + closest_candidate: PlayerInfo = None + closest_distance = 999999 + for i in range(packet.num_cars): + car = packet.game_cars[i] + car_location = Vec3(car.physics.location) + distance = car_location.dist(ball_location) + if distance < 190: + if distance < closest_distance: + closest_candidate = car + closest_distance = distance + if closest_candidate != self.carrying_car and closest_candidate is not None: + self.spike_moment = packet.game_info.seconds_elapsed + + self.carrying_car = closest_candidate + if self.carrying_car is not None: + self.carry_duration = packet.game_info.seconds_elapsed - self.spike_moment + print(closest_distance) From 7688c7c43c19aa32d2a8f69a123c745f3c8853ce Mon Sep 17 00:00:00 2001 From: oxrock Date: Thu, 13 Feb 2020 00:44:44 -0500 Subject: [PATCH 03/11] Added a function that parses ball prediction and returns the vector and time of the first predicted scoring ball. Added that information to debug rendering function. --- src/bot.py | 29 +++++++++++++++++++++++++++++ 1 file changed, 29 insertions(+) diff --git a/src/bot.py b/src/bot.py index 28c90e4..331cbf9 100644 --- a/src/bot.py +++ b/src/bot.py @@ -1,4 +1,5 @@ import math +from typing import List from rlbot.agents.base_agent import BaseAgent, SimpleControllerState from rlbot.utils.structures.game_data_struct import GameTickPacket @@ -25,6 +26,13 @@ def get_output(self, packet: GameTickPacket) -> SimpleControllerState: self.spike_watcher.read_packet(packet) + ball_prediction = self.get_ball_prediction_struct() + predicted_goal = find_future_goal(ball_prediction) + goal_text = "No Goal Threats" + + if predicted_goal: + goal_text = f"Goal in {'%.4s' % str(predicted_goal[1]-packet.game_info.seconds_elapsed)}s" + ball_location = Vec3(packet.game_ball.physics.location) my_car = packet.game_cars[self.index] car_location = Vec3(my_car.physics.location) @@ -62,6 +70,8 @@ def get_output(self, packet: GameTickPacket) -> SimpleControllerState: self.controller_state.throttle = 1.0 self.controller_state.steer = -1 if steer_correction_radians > 0 else 1.0 + draw_debug(self.renderer, [goal_text]) + return self.controller_state @@ -82,3 +92,22 @@ def find_correction(current: Vec3, ideal: Vec3) -> float: diff -= 2 * math.pi return diff + + +def draw_debug(renderer, text_lines: List[str]): + renderer.begin_rendering() + y = 200 + for line in text_lines: + renderer.draw_string_2d(50, y, 1, 1, line, renderer.yellow()) + y += 20 + renderer.end_rendering() + + +def find_future_goal(ball_predictions): + for i in range(0, ball_predictions.num_slices): + # field length(5120) + ball radius(93) = 5213 however that results in false positives + if abs(ball_predictions.slices[i].physics.location.y) >= 5235: + # returns the position the ball crosses the goal as well as the time it's predicted to occur + return [Vec3(ball_predictions.slices[i].physics.location), ball_predictions.slices[i].game_seconds] + + return None From 7b92424411ce10f8c1883ed3bd976428539910be Mon Sep 17 00:00:00 2001 From: Tyler Arehart Date: Wed, 12 Feb 2020 22:44:03 -0800 Subject: [PATCH 04/11] Refactoring the goal detector a bit. --- src/bot.py | 19 +++++-------------- src/util/goal_detector.py | 29 +++++++++++++++++++++++++++++ src/util/spikes.py | 2 +- 3 files changed, 35 insertions(+), 15 deletions(-) create mode 100644 src/util/goal_detector.py diff --git a/src/bot.py b/src/bot.py index 331cbf9..2638d0c 100644 --- a/src/bot.py +++ b/src/bot.py @@ -5,10 +5,11 @@ from rlbot.utils.structures.game_data_struct import GameTickPacket from rlbot.utils.structures.quick_chats import QuickChats +from util.goal_detector import find_future_goal from util.orientation import Orientation +from util.sequence import Sequence, ControlStep from util.spikes import SpikeWatcher from util.vec import Vec3 -from util.sequence import Sequence, ControlStep class MyBot(BaseAgent): @@ -25,13 +26,13 @@ def get_output(self, packet: GameTickPacket) -> SimpleControllerState: return self.active_sequence.tick(packet) self.spike_watcher.read_packet(packet) - ball_prediction = self.get_ball_prediction_struct() + + # Example of predicting a goal event predicted_goal = find_future_goal(ball_prediction) goal_text = "No Goal Threats" - if predicted_goal: - goal_text = f"Goal in {'%.4s' % str(predicted_goal[1]-packet.game_info.seconds_elapsed)}s" + goal_text = f"Goal in {predicted_goal.time - packet.game_info.seconds_elapsed:.2f}s" ball_location = Vec3(packet.game_ball.physics.location) my_car = packet.game_cars[self.index] @@ -101,13 +102,3 @@ def draw_debug(renderer, text_lines: List[str]): renderer.draw_string_2d(50, y, 1, 1, line, renderer.yellow()) y += 20 renderer.end_rendering() - - -def find_future_goal(ball_predictions): - for i in range(0, ball_predictions.num_slices): - # field length(5120) + ball radius(93) = 5213 however that results in false positives - if abs(ball_predictions.slices[i].physics.location.y) >= 5235: - # returns the position the ball crosses the goal as well as the time it's predicted to occur - return [Vec3(ball_predictions.slices[i].physics.location), ball_predictions.slices[i].game_seconds] - - return None diff --git a/src/util/goal_detector.py b/src/util/goal_detector.py new file mode 100644 index 0000000..ebffe64 --- /dev/null +++ b/src/util/goal_detector.py @@ -0,0 +1,29 @@ +from dataclasses import dataclass + +from util.vec import Vec3 + +# field length(5120) + ball radius(93) = 5213 however that results in false positives +GOAL_THRESHOLD = 5235 + +# We will jump this number of frames when looking for a moment where the ball is inside the goal. +# Big number for efficiency, but not so big that the ball could go in and then back out during that +# time span. Unit is the number of frames in the ball prediction, and the prediction is at 60 frames per second. +COARSE_SEARCH_INCREMENT = 20 + + +@dataclass +class FutureGoal: + location: Vec3 + velocity: Vec3 + time: float + + +def find_future_goal(ball_predictions): + for coarse_index in range(0, ball_predictions.num_slices, COARSE_SEARCH_INCREMENT): + if abs(ball_predictions.slices[coarse_index].physics.location.y) >= GOAL_THRESHOLD: + for j in range(max(0, coarse_index - COARSE_SEARCH_INCREMENT), coarse_index): + slice = ball_predictions.slices[j] + if abs(slice.physics.location.y) >= GOAL_THRESHOLD: + # returns the position the ball crosses the goal as well as the time it's predicted to occur + return FutureGoal(Vec3(slice.physics.location), Vec3(slice.physics.velocity), slice.game_seconds) + return None diff --git a/src/util/spikes.py b/src/util/spikes.py index 44334e9..e041142 100644 --- a/src/util/spikes.py +++ b/src/util/spikes.py @@ -17,7 +17,7 @@ def read_packet(self, packet: GameTickPacket): car = packet.game_cars[i] car_location = Vec3(car.physics.location) distance = car_location.dist(ball_location) - if distance < 190: + if distance < 195: if distance < closest_distance: closest_candidate = car closest_distance = distance From 935acd891b225aa4faa6d16c91ee907402469498 Mon Sep 17 00:00:00 2001 From: Viliam Vadocz Date: Fri, 14 Feb 2020 05:33:15 +0100 Subject: [PATCH 05/11] Add numpy utilities and game data pre-processing (#36) --- src/bot.py | 3 +- src/np_util/data.py | 141 +++++++++++++++++ src/np_util/utils.py | 365 +++++++++++++++++++++++++++++++++++++++++++ 3 files changed, 508 insertions(+), 1 deletion(-) create mode 100644 src/np_util/data.py create mode 100644 src/np_util/utils.py diff --git a/src/bot.py b/src/bot.py index 2638d0c..088b637 100644 --- a/src/bot.py +++ b/src/bot.py @@ -11,6 +11,7 @@ from util.spikes import SpikeWatcher from util.vec import Vec3 +# Would you like to use numpy utilities? Check out the np_util folder! class MyBot(BaseAgent): @@ -97,7 +98,7 @@ def find_correction(current: Vec3, ideal: Vec3) -> float: def draw_debug(renderer, text_lines: List[str]): renderer.begin_rendering() - y = 200 + y = 250 for line in text_lines: renderer.draw_string_2d(50, y, 1, 1, line, renderer.yellow()) y += 20 diff --git a/src/np_util/data.py b/src/np_util/data.py new file mode 100644 index 0000000..6c23ec2 --- /dev/null +++ b/src/np_util/data.py @@ -0,0 +1,141 @@ +'''Rocket League data pre-processing.''' + +from .utils import Car, Ball, BoostPad, arr_from_list, arr_from_rot, arr_from_vec, orient_matrix + +import numpy as np + + +def setup(self, packet, field_info): + """Sets up the variables and classes for the agent. + + Arguments: + self {BaseAgent} -- The agent. + packet {GameTickPacket} -- Information about the game. + field_info {FieldInfoPacket} -- Information about the game field. + """ + + # Game info + self.game_time = packet.game_info.seconds_elapsed + self.dt = 1.0 / 120.0 + self.last_time = 0.0 + self.r_active = packet.game_info.is_round_active + self.ko_pause = packet.game_info.is_kickoff_pause + self.m_ended = packet.game_info.is_match_ended + self.gravity = packet.game_info.world_gravity_z + + # Creates Car objects for each car. + self.teammates = [] + self.opponents = [] + for index in range(packet.num_cars): + car = packet.game_cars[index] + if index == self.index: + self.player = Car(self.index, self.team, self.name) + elif car.team == self.team: + self.teammates.append(Car(index, car.team, car.name)) + else: + self.opponents.append(Car(index, car.team, car.name)) + + # Creates a Ball object. + self.ball = Ball() + + # Creates Boostpad objects. + self.l_pads = [] + self.s_pads = [] + for i in range(field_info.num_boosts): + pad = field_info.boost_pads[i] + pad_type = self.l_pads if pad.is_full_boost else self.s_pads + pad_obj = BoostPad(i, arr_from_vec(pad.location)) + pad_type.append(pad_obj) + + +def process(self, packet): + """Processes the gametick packet. + Arguments: + self {BaseAgent} -- The agent. + packet {GameTickPacket} -- The game packet being processed. + """ + + # Processing game info. + self.game_time = packet.game_info.seconds_elapsed + self.dt = self.game_time - self.last_time + self.last_time = self.game_time + self.r_active = packet.game_info.is_round_active + self.ko_pause = packet.game_info.is_kickoff_pause + self.m_ended = packet.game_info.is_match_ended + self.gravity = packet.game_info.world_gravity_z + + # Processing player data. + # From packet: + self.player.pos = arr_from_vec(packet.game_cars[self.player.index].physics.location) + self.player.rot = arr_from_rot(packet.game_cars[self.player.index].physics.rotation) + self.player.vel = arr_from_vec(packet.game_cars[self.player.index].physics.velocity) + self.player.ang_vel = arr_from_vec(packet.game_cars[self.player.index].physics.angular_velocity) + self.player.dead = packet.game_cars[self.player.index].is_demolished + self.player.wheel_c = packet.game_cars[self.player.index].has_wheel_contact + self.player.sonic = packet.game_cars[self.player.index].is_super_sonic + self.player.jumped = packet.game_cars[self.player.index].jumped + self.player.d_jumped = packet.game_cars[self.player.index].double_jumped + self.player.boost = packet.game_cars[self.player.index].boost + # Calculated: + self.player.orient_m = orient_matrix(self.player.rot, self.player.orient_m) + + # Processing teammates. + for teammate in self.teammates: + # From packet: + teammate.pos = arr_from_vec(packet.game_cars[teammate.index].physics.location) + teammate.rot = arr_from_rot(packet.game_cars[teammate.index].physics.rotation) + teammate.vel = arr_from_vec(packet.game_cars[teammate.index].physics.velocity) + teammate.ang_vel = arr_from_vec(packet.game_cars[teammate.index].physics.angular_velocity) + teammate.dead = packet.game_cars[teammate.index].is_demolished + teammate.wheel_c = packet.game_cars[teammate.index].has_wheel_contact + teammate.sonic = packet.game_cars[teammate.index].is_super_sonic + teammate.jumped = packet.game_cars[teammate.index].jumped + teammate.d_jumped = packet.game_cars[teammate.index].double_jumped + teammate.boost = packet.game_cars[teammate.index].boost + + # Processing opponents. + for opponent in self.opponents: + # From packet: + opponent.pos = arr_from_vec(packet.game_cars[opponent.index].physics.location) + opponent.rot = arr_from_rot(packet.game_cars[opponent.index].physics.rotation) + opponent.vel = arr_from_vec(packet.game_cars[opponent.index].physics.velocity) + opponent.ang_vel = arr_from_vec(packet.game_cars[opponent.index].physics.angular_velocity) + opponent.dead = packet.game_cars[opponent.index].is_demolished + opponent.wheel_c = packet.game_cars[opponent.index].has_wheel_contact + opponent.sonic = packet.game_cars[opponent.index].is_super_sonic + opponent.jumped = packet.game_cars[opponent.index].jumped + opponent.d_jumped = packet.game_cars[opponent.index].double_jumped + opponent.boost = packet.game_cars[opponent.index].boost + + # Processing Ball data. + self.ball.pos = arr_from_vec(packet.game_ball.physics.location) + self.ball.rot = arr_from_rot(packet.game_ball.physics.rotation) + self.ball.vel = arr_from_vec(packet.game_ball.physics.velocity) + self.ball.ang_vel = arr_from_vec(packet.game_ball.physics.angular_velocity) + self.ball.last_touch = packet.game_ball.latest_touch + + # Processing ball prediction. + ball_prediction_struct = self.get_ball_prediction_struct() + dtype = [ + ('physics', [ + ('pos', ' np.ndarray: + """Converts list to numpy array. + Arguments: + L {list} -- The list to convert containing 3 elements. + Returns: + np.array -- Numpy array with the same contents as the list. + """ + if len(L) != 3: + raise ValueError('Expected a list of length 3.') + return np.array(L[0], L[1], L[2]) + + +def arr_from_rot(R: Rotator) -> np.ndarray: + """Converts rotator to numpy array. + + Arguments: + R {Rotator} -- Rotator class containing pitch, yaw, and roll. + + Returns: + np.ndarray -- Numpy array with the same contents as the rotator. + """ + return np.array([R.pitch, R.yaw, R.roll]) + + +def arr_from_vec(V: Vector3) -> np.ndarray: + """Converts vector3 to numpy array. + + Arguments: + V {Vector3} -- Vector3 class containing x, y, and z. + + Returns: + np.ndarray -- Numpy array with the same contents as the vector3. + """ + return np.array([V.x, V.y, V.z]) + + +# ----------------------------------------------------------- + +# USEFUL UTILITY FUNCTIONS: + +def normalise(V: np.ndarray) -> np.ndarray: + """Normalises a vector. + + Arguments: + V {np.ndarray} -- Vector. + + Returns: + np.ndarray -- Normalised vector. + """ + magnitude = np.linalg.norm(V) + if magnitude != 0.0: + return V / magnitude + else: + return V + + +def angle_between_vectors(v1: np.ndarray, v2: np.ndarray) -> float: + """Returns the positive angle in radians between vectors v1 and v2. + + Arguments: + v1 {np.ndarray} -- First vector. + v2 {np.ndarray} -- Second vector + + Returns: + float -- Positive acute angle between the vectors in radians. + """ + v1_u = normalise(v1) + v2_u = normalise(v2) + return np.arccos(np.clip(np.dot(v1_u, v2_u), -1.0, 1.0)) + +# ----------------------------------------------------------- + +# FUNCTIONS FOR CONVERTING BETWEEN WORLD AND LOCAL COORDINATES: + +def orient_matrix(R: np.ndarray, A: np.ndarray = np.zeros((3, 3))) -> np.ndarray: + """Converts from Euler angles to an orientation matrix. + + Arguments: + R {np.ndarray} -- Pitch, yaw, and roll. + A {np.ndarray} -- Previous orientation matrix. (default: {np.zeros((3, 3))}) + + Returns: + np.ndarray -- Orientation matrix of shape (3, 3). + """ + # Credits to chip https://samuelpmish.github.io/notes/RocketLeague/aerial_control/ + pitch: float = R[0] + yaw: float = R[1] + roll: float = R[2] + + CR: float = np.cos(roll) + SR: float = np.sin(roll) + CP: float = np.cos(pitch) + SP: float = np.sin(pitch) + CY: float = np.cos(yaw) + SY: float = np.sin(yaw) + + A = np.zeros((3, 3)) + + # front direction + A[0, 0] = CP * CY + A[1, 0] = CP * SY + A[2, 0] = SP + + # right direction + A[0, 1] = CY * SP * SR - CR * SY + A[1, 1] = SY * SP * SR + CR * CY + A[2, 1] = -CP * SR + + # up direction + A[0, 2] = -CR * CY * SP - SR * SY + A[1, 2] = -CR * SY * SP + SR * CY + A[2, 2] = CP * CR + + return A + + +def local(A: np.ndarray, p0: np.ndarray, p1: np.ndarray) -> np.ndarray: + """Transforms world coordinates into local coordinates. + + Arguments: + A {np.ndarray} -- The local orientation matrix. + p0 {np.ndarray} -- World x, y, and z coordinates of the start point for the vector. + p1 {np.ndarray} -- World x, y, and z coordinates of the end point for the vector. + + Returns: + np.ndarray -- Local x, y, and z coordinates. + """ + return np.dot(A.T, p1 - p0) + + +def world(A: np.ndarray, p0: np.ndarray, p1: np.ndarray) -> np.ndarray: + """Transforms local into world coordinates. + + Arguments: + A {np.ndarray} -- The local orientation matrix. + p0 {np.ndarray} -- World x, y, and z coordinates of the start point for the vector. + p1 {np.ndarray} -- Local x, y, and z coordinates of the end point for the vector. + + Returns: + np.ndarray -- World x, y, and z coordinates. + """ + return p0 + A * p1 + +# ----------------------------------------------------------- + +# ROCKET LEAGUE SPECIFIC FUNCTIONS: + +def team_sign(team: int) -> int: + """Gives the sign for a calculation based on team. + + Arguments: + team {int} -- 0 if Blue, 1 if Orange. + + Returns: + int -- 1 if Blue, -1 if Orange + """ + # Other creative ways to do it: + + # return (1, -1)[team] + + # return 1 if team == 0 else -1 + + # for i in range(team): + # return 1 + # return -1 + + # return -range(-1, 2, -2)[team] + + return -2 * team + 1 + + +def turn_r(v: np.ndarray) -> float: + """Calculates the minimum turning radius for given velocity. + + Arguments: + v {np.ndarray} -- A velocity vector. + + Returns: + float -- The smallest radius possible for the given velocity. + """ + s = np.linalg.norm(v) + return -6.901E-11 * s**4 + 2.1815E-07 * s**3 - 5.4437E-06 * s**2 + 0.12496671 * s + 157 + +# ----------------------------------------------------------- + +# OTHER: + +def linear_predict(start_pos: np.ndarray, start_vel: np.ndarray, start_time: float, seconds: float) -> np.ndarray: + """Predicts motion of object in a straight line. + + Arguments: + start_pos {np.ndarray} -- Current position. + start_vel {np.ndarray} -- Current velocity. + start_time {float} -- Current time. + seconds {float} -- Time for which to predict. + + Returns: + np.ndarray -- linear prediction, 60 tps. + dtype = [ + ('pos', float, (ticks, 3)), + ('vel', float, (ticks, 3)), + ('time', float, (ticks, 1)) + ] + """ + ticks = int(60*seconds) + time = np.linspace(0, seconds, ticks)[:, np.newaxis] + pos = start_pos + time * start_vel + vel = np.ones_like(time) * start_vel + time += start_time + + dtype = [('pos', float, (ticks, 3)), ('vel', float, (ticks, 3)), ('time', float, (ticks, 1))] + prediction = np.array((pos, vel, time), dtype=dtype) + return prediction + + +def closest_to(pos: np.ndarray, others: np.ndarray) -> int: + """Finds the index of the closest point. + + Arguments: + pos {np.ndarray} -- coordinates of the position of interest. + others {np.ndarray} -- Array where each row is a position. + + Returns: + int -- index of the closest position to the position of interest. + """ + vectors = others - pos + distances = np.sqrt(np.einsum('ij,ij->i', vectors, vectors)) + return np.argmin(distances) # returns first if more than one is closest + + +def special_sauce(x: float, a: float) -> float: + """Modified sigmoid function. + + Arguments: + x {float} -- The input. + a {float} -- Constant; larger => faster rise. + + Returns: + float -- Output. + """ + # Graph showing how it can be used for steering: + # https://www.geogebra.org/m/udfp2zcy + return 2 / (1 + np.exp(a*x)) - 1 From f94c51b1569224a48711480a3f434b7de02d52d5 Mon Sep 17 00:00:00 2001 From: Tyler Arehart Date: Thu, 13 Feb 2020 21:37:16 -0800 Subject: [PATCH 06/11] Smooth steering, better spike threshold, documentation. --- rlbot.cfg | 3 ++- src/bot.py | 32 +++++++++++++++++++++++++++++--- src/util/spikes.py | 10 ++++++++-- 3 files changed, 39 insertions(+), 6 deletions(-) diff --git a/rlbot.cfg b/rlbot.cfg index 7b7051a..bc3f28d 100644 --- a/rlbot.cfg +++ b/rlbot.cfg @@ -6,12 +6,13 @@ [Match Configuration] # Number of bots/players which will be spawned. We support up to max 64. -num_participants = 2 +num_participants = 1 game_mode = Soccer game_map = Mannfield [Mutator Configuration] # Visit https://github.com/RLBot/RLBot/wiki/Config-File-Documentation to see what you can put here. +Rumble = Spike Rush [Participant Configuration] # Put the name of your bot config file here. Only num_participants config files will be read! diff --git a/src/bot.py b/src/bot.py index 088b637..b428a79 100644 --- a/src/bot.py +++ b/src/bot.py @@ -22,6 +22,10 @@ def initialize_agent(self): self.spike_watcher = SpikeWatcher() def get_output(self, packet: GameTickPacket) -> SimpleControllerState: + """ + This function will be called by the framework many times per second. This is where you can + see the motion of the ball, etc. and return controls to drive your car. + """ if self.active_sequence and not self.active_sequence.done: return self.active_sequence.tick(packet) @@ -61,7 +65,7 @@ def get_output(self, packet: GameTickPacket) -> SimpleControllerState: return SimpleControllerState(use_item=True) # The rest of this code just ball chases. - # Find the direction of our car using the Orientation class + # Find the direction of your car using the Orientation class car_orientation = Orientation(my_car.physics.rotation) car_direction = car_orientation.forward @@ -70,15 +74,32 @@ def get_output(self, packet: GameTickPacket) -> SimpleControllerState: steer_correction_radians = find_correction(car_direction, car_to_target) self.controller_state.throttle = 1.0 - self.controller_state.steer = -1 if steer_correction_radians > 0 else 1.0 + + # Change the multiplier to influence the sharpness of steering. You'll wiggle if it's too high. + self.controller_state.steer = limit_to_safe_range(-steer_correction_radians * 5) draw_debug(self.renderer, [goal_text]) return self.controller_state +def limit_to_safe_range(value: float) -> float: + """ + Controls like throttle, steer, pitch, yaw, and roll need to be in the range of -1 to 1. + This will ensure your number is in that range. Something like 0.45 will stay as it is, + but a value of -5.6 would be changed to -1. + """ + if value < -1: + return -1 + if value > 1: + return 1 + return value + + def find_correction(current: Vec3, ideal: Vec3) -> float: - # Finds the angle from current to ideal vector in the xy-plane. Angle will be between -pi and +pi. + """ + Finds the angle from current to ideal vector in the xy-plane. Angle will be between -pi and +pi. + """ # The in-game axes are left handed, so use -x current_in_radians = math.atan2(current.y, -current.x) @@ -97,6 +118,11 @@ def find_correction(current: Vec3, ideal: Vec3) -> float: def draw_debug(renderer, text_lines: List[str]): + """ + This will draw the lines of text in the upper left corner. + This function will automatically put appropriate spacing between each line + so they don't overlap. + """ renderer.begin_rendering() y = 250 for line in text_lines: diff --git a/src/util/spikes.py b/src/util/spikes.py index e041142..582c30a 100644 --- a/src/util/spikes.py +++ b/src/util/spikes.py @@ -2,6 +2,13 @@ from util.vec import Vec3 +# When the ball is attached to a car's spikes, the distance will vary a bit depending on whether the ball is +# on the front bumper, the roof, etc. It tends to be most far away when the ball is on one of the front corners +# and that distance is a little under 200. We want to be sure that it's never over 200, otherwise bots will +# suffer from bad bugs when they don't think the ball is spiked to them but it actually is; they'll probably +# drive in circles. The opposite problem, where they think it's spiked before it really is, is not so bad because +# they usually spike it for real a split second later. +MAX_DISTANCE_WHEN_SPIKED = 200 class SpikeWatcher: def __init__(self): @@ -17,7 +24,7 @@ def read_packet(self, packet: GameTickPacket): car = packet.game_cars[i] car_location = Vec3(car.physics.location) distance = car_location.dist(ball_location) - if distance < 195: + if distance < MAX_DISTANCE_WHEN_SPIKED: if distance < closest_distance: closest_candidate = car closest_distance = distance @@ -27,4 +34,3 @@ def read_packet(self, packet: GameTickPacket): self.carrying_car = closest_candidate if self.carrying_car is not None: self.carry_duration = packet.game_info.seconds_elapsed - self.spike_moment - print(closest_distance) From 0cbea234d71cba7dcbfa30baa7c54992e2f94cd9 Mon Sep 17 00:00:00 2001 From: Tyler Arehart Date: Thu, 13 Feb 2020 23:48:58 -0800 Subject: [PATCH 07/11] Adding easy aerials, and extracting a steering function. --- .gitignore | 4 ++- requirements.txt | 1 + run.py | 3 +- src/bot.py | 69 ++++++++++++----------------------------- src/util/aerial.py | 77 ++++++++++++++++++++++++++++++++++++++++++++++ src/util/drive.py | 26 ++++++++++++++++ 6 files changed, 128 insertions(+), 52 deletions(-) create mode 100644 src/util/aerial.py create mode 100644 src/util/drive.py diff --git a/.gitignore b/.gitignore index 51410e9..e7803d8 100644 --- a/.gitignore +++ b/.gitignore @@ -108,4 +108,6 @@ ENV/ /build # Gradle files -/.gradle \ No newline at end of file +/.gradle + +.vscode/ diff --git a/requirements.txt b/requirements.txt index 25c0c97..a239ec8 100644 --- a/requirements.txt +++ b/requirements.txt @@ -2,6 +2,7 @@ # You will automatically get updates for all versions starting with "1.". rlbot==1.* rlbottraining +RLUtilities # This will cause pip to auto-upgrade and stop scaring people with warning messages pip diff --git a/run.py b/run.py index 6a223de..061aa9c 100644 --- a/run.py +++ b/run.py @@ -13,7 +13,8 @@ logger.log(logging_utils.logging_level, 'Skipping upgrade check for now since it looks like you have no internet') elif public_utils.is_safe_to_upgrade(): - subprocess.call([sys.executable, "-m", "pip", "install", '-r', 'requirements.txt', '--upgrade', '--upgrade-strategy=eager']) + subprocess.call([sys.executable, "-m", "pip", "install", '-r', 'requirements.txt']) + subprocess.call([sys.executable, "-m", "pip", "install", 'rlbot', '--upgrade']) # https://stackoverflow.com/a/44401013 rlbots = [module for module in sys.modules if module.startswith('rlbot')] diff --git a/src/bot.py b/src/bot.py index b428a79..99bfb64 100644 --- a/src/bot.py +++ b/src/bot.py @@ -1,16 +1,17 @@ -import math from typing import List from rlbot.agents.base_agent import BaseAgent, SimpleControllerState from rlbot.utils.structures.game_data_struct import GameTickPacket from rlbot.utils.structures.quick_chats import QuickChats +from util.aerial import AerialStep, LineUpForAerialStep +from util.drive import steer_toward_target from util.goal_detector import find_future_goal -from util.orientation import Orientation from util.sequence import Sequence, ControlStep from util.spikes import SpikeWatcher from util.vec import Vec3 + # Would you like to use numpy utilities? Check out the np_util folder! class MyBot(BaseAgent): @@ -27,6 +28,8 @@ def get_output(self, packet: GameTickPacket) -> SimpleControllerState: see the motion of the ball, etc. and return controls to drive your car. """ + # This is good to keep at the beginning of get_output. It will allow you to continue + # any sequences that you may have started during a previous call to get_output. if self.active_sequence and not self.active_sequence.done: return self.active_sequence.tick(packet) @@ -39,9 +42,7 @@ def get_output(self, packet: GameTickPacket) -> SimpleControllerState: if predicted_goal: goal_text = f"Goal in {predicted_goal.time - packet.game_info.seconds_elapsed:.2f}s" - ball_location = Vec3(packet.game_ball.physics.location) my_car = packet.game_cars[self.index] - car_location = Vec3(my_car.physics.location) car_velocity = Vec3(my_car.physics.velocity) # Example of using a sequence @@ -57,64 +58,32 @@ def get_output(self, packet: GameTickPacket) -> SimpleControllerState: # Example of using the spike watcher. # This will make the bot say I got it! when it spikes the ball, - # then release it 3 seconds later. + # then release it 2 seconds later. if self.spike_watcher.carrying_car == my_car: if self.spike_watcher.carry_duration == 0: self.send_quick_chat(QuickChats.CHAT_EVERYONE, QuickChats.Information_IGotIt) - elif self.spike_watcher.carry_duration > 3: + elif self.spike_watcher.carry_duration > 2: return SimpleControllerState(use_item=True) - # The rest of this code just ball chases. - # Find the direction of your car using the Orientation class - car_orientation = Orientation(my_car.physics.rotation) - car_direction = car_orientation.forward - - target = ball_location - car_to_target = target - car_location - steer_correction_radians = find_correction(car_direction, car_to_target) + # Example of doing an aerial. This will cause the car to jump and fly toward the + # ceiling in the middle of the field. + if my_car.boost > 50 and my_car.has_wheel_contact: + self.start_aerial(Vec3(0, 0, 2000), packet.game_info.seconds_elapsed + 4) + # If nothing else interesting happened, just chase the ball! + ball_location = Vec3(packet.game_ball.physics.location) + self.controller_state.steer = steer_toward_target(my_car, ball_location) self.controller_state.throttle = 1.0 - # Change the multiplier to influence the sharpness of steering. You'll wiggle if it's too high. - self.controller_state.steer = limit_to_safe_range(-steer_correction_radians * 5) - + # Draw some text on the screen draw_debug(self.renderer, [goal_text]) return self.controller_state - -def limit_to_safe_range(value: float) -> float: - """ - Controls like throttle, steer, pitch, yaw, and roll need to be in the range of -1 to 1. - This will ensure your number is in that range. Something like 0.45 will stay as it is, - but a value of -5.6 would be changed to -1. - """ - if value < -1: - return -1 - if value > 1: - return 1 - return value - - -def find_correction(current: Vec3, ideal: Vec3) -> float: - """ - Finds the angle from current to ideal vector in the xy-plane. Angle will be between -pi and +pi. - """ - - # The in-game axes are left handed, so use -x - current_in_radians = math.atan2(current.y, -current.x) - ideal_in_radians = math.atan2(ideal.y, -ideal.x) - - diff = ideal_in_radians - current_in_radians - - # Make sure that diff is between -pi and +pi. - if abs(diff) > math.pi: - if diff < 0: - diff += 2 * math.pi - else: - diff -= 2 * math.pi - - return diff + def start_aerial(self, target: Vec3, arrival_time: float): + self.active_sequence = Sequence([ + LineUpForAerialStep(target, arrival_time, self.index), + AerialStep(target, arrival_time, self.index)]) def draw_debug(renderer, text_lines: List[str]): diff --git a/src/util/aerial.py b/src/util/aerial.py new file mode 100644 index 0000000..1870556 --- /dev/null +++ b/src/util/aerial.py @@ -0,0 +1,77 @@ +from RLUtilities.GameInfo import GameInfo +from RLUtilities.LinearAlgebra import vec3 +from RLUtilities.Maneuvers import Aerial +from rlbot.agents.base_agent import SimpleControllerState +from rlbot.utils.structures.game_data_struct import GameTickPacket + +from util.drive import steer_toward_target +from util.sequence import Step, StepResult +from util.vec import Vec3 + + +MAX_SPEED_WITHOUT_BOOST = 1410 +SECONDS_PER_TICK = 0.008 # Assume a 120Hz game. It's OK if we're wrong, aerial will still go OK + + +class LineUpForAerialStep(Step): + """ + This will cause the car to steer toward the target until it is lined up enough and going at + an appropriate speed for a successful aerial. + """ + def __init__(self, target: Vec3, arrival_time: float, index: int): + self.target = target + self.arrival_time = arrival_time + self.index = index + + def tick(self, packet: GameTickPacket) -> StepResult: + car = packet.game_cars[self.index] + + seconds_till_arrival = self.arrival_time - packet.game_info.seconds_elapsed + if seconds_till_arrival <= 0: + return StepResult(SimpleControllerState(), done=True) + current_speed = Vec3(car.physics.velocity).length() + avg_speed_needed = Vec3(car.physics.location).flat().dist(self.target.flat()) / seconds_till_arrival + + steering = steer_toward_target(car, self.target) + controls = SimpleControllerState( + steer=steering, + throttle=1 if avg_speed_needed > current_speed else 0, + boost=avg_speed_needed > current_speed and avg_speed_needed > MAX_SPEED_WITHOUT_BOOST) + + ready_to_jump = abs(steering) < 0.1 and current_speed / avg_speed_needed > 0.7 + + return StepResult(controls, done=ready_to_jump) + + +class AerialStep(Step): + """ + This uses the Aerial controller provided by RLUtilities. Thanks chip! + It will take care of jumping off the ground and flying toward the target. + This will only work properly if you call tick repeatedly on the same instance. + """ + def __init__(self, target: Vec3, arrival_time: float, index: int): + self.index = index + self.aerial: Aerial = None + self.game_info: GameInfo = None + self.target = target + self.arrival_time = arrival_time + + def tick(self, packet: GameTickPacket) -> StepResult: + + if self.game_info is None: + self.game_info = GameInfo(self.index, packet.game_cars[self.index].team) + self.game_info.read_packet(packet) + + if self.aerial is None: + self.aerial = Aerial(self.game_info.my_car, vec3(self.target.x, self.target.y, self.target.z), + self.arrival_time) + + self.aerial.step(SECONDS_PER_TICK) + controls = SimpleControllerState() + controls.boost = self.aerial.controls.boost + controls.pitch = self.aerial.controls.pitch + controls.yaw = self.aerial.controls.yaw + controls.roll = self.aerial.controls.roll + controls.jump = self.aerial.controls.jump + + return StepResult(controls, packet.game_info.seconds_elapsed > self.arrival_time) diff --git a/src/util/drive.py b/src/util/drive.py new file mode 100644 index 0000000..ad1cad8 --- /dev/null +++ b/src/util/drive.py @@ -0,0 +1,26 @@ +import math + +from rlbot.utils.structures.game_data_struct import PlayerInfo + +from util.orientation import Orientation, relative_location +from util.vec import Vec3 + + +def limit_to_safe_range(value: float) -> float: + """ + Controls like throttle, steer, pitch, yaw, and roll need to be in the range of -1 to 1. + This will ensure your number is in that range. Something like 0.45 will stay as it is, + but a value of -5.6 would be changed to -1. + """ + if value < -1: + return -1 + if value > 1: + return 1 + return value + + + +def steer_toward_target(car: PlayerInfo, target: Vec3) -> float: + relative = relative_location(Vec3(car.physics.location), Orientation(car.physics.rotation), target) + angle = math.atan2(relative.y, relative.x) + return limit_to_safe_range(angle * 5) From a7fdfcaa5b553d55b6f8602fb9abae6bb80112c3 Mon Sep 17 00:00:00 2001 From: Viliam Vadocz Date: Fri, 14 Feb 2020 09:07:37 +0100 Subject: [PATCH 08/11] Vec3 improvements (#37) --- .gitignore | 2 ++ src/bot.py | 2 +- src/util/vec.py | 26 +++++++++++++++++++------- 3 files changed, 22 insertions(+), 8 deletions(-) diff --git a/.gitignore b/.gitignore index e7803d8..89563b0 100644 --- a/.gitignore +++ b/.gitignore @@ -110,4 +110,6 @@ ENV/ # Gradle files /.gradle +# VSCode .vscode/ +*.code-workspace diff --git a/src/bot.py b/src/bot.py index 99bfb64..9ac2d85 100644 --- a/src/bot.py +++ b/src/bot.py @@ -47,7 +47,7 @@ def get_output(self, packet: GameTickPacket) -> SimpleControllerState: # Example of using a sequence # This will do a front flip if the car's velocity is between 550 and 600 - if 550 < car_velocity.length() < 600: + if 550 < car_velocity.length < 600: self.active_sequence = Sequence([ ControlStep(0.05, SimpleControllerState(jump=True)), ControlStep(0.05, SimpleControllerState(jump=False)), diff --git a/src/util/vec.py b/src/util/vec.py index 6850e4f..055bc93 100644 --- a/src/util/vec.py +++ b/src/util/vec.py @@ -1,4 +1,5 @@ import math +from typing import Union # This is a helper class for vector math. You can extend it or delete if you want. @@ -13,8 +14,14 @@ class Vec3: When in doubt visit the wiki: https://github.com/RLBot/RLBot/wiki/Useful-Game-Values """ - - def __init__(self, x: float or 'Vec3'=0, y: float=0, z: float=0): + # https://docs.python.org/3/reference/datamodel.html#slots + __slots__ = [ + 'x', + 'y', + 'z' + ] + + def __init__(self, x: Union[float, 'Vec3']=0, y: float=0, z: float=0): """ Create a new Vec3. The x component can alternatively be another vector with an x, y, and z component, in which case the created vector is a copy of the given vector and the y and z parameter is ignored. Examples: @@ -58,27 +65,32 @@ def __truediv__(self, scale: float) -> 'Vec3': return self * scale def __str__(self): - return "Vec3(" + str(self.x) + ", " + str(self.y) + ", " + str(self.z) + ")" + return f"Vec3({self.x:.2f}, {self.y:.2f}, {self.z:.2f})" + + def __repr__(self): + return self.__str__() def flat(self): """Returns a new Vec3 that equals this Vec3 but projected onto the ground plane. I.e. where z=0.""" return Vec3(self.x, self.y, 0) + @property def length(self): """Returns the length of the vector. Also called magnitude and norm.""" return math.sqrt(self.x**2 + self.y**2 + self.z**2) def dist(self, other: 'Vec3') -> float: """Returns the distance between this vector and another vector using pythagoras.""" - return (self - other).length() + return (self - other).length + @property def normalized(self): """Returns a vector with the same direction but a length of one.""" - return self / self.length() + return self / self.length def rescale(self, new_len: float) -> 'Vec3': """Returns a vector with the same direction but a different length.""" - return new_len * self.normalized() + return new_len * self.normalized def dot(self, other: 'Vec3') -> float: """Returns the dot product.""" @@ -94,5 +106,5 @@ def cross(self, other: 'Vec3') -> 'Vec3': def ang_to(self, ideal: 'Vec3') -> float: """Returns the angle to the ideal vector. Angle will be between 0 and pi.""" - cos_ang = self.dot(ideal) / (self.length() * ideal.length()) + cos_ang = self.dot(ideal) / (self.length * ideal.length) return math.acos(cos_ang) From 5d5367b524bd15acd2427f8b8bb2d6f28dff66b3 Mon Sep 17 00:00:00 2001 From: Tyler Arehart Date: Fri, 14 Feb 2020 00:28:07 -0800 Subject: [PATCH 09/11] Reverting property annotations on Vec3 length() and normalized() --- src/bot.py | 2 +- src/util/vec.py | 15 +++++++-------- 2 files changed, 8 insertions(+), 9 deletions(-) diff --git a/src/bot.py b/src/bot.py index 9ac2d85..99bfb64 100644 --- a/src/bot.py +++ b/src/bot.py @@ -47,7 +47,7 @@ def get_output(self, packet: GameTickPacket) -> SimpleControllerState: # Example of using a sequence # This will do a front flip if the car's velocity is between 550 and 600 - if 550 < car_velocity.length < 600: + if 550 < car_velocity.length() < 600: self.active_sequence = Sequence([ ControlStep(0.05, SimpleControllerState(jump=True)), ControlStep(0.05, SimpleControllerState(jump=False)), diff --git a/src/util/vec.py b/src/util/vec.py index 055bc93..9ce74ac 100644 --- a/src/util/vec.py +++ b/src/util/vec.py @@ -1,8 +1,9 @@ import math from typing import Union +from rlbot.utils.structures.game_data_struct import Vector3 + -# This is a helper class for vector math. You can extend it or delete if you want. class Vec3: """ This class should provide you with all the basic vector operations that you need, but feel free to extend its @@ -21,7 +22,7 @@ class Vec3: 'z' ] - def __init__(self, x: Union[float, 'Vec3']=0, y: float=0, z: float=0): + def __init__(self, x: Union[float, 'Vec3', 'Vector3']=0, y: float=0, z: float=0): """ Create a new Vec3. The x component can alternatively be another vector with an x, y, and z component, in which case the created vector is a copy of the given vector and the y and z parameter is ignored. Examples: @@ -74,23 +75,21 @@ def flat(self): """Returns a new Vec3 that equals this Vec3 but projected onto the ground plane. I.e. where z=0.""" return Vec3(self.x, self.y, 0) - @property def length(self): """Returns the length of the vector. Also called magnitude and norm.""" return math.sqrt(self.x**2 + self.y**2 + self.z**2) def dist(self, other: 'Vec3') -> float: """Returns the distance between this vector and another vector using pythagoras.""" - return (self - other).length + return (self - other).length() - @property def normalized(self): """Returns a vector with the same direction but a length of one.""" - return self / self.length + return self / self.length() def rescale(self, new_len: float) -> 'Vec3': """Returns a vector with the same direction but a different length.""" - return new_len * self.normalized + return new_len * self.normalized() def dot(self, other: 'Vec3') -> float: """Returns the dot product.""" @@ -106,5 +105,5 @@ def cross(self, other: 'Vec3') -> 'Vec3': def ang_to(self, ideal: 'Vec3') -> float: """Returns the angle to the ideal vector. Angle will be between 0 and pi.""" - cos_ang = self.dot(ideal) / (self.length * ideal.length) + cos_ang = self.dot(ideal) / (self.length() * ideal.length()) return math.acos(cos_ang) From ee389dc6e8106350e6c07741d3117a215ac8e64a Mon Sep 17 00:00:00 2001 From: L0laapk3 Date: Sat, 15 Feb 2020 22:05:30 +0100 Subject: [PATCH 10/11] pylint fixes (#39) --- .env | 1 + .gitignore | 3 --- .pylintrc | 2 ++ src/util/__init__.py | 0 4 files changed, 3 insertions(+), 3 deletions(-) create mode 100644 .env create mode 100644 .pylintrc create mode 100644 src/util/__init__.py diff --git a/.env b/.env new file mode 100644 index 0000000..56a282d --- /dev/null +++ b/.env @@ -0,0 +1 @@ +PYTHONPATH=src diff --git a/.gitignore b/.gitignore index 89563b0..122e90e 100644 --- a/.gitignore +++ b/.gitignore @@ -79,9 +79,6 @@ celerybeat-schedule # SageMath parsed files *.sage.py -# dotenv -.env - # virtualenv .venv venv/ diff --git a/.pylintrc b/.pylintrc new file mode 100644 index 0000000..9db66d4 --- /dev/null +++ b/.pylintrc @@ -0,0 +1,2 @@ +[TYPECHECK] +generated-members=QuickChats.* diff --git a/src/util/__init__.py b/src/util/__init__.py new file mode 100644 index 0000000..e69de29 From 2dc260bf94b1461b07305ef9a773df2be757bd89 Mon Sep 17 00:00:00 2001 From: Scott Conner <39441634+DaCoolOne@users.noreply.github.com> Date: Sat, 15 Feb 2020 15:09:20 -0600 Subject: [PATCH 11/11] 1D Kinematics (#38) --- src/util/acceleration.txt | 149 +++++++++++++++++++++++++ src/util/boost_acceleration.txt | 103 +++++++++++++++++ src/util/constants.py | 41 +++++++ src/util/kinematics.py | 191 ++++++++++++++++++++++++++++++++ 4 files changed, 484 insertions(+) create mode 100644 src/util/acceleration.txt create mode 100644 src/util/boost_acceleration.txt create mode 100644 src/util/constants.py create mode 100644 src/util/kinematics.py diff --git a/src/util/acceleration.txt b/src/util/acceleration.txt new file mode 100644 index 0000000..3c95dc4 --- /dev/null +++ b/src/util/acceleration.txt @@ -0,0 +1,149 @@ +0.0,0.0,0.0 +0.016666412353515625,26.55099868774414,0.330078125 +0.033333778381347656,52.64099884033203,1.10009765625 +0.05000019073486328,78.29100036621094,2.30029296875 +0.0666666030883789,103.5009994506836,3.919921875 +0.08333301544189453,128.281005859375,5.9599609375 +0.10000038146972656,152.64100646972656,8.39990234375 +0.11666679382324219,176.5810089111328,11.240234375 +0.1333332061767578,200.1110076904297,14.47998046875 +0.14999961853027344,223.2310028076172,18.10009765625 +0.16666698455810547,245.97100830078125,22.10986328125 +0.1833333969116211,268.3210144042969,26.490234375 +0.19999980926513672,290.291015625,31.240234375 +0.21666717529296875,311.8810119628906,36.35009765625 +0.23333358764648438,333.1109924316406,41.8203125 +0.25,353.96099853515625,47.6298828125 +0.2666664123535156,374.47100830078125,53.7900390625 +0.28333377838134766,394.6210021972656,60.2900390625 +0.3000001907348633,414.4309997558594,67.10986328125 +0.3166666030883789,433.9010009765625,74.27001953125 +0.33333301544189453,453.0409851074219,81.75 +0.35000038146972656,471.8609924316406,89.5302734375 +0.3666667938232422,490.3609924316406,97.6298828125 +0.3833332061767578,508.5409851074219,106.0302734375 +0.39999961853027344,526.4009399414062,114.72998046875 +0.41666698455810547,543.9609375,123.72021484375 +0.4333333969116211,561.220947265625,133.01025390625 +0.4499998092651367,578.1909790039062,142.580078125 +0.46666717529296875,594.8609619140625,152.43017578125 +0.4833335876464844,611.2509765625,162.55029296875 +0.5,627.3609619140625,172.93994140625 +0.5166664123535156,643.1909790039062,183.59033203125 +0.5333337783813477,658.760986328125,194.51025390625 +0.5500001907348633,674.0609741210938,205.68017578125 +0.5666666030883789,689.1009521484375,217.10009765625 +0.5833330154418945,703.8809814453125,228.77001953125 +0.6000003814697266,718.4109497070312,240.68994140625 +0.6166667938232422,732.6909790039062,252.85009765625 +0.6333332061767578,746.73095703125,265.22998046875 +0.6499996185302734,760.5309448242188,277.85009765625 +0.6666669845581055,774.0909423828125,290.68994140625 +0.6833333969116211,787.4309692382812,303.76025390625 +0.6999998092651367,800.5309448242188,317.05029296875 +0.7166671752929688,813.4109497070312,330.56005859375 +0.7333335876464844,826.0709838867188,344.27001953125 +0.75,838.5209350585938,358.2001953125 +0.7666664123535156,850.7509765625,372.330078125 +0.7833337783813477,862.7809448242188,386.66015625 +0.8000001907348633,874.6009521484375,401.18994140625 +0.8166666030883789,886.2109375,415.919921875 +0.8333330154418945,897.6309814453125,430.830078125 +0.8500003814697266,908.8609619140625,445.93017578125 +0.8666667938232422,919.8909301757812,461.22021484375 +0.8833332061767578,930.740966796875,476.68994140625 +0.8999996185302734,941.4009399414062,492.34033203125 +0.9166669845581055,951.8809814453125,508.16015625 +0.9333333969116211,962.1809692382812,524.16015625 +0.9499998092651367,972.3009643554688,540.320068359375 +0.9666671752929688,982.2509765625,556.650146484375 +0.9833335876464844,992.0309448242188,573.150146484375 +1.0,1001.6509399414062,589.81005859375 +1.0166664123535156,1011.0909423828125,606.630126953125 +1.0333337783813477,1020.3709716796875,623.60009765625 +1.0500001907348633,1029.5009765625,640.719970703125 +1.066666603088379,1038.48095703125,657.989990234375 +1.0833330154418945,1047.3009033203125,675.41015625 +1.1000003814697266,1055.970947265625,692.969970703125 +1.1166667938232422,1064.490966796875,710.68017578125 +1.1333332061767578,1072.8709716796875,728.530029296875 +1.1499996185302734,1081.1009521484375,746.52001953125 +1.1666669845581055,1089.19091796875,764.64013671875 +1.183333396911621,1097.1409912109375,782.89013671875 +1.1999998092651367,1104.9609375,801.280029296875 +1.2166671752929688,1112.6510009765625,819.7900390625 +1.2333335876464844,1120.200927734375,838.43017578125 +1.25,1127.6309814453125,857.2001953125 +1.2666664123535156,1134.930908203125,876.090087890625 +1.2833337783813477,1142.1009521484375,895.10009765625 +1.3000001907348633,1149.1510009765625,914.22998046875 +1.3249998092651367,1159.5009765625,943.130126953125 +1.3416671752929688,1166.2509765625,962.5400390625 +1.3583335876464844,1172.8909912109375,982.06005859375 +1.375,1179.4208984375,1001.690185546875 +1.3916664123535156,1185.8309326171875,1021.43017578125 +1.4083337783813477,1192.1409912109375,1041.280029296875 +1.4250001907348633,1198.3409423828125,1061.22998046875 +1.441666603088379,1204.430908203125,1081.280029296875 +1.4583330154418945,1210.4208984375,1101.43017578125 +1.4750003814697266,1216.3109130859375,1121.68017578125 +1.4916667938232422,1222.1009521484375,1142.02001953125 +1.5083332061767578,1227.7908935546875,1162.4599609375 +1.5249996185302734,1233.3809814453125,1183.0 +1.5416669845581055,1238.8809814453125,1203.6201171875 +1.558333396911621,1244.281005859375,1224.340087890625 +1.5749998092651367,1249.5909423828125,1245.14013671875 +1.5916671752929688,1254.8109130859375,1266.0400390625 +1.6083335876464844,1259.94091796875,1287.02001953125 +1.625,1264.98095703125,1308.080078125 +1.6416664123535156,1269.94091796875,1329.219970703125 +1.6583337783813477,1274.8109130859375,1350.440185546875 +1.6750001907348633,1279.5909423828125,1371.739990234375 +1.691666603088379,1284.2908935546875,1393.1201171875 +1.7083330154418945,1288.910888671875,1414.580078125 +1.7250003814697266,1293.450927734375,1436.1201171875 +1.7416667938232422,1297.910888671875,1457.739990234375 +1.7583332061767578,1302.3109130859375,1479.420166015625 +1.7749996185302734,1306.6309814453125,1501.18017578125 +1.7916669845581055,1310.8709716796875,1523.010009765625 +1.808333396911621,1315.0408935546875,1544.91015625 +1.8249998092651367,1319.1409912109375,1566.880126953125 +1.8416671752929688,1323.1708984375,1588.920166015625 +1.8583335876464844,1327.1309814453125,1611.02001953125 +1.875,1331.031005859375,1633.190185546875 +1.8916664123535156,1334.8609619140625,1655.420166015625 +1.9083337783813477,1338.6209716796875,1677.719970703125 +1.9250001907348633,1342.3209228515625,1700.080078125 +1.941666603088379,1345.9609375,1722.5 +1.9583330154418945,1349.531005859375,1744.97998046875 +1.9750003814697266,1353.0408935546875,1767.52001953125 +1.9916667938232422,1356.490966796875,1790.110107421875 +2.008333206176758,1359.8809814453125,1812.760009765625 +2.0249996185302734,1363.2109375,1835.469970703125 +2.0416669845581055,1366.490966796875,1858.22998046875 +2.058333396911621,1369.7109375,1881.0400390625 +2.0749998092651367,1372.8809814453125,1903.91015625 +2.0916671752929688,1375.990966796875,1926.830078125 +2.1083335876464844,1379.0509033203125,1949.800048828125 +2.125,1382.0609130859375,1972.830078125 +2.1416664123535156,1385.010986328125,1995.900146484375 +2.1583337783813477,1387.9208984375,2019.02001953125 +2.1750001907348633,1390.77099609375,2042.18994140625 +2.191666603088379,1393.5809326171875,2065.400146484375 +2.2083330154418945,1396.3409423828125,2088.66015625 +2.2250003814697266,1399.0509033203125,2111.969970703125 +2.241666793823242,1401.69091796875,2135.320068359375 +2.258333206176758,1403.8009033203125,2158.7099609375 +2.2749996185302734,1405.3909912109375,2182.130126953125 +2.2916669845581055,1406.5809326171875,2205.570068359375 +2.308333396911621,1407.470947265625,2229.030029296875 +2.3249998092651367,1408.1409912109375,2252.489990234375 +2.3416671752929688,1408.6409912109375,2275.969970703125 +2.3583335876464844,1409.02099609375,2299.449951171875 +2.375,1409.3009033203125,2322.929931640625 +2.3916664123535156,1409.52099609375,2346.429931640625 +2.4083337783813477,1409.680908203125,2369.929931640625 +2.4250001907348633,1409.8109130859375,2393.429931640625 +2.441666603088379,1409.9010009765625,2416.929931640625 +2.4583330154418945,1409.970947265625,2440.429931640625 +2.4750003814697266,1410.02099609375,2463.929931640625 \ No newline at end of file diff --git a/src/util/boost_acceleration.txt b/src/util/boost_acceleration.txt new file mode 100644 index 0000000..8f8e458 --- /dev/null +++ b/src/util/boost_acceleration.txt @@ -0,0 +1,103 @@ +0.0,0.0,0.0 +0.016666412353515625,43.00764765268562,0.5374943827180316 +0.03333282470703125,85.27849016884997,1.7819228992694198 +0.049999237060546875,126.81394098520472,3.7336308384274672 +0.0666666030883789,167.64229041516953,6.363961030678928 +0.08333301544189453,207.77767822065763,9.6591891164428 +0.09999942779541016,247.2342279791924,13.618969828011853 +0.11666584014892578,286.0119612699603,18.229319851958284 +0.1333332061767578,324.1250340392644,23.47608324218658 +0.14999961853027344,361.5875374958483,29.34493141924172 +0.16666603088378906,398.4065928341092,35.83629596664811 +0.1833324432373047,434.5963127109964,42.92859828146851 +0.19999980926513672,470.163775168645,50.621837108024934 +0.21666622161865234,505.1372920994572,58.89503817888214 +0.23333263397216797,539.5168203449174,67.74802883307527 +0.25,573.3023814838411,77.16665312491072 +0.2666664123535156,606.5081098840631,87.13684144620449 +0.28333282470703125,639.1481614907888,97.65859376490442 +0.2999992370605469,671.2366490928828,108.71775416023387 +0.3166666030883789,702.7736374271519,120.3001666609025 +0.33333301544189453,733.7731313858027,132.39167534146918 +0.34999942779541016,764.2494163902325,144.99228018175978 +0.3666658401489258,794.1954145611222,158.10206751199178 +0.3833332061767578,823.625195336956,171.70688138445362 +0.39999961853027344,852.5670708554302,185.79239321164124 +0.41666603088378906,881.0069280275319,200.35886194743838 +0.4333324432373047,908.965914611802,215.39187269226483 +0.4499998092651367,936.4440737634242,230.89159808534984 +0.46666622161865234,963.4555614287252,246.84396849394528 +0.48333263397216797,990.01449039843,263.23474165723405 +0.5,1016.1208606687416,280.0638312513733 +0.5166664123535156,1041.7747154007673,297.31716765373824 +0.5333328247070312,1066.9901242239603,314.99483717867383 +0.5499992370605469,1091.7742084275621,333.08268387982463 +0.5666666030883789,1116.141123637369,351.5947773884452 +0.5833330154418945,1140.0837488827951,370.50271948990405 +0.5999994277954102,1163.6303960564699,389.8068554579696 +0.6166658401489258,1186.7669523735822,409.4856062832123 +0.6333332061767578,1209.5074874594986,429.5532138577795 +0.6499996185302734,1231.8662004219514,449.98861708411994 +0.6666660308837891,1253.828978471962,470.7918159728739 +0.6833324432373047,1275.4240040279465,491.96255156029065 +0.6999998092651367,1296.6513202453873,513.4869268532884 +0.7166662216186523,1317.5251262318113,535.3506132768902 +0.733332633972168,1338.0312228815735,557.5536971452981 +0.75,1358.1979220872593,580.0962647731118 +0.7666664123535156,1378.025180691369,602.9783161686228 +0.7833328247070312,1397.5130418496062,626.192600912765 +0.7999992370605469,1415.798812849388,649.7108930277763 +0.8166666030883789,1432.316860055867,673.5121314417574 +0.8333330154418945,1448.8348641031853,697.5962298294531 +0.8499994277954102,1465.3528681489101,721.956024146598 +0.8666658401489258,1481.8708721980347,746.5775305865834 +0.8833332061767578,1498.388876243933,771.4818106866253 +0.8999996185302734,1514.9068802931533,796.6690370801418 +0.9166660308837891,1531.4249706575538,822.1248811872807 +0.9333324432373047,1547.9428883885237,847.8493430079805 +0.9499998092651367,1564.4609787530442,873.8567511219721 +0.9666662216186523,1580.9789827993325,900.1469328957153 +0.983332633972168,1597.4969868487628,926.698827286483 +1.0,1614.0149908951919,953.5264171136503 +1.0166664123535156,1630.5329949446964,980.6369532342433 +1.0333328247070312,1647.0510853094852,1008.0302630147169 +1.0499992370605469,1663.5690030408316,1035.6781208762318 +1.066666603088379,1680.087093405717,1063.6088387172995 +1.0833330154418945,1696.6050974524646,1091.822416537646 +1.0999994277954102,1713.1231015021333,1120.3046983852773 +1.1166658401489258,1729.6411055489966,1149.055597946279 +1.1333332061767578,1746.1591095987237,1178.0894438004054 +1.1499996185302734,1762.6771136456969,1207.4060633141346 +1.166666030883789,1779.1951176954785,1236.9914731746321 +1.1833324432373047,1795.7132080606605,1266.845500748379 +1.1999998092651367,1812.2311257923886,1296.9823882960416 +1.2166662216186523,1828.7492161576452,1327.4020495079078 +1.233332633972168,1845.2672202048693,1358.0835096105764 +1.25,1861.7852242547729,1389.0405788727621 +1.2666664123535156,1878.303228302089,1420.2805944333088 +1.2833328247070312,1894.8212323520363,1451.8033836482684 +1.2999992370605469,1911.3392363994399,1483.5808072687178 +1.316666603088379,1927.8572404494282,1515.6409613865883 +1.3333330154418945,1944.3753308149162,1547.9840186403153 +1.3499994277954102,1960.8932485469425,1580.6029012626748 +1.3666658401489258,1977.411338912489,1613.4833665720246 +1.3833332061767578,1993.9293429600946,1646.6466486980312 +1.3999996185302734,2010.447347010175,1680.0928339600403 +1.416666030883789,2026.965351057855,1713.8076369352905 +1.4333324432373047,2043.483355107969,1747.7912302572531 +1.4499998092651367,2060.00135915572,1782.0575972387146 +1.4666662216186523,2076.519363205865,1816.606867355953 +1.483332633972168,2093.037453571596,1851.4177637303803 +1.5,2109.555371303859,1886.5043555844404 +1.5166664123535156,2126.0734616696373,1921.8738074173764 +1.5333328247070312,2142.591465717553,1957.5261192240416 +1.5499992370605469,2159.1094697677695,1993.433022274007 +1.566666603088379,2175.6274738157467,2029.622785302613 +1.5833330154418945,2192.1454778659886,2066.0953219858793 +1.5999994277954102,2208.6634819140245,2102.843640876819 +1.6166658401489258,2225.1814859642905,2139.8536287784473 +1.6333332061767578,2241.6995763302175,2177.1464550752225 +1.6499996185302734,2258.2174940626724,2214.7220981911396 +1.666666030883789,2274.7355844286376,2252.566466915552 +1.6833324432373047,2291.253588476811,2290.67953966989 +1.6999998092651367,2299.9934179346674,2329.0754076622866 \ No newline at end of file diff --git a/src/util/constants.py b/src/util/constants.py new file mode 100644 index 0000000..60aa9bb --- /dev/null +++ b/src/util/constants.py @@ -0,0 +1,41 @@ + + +from enum import Enum + + +class DIRECTION(Enum): + NONE = 0 + FORWARD = 1 + BACKWARD = 2 + LEFT = 3 + RIGHT = 4 + UP = 5 + DOWN = 6 + + +class AERIALTAKEOFF(Enum): + NONE = 0 + QUICK = 1 + QUICK_SINGLE = 2 + SINGLE = 3 + DOUBLE = 4 + FALL = 5 + + +class KICKOFFTYPE(Enum): + NONE = 0 + OFF_CENTER = 1 + DIAGONAL = 2 + STRAIGHT = 3 + + +OCTANE_LENGTH = 82.5 +BALL_RADIUS = 92.75 +BOOST_CONSUMPTION_RATE = 33.3 +INV_BOOST_CONSUMPTION_RATE = 1 / BOOST_CONSUMPTION_RATE +BOOST_ACCELERATION = 1066 +CAR_BRAKING = 3500 +ZERO_THROTTLE = 525 +MAX_CAR_VEL = 2300 +INV_MAX_CAR_VEL = 1 / MAX_CAR_VEL +TOP_DRIVE_VEL = 1410 diff --git a/src/util/kinematics.py b/src/util/kinematics.py new file mode 100644 index 0000000..f18c0e6 --- /dev/null +++ b/src/util/kinematics.py @@ -0,0 +1,191 @@ + +from __future__ import annotations + +import math +import os +import sys + +from util.constants import INV_BOOST_CONSUMPTION_RATE, TOP_DRIVE_VEL, BOOST_CONSUMPTION_RATE + +from typing import List + +epsilon = sys.float_info.epsilon + + +def sign(n: float) -> float: + return -1 if n < 0 else 1 + + +def not_zero(n: float) -> float: + return n if abs(n) > epsilon else epsilon * sign(n) + + +# A single point of a loop-up table. +class AccelerationModelPoint: + def __init__(self, time: float, velocity: float, position: float): + self.time = time + self.velocity = velocity + self.position = position + + def copy(self) -> AccelerationModelPoint: + return AccelerationModelPoint(self.time, self.velocity, self.position) + + @staticmethod + def create(line: str) -> AccelerationModelPoint: + nums = line.split(",") + return AccelerationModelPoint(float(nums[0]), float(nums[1]), float(nums[2])) + + +# Creates a lookup table from a file. +def create_lut(file_name: str) -> List[AccelerationModelPoint]: + cwd = os.getcwd() + _path = os.path.dirname(os.path.realpath(__file__)) + os.chdir(_path) + + lut_f = open(file_name, "r") + lut = [] + for line in lut_f.readlines(): + if len(line) > 0: + a = AccelerationModelPoint.create(line) + lut.append(a) + + os.chdir(cwd) + + return lut + + +# Binary search time +def get_time(arr: List[AccelerationModelPoint], val: float, s: int = 0, e: int = -1) -> AccelerationModelPoint: + if e < 0: + e = len(arr) + length = e - s + div = s + int(length * 0.5) + if length == 1: + return arr[div] + elif arr[div].time > val: + return get_time(arr, val, s, div) + else: + return get_time(arr, val, div, e) + + +# Binary search velocity +def get_velocity(arr: List[AccelerationModelPoint], val: float, s: int = 0, e: int = -1) -> AccelerationModelPoint: + if e < 0: + e = len(arr) + length = e - s + div = s + math.floor(length * 0.5) + if length == 1: + return arr[div] + elif arr[div].velocity > val: + return get_velocity(arr, val, s, div) + else: + return get_velocity(arr, val, div, e) + + +# Binary search position +def get_position(arr: List[AccelerationModelPoint], val: float, s: int = 0, e: int = -1) -> AccelerationModelPoint: + if e < 0: + e = len(arr) + length = e - s + div = s + math.floor(length * 0.5) + if length == 1: + return arr[div] + elif arr[div].position > val: + return get_position(arr, val, s, div) + else: + return get_position(arr, val, div, e) + + +# Lookup tables. I could have put these in constants, but that would create some awkward imports. +ACCELERATION_LUT = create_lut("util/acceleration.txt") +BOOST_ACCELERATION_LUT = create_lut("util/boost_acceleration.txt") + + +# Returned by Kinematics1D functions. +class DriveManeuver: + def __init__(self, dist: float, vel: float, time: float, boost: float): + self.distance = dist + self.velocity = vel + self.time = time + self.boost = boost + + +DriveManeuver.failed = DriveManeuver(0, 0, 0, 0) + + +# Simple drive without flips or wave dashes. +# PLEASE DO NOT ASK ME HOW THIS WORKS! I DON'T HAVE THE BRAIN SPACE TO ACTUALLY FIGURE IT OUT AND I DON'T REMEMBER WHAT +# I WAS THINKING AT THE TIME I WROTE THIS! +class Kinematics1D: + + @staticmethod + def from_length(length: float, initial_v: float = 0, boost: float = 0) -> DriveManeuver: + no_boost_time = boost * INV_BOOST_CONSUMPTION_RATE + initial_conditions = get_velocity(BOOST_ACCELERATION_LUT, initial_v) + no_boost = get_time(BOOST_ACCELERATION_LUT, initial_conditions.time + no_boost_time) + + # assume we have boost the entire time + end_loc_1 = get_position(BOOST_ACCELERATION_LUT, initial_conditions.position + length) + + if end_loc_1.time > no_boost.time: + if no_boost.velocity > TOP_DRIVE_VEL: + extra_time = (length - (no_boost.position - initial_conditions.position)) / no_boost.velocity + return DriveManeuver(length, no_boost.velocity, no_boost.time + extra_time, boost) + else: + initial_no_boost = get_velocity(ACCELERATION_LUT, no_boost.velocity) + final_no_boost = get_position(ACCELERATION_LUT, initial_no_boost.position + length - ( + no_boost.position - initial_conditions.position)) + + return DriveManeuver(length, final_no_boost.velocity, no_boost.time - initial_conditions.time + + initial_no_boost.time - final_no_boost.time, boost) + else: + extra_time = (length - (end_loc_1.position - initial_conditions.position)) / not_zero(end_loc_1.velocity) + time = end_loc_1.time - initial_conditions.time + return DriveManeuver(length, end_loc_1.velocity, time + extra_time, time * BOOST_CONSUMPTION_RATE) + + @staticmethod + def from_velocity(vel: float, initial_v: float = 0, boost: float = 0) -> DriveManeuver: + no_boost_time = boost * INV_BOOST_CONSUMPTION_RATE + initial_conditions = get_velocity(BOOST_ACCELERATION_LUT, initial_v) + no_boost = get_time(BOOST_ACCELERATION_LUT, initial_conditions.time + no_boost_time) + + end_loc_1 = get_velocity(BOOST_ACCELERATION_LUT, vel) + + if end_loc_1.time > no_boost.time: + if vel > TOP_DRIVE_VEL: + return DriveManeuver.failed + else: + start_loc_2 = get_velocity(ACCELERATION_LUT, no_boost.velocity) + end_loc_2 = get_velocity(ACCELERATION_LUT, vel) + length = end_loc_1.position - initial_conditions.position + end_loc_2.position - start_loc_2.position + time = end_loc_1.time - initial_conditions.time + end_loc_2.time - start_loc_2.time + return DriveManeuver(length, vel, time, boost) + else: + time = end_loc_1.time - initial_conditions.time + return DriveManeuver(end_loc_1.position - initial_conditions.position, vel, time, + time * BOOST_CONSUMPTION_RATE) + + @staticmethod + def from_time(time: float, initial_v: float = 0, boost: float = 0) -> DriveManeuver: + + initial_conditions = get_velocity(BOOST_ACCELERATION_LUT, initial_v) + + no_boost_time = boost * INV_BOOST_CONSUMPTION_RATE + + no_boost = get_time(BOOST_ACCELERATION_LUT, initial_conditions.time + no_boost_time) + + if no_boost.velocity > TOP_DRIVE_VEL: + extra = no_boost.velocity * (time - (no_boost.time - initial_conditions.time)) + return DriveManeuver(no_boost.position - initial_conditions.position + extra, no_boost.velocity, + time, (no_boost.time - initial_conditions.time) * BOOST_CONSUMPTION_RATE) + else: + start_loc_2 = get_velocity(ACCELERATION_LUT, no_boost.velocity) + end_loc_2 = get_time( + ACCELERATION_LUT, start_loc_2.time + time - (no_boost.time - initial_conditions.time)) + length = ( + no_boost.position - initial_conditions.position + end_loc_2.position - start_loc_2.position) + + time_taken = (end_loc_2.time - start_loc_2.time + no_boost.time - initial_conditions.time) + extra = end_loc_2.velocity * (time - time_taken) + + return DriveManeuver(length + extra, end_loc_2.velocity, time, boost)