def execute(s, drone):
drone.role.timer += s.dt
if s.strategy == Strategy.KICKOFF:
distance = np.linalg.norm(drone.pos - s.ball.pos)
time_to_hit = distance / np.linalg.norm(drone.vel)
if time_to_hit <= KO_DODGE_TIME:
drone.controller = Dodge(drone, local(drone.orient_m, drone.pos, s.ball.pos))
drone.role.timer = 0
if drone.controller is None:
if drone.kickoff == 'r_back' or drone.kickoff == 'l_back':
drone.controller = AB_Control(drone, a3l([0.0, -2816.0, 70.0])*team_sign(s.team))
drone.role.timer = 0
#drone.controller = LINE_PD_Control(drone, a3l([0,-6000,0])*team_sign(s.team), s.ball.pos)
else:
drone.controller = AB_Control(drone, s.ball.pos)
elif isinstance(drone.controller,AB_Control):
if drone.role.timer >= KO_PAD_TIME:
AB_Control(drone, s.ball.pos)
else:
drone.controller = AB_Control(drone, s.ball.pos)
if drone.controller is not None:
if isinstance(drone.controller,Dodge):
drone.controller.run(drone.role.timer)
else:
drone.controller.run()
def run(self, agent, player, target):
"""Runs the controller.
Arguments:
agent {BaseAgent} -- The agent.
player {Car} -- Car object for which to generate controls.
target {np.ndarray} -- World coordinates of where we want to hit the ball.
"""
# Calculate drone's distance to ball.
distance = np.linalg.norm(agent.ball.pos - agent.pos)
# Find directions based on where we want to hit the ball.
direction_to_hit = normalise(target - agent.ball.pos)
perpendicular_to_hit = np.cross(direction_to_hit, a3l([0, 0, 1]))
# Calculating component lengths and multiplying with direction.
perpendicular_component = perpendicular_to_hit * cap(
np.dot(perpendicular_to_hit, agent.ball.pos),
-distance * self.PERP_DIST_COEFF, distance * self.PERP_DIST_COEFF)
in_direction_component = -direction_to_hit * distance * self.DIRECT_DIST_COEFF
# Combine components to get a drive target.
drive_target = agent.ball.pos + in_direction_component + perpendicular_component
super().run(agent, player, drive_target)
def run(self, hive, drone, target):
"""Runs the controller.
Arguments:
hive {Hivemind} -- The hivemind.
drone {Drone} -- Drone being controlled.
target {np.ndarray} -- World coordinates of where we want to hit the ball.
"""
# Calculate drone's distance to ball.
distance = np.linalg.norm(hive.ball.pos - drone.pos)
# Find directions based on where we want to hit the ball.
direction_to_hit = normalise(target - hive.ball.pos)
perpendicular_to_hit = np.cross(direction_to_hit, a3l([0, 0, 1]))
# Calculating component lengths and multiplying with direction.
perpendicular_component = perpendicular_to_hit * cap(
np.dot(perpendicular_to_hit, hive.ball.pos),
-distance * self.PERP_DIST_COEFF, distance * self.PERP_DIST_COEFF)
in_direction_component = -direction_to_hit * distance * self.DIRECT_DIST_COEFF
# Combine components to get a drive target.
drive_target = hive.ball.pos + in_direction_component + perpendicular_component
super().run(hive, drone, drive_target)
def drift_render(self):
"""Renders information on the screen."""
self.renderer.begin_rendering()
car = self.agent
# Calculates a vector from the car to the position 1000 uu in the front direction of the car.
front = world(car.orient_m, car.pos, a3l([1000, 0, 0])) - car.pos
# Calculated the velocity vector in local coordinates.
local_v = local(car.orient_m, a3l([0, 0, 0]), car.vel)
# Uses two methods to calculate angle. (The were just for testing which produces better results.)
angle2D = np.arctan2(local_v[1], local_v[0])
angle_pure = angle_between_vectors(car.vel, front)
# Rendering front vector and velocity vector.
self.renderer.draw_line_3d(car.pos, car.pos + front,
self.renderer.yellow())
self.renderer.draw_line_3d(car.pos, car.pos + car.vel,
self.renderer.cyan())
# Rendering angles.
self.renderer.draw_string_2d(10, 10, 2, 2,
"angle 2D: {}".format(angle2D),
self.renderer.pink())
self.renderer.draw_string_2d(10, 50, 2, 2,
"angle 3D: {}".format(angle_pure),
self.renderer.pink())
# Rendering position and velocity.
self.renderer.draw_string_2d(10, 110, 2, 2, "pos: {}".format(car.pos),
self.renderer.cyan())
self.renderer.draw_string_2d(10, 150, 2, 2, "vel: {}".format(car.vel),
self.renderer.cyan())
# Rendering test related stuff.
self.renderer.draw_string_2d(
10, 210, 2, 2, "test: {}/{}".format(self.count, self.tests),
self.renderer.white())
self.renderer.draw_string_2d(10, 250, 2, 2,
"timer: {}".format(self.timer),
self.renderer.white())
self.renderer.end_rendering()
def render_role(hive, rndr, drone):
"""Renders roles above the drones.
Arguments:
hive {Hivemind} -- The hivemind.
rndr {?} -- The renderer.
drone {Drone} -- The drone who's role is being rendered.
"""
# Rendering role names above drones.
above = drone.pos + a3l([0, 0, 100])
rndr.begin_rendering(f'role_{hive.team}_{drone.index}')
rndr.draw_string_3d(above, 1, 1, drone.role.name, rndr.cyan())
rndr.end_rendering()
def execute(self, hive, drone):
if hive.strategy == Strategy.KICKOFF:
# Find estimated time to hit the ball.
distance = np.linalg.norm(drone.pos - hive.ball.pos)
time_to_hit = distance / np.linalg.norm(
drone.vel) if np.linalg.norm(drone.vel) > 0 else 10
# If the estimated time to hit is small, dodge.
if time_to_hit <= self.KO_DODGE_TIME:
drone.controller = Dodge()
# Use AngleBased controller if none is set.
if drone.controller is None:
drone.controller = AngleBased()
# If using AngleBased controller.
elif isinstance(drone.controller, AngleBased):
# Drive towards the pad on r_left or l_left kickoffs.
if drone.kickoff in (
'r_back',
'l_back') and drone.role.timer <= self.KO_PAD_TIME:
drone.controller.run(
hive, drone,
a3l([0.0, -2816.0, 70.0]) * team_sign(hive.team))
# Drive towards the ball.
else:
drone.controller.run(hive, drone, hive.ball.pos)
# If using Dodge controller.
elif isinstance(drone.controller, Dodge):
drone.controller.run(hive, drone, hive.ball.pos)
else:
# Else
if drone.controller is None:
drone.controller = TargetShot()
elif isinstance(drone.controller, TargetShot):
drone.controller.run(hive, drone,
goal_pos * team_sign((hive.team + 1) % 2))
super().execute(hive)
def aerial_control_predict(current: state, roll: float, pitch: float,
yaw: float, dt: float):
T = np.zeros((3, 3))
T[0, 0] = T_r
T[1, 1] = T_p
T[2, 2] = T_y
D = np.zeros((3, 3))
D[0, 0] = D_r
D[1, 1] = D_p * (1 - abs(pitch))
D[2, 2] = D_y * (1 - abs(yaw))
# Compute the net torque on the car.
tau = np.dot(D, np.dot(current.theta.T, current.omega))
tau += np.dot(T, a3l([roll, pitch, yaw]))
tau = np.dot(T, tau)
# Use the torque to get the update angular velocity.
omega_next = current.theta + tau * dt
# Prevent the angular velocity from exceeding a threshold.
omega_next *= min(1.0, omega_max / np.linalg.norm(current.theta))
# Compute the average angular velocity for this step
omega_avg = 0.5 * (current.theta + omega_next)
phi: float = np.linalg.norm(omega_avg) * dt
omega_dt = np.array([[0.0, -omega_avg[2] * dt, omega_avg[1] * dt],
[omega_avg[2] * dt, 0.0, -omega_avg[0] * dt],
[-omega_avg[1] * dt, omega_avg[0] * dt, 0.0]])
R = np.identity(3)
R += (np.sin(phi) / phi) * omega_dt
R += (1.0 - np.cos(phi)) / (phi * phi) * np.dot(omega_dt, omega_dt)
return state(omega_next, np.dot(R, current.theta))
def test_path(detail):
# Path definition.
a = a3l([3072, -4096, 0])
b = a3l([3072, 2300, 0])
c = a3l([1072, 2300, 0])
part1 = straight(a, b, detail)
part2 = arc(c, 2000, 0, 3 * np.pi / 4, detail)
d = part2[-1]
e = d + 1500 * normalise(part2[-1] - part2[-2])
f = a3l([0, 1024, 0])
g = a3l([0, 0, 0])
part3 = bezier_cubic(d, e, f, g, detail)
h = a3l([-512, 0, 0])
part4 = arc(h, 512, 0, -np.pi, detail)
i = part4[-1]
j = i + 1500 * normalise(part4[-1] - part4[-2])
k = a3l([-2800, 1200, 0])
l = a3l([-3500, 500, 0])
part5 = bezier_cubic(i, j, k, l, detail)
m = 2 * l - k
n = a3l([-3072, -1200, 0])
o = a3l([-3072, -2000, 0])
p = a3l([-3072, -4096, 0])
part6 = bezier_cubic(l, m, n, o, detail)
part7 = straight(o, p, detail)
# Connect all the parts.
path = np.concatenate((part1, part2, part3, part4, part5, part6, part7))
return path
import numpy as np
from utils import a3l, team_sign, local
from control import AB_Control, GK_Control, LINE_PD_Control, Dodge
#PARAMETERS:
KO_DODGE_TIME = 0.35
KO_PAD_TIME = 0.1
kickoff_positions = {
'r_corner' : a3l([-1952, -2464, 0]),
'l_corner' : a3l([1952, -2464, 0]),
'r_back' : a3l([-256.0, -3840, 0]),
'l_back' : a3l([256.0, -3840, 0]),
'centre' : a3l([0.0, -4608, 0])
}
goalie_positions = {
'centre' : a3l([0.0, -4608, 0]),
'r_back' : a3l([-256.0, -3840, 0]),
'l_back' : a3l([256.0, -3840, 0])
}
best_boost = {
'r_corner' : a3l([-3584.0, 0.0, 73.0]),
'l_corner' : a3l([3584.0, 0.0, 73.0]),
'r_back' : a3l([-3072.0, -4096.0, 73.0]),
'l_back' : a3l([3072.0, -4096.0, 73.0])
}