├── LQR.ipynb ├── coil.gif └── cubic_spline_planner.py /coil.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kowshikchills/LQR_python/faec25f09fdbc09d1a011773b5c08935ce734497/coil.gif -------------------------------------------------------------------------------- /cubic_spline_planner.py: -------------------------------------------------------------------------------- 1 | import math 2 | import numpy as np 3 | import bisect 4 | 5 | 6 | class Spline: 7 | """ 8 | Cubic Spline class 9 | """ 10 | 11 | def __init__(self, x, y): 12 | self.b, self.c, self.d, self.w = [], [], [], [] 13 | 14 | self.x = x 15 | self.y = y 16 | 17 | self.nx = len(x) # dimension of x 18 | h = np.diff(x) 19 | 20 | # calc coefficient c 21 | self.a = [iy for iy in y] 22 | 23 | # calc coefficient c 24 | A = self.__calc_A(h) 25 | B = self.__calc_B(h) 26 | self.c = np.linalg.solve(A, B) 27 | # print(self.c1) 28 | 29 | # calc spline coefficient b and d 30 | for i in range(self.nx - 1): 31 | self.d.append((self.c[i + 1] - self.c[i]) / (3.0 * h[i])) 32 | tb = (self.a[i + 1] - self.a[i]) / h[i] - h[i] * \ 33 | (self.c[i + 1] + 2.0 * self.c[i]) / 3.0 34 | self.b.append(tb) 35 | 36 | def calc(self, t): 37 | """ 38 | Calc position 39 | if t is outside of the input x, return None 40 | """ 41 | 42 | if t < self.x[0]: 43 | return None 44 | elif t > self.x[-1]: 45 | return None 46 | 47 | i = self.__search_index(t) 48 | dx = t - self.x[i] 49 | result = self.a[i] + self.b[i] * dx + \ 50 | self.c[i] * dx ** 2.0 + self.d[i] * dx ** 3.0 51 | 52 | return result 53 | 54 | def calcd(self, t): 55 | """ 56 | Calc first derivative 57 | if t is outside of the input x, return None 58 | """ 59 | 60 | if t < self.x[0]: 61 | return None 62 | elif t > self.x[-1]: 63 | return None 64 | 65 | i = self.__search_index(t) 66 | dx = t - self.x[i] 67 | result = self.b[i] + 2.0 * self.c[i] * dx + 3.0 * self.d[i] * dx ** 2.0 68 | return result 69 | 70 | def calcdd(self, t): 71 | """ 72 | Calc second derivative 73 | """ 74 | 75 | if t < self.x[0]: 76 | return None 77 | elif t > self.x[-1]: 78 | return None 79 | 80 | i = self.__search_index(t) 81 | dx = t - self.x[i] 82 | result = 2.0 * self.c[i] + 6.0 * self.d[i] * dx 83 | return result 84 | 85 | def __search_index(self, x): 86 | """ 87 | search data segment index 88 | """ 89 | return bisect.bisect(self.x, x) - 1 90 | 91 | def __calc_A(self, h): 92 | """ 93 | calc matrix A for spline coefficient c 94 | """ 95 | A = np.zeros((self.nx, self.nx)) 96 | A[0, 0] = 1.0 97 | for i in range(self.nx - 1): 98 | if i != (self.nx - 2): 99 | A[i + 1, i + 1] = 2.0 * (h[i] + h[i + 1]) 100 | A[i + 1, i] = h[i] 101 | A[i, i + 1] = h[i] 102 | 103 | A[0, 1] = 0.0 104 | A[self.nx - 1, self.nx - 2] = 0.0 105 | A[self.nx - 1, self.nx - 1] = 1.0 106 | # print(A) 107 | return A 108 | 109 | def __calc_B(self, h): 110 | """ 111 | calc matrix B for spline coefficient c 112 | """ 113 | B = np.zeros(self.nx) 114 | for i in range(self.nx - 2): 115 | B[i + 1] = 3.0 * (self.a[i + 2] - self.a[i + 1]) / \ 116 | h[i + 1] - 3.0 * (self.a[i + 1] - self.a[i]) / h[i] 117 | return B 118 | 119 | 120 | class Spline2D: 121 | """ 122 | 2D Cubic Spline class 123 | """ 124 | 125 | def __init__(self, x, y): 126 | self.s = self.__calc_s(x, y) 127 | self.sx = Spline(self.s, x) 128 | self.sy = Spline(self.s, y) 129 | 130 | def __calc_s(self, x, y): 131 | dx = np.diff(x) 132 | dy = np.diff(y) 133 | self.ds = [math.sqrt(idx ** 2 + idy ** 2) 134 | for (idx, idy) in zip(dx, dy)] 135 | s = [0] 136 | s.extend(np.cumsum(self.ds)) 137 | return s 138 | 139 | def calc_position(self, s): 140 | """ 141 | calc position 142 | """ 143 | x = self.sx.calc(s) 144 | y = self.sy.calc(s) 145 | 146 | return x, y 147 | 148 | def calc_curvature(self, s): 149 | """ 150 | calc curvature 151 | """ 152 | dx = self.sx.calcd(s) 153 | ddx = self.sx.calcdd(s) 154 | dy = self.sy.calcd(s) 155 | ddy = self.sy.calcdd(s) 156 | k = (ddy * dx - ddx * dy) / ((dx ** 2 + dy ** 2)**(3 / 2)) 157 | return k 158 | 159 | def calc_yaw(self, s): 160 | """ 161 | calc yaw 162 | """ 163 | dx = self.sx.calcd(s) 164 | dy = self.sy.calcd(s) 165 | yaw = math.atan2(dy, dx) 166 | return yaw 167 | 168 | 169 | def calc_spline_course(x, y, ds=0.1): 170 | sp = Spline2D(x, y) 171 | s = list(np.arange(0, sp.s[-1], ds)) 172 | 173 | rx, ry, ryaw, rk = [], [], [], [] 174 | for i_s in s: 175 | ix, iy = sp.calc_position(i_s) 176 | rx.append(ix) 177 | ry.append(iy) 178 | ryaw.append(sp.calc_yaw(i_s)) 179 | rk.append(sp.calc_curvature(i_s)) 180 | 181 | return rx, ry, ryaw, rk, s 182 | 183 | 184 | def main(): 185 | print("Spline 2D test") 186 | import matplotlib.pyplot as plt 187 | x = [-2.5, 0.0, 2.5, 5.0, 7.5, 3.0, -1.0] 188 | y = [0.7, -6, 5, 6.5, 0.0, 5.0, -2.0] 189 | ds = 0.1 # [m] distance of each intepolated points 190 | 191 | sp = Spline2D(x, y) 192 | s = np.arange(0, sp.s[-1], ds) 193 | 194 | rx, ry, ryaw, rk = [], [], [], [] 195 | for i_s in s: 196 | ix, iy = sp.calc_position(i_s) 197 | rx.append(ix) 198 | ry.append(iy) 199 | ryaw.append(sp.calc_yaw(i_s)) 200 | rk.append(sp.calc_curvature(i_s)) 201 | 202 | flg, ax = plt.subplots(1) 203 | plt.plot(x, y, "xb", label="input") 204 | plt.plot(rx, ry, "-r", label="spline") 205 | plt.grid(True) 206 | plt.axis("equal") 207 | plt.xlabel("x[m]") 208 | plt.ylabel("y[m]") 209 | plt.legend() 210 | 211 | flg, ax = plt.subplots(1) 212 | plt.plot(s, [math.degrees(iyaw) for iyaw in ryaw], "-r", label="yaw") 213 | plt.grid(True) 214 | plt.legend() 215 | plt.xlabel("line length[m]") 216 | plt.ylabel("yaw angle[deg]") 217 | 218 | flg, ax = plt.subplots(1) 219 | plt.plot(s, rk, "-r", label="curvature") 220 | plt.grid(True) 221 | plt.legend() 222 | plt.xlabel("line length[m]") 223 | plt.ylabel("curvature [1/m]") 224 | 225 | plt.show() 226 | 227 | 228 | if __name__ == '__main__': 229 | main() --------------------------------------------------------------------------------