def run(self, drone: CarObject, agent: MyHivemind): # An example of pushing routines to the stack: agent.line(Vector3(0, 0, 50), 2000 * self.vector.flatten(), color=[255, 0, 0]) agent.line(Vector3(0, 0, 50), 2000 * drone.forward.flatten(), color=[0, 255, 0]) robbies_constant = (self.vector * 1.5 * 2200 - drone.velocity * 1.5) * 2 * 1.5 ** -2 robbies_boost_constant = drone.forward.flatten().normalize().dot(robbies_constant.flatten().normalize()) > ( 0.3 if not drone.airborne else 0.1) drone.controller.boost = robbies_boost_constant and self.boost and not drone.supersonic if self.time == -1: elapsed = 0 self.time = agent.time else: elapsed = agent.time - self.time if elapsed < self.delay: if elapsed < self.duration: drone.controller.jump = True else: drone.controller.jump = False self.counter += 1 defaultPD(drone, self.preorientation) elif elapsed >= self.delay and self.counter < 3: drone.controller.jump = False defaultPD(drone, self.preorientation) self.counter += 1 elif elapsed < self.delay + 0.05: drone.controller.jump = True defaultPD(drone, self.vector) else: drone.pop() drone.push(Recovery(boost=self.boost, time=agent.time))
def defaultPD(drone: CarObject, local_target: Vector3, upside_down=False, up=None) -> []: # points the car towards a given local target. # Direction can be changed to allow the car to steer towards a target while driving backwards if up is None: up = drone.local( Vector3(0, 0, -1) if upside_down else Vector3( 0, 0, 1)) # where "up" is in local coordinates target_angles = ( math.atan2(local_target[2], local_target[0]), # angle required to pitch towards target math.atan2(local_target[1], local_target[0]), # angle required to yaw towards target math.atan2(up[1], up[2]) # angle required to roll upright ) # Once we have the angles we need to rotate, we feed them into PD loops to determining the controller inputs drone.controller.steer = steerPD(target_angles[1], 0) drone.controller.pitch = steerPD(target_angles[0], drone.angular_velocity[1] / 4) drone.controller.yaw = steerPD(target_angles[1], -drone.angular_velocity[2] / 4) drone.controller.roll = steerPD(target_angles[2], drone.angular_velocity[0] / 4) # Returns the angles, which can be useful for other purposes return target_angles
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 shot_valid(agent: MyHivemind, shot: Union[AerialShot, JumpShot, Aerial], threshold: float = 45) -> bool: # Returns True if the ball is still where the shot anticipates it to be # First finds the two closest slices in the ball prediction to shot's intercept_time # threshold controls the tolerance we allow the ball to be off by slices = agent.get_ball_prediction_struct().slices soonest = 0 latest = len(slices) - 1 while len(slices[soonest:latest + 1]) > 2: midpoint = (soonest + latest) // 2 if slices[midpoint].game_seconds > shot.intercept_time: latest = midpoint else: soonest = midpoint # preparing to interpolate between the selected slices dt = slices[latest].game_seconds - slices[soonest].game_seconds time_from_soonest = shot.intercept_time - slices[soonest].game_seconds slopes = (Vector3(slices[latest].physics.location) - Vector3(slices[soonest].physics.location)) * (1 / dt) # Determining exactly where the ball will be at the given shot's intercept_time predicted_ball_location = Vector3( slices[soonest].physics.location) + (slopes * time_from_soonest) # Comparing predicted location with where the shot expects the ball to be return (shot.ball_location - predicted_ball_location).magnitude() < threshold
def bestShotVector(car, ball_location): relative = (ball_location - car.location) left_post_vector = Vector3(750 * -side(car.team), 5150 * -side(car.team), 100) - ball_location right_post_vector = Vector3(750 * side(car.team), 5150 * -side(car.team), 100) - ball_location return (relative).clamp(left_post_vector, right_post_vector).normalize()
def find_slope(shot_vector: Vector3, car_to_target: Vector3) -> float: # Finds the slope of your car's position relative to the shot vector (shot vector is y axis) # 10 = you are on the axis and the ball is between you and the direction to shoot in # -10 = you are on the wrong side # 1.0 = you're about 45 degrees offcenter d = shot_vector.dot(car_to_target) e = abs(shot_vector.cross((0, 0, 1)).dot(car_to_target)) return cap(d / e if e != 0 else 10 * sign(d), -3.0, 3.0)
def run(self, drone: CarObject, agent: MyHivemind): target = Vector3(0, 3800 * agent.side(), 0) local_target = drone.local(target - drone.location) defaultPD(drone, local_target) defaultThrottle(drone, 2300) if local_target.magnitude() < 100: drone.pop() drone.push(DiagonalKickoff()) drone.push(Flip(Vector3(1, 0, 0)))
def align(point_location, ball_location, goal_location): position_to_ball = (ball_location - point_location).normalize().flatten() ball_to_goal_center = (goal_location - ball_location).normalize().flatten() ball_to_goal_left_post = (goal_location + Vector3(800, 0, 0) - ball_location).normalize().flatten() ball_to_goal_right_post = (goal_location + Vector3(-800, 0, 0) - ball_location).normalize().flatten() best_case = min(position_to_ball.dot(ball_to_goal_center), position_to_ball.dot(ball_to_goal_right_post), position_to_ball.dot(ball_to_goal_left_post)) return best_case
def find_slope(shot_vector: Vector3, car_to_target: Vector3) -> float: # Finds the slope of your car's position relative to the shot vector (shot vector is y axis) # 10 = you are on the axis and the ball is between you and the direction to shoot in # -10 = you are on the wrong side # 1 = you're about 45 degrees offcenter d = shot_vector.dot(car_to_target) e = abs(shot_vector.cross(Vector3(0, 0, 1)).dot(car_to_target)) try: f = d / e except ZeroDivisionError: return 10 * sign(d) return cap(f, -3, 3)
def distance_to_wall(point): #determines how close the car is to the wall point = Vector3(abs(point[0]), abs(point[1]), abs(point[2])) if 4096 - point[0] < 5120 - point[1]: return 4096 - point[0] else: return 5120 - point[1]
def backsolve(target, car, time, gravity=650): #Finds the acceleration required for a car to reach a target in a specific amount of time d = target - car.location dvx = ((d[0] / time) - car.velocity[0]) / time dvy = ((d[1] / time) - car.velocity[1]) / time dvz = (((d[2] / time) - car.velocity[2]) / time) + (gravity * time) return Vector3(dvx, dvy, dvz)
def shotValid(slices, shot): mi = 0 ma = len(slices) - 1 while len(slices[mi:ma + 1]) > 2: if slices[(ma + mi) // 2].game_seconds > shot.intercept_time: ma = (ma + mi) // 2 else: mi = (ma + mi) // 2 dt = slices[ma].game_seconds - slices[mi].game_seconds time_from_mi = shot.intercept_time - slices[mi].game_seconds mi = slices[mi].physics.location ma = slices[ma].physics.location slopes = Vector3(ma.x - mi.x, ma.y - mi.y, ma.z - mi.z) * (1 / dt) slice_intercept = Vector3(mi.x, mi.y, mi.z) + (slopes * time_from_mi) if (shot.ball - slice_intercept).magnitude() > 30: return False return True
def get_closest_post_vector(agent, future_ball_location=None): y = 4400 * utils.side(agent.team) z = 0 closest_post = closest_point( agent.me.location, [agent.friend_goal.left_post, agent.friend_goal.right_post]) return_value = Vector3(closest_post.x, y, z) return return_value
def run(self, drone: CarObject, agent: MyHivemind): target = agent.ball.location + Vector3(0, 200 * agent.side(), 0) local_target = drone.local(target - drone.location) defaultPD(drone, local_target) defaultThrottle(drone, 2300) if local_target.magnitude() < 650: drone.pop() drone.push(Flip(drone.local(agent.foe_goal.location - drone.location)))
def __init__(self, vector: Vector3, duration: float = 0.1, delay: float = 0.1, angle: float = 0, boost: bool = False): super().__init__() self.vector = vector.normalize() self.pitch = abs(self.vector[0]) * -sign(self.vector[0]) self.yaw = abs(self.vector[1]) * sign(self.vector[1]) self.delay = delay if delay >= duration else duration self.duration = duration self.boost = boost self.angle = math.radians(angle) if boost else 0 x = math.cos(self.angle) * self.vector.x - math.sin(self.angle) * self.vector.y y = math.sin(self.angle) * self.vector.x + math.cos(self.angle) * self.vector.y self.preorientation = Vector3(x, y, 0) # the time the jump began self.time = -1 # keeps track of the frames the jump button has been released self.counter = 0
def push_shot(drone: CarObject, agent: MyHivemind): left = Vector3(4200 * -agent.side(), agent.ball.location.y + (1000 * -agent.side()), 0) right = Vector3(4200 * agent.side(), agent.ball.location.y + (1000 * -agent.side()), 0) targets = { "goal": (agent.foe_goal.left_post, agent.foe_goal.right_post), "upfield": (left, right) } shots = find_hits(drone, agent, targets) if len(shots["goal"]) > 0: drone.clear() drone.push(shots["goal"][0]) drone.action = Action.Going elif len(shots["upfield"]) > 0: drone.clear() drone.push(shots["upfield"][0]) drone.action = Action.Going
def backsolve(target: Vector3, car: CarObject, time: float, gravity: int = 650) -> Vector3: # Finds the acceleration required for a car to reach a target in a specific amount of time d = target - car.location dvx = ((d[0] / time) - car.velocity[0]) / time dvy = ((d[1] / time) - car.velocity[1]) / time dvz = (((d[2] / time) - car.velocity[2]) / time) + (gravity * time) return Vector3(dvx, dvy, dvz)
def defaultPD(agent, local, direction=0): yaw = math.atan2(local[1], local[0]) turn = (math.pi * direction) + yaw if direction != 0 else yaw up = agent.me.matrix.dot(Vector3(0, 0, 1)) target = [math.atan2(up[1], up[2]), math.atan2(local[2], local[0]), turn] agent.c.steer = steerPD(turn, 0) agent.c.yaw = steerPD(target[2], -agent.me.rvel[2] / 4) agent.c.pitch = steerPD(target[1], agent.me.rvel[1] / 4) agent.c.roll = steerPD(target[0], agent.me.rvel[0] / 2.5) return target
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 get_back_post_vector(agent, future_ball_location=None): #returns the location of our goal's back post. y = 4400 * utils.side(agent.team) z = 0 ball_location = future_ball_location if future_ball_location is not None else agent.ball.location if ball_location[0] > 0: x = -850 else: x = +850 back_post = Vector3(x, y, z) return back_post
def __init__(self, vector: Vector3, duration: float = 0.1, delay: float = 0.1): super().__init__() self.vector = vector.normalize() self.pitch = abs(self.vector[0]) * -sign(self.vector[0]) self.yaw = abs(self.vector[1]) * sign(self.vector[1]) self.delay = delay if delay >= duration else duration self.duration = duration # the time the jump began self.time = -1 # keeps track of the frames the jump button has been released self.counter = 0
def field(point, radius): point = Vector3(abs(point[0]), abs(point[1]), abs(point[2])) if point[0] > 3860 - radius: return False elif point[1] > 5800 - radius: return False elif point[0] > 820 - radius and point[1] > 4950 - radius: return False elif point[0] > 2800 - radius and point[1] > -point[0] + 7750 - radius: return False return True
def test_setGet(self): vec = Vector3(0, 0, 0) vec.x = 1 self.assertEqual(vec.x, 1) self.assertEqual(vec[0], 1) vec.y = 1 self.assertEqual(vec.y, 1) self.assertEqual(vec[1], 1) vec.z = 1 self.assertEqual(vec.z, 1) self.assertEqual(vec[2], 1)
def ball_going_into_their_danger_zone(agent): slices = get_slices(agent, 6) if slices is not None: for slice in slices: ball_location = Vector3(slice.physics.location) if side( agent.team ) * ball_location.y < -3600 and -1600 < ball_location.x < 1600: return True return False
def in_field(point: Vector3, radius: float) -> bool: # determines if a point is inside the standard soccer field point = Vector3(abs(point[0]), abs(point[1]), abs(point[2])) if point[0] > 4080 - radius: return False elif point[1] > 5900 - radius: return False elif point[0] > 880 - radius and point[1] > 5105 - radius: return False elif point[0] > 2650 and point[1] > -point[0] + 8025 - radius: return False return True
def shotFinder(agent): shots = [] struct = agent.get_ball_prediction_struct() for i in range(18, struct.num_slices, 18): intercept_time = struct.slices[i].game_seconds time_remaining = intercept_time - agent.time temp = struct.slices[i].physics.location ball = Vector3(temp.x, temp.y, temp.z) if (ball - agent.me.location).magnitude() / time_remaining < 2250: ratio = shotConeRatio(agent, agent.me, ball, True) if ratio < -0.1: upfield_vector = Vector3(0, 1.0 * -side(agent.team), 0) intercept = ball - (93 * upfield_vector) shots.append( shotObject(intercept, upfield_vector, intercept_time, True)) if ratio < -0.5: shot_vector = bestShotVector(agent.me, ball) intercept = ball - (93 * shot_vector) shots.append( shotObject(intercept, shot_vector, intercept_time, False)) return shots
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 find_intercept_time_with_detour(car, agent, ball_prediction_slices=None, return_intercept_location_too=False, time_limit: float = None, time_to_subtract=0.0, ball_height_max=9999): #find the earliest time that a car can intercept the ball. Returns None otherwise. earliest_intercept_time = None earliest_intercept_location = None ball_prediction_struct = ball_prediction_slices if ball_prediction_slices is not None else get_slices( agent, 6) if ball_prediction_struct is not None: for slice in ball_prediction_slices: prediction_slice = slice intercept_time = prediction_slice.game_seconds time_remaining = intercept_time - agent.time - time_to_subtract if time_limit is not None: if intercept_time - agent.time > time_limit: break if time_remaining > 0: current_prediction_slice_ball_location = Vector3( prediction_slice.physics.location.x, prediction_slice.physics.location.y, prediction_slice.physics.location.z) reposition_target = find_reposition_target( agent, current_prediction_slice_ball_location) time_to_reach_intercept = time_to_reach_multiple_locations( car, [ reposition_target, current_prediction_slice_ball_location ]) if time_to_reach_intercept < time_remaining: earliest_intercept_time = intercept_time earliest_intercept_location = current_prediction_slice_ball_location #print("Time to run non np code: " + str(toc - tic)) if return_intercept_location_too: return earliest_intercept_time, earliest_intercept_location else: return earliest_intercept_time #print("Time to run non np code: " + str(toc - tic)) if return_intercept_location_too: return None, None else: return None
def defaultPD(agent, local, direction=1.0): #to reverse, multiply local by -1.0 and agent.c.steer by -1.0 local *= direction up = agent.me.matrix.dot(Vector3(0, 0, 1)) target = [ math.atan2(up[1], up[2]), math.atan2(local[2], local[0]), math.atan2(local[1], local[0]) ] agent.c.steer = steerPD(target[2], 0) * direction agent.c.yaw = steerPD(target[2], -agent.me.rvel[2] / 4) * direction agent.c.pitch = steerPD(target[1], agent.me.rvel[1] / 4) * direction agent.c.roll = steerPD(target[0], agent.me.rvel[0] / 2.5) return target
def push_shot(drone: CarObject, agent: MyHivemind): left = Vector3(4200 * -agent.side(), agent.side() * 5120, 0) right = Vector3(4200 * agent.side(), agent.side() * 5120, 0) targets = {"goal": (agent.foe_goal.left_post, agent.foe_goal.right_post)} if not agent.conceding: drones = copy(agent.drones) drones.remove(drone) team = agent.friends + drones for teammate in team: a = teammate.location b = teammate.location + 2000 * teammate.forward local_a = drone.local(a) angle_a = math.atan2(local_a.y, local_a.x) if angle_a > 0: targets["teammate" + str(team.index(teammate))] = (b, a) else: targets["teammate" + str(team.index(teammate))] = (a, b) targets["upfield"] = (left, right) shots = find_hits(drone, agent, targets) if len(shots["goal"]) > 0: drone.clear() drone.push(shots["goal"][0]) drone.action = Action.Going elif shots.get("teammate0") is not None and len( shots.get("teammate0")) > 0: drone.clear() drone.push(shots["teammate0"][0]) drone.action = Action.Going elif shots.get("teammate1") is not None and len( shots.get("teammate1")) > 0: drone.clear() drone.push(shots["teammate1"][0]) drone.action = Action.Going elif len(shots["upfield"]) > 0: drone.clear() drone.push(shots["upfield"][0]) drone.action = Action.Going