Raceline cubic spline

driverless_ws/src/planning/src/testing_framework/desc_math.txt

@@ -0,0 +1 @@
+Describe the math and intuition in this file

driverless_ws/src/planning/src/testing_framework/my_optimizer.py

@@ -0,0 +1,130 @@
+import numpy as np
+
+# find cublic spline given 2 points
+def cubic_spline_least_squares(point1, point2):
+    x1, y1 = point1
+    x2, y2 = point2
+
+    A = np.array([
+        [x1**3, x1**2, x1, 1],
+        [x2**3, x2**2, x2, 1]
+    ])
+    y = np.array([y1, y2])
+
+    coefficients, _, _, _ = np.linalg.lstsq(A, y, rcond=None)
+    return tuple(coefficients)
+
+# interpolate points given a cubic spline
+def interpolate_cubic(coefficients, x_values):
+    a, b, c, d = coefficients
+    y_values = [a * x**3 + b * x**2 + c * x + d for x in x_values]
+    return y_values
+
+# calculate point distance d from yellow point
+def point_along_line(blue_point, yellow_point, distance):
+    # vector from yellow to blue
+    direction_vector = np.array(blue_point) - np.array(yellow_point)
+    unit_vector = direction_vector / np.linalg.norm(direction_vector)
+    target_point = np.array(yellow_point) + distance * unit_vector
+    return tuple(target_point)
+
+# find initial three points
+def initial_points(blue_cones, yellow_cones, d1, d2):
+    blue_start = blue_cones[0]
+    yellow_start = yellow_cones[0]
+    blue_mid = blue_cones[len(blue_cones)//2]
+    yellow_mid = yellow_cones[len(yellow_cones)//2]
+    blue_end = blue_cones[-1]
+    yellow_end = yellow_cones[-1]
+
+    # first, second, and third points
+    points = []
+    points.append(((blue_start[0]+yellow_start[0])/2,(blue_start[1]+yellow_start[1])/2))
+
+    #points.append(((blue_mid[0]+yellow_mid[0])/2,(blue_mid[1]+yellow_mid[1])/2))
+    second_point = point_along_line(blue_mid, yellow_mid, d1)
+    points.append(second_point)
+
+    #points.append(((blue_end[0]+yellow_end[0])/2,(blue_end[1]+yellow_end[1])/2))
+    third_point = point_along_line(blue_end, yellow_end, d2)
+    points.append(third_point)
+
+    return points
+
+"""change this function, this is where you implement your raceline optimizer"""
+def run_optimizer(blue_cones_x, blue_cones_y, yellow_cones_x, yellow_cones_y):
+    blue_c = list(zip(blue_cones_x, blue_cones_y))
+    yellow_c = list(zip(yellow_cones_x, yellow_cones_y))
+
+    # sort blue and yellow cones by x value
+    blue_cones = sorted(blue_c, key=lambda cone: cone[0])
+    yellow_cones = sorted(yellow_c, key=lambda cone: cone[0])
+
+    # extract initial points
+    # 2 and 4 are d1 and d2 respectively
+    points = initial_points(blue_cones,yellow_cones,d1=2,d2=4)
+    print(points)
+    # test initial points
+    # points_x = [point[0] for point in points]
+    # points_y = [point[1] for point in points]
+    # return [points_x, points_y]
+
+    # find k and l by taking slope from first and last couple points
+    k = (blue_cones[0][1]-blue_cones[5][1])/(blue_cones[0][0]-blue_cones[5][0])
+    l = (blue_cones[-1][1]-blue_cones[-6][1])/(blue_cones[-1][0]-blue_cones[-6][0])
+    # print(k)
+    # print(l)
+
+    # set inital points
+    x1,y1 = points[0]
+    x2,y2 = points[1]
+    x3,y3 = points[2]
+
+    # input matrix for x and y values
+    b_x = np.array([x1,x2,x2,x3,k,l,0,0])
+    b_y = np.array([y1,y2,y2,y3,k,l,0,0])
+    # print(b_x)
+    # print(b_y)
+
+    # hardcode inverse matrix
+    A_inverse = np.array([
+        [10, -10, -6, 6, 3, -1, 2, 0.25],
+        [-9, 9, 3, -3, -3.5, 0.5, -1, -0.125],
+        [0, 0, 0, 0, 1, 0, 0, 0],
+        [1, 0, 0, 0, 0, 0, 0, 0],
+        [-6, 6, 10, -10, -1, 3, -2, 0.25],
+        [6, -6, -6, 6, 1, -1, 2, -0.25],
+        [-1.5, 1.5, -1.5, 1.5, -0.25, -0.25, -0.5, 0.0625],
+        [0, 0, 1, 0, 0, 0, 0, 0]
+    ])
+
+    # solve large matrix equation
+    X = A_inverse @ b_x
+    Y = A_inverse @ b_y
+
+    # set coefficients
+    a1x, b1x, c1x, d1x, a2x, b2x, c2x, d2x = X
+    a1y, b1y, c1y, d1y, a2y, b2y, c2y, d2y = Y
+
+    # dump coefficients into np array
+    spline_x1 = np.array([a1x,b1x,c1x,d1x])
+    spline_x2 = np.array([a2x,b2x,c2x,d2x])
+    spline_y1 = np.array([a1y,b1y,c1y,d1y])
+    spline_y2 = np.array([a2y,b2y,c2y,d2y])
+
+    # set base values (25 points each)
+    base = np.linspace(0,0.5,num=500)
+
+    # outputs of cubic funtion
+    splinex1 = interpolate_cubic(spline_x1,base)
+    splinex2 = interpolate_cubic(spline_x2,base)
+    spliney1 = interpolate_cubic(spline_y1,base)
+    spliney2 = interpolate_cubic(spline_y2,base)
+
+    # combine the arrays
+    spline_x = np.append(splinex1,splinex2)
+    spline_y = np.append(spliney1,spliney2)
+
+    return [spline_x,spline_y]
+
+    # return [np.array([1,2,3]), np.array([1,2,3])]

driverless_ws/src/planning/src/testing_framework/optimizer_tester.py

@@ -0,0 +1,169 @@
+import my_optimizer
+import matplotlib.pyplot as plt
+import math
+import numpy as np
+from sklearn import svm
+import time
+
+class TestSuite:
+    def __init__(self):
+        self.s_curve_horizontal = []
+        self.s_curve_vertical = []
+        self.straight_horizontal = []
+        self.straight_vertical = []
+        self.hairpin_horizontal = []
+        self.hairpin_vertical = []
+
+        self.track_names = [
+            "s_curve_horizontal",
+            "s_curve_vertical",
+            "straight_horizontal",
+            "straight_vertical",
+            "hairpin_horizontal",
+            "hairpin_vertical"
+        ]
+
+        self.tracks = {
+                0 : self.s_curve_horizontal,
+                1 : self.s_curve_vertical,
+                2 : self.straight_horizontal,
+                3 : self.straight_vertical,
+                4 : self.hairpin_horizontal,
+                5 : self.hairpin_vertical,
+                }
+
+
+    def generate_tracks(self):
+        """generate_tracks Generates the lists of tuples representing a track
+                           track segment. Each tuple is in the form (x, y)
+                           where x and y are both integers, representing the
+                           coordinates of a cone on the track"""
+
+        # Generating the S curve using a sin function
+        blue_cones_x   = np.array([])
+        blue_cones_y   = np.array([])
+        yellow_cones_x = np.array([])
+        yellow_cones_y = np.array([])
+
+        for i in range(50):
+            v = math.sin(i / 10)
+            blue_cones_x = np.append(blue_cones_x, [i / 10])
+            blue_cones_y = np.append(blue_cones_y, [v + 2])
+            yellow_cones_x = np.append(yellow_cones_x, [i / 10])
+            yellow_cones_y = np.append(yellow_cones_y, [v - 2])
+
+        self.tracks[0] = [blue_cones_x, blue_cones_y, yellow_cones_x, yellow_cones_y]
+        self.tracks[1] = [blue_cones_y, blue_cones_x, yellow_cones_y, yellow_cones_x]
+        # Generating the straights
+        blue_cones_y = np.array([])
+        yellow_cones_y = np.array([])
+        for i in range(50):
+
+            blue_cones_y = np.append(blue_cones_y, [2])
+            yellow_cones_y = np.append(yellow_cones_y, [-2])
+
+        self.tracks[2] = [blue_cones_x, blue_cones_y, yellow_cones_x, yellow_cones_y]
+        self.tracks[3] = [blue_cones_y, blue_cones_x, yellow_cones_y, yellow_cones_x]
+
+        # Generating the hairpins
+        blue_cones_x = np.array([])
+        blue_cones_y = np.array([])
+        yellow_cones_x = np.array([])
+        yellow_cones_y = np.array([])
+        for i in range(1,100):
+            x = (-3.99 + (i/10))
+            v_bottom_blue = -1 * math.log2(x + 4) - 12
+            v_top_blue = math.log2(x + 4) + 5
+            v_bottom_yellow = -1 * math.log2(i/10) - 7
+            v_top_yellow = math.log2(i/10)
+            blue_cones_x = np.append(blue_cones_x, [x, x])
+            blue_cones_y = np.append(blue_cones_y, [v_bottom_blue, v_top_blue])
+            yellow_cones_x = np.append(yellow_cones_x, [i/10, i/10])
+            yellow_cones_y = np.append(yellow_cones_y, [v_bottom_yellow, v_top_yellow])
+
+        for i in range(4):
+            x = -3.995 + i * 0.01
+            v_bottom_blue = -1 * math.log2(x + 4) - 12
+            v_top_blue = math.log2(x + 4) + 5
+
+            blue_cones_x = np.append(blue_cones_x, [x, x])
+            blue_cones_y = np.append(blue_cones_y, [v_bottom_blue, v_top_blue])
+
+
+        self.tracks[4] = [blue_cones_y, blue_cones_x, yellow_cones_y, yellow_cones_x]
+        self.tracks[5] = [blue_cones_x, blue_cones_y, yellow_cones_x, yellow_cones_y]
+
+
+TS = TestSuite()
+TS.generate_tracks()
+prompt_string = """Welcome to the Raceline Optimizer test suite!
+1.) Name your raceline optimizer file: my_optimizer.py
+When defining your raceline optimizer in my_optimizer.py, please define the
+following functions:
+###############################################################################
+
+run_optimizer This function takes in cones and runs the optimizer on cones
+
+:param blue_cones_x: A numpy array of x coordinates, representing x-coordinate
+of blue cones in the racetrack segment your optimizer will be tested on
+
+:param blue_cones_y: A numpy array of y coordinates, representing y-coordinate
+of blue cones in the racetrack segment your optimizer will be tested on
+
+:param yellow_cones_x: A numpy array of x coordinates, representing x-coordinate
+of yellow cones in the racetrack segment your optimizer will be tested on
+
+:param yellow_cones_y: A numpy array of y coordinates, representing y-coordinate
+of yellow cones in the racetrack segment your optimizer will be tested on
+
+:return: A list containing 2 numpy arrays. The 1st numpy array contains the
+x coordinate of the raceline. The 2nd numpy array contains the y coordinate
+of the raceline.
+###############################################################################
+2.) Select a track that you would like to test on
+Below are options for different tracks that you can test you're optimizer on:\n"""
+
+print(prompt_string)
+
+
+for i in range(len(TS.track_names)):
+    print("\t",i, " : ", TS.track_names[i])
+
+while True:
+    tracktype = input(("\nTo test on a specific track, PLEASE TYPE THE NUMBER" +
+                    " to the left of the track (ex. 1). \nTo quit, type q." +
+                    "To view the tracks, type t: "))
+    if tracktype == 'q':
+        break
+    elif tracktype == 't':
+        print()
+        for i in range(len(TS.track_names)):
+            print("\t",i, " : ", TS.track_names[i])
+
+    elif int(tracktype) in TS.tracks.keys():
+        print("Press the x on the top right corner to close",
+                "of the visualization window")
+        cur_track = TS.tracks[int(tracktype)]
+        start_time = time.time()
+        raceline = my_optimizer.run_optimizer(cur_track[0], cur_track[1],
+                                                cur_track[2], cur_track[3])
+        end_time = time.time()
+        calc_time = end_time - start_time
+        # raceline = [np.array([1, 2, 3, 4, 5]), np.array([1, 2, 3, 5, 6])]
+        plt.style.use('dark_background')
+        plt.scatter(cur_track[0], cur_track[1], color='blue')
+        plt.scatter(cur_track[2], cur_track[3], color='yellow')
+        plt.scatter(raceline[0], raceline[1])
+        print("\nRaceline generation time: ", calc_time, "seconds")
+        plt.show()
+
+    else:
+        print("Error: please type in a number corresponding to a track")
+
+
+
+
+
+
+
+