"""
Core lap simulation functions.
"""
import copy
from functools import lru_cache
import numpy as np
import numpy.typing as npt
from ..constants import in_to_m
from ..models import SimulationState
from ..track import Track
from ..vehicle import Car
from .models import *
def _calculate_v_max_profile(
mycar: Car, r_full_track: npt.NDArray[np.float64]
) -> npt.NDArray[np.float64]:
"""
Calculate the maximum velocity profile for the entire track.
Uses LRU caching to efficiently handle repeated radius values.
The cache is created per function call to be specific to the mycar instance.
Parameters
----------
mycar : Car
The car object
r_full_track : np.ndarray
Array of radius values for each point on the track
Returns
-------
np.ndarray
Array of maximum velocities for each point on the track
"""
@lru_cache(maxsize=1024)
def cached_max_speed(radius: float) -> float:
return mycar.max_stable_speed(radius)
v_max_profile = np.array(
[cached_max_speed(r) for r in r_full_track], dtype=np.float64
)
return v_max_profile
def _should_terminate_forward_pass(
idx: int, v_curr: float, lap_vels: npt.NDArray[np.float64]
) -> bool:
"""
Determines whether to terminate during the forward pass of the lap simulation.
Parameters
----------
idx : int
Current index in the track array
v_curr : float
Current velocity at the index
lap_vels : np.ndarray
Current most restrictive lap velocities
Returns
-------
bool
Whether to terminate the forward pass
"""
# Terminate due to reaching end of track
if idx == (len(lap_vels) - 1):
return True
# Do not terminate if the current speed is more restrictive
if v_curr <= lap_vels[idx] + 1e-5:
return False
# Terminate forward pass if velocity limit is decreasing and current simulated
# speed has exceeded velocity limit
if (
lap_vels[idx] < lap_vels[idx - 1] - 1e-6
and v_curr > lap_vels[idx] + 1e-6
and np.isfinite(lap_vels[idx + 1])
):
return True
return False
def _should_terminate_backward_pass(
idx: int, v_curr: float, lap_vels: npt.NDArray[np.float64]
) -> bool:
"""
Determines whether to terminate during the backward pass of the lap simulation.
Parameters
----------
idx : int
Current index in the track array
v_curr : float
Current velocity at the index
lap_vels : np.ndarray
Current most restrictive lap velocities
Returns
-------
bool
Whether to terminate the backward pass
"""
# Terminate due to reaching start of track
if idx == 0:
return True
# Do not terminate if the current speed is more restrictive
if v_curr <= lap_vels[idx] + 1e-5:
return False
# Terminate backwards pass if velocity limit is decreasing (as we move backwards)
# and current simulated speed has exceeded velocity limit
if (
lap_vels[idx - 1] + 1e-6 < lap_vels[idx]
and v_curr > lap_vels[idx] + 1e-6
and np.isfinite(lap_vels[idx - 1])
):
return True
return False
[docs]
def lapsim(
mycar: Car,
track: Track,
multilap: bool = False, # TODO: Reimplement this
initial_velocity: float = -1,
use_coast: bool = False,
extract_internal_data: bool = False,
) -> LapsimResults:
"""
Simulates a lap (TODO: or multiple laps) around a given track with a given car.
Parameters
----------
mycar : Car
Suboptimumg's Car object representing the vehicle to simulate
track : Track
Suboptimumg's Track object representing the track to move around
multilap : bool, optional
Whether to simulate multiple laps, by default False
initial_velocity : float, optional
The initial velocity to start the simulation with, by default -1
extract_internal_data : bool, optional
Whether to extract internal data during simulation, by default False
Returns
-------
LapsimResults
The results of the lap simulation, including lap times, velocities, accelerations, etc.
Notes
-----
Overview of Algorithm:
Based on the track curvature, we find points of velocity minima (or middles of constant radius turns),
and use those as seeds to simulate both forwards and backwards along the track.
Then, we construct "proposals" from each seed. The velocity we choose is the lowest velocity simulation out
of all the seeds. Acceleration, power, and force profiles correspond to the velocity profile chosen.
"""
# Get track data directly from Track object
r_full_track = track.get_curvature_array()
seed_idx = track.get_simulation_seeds()
# Calculate v_max_profile
v_max_profile = _calculate_v_max_profile(mycar, r_full_track)
# Obtain seed velocities from v_max_profile
seed_v = v_max_profile[seed_idx]
# Set a seed to be the initial velocity, if one is provided
if initial_velocity != -1:
seed_v = np.append(seed_v, initial_velocity)
seed_idx = np.append(seed_idx, 0)
# Seeds sorted by velocity (lowest to highest)
sorted_indices = np.argsort(seed_v)
seed_idx = seed_idx[sorted_indices]
seed_v = seed_v[sorted_indices]
lap_dxs = track.get_dx_array()
lap_vels = v_max_profile.copy()
lap_accs = np.full(v_max_profile.shape, 0.0)
lap_powers = np.full(v_max_profile.shape, 0.0)
lap_eff_motor_torques = np.full(v_max_profile.shape, 0.0)
top_speed = mycar.calculate_top_speed()
per_seed_proposals: List[InternalDataSeedInfo] = []
# Simulate each seed
for i in range(len(seed_idx)):
idx_initial = int(seed_idx[i])
v_initial = min(seed_v[i], top_speed - 1) # In case we seeded on a straightaway
# Set up proposals for new most conservative simulation
dt_proposal = np.full(v_max_profile.shape, np.inf)
v_proposal = np.full(v_max_profile.shape, np.inf)
p_proposal = np.full(v_max_profile.shape, np.inf)
acc_proposal = np.full(v_max_profile.shape, np.inf)
eff_motor_torque_proposal = np.full(v_max_profile.shape, np.inf)
is_grown_forward_proposal = np.full(v_max_profile.shape, False)
# Grow forwards
idx = idx_initial
terminate_forward = False
is_coasting = False
old_state = SimulationState(sliding=False, acc=0.0, v=v_initial, dt=0.0, p=0.0)
while not terminate_forward:
radius = r_full_track[idx]
if use_coast and is_coasting:
# Coast and regen
new_state = mycar.coast_forwards_dx(radius, lap_dxs[idx], old_state)
if new_state.v < mycar.params.pwrtn.coast_trigger - abs(
mycar.params.pwrtn.coast_release_offset
):
is_coasting = False
else:
# Regular move
new_state = mycar.accelerate_forwards_dx(
radius, lap_dxs[idx], old_state
)
if use_coast and new_state.v > mycar.params.pwrtn.coast_trigger:
is_coasting = True
# P = torque * angular_velocity_motor
# = torque * ((v / tire_radius) * ratio)
tire_radius_m = in_to_m(mycar.params.tires.tire_radius)
eff_motor_torque = (
new_state.p * tire_radius_m / (new_state.v * mycar.params.pwrtn.ratio)
if new_state.v != 0
else 0
)
if not new_state.sliding:
# The new state is valid and we update the car's current state with it
updated_state = SimulationState(
sliding=new_state.sliding,
v=new_state.v,
dt=new_state.dt,
p=new_state.p,
acc=new_state.acc,
roll=new_state.roll,
pitch=new_state.pitch,
)
else:
# The new state is invalid, we retain the old state but with acceleration zeroed out
updated_state = SimulationState(
sliding=old_state.sliding,
v=old_state.v,
dt=old_state.dt,
p=old_state.p,
acc=0,
roll=old_state.roll,
pitch=old_state.pitch,
)
old_state = updated_state
dt_proposal[idx] = new_state.dt
v_proposal[idx] = new_state.v
p_proposal[idx] = new_state.p
acc_proposal[idx] = new_state.acc
eff_motor_torque_proposal[idx] = eff_motor_torque
is_grown_forward_proposal[idx] = True
terminate_forward = _should_terminate_forward_pass(
idx, new_state.v, lap_vels
)
idx += 1
# Reset to initial seed state for backwards pass
idx = idx_initial
terminate_backward = False
old_state = SimulationState(sliding=False, acc=0.0, v=v_initial, dt=0.0, p=0.0)
# Grow backwards
while not terminate_backward:
radius = r_full_track[idx]
new_state = mycar.brake_backwards_dx(radius, lap_dxs[idx], old_state)
tire_radius_m = in_to_m(mycar.params.tires.tire_radius)
eff_motor_torque = (
new_state.p * tire_radius_m / (new_state.v * -mycar.params.pwrtn.ratio)
if new_state.v != 0
else 0
)
if not new_state.sliding:
# The new state is valid and we update the car's current state with it
updated_state = SimulationState(
sliding=new_state.sliding,
v=new_state.v,
dt=new_state.dt,
p=new_state.p,
acc=new_state.acc,
roll=new_state.roll,
pitch=new_state.pitch,
)
else:
# The new state is invalid, we retain the old state but with acceleration zeroed out
updated_state = SimulationState(
sliding=old_state.sliding,
v=old_state.v,
dt=old_state.dt,
p=old_state.p,
acc=0,
roll=old_state.roll,
pitch=old_state.pitch,
)
old_state = updated_state
dt_proposal[idx] = new_state.dt
v_proposal[idx] = new_state.v
p_proposal[idx] = new_state.p
acc_proposal[idx] = -new_state.acc
eff_motor_torque_proposal[idx] = eff_motor_torque
terminate_backward = _should_terminate_backward_pass(
idx, new_state.v, lap_vels
)
idx -= 1
# Construct the masks for what states are more conservative
is_slower_mask = v_proposal < lap_vels
lap_vels = np.where(is_slower_mask, v_proposal, lap_vels)
lap_powers = np.where(is_slower_mask, p_proposal, lap_powers)
lap_accs = np.where(is_slower_mask, acc_proposal, lap_accs)
lap_eff_motor_torques = np.where(
is_slower_mask, eff_motor_torque_proposal, lap_eff_motor_torques
)
# The sim will slightly overshoot the initial v_max_profile,
# meaning flat or slightly limited regions never save power or torque data.
# We still need that data, as it's the best estimate of what should
# actually be happening, so add some margin to capture those cases.
is_almost_slower_mask = (
v_proposal < lap_vels + 0.2
) & is_grown_forward_proposal
lap_powers = np.where(is_almost_slower_mask, p_proposal, lap_powers)
lap_eff_motor_torques = np.where(
is_almost_slower_mask, eff_motor_torque_proposal, lap_eff_motor_torques
)
if extract_internal_data:
per_seed_proposals.append(
InternalDataSeedInfo(
v_proposal=v_proposal,
acc_proposal=acc_proposal,
slower_mask=is_slower_mask,
grown_forward_mask=is_grown_forward_proposal,
p_proposal=p_proposal,
acc_max_post=lap_accs.copy(),
v_max_post=lap_vels.copy(),
)
)
# Final laptime calc
lap_dts = np.divide(lap_dxs, lap_vels)
lap_t = np.cumsum(lap_dts)
# Safety checks
nan_indices = np.where(np.isnan(lap_accs))[0]
inf_indices = np.where(np.isinf(lap_accs))[0]
if nan_indices.size > 0:
raise ValueError(f"NaN found at indices: {nan_indices}")
if inf_indices.size > 0:
raise ValueError(f"Inf found at indices: {inf_indices}")
internal_data = (
InternalData(
initial_velocity=initial_velocity,
v_max_profile=v_max_profile.copy(),
seed_idx_list=copy.deepcopy(seed_idx),
seed_v_list=copy.deepcopy(seed_v),
cumulative_dist=track.get_dist_from_start_array(),
per_seed=per_seed_proposals,
)
if extract_internal_data
else None
)
return LapsimResults(
lap_t=lap_t,
lap_dxs=lap_dxs,
lap_vels=lap_vels,
lap_accs=lap_accs,
lap_powers=lap_powers,
lap_eff_motor_torques=lap_eff_motor_torques,
internal_data=internal_data,
)