from typing import List, Tuple
import numpy as np
import numpy.typing as npt
from scipy.interpolate import splprep
from .models import ArcData
[docs]
def smooth_and_normalize_corner_radii(corners: List[List]):
"""
Takes the absolute value of all corner radii. Scales radius of small corners (r <= 10) by
a factor of ((20 - r) / 10)^1.2
Parameters
----------
corners : List[List]
List of corners specified as [radius (m), length (decimeters)]
Returns
-------
List[List]
The same corners, but with cleaned radii
Notes
-----
Currently used for Mich 2023 AutoX and Endurance tracks. Subject to review.
"""
def adjust(radius):
radius = abs(radius)
return radius if radius > 10 else radius * ((20 - radius) / 10) ** 1.2
return [[adjust(corners[i][0]), corners[i][1]] for i in range(len(corners))]
[docs]
def scale_and_discretize_corner_lengths(corners: List[List], ratio) -> List[List]:
"""
Scales each corner length (corner[i][1]) by ratio and rounds to nearest integer. Used to
convert corner length (meters) to number of simulation steps (unitless).
Parameters
----------
corners : List[List]
List of corners specified as [radius (m), length (decimeters)]
ratio : float
The scaling to apply, before rounding.
Returns
-------
List[List]
Same list of corners with lengths scaled, represented as [radius (m), steps]
"""
return [
[corners[i][0], int(round(corners[i][1] * ratio))] for i in range(len(corners))
]
[docs]
def unroll_corners_into_track(
corners: List[List], distance_step: float
) -> Tuple[npt.NDArray[np.float64], npt.NDArray[np.float64], npt.NDArray[np.float64]]:
"""
Unrolls the compressed corner list into discrete chunks.
Parameters
----------
corners : List[List[int]]
List of corners specified as [radius (m), length (decimeters)]
distance_step : float
Length of each step in the simulation
Returns
-------
Tuple[npt.NDArray[float64], npt.NDArray[float64], npt.NDArray[float64]]
(dx, radius, cumulative_dist)
"""
num_steps_per_corner = np.array([n for _, n in corners], dtype=np.int64)
radii = np.array([r for r, _ in corners], dtype=np.float64)
total_steps = num_steps_per_corner.sum()
radius_array = np.repeat(radii, num_steps_per_corner)
dx_array = np.full(total_steps, distance_step, dtype=np.float64)
cumulative_dist_array = np.arange(total_steps, dtype=np.float64) * distance_step
return dx_array, radius_array, cumulative_dist_array
[docs]
def get_2d_rotation_matrix(alpha: float) -> npt.NDArray[np.float64]:
"""
Returns a 2D rotation matrix for a given angle alpha (in radians).
Parameters
----------
alpha : float
Rotation angle in radians
Returns
-------
NDArray[float64]
2x2 rotation matrix
"""
return np.array([[np.cos(alpha), -np.sin(alpha)], [np.sin(alpha), np.cos(alpha)]])
[docs]
def corners_to_cartesian(
corners: List[List], distance_step: float, start_angle_deg: float = 0.0
) -> Tuple[npt.NDArray[np.float64], npt.NDArray[np.float64], List[ArcData]]:
"""
Converts a list of corners (radius, length) into absolute cartesian coordinates (x, y)
and creates ArcData objects for visualization.
This function simulates driving through each corner, starting from the origin (0, 0)
with an initial heading angle, and tracks the path by computing arc geometry.
Parameters
----------
corners : List[List]
List of corners specified as [radius (m), length (steps)]
where length is the number of discrete steps (not meters)
distance_step : float
The distance in meters represented by each step
start_angle_deg : float, optional
Initial heading angle in degrees (default: 0)
Returns
-------
Tuple[NDArray[float64], NDArray[float64], List[ArcData]]
(x_m, y_m, arcs) - Arrays of x and y coordinates in meters, and list of ArcData objects
"""
x_coords = []
y_coords = []
arc_data_list = []
curr_pos = np.array([0.0, 0.0])
curr_heading = np.deg2rad(start_angle_deg)
for radius, num_steps in corners:
# Convert steps to actual arc length
arc_length = num_steps * distance_step
# Compute center of arc relative to current heading
# The center is perpendicular to the current heading direction
center = curr_pos + get_2d_rotation_matrix(curr_heading + np.pi / 2) @ np.array(
[radius, 0]
)
# Calculate arc travel angles
travel_start = curr_heading + np.pi / 2 + np.pi * np.sign(radius)
travel_rads = arc_length / radius # angle = arc_length / radius
# Convert angles to degrees for ArcData
travel_start_deg = np.rad2deg(travel_start)
travel_end_deg = np.rad2deg(travel_start + travel_rads)
# Store original values before sorting
theta1_original = travel_start_deg
theta2_original = travel_end_deg
# Ensure theta1 < theta2 for matplotlib Arc compatibility
theta1 = min(travel_start_deg, travel_end_deg)
theta2 = max(travel_start_deg, travel_end_deg)
# Create ArcData object matching old track_vis behavior
arc_data = ArcData(
center=(float(center[0]), float(center[1])),
width=float(2 * radius),
height=float(2 * radius),
angle=0.0,
theta1=float(theta1),
theta2=float(theta2),
theta1_original=float(theta1_original),
theta2_original=float(theta2_original),
n_points=int(num_steps),
)
arc_data_list.append(arc_data)
# Generate points along the arc
# Use num_steps to determine resolution
n_points = max(int(num_steps), 2)
angles = np.linspace(travel_start, travel_start + travel_rads, n_points)
# Calculate positions along the arc
arc_x = center[0] + radius * np.cos(angles)
arc_y = center[1] + radius * np.sin(angles)
x_coords.extend(arc_x)
y_coords.extend(arc_y)
# Update heading and position for next corner
curr_heading += travel_rads
curr_pos = center + radius * np.array(
[np.cos(travel_start + travel_rads), np.sin(travel_start + travel_rads)]
)
return np.array(x_coords), np.array(y_coords), arc_data_list
[docs]
def calculate_cumulative_distances(
x_m: npt.NDArray[np.float64], y_m: npt.NDArray[np.float64]
) -> Tuple[float, npt.NDArray[np.float64]]:
"""
Calculates cumulative distances along a path defined by x, y coordinates.
Parameters
----------
x_m : NDArray[float64]
Array of x coordinates in meters
y_m : NDArray[float64]
Array of y coordinates in meters
Returns
-------
Tuple[float, NDArray[float64]]
(track_length, cumulative_distances) - Total track length and array of cumulative distances
"""
cum_sum = [0.0]
track_length = 0.0
for i in range(len(x_m) - 1):
track_length += np.sqrt((x_m[i] - x_m[i + 1]) ** 2 + (y_m[i] - y_m[i + 1]) ** 2)
cum_sum.append(track_length)
return track_length, np.array(cum_sum)
[docs]
def calculate_menger_curvature(
x_m: npt.NDArray[np.float64],
y_m: npt.NDArray[np.float64],
cumulative_dist: npt.NDArray[np.float64],
dist: float,
sample_dist: float,
) -> Tuple[float, float]:
"""
Calculates curvature at a given distance using the Menger curvature formula.
Uses three points: one at dist-sample_dist, one at dist, and one at dist+sample_dist.
Parameters
----------
x_m : NDArray[float64]
Array of x coordinates in meters
y_m : NDArray[float64]
Array of y coordinates in meters
cumulative_dist : NDArray[float64]
Array of cumulative distances
dist : float
Distance along track to calculate curvature
sample_dist : float
Distance to sample before/after the point
Returns
-------
Tuple[float, float]
(curvature, radius) - Curvature (1/m) and radius of curvature (m)
"""
if dist < 0 or dist > cumulative_dist[-1]:
raise ValueError("Distance along track is out of track's range.")
if dist == 0 or dist == cumulative_dist[-1]:
# We use an arbitrarily large number instead of "inf" to denote straights,
# for stability during computation. Ensure this value is greater than the
# corresponding sentinel value in max_stable_speed()
return (0.0, 11000.0)
# Find indices for three points
p_back = max(dist - sample_dist, 0)
p_front = min(dist + sample_dist, cumulative_dist[-1])
idx1 = np.searchsorted(cumulative_dist, p_back, side="left")
idx2 = np.searchsorted(cumulative_dist, dist, side="left")
idx3 = min(
np.searchsorted(cumulative_dist, p_front, side="left"), len(cumulative_dist) - 1
)
x1, y1 = x_m[idx1], y_m[idx1]
x2, y2 = x_m[idx2], y_m[idx2]
x3, y3 = x_m[idx3], y_m[idx3]
# Calculate side lengths of triangle
a = np.hypot(x2 - x1, y2 - y1)
b = np.hypot(x3 - x2, y3 - y2)
c = np.hypot(x3 - x1, y3 - y1)
# Calculate area of triangle
area = abs(0.5 * ((x1 * (y2 - y3)) + (x2 * (y3 - y1)) + (x3 * (y1 - y2))))
# Return inf radius (straight line) if area is too small
if area < 1e-9:
return (0.0, 11000.0)
# Menger curvature formula: κ = 4A / (abc)
curvature = (4 * area) / (a * b * c)
radius = 1 / curvature
return (curvature, radius)
[docs]
def calculate_radii(
x_m: npt.NDArray[np.float64],
y_m: npt.NDArray[np.float64],
cumulative_dist: npt.NDArray[np.float64],
sample_dist: float,
) -> Tuple[npt.NDArray[np.float64], npt.NDArray[np.float64], npt.NDArray[np.float64]]:
"""
Calculates the radius of curvature at each point in a track.
Parameters
----------
x_m : NDArray[float64]
Array of x coordinates in meters
y_m : NDArray[float64]
Array of y coordinates in meters
cumulative_dist : NDArray[float64]
Array of cumulative distances
sample_dist : float
Distance to sample for Menger curvature calculation
Returns
-------
npt.NDArray[float64]
An array containing the Radius of Curvature at each point
"""