Add inaccurate version of cart2frenet for use in lap counting

gym/f110_gym/envs/cubic_spline.py

@@ -8,6 +8,47 @@
 import numpy as np
 import scipy.optimize as so
 from scipy import interpolate
+from numba import njit
+
+@njit(fastmath=False, cache=True)
+def nearest_point_on_trajectory(point, trajectory):
+    """
+    Return the nearest point along the given piecewise linear trajectory.
+
+    Same as nearest_point_on_line_segment, but vectorized. This method is quite fast, time constraints should
+    not be an issue so long as trajectories are not insanely long.
+
+        Order of magnitude: trajectory length: 1000 --> 0.0002 second computation (5000fps)
+
+    point: size 2 numpy array
+    trajectory: Nx2 matrix of (x,y) trajectory waypoints
+        - these must be unique. If they are not unique, a divide by 0 error will destroy the world
+    """
+    diffs = trajectory[1:, :] - trajectory[:-1, :]
+    l2s = diffs[:, 0] ** 2 + diffs[:, 1] ** 2
+    # this is equivalent to the elementwise dot product
+    # dots = np.sum((point - trajectory[:-1,:]) * diffs[:,:], axis=1)
+    dots = np.empty((trajectory.shape[0] - 1,))
+    for i in range(dots.shape[0]):
+        dots[i] = np.dot((point - trajectory[i, :]), diffs[i, :])
+    t = dots / l2s
+    t[t < 0.0] = 0.0
+    t[t > 1.0] = 1.0
+    # t = np.clip(dots / l2s, 0.0, 1.0)
+    projections = trajectory[:-1, :] + (t * diffs.T).T
+    # dists = np.linalg.norm(point - projections, axis=1)
+    dists = np.empty((projections.shape[0],))
+    for i in range(dists.shape[0]):
+        temp = point - projections[i]
+        dists[i] = np.sqrt(np.sum(temp * temp))
+    min_dist_segment = np.argmin(dists)
+    return (
+        projections[min_dist_segment],
+        dists[min_dist_segment],
+        t[min_dist_segment],
+        min_dist_segment,
+    )
+
 class CubicSpline2D:
     """
     Cubic CubicSpline2D class
@@ -20,13 +61,13 @@ class CubicSpline2D:
     """
 
     def __init__(self, x, 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])
+        self.points = np.c_[x, y]
+        if not np.all(self.points[-1] == self.points[0]):
+            self.points = np.vstack((self.points, self.points[0]))  # Ensure the path is closed
+        self.s = self.__calc_s(self.points[:, 0], self.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')
+        self.spline = interpolate.CubicSpline(self.s, self.points, bc_type='periodic')
 
     def __calc_s(self, x, y):
         dx = np.diff(x)
@@ -114,6 +155,30 @@ def distance_to_spline(s):
         absolute_distance = output[1]
         return closest_s, absolute_distance
 
+    def calc_arclength_inaccurate(self, x, y, s_guess=0.0):
+        """
+        calc arclength, use nearest_point_on_trajectory
+        Less accuarate and less smooth than calc_arclength but
+        much faster - suitable for lap counting
+        Parameters
+        ----------
+        x : float
+            x position.
+        y : float
+            y position.
+        Returns
+        -------
+        s : float
+            distance from the start point for given x, y.
+        ey : float
+            lateral deviation for given x, y.
+        """
+        _, ey, t, min_dist_segment = nearest_point_on_trajectory(np.array([x, y]), self.points)
+        # s = s at closest_point + t
+        s = self.s[min_dist_segment] + t * (self.s[min_dist_segment + 1] - self.s[min_dist_segment])
+
+        return s, 0
+
     def _calc_tangent(self, s):
         '''
         calculates the tangent to the curve at a given point

gym/f110_gym/envs/track.py

@@ -281,10 +281,12 @@ def cartesian_to_frenet(self, x, y, phi, s_guess=0):
             ephi: heading deviation
         """
         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 > self.centerline.spline.s[-1]:
+            # Wrap around
+            s = s - self.centerline.spline.s[-1]
         if s < 0:
-            s = self.centerline.spline.s[-1]
+            # Negative s means we are behind the start point
+            s = s + self.centerline.spline.s[-1]
 
         # Use the normal to calculate the signed lateral deviation
         normal = self.centerline.spline._calc_normal(s)