def run(self, drone: CarObject, agent: MyHivemind):
car_to_ball, distance = (agent.ball.location - drone.location).normalize(True)
ball_to_target = (self.target - agent.ball.location).normalize()
relative_velocity = car_to_ball.dot(drone.velocity - agent.ball.velocity)
if relative_velocity != 0.0:
eta = cap(distance / cap(relative_velocity, 400, 2300), 0.0, 1.5)
else:
eta = 1.5
# If we are approaching the ball from the wrong side the car will try to only hit the very edge of the ball
left_vector = car_to_ball.cross((0, 0, 1))
right_vector = car_to_ball.cross((0, 0, -1))
target_vector = -ball_to_target.clamp(left_vector, right_vector)
final_target = agent.ball.location + (target_vector * (distance / 2))
# Some adjustment to the final target to ensure we don't try to drive through any goalposts to reach it
if abs(drone.location[1]) > 5150:
final_target[0] = cap(final_target[0], -750, 750)
agent.line(final_target - Vector3(0, 0, 100), final_target + Vector3(0, 0, 100), [255, 255, 255])
angles = defaultPD(drone, drone.local(final_target - drone.location))
defaultThrottle(drone, 2300 if distance > 1600 else 2300 - cap(1600 * abs(angles[1]), 0, 2050))
drone.controller.boost = False if drone.airborne or abs(angles[1]) > 0.3 else drone.controller.boost
drone.controller.handbrake = True if abs(angles[1]) > 2.3 else drone.controller.handbrake
if abs(angles[1]) < 0.05 and (eta < 0.45 or distance < 150):
drone.pop()
drone.push(Flip(drone.local(car_to_ball)))
def aerial_input_generate(omega_start: np.ndarray, omega_end: np.ndarray,
theta_start: np.ndarray, dt: float):
# Net torque in world coordinates.
tau = (omega_end - omega_start) / dt
# Ner torque in local coordinates.
tau = np.dot(theta_start.T, tau)
# Beggining-step angular velocity, in local coordinates.
omega_local = np.dot(theta_start.T, omega_start)
rhs = np.zeros(3)
rhs[0] = tau[0] - D_r * omega_local[0]
rhs[1] = tau[1] - D_p * omega_local[1]
rhs[2] = tau[2] - D_y * omega_local[2]
# User inputs: roll, pitch, yaw.
u = np.zeros(3)
u[0] = rhs[0] / T_r
u[1] = rhs[1] / (T_p + np.sign(rhs[1]) * omega_local[1] * D_p)
u[2] = rhs[2] / (T_y - np.sign(rhs[2]) * omega_local[2] * D_y)
# Ensure that values are between -1 and 1.
u[0] = cap(u[0], -1, 1)
u[1] = cap(u[1], -1, 1)
u[2] = cap(u[2], -1, 1)
return u
def run(self):
local_target = local(self.drone.orient_m, self.drone.pos, self.target)
angle = np.arctan2(local_target[1], local_target[0])
if abs(angle) > GK_MIN_ANGLE or np.pi - abs(angle) > GK_MIN_ANGLE:
self.drone.ctrl.steer = 1 if angle > 0 else -1
distance = abs(local_target[0])
if abs(angle) > GK_BACK_ANGLE:
self.drone.ctrl.throttle = cap(-1 * GK_THROTTLE * distance, -1, 1)
else:
self.drone.ctrl.throttle = cap(1 * GK_THROTTLE * distance, -1, 1)
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 _readFuzzingFile(self):
"""
Read the fuzzed data
The fuzzer has generated a new file with fuzzed data.
Read it, then remove that file.
Also remove the original input file.
"""
file = open(self.fuzzingOutFile, "r")
data = file.read()
file.close()
logging.debug("Read fuzzing data: " + utils.cap(data, 64))
self.choice["data"] = data
self.choice["isFuzzed"] = True
try:
os.remove(self.fuzzingInFile)
except:
print("Failed to remove file %s!" % self.fuzzingInFile)
# keep fuzzed files for debugging purposes
if "keep_temp" in self.config and self.config["keep_temp"]:
return True
try:
os.remove(self.fuzzingOutFile)
except:
print("Failed to remove file %s!" % self.fuzzingOutFile)
return True
def _readDataFromFile(self):
"""
Read the mutated data.
The fuzzer has generated a new file with fuzzed data.
Read it, then remove that file.
Also remove the original input file.
"""
file = open(self.fuzzingOutFile, "r")
data = file.read()
file.close()
logging.debug("Read fuzzing data: " + utils.cap(data, 64))
try:
os.remove(self.fuzzingInFile)
except:
logging.warn("Failed to remove file %s!" % self.fuzzingInFile)
# keep fuzzed files for debugging purposes
if "keep_temp" in self.config and self.config["keep_temp"]:
pass
else:
try:
os.remove(self.fuzzingOutFile)
except:
logging.warn("Failed to remove file %s!" % self.fuzzingOutFile)
return data
def calc_value(self, value=0):
if self.timeout:
if time.time() - self.start_time > self.timeout:
self.start_time = time.time()
if self.initial:
value = self.initial
nval = self.calculate()
return cap(value + nval)
def is_viable(self, drone: CarObject, time: float):
T = self.intercept_time - time
xf = drone.location + drone.velocity * T + 0.5 * gravity * T ** 2
vf = drone.velocity + gravity * T
if not drone.airborne:
vf += drone.up * (2 * jump_speed + jump_acc * jump_max_duration)
xf += drone.up * (jump_speed * (2 * T - jump_max_duration) + jump_acc * (
T * jump_max_duration - 0.5 * jump_max_duration ** 2))
delta_x = self.ball_location - xf
f = delta_x.normalize()
phi = f.angle3D(drone.forward)
turn_time = 0.7 * (2 * math.sqrt(phi / 9))
tau1 = turn_time * cap(1 - 0.3 / phi, 0, 1)
required_acc = (2 * delta_x.magnitude()) / ((T - tau1) ** 2)
ratio = required_acc / boost_accel
tau2 = T - (T - tau1) * math.sqrt(1 - cap(ratio, 0, 1))
velocity_estimate = vf + boost_accel * (tau2 - tau1) * f
boos_estimate = (tau2 - tau1) * 30
enough_boost = boos_estimate < 0.95 * drone.boost
enough_time = abs(ratio) < 0.9
return velocity_estimate.magnitude() < 0.9 * max_speed and enough_boost and enough_time
def run(self):
line_v = self.line[1] - self.line[0]
theta = angle_between_vectors(line_v, self.line[1] - self.drone.pos)
distance = np.linalg.norm(self.line[1] - self.drone.pos)
print("theta", theta)
phi = angle_between_vectors(line_v, self.drone.vel)
print("phi", phi)
self.drone.ctrl.throttle = 1
self.drone.ctrl.steer = cap(
LINE_PD_ALPHA * (np.sin(theta) * distance) + LINE_PD_BETA * phi,
-1, 1)
local_target = local(self.drone.orient_m, self.drone.pos, self.line[1])
angle = np.arctan2(local_target[1], local_target[0])
if abs(angle) < LINE_PD_BOOST_ANGLE:
self.drone.ctrl.boost = True
def run(self, drone: CarObject, agent: MyHivemind):
if self.boost is None:
drone.pop()
return
car_to_boost = self.boost.location - drone.location
distance_remaining = car_to_boost.flatten().magnitude()
agent.line(self.boost.location - Vector3(0, 0, 500), self.boost.location + Vector3(0, 0, 500), [0, 255, 0])
if self.target is not None:
vector = (self.target - self.boost.location).normalize()
side_of_vector = sign(vector.cross((0, 0, 1)).dot(car_to_boost))
car_to_boost_perp = car_to_boost.cross((0, 0, side_of_vector)).normalize()
adjustment = car_to_boost.angle2D(vector) * distance_remaining / 3.14
final_target = self.boost.location + (car_to_boost_perp * adjustment)
car_to_target = (self.target - drone.location).magnitude()
else:
adjustment = 9999
car_to_target = 0
final_target = self.boost.location
# Some adjustment to the final target to ensure it's inside the field and
# we don't try to dirve through any goalposts to reach it
if abs(drone.location[1]) > 5150:
final_target[0] = cap(final_target[0], -750, 750)
local_target = drone.local(final_target - drone.location)
angles = defaultPD(drone, local_target)
defaultThrottle(drone, 2300)
drone.controller.boost = self.boost.large if abs(angles[1]) < 0.3 else False
drone.controller.handbrake = True if abs(angles[1]) > 2.3 else drone.controller.handbrake
velocity = 1 + drone.velocity.magnitude()
if not self.boost.active or drone.boost >= 99.0 or distance_remaining < 350:
drone.pop()
elif drone.airborne:
drone.push(Recovery(self.target))
elif abs(angles[1]) < 0.05 and 600 < velocity < 2150 and (
distance_remaining / velocity > 2.0 or (adjustment < 90 and car_to_target / velocity > 2.0)):
drone.push(Flip(local_target))
def run(self, drone: CarObject, agent: MyHivemind):
target = agent.friend_goal.location + (agent.ball.location - agent.friend_goal.location) / 2
car_to_target = target - drone.location
distance_remaining = car_to_target.flatten().magnitude()
agent.line(target - Vector3(0, 0, 500), target + Vector3(0, 0, 500), [255, 0, 255])
if self.vector is not None:
# See commends for adjustment in jump_shot or aerial for explanation
side_of_vector = sign(self.vector.cross((0, 0, 1)).dot(car_to_target))
car_to_target_perp = car_to_target.cross((0, 0, side_of_vector)).normalize()
adjustment = car_to_target.angle(self.vector) * distance_remaining / 3.14
final_target = target + (car_to_target_perp * adjustment)
else:
final_target = target
# Some adjustment to the final target to ensure it's inside the field and
# we don't try to drive through any goalposts to reach it
if abs(drone.location[1]) > 5150:
final_target[0] = cap(final_target[0], -750, 750)
local_target = drone.local(final_target - drone.location)
angles = defaultPD(drone, local_target, self.direction)
defaultThrottle(drone, 2300, self.direction)
drone.controller.boost = False
drone.controller.handbrake = True if abs(angles[1]) > 2.3 else drone.controller.handbrake
velocity = 1 + drone.velocity.magnitude()
if distance_remaining < 350:
drone.pop()
elif abs(angles[1]) < 0.05 and 600 < velocity < 2150 and distance_remaining / velocity > 2.0:
drone.push(Flip(local_target))
# TODO Halfflip
# elif abs(angles[1]) > 2.8 and velocity < 200:
# agent.push(flip(local_target, True))
elif drone.airborne:
drone.push(Recovery(target))
def get_slices(drone: CarObject, cap_):
# Get the struct
struct = drone.ball_prediction_struct
# Make sure it isn't empty
if struct is None:
return
start_slice = 6
end_slices = None
# If we're shooting, crop the struct
if len(
drone.stack
) > 0 and drone.stack[0].__class__.__name__ != "ShortShot" and hasattr(
drone.stack[0], "intercept_time"):
# Get the time remaining
time_remaining = drone.stack[0].intercept_time - drone.time
if 0.5 > time_remaining >= 0:
return
# if the shot is done but it's working on it's 'follow through', then ignore this stuff
if time_remaining > 0:
# Convert the time remaining into number of slices, and take off the minimum gain accepted from the time
min_gain = 0.05
end_slice = round(min(time_remaining - min_gain, cap_) * 60)
if end_slices is None:
# Cap the slices
end_slice = round(cap_ * 60)
# We can't end a slice index that's lower than the start index
if end_slice <= start_slice:
return
# for every second worth of slices that we have to search,
# skip 1 more slice (for performance reasons) - min 1 and max 3
skip = cap(end_slice - start_slice / 60, 1, 3)
return struct.slices[start_slice:end_slice:skip]
def find_hits(drone: CarObject, agent: MyHivemind, targets):
# find_hits takes a dict of (left,right) target pairs and finds routines that could hit the ball
# between those target pairs
# find_hits is only meant for routines that require a defined intercept time/place in the future
# find_hits should not be called more than once in a given tick,
# as it has the potential to use an entire tick to calculate
# Example Useage:
# targets = {"goal":(opponent_left_post,opponent_right_post), "anywhere_but_my_net":(my_right_post,my_left_post)}
# hits = find_hits(agent,targets)
# print(hits)
# >{"goal":[a ton of jump and aerial routines,in order from soonest to latest],
# "anywhere_but_my_net":[more routines and stuff]}
hits = {name: [] for name in targets}
struct = agent.get_ball_prediction_struct()
# Begin looking at slices 0.25s into the future
# The number of slices
i = 15
while i < struct.num_slices:
# Gather some data about the slice
intercept_time = struct.slices[i].game_seconds
time_remaining = intercept_time - agent.time
if time_remaining > 0:
ball_location = Vector3(struct.slices[i].physics.location)
ball_velocity = Vector3(
struct.slices[i].physics.velocity).magnitude()
if abs(ball_location[1]) > 5250:
break # abandon search if ball is scored at/after this point
# determine the next slice we will look at, based on ball velocity (slower ball needs fewer slices)
i += 15 - cap(int(ball_velocity // 150), 0, 13)
car_to_ball = ball_location - drone.location
# Adding a True to a vector's normalize will have it also return the magnitude of the vector
direction, distance = car_to_ball.normalize(True)
# How far the car must turn in order to face the ball, for forward and reverse
forward_angle = direction.angle(drone.forward)
backward_angle = math.pi - forward_angle
# Accounting for the average time it takes to turn and face the ball
# Backward is slightly longer as typically the car is moving forward and takes time to slow down
forward_time = time_remaining - (forward_angle * 0.318)
backward_time = time_remaining - (backward_angle * 0.418)
# If the car only had to drive in a straight line, we ensure it has enough time to reach the ball
# (a few assumptions are made)
forward_flag = forward_time > 0.0 and (
distance * 1.05 / forward_time) < (
2290 if drone.boost > distance / 100 else 1400)
backward_flag = distance < 1500 and backward_time > 0.0 and (
distance * 1.05 / backward_time) < 1200
# Provided everything checks out, we begin to look at the target pairs
if forward_flag or backward_flag:
for pair in targets:
# First we correct the target coordinates to account for the ball's radius
# If swapped == True, the shot isn't possible because the ball wouldn't fit between the targets
left, right, swapped = post_correction(
ball_location, targets[pair][0], targets[pair][1])
if not swapped:
# Now we find the best direction to hit the ball in order to land it between the target points
left_vector = (left - ball_location).normalize()
right_vector = (right - ball_location).normalize()
best_shot_vector = direction.clamp(
left_vector, right_vector)
# Check to make sure our approach is inside the field
if in_field(ball_location - (200 * best_shot_vector),
1):
# The slope represents how close the car is to the chosen vector, higher = better
# A slope of 1.0 would mean the car is 45 degrees off
slope = find_slope(best_shot_vector, car_to_ball)
if forward_flag:
if ball_location[2] <= 300 and slope > 0.0:
hits[pair].append(
JumpShot(ball_location, intercept_time,
best_shot_vector, slope))
if 300 < ball_location[
2] < 600 and slope > 1.0 and (
ball_location[2] -
250) * 0.14 > drone.boost:
hits[pair].append(
AerialShot(ball_location,
intercept_time,
best_shot_vector))
elif backward_flag and ball_location[
2] <= 280 and slope > 0.25:
hits[pair].append(
JumpShot(ball_location, intercept_time,
best_shot_vector, slope, -1))
return hits
def run(self, drone: CarObject, agent: MyHivemind):
if self.time == -1:
elapsed = 0
self.time = agent.time
else:
elapsed = agent.time - self.time
T = self.intercept_time - agent.time
xf = drone.location + drone.velocity * T + 0.5 * gravity * T ** 2
vf = drone.velocity + gravity * T
if self.jumping:
if self.jump_time == -1:
jump_elapsed = 0
self.jump_time = agent.time
else:
jump_elapsed = agent.time - self.jump_time
tau = jump_max_duration - jump_elapsed
if jump_elapsed == 0:
vf += drone.up * jump_speed
xf += drone.up * jump_speed * T
vf += drone.up * jump_acc * tau
xf += drone.up * jump_acc * tau * (T - 0.5 * tau)
vf += drone.up * jump_speed
xf += drone.up * jump_speed * (T - tau)
if jump_elapsed < jump_max_duration:
drone.controller.jump = True
elif elapsed >= jump_max_duration and self.counter < 3:
drone.controller.jump = False
self.counter += 1
elif elapsed < 0.3:
drone.controller.jump = True
else:
self.jumping = jump_elapsed <= 0.3
else:
drone.controller.jump = 0
delta_x = self.ball_location - xf
direction = delta_x.normalize()
if delta_x.magnitude() > 50:
defaultPD(drone, drone.local(delta_x))
else:
if self.target is not None:
defaultPD(drone, drone.local(self.target))
else:
defaultPD(drone, drone.local(self.ball_location - drone.location))
if jump_max_duration <= elapsed < 0.3 and self.counter == 3:
drone.controller.roll = 0
drone.controller.pitch = 0
drone.controller.yaw = 0
drone.controller.steer = 0
if drone.forward.angle3D(direction) < 0.3:
if delta_x.magnitude() > 50:
drone.controller.boost = 1
drone.controller.throttle = 0
else:
drone.controller.boost = 0
drone.controller.throttle = cap(0.5 * throttle_accel * T ** 2, 0, 1)
else:
drone.controller.boost = 0
drone.controller.throttle = 0
if T <= 0 or not shot_valid(agent, self, threshold=150):
drone.pop()
drone.push(Recovery(agent.friend_goal.location))
def calculate(self):
return cap(scipy.interpolate.splev([self.current_time], self.rep, der=0)[0])
def run_simba(self):
verbose = self.verbose
for index in tqdm(range(len(self.X))):
if verbose:
print()
print()
print("Index:", index)
queries_count = 0
img = self.X[index].copy()
if verbose:
print("INITIAL IMAGE")
if self.reshape_flag:
new_shape = self.reshape
else:
new_shape = np.shape(img)
if verbose:
plt.imshow(np.reshape(img, new_shape))
plt.show()
label = np.argmax(self.y[index])
shape = np.shape(img)
selected = set()
init_probs_distribution = self.model.predict(np.array([img]))[0]
queries_count += 1
curr_probs_distribution = init_probs_distribution.copy()
init_prob = init_probs_distribution[label]
curr_prob = init_prob
perturbed = False
l0_distance = 0
for step in range(self.max_iterations):
if (l0_distance > self.max_l0_distance):
if verbose:
print("l0_distance exceeded")
print()
break
if (step % 100 == 0):
if verbose:
print(" Step:", step)
print(" Prob:", curr_prob)
print()
if SimbaWrapper.stop_condition_success(curr_probs_distribution,
label):
if verbose:
print("Perturbation successful")
print("Classified as: ",
np.argmax(curr_probs_distribution))
print("Perturbed image:")
if self.reshape_flag:
new_shape = self.reshape
else:
new_shape = np.shape(img)
plt.imshow(np.reshape(img, new_shape))
plt.show()
perturbed = True
break
if queries_count >= self.max_queries:
if verbose:
print("max queries exceeded")
print()
break
if len(selected) >= np.shape(self.X[0])[0] * np.shape(
self.X[0])[1]:
if verbose:
print("all pixels consumed")
print()
break
# Pick next pixel to alter. This can't be part of the pixels already expored, as per the SimBA paper.
i = random.randint(0, shape[0] - 1)
j = random.randint(0, shape[1] - 1)
while (i, j) in selected:
i = random.randint(0, shape[0] - 1)
j = random.randint(0, shape[1] - 1)
selected.add((i, j))
img_pos = img.copy()
img_pos[i][j][
0] = img_pos[i][j][0] + self.epsilon * self.max_value
img_pos[i][j][0] = utils.cap(img_pos[i][j][0], 0,
self.max_value)
# random_intensity = random.randint(0,255) / 255
# img_pos[i][j][0] = random_intensity
res_pos_distribution = self.model.predict(np.array([img_pos
]))[0]
queries_count += 1
res_pos = res_pos_distribution[label]
if (res_pos < curr_prob):
curr_probs_distribution = res_pos_distribution
curr_prob = res_pos
img = img_pos
l0_distance += 1
else:
img_neg = img.copy()
img_neg[i][j][
0] = img_neg[i][j][0] - self.epsilon * self.max_value
img_neg[i][j][0] = utils.cap(img_neg[i][j][0], 0,
self.max_value)
# img_neg[i][j][0] = random_intensity
res_neg_distribution = self.model.predict(
np.array([img_neg]))[0]
queries_count += 1
res_neg = res_neg_distribution[label]
if (res_neg < curr_prob):
curr_probs_distribution = res_neg_distribution
curr_prob = res_neg
img = img_neg
l0_distance += 1
if verbose:
print(" Queries:", queries_count)
print("__________________")
self.queries.append(queries_count)
self.perturbed.append(perturbed)
self.l0_distances.append(l0_distance)
self.X_modified.append(img)
if index % 20 == 0:
FILE_COUNT = index // 20
save_data = {
"queries": self.queries,
"perturbed": self.perturbed,
"l0_distances": self.l0_distances
}
with open(self.folder + "/" + str(FILE_COUNT) + ".json",
"w") as fp:
json.dump(save_data, fp)
def run(self, drone: CarObject, agent: MyHivemind):
raw_time_remaining = self.intercept_time - agent.time
# Capping raw_time_remaining above 0 to prevent division problems
time_remaining = cap(raw_time_remaining, 0.01, 10.0)
car_to_ball = self.ball_location - drone.location
# whether we are to the left or right of the shot vector
side_of_shot = sign(self.shot_vector.cross((0, 0, 1)).dot(car_to_ball))
car_to_intercept = self.intercept - drone.location
car_to_intercept_perp = car_to_intercept.cross((0, 0, side_of_shot)) # perpendicular
distance_remaining = car_to_intercept.flatten().magnitude()
speed_required = distance_remaining / time_remaining
# When still on the ground we pretend gravity doesn't exist, for better or worse
acceleration_required = backsolve(self.intercept, drone, time_remaining, 0 if self.jump_time == 0 else 325)
local_acceleration_required = drone.local(acceleration_required)
# The adjustment causes the car to circle around the dodge point in an effort to line up with the shot vector
# The adjustment slowly decreases to 0 as the bot nears the time to jump
adjustment = car_to_intercept.angle(self.shot_vector) * distance_remaining / 1.57 # size of adjustment
adjustment *= (cap(self.jump_threshold - (acceleration_required[2]), 0.0,
self.jump_threshold) / self.jump_threshold) # factoring in how close to jump we are
# we don't adjust the final target if we are already jumping
final_target = self.intercept + ((car_to_intercept_perp.normalize() * adjustment) if self.jump_time == 0 else 0)
# Some extra adjustment to the final target to ensure it's inside the field and
# we don't try to drive through any goalposts to reach it
if abs(drone.location[1] > 5150):
final_target[0] = cap(final_target[0], -750, 750)
local_final_target = drone.local(final_target - drone.location)
# drawing debug lines to show the dodge point and final target (which differs due to the adjustment)
agent.line(drone.location, self.intercept)
agent.line(self.intercept - Vector3(0, 0, 100), self.intercept + Vector3(0, 0, 100), [255, 0, 0])
agent.line(final_target - Vector3(0, 0, 100), final_target + Vector3(0, 0, 100), [0, 255, 0])
angles = defaultPD(drone, local_final_target)
if self.jump_time == 0:
defaultThrottle(drone, speed_required)
drone.controller.boost = False if abs(angles[1]) > 0.3 or drone.airborne else drone.controller.boost
drone.controller.handbrake = True if abs(angles[1]) > 2.3 else drone.controller.handbrake
if acceleration_required[2] > self.jump_threshold:
# Switch into the jump when the upward acceleration required reaches our threshold,
# hopefully we have aligned already...
self.jump_time = agent.time
else:
time_since_jump = agent.time - self.jump_time
# While airborne we boost if we're within 30 degrees of our local acceleration requirement
if drone.airborne and local_acceleration_required.magnitude() * time_remaining > 100:
angles = defaultPD(drone, local_acceleration_required)
if abs(angles[0]) + abs(angles[1]) < 0.5:
drone.controller.boost = True
if self.counter == 0 and (time_since_jump <= 0.2 and local_acceleration_required[2] > 0):
# hold the jump button up to 0.2 seconds to get the most acceleration from the first jump
drone.controller.jump = True
elif time_since_jump > 0.2 and self.counter < 3:
# Release the jump button for 3 ticks
drone.controller.jump = False
self.counter += 1
elif local_acceleration_required[2] > 300 and self.counter == 3:
# the acceleration from the second jump is instant, so we only do it for 1 frame
drone.controller.jump = True
drone.controller.pitch = 0
drone.controller.yaw = 0
drone.controller.roll = 0
self.counter += 1
if raw_time_remaining < -0.25 or not shot_valid(agent, self):
drone.pop()
drone.push(Recovery())
def run(self, drone: CarObject, agent: MyHivemind):
raw_time_remaining = self.intercept_time - agent.time
# Capping raw_time_remaining above 0 to prevent division problems
time_remaining = cap(raw_time_remaining, 0.001, 10.0)
car_to_ball = self.ball_location - drone.location
# whether we are to the left or right of the shot vector
side_of_shot = sign(self.shot_vector.cross((0, 0, 1)).dot(car_to_ball))
car_to_dodge_point = self.dodge_point - drone.location
car_to_dodge_perp = car_to_dodge_point.cross((0, 0, side_of_shot)) # perpendicular
distance_remaining = car_to_dodge_point.magnitude()
speed_required = distance_remaining / time_remaining
acceleration_required = backsolve(self.dodge_point, drone, time_remaining, 0 if not self.jumping else 650)
local_acceleration_required = drone.local(acceleration_required)
# The adjustment causes the car to circle around the dodge point in an effort to line up with the shot vector
# The adjustment slowly decreases to 0 as the bot nears the time to jump
adjustment = car_to_dodge_point.angle(self.shot_vector) * distance_remaining / 2.0 # size of adjustment
adjustment *= (cap(self.jump_threshold - (acceleration_required[2]), 0.0,
self.jump_threshold) / self.jump_threshold) # factoring in how close to jump we are
# we don't adjust the final target if we are already jumping
final_target = self.dodge_point + (
(car_to_dodge_perp.normalize() * adjustment) if not self.jumping else 0) + Vector3(0, 0, 50)
# Ensuring our target isn't too close to the sides of the field,
# where our car would get messed up by the radius of the curves
# Some adjustment to the final target to ensure it's inside the field and
# we don't try to dirve through any goalposts to reach it
if abs(drone.location[1]) > 5150:
final_target[0] = cap(final_target[0], -750, 750)
local_final_target = drone.local(final_target - drone.location)
# drawing debug lines to show the dodge point and final target (which differs due to the adjustment)
agent.line(drone.location, self.dodge_point)
agent.line(self.dodge_point - Vector3(0, 0, 100), self.dodge_point + Vector3(0, 0, 100), [255, 0, 0])
agent.line(final_target - Vector3(0, 0, 100), final_target + Vector3(0, 0, 100), [0, 255, 0])
# Calling our drive utils to get us going towards the final target
angles = defaultPD(drone, local_final_target, self.direction)
defaultThrottle(drone, speed_required, self.direction)
agent.line(drone.location, drone.location + (self.shot_vector * 200), [255, 255, 255])
drone.controller.boost = False if abs(angles[1]) > 0.3 or drone.airborne else drone.controller.boost
drone.controller.handbrake = True if abs(
angles[1]) > 2.3 and self.direction == 1 else drone.controller.handbrake
if not self.jumping:
if raw_time_remaining <= 0.0 or (speed_required - 2300) * time_remaining > 45 or not shot_valid(agent,
self):
# If we're out of time or not fast enough to be within 45 units of target at the intercept time, we pop
drone.pop()
if drone.airborne:
drone.push(Recovery())
elif local_acceleration_required[2] > self.jump_threshold \
and local_acceleration_required[2] > local_acceleration_required.flatten().magnitude():
# Switch into the jump when the upward acceleration required reaches our threshold,
# and our lateral acceleration is negligible
self.jumping = True
else:
if (raw_time_remaining > 0.2 and not shot_valid(agent, self, 150)) or raw_time_remaining <= -0.9 or (
not drone.airborne and self.counter > 0):
drone.pop()
drone.push(Recovery())
elif self.counter == 0 and local_acceleration_required[2] > 0.0 and raw_time_remaining > 0.083:
# Initial jump to get airborne + we hold the jump button for extra power as required
drone.controller.jump = True
elif self.counter < 3:
# make sure we aren't jumping for at least 3 frames
drone.controller.jump = False
self.counter += 1
elif 0.1 >= raw_time_remaining > -0.9:
# dodge in the direction of the shot_vector
drone.controller.jump = True
if not self.dodging:
vector = drone.local(self.shot_vector)
self.p = abs(vector[0]) * -sign(vector[0])
self.y = abs(vector[1]) * sign(vector[1]) * self.direction
self.dodging = True
# simulating a deadzone so that the dodge is more natural
drone.controller.pitch = self.p if abs(self.p) > 0.2 else 0
drone.controller.yaw = self.y if abs(self.y) > 0.3 else 0