From ec9834d8b65547b7b6e40e914edae1bb697c7be4 Mon Sep 17 00:00:00 2001 From: Ahmad Amine Date: Thu, 4 Apr 2024 00:50:42 -0400 Subject: [PATCH] Now uses scipy cubicspline for interpolation with proper endpoint check. Works for full track --- gym/f110_gym/envs/cubic_spline.py | 195 ++++-------------------------- gym/f110_gym/envs/track.py | 17 +-- 2 files changed, 31 insertions(+), 181 deletions(-) diff --git a/gym/f110_gym/envs/cubic_spline.py b/gym/f110_gym/envs/cubic_spline.py index 55111508..aedf7435 100644 --- a/gym/f110_gym/envs/cubic_spline.py +++ b/gym/f110_gym/envs/cubic_spline.py @@ -1,151 +1,13 @@ """ -Code from Cubic spline planner -Author: Atsushi Sakai(@Atsushi_twi) +Cubic Spline interpolation using scipy.interpolate +Provides utilities for position, curvature, yaw, and arclength calculation """ -import bisect + import math import numpy as np -from scipy.spatial import distance as ssd import scipy.optimize as so - - -class CubicSpline1D: - """ - 1D Cubic Spline class - Parameters - ---------- - x : list - x coordinates for data points. This x coordinates must be - sorted in ascending order. - y : list - y coordinates for data points - """ - - def __init__(self, x, y): - h = np.diff(x) - if np.any(h < 0): - raise ValueError("x coordinates must be sorted in ascending order") - - self.a, self.b, self.c, self.d = [], [], [], [] - self.x = x - self.y = y - self.nx = len(x) # dimension of x - - # calc coefficient a - self.a = [iy for iy in y] - - # calc coefficient c - A = self.__calc_A(h) - B = self.__calc_B(h, self.a) - self.c = np.linalg.solve(A, B) - - # calc spline coefficient b and d - for i in range(self.nx - 1): - d = (self.c[i + 1] - self.c[i]) / (3.0 * h[i]) - b = 1.0 / h[i] * (self.a[i + 1] - self.a[i]) - h[i] / 3.0 * ( - 2.0 * self.c[i] + self.c[i + 1] - ) - self.d.append(d) - self.b.append(b) - - def calc_position(self, x): - """ - Calc `y` position for given `x`. - if `x` is outside the data point's `x` range, return None. - Returns - ------- - y : float - y position for given x. - """ - if x < self.x[0]: - return None - elif x > self.x[-1]: - return None - - i = self.__search_index(x) - dx = x - self.x[i] - position = ( - self.a[i] + self.b[i] * dx + self.c[i] * dx**2.0 + self.d[i] * dx**3.0 - ) - - return position - - def calc_first_derivative(self, x): - """ - Calc first derivative at given x. - if x is outside the input x, return None - Returns - ------- - dy : float - first derivative for given x. - """ - - if x < self.x[0]: - return None - elif x > self.x[-1]: - return None - - i = self.__search_index(x) - dx = x - self.x[i] - dy = self.b[i] + 2.0 * self.c[i] * dx + 3.0 * self.d[i] * dx**2.0 - return dy - - def calc_second_derivative(self, x): - """ - Calc second derivative at given x. - if x is outside the input x, return None - Returns - ------- - ddy : float - second derivative for given x. - """ - - if x < self.x[0]: - return None - elif x > self.x[-1]: - return None - - i = self.__search_index(x) - dx = x - self.x[i] - ddy = 2.0 * self.c[i] + 6.0 * self.d[i] * dx - return ddy - - def __search_index(self, x): - """ - search data segment index - """ - return bisect.bisect(self.x[:-1], x) - 1 - - def __calc_A(self, h): - """ - calc matrix A for spline coefficient c - """ - A = np.zeros((self.nx, self.nx)) - A[0, 0] = 1.0 - for i in range(self.nx - 1): - if i != (self.nx - 2): - A[i + 1, i + 1] = 2.0 * (h[i] + h[i + 1]) - A[i + 1, i] = h[i] - A[i, i + 1] = h[i] - - A[0, 1] = 0.0 - A[self.nx - 1, self.nx - 2] = 0.0 - A[self.nx - 1, self.nx - 1] = 1.0 - return A - - def __calc_B(self, h, a): - """ - calc matrix B for spline coefficient c - """ - B = np.zeros(self.nx) - for i in range(self.nx - 2): - B[i + 1] = ( - 3.0 * (a[i + 2] - a[i + 1]) / h[i + 1] - 3.0 * (a[i + 1] - a[i]) / h[i] - ) - return B - - +from scipy import interpolate class CubicSpline2D: """ Cubic CubicSpline2D class @@ -158,9 +20,13 @@ class CubicSpline2D: """ def __init__(self, x, y): - self.s = self.__calc_s(x, y) - self.sx = CubicSpline1D(self.s, x) - self.sy = CubicSpline1D(self.s, y) + points = np.c_[x, y] + if not np.all(points[-1] == points[0]): + points = np.vstack((points, points[0])) # Ensure the path is closed + self.s = self.__calc_s(points[:, 0], points[:, 1]) + # Use scipy CubicSpline to interpolate the points with periodic boundary conditions + # This is necessary to ensure the path is continuous + self.spline = interpolate.CubicSpline(self.s, points, bc_type='periodic') def __calc_s(self, x, y): dx = np.diff(x) @@ -185,10 +51,7 @@ def calc_position(self, s): y : float y position for given s. """ - x = self.sx.calc_position(s) - y = self.sy.calc_position(s) - - return x, y + return self.spline(s) def calc_curvature(self, s): """ @@ -203,10 +66,8 @@ def calc_curvature(self, s): k : float curvature for given s. """ - dx = self.sx.calc_first_derivative(s) - ddx = self.sx.calc_second_derivative(s) - dy = self.sy.calc_first_derivative(s) - ddy = self.sy.calc_second_derivative(s) + dx, dy = self.spline(s, 1) + ddx, ddy = self.spline(s, 2) k = (ddy * dx - ddx * dy) / ((dx**2 + dy**2) ** (3 / 2)) return k @@ -223,12 +84,11 @@ def calc_yaw(self, s): yaw : float yaw angle (tangent vector) for given s. """ - dx = self.sx.calc_first_derivative(s) - dy = self.sy.calc_first_derivative(s) + dx, dy = self.spline(s, 1) yaw = math.atan2(dy, dx) return yaw - def calc_arclength(self, x, y): + def calc_arclength(self, x, y, s_guess=0.0): """ calc arclength Parameters @@ -246,18 +106,10 @@ def calc_arclength(self, x, y): """ def distance_to_spline(s): - if s < 0: - x_eval = self.sx.calc_position(0) - y_eval = self.sy.calc_position(0) - elif s > self.s[-1]: - x_eval = self.sx.calc_position(self.s[-1]) - y_eval = self.sy.calc_position(self.s[-1]) - else: - x_eval = self.sx.calc_position(s) - y_eval = self.sy.calc_position(s) + x_eval, y_eval = self.spline(s)[0] return np.sqrt((x - x_eval)**2 + (y - y_eval)**2) - output = so.fmin(distance_to_spline, 0, full_output=True, disp=False) + output = so.fmin(distance_to_spline, s_guess, full_output=True, disp=False) closest_s = output[0][0] absolute_distance = output[1] return closest_s, absolute_distance @@ -275,12 +127,7 @@ def _calc_tangent(self, s): tangent : float tangent vector for given s. ''' - if s < 0: - return None - elif s > self.s[-1]: - return None - dx = self.sx.calc_first_derivative(s) - dy = self.sy.calc_first_derivative(s) + dx, dy = self.spline(s, 1) tangent = np.array([dx, dy]) return tangent @@ -297,6 +144,6 @@ def _calc_normal(self, s): normal : float normal vector for given s. ''' - tangent = self._calc_tangent(s) - normal = np.array([-tangent[1], tangent[0]]) + dx, dy = self.spline(s, 1) + normal = np.array([-dy, dx]) return normal \ No newline at end of file diff --git a/gym/f110_gym/envs/track.py b/gym/f110_gym/envs/track.py index 10ea753d..8cabfb49 100644 --- a/gym/f110_gym/envs/track.py +++ b/gym/f110_gym/envs/track.py @@ -255,8 +255,8 @@ def frenet_to_cartesian(self, s, ey, ephi): y: y-coordinate psi: yaw angle """ - x, y = self.raceline.spline.calc_position(s) - psi = self.raceline.spline.calc_yaw(s) + x, y = self.centerline.spline.calc_position(s) + psi = self.centerline.spline.calc_yaw(s) # Adjust x,y by shifting along the normal vector x -= ey * np.sin(psi) @@ -267,7 +267,7 @@ def frenet_to_cartesian(self, s, ey, ephi): return x, y, psi - def cartesian_to_frenet(self, x, y, phi): + def cartesian_to_frenet(self, x, y, phi, s_guess=0): """ Convert Cartesian coordinates to Frenet coordinates. @@ -280,12 +280,15 @@ def cartesian_to_frenet(self, x, y, phi): ey: lateral deviation ephi: heading deviation """ - s, ey = self.centerline.spline.calc_arclength(x, y) - + s, ey = self.centerline.spline.calc_arclength(x, y, s_guess) + if abs(s - self.centerline.spline.s[-1]) < 1e-6 or s > self.centerline.spline.s[-1]: + s = 0 + if s < 0: + s = self.centerline.spline.s[-1] + # Use the normal to calculate the signed lateral deviation normal = self.centerline.spline._calc_normal(s) - x_eval = self.centerline.spline.sx.calc_position(s) - y_eval = self.centerline.spline.sy.calc_position(s) + x_eval, y_eval = self.centerline.spline.calc_position(s) dx = x - x_eval dy = y - y_eval distance_sign = np.sign(np.dot([dx, dy], normal))