def simulation(path=QuinticPolinomial):
"""
Creates an animated plot of a car following the quintic polinomial
"""
for i in range(len(path.t)):
plt.gcf().canvas.mpl_connect(
"key_release_event",
lambda event: [exit(0) if event.key == "escape" else None],
)
draw.Car(path.x[i], path.y[i], np.degrees(path.yaw[i]), 1.5, 3)
plt.pause(0.001)
plt.show()
def main():
# choose states pairs: (s, y, yaw)
# simulation-1
# states = [(0, 0, 0), (10, 10, -90), (20, 5, 60), (30, 10, 120),
# (35, -5, 30), (25, -10, -120), (15, -15, 100), (0, -10, -90)]
# simulation-2
states = [(-3, 3, 120), (10, -7, 30), (10, 13, 30), (20, 5, -25),
(35, 10, 180), (32, -10, 180), (5, -12, 90)]
max_c = 0.1 # max curvature
path_x, path_y, yaw = [], [], []
for i in range(len(states) - 1):
s_x = states[i][0]
s_y = states[i][1]
s_yaw = np.deg2rad(states[i][2])
g_x = states[i + 1][0]
g_y = states[i + 1][1]
g_yaw = np.deg2rad(states[i + 1][2])
path_i = calc_optimal_path(s_x, s_y, s_yaw,
g_x, g_y, g_yaw, max_c)
path_x += path_i.x
path_y += path_i.y
yaw += path_i.yaw
# animation
plt.ion()
plt.figure(1)
for i in range(len(path_x)):
plt.clf()
plt.plot(path_x, path_y, linewidth=1, color='gray')
for x, y, theta in states:
draw.Arrow(x, y, np.deg2rad(theta), 2, 'blueviolet')
draw.Car(path_x[i], path_y[i], yaw[i], 1.5, 3)
plt.axis("equal")
plt.title("Simulation of Reeds-Shepp Curves")
plt.axis([-10, 42, -20, 20])
plt.draw()
plt.pause(0.001)
plt.pause(1)
def simulation_quintic():
sx, sy, syaw, sv, sa = 10.0, 10.0, np.deg2rad(0.0), 4.0, 1.0
gx, gy, gyaw, gv, ga = 30.0, -10.0, np.deg2rad(180.0), 4.0, 0
MAX_ACCEL = 2.0 # max accel [m/s2]
MAX_CURV = 1 / 2.0 # max curvature [1/m]
dt = 0.1 # T tick [s]
MIN_T = 5
MAX_T = 100
T_STEP = 5
sv_x = sv * math.cos(syaw)
sv_y = sv * math.sin(syaw)
gv_x = gv * math.cos(gyaw)
gv_y = gv * math.sin(gyaw)
sa_x = sa * math.cos(syaw)
sa_y = sa * math.sin(syaw)
ga_x = ga * math.cos(gyaw)
ga_y = ga * math.sin(gyaw)
path = Trajectory()
for T in np.arange(MIN_T, 100, T_STEP):
path = Trajectory()
cp = QuinticPolynomial2D(sx, sv_x, sa_x, gx, gv_x, ga_x, sy, sv_y,
sa_y, gy, gv_y, ga_y, T)
for t in np.arange(0.0, T + dt, dt):
path.t.append(t)
x, y = cp.calc_position(t)
path.x.append(x)
path.y.append(y)
v = cp.calc_speed(t)
yaw = cp.calc_yaw(t)
path.v.append(v)
path.yaw.append(yaw)
ax, ay = cp.calc_acc(t)
a = np.hypot(ax, ay)
path.a.append(a)
if len(path.v) >= 2 and path.v[-1] - path.v[-2] < 0.0:
a *= -1
path.a.append(a)
k = cp.calc_curvature(t)
path.k.append(k)
if max(np.abs(path.a)) <= MAX_ACCEL and max(np.abs(
path.k)) <= MAX_CURV:
break
print("t_len: ", path.t, "s")
print("max_v: ", max(path.v), "m/s")
print("max_a: ", max(np.abs(path.a)), "m/s2")
print(f"max_curvature: {max(np.abs(path.k))} 1/m")
for i in range(len(path.t)):
plt.cla()
plt.gcf().canvas.mpl_connect(
'key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
plt.axis("equal")
plt.plot(path.x, path.y, linewidth=2, color='gray')
draw.Car(sx, sy, syaw, 1.5, 3)
draw.Car(gx, gy, gyaw, 1.5, 3)
draw.Car(path.x[i], path.y[i], path.yaw[i], 1.5, 3)
plt.title(
f"Quintic Polynomial Curves: speed {int(path.v[i]*10)/10} m/s")
plt.grid(True)
plt.pause(0.001)
plt.show()
draw.Hairpin(ax)
# draw waypoints
for wp in wps:
draw.Arrow(wp.x, wp.y, wp.theta, 2, width=4, color="g")
# fit and animate quintic polinomial
# wps = [
# Waypoint(20, 40, 270, 1, .2, 5),
# Waypoint(20, 30, 270, 3, 0, 5),
# Waypoint(15, 5, 180, 3, 0, 5),
# ]
# wps = [
# Waypoint(x=20.682933975843298, y=36.4433300139396, theta=241.34371213247948, speed=3.4675332536458368, accel=3.4675332536458368, segment_duration=1),
# Waypoint(x=20.09675309568283, y=21.35347306956288, theta=274.07352782608245, speed=62.565652699469716, accel=0.7206048508470744, segment_duration=1)
# ]
qp = QuinticPolinomial(wps).fit()
# draw path and iitial/final positoin
draw.Tracking(qp.x, qp.y, alpha=1, lw=1, color="k")
draw.Car(
qp.waypoints[0].x, qp.waypoints[0].y, qp.waypoints[0].theta, 1.5, 3
)
draw.Car(
qp.waypoints[1].x, qp.waypoints[1].y, qp.waypoints[1].theta, 1.5, 3
)
simulation(qp)
plt.show()
def simulation():
sx, sy, syaw, sv, sa = 10.0, 10.0, np.deg2rad(10.0), 1.0, 0.1
gx, gy, gyaw, gv, ga = 30.0, -10.0, np.deg2rad(180.0), 1.0, 0.1
MAX_ACCEL = 1.0 # max accel [m/s2]
MAX_JERK = 0.5 # max jerk [m/s3]
dt = 0.1 # T tick [s]
MIN_T = 5
MAX_T = 100
T_STEP = 5
sv_x = sv * math.cos(syaw)
sv_y = sv * math.sin(syaw)
gv_x = gv * math.cos(gyaw)
gv_y = gv * math.sin(gyaw)
sa_x = sa * math.cos(syaw)
sa_y = sa * math.sin(syaw)
ga_x = ga * math.cos(gyaw)
ga_y = ga * math.sin(gyaw)
path = Trajectory()
for T in np.arange(MIN_T, MAX_T, T_STEP):
path = Trajectory()
xqp = QuinticPolynomial(sx, sv_x, sa_x, gx, gv_x, ga_x, T)
yqp = QuinticPolynomial(sy, sv_y, sa_y, gy, gv_y, ga_y, T)
for t in np.arange(0.0, T + dt, dt):
path.t.append(t)
path.x.append(xqp.calc_xt(t))
path.y.append(yqp.calc_xt(t))
vx = xqp.calc_dxt(t)
vy = yqp.calc_dxt(t)
path.v.append(np.hypot(vx, vy))
path.yaw.append(math.atan2(vy, vx))
ax = xqp.calc_ddxt(t)
ay = yqp.calc_ddxt(t)
a = np.hypot(ax, ay)
if len(path.v) >= 2 and path.v[-1] - path.v[-2] < 0.0:
a *= -1
path.a.append(a)
jx = xqp.calc_dddxt(t)
jy = yqp.calc_dddxt(t)
j = np.hypot(jx, jy)
if len(path.a) >= 2 and path.a[-1] - path.a[-2] < 0.0:
j *= -1
path.jerk.append(j)
if max(np.abs(path.a)) <= MAX_ACCEL and max(np.abs(
path.jerk)) <= MAX_JERK:
break
print("t_len: ", path.t, "s")
print("max_v: ", max(path.v), "m/s")
print("max_a: ", max(np.abs(path.a)), "m/s2")
print("max_jerk: ", max(np.abs(path.jerk)), "m/s3")
for i in range(len(path.t)):
plt.cla()
plt.gcf().canvas.mpl_connect(
'key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
plt.axis("equal")
plt.plot(path.x, path.y, linewidth=2, color='gray')
draw.Car(sx, sy, syaw, 1.5, 3)
draw.Car(gx, gy, gyaw, 1.5, 3)
draw.Car(path.x[i], path.y[i], path.yaw[i], 1.5, 3)
plt.title("Quintic Polynomial Curves")
plt.grid(True)
plt.pause(0.001)
plt.show()
