def main():
# generate path
ax = np.arange(0, 50, 0.5)
ay = [math.sin(ix / 5.0) * ix / 2.0 for ix in ax]
cx, cy, cyaw, ck, _ = cs.calc_spline_course(ax, ay, ds=C.dt)
t = 0.0
maxTime = 100.0
yaw_old = 0.0
x0, y0, yaw0 = cx[0], cy[0], cyaw[0]
xrec, yrec, yawrec = [], [], []
node = Node(x=x0, y=y0, yaw=yaw0, v=0.0)
ref_trajectory = Trajectory(cx, cy, cyaw, ck)
e, e_th = 0.0, 0.0
while t < maxTime:
speed_ref = 25.0 / 3.6
sp = calc_speed_profile(cyaw, speed_ref)
dl, target_ind, e, e_th, ai = ref_trajectory.lqr_control(
node, e, e_th, sp, C.lqr_Q, C.lqr_R)
dist = math.hypot(node.x - cx[-1], node.y - cy[-1])
node.update(ai, dl, 1.0)
t += C.dt
if dist <= C.dref:
break
dy = (node.yaw - yaw_old) / (node.v * C.dt)
steer = rs.pi_2_pi(-math.atan(C.WB * dy))
yaw_old = node.yaw
xrec.append(node.x)
yrec.append(node.y)
yawrec.append(node.yaw)
plt.cla()
plt.plot(cx, cy, color='gray', linewidth=2.0)
plt.plot(xrec, yrec, linewidth=2.0, color='darkviolet')
plt.plot(cx[target_ind], cy[target_ind], '.r')
draw.draw_car(node.x, node.y, node.yaw, steer, C)
plt.axis("equal")
plt.title("FrontWheelFeedback: v=" + str(node.v * 3.6)[:4] + "km/h")
plt.gcf().canvas.mpl_connect(
'key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
plt.pause(0.001)
plt.show()
def main():
ax = [0.0, 15.0, 30.0, 50.0, 60.0]
ay = [0.0, 40.0, 15.0, 30.0, 0.0]
cx, cy, cyaw, ck, s = cs.calc_spline_course(ax, ay, ds=P.d_dist)
sp = calc_speed_profile(cx, cy, cyaw, P.target_speed)
ref_path = PATH(cx, cy, cyaw, ck)
node = Node(x=cx[0], y=cy[0], yaw=cyaw[0], v=0.0)
time = 0.0
x = [node.x]
y = [node.y]
yaw = [node.yaw]
v = [node.v]
t = [0.0]
d = [0.0]
a = [0.0]
delta_opt, a_opt = None, None
a_exc, delta_exc = 0.0, 0.0
while time < P.time_max:
z_ref, target_ind = \
calc_ref_trajectory_in_T_step(node, ref_path, sp)
z0 = [node.x, node.y, node.v, node.yaw]
a_opt, delta_opt, x_opt, y_opt, yaw_opt, v_opt = \
linear_mpc_control(z_ref, z0, a_opt, delta_opt)
if delta_opt is not None:
delta_exc, a_exc = delta_opt[0], a_opt[0]
node.update(a_exc, delta_exc, 1.0)
time += P.dt
x.append(node.x)
y.append(node.y)
yaw.append(node.yaw)
v.append(node.v)
t.append(time)
d.append(delta_exc)
a.append(a_exc)
dist = math.hypot(node.x - cx[-1], node.y - cy[-1])
if dist < P.dist_stop and \
abs(node.v) < P.speed_stop:
break
dy = (node.yaw - yaw[-2]) / (node.v * P.dt)
steer = rs.pi_2_pi(-math.atan(P.WB * dy))
plt.cla()
draw.draw_car(node.x, node.y, node.yaw, steer, P)
plt.gcf().canvas.mpl_connect(
'key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
if x_opt is not None:
plt.plot(x_opt, y_opt, color='darkviolet', marker='*')
plt.plot(cx, cy, color='gray')
plt.plot(x, y, '-b')
plt.plot(cx[target_ind], cy[target_ind])
plt.axis("equal")
plt.title("Linear MPC, " + "v = " + str(round(node.v * 3.6, 2)))
plt.pause(0.001)
plt.show()
def main():
# generate path
states = [(0, 0, 0), (20, 15, 0), (35, 20, 90), (40, 0, 180), (20, 0, 120),
(5, -10, 180), (15, 5, 30)]
#
# states = [(-3, 3, 120), (10, -7, 30), (10, 13, 30), (20, 5, -25),
# (35, 10, 180), (30, -10, 160), (5, -12, 90)]
x_ref, y_ref, yaw_ref, direct, curv, x_all, y_all = generate_path(states)
maxTime = 100.0
yaw_old = 0.0
x0, y0, yaw0, direct0 = \
x_ref[0][0], y_ref[0][0], yaw_ref[0][0], direct[0][0]
x_rec, y_rec, yaw_rec, direct_rec = [], [], [], []
for cx, cy, cyaw, cdirect, ccurv in zip(x_ref, y_ref, yaw_ref, direct,
curv):
t = 0.0
er, theta_e = 0.0, 0.0
node = Node(x=x0, y=y0, yaw=yaw0, v=0.0, direct=cdirect[0])
ref_path = PATH(cx, cy, cyaw, ccurv)
while t < maxTime:
if cdirect[0] > 0:
speed_ref = 30.0 / 3.6
else:
speed_ref = 20.0 / 3.6
delta, er, theta_e, ind = lqr_lateral_control(
node, er, theta_e, ref_path)
dist = math.hypot(node.x - cx[-1], node.y - cy[-1])
acceleration = pid_longitudinal_control(speed_ref, node.v, dist,
node.direct)
node.update(acceleration, delta, node.direct)
t += C.dt
if dist <= C.dist_stop:
break
x_rec.append(node.x)
y_rec.append(node.y)
yaw_rec.append(node.yaw)
direct_rec.append(node.direct)
dy = (node.yaw - yaw_old) / (node.v * C.dt)
steer = rs.pi_2_pi(-math.atan(C.WB * dy))
yaw_old = node.yaw
x0 = x_rec[-1]
y0 = y_rec[-1]
yaw0 = yaw_rec[-1]
plt.cla()
plt.plot(x_all, y_all, color='gray', linewidth=2.0)
plt.plot(x_rec, y_rec, linewidth=2.0, color='darkviolet')
plt.plot(cx[ind], cy[ind], '.r')
draw.draw_car(node.x, node.y, node.yaw, steer, C)
plt.axis("equal")
plt.title("LQR & PID: v=" + str(node.v * 3.6)[:4] + "km/h")
plt.gcf().canvas.mpl_connect(
'key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
plt.pause(0.001)
plt.show()
def main():
# generate path: [x, y, yaw]
states = [(0, 0, 0), (20, 15, 0), (35, 20, 90), (40, 0, 180),
(20, 0, 120), (5, -10, 180), (15, 5, 30)]
# states = [(-3, 3, 120), (10, -7, 30), (10, 13, 30), (20, 5, -25),
# (35, 10, 180), (30, -10, 160), (5, -12, 90)]
x, y, yaw, direct, path_x, path_y = generate_path(states)
# simulation
maxTime = 100.0
yaw_old = 0.0
x0, y0, yaw0, direct0 = x[0][0], y[0][0], yaw[0][0], direct[0][0]
x_rec, y_rec = [], []
for cx, cy, cyaw, cdirect in zip(x, y, yaw, direct):
t = 0.0
node = Node(x=x0, y=y0, yaw=yaw0, v=0.0, direct=direct0)
nodes = Nodes()
nodes.add(t, node)
ref_trajectory = PATH(cx, cy)
target_ind, _ = ref_trajectory.target_index(node)
while t <= maxTime:
if cdirect[0] > 0:
target_speed = 30.0 / 3.6
C.Ld = 4.0
C.dist_stop = 1.5
C.dc = -1.1
else:
target_speed = 20.0 / 3.6
C.Ld = 2.5
C.dist_stop = 0.2
C.dc = 0.2
xt = node.x + C.dc * math.cos(node.yaw)
yt = node.y + C.dc * math.sin(node.yaw)
dist = math.hypot(xt - cx[-1], yt - cy[-1])
if dist < C.dist_stop:
break
acceleration = pid_control(target_speed, node.v, dist, cdirect[0])
delta, target_ind = pure_pursuit(node, ref_trajectory, target_ind)
t += C.dt
node.update(acceleration, delta, cdirect[0])
nodes.add(t, node)
x_rec.append(node.x)
y_rec.append(node.y)
dy = (node.yaw - yaw_old) / (node.v * C.dt)
steer = rs.pi_2_pi(-math.atan(C.WB * dy))
yaw_old = node.yaw
x0 = nodes.x[-1]
y0 = nodes.y[-1]
yaw0 = nodes.yaw[-1]
direct0 = nodes.direct[-1]
# animation
plt.cla()
plt.plot(node.x, node.y, marker='.', color='k')
plt.plot(path_x, path_y, color='gray', linewidth=2)
plt.plot(x_rec, y_rec, color='darkviolet', linewidth=2)
plt.plot(cx[target_ind], cy[target_ind], ".r")
draw.draw_car(node.x, node.y, yaw_old, steer, C)
# for m in range(len(states)):
# draw.Arrow(states[m][0], states[m][1], np.deg2rad(states[m][2]), 2, 'blue')
plt.axis("equal")
plt.title("PurePursuit: v=" + str(node.v * 3.6)[:4] + "km/h")
plt.gcf().canvas.mpl_connect('key_release_event',
lambda event:
[exit(0) if event.key == 'escape' else None])
plt.pause(0.001)
plt.show()
def main():
# generate path
states = [(0, 0, 0), (20, 15, 0), (35, 20, 90), (40, 0, 180), (20, 0, 120),
(5, -10, 180), (15, 5, 30)]
#
# states = [(-3, 3, 120), (10, -7, 30), (10, 13, 30), (20, 5, -25),
# (35, 10, 180), (30, -10, 160), (5, -12, 90)]
x_ref, y_ref, yaw_ref, direct, curv, x_all, y_all = generate_path(states)
maxTime = 100.0
yaw_old = 0.0
x0, y0, yaw0, direct0 = \
x_ref[0][0], y_ref[0][0], yaw_ref[0][0], direct[0][0]
x_rec, y_rec, yaw_rec, direct_rec = [], [], [], []
for cx, cy, cyaw, cdirect, ccurv in zip(x_ref, y_ref, yaw_ref, direct,
curv):
t = 0.0
node = Node(x=x0, y=y0, yaw=yaw0, v=0.0, direct=cdirect[0])
ref_trajectory = Trajectory(cx, cy, cyaw, ccurv)
speed_ref = 30 / 3.6
while t < maxTime:
if cdirect[0] > 0:
speed_ref = 30.0 / 3.6
C.Ld = 3.5
else:
speed_ref = 20.0 / 3.6
C.Ld = 2.5
e, k, yawref, s0 = ref_trajectory.calc_track_error(node)
di = rear_wheel_feedback_control(node, e, k, yawref)
dist = math.hypot(node.x - cx[-1], node.y - cy[-1])
ai = pid_control(speed_ref, node.v, dist, node.direct)
node.update(ai, di, node.direct)
t += C.dt
if dist <= C.dref:
break
x_rec.append(node.x)
y_rec.append(node.y)
yaw_rec.append(node.yaw)
direct_rec.append(node.direct)
dy = (node.yaw - yaw_old) / (node.v * C.dt)
steer = rs.pi_2_pi(-math.atan(C.WB * dy))
yaw_old = node.yaw
x0 = x_rec[-1]
y0 = y_rec[-1]
yaw0 = yaw_rec[-1]
plt.cla()
plt.plot(x_all, y_all, color='gray', linewidth=2.0)
plt.plot(x_rec, y_rec, linewidth=2.0, color='darkviolet')
plt.plot(cx[s0], cy[s0], '.r')
draw.draw_car(node.x, node.y, node.yaw, steer, C)
plt.axis("equal")
plt.title("RearWheelFeedback: v=" + str(node.v * 3.6)[:4] + "km/h")
plt.gcf().canvas.mpl_connect(
'key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
plt.pause(0.001)
plt.show()