def estimate_time(car: Car, target, dd=1) -> float:
turning_radius = 1 / Drive.max_turning_curvature(norm(car.velocity) + 500)
turning = angle_between(car.forward() * dd, direction(
car, target)) * turning_radius / 1800
if turning < 0.5: turning = 0
dist = ground_distance(car, target) - 200
if dist < 0: return turning
speed = dot(car.velocity, car.forward())
time = 0
result = None
if car.boost > 0 and dd > 0:
boost_time = car.boost / 33.33
result = BOOST.simulate_until_limit(speed,
distance_limit=dist,
time_limit=boost_time)
dist -= result.distance_traveled
time += result.time_passed
speed = result.speed_reached
if dist > 0 and speed < 1410:
result = THROTTLE.simulate_until_limit(speed, distance_limit=dist)
dist -= result.distance_traveled
time += result.time_passed
speed = result.speed_reached
if result is None or not result.distance_limit_reached:
time += dist / speed
return time * 1.05 + turning
def step(self, packet: GameTickPacket, drone: Drone, index: int):
up = normalize(drone.position)
drone.aerial_turn.target = look_at(vec3(0, 0, 1), up)
drone.aerial_turn.step(self.dt)
drone.controls = drone.aerial_turn.controls
drone.controls.boost = angle_between(vec3(0, 0, 1), drone.forward()) < 0.5
drone.controls.jump = True
def estimate_time(car: Car, target, speed, dd=1) -> float:
dist = distance(car, target)
if dist < 100:
return 0
travel = dist / speed
turning = angle_between(car.forward() * dd, direction(car, target)) / math.pi * 2
if turning < 1:
turning **= 2
acceleration = (speed * dd - dot(car.velocity, car.forward())) / 2100 * 0.2 * dd / max(car.boost / 20, 1)
return travel + acceleration + turning * 0.7
def step(self, dt):
self.simulate_landing()
self.aerial_turn.step(dt)
self.controls = self.aerial_turn.controls
self.controls.boost = angle_between(self.car.forward(), vec3(
0, 0, -1)) < 1.5 and not self.landing
self.controls.throttle = 1 # in case we're turtling
self.finished = self.car.on_ground
def step(self, dt):
self.simulate_landing()
self.aerial_turn.step(dt)
self.controls = self.aerial_turn.controls
self.controls.boost = angle_between(self.car.forward(), vec3(
0, 0, -1)) < 1.5 and not self.landing
self.controls.throttle = 1 # in case we're turtling
# jump if the car is upside down and has wheel contact
if (self.jump_when_upside_down and self.car.on_ground
and dot(self.car.up(), vec3(0, 0, 1)) < -0.95):
self.controls.jump = True
self.landing = False
else:
self.finished = self.car.on_ground
def rotate_by_axis(orientation: Orientation,
original: Vec3,
target: Vec3,
percent=1) -> Rotator:
"""
Updates the given orientation's original direction to the target direction.
original should be `orientation.forward` or `orientation.up` or `orientation.right`
"""
angle = angle_between(to_rlu_vec(target), to_rlu_vec(original)) * percent
rotate_axis = cross(to_rlu_vec(target), to_rlu_vec(original))
ortho = normalize(rotate_axis) * -angle
rot_mat_initial: mat3 = orientation.to_rot_mat()
rot_mat_adj = axis_to_rotation(ortho)
rot_mat = dot(rot_mat_adj, rot_mat_initial)
r = rot_mat_to_rot(rot_mat)
# printO(orientation)
# printO(Orientation(r))
return r
def step(self, dt):
if self.car.on_ground and norm(self.car.position + self.car.velocity - xy(self.target)) > 2500:
self.drive.speed = 1000
self.drive.target = self.target
self.drive.step(dt)
self.controls = self.drive.controls
self.controls.handbrake = angle_between(self.car.forward(), self.target - self.car.position) > 1.2
return
self.hover.target = self.target
self.hover.up = normalize(self.car.position * -1)
self.hover.step(dt)
self.controls = self.hover.controls
self.controls.throttle = not self.car.on_ground
self.controls.jump = (self.car.position[2] < 30 or self.car.on_ground) and self.__time_spent_on_ground > 0.1
if self.info.round_active:
self.__time_spent_on_ground += dt
if not self.car.on_ground:
self.__time_spent_on_ground = 0.0
def angle_to(car, target, backwards=False) -> float:
return abs(
angle_between(car.forward() * (-1 if backwards else 1),
direction(car.position, target)))
def angle_to(car: Car, target: vec3, backwards=False) -> float:
return abs(angle_between(xy(car.forward()) * (-1 if backwards else 1), ground_direction(car.position, target)))
def step(self):
""""Gives output for the dribbling strategy"""
# direction of ball relative to center of car (where should we aim)
# direction of ball relative to yaw of car (where should we aim verse where we are aiming)
local_bot_to_ball = dot(self.ball.position - self.car.position,
self.car.orientation)
angle_front_to_ball = math.atan2(local_bot_to_ball[1],
local_bot_to_ball[0])
# distance between bot and ball
distance = distance_2d(self.car.position, self.ball.position)
# direction of ball velocity relative to yaw of car (which way the ball is moving verse which way we are moving)
if velocity_2d(self.ball.velocity) < 1e-10:
angle_car_forward_to_ball_velocity = 0
else:
angle_car_forward_to_ball_velocity = angle_between(
z_0(self.car.forward()), z_0(self.ball.velocity))
# magnitude of ball_bot_angle (squared)
ball_bot_diff = (self.ball.velocity[0]**2 +
self.ball.velocity[1]**2) - (self.car.velocity[0]**2 +
self.car.velocity[1]**2)
# p is the distance between ball and car
# i is the magnitude of the ball's velocity (squared) the i term would normally
# be the integral of p over time, but the ball's velocity is essentially that number
# d is the relative speed between ball and car
# note that bouncing a ball is distinctly different than balancing something that doesnt bounce
# p_s is the x component of the distance to the ball
# d_s is the one frame change of p_s, that's why p_s has to be global
# we modify distance and ball_bot_diff so that only the component along the car's path is counted
# if the ball is too far to the left, we don't want the bot to think it has to drive forward
# to catch it
distance_y = math.fabs(distance * math.cos(angle_front_to_ball))
distance_x = math.fabs(distance * math.sin(angle_front_to_ball))
# ball moving forward WRT car yaw?
forward = False
if math.fabs(angle_car_forward_to_ball_velocity) < math.radians(90):
forward = True
# first we give the distance values signs
if forward:
d = ball_bot_diff
i = (self.ball.velocity[0]**2 + self.ball.velocity[1]**2)
else:
d = -ball_bot_diff
i = -(self.ball.velocity[0]**2 + self.ball.velocity[1]**2)
if math.fabs(math.degrees(angle_front_to_ball)) < 90:
p = distance_y
else:
p = -1 * distance_y
# this is the PID correction. all of the callibration goes on right here
# there is literature about how to set the variables but it doesn't work quite the same
# because the car is only touching the ball (and interacting with the system) on bounces
# we run the PID formula through tanh to give a value between -1 and 1 for steering input
# if the ball is lower we have no velocity bias
bias_v = 600000 # 600000
# just the basic PID if the ball is too low
if self.ball.position[2] < 120:
correction = np.tanh((20 * p + .0015 * i + .006 * d) / 500)
# if the ball is on top of the car we use our bias (the bias is in velocity units squared)
else:
correction = np.tanh(
(20 * p + .0015 * (i - bias_v) + .006 * d) / 500)
# makes sure we don't get value over .99 so we dont exceed maximum thrust
self.controls.throttle = correction * .99
# anything over .9 is boost
if correction > .99:
self.controls.boost = True
else:
self.controls.boost = False
# this is the PID steering section
# p_s is the x component of the distance to the ball (relative to the cars direction)
# d_s is the on frame change in p_s
# we use absolute value and then set the sign later
d_s = math.fabs(self.p_s) - math.fabs(distance_x)
self.p_s = math.fabs(distance_x)
# give the values the correct sign
if angle_front_to_ball < 0:
self.p_s = -self.p_s
d_s = -d_s
# d_s is actually -d_s ...whoops
d_s = -d_s
max_bias = 35
backline_intersect = line_backline_intersect(self.goal.center[1],
vec2(self.car.position),
vec2(self.car.forward()))
if abs(backline_intersect) < 1000 or self.ball.position[2] > 200:
bias = 0
# Right of the ball
elif -850 > backline_intersect:
bias = max_bias
# Left of the ball
elif 850 < backline_intersect:
bias = -max_bias
# the correction settings can be altered to change performance
correction = np.tanh((100 * (self.p_s + bias) + 1500 * d_s) / 8000)
# apply the correction
self.controls.steer = correction
def get_output(self, packet: GameTickPacket) -> SimpleControllerState:
self.renderer.begin_rendering()
self.game.read_game_information(packet, self.get_rigid_body_tick(),
self.get_field_info())
if self.lastReset + 300 < self.currentTick:
if self.tests > 0:
score = min(300, self.currentTick - self.lastDoneTick)
self.totalScore += score
print(self.tests, score, round(self.totalScore / self.tests,
2))
self.tests += 1
self.lastReset = self.currentTick
self.target = vec3(2 * random() - 1, 2 * random() - 1,
2 * random() - 1)
self.up = orthogonalize(
vec3(2 * random() - 1, 2 * random() - 1, 2 * random() - 1),
self.target)
self.targetOrientation = look_at(self.target, self.up)
car_state = CarState(
physics=Physics(location=Vector3(0, 1000, 17),
velocity=Vector3(0, 0, 0),
rotation=Rotator(0, 0, 0),
angular_velocity=Vector3(0, 0, 0)))
self.set_game_state(GameState(cars={self.index: car_state}))
self.stage = 0
self.lastDodgeTick = -math.inf
# print("TELEPORT TO GROUND")
return self.controls
else:
car_state = CarState(physics=Physics(location=Vector3(0, 0, 400),
velocity=Vector3(0, 0, 0)))
self.set_game_state(GameState(cars={self.index: car_state}))
if self.stage <= 5:
self.stage += 1
if self.stage == 6:
self.dodgeDirection = normalize(vec2(0, 2 * random() - 1))
self.controls.jump = True #random() > 0.5
if self.controls.jump:
self.lastDodgeTick = self.currentTick
self.controls.roll, self.controls.pitch, self.controls.yaw = self.dodgeDirection[
0], self.dodgeDirection[1], 0
self.stage += 1
return self.controls
else:
self.controls.jump = False
self.packet = packet
self.handleTime()
car = packet.game_cars[self.index]
position = vec3(car.physics.location.x, car.physics.location.y,
car.physics.location.z)
self.renderer.draw_line_3d(car.physics.location,
position + 300 * normalize(self.target),
self.renderer.yellow())
self.renderer.draw_line_3d(car.physics.location,
position + 300 * normalize(self.up),
self.renderer.pink())
carOrientation = rotationToOrientation(car.physics.rotation)
ang = parseVector(car.physics.angular_velocity)
if angle_between(carOrientation,
self.targetOrientation) > 1 / 180 * math.pi:
self.lastDoneTick = self.currentTick
o_rlu = dot(transpose(self.targetOrientation), carOrientation)
w_rlu = dot(transpose(self.targetOrientation), ang)
o = torch.tensor([[o_rlu[i, j] for j in range(3)]
for i in range(3)])[None, :].float().to(device)
w = torch.tensor([w_rlu[i]
for i in range(3)])[None, :].float().to(device)
noPitchTime = max(0,
(self.lastDodgeTick - self.currentTick) / 120 + .95)
dodgeTime = max(0, (self.lastDodgeTick - self.currentTick) / 120 + .65)
if dodgeTime == 0:
self.dodgeDirection = vec2(0, 0)
noPitchTime = torch.tensor([noPitchTime]).float().to(device)
dodgeTime = torch.tensor([dodgeTime]).float().to(device)
dodgeDirection = torch.tensor([
self.dodgeDirection[i] for i in range(2)
])[None, :].float().to(device)
# if self.simulation.o is not None and self.simulation.w is not None:
# print("=====================================")
# print("-------------------------------------")
# print(self.simulation.o, o)
# print(self.simulation.w, w)
# print(self.simulation.noPitchTime, noPitchTime)
# print(self.simulation.dodgeTime, dodgeTime)
# print(self.simulation.dodgeDirection, dodgeDirection)
# self.simulation.step(self.ticksNowPassed / 120)
# print(self.simulation.o, o)
# print(self.simulation.w, w)
# print(self.simulation.noPitchTime, noPitchTime)
# print(self.simulation.dodgeTime, dodgeTime)
# print(self.simulation.dodgeDirection, dodgeDirection)
self.simulation.o = o
self.simulation.w = w
self.simulation.noPitchTime = noPitchTime
self.simulation.dodgeTime = dodgeTime
self.simulation.dodgeDirection = dodgeDirection
if True:
rpy = self.policy(self.simulation.o.permute(0, 2, 1),
self.simulation.w_local(),
self.simulation.noPitchTime,
self.simulation.dodgeTime,
self.simulation.dodgeDirection)[0]
self.controls.roll, self.controls.pitch, self.controls.yaw = rpy
else:
self.reorientML.target_orientation = self.targetOrientation
self.reorientML.step(1 / self.FPS)
self.controls.roll, self.controls.pitch, self.controls.yaw = self.reorientML.controls.roll, self.reorientML.controls.pitch, self.reorientML.controls.yaw
if self.simulation.error()[0].item() < 0.01:
self.frames_done += 1
else:
self.frames_done = 0
if self.frames_done >= 10:
self.finished = True
self.renderer.end_rendering()
return self.controls