├── Optimal Trajectory in a Frenet Frame ├── cubic_spline_planner.py └── frenet_optimal_trajectory.py ├── OptimalTrajectoryGenerationforDynamicStreetScenariosinaFrenetFrame.pdf ├── Quintic-Polynomial-Solver └── main.cpp ├── README.md ├── TrajectoryExercise2 ├── constants.py ├── cost_functions.py ├── evaluate_ptg.py ├── helpers.py └── ptg.py └── readme_img ├── Jerk Minimizing Trajectories.png ├── Polynomial solver.png ├── animation.gif ├── cost1.png ├── feasibility.png ├── fenet.png ├── formula1.png ├── formula2.png ├── formula3.png ├── jerk1.png ├── overshoot.png ├── paper_pic1.png ├── paper_pic2.png ├── paper_pic3.png └── paper_pic_result.png /Optimal Trajectory in a Frenet Frame/cubic_spline_planner.py: -------------------------------------------------------------------------------- 1 | """ 2 | cubic spline planner 3 | 4 | Author: Atsushi Sakai 5 | 6 | """ 7 | import math 8 | import numpy as np 9 | import bisect 10 | 11 | 12 | class Spline: 13 | u""" 14 | Cubic Spline class 15 | """ 16 | 17 | def __init__(self, x, y): 18 | self.b, self.c, self.d, self.w = [], [], [], [] 19 | 20 | self.x = x 21 | self.y = y 22 | 23 | self.nx = len(x) # dimension of x 24 | h = np.diff(x) 25 | 26 | # calc coefficient c 27 | self.a = [iy for iy in y] 28 | 29 | # calc coefficient c 30 | A = self.__calc_A(h) 31 | B = self.__calc_B(h) 32 | self.c = np.linalg.solve(A, B) 33 | # print(self.c1) 34 | 35 | # calc spline coefficient b and d 36 | for i in range(self.nx - 1): 37 | self.d.append((self.c[i + 1] - self.c[i]) / (3.0 * h[i])) 38 | tb = (self.a[i + 1] - self.a[i]) / h[i] - h[i] * \ 39 | (self.c[i + 1] + 2.0 * self.c[i]) / 3.0 40 | self.b.append(tb) 41 | 42 | def calc(self, t): 43 | u""" 44 | Calc position 45 | 46 | if t is outside of the input x, return None 47 | 48 | """ 49 | 50 | if t < self.x[0]: 51 | return None 52 | elif t > self.x[-1]: 53 | return None 54 | 55 | i = self.__search_index(t) 56 | dx = t - self.x[i] 57 | result = self.a[i] + self.b[i] * dx + \ 58 | self.c[i] * dx ** 2.0 + self.d[i] * dx ** 3.0 59 | 60 | return result 61 | 62 | def calcd(self, t): 63 | u""" 64 | Calc first derivative 65 | 66 | if t is outside of the input x, return None 67 | """ 68 | 69 | if t < self.x[0]: 70 | return None 71 | elif t > self.x[-1]: 72 | return None 73 | 74 | i = self.__search_index(t) 75 | dx = t - self.x[i] 76 | result = self.b[i] + 2.0 * self.c[i] * dx + 3.0 * self.d[i] * dx ** 2.0 77 | return result 78 | 79 | def calcdd(self, t): 80 | u""" 81 | Calc second derivative 82 | """ 83 | 84 | if t < self.x[0]: 85 | return None 86 | elif t > self.x[-1]: 87 | return None 88 | 89 | i = self.__search_index(t) 90 | dx = t - self.x[i] 91 | result = 2.0 * self.c[i] + 6.0 * self.d[i] * dx 92 | return result 93 | 94 | def __search_index(self, x): 95 | u""" 96 | search data segment index 97 | """ 98 | return bisect.bisect(self.x, x) - 1 99 | 100 | def __calc_A(self, h): 101 | u""" 102 | calc matrix A for spline coefficient c 103 | """ 104 | A = np.zeros((self.nx, self.nx)) 105 | A[0, 0] = 1.0 106 | for i in range(self.nx - 1): 107 | if i != (self.nx - 2): 108 | A[i + 1, i + 1] = 2.0 * (h[i] + h[i + 1]) 109 | A[i + 1, i] = h[i] 110 | A[i, i + 1] = h[i] 111 | 112 | A[0, 1] = 0.0 113 | A[self.nx - 1, self.nx - 2] = 0.0 114 | A[self.nx - 1, self.nx - 1] = 1.0 115 | # print(A) 116 | return A 117 | 118 | def __calc_B(self, h): 119 | u""" 120 | calc matrix B for spline coefficient c 121 | """ 122 | B = np.zeros(self.nx) 123 | for i in range(self.nx - 2): 124 | B[i + 1] = 3.0 * (self.a[i + 2] - self.a[i + 1]) / \ 125 | h[i + 1] - 3.0 * (self.a[i + 1] - self.a[i]) / h[i] 126 | # print(B) 127 | return B 128 | 129 | 130 | class Spline2D: 131 | u""" 132 | 2D Cubic Spline class 133 | 134 | """ 135 | 136 | def __init__(self, x, y): 137 | self.s = self.__calc_s(x, y) 138 | self.sx = Spline(self.s, x) 139 | self.sy = Spline(self.s, y) 140 | 141 | def __calc_s(self, x, y): 142 | dx = np.diff(x) 143 | dy = np.diff(y) 144 | self.ds = [math.sqrt(idx ** 2 + idy ** 2) 145 | for (idx, idy) in zip(dx, dy)] 146 | s = [0] 147 | s.extend(np.cumsum(self.ds)) 148 | return s 149 | 150 | def calc_position(self, s): 151 | u""" 152 | calc position 153 | """ 154 | x = self.sx.calc(s) 155 | y = self.sy.calc(s) 156 | 157 | return x, y 158 | 159 | def calc_curvature(self, s): 160 | u""" 161 | calc curvature 162 | """ 163 | dx = self.sx.calcd(s) 164 | ddx = self.sx.calcdd(s) 165 | dy = self.sy.calcd(s) 166 | ddy = self.sy.calcdd(s) 167 | k = (ddy * dx - ddx * dy) / (dx ** 2 + dy ** 2) 168 | return k 169 | 170 | def calc_yaw(self, s): 171 | u""" 172 | calc yaw 173 | """ 174 | dx = self.sx.calcd(s) 175 | dy = self.sy.calcd(s) 176 | yaw = math.atan2(dy, dx) 177 | return yaw 178 | 179 | 180 | def calc_spline_course(x, y, ds=0.1): 181 | sp = Spline2D(x, y) 182 | s = list(np.arange(0, sp.s[-1], ds)) 183 | 184 | rx, ry, ryaw, rk = [], [], [], [] 185 | for i_s in s: 186 | ix, iy = sp.calc_position(i_s) 187 | rx.append(ix) 188 | ry.append(iy) 189 | ryaw.append(sp.calc_yaw(i_s)) 190 | rk.append(sp.calc_curvature(i_s)) 191 | 192 | return rx, ry, ryaw, rk, s 193 | 194 | 195 | def main(): 196 | print("Spline 2D test") 197 | import matplotlib.pyplot as plt 198 | x = [-2.5, 0.0, 2.5, 5.0, 7.5, 3.0, -1.0] 199 | y = [0.7, -6, 5, 6.5, 0.0, 5.0, -2.0] 200 | 201 | sp = Spline2D(x, y) 202 | s = np.arange(0, sp.s[-1], 0.1) 203 | 204 | rx, ry, ryaw, rk = [], [], [], [] 205 | for i_s in s: 206 | ix, iy = sp.calc_position(i_s) 207 | rx.append(ix) 208 | ry.append(iy) 209 | ryaw.append(sp.calc_yaw(i_s)) 210 | rk.append(sp.calc_curvature(i_s)) 211 | 212 | flg, ax = plt.subplots(1) 213 | plt.plot(x, y, "xb", label="input") 214 | plt.plot(rx, ry, "-r", label="spline") 215 | plt.grid(True) 216 | plt.axis("equal") 217 | plt.xlabel("x[m]") 218 | plt.ylabel("y[m]") 219 | plt.legend() 220 | 221 | flg, ax = plt.subplots(1) 222 | plt.plot(s, [math.degrees(iyaw) for iyaw in ryaw], "-r", label="yaw") 223 | plt.grid(True) 224 | plt.legend() 225 | plt.xlabel("line length[m]") 226 | plt.ylabel("yaw angle[deg]") 227 | 228 | flg, ax = plt.subplots(1) 229 | plt.plot(s, rk, "-r", label="curvature") 230 | plt.grid(True) 231 | plt.legend() 232 | plt.xlabel("line length[m]") 233 | plt.ylabel("curvature [1/m]") 234 | 235 | plt.show() 236 | 237 | 238 | if __name__ == '__main__': 239 | main() 240 | -------------------------------------------------------------------------------- /Optimal Trajectory in a Frenet Frame/frenet_optimal_trajectory.py: -------------------------------------------------------------------------------- 1 | """ 2 | 3 | Frenet optimal trajectory generator 4 | 5 | author: Atsushi Sakai (@Atsushi_twi) 6 | 7 | Ref: 8 | 9 | - [Optimal Trajectory Generation for Dynamic Street Scenarios in a Frenet Frame](https://www.researchgate.net/profile/Moritz_Werling/publication/224156269_Optimal_Trajectory_Generation_for_Dynamic_Street_Scenarios_in_a_Frenet_Frame/links/54f749df0cf210398e9277af.pdf) 10 | 11 | - [Optimal trajectory generation for dynamic street scenarios in a Frenet Frame](https://www.youtube.com/watch?v=Cj6tAQe7UCY) 12 | 13 | """ 14 | 15 | import numpy as np 16 | import matplotlib.pyplot as plt 17 | import copy 18 | import math 19 | import cubic_spline_planner 20 | 21 | # Parameter 22 | MAX_SPEED = 50.0 / 3.6 # maximum speed [m/s] 23 | MAX_ACCEL = 2.0 # maximum acceleration [m/ss] 24 | MAX_CURVATURE = 1.0 # maximum curvature [1/m] 25 | MAX_ROAD_WIDTH = 7.0 # maximum road width [m] 26 | D_ROAD_W = 1.0 # road width sampling length [m] 27 | DT = 0.2 # time tick [s] 28 | MAXT = 5.0 # max prediction time [m] 29 | MINT = 4.0 # min prediction time [m] 30 | TARGET_SPEED = 30.0 / 3.6 # target speed [m/s] 31 | D_T_S = 5.0 / 3.6 # target speed sampling length [m/s] 32 | N_S_SAMPLE = 1 # sampling number of target speed 33 | ROBOT_RADIUS = 2.0 # robot radius [m] 34 | 35 | # cost weights 36 | KJ = 0.1 37 | KT = 0.1 38 | KD = 1.0 39 | KLAT = 1.0 40 | KLON = 1.0 41 | 42 | show_animation = True 43 | 44 | 45 | class quintic_polynomial: 46 | 47 | def __init__(self, xs, vxs, axs, xe, vxe, axe, T): 48 | 49 | # calc coefficient of quintic polynomial 50 | self.xs = xs 51 | self.vxs = vxs 52 | self.axs = axs 53 | self.xe = xe 54 | self.vxe = vxe 55 | self.axe = axe 56 | 57 | self.a0 = xs 58 | self.a1 = vxs 59 | self.a2 = axs / 2.0 60 | 61 | A = np.array([[T**3, T**4, T**5], 62 | [3 * T ** 2, 4 * T ** 3, 5 * T ** 4], 63 | [6 * T, 12 * T ** 2, 20 * T ** 3]]) 64 | b = np.array([xe - self.a0 - self.a1 * T - self.a2 * T**2, 65 | vxe - self.a1 - 2 * self.a2 * T, 66 | axe - 2 * self.a2]) 67 | x = np.linalg.solve(A, b) 68 | 69 | self.a3 = x[0] 70 | self.a4 = x[1] 71 | self.a5 = x[2] 72 | 73 | def calc_point(self, t): 74 | xt = self.a0 + self.a1 * t + self.a2 * t**2 + \ 75 | self.a3 * t**3 + self.a4 * t**4 + self.a5 * t**5 76 | 77 | return xt 78 | 79 | def calc_first_derivative(self, t): 80 | xt = self.a1 + 2 * self.a2 * t + \ 81 | 3 * self.a3 * t**2 + 4 * self.a4 * t**3 + 5 * self.a5 * t**4 82 | 83 | return xt 84 | 85 | def calc_second_derivative(self, t): 86 | xt = 2 * self.a2 + 6 * self.a3 * t + 12 * self.a4 * t**2 + 20 * self.a5 * t**3 87 | 88 | return xt 89 | 90 | def calc_third_derivative(self, t): 91 | xt = 6 * self.a3 + 24 * self.a4 * t + 60 * self.a5 * t**2 92 | 93 | return xt 94 | 95 | 96 | class quartic_polynomial: 97 | 98 | def __init__(self, xs, vxs, axs, vxe, axe, T): 99 | 100 | # calc coefficient of quintic polynomial 101 | self.xs = xs 102 | self.vxs = vxs 103 | self.axs = axs 104 | self.vxe = vxe 105 | self.axe = axe 106 | 107 | self.a0 = xs 108 | self.a1 = vxs 109 | self.a2 = axs / 2.0 110 | 111 | A = np.array([[3 * T ** 2, 4 * T ** 3], 112 | [6 * T, 12 * T ** 2]]) 113 | b = np.array([vxe - self.a1 - 2 * self.a2 * T, 114 | axe - 2 * self.a2]) 115 | x = np.linalg.solve(A, b) 116 | 117 | self.a3 = x[0] 118 | self.a4 = x[1] 119 | 120 | def calc_point(self, t): 121 | xt = self.a0 + self.a1 * t + self.a2 * t**2 + \ 122 | self.a3 * t**3 + self.a4 * t**4 123 | 124 | return xt 125 | 126 | def calc_first_derivative(self, t): 127 | xt = self.a1 + 2 * self.a2 * t + \ 128 | 3 * self.a3 * t**2 + 4 * self.a4 * t**3 129 | 130 | return xt 131 | 132 | def calc_second_derivative(self, t): 133 | xt = 2 * self.a2 + 6 * self.a3 * t + 12 * self.a4 * t**2 134 | 135 | return xt 136 | 137 | def calc_third_derivative(self, t): 138 | xt = 6 * self.a3 + 24 * self.a4 * t 139 | 140 | return xt 141 | 142 | 143 | class Frenet_path: 144 | 145 | def __init__(self): 146 | self.t = [] 147 | self.d = [] 148 | self.d_d = [] 149 | self.d_dd = [] 150 | self.d_ddd = [] 151 | self.s = [] 152 | self.s_d = [] 153 | self.s_dd = [] 154 | self.s_ddd = [] 155 | self.cd = 0.0 156 | self.cv = 0.0 157 | self.cf = 0.0 158 | 159 | self.x = [] 160 | self.y = [] 161 | self.yaw = [] 162 | self.ds = [] 163 | self.c = [] 164 | 165 | 166 | def calc_frenet_paths(c_speed, c_d, c_d_d, c_d_dd, s0): 167 | 168 | frenet_paths = [] 169 | 170 | # generate path to each offset goal 171 | for di in np.arange(-MAX_ROAD_WIDTH, MAX_ROAD_WIDTH, D_ROAD_W): 172 | 173 | # Lateral motion planning 174 | for Ti in np.arange(MINT, MAXT, DT): 175 | fp = Frenet_path() 176 | 177 | lat_qp = quintic_polynomial(c_d, c_d_d, c_d_dd, di, 0.0, 0.0, Ti) 178 | 179 | fp.t = [t for t in np.arange(0.0, Ti, DT)] 180 | fp.d = [lat_qp.calc_point(t) for t in fp.t] 181 | fp.d_d = [lat_qp.calc_first_derivative(t) for t in fp.t] 182 | fp.d_dd = [lat_qp.calc_second_derivative(t) for t in fp.t] 183 | fp.d_ddd = [lat_qp.calc_third_derivative(t) for t in fp.t] 184 | 185 | # Loongitudinal motion planning (Velocity keeping) 186 | for tv in np.arange(TARGET_SPEED - D_T_S * N_S_SAMPLE, TARGET_SPEED + D_T_S * N_S_SAMPLE, D_T_S): 187 | tfp = copy.deepcopy(fp) 188 | lon_qp = quartic_polynomial(s0, c_speed, 0.0, tv, 0.0, Ti) 189 | 190 | tfp.s = [lon_qp.calc_point(t) for t in fp.t] 191 | tfp.s_d = [lon_qp.calc_first_derivative(t) for t in fp.t] 192 | tfp.s_dd = [lon_qp.calc_second_derivative(t) for t in fp.t] 193 | tfp.s_ddd = [lon_qp.calc_third_derivative(t) for t in fp.t] 194 | 195 | Jp = sum(np.power(tfp.d_ddd, 2)) # square of jerk 196 | Js = sum(np.power(tfp.s_ddd, 2)) # square of jerk 197 | 198 | # square of diff from target speed 199 | ds = (TARGET_SPEED - tfp.s_d[-1])**2 200 | 201 | tfp.cd = KJ * Jp + KT * Ti + KD * tfp.d[-1]**2 202 | tfp.cv = KJ * Js + KT * Ti + KD * ds 203 | tfp.cf = KLAT * tfp.cd + KLON * tfp.cv 204 | 205 | frenet_paths.append(tfp) 206 | 207 | return frenet_paths 208 | 209 | 210 | def calc_global_paths(fplist, csp): 211 | 212 | for fp in fplist: 213 | 214 | # calc global positions 215 | for i in range(len(fp.s)): 216 | ix, iy = csp.calc_position(fp.s[i]) 217 | if ix is None: 218 | break 219 | iyaw = csp.calc_yaw(fp.s[i]) 220 | di = fp.d[i] 221 | fx = ix + di * math.cos(iyaw + math.pi / 2.0) 222 | fy = iy + di * math.sin(iyaw + math.pi / 2.0) 223 | fp.x.append(fx) 224 | fp.y.append(fy) 225 | 226 | # calc yaw and ds 227 | for i in range(len(fp.x) - 1): 228 | dx = fp.x[i + 1] - fp.x[i] 229 | dy = fp.y[i + 1] - fp.y[i] 230 | fp.yaw.append(math.atan2(dy, dx)) 231 | fp.ds.append(math.sqrt(dx**2 + dy**2)) 232 | 233 | fp.yaw.append(fp.yaw[-1]) 234 | fp.ds.append(fp.ds[-1]) 235 | 236 | # calc curvature 237 | for i in range(len(fp.yaw) - 1): 238 | fp.c.append((fp.yaw[i + 1] - fp.yaw[i]) / fp.ds[i]) 239 | 240 | return fplist 241 | 242 | 243 | def check_collision(fp, ob): 244 | 245 | for i in range(len(ob[:, 0])): 246 | d = [((ix - ob[i, 0])**2 + (iy - ob[i, 1])**2) 247 | for (ix, iy) in zip(fp.x, fp.y)] 248 | 249 | collision = any([di <= ROBOT_RADIUS**2 for di in d]) 250 | 251 | if collision: 252 | return False 253 | 254 | return True 255 | 256 | 257 | def check_paths(fplist, ob): 258 | 259 | okind = [] 260 | for i in range(len(fplist)): 261 | if any([v > MAX_SPEED for v in fplist[i].s_d]): # Max speed check 262 | continue 263 | elif any([abs(a) > MAX_ACCEL for a in fplist[i].s_dd]): # Max accel check 264 | continue 265 | elif any([abs(c) > MAX_CURVATURE for c in fplist[i].c]): # Max curvature check 266 | continue 267 | elif not check_collision(fplist[i], ob): 268 | continue 269 | 270 | okind.append(i) 271 | 272 | return [fplist[i] for i in okind] 273 | 274 | 275 | def frenet_optimal_planning(csp, s0, c_speed, c_d, c_d_d, c_d_dd, ob): 276 | 277 | fplist = calc_frenet_paths(c_speed, c_d, c_d_d, c_d_dd, s0) 278 | fplist = calc_global_paths(fplist, csp) 279 | fplist = check_paths(fplist, ob) 280 | 281 | # find minimum cost path 282 | mincost = float("inf") 283 | bestpath = None 284 | for fp in fplist: 285 | if mincost >= fp.cf: 286 | mincost = fp.cf 287 | bestpath = fp 288 | 289 | return bestpath 290 | 291 | 292 | def generate_target_course(x, y): 293 | csp = cubic_spline_planner.Spline2D(x, y) 294 | s = np.arange(0, csp.s[-1], 0.1) 295 | 296 | rx, ry, ryaw, rk = [], [], [], [] 297 | for i_s in s: 298 | ix, iy = csp.calc_position(i_s) 299 | rx.append(ix) 300 | ry.append(iy) 301 | ryaw.append(csp.calc_yaw(i_s)) 302 | rk.append(csp.calc_curvature(i_s)) 303 | 304 | return rx, ry, ryaw, rk, csp 305 | 306 | 307 | def main(): 308 | print(__file__ + " start!!") 309 | 310 | # way points 311 | wx = [0.0, 10.0, 20.5, 35.0, 70.5] 312 | wy = [0.0, -6.0, 5.0, 6.5, 0.0] 313 | # obstacle lists 314 | ob = np.array([[20.0, 10.0], 315 | [30.0, 6.0], 316 | [30.0, 8.0], 317 | [35.0, 8.0], 318 | [50.0, 3.0] 319 | ]) 320 | 321 | tx, ty, tyaw, tc, csp = generate_target_course(wx, wy) 322 | 323 | # initial state 324 | c_speed = 10.0 / 3.6 # current speed [m/s] 325 | c_d = 2.0 # current lateral position [m] 326 | c_d_d = 0.0 # current lateral speed [m/s] 327 | c_d_dd = 0.0 # current latral acceleration [m/s] 328 | s0 = 0.0 # current course position 329 | 330 | area = 20.0 # animation area length [m] 331 | 332 | for i in range(500): 333 | path = frenet_optimal_planning( 334 | csp, s0, c_speed, c_d, c_d_d, c_d_dd, ob) 335 | 336 | s0 = path.s[1] 337 | c_d = path.d[1] 338 | c_d_d = path.d_d[1] 339 | c_d_dd = path.d_dd[1] 340 | c_speed = path.s_d[1] 341 | 342 | if np.hypot(path.x[1] - tx[-1], path.y[1] - ty[-1]) <= 1.0: 343 | print("Goal") 344 | break 345 | 346 | if show_animation: 347 | plt.cla() 348 | plt.plot(tx, ty) 349 | plt.plot(ob[:, 0], ob[:, 1], "xk") 350 | plt.plot(path.x[1:], path.y[1:], "-or") 351 | plt.plot(path.x[1], path.y[1], "vc") 352 | plt.xlim(path.x[1] - area, path.x[1] + area) 353 | plt.ylim(path.y[1] - area, path.y[1] + area) 354 | plt.title("v[km/h]:" + str(c_speed * 3.6)[0:4]) 355 | plt.grid(True) 356 | plt.pause(0.0001) 357 | 358 | print("Finish") 359 | if show_animation: 360 | plt.grid(True) 361 | plt.pause(0.0001) 362 | plt.show() 363 | 364 | 365 | if __name__ == '__main__': 366 | main() 367 | -------------------------------------------------------------------------------- /OptimalTrajectoryGenerationforDynamicStreetScenariosinaFrenetFrame.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ChenBohan/Robotics-Path-Planning-04-Quintic-Polynomial-Solver/f84483fb41be1c472a2032165132b313bdbb29ec/OptimalTrajectoryGenerationforDynamicStreetScenariosinaFrenetFrame.pdf -------------------------------------------------------------------------------- /Quintic-Polynomial-Solver/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | #include "Dense" 7 | 8 | using namespace std; 9 | using Eigen::MatrixXd; 10 | using Eigen::VectorXd; 11 | 12 | // TODO - complete this function 13 | vector JMT(vector< double> start, vector end, double T) 14 | { 15 | /* 16 | Calculate the Jerk Minimizing Trajectory that connects the initial state 17 | to the final state in time T. 18 | 19 | INPUTS 20 | 21 | start - the vehicles start location given as a length three array 22 | corresponding to initial values of [s, s_dot, s_double_dot] 23 | 24 | end - the desired end state for vehicle. Like "start" this is a 25 | length three array. 26 | 27 | T - The duration, in seconds, over which this maneuver should occur. 28 | 29 | OUTPUT 30 | an array of length 6, each value corresponding to a coefficent in the polynomial 31 | s(t) = a_0 + a_1 * t + a_2 * t**2 + a_3 * t**3 + a_4 * t**4 + a_5 * t**5 32 | 33 | EXAMPLE 34 | 35 | > JMT( [0, 10, 0], [10, 10, 0], 1) 36 | [0.0, 10.0, 0.0, 0.0, 0.0, 0.0] 37 | */ 38 | 39 | MatrixXd A = MatrixXd(3, 3); 40 | A << T*T*T, T*T*T*T, T*T*T*T*T, 41 | 3*T*T, 4*T*T*T,5*T*T*T*T, 42 | 6*T, 12*T*T, 20*T*T*T; 43 | 44 | MatrixXd B = MatrixXd(3,1); 45 | B << end[0]-(start[0]+start[1]*T+.5*start[2]*T*T), 46 | end[1]-(start[1]+start[2]*T), 47 | end[2]-start[2]; 48 | 49 | MatrixXd Ai = A.inverse(); 50 | 51 | MatrixXd C = Ai*B; 52 | 53 | vector result = {start[0], start[1], .5*start[2]}; 54 | for(int i = 0; i < C.size(); i++) 55 | { 56 | result.push_back(C.data()[i]); 57 | } 58 | 59 | return result; 60 | 61 | } 62 | 63 | bool close_enough(vector< double > poly, vector target_poly, double eps=0.01) { 64 | 65 | 66 | if(poly.size() != target_poly.size()) 67 | { 68 | cout << "your solution didn't have the correct number of terms" << endl; 69 | return false; 70 | } 71 | for(int i = 0; i < poly.size(); i++) 72 | { 73 | double diff = poly[i]-target_poly[i]; 74 | if(abs(diff) > eps) 75 | { 76 | cout << "at least one of your terms differed from target by more than " << eps << endl; 77 | return false; 78 | } 79 | 80 | } 81 | return true; 82 | } 83 | 84 | struct test_case { 85 | 86 | vector start; 87 | vector end; 88 | double T; 89 | }; 90 | 91 | vector< vector > answers = {{0.0, 10.0, 0.0, 0.0, 0.0, 0.0},{0.0,10.0,0.0,0.0,-0.625,0.3125},{5.0,10.0,1.0,-3.0,0.64,-0.0432}}; 92 | 93 | int main() { 94 | 95 | //create test cases 96 | 97 | vector< test_case > tc; 98 | 99 | test_case tc1; 100 | tc1.start = {0,10,0}; 101 | tc1.end = {10,10,0}; 102 | tc1.T = 1; 103 | tc.push_back(tc1); 104 | 105 | test_case tc2; 106 | tc2.start = {0,10,0}; 107 | tc2.end = {20,15,20}; 108 | tc2.T = 2; 109 | tc.push_back(tc2); 110 | 111 | test_case tc3; 112 | tc3.start = {5,10,2}; 113 | tc3.end = {-30,-20,-4}; 114 | tc3.T = 5; 115 | tc.push_back(tc3); 116 | 117 | bool total_correct = true; 118 | for(int i = 0; i < tc.size(); i++) 119 | { 120 | vector< double > jmt = JMT(tc[i].start, tc[i].end, tc[i].T); 121 | bool correct = close_enough(jmt,answers[i]); 122 | total_correct &= correct; 123 | 124 | } 125 | if(!total_correct) 126 | { 127 | cout << "Try again!" << endl; 128 | } 129 | else 130 | { 131 | cout << "Nice work!" << endl; 132 | } 133 | 134 | return 0; 135 | } 136 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Robotics-Path-Planning-04-Quintic-Polynomial-Solver 2 | Udacity Self-Driving Car Engineer Nanodegree: Quintic Polynomial Solver. 3 | & Paper 'Optimal Trajectory Generation for Dynamic Street Scenarios in a Frenet Frame' 4 | 5 | ## Implement the Quintic Polynomial Solver 6 | 7 | Calculate the Jerk Minimizing Trajectory that connects the initial state to the final state in time T. 8 | 9 | 10 | 11 | Our problem is to find a function s of t that minimize the total jerk. 12 | 13 | We find that all the time derivatives of s are further six and more have to be zero in order for s to be jerk minimize. 14 | 15 | All minimum jerk trajectories can be repersented as a fifth order polynomial like that. 16 | 17 | 18 | 19 | INPUTS: 20 | - ``start``: The vehicles start location given as a length three array corresponding to initial values of [s, s_dot, s_double_dot]. 21 | - ``end``: The desired end state for vehicle. Like "start" this is a length three array. 22 | - ``T``: The duration, in seconds, over which this maneuver should occur. 23 | 24 | OUTPUT: 25 | - an array of length 6, each value corresponding to a coefficent in the polynomial s(t) = a_0 + a_1 * t + a_2 * t ** 2 + a_3 * t ** 3 + a_4 * t ** 4 + a_5 * t ** 5 26 | 27 | The equations for position, velocity, and acceleration are given by: 28 | 29 | 30 | 31 | 32 | 33 | PS: We are solving a system of equations: matrices will be helpful! The Eigen library used from Sensor Fusion is included. 34 | 35 | ```cpp 36 | vector JMT(vector< double> start, vector end, double T) 37 | { 38 | MatrixXd A = MatrixXd(3, 3); 39 | A << T*T*T, T*T*T*T, T*T*T*T*T, 40 | 3*T*T, 4*T*T*T,5*T*T*T*T, 41 | 6*T, 12*T*T, 20*T*T*T; 42 | 43 | MatrixXd B = MatrixXd(3,1); 44 | B << end[0]-(start[0]+start[1]*T+.5*start[2]*T*T), 45 | end[1]-(start[1]+start[2]*T), 46 | end[2]-start[2]; 47 | 48 | MatrixXd Ai = A.inverse(); 49 | MatrixXd C = Ai*B; 50 | 51 | vector result = {start[0], start[1], .5*start[2]}; 52 | for(int i = 0; i < C.size(); i++) 53 | { 54 | result.push_back(C.data()[i]); 55 | } 56 | return result; 57 | } 58 | ``` 59 | 60 | 61 | 62 | ## Paper 'Optimal Trajectory Generation for Dynamic Street Scenarios in a Frene´t Frame' 63 | 64 | - The paper discusses some topics like: 65 | - Cost Functions. 66 | - Differences between high speed and low speed trajectory generation. 67 | - Implementation of specific maneuvers relevant to highway driving like following, merging, and velocity keeping. 68 | - How to combining lateral and longitudinal trajectories. 69 | - A derivation of the transformation from Frenet coordinates to global coordinates (in the appendix). 70 | 71 | - Abstract 72 | - semi-reactive trajectory generation method 73 | - be tightly integrated into the behavioral layer 74 | - realize long-term objectives (such as velocity keeping, merging, following, stopping) 75 | - combine with a reactive collision avoidance 76 | - Frenét-Frame 77 | 78 | - Related work 79 | - [11], [19], [2], [4]: fail to model the inherent unpredictability of other traffic, and the resulting uncertainty, given that they **rely on precise prediction** of other traffic participant’s motions over a long time period. 80 | - [16], [1], [7]: The trajectories are represented parametrically. A finite set of trajectories is computed, typically by forward integration of the differential equations that describe vehicle dynamics.While this reduces the solution space and allows for fast planning, it may introduce **suboptimality**. 81 | - [9]: a tree of trajectories is sampled by simulating the closed loop system using the **rapidly exploring random tree algorithm** [10]. 82 | - [17]: in a similar spirit to our method but only considers the free problem that is **not constrained by obstacle**. 83 | - We propose a **local method**, which is capable of realizing **high-level decisions** made by an upstream, **behavioral layer** (long-term objectives) and also **performs (reactive) emergency obstacle avoidance** in unexpected critical situations. 84 | 85 | - Optimal control approach 86 | - system inputs or curvature to be **polynomials**. 87 | - cost functional is compliance with **Bellman’s principle** of optimality. 88 | - making the best compromise between the **jerk** and the **time**. 89 | - not limited to a certain function class, the problem becomes highly **complicated** and can be solved numerically at best. 90 | 91 | 92 | 93 | - Motion planning in the Frenet Frame 94 | 95 | 96 | 97 | 98 | 99 | Total jerk: 100 | 101 | 102 | Cost function: 103 | 104 | 105 | A quintic polynomial through the same points and the same time interval will always lead to a smaller cost. 106 | 107 | - Generation of lateral movement 108 | - High speed trajectories 109 | - at high speed, d(t) and s(t) can be chosen independently. 110 | - cost function: g(T)=T, h(d1)=d1^2. 111 | - process: 112 | 1. calculate its coefficients and T minimizing. 113 | 2. check it against collision. 114 | 3. if not, check and find the second best and collision-free trajectory. 115 | - Low speed trajectories 116 | - at low speed, we must consider the non-holonomic property (invalid curvatures) of the car. 117 | - cost function: see in the paper. 118 | - Generation of longitudianal movement 119 | - Following 120 | - safe distance (constant time gap law) 121 | - Merging 122 | - Stopping 123 | - Velocity keeping 124 | - Combining lateral and longitudinal trajectories 125 | - check aginst outsized acceleration values first. 126 | - derive higher order infomation (heading, curvature, velocity, acceleration) 127 | - calculate the conjoint cost: Cost_total = w_lat * Cost_lat + w_lon * Cost_lon 128 | - for collison dectection: we add a certain safety distance to the size of our car on each side. 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | -------------------------------------------------------------------------------- /TrajectoryExercise2/constants.py: -------------------------------------------------------------------------------- 1 | N_SAMPLES = 10 2 | SIGMA_S = [10.0, 4.0, 2.0] # s, s_dot, s_double_dot 3 | SIGMA_D = [1.0, 1.0, 1.0] 4 | SIGMA_T = 2.0 5 | 6 | MAX_JERK = 10 # m/s/s/s 7 | MAX_ACCEL= 10 # m/s/s 8 | 9 | EXPECTED_JERK_IN_ONE_SEC = 2 # m/s/s 10 | EXPECTED_ACC_IN_ONE_SEC = 1 # m/s 11 | 12 | SPEED_LIMIT = 30 13 | VEHICLE_RADIUS = 1.5 # model vehicle as circle to simplify collision detection -------------------------------------------------------------------------------- /TrajectoryExercise2/cost_functions.py: -------------------------------------------------------------------------------- 1 | from helpers import logistic, to_equation, differentiate, nearest_approach_to_any_vehicle, get_f_and_N_derivatives 2 | from constants import * 3 | import numpy as np 4 | # COST FUNCTIONS 5 | def time_diff_cost(traj, target_vehicle, delta, T, predictions): 6 | """ 7 | Penalizes trajectories that span a duration which is longer or 8 | shorter than the duration requested. 9 | """ 10 | _, _, t = traj 11 | return logistic(float(abs(t-T)) / T) 12 | 13 | def s_diff_cost(traj, target_vehicle, delta, T, predictions): 14 | """ 15 | Penalizes trajectories whose s coordinate (and derivatives) 16 | differ from the goal. 17 | """ 18 | s, _, T = traj 19 | target = predictions[target_vehicle].state_in(T) 20 | target = list(np.array(target) + np.array(delta)) 21 | s_targ = target[:3] 22 | S = [f(T) for f in get_f_and_N_derivatives(s, 2)] 23 | cost = 0 24 | for actual, expected, sigma in zip(S, s_targ, SIGMA_S): 25 | diff = float(abs(actual-expected)) 26 | cost += logistic(diff/sigma) 27 | return cost 28 | 29 | def d_diff_cost(traj, target_vehicle, delta, T, predictions): 30 | """ 31 | Penalizes trajectories whose d coordinate (and derivatives) 32 | differ from the goal. 33 | """ 34 | _, d_coeffs, T = traj 35 | 36 | d_dot_coeffs = differentiate(d_coeffs) 37 | d_ddot_coeffs = differentiate(d_dot_coeffs) 38 | 39 | d = to_equation(d_coeffs) 40 | d_dot = to_equation(d_dot_coeffs) 41 | d_ddot = to_equation(d_ddot_coeffs) 42 | 43 | D = [d(T), d_dot(T), d_ddot(T)] 44 | 45 | target = predictions[target_vehicle].state_in(T) 46 | target = list(np.array(target) + np.array(delta)) 47 | d_targ = target[3:] 48 | cost = 0 49 | for actual, expected, sigma in zip(D, d_targ, SIGMA_D): 50 | diff = float(abs(actual-expected)) 51 | cost += logistic(diff/sigma) 52 | return cost 53 | 54 | def collision_cost(traj, target_vehicle, delta, T, predictions): 55 | """ 56 | Binary cost function which penalizes collisions. 57 | """ 58 | nearest = nearest_approach_to_any_vehicle(traj, predictions) 59 | if nearest < 2*VEHICLE_RADIUS: return 1.0 60 | else : return 0.0 61 | 62 | def buffer_cost(traj, target_vehicle, delta, T, predictions): 63 | """ 64 | Penalizes getting close to other vehicles. 65 | """ 66 | nearest = nearest_approach_to_any_vehicle(traj, predictions) 67 | return logistic(2*VEHICLE_RADIUS / nearest) 68 | 69 | def stays_on_road_cost(traj, target_vehicle, delta, T, predictions): 70 | pass 71 | 72 | def exceeds_speed_limit_cost(traj, target_vehicle, delta, T, predictions): 73 | pass 74 | 75 | def efficiency_cost(traj, target_vehicle, delta, T, predictions): 76 | """ 77 | Rewards high average speeds. 78 | """ 79 | s, _, t = traj 80 | s = to_equation(s) 81 | avg_v = float(s(t)) / t 82 | targ_s, _, _, _, _, _ = predictions[target_vehicle].state_in(t) 83 | targ_v = float(targ_s) / t 84 | return logistic(2*float(targ_v - avg_v) / avg_v) 85 | 86 | def total_accel_cost(traj, target_vehicle, delta, T, predictions): 87 | s, d, t = traj 88 | s_dot = differentiate(s) 89 | s_d_dot = differentiate(s_dot) 90 | a = to_equation(s_d_dot) 91 | total_acc = 0 92 | dt = float(T) / 100.0 93 | for i in range(100): 94 | t = dt * i 95 | acc = a(t) 96 | total_acc += abs(acc*dt) 97 | acc_per_second = total_acc / T 98 | 99 | return logistic(acc_per_second / EXPECTED_ACC_IN_ONE_SEC ) 100 | 101 | def max_accel_cost(traj, target_vehicle, delta, T, predictions): 102 | s, d, t = traj 103 | s_dot = differentiate(s) 104 | s_d_dot = differentiate(s_dot) 105 | a = to_equation(s_d_dot) 106 | all_accs = [a(float(T)/100 * i) for i in range(100)] 107 | max_acc = max(all_accs, key=abs) 108 | if abs(max_acc) > MAX_ACCEL: return 1 109 | else: return 0 110 | 111 | 112 | def max_jerk_cost(traj, target_vehicle, delta, T, predictions): 113 | s, d, t = traj 114 | s_dot = differentiate(s) 115 | s_d_dot = differentiate(s_dot) 116 | jerk = differentiate(s_d_dot) 117 | jerk = to_equation(jerk) 118 | all_jerks = [jerk(float(T)/100 * i) for i in range(100)] 119 | max_jerk = max(all_jerks, key=abs) 120 | if abs(max_jerk) > MAX_JERK: return 1 121 | else: return 0 122 | 123 | def total_jerk_cost(traj, target_vehicle, delta, T, predictions): 124 | s, d, t = traj 125 | s_dot = differentiate(s) 126 | s_d_dot = differentiate(s_dot) 127 | jerk = to_equation(differentiate(s_d_dot)) 128 | total_jerk = 0 129 | dt = float(T) / 100.0 130 | for i in range(100): 131 | t = dt * i 132 | j = jerk(t) 133 | total_jerk += abs(j*dt) 134 | jerk_per_second = total_jerk / T 135 | return logistic(jerk_per_second / EXPECTED_JERK_IN_ONE_SEC ) -------------------------------------------------------------------------------- /TrajectoryExercise2/evaluate_ptg.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from ptg import PTG 4 | from helpers import Vehicle, show_trajectory 5 | 6 | def main(): 7 | vehicle = Vehicle([0,10,0, 0,0,0]) 8 | predictions = {0: vehicle} 9 | target = 0 10 | delta = [0, 0, 0, 0, 0 ,0] 11 | start_s = [10, 10, 0] 12 | start_d = [4, 0, 0] 13 | T = 5.0 14 | best = PTG(start_s, start_d, target, delta, T, predictions) 15 | show_trajectory(best[0], best[1], best[2], vehicle) 16 | 17 | if __name__ == "__main__": 18 | main() -------------------------------------------------------------------------------- /TrajectoryExercise2/helpers.py: -------------------------------------------------------------------------------- 1 | from math import sqrt, exp 2 | from matplotlib import pyplot as plt 3 | 4 | class Vehicle(object): 5 | """ 6 | Helper class. Non-ego vehicles move w/ constant acceleration 7 | """ 8 | def __init__(self, start): 9 | self.start_state = start 10 | 11 | def state_in(self, t): 12 | s = self.start_state[:3] 13 | d = self.start_state[3:] 14 | state = [ 15 | s[0] + (s[1] * t) + s[2] * t**2 / 2.0, 16 | s[1] + s[2] * t, 17 | s[2], 18 | d[0] + (d[1] * t) + d[2] * t**2 / 2.0, 19 | d[1] + d[2] * t, 20 | d[2], 21 | ] 22 | return state 23 | 24 | def logistic(x): 25 | """ 26 | A function that returns a value between 0 and 1 for x in the 27 | range [0, infinity] and -1 to 1 for x in the range [-infinity, infinity]. 28 | 29 | Useful for cost functions. 30 | """ 31 | return 2.0 / (1 + exp(-x)) - 1.0 32 | 33 | def to_equation(coefficients): 34 | """ 35 | Takes the coefficients of a polynomial and creates a function of 36 | time from them. 37 | """ 38 | def f(t): 39 | total = 0.0 40 | for i, c in enumerate(coefficients): 41 | total += c * t ** i 42 | return total 43 | return f 44 | 45 | def differentiate(coefficients): 46 | """ 47 | Calculates the derivative of a polynomial and returns 48 | the corresponding coefficients. 49 | """ 50 | new_cos = [] 51 | for deg, prev_co in enumerate(coefficients[1:]): 52 | new_cos.append((deg+1) * prev_co) 53 | return new_cos 54 | 55 | def nearest_approach_to_any_vehicle(traj, vehicles): 56 | """ 57 | Calculates the closest distance to any vehicle during a trajectory. 58 | """ 59 | closest = 999999 60 | for v in vehicles.values(): 61 | d = nearest_approach(traj,v) 62 | if d < closest: 63 | closest = d 64 | return closest 65 | 66 | def nearest_approach(traj, vehicle): 67 | closest = 999999 68 | s_,d_,T = traj 69 | s = to_equation(s_) 70 | d = to_equation(d_) 71 | for i in range(100): 72 | t = float(i) / 100 * T 73 | cur_s = s(t) 74 | cur_d = d(t) 75 | targ_s, _, _, targ_d, _, _ = vehicle.state_in(t) 76 | dist = sqrt((cur_s-targ_s)**2 + (cur_d-targ_d)**2) 77 | if dist < closest: 78 | closest = dist 79 | return closest 80 | 81 | def show_trajectory(s_coeffs, d_coeffs, T, vehicle=None): 82 | s = to_equation(s_coeffs) 83 | d = to_equation(d_coeffs) 84 | X = [] 85 | Y = [] 86 | if vehicle: 87 | X2 = [] 88 | Y2 = [] 89 | t = 0 90 | while t <= T+0.01: 91 | X.append(s(t)) 92 | Y.append(d(t)) 93 | if vehicle: 94 | s_, _, _, d_, _, _ = vehicle.state_in(t) 95 | X2.append(s_) 96 | Y2.append(d_) 97 | t += 0.25 98 | plt.scatter(X,Y,color="blue") 99 | if vehicle: 100 | plt.scatter(X2, Y2,color="red") 101 | plt.show() 102 | 103 | def get_f_and_N_derivatives(coeffs, N=3): 104 | functions = [to_equation(coeffs)] 105 | for i in range(N): 106 | coeffs = differentiate(coeffs) 107 | functions.append(to_equation(coeffs)) 108 | return functions 109 | 110 | -------------------------------------------------------------------------------- /TrajectoryExercise2/ptg.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import random 3 | from cost_functions import * 4 | from constants import * 5 | 6 | # TODO - tweak weights to existing cost functions 7 | WEIGHTED_COST_FUNCTIONS = [ 8 | (time_diff_cost, 1), 9 | (s_diff_cost, 1), 10 | (d_diff_cost, 1), 11 | (efficiency_cost, 1), 12 | (max_jerk_cost, 1), 13 | (total_jerk_cost, 1), 14 | (collision_cost, 1), 15 | (buffer_cost, 1), 16 | (max_accel_cost, 1), 17 | (total_accel_cost, 1), 18 | ] 19 | 20 | def PTG(start_s, start_d, target_vehicle, delta, T, predictions): 21 | """ 22 | Finds the best trajectory according to WEIGHTED_COST_FUNCTIONS (global). 23 | 24 | arguments: 25 | start_s - [s, s_dot, s_ddot] 26 | 27 | start_d - [d, d_dot, d_ddot] 28 | 29 | target_vehicle - id of leading vehicle (int) which can be used to retrieve 30 | that vehicle from the "predictions" dictionary. This is the vehicle that 31 | we are setting our trajectory relative to. 32 | 33 | delta - a length 6 array indicating the offset we are aiming for between us 34 | and the target_vehicle. So if at time 5 the target vehicle will be at 35 | [100, 10, 0, 0, 0, 0] and delta is [-10, 0, 0, 4, 0, 0], then our goal 36 | state for t = 5 will be [90, 10, 0, 4, 0, 0]. This would correspond to a 37 | goal of "follow 10 meters behind and 4 meters to the right of target vehicle" 38 | 39 | T - the desired time at which we will be at the goal (relative to now as t=0) 40 | 41 | predictions - dictionary of {v_id : vehicle }. Each vehicle has a method 42 | vehicle.state_in(time) which returns a length 6 array giving that vehicle's 43 | expected [s, s_dot, s_ddot, d, d_dot, d_ddot] state at that time. 44 | 45 | return: 46 | (best_s, best_d, best_t) where best_s are the 6 coefficients representing s(t) 47 | best_d gives coefficients for d(t) and best_t gives duration associated w/ 48 | this trajectory. 49 | """ 50 | target = predictions[target_vehicle] 51 | # generate alternative goals 52 | all_goals = [] 53 | timestep = 0.5 54 | t = T - 4 * timestep 55 | while t <= T + 4 * timestep: 56 | target_state = np.array(target.state_in(t)) + np.array(delta) 57 | goal_s = target_state[:3] 58 | goal_d = target_state[3:] 59 | goals = [(goal_s, goal_d, t)] 60 | for _ in range(N_SAMPLES): 61 | perturbed = perturb_goal(goal_s, goal_d) 62 | goals.append((perturbed[0], perturbed[1], t)) 63 | all_goals += goals 64 | t += timestep 65 | 66 | # find best trajectory 67 | trajectories = [] 68 | for goal in all_goals: 69 | s_goal, d_goal, t = goal 70 | s_coefficients = JMT(start_s, s_goal, t) 71 | d_coefficients = JMT(start_d, d_goal, t) 72 | trajectories.append(tuple([s_coefficients, d_coefficients, t])) 73 | 74 | best = min(trajectories, key=lambda tr: calculate_cost(tr, target_vehicle, delta, T, predictions, WEIGHTED_COST_FUNCTIONS)) 75 | calculate_cost(best, target_vehicle, delta, T, predictions, WEIGHTED_COST_FUNCTIONS, verbose=True) 76 | return best 77 | 78 | 79 | def calculate_cost(trajectory, target_vehicle, delta, goal_t, predictions, cost_functions_with_weights, verbose=False): 80 | cost = 0 81 | for cf, weight in cost_functions_with_weights: 82 | new_cost = weight * cf(trajectory, target_vehicle, delta, goal_t, predictions) 83 | cost += new_cost 84 | if verbose: 85 | print "cost for {} is \t {}".format(cf.func_name, new_cost) 86 | return cost 87 | 88 | def perturb_goal(goal_s, goal_d): 89 | """ 90 | Returns a "perturbed" version of the goal. 91 | """ 92 | new_s_goal = [] 93 | for mu, sig in zip(goal_s, SIGMA_S): 94 | new_s_goal.append(random.gauss(mu, sig)) 95 | 96 | new_d_goal = [] 97 | for mu, sig in zip(goal_d, SIGMA_D): 98 | new_d_goal.append(random.gauss(mu, sig)) 99 | 100 | return tuple([new_s_goal, new_d_goal]) 101 | 102 | def JMT(start, end, T): 103 | """ 104 | Calculates Jerk Minimizing Trajectory for start, end and T. 105 | """ 106 | a_0, a_1, a_2 = start[0], start[1], start[2] / 2.0 107 | c_0 = a_0 + a_1 * T + a_2 * T**2 108 | c_1 = a_1 + 2* a_2 * T 109 | c_2 = 2 * a_2 110 | 111 | A = np.array([ 112 | [ T**3, T**4, T**5], 113 | [3*T**2, 4*T**3, 5*T**4], 114 | [6*T, 12*T**2, 20*T**3], 115 | ]) 116 | B = np.array([ 117 | end[0] - c_0, 118 | end[1] - c_1, 119 | end[2] - c_2 120 | ]) 121 | a_3_4_5 = np.linalg.solve(A,B) 122 | alphas = np.concatenate([np.array([a_0, a_1, a_2]), a_3_4_5]) 123 | return alphas -------------------------------------------------------------------------------- /readme_img/Jerk Minimizing Trajectories.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ChenBohan/Robotics-Path-Planning-04-Quintic-Polynomial-Solver/f84483fb41be1c472a2032165132b313bdbb29ec/readme_img/Jerk Minimizing Trajectories.png -------------------------------------------------------------------------------- /readme_img/Polynomial solver.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ChenBohan/Robotics-Path-Planning-04-Quintic-Polynomial-Solver/f84483fb41be1c472a2032165132b313bdbb29ec/readme_img/Polynomial solver.png -------------------------------------------------------------------------------- /readme_img/animation.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ChenBohan/Robotics-Path-Planning-04-Quintic-Polynomial-Solver/f84483fb41be1c472a2032165132b313bdbb29ec/readme_img/animation.gif -------------------------------------------------------------------------------- /readme_img/cost1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ChenBohan/Robotics-Path-Planning-04-Quintic-Polynomial-Solver/f84483fb41be1c472a2032165132b313bdbb29ec/readme_img/cost1.png -------------------------------------------------------------------------------- /readme_img/feasibility.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ChenBohan/Robotics-Path-Planning-04-Quintic-Polynomial-Solver/f84483fb41be1c472a2032165132b313bdbb29ec/readme_img/feasibility.png -------------------------------------------------------------------------------- /readme_img/fenet.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ChenBohan/Robotics-Path-Planning-04-Quintic-Polynomial-Solver/f84483fb41be1c472a2032165132b313bdbb29ec/readme_img/fenet.png -------------------------------------------------------------------------------- /readme_img/formula1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ChenBohan/Robotics-Path-Planning-04-Quintic-Polynomial-Solver/f84483fb41be1c472a2032165132b313bdbb29ec/readme_img/formula1.png -------------------------------------------------------------------------------- /readme_img/formula2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ChenBohan/Robotics-Path-Planning-04-Quintic-Polynomial-Solver/f84483fb41be1c472a2032165132b313bdbb29ec/readme_img/formula2.png -------------------------------------------------------------------------------- /readme_img/formula3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ChenBohan/Robotics-Path-Planning-04-Quintic-Polynomial-Solver/f84483fb41be1c472a2032165132b313bdbb29ec/readme_img/formula3.png -------------------------------------------------------------------------------- /readme_img/jerk1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ChenBohan/Robotics-Path-Planning-04-Quintic-Polynomial-Solver/f84483fb41be1c472a2032165132b313bdbb29ec/readme_img/jerk1.png -------------------------------------------------------------------------------- /readme_img/overshoot.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ChenBohan/Robotics-Path-Planning-04-Quintic-Polynomial-Solver/f84483fb41be1c472a2032165132b313bdbb29ec/readme_img/overshoot.png -------------------------------------------------------------------------------- /readme_img/paper_pic1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ChenBohan/Robotics-Path-Planning-04-Quintic-Polynomial-Solver/f84483fb41be1c472a2032165132b313bdbb29ec/readme_img/paper_pic1.png -------------------------------------------------------------------------------- /readme_img/paper_pic2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ChenBohan/Robotics-Path-Planning-04-Quintic-Polynomial-Solver/f84483fb41be1c472a2032165132b313bdbb29ec/readme_img/paper_pic2.png -------------------------------------------------------------------------------- /readme_img/paper_pic3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ChenBohan/Robotics-Path-Planning-04-Quintic-Polynomial-Solver/f84483fb41be1c472a2032165132b313bdbb29ec/readme_img/paper_pic3.png -------------------------------------------------------------------------------- /readme_img/paper_pic_result.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ChenBohan/Robotics-Path-Planning-04-Quintic-Polynomial-Solver/f84483fb41be1c472a2032165132b313bdbb29ec/readme_img/paper_pic_result.png --------------------------------------------------------------------------------