def main():
# ===== Vehicle parameters =====
l_f = 1.25
l_r = 1.40
dt = 0.02
m = 1300
l_f = l_f
l_r = l_r
width = 1.78
length = 4.25
turning_circle = 10.4
C_d = 0.34
A_f = 2.0
C_roll = 0.015
vehicle = vehicle_dynamics.Vehicle_Dynamics(m=m,
l_f=l_f,
l_r=l_r,
width=width,
length=length,
turning_circle=turning_circle,
C_d=C_d,
A_f=A_f,
C_roll=C_roll,
dt=dt)
# ===== Initial state =====
# States : [x; y; v; yaw]
# Actions : [steer; accel]
x = np.array([[0.0], [0.0], [0.0], [np.deg2rad(0)]]) # [X; Y; V; Yaw]
u = np.array([[0 * math.pi / 180], [0.0]]) # [steer; accel]
# Reference state
xr = np.array([[10.0], [10.0], [10.0],
[np.deg2rad(90)]]) # [X; Y; vel_x; Yaw]
# ===== Simulation Setup =====
nx = x.shape[0]
nu = u.shape[0]
sim_time = 1000
sim_tic = np.linspace(0, sim_time, sim_time)
states_plot = np.zeros((nx, sim_time))
actions_plot = np.zeros((nu, sim_time))
states_nonlinear_plot = np.zeros((nx, sim_time))
states_nonlinear_plot[:, 0] = x.T
# Ad, Bd, gd = vehicle.get_kinematics_model(x, u)
for i in range(sim_time):
print("===================== nsim :", i, "=====================")
# timestamp
tic = time.time()
if i >= 0.5 * sim_time:
xr = np.array([[10.0], [10.0], [10.0],
[np.deg2rad(90)]]) # [X; Y; vel_x; Yaw]
# ===== Get System matrices =====
# using current state and past action
Ad, Bd, gd = vehicle.get_kinematics_model(x, u)
# ===== PID Control ===== #
# p_steer = 5.0
# error_yaw = vehicle_dynamics.normalize_angle(xr[2] - x[2])
# # Steer control
# u[0] = p_steer * error_yaw
# if u[0] >= np.deg2rad(15):
# u[0] = np.deg2rad(15)
# if u[0] <= -np.deg2rad(15):
# u[0] = -np.deg2rad(15)
# # Speed control
# p_accel = 10.0
# error_vx = xr[3] - x[3]
# u[1] = p_accel * error_vx
# if u[1] >= 1:
# u[1] = 1
# if u[1] <= -3:
# u[1] = -3
u[0] = np.deg2rad(10)
u[1] = 1
# Plot
print("x :", x[0], "y :", x[1], "v :", x[2], "yaw :", x[3])
print("--------------------------------------------------")
print("steer :", u[0], "accel :", u[1])
plt.cla()
plt.plot(states_plot[0, :], states_plot[1, :], "-b", label="Drived")
plt.plot(states_nonlinear_plot[0, :],
states_nonlinear_plot[1, :],
"--r",
label="Non linear")
plt.grid(True)
plt.axis("equal")
plot_car(x[0], x[1], x[3], steer=u[0]) # plotting w.r.t. rear axle.
# plt.plot(x_pred, y_pred, "r")
# plt.plot(X_pred_last, Y_pred_last, ".r")
plt.pause(0.0001)
# Update States
x_next = np.matmul(Ad, x) + np.matmul(Bd, u) + gd
x_next_nonlinear = vehicle.update_kinematic_model(x, u)
x_next[3] = vehicle_dynamics.normalize_angle(x_next[3])
print("x_next :", x_next)
states_plot[:, i] = x.T
actions_plot[:, i] = u.T
states_nonlinear_plot[:, i] = x_next_nonlinear.T
x = x_next
ax1.set_title("Steer")
ax2.set_title("Accel")
ax3.set_title("X-Y plot")
ax1.plot(TT, UU[0,:])
ax2.plot(TT, UU[1,:])
ax3.plot(XX[0,:], XX[1,:])
ax3.plot(XX_front_axle[0,:], XX_front_axle[1,:], color='red')
ax3.plot(XX_rear_axle[0,:], XX_rear_axle[1,:], color='blue')
ax1.grid()
ax2.grid()
ax3.grid()
ax3.axis('square')
plot_car(XX_rear_axle[0,-1], XX_rear_axle[1,-1], XX[2,-1], UU[0,-1]) # vehicle at final state
# Figure 3
fig3 = plt.figure()
ax1 = fig3.add_subplot(1,2,1)
ax2 = fig3.add_subplot(1,2,2)
ax1.set_title("Front Slip angle")
ax2.set_title("Rear Slip angle")
ax1.plot(TT, XX_alpha[0,:])
ax2.plot(TT, XX_alpha[1,:])
ax1.grid()
ax2.grid()
def main():
# ===== Vehicle parameters =====
l_f = 1.25
l_r = 1.40
dt = 0.02
m = 1300
width = 1.78
length = 4.25
turning_circle = 10.4
C_d = 0.34
A_f = 2.0
C_roll = 0.015
vehicle = vehicle_models.Vehicle_Kinematics(
l_f=l_f, l_r=l_r, dt=dt) # Vehicle Kinematic Model
# ========== MPC parameters ==========
N = 100 # Prediction horizon
# ========== Initialization ==========
# Path
path_x = np.linspace(-10, 100, 100 / 0.5)
# path_y = np.ones(int(100/0.5))*5
path_y = np.linspace(-10, 100, 100 / 0.5) * 0.0 - 5.0
# Initial state
# States : [x; y; v; yaw]
# Actions : [steer; accel]
x = np.array([[0.0], [0.0], [20.0], [np.deg2rad(0)]]) # [X; Y; V; Yaw]
u = np.array([[0 * math.pi / 180], [0.01]]) # [steer; accel]
x0 = x
u0 = u
x_vec = np.squeeze(x, axis=1) # (N,) shape for QP solver, NOT (N,1).
nx = x.shape[0]
nu = u.shape[0]
# ========== Initialize Predictive States and Controls ==========
u_noise = np.zeros((nu, 1))
mu_steer = 0.0
sigma_steer = np.deg2rad(1)
mu_accel = 0.0
sigma_accel = 0.1
pred_u = np.zeros((nu, N + 1))
for i in range(N):
# u_noise[0] = np.random.normal(mu_steer, sigma_steer, 1)
# u_noise[1] = np.random.normal(mu_accel, sigma_accel, 1)
# pred_u[:,i] = np.transpose(u0 + u_noise)
pred_u[:, i] = np.transpose(u0)
pred_u[:, -1] = pred_u[:, -2] # append last pred_u for N+1
pred_x = np.zeros((nx, N + 1))
pred_x[:, 0] = x0.T
x_k = np.copy(
x0) # if x_k = x0, x0 would be changed by update kinematic model
for i in range(0, N):
x_k1 = vehicle.update_kinematics_model(x_k, pred_u[:, i])
pred_x[:, i + 1] = x_k1.T
x_k = x_k1
# pred_x[:,i+1] = x0.T
# ========== Constraints ==========
umin = np.array([-np.deg2rad(15), -3.]) # u : [steer, accel]
umax = np.array([np.deg2rad(15), 1.])
xmin = np.array([-np.inf, -np.inf, -100.,
-2 * np.pi]) # [X; Y; vel_x; Yaw]
xmax = np.array([np.inf, np.inf, 100., 2 * np.pi])
# ========== Objective function ==========
# MPC weight matrix
Q = sparse.diags([10.0, 10.0, 100.0, 10.0]) # weight matrix for state
# QN = Q
QN = sparse.diags([100.0, 100.0, 1000.0,
100.0]) # weight matrix for terminal state
R = sparse.diags([1000, 100]) # weight matrix for control input
# R_before = 10*sparse.eye(nu) # weight matrix for control input
# ========== Simulation Setup ==========
sim_time = 1000
plt_tic = np.linspace(0, sim_time, sim_time)
plt_states = np.zeros((nx, sim_time))
plt_actions = np.zeros((nu, sim_time))
for i in range(sim_time):
tic = time.time()
print("===================== sim_time :", i, "=====================")
# Discrete time model of the vehicle lateral dynamics
# Reference states
Xr, _ = reference_search(path_x, path_y, pred_x, dt, N)
plt.plot(pred_x[0, :], pred_x[1, :], "y")
print("x_vec :", x_vec)
print("pred_x[:,0] :", pred_x[:, 0])
# Discrete time model of the vehicle lateral dynamics
Ad_list, Bd_list, gd_list = [], [], []
for ii in range(N):
Ad, Bd, gd = vehicle.get_kinematics_model(pred_x[:, ii],
pred_u[:, ii])
Ad_list.append(Ad)
Bd_list.append(Bd)
gd_list.append(gd)
# ========== Constraints ==========
umin = np.array([-np.deg2rad(15), -3.]) # u : [steer, accel]
umax = np.array([np.deg2rad(15), 1.])
xmin = np.array([-np.inf, -np.inf, -100.,
-2 * np.pi]) # [X; Y; vel_x; Yaw]
xmax = np.array([np.inf, np.inf, 100., 2 * np.pi])
# Solve MPC
# res = mpc(Ad_mat, Bd_mat, gd_mat, x_vec, Xr, Q, QN, R, N, xmin, xmax, umin, umax)
# if i <= -1:
# res = mpc(Ad_list[0], Bd_list[0], gd_list[0], x_vec, Xr, Q, QN, R, N, xmin, xmax, umin, umax)
# else:
# res = mpc__(Ad_list, Bd_list, gd_list, x_vec, Xr, Q, QN, R, N, xmin, xmax, umin, umax)
res = mpc__(Ad_list, Bd_list, gd_list, x_vec, Xr, Q, QN, R, N, xmin,
xmax, umin, umax)
# Check solver status
if res.info.status != 'solved':
print('OSQP did not solve the problem!')
# raise ValueError('OSQP did not solve the problem!')
plt.pause(5.0)
continue
# Apply first control input to the plant
ctrl = res.x[-N * nu:-(N - 1) * nu]
toc = time.time()
print("ctrl :", ctrl)
# Predictive States and Actions
sol_state = res.x[:-N * nu]
sol_action = res.x[-N * nu:]
for ii in range((N + 1) * nx):
if ii % nx == 0:
pred_x[0, ii // nx] = sol_state[ii]
elif ii % nx == 1:
pred_x[1, ii // nx] = sol_state[ii]
elif ii % nx == 2:
pred_x[2, ii // nx] = sol_state[ii]
else: # ii % 4 == 3:
pred_x[3, ii // nx] = sol_state[ii]
for jj in range((N) * nu):
if jj % nu == 0:
pred_u[0, jj // nu] = sol_action[jj]
else: # jj % nu == 1
pred_u[1, jj // nu] = sol_action[jj]
pred_u[:, -1] = pred_u[:, -2] # append last control
# Plot
print("Current x :", x[0], "y :", x[1], "v :", x[2], "yaw :", x[3])
print("------------------------------------------------------------")
# print("Reference x :", xr[0], "y :", xr[1], "v :", xr[2], "yaw :", xr[3])
print("Reference x :", Xr[0, 0], "y :", Xr[1, 0], "v :", Xr[2, 0],
"yaw :", Xr[3, 0])
print("------------------------------------------------------------")
print("steer :", u[0], "accel :", u[1])
print("Process time :", toc - tic)
plt.cla()
plt.plot(plt_states[0, :i], plt_states[1, :i], "-b",
label="Drived") # plot from 0 to i
plt.grid(True)
plt.axis("equal")
plt.plot(x0[0], x0[1], "*g", label="Initial")
plot_car(x[0], x[1], x[3], steer=u[0]) # plotting w.r.t. rear axle.
plt.plot(pred_x[0, :], pred_x[1, :], "r")
plt.plot(path_x, path_y, label="Path")
plt.plot(Xr[0, :], Xr[1, :], "g")
plt.pause(0.0001)
# Update States
u = np.expand_dims(ctrl, axis=1) # from (N,) to (N,1)
x_next = np.matmul(Ad_list[0], x) + np.matmul(Bd_list[0],
u) + gd_list[0]
plt_states[:, i] = x.T
plt_actions[:, i] = u.T
x = x_next
x_vec = np.squeeze(x, axis=1) # (N,) shape for QP solver, NOT (N,1).
# Update Predictive States and Actions
temp_pred_x = pred_x
temp_pred_u = pred_u
# index 0
pred_x[:, 0] = x_next.T
pred_u[:, 0] = temp_pred_u[:, 1] # before u.T
# index 1 ~ N-2
for ii in range(0, N - 2):
pred_x[:, ii + 1] = temp_pred_x[:, ii + 2]
pred_u[:, ii + 1] = temp_pred_u[:, ii + 2]
# index N-1
pred_x[:, -2] = temp_pred_x[:, N]
pred_u[:, -2] = temp_pred_u[:, N - 1]
# index N
# append last state using last A, B matrix and last pred state
# append last control with last pred control
last_state = np.expand_dims(temp_pred_x[:, N],
axis=1) # from (N,) to (N,1)
last_control = np.expand_dims(temp_pred_u[:, N - 1], axis=1)
pred_x[:, -1] = np.transpose(
vehicle.update_kinematics_model(last_state, last_control))
pred_u[:, -1] = pred_u[:, N - 1]
def main():
# ===== Vehicle parameters =====
l_f = 1.25
l_r = 1.40
dt = 0.05
m = 1300
width = 1.78
length = 4.25
turning_circle = 10.4
C_d = 0.34
A_f = 2.0
C_roll = 0.015
vehicle = vehicle_models.Vehicle_Dynamics(m=m,
l_f=l_f,
l_r=l_r,
width=width,
length=length,
turning_circle=turning_circle,
C_d=C_d,
A_f=A_f,
C_roll=C_roll,
dt=dt)
# ========== MPC parameters ==========
N = 30 # Prediction horizon
Q = sparse.diags([100.0, 100.0, 100.0, 50.0, 50.0,
50.0]) # weight matrix for state
QN = sparse.diags([1000.0, 1000.0, 1000.0, 500.0, 500.0,
500.0]) # weight matrix for terminal state
R = sparse.diags([50, 50]) # weight matrix for control input
# ========== Constraints ==========
del_umin = np.array(
[-np.deg2rad(2.0), -0.5]
) # del_u / tic (not del_u / sec) => del_u/sec = del_u/tic * tic/sec = del_u/tic * 20(Hz)
del_umax = np.array([np.deg2rad(2.0),
0.5]) # 15 * 14 deg = 210 deg (steering wheel).
xmin_tilda = np.array([
-np.inf, -np.inf, -2 * np.pi, -100., -30., -0.5 * np.pi,
-np.deg2rad(15), -3.
]) # (x_min, u_min)
xmax_tilda = np.array([
np.inf, np.inf, 2 * np.pi, 100., 30., 0.5 * np.pi,
np.deg2rad(15), 1.
]) # (x_max, u_max)
# ========== Initialization ==========
# Path
path_x = np.linspace(-10, 100, 100 / 0.5)
path_y = np.linspace(-10, 100, 100 / 0.5) * 0.5 + 5
# Initial state
# States : [X; Y; Yaw; V_x; V_y; Yaw_rate]
# Actions : [steer; accel]
x0 = np.array([[0.0], [0.0], [np.deg2rad(0)], [15.0], [0.0],
[np.deg2rad(0)]]) # [X; Y; Yaw; V_x; V_y; Yaw_rate]
u0 = np.array([[0 * math.pi / 180], [0.0]]) # [steer; accel]
del_u0 = np.array([[0 * math.pi / 180], [0.0]]) # [del steer; del accel]
nx = x0.shape[0]
nu = u0.shape[0]
# for Incremental MPC
x0_tilda = np.vstack([x0, u0])
x_tilda = np.copy(x0_tilda)
x_tilda_vec = np.squeeze(x_tilda, axis=1)
# ========== Initialial guess states and controls ==========
x_tilda_k = np.copy(x0_tilda)
del_u_k = np.copy(del_u0)
pred_del_u = np.zeros((nu, N + 1))
for i in range(N):
pred_del_u[:, i] = del_u_k.T
pred_del_u[:, -1] = pred_del_u[:, -2]
pred_x_tilda = np.zeros((nx + nu, N + 1))
pred_x_tilda[:, 0] = x_tilda_k.T
for i in range(0, N):
x_k = x_tilda_k[:-nu] # decompose to xk, uk
u_k = x_tilda_k[-nu:]
Ad, Bd, gd = vehicle.get_dynamics_model(x_k, u_k)
x_k1 = np.matmul(Ad, x_k) + np.matmul(Bd, u_k) + gd # calc next state
u_k1 = u_k + np.expand_dims(pred_del_u[:, i],
axis=1) # calc next action
x_tilda_k1 = np.vstack([x_k1, u_k1]) # compose x tilda
pred_x_tilda[:, i + 1] = x_tilda_k1.T
x_tilda_k = x_tilda_k1
# ========== Simulation Setup ==========
sim_time = 1000
plt_tic = np.linspace(0, sim_time, sim_time)
plt_states = np.zeros((nx, sim_time))
plt_actions = np.zeros((nu, sim_time))
# x = np.copy(x0)
for i in range(sim_time):
tic = time.time()
print("===================== sim_time :", i, "=====================")
# Discrete time model of the vehicle lateral dynamics
# Reference states
# Xr, _ = reference_search(path_x, path_y, pred_x, dt, N)
Xr, _ = reference_search(path_x, path_y, pred_x_tilda[:-nu, :], dt, N)
# Discrete time model of the vehicle lateral dynamics
Ad_list, Bd_list, gd_list = [], [], []
for ii in range(N):
# Ad, Bd, gd = vehicle.get_dynamics_model(pred_x[:,ii], pred_u[:,ii])
Ad, Bd, gd = vehicle.get_dynamics_model(pred_x_tilda[:-nu, ii],
pred_x_tilda[-nu:, ii])
Ad_list.append(Ad)
Bd_list.append(Bd)
gd_list.append(gd)
# Solve MPC
pred_x_tilda, pred_del_u = mpc_increment(Ad_list, Bd_list, gd_list,
x_tilda_vec, Xr, pred_x_tilda,
pred_del_u, Q, QN, R, N,
xmin_tilda, xmax_tilda,
del_umin, del_umax)
# iter_damping = 0.5
# for _ in range(1):
# pred_x_tilda_up, pred_del_u_up = mpc_increment(Ad_list, Bd_list, gd_list, x_tilda_vec, Xr, pred_x_tilda, pred_del_u, Q, QN, R, N, xmin_tilda, xmax_tilda, del_umin, del_umax)
# pred_x_tilda = iter_damping * pred_x_tilda + (1 - iter_damping) * pred_x_tilda_up
# pred_del_u = iter_damping * pred_del_u + (1 - iter_damping) * pred_del_u_up
# Plot
print("Current x :", x_tilda[0], "y :", x_tilda[1], "yaw :",
x_tilda[2], "vx :", x_tilda[3], "vy :", x_tilda[4], "yawrate :",
x_tilda[5])
print("------------------------------------------------------------")
print("Reference x :", Xr[0, 0], "y :", Xr[1, 0], "yaw :", Xr[2, 0],
"vx :", Xr[3, 0], "vy :", Xr[4, 0], "yawrate :", Xr[5, 0])
print("------------------------------------------------------------")
print("steer :", x_tilda[6], "accel :", x_tilda[7])
# Save current state
plt_states[:, i] = x_tilda[:-nu].T
plt_actions[:, i] = x_tilda[-nu:].T
if VISUALIZE_PLOT:
plt.cla()
plt.plot(path_x, path_y, label="Path")
plt.plot(Xr[0, :], Xr[1, :], "g", label="Local Reference")
plt.plot(plt_states[0, :i + 1],
plt_states[1, :i + 1],
"-b",
label="Drived") # plot from 0 to i
plt.grid(True)
plt.axis("equal")
# plot_car(x[0], x[1], vehicle_models.normalize_angle(x[2]), steer=u[0]) # plotting w.r.t. rear axle.
plot_car(x_tilda[0],
x_tilda[1],
vehicle_models.normalize_angle(x_tilda[2]),
steer=x_tilda[6]) # plotting w.r.t. rear axle.
plt.plot(pred_x_tilda[0, :],
pred_x_tilda[1, :],
"r",
label="Predictive States")
plt.pause(0.001)
# Update States
u_past = x_tilda[-nu:]
u = u_past + np.expand_dims(pred_del_u[:, 0], axis=1)
x_next = np.matmul(Ad_list[0], x_tilda[:-nu]) + np.matmul(
Bd_list[0], u) + gd_list[0]
x = x_next
x_tilda = np.vstack([x, u])
x_tilda_vec = np.squeeze(x_tilda, axis=1)
# Update Predictive States and Actions
temp_pred_x_tilda = np.copy(pred_x_tilda)
temp_pred_del_u = np.copy(pred_del_u)
# index 0
pred_x_tilda[:, 0] = x_tilda.T # shift one step
pred_del_u[:, 0] = temp_pred_del_u[:, 1] # shift one step
# index 1 ~ N-2
for ii in range(0, N - 2):
pred_x_tilda[:, ii + 1] = temp_pred_x_tilda[:, ii + 2]
pred_del_u[:, ii + 1] = temp_pred_del_u[:, ii + 2]
# index N-1
pred_x_tilda[:, -2] = temp_pred_x_tilda[:, N]
pred_del_u[:, -2] = temp_pred_del_u[:, N - 1]
# index N
# append last state using last A, B matrix and last pred state
# append last control with last pred control
# ODE?
last_state = np.expand_dims(temp_pred_x_tilda[:-nu, N],
axis=1) # from (N,) to (N,1)
last_control = np.expand_dims(temp_pred_x_tilda[-nu:, N - 1], axis=1)
final_state, _, _ = vehicle.update_dynamics_model(
last_state, last_control)
pred_x_tilda[:-nu, -1] = np.transpose(final_state)
pred_x_tilda[-nu:, -1] = pred_x_tilda[-nu:, -2]
pred_del_u[:, -1] = pred_del_u[:, -2]
if temp_pred_x_tilda[2, 0] - temp_pred_x_tilda[2, 1] > np.pi:
temp_pred_x_tilda[2, 1:] = temp_pred_x_tilda[2, 1:] + 2 * np.pi
print("from '-pi ~ +pi' to '0 ~ +2pi'")
if temp_pred_x_tilda[2, 0] - temp_pred_x_tilda[2, 1] < -np.pi:
temp_pred_x_tilda[2, 1:] = temp_pred_x_tilda[2, 1:] - 2 * np.pi
print("from '-pi ~ +pi' to '0 ~ +2pi'")
# if check_goal(x, xr, goal_dist=1, stop_speed=5):
# print("Goal")
# break
toc = time.time()
print("Process time :", toc - tic)
def main():
# Simulation
# Initial state
x0 = np.array([[ 0.],
[ 0.],
[ np.deg2rad(0)],
[ 5.0],
[ 0.],
[ 0.]]) # [X; Y; Yaw; vel_x; vel_y; Yaw_rate]
u0 = np.array([[0*math.pi/180],
[0.0]]) # [steer; traction_accel]
# Reference state
xr = np.array([[ 0.],
[ 0.],
[ np.deg2rad(0)],
[ 25.0],
[ 0.],
[ 0.]]) # [X; Y; Yaw; vel_x; vel_y; Yaw_rate]
# num of state, action
nx = 6
nu = 2
# Vehicle parameters
l_f = 1.25
l_r = 1.40
dt = 0.02
m=1300
l_f=l_f
l_r=l_r
width = 1.78
length = 4.25
turning_circle=10.4
C_d = 0.34
A_f = 2.0
C_roll = 0.015
sim_time = 500 # time. [sec]
XX = np.zeros([nx, sim_time])
XX_front_axle = np.zeros([2, sim_time])
XX_rear_axle = np.zeros([2, sim_time])
XX_alpha = np.zeros([2, sim_time]) # front, rear
UU = np.zeros([nu, sim_time])
TT = np.linspace(0, sim_time*dt, sim_time)
vehicle = Vehicle_Dynamics(m=m, l_f=l_f, l_r=l_r, width = width, length = length,
turning_circle=turning_circle,
C_d = C_d, A_f = A_f, C_roll = C_roll, dt = dt)
for i in range(sim_time):
print("===================== nsim :", i, "=====================")
# timestamp
tic = time.time()
if i >= 0.1*sim_time:
xr = np.array([[ 0.],
[ 0.],
[ np.deg2rad(10)],
[ 15.0],
[ 0.],
[ 0.]]) # [X; Y; Yaw; vel_x; vel_y; Yaw_rate]
if i >= 0.5*sim_time:
xr = np.array([[ 0.],
[ 0.],
[ np.deg2rad(100)],
[ 15.0],
[ 0.],
[ 0.]]) # [X; Y; Yaw; vel_x; vel_y; Yaw_rate]
# u[0] += np.deg2rad(0.1)
# if u[0] >= np.deg2rad(5):
# u[0] = np.deg2rad(5)
# ===== PID Control ===== #
p_steer = 5.0
error_yaw = normalize_angle(xr[2,0] - x0[2,0])
# Steer control
u0[0,0] = p_steer * error_yaw
if u0[0,0] >= np.deg2rad(15):
u0[0,0] = np.deg2rad(15)
if u0[0,0] <= -np.deg2rad(15):
u0[0,0] = -np.deg2rad(15)
# Speed control
p_accel = 2.0
error_vx = xr[3,0] - x0[3,0]
u0[1,0] = p_accel * error_vx
if u0[1,0] >= 1:
u0[1,0] = 1
if u0[1,0] <= -3:
u0[1,0] = -3
XX[:,i] = x0.T # list for plot.
TT[i] = dt * i
UU[:,i] = u0.T
# normalize angle (Better not to normalize.)
# x_k1[2] = normalize_angle(x_k1[2])
# print("Normalized in loop. x_k1[2] :", x_k1[2])
# State Prediction
N = 50
pred_x = np.zeros((nx, N+1))
pred_x[:,0] = x0.T
x = x0
for ii in range(0, N):
# x_next, _, _ = vehicle.update_rear_wheel_dynamics_model(x, u0)
x_next, _, _ = vehicle.update_dynamics_model(x, u0)
pred_x[:,ii+1] = x_next.T
x = x_next
# print
print("x :", x0[0,0], "y :", x0[1,0], "yaw :", x0[2,0], "Vx :", x0[3,0], "Vy :", x0[4,0], "yawRate :", x0[5,0])
print(" ------------------------------------------------------------------------------ ")
print("steer :", u0[0,0], "accel :", u0[1,0])
# print("Error yaw :", np.rad2deg(error_yaw))
plt.cla()
plt.plot(XX[0,:i+1], XX[1,:i+1], "-b", label="Driven Path")
plt.grid(True)
plt.axis("equal")
plot_car(x0[0,0], x0[1,0], x0[2,0], steer=u0[0,0])
plt.plot(pred_x[0,:], pred_x[1,:], "r")
plt.pause(0.0001)
# x1, alpha_f, alpha_r = vehicle.update_rear_wheel_dynamics_model(x0, u0)
x1, alpha_f, alpha_r = vehicle.update_dynamics_model(x0, u0)
XX_alpha[0,i] = alpha_f
XX_alpha[1,i] = alpha_r
x0 = x1 # update t+1 state
toc = time.time()
print("process time :", toc - tic)
print("final position")
print("x, y, yaw :", XX[0,-1], XX[1,-1], XX[2,-1])
for i in range(sim_time):
XX_front_axle[0,i] = XX[0,i] + l_f * math.cos(XX[2,i])
XX_front_axle[1,i] = XX[1,i] + l_f * math.sin(XX[2,i])
XX_rear_axle[0,i] = XX[0,i] - l_r * math.cos(XX[2,i])
XX_rear_axle[1,i] = XX[1,i] - l_r * math.sin(XX[2,i])
# Figure 1
fig1 = plt.figure()
ax1 = fig1.add_subplot(2,3,1)
ax2 = fig1.add_subplot(2,3,2)
ax3 = fig1.add_subplot(2,3,3)
ax4 = fig1.add_subplot(2,3,4)
ax5 = fig1.add_subplot(2,3,5)
ax6 = fig1.add_subplot(2,3,6)
ax1.set_title("X")
ax2.set_title("Y")
ax3.set_title("Yaw")
ax4.set_title("Vel_x")
ax5.set_title("Vel_y")
ax6.set_title("Yaw_rate")
ax1.plot(TT, XX[0,:])
ax2.plot(TT, XX[1,:])
ax3.plot(TT, XX[2,:])
ax4.plot(TT, XX[3,:])
ax5.plot(TT, XX[4,:])
ax6.plot(TT, XX[5,:])
ax1.grid()
ax2.grid()
ax3.grid()
ax4.grid()
ax5.grid()
ax6.grid()
# Figure 2
fig2 = plt.figure()
ax1 = fig2.add_subplot(1,3,1)
ax2 = fig2.add_subplot(1,3,2)
ax3 = fig2.add_subplot(1,3,3)
ax1.set_title("Steer")
ax2.set_title("Accel")
ax3.set_title("X-Y plot")
ax1.plot(TT, UU[0,:])
ax2.plot(TT, UU[1,:])
ax3.plot(XX[0,:], XX[1,:])
ax3.plot(XX_front_axle[0,:], XX_front_axle[1,:], color='red')
ax3.plot(XX_rear_axle[0,:], XX_rear_axle[1,:], color='blue')
ax1.grid()
ax2.grid()
ax3.grid()
ax3.axis('square')
plot_car(XX_rear_axle[0,-1], XX_rear_axle[1,-1], XX[2,-1], UU[0,-1]) # vehicle at final state
# Figure 3
fig3 = plt.figure()
ax1 = fig3.add_subplot(1,2,1)
ax2 = fig3.add_subplot(1,2,2)
ax1.set_title("Front Slip angle")
ax2.set_title("Rear Slip angle")
ax1.plot(TT, XX_alpha[0,:])
ax2.plot(TT, XX_alpha[1,:])
ax1.grid()
ax2.grid()
plt.show()
def main():
# ===== Vehicle parameters =====
l_f = 1.25
l_r = 1.40
dt = 0.02
m = 1300
width = 1.78
length = 4.25
turning_circle = 10.4
C_d = 0.34
A_f = 2.0
C_roll = 0.015
vehicle = vehicle_models.Vehicle_Kinematics(
l_f=l_f, l_r=l_r, dt=dt) # Vehicle Kinematic Model
# ========== MPC parameters ==========
N = 40 # Prediction horizon
# ========== Constraints ==========
del_umin = np.array(
[-np.deg2rad(0.5), -0.5]
) # del_u / tic (not del_u / sec) => del_u/sec = del_u/tic * tic/sec = del_u/tic * 20(Hz)
del_umax = np.array([np.deg2rad(0.5), 0.5])
xmin_tilda = np.array(
[-np.inf, -np.inf, -100., -2 * np.pi, -np.deg2rad(15),
-3.]) # (x_min, u_min)
xmax_tilda = np.array(
[np.inf, np.inf, 100., 2 * np.pi,
np.deg2rad(15), 1.]) # (x_max, u_max)
# ========== Objective function ==========
# MPC weight matrix
Q = sparse.diags([50.0, 50.0, 10.0, 50.0]) # weight matrix for state
# QN = Q
QN = sparse.diags([1000.0, 1000.0, 100.0,
1000.0]) # weight matrix for terminal state
R = sparse.diags([100, 100]) # weight matrix for control input
# R_before = 10*sparse.eye(nu) # weight matrix for control input
# ========== Initialization ==========
# Path
path_x = np.linspace(-10, 100, 100 / 0.5)
# path_y = np.ones(int(100/0.5))*5
path_y = np.linspace(-10, 100, 100 / 0.5) * 0.0 + 0.0
# Initial state
# States : [x; y; v; yaw]
# Actions : [steer; accel]
x0 = np.array([[0.0], [0.0], [20.0], [np.deg2rad(0)]]) # [X; Y; V; Yaw]
u0 = np.array([[0 * math.pi / 180], [0.01]]) # [steer; accel]
del_u0 = np.array([[0 * math.pi / 180], [0.0]]) # [del steer; del accel]
nx = x0.shape[0]
nu = u0.shape[0]
# for Incremental MPC
x0_tilda = np.vstack([x0, u0])
x_tilda = np.copy(x0_tilda)
x_tilda_vec = np.squeeze(x_tilda, axis=1)
# ========== Initialize Predictive States and Controls ==========
# u_noise = np.zeros((nu, 1))
# mu_steer = 0.0
# sigma_steer = np.deg2rad(1)
# mu_accel = 0.0
# sigma_accel = 0.1
x_tilda_k = np.copy(x0_tilda)
del_u_k = np.copy(del_u0)
pred_del_u = np.zeros((nu, N + 1))
for i in range(N):
pred_del_u[:, i] = del_u_k.T
pred_del_u[:, -1] = pred_del_u[:, -2]
pred_x_tilda = np.zeros((nx + nu, N + 1))
pred_x_tilda[:, 0] = x_tilda_k.T
for i in range(0, N):
x_k = x_tilda_k[:-nu] # decompose to xk, uk
u_k = x_tilda_k[-nu:]
Ad, Bd, gd = vehicle.get_kinematics_model(x_k, u_k)
x_k1 = np.matmul(Ad, x_k) + np.matmul(Bd, u_k) + gd # calc next state
u_k1 = u_k + np.expand_dims(pred_del_u[:, i],
axis=1) # calc next action
x_tilda_k1 = np.vstack([x_k1, u_k1]) # compose x tilda
pred_x_tilda[:, i + 1] = x_tilda_k1.T
x_tilda_k = x_tilda_k1
# ========== Simulation Setup ==========
sim_time = 1000
plt_tic = np.linspace(0, sim_time, sim_time)
plt_states = np.zeros((nx, sim_time))
plt_actions = np.zeros((nu, sim_time))
# plt_pred_final_states = np.zeros((nx, sim_time))
for i in range(sim_time):
# Scenario : Double Lane Change
if i > 0.05 * sim_time:
# Path
path_x = np.linspace(-10, 100, 100 / 0.5)
path_y = np.linspace(-10, 100, 100 / 0.5) * 0.0 + 4
# path_x = np.ones(int(100/0.5))*5
# path_y = np.ones(int(100/0.5))*5
if i > 0.15 * sim_time:
# Path
path_x = np.linspace(-10, 100, 100 / 0.5)
# path_y = np.ones(int(100/0.5))*5
path_y = np.linspace(-10, 100, 100 / 0.5) * 0.0 + 0
tic = time.time()
print("===================== sim_time :", i, "=====================")
# Discrete time model of the vehicle lateral dynamics
# Reference states
Xr, _ = reference_search(path_x, path_y, pred_x_tilda[:-nu, :], dt, N)
plt.plot(pred_x_tilda[0, :], pred_x_tilda[1, :], "y")
# Discrete time model of the vehicle lateral dynamics
Ad_list, Bd_list, gd_list = [], [], []
for ii in range(N):
Ad, Bd, gd = vehicle.get_kinematics_model(pred_x_tilda[:-nu, ii],
pred_x_tilda[-nu:, ii])
Ad_list.append(Ad)
Bd_list.append(Bd)
gd_list.append(gd)
# Solve MPC
pred_x_tilda, pred_del_u = mpc_increment(Ad_list, Bd_list, gd_list,
x_tilda_vec, Xr, pred_x_tilda,
pred_del_u, Q, QN, R, N,
xmin_tilda, xmax_tilda,
del_umin, del_umax)
toc = time.time()
# Plot
print("Current x :", x_tilda[0], "y :", x_tilda[1], "v :",
x_tilda[2], "yaw :", x_tilda[3])
print("------------------------------------------------------------")
print("Reference x :", Xr[0, 0], "y :", Xr[1, 0], "v :", Xr[2, 0],
"yaw :", Xr[3, 0])
print("------------------------------------------------------------")
print("steer :", x_tilda[4], "accel :", x_tilda[5])
print("Process time :", toc - tic)
# Save current state
plt_states[:, i] = x_tilda[:-nu].T
plt_actions[:, i] = x_tilda[-nu:].T
# plt_pred_final_states[:,i] = pred_x_tilda[:-nu, -1].T
# Plot current state
plt.cla()
plt.plot(path_x, path_y, "y", label="Path")
plt.plot(Xr[0, :], Xr[1, :], "g", label="Local Reference")
plt.plot(plt_states[0, :i + 1],
plt_states[1, :i + 1],
"-b",
label="Drived") # plot from 0 to i
plt.grid(True)
plt.axis("equal")
plt.plot(x_tilda[0], x_tilda[1], "*g", label="Initial")
plot_car(x_tilda[0], x_tilda[1], x_tilda[3],
steer=x_tilda[4]) # plotting w.r.t. rear axle.
plt.plot(pred_x_tilda[0, :],
pred_x_tilda[1, :],
"r",
label="Predictive States")
# plt.plot(plt_pred_final_states[0,:], plt_pred_final_states[1,:], "*r", label="Predictive Final States")
plt.pause(0.0001)
# Update States
u_past = x_tilda[-nu:]
u = u_past + np.expand_dims(pred_del_u[:, 0], axis=1)
x_next = np.matmul(Ad_list[0], x_tilda[:-nu]) + np.matmul(
Bd_list[0], u) + gd_list[0]
x = x_next
x_tilda = np.vstack([x, u])
x_tilda_vec = np.squeeze(x_tilda, axis=1)
# Update Predictive States and Actions
temp_pred_x_tilda = pred_x_tilda
temp_pred_del_u = pred_del_u
# index 0
pred_x_tilda[:, 0] = x_tilda.T
pred_del_u[:, 0] = temp_pred_del_u[:, 1] # before u.T
# index 1 ~ N-2
for ii in range(0, N - 2):
pred_x_tilda[:, ii + 1] = temp_pred_x_tilda[:, ii + 2]
pred_del_u[:, ii + 1] = temp_pred_del_u[:, ii + 2]
# index N-1
pred_x_tilda[:, -2] = temp_pred_x_tilda[:, N]
pred_del_u[:, -2] = temp_pred_del_u[:, N - 1]
# index N
# append last state using last A, B matrix and last pred state
# append last control with last pred control
last_state = np.expand_dims(temp_pred_x_tilda[:-nu, N],
axis=1) # from (N,) to (N,1)
last_control = np.expand_dims(temp_pred_x_tilda[-nu:, N - 1], axis=1)
pred_x_tilda[:-nu, -1] = np.transpose(
vehicle.update_kinematics_model(last_state, last_control))
pred_x_tilda[-nu:, -1] = pred_x_tilda[-nu:, -2]
pred_del_u[:, -1] = pred_del_u[:, -2]
def main():
# ===== Vehicle parameters =====
l_f = 1.25
l_r = 1.40
dt = 0.02
m = 1300
width = 1.78
length = 4.25
turning_circle = 10.4
C_d = 0.34
A_f = 2.0
C_roll = 0.015
vehicle = vehicle_models.Vehicle_Kinematics(l_f=l_f, l_r=l_r, dt=dt)
# ========== MPC parameters ==========
N = 100 # Prediction horizon
# ========== Initialization ==========
# Path
path_x = np.linspace(-10, 100, 100 / 0.5)
# path_y = np.ones(int(100/0.5))*5
path_y = np.linspace(-10, 100, 100 / 0.5) * 0.0 - 5.0
# Initial state
# States : [x; y; v; yaw]
# Actions : [steer; accel]
x = np.array([[0.0], [0.0], [5.0], [np.deg2rad(30)]]) # [X; Y; V; Yaw]
u = np.array([[0 * math.pi / 180], [0.01]]) # [steer; accel]
x0 = x
u0 = u
x_vec = np.squeeze(x, axis=1) # (N,) shape for QP solver, NOT (N,1).
nx = x.shape[0]
nu = u.shape[0]
# Initialize Predictive States and Controls
u_noise = np.zeros((nu, 1))
mu_steer = 0.0
sigma_steer = np.deg2rad(1)
mu_accel = 0.0
sigma_accel = 0.1
pred_u = np.zeros((nu, N + 1))
for i in range(N):
u_noise[0] = np.random.normal(mu_steer, sigma_steer, 1)
u_noise[1] = np.random.normal(mu_accel, sigma_accel, 1)
pred_u[:, i] = np.transpose(u0 + u_noise)
pred_u[:, -1] = pred_u[:, -2] # append last control input
pred_x = np.zeros((nx, N + 1))
pred_x[:, 0] = x0.T
for i in range(1, N):
x0 = vehicle.update_kinematics_model(x0, pred_u[:, i])
pred_x[:, i] = x0.T
# ========== Reference state ==========
# xr = np.array([[5.0],
# [10.0],
# [10.0],
# [np.deg2rad(90)]]) # [X; Y; vel_x; Yaw]
Xr, _ = reference_search(path_x, path_y, pred_x, dt, N)
# Discrete time model of the vehicle lateral dynamics
# Ad_mat, Bd_mat, gd_mat = [], [], []
# for i in range(N+1):
Ad_mat, Bd_mat, gd_mat = vehicle.get_kinematics_model(x, u)
# ========== Constraints ==========
umin = np.array([-np.deg2rad(15), -3.]) # u : [steer, accel]
umax = np.array([np.deg2rad(15), 1.])
xmin = np.array([-np.inf, -np.inf, -100., -np.pi]) # [X; Y; vel_x; Yaw]
xmax = np.array([np.inf, np.inf, 100., np.pi])
# ========== Objective function ==========
# MPC weight matrix
Q = sparse.diags([1.0, 1.0, 5.0, 10.0]) # weight matrix for state
# QN = Q
QN = sparse.diags([10.0, 10.0, 50.0,
100.0]) # weight matrix for terminal state
R = sparse.diags([0.1, 0.1]) # weight matrix for control input
# R_before = 10*sparse.eye(nu) # weight matrix for control input
# ========== Simulation Setup ==========
sim_time = 1000
plt_tic = np.linspace(0, sim_time, sim_time)
plt_states = np.zeros((nx, sim_time))
plt_actions = np.zeros((nu, sim_time))
for i in range(sim_time):
tic = time.time()
print("===================== sim_time :", i, "=====================")
# Discrete time model of the vehicle lateral dynamics
# Reference states
# xr = np.array([[5.0],
# [10.0],
# [10.0],
# [np.deg2rad(90)]]) # [X; Y; vel_x; Yaw]
# Xr, _ = reference_search(path_x, path_y, x, dt, N)
Xr, _ = reference_search(path_x, path_y, pred_x, dt, N)
# Discrete time model of the vehicle lateral dynamics
Ad_mat, Bd_mat, gd_mat = vehicle.get_kinematics_model(x, u)
# ========== Constraints ==========
umin = np.array([-np.deg2rad(15), -3.]) # u : [steer, accel]
umax = np.array([np.deg2rad(15), 1.])
xmin = np.array([-np.inf, -np.inf, -100.,
-np.pi]) # [X; Y; vel_x; Yaw]
xmax = np.array([np.inf, np.inf, 100., np.pi])
# ========== Objective function ==========
# MPC weight matrix
Q = sparse.diags([1.0, 1.0, 5.0, 10.0]) # weight matrix for state
# QN = Q
QN = sparse.diags([10.0, 10.0, 50.0,
50.0]) # weight matrix for terminal state
R = sparse.diags([0.1, 0.1]) # weight matrix for control input
# R_before = 10*sparse.eye(nu) # weight matrix for control input
# Solve MPC
res = mpc(Ad_mat, Bd_mat, gd_mat, x_vec, Xr, Q, QN, R, N, xmin, xmax,
umin, umax)
# Check solver status
if res.info.status != 'solved':
print('OSQP did not solve the problem!')
# raise ValueError('OSQP did not solve the problem!')
plt.pause(5.0)
continue
# Apply first control input to the plant
ctrl = res.x[-N * nu:-(N - 1) * nu]
toc = time.time()
print("ctrl :", ctrl)
# for Plotting predictive states
sol_state = res.x[:-N * nu]
for ii in range((N + 1) * nx):
if ii % 4 == 0:
pred_x[0, ii // 4] = sol_state[ii]
elif ii % 4 == 1:
pred_x[1, ii // 4] = sol_state[ii]
elif ii % 4 == 2:
pred_x[2, ii // 4] = sol_state[ii]
else: # ii % 4 == 3:
pred_x[3, ii // 4] = sol_state[ii]
print("pred_x[:,0] :")
print(pred_x[:, 0])
print("x0")
print(x)
# Plot
print("Current x :", x[0], "y :", x[1], "v :", x[2], "yaw :", x[3])
print("------------------------------------------------------------")
# print("Reference x :", xr[0], "y :", xr[1], "v :", xr[2], "yaw :", xr[3])
print("Reference x :", Xr[0, 0], "y :", Xr[1, 0], "v :", Xr[2, 0],
"yaw :", Xr[3, 0])
print("------------------------------------------------------------")
print("steer :", u[0], "accel :", u[1])
print("Process time :", toc - tic)
plt.cla()
plt.plot(plt_states[0, :i], plt_states[1, :i], "-b",
label="Drived") # plot from 0 to i
plt.grid(True)
plt.axis("equal")
plot_car(x[0], x[1], x[3], steer=u[0]) # plotting w.r.t. rear axle.
plt.plot(pred_x[0, :], pred_x[1, :], "r")
plt.plot(path_x, path_y, label="Path")
plt.plot(Xr[0, :], Xr[1, :], "g")
plt.pause(0.0001)
# Update States
u = np.expand_dims(ctrl, axis=1) # from (N,) to (N,1)
x_next = np.matmul(Ad_mat, x) + np.matmul(Bd_mat, u) + gd_mat
plt_states[:, i] = x.T
plt_actions[:, i] = u.T
x = x_next
x_vec = np.squeeze(x, axis=1) # (N,) shape for QP solver, NOT (N,1).
def main():
# ===== Vehicle Dynamics ===== #
vehicle = vehicle_dynamics.Vehicle_Dynamics(m=M, l_f=L_F, l_r=L_R, width = WIDTH, length = LENGTH, turning_circle=TURNING_CIRCLE,
C_d = C_D, A_f = A_F, C_roll = C_ROLL, dt = DT)
ox, oy = [], []
"""
Initialization
- Initial State
- Goal State
- MPC init
-- Horizon
-- Constraints
-- Objective function Weights Matrix
"""
# ==========================================
# ========== Initial & Goal State ==========
# ==========================================
start = [0.0, 0.0, np.deg2rad(90.0)]
goal = [-2.0, 30.0, np.deg2rad(90.0)]
pos_cars = [(0,15), (5,2), (5, 18)]
# start = [17.0, 20.0, np.deg2rad(-90.0)]
# goal = [5.0, 20.0, np.deg2rad(90.0)]
# goal = [20.0, 42.0, np.deg2rad(0.0)]
#goal = [55.0, 50.0, np.deg2rad(0.0)]
reeds_sheep.plot_arrow(start[0], start[1], start[2], fc='g')
reeds_sheep.plot_arrow(goal[0], goal[1], goal[2])
# Initial state
# States : [x; y; v; yaw]
# Actions : [steer; accel]
x = np.array([[start[0]],
[start[1]],
[5.0],
[start[2]]]) # [X; Y; V; Yaw]
u = np.array([[0*math.pi/180],
[0.01]]) # [steer; accel]
x_vec = np.squeeze(x, axis=1) # (N,) shape for QP solver, NOT (N,1).
nx = x.shape[0]
nu = u.shape[0]
# =============================================
# ========== MPC initialization ===============
# =============================================
# Prediction horizon
N = 100
# Initialize predictive states
pred_x = np.zeros((nx, N+1))
for i in range(N+1):
pred_x[:,i] = x.T
pred_u = np.zeros((nu, N))
for i in range(N):
pred_u[:,i] = u.T
# MPC Constraints
umin = np.array([-np.deg2rad(15), -3.]) # u : [steer, accel]
umax = np.array([ np.deg2rad(15), 2.])
xmin = np.array([-np.inf,-np.inf, -100., -2*np.pi]) # [X; Y; vel_x; Yaw]
xmax = np.array([ np.inf, np.inf, 100., 2*np.pi])
# MPC Objective function
# MPC weight matrix
Q = sparse.diags([1.0, 1.0, 5.0, 5.0]) # weight matrix for state
# QN = Q
QN = sparse.diags([100.0, 100.0, 50.0, 100.0]) # weight matrix for terminal state
R = sparse.diags([0.01, 0.1]) # weight matrix for control input
# R_before = 10*sparse.eye(nu) # weight matrix for control input
# ========================================
# ============== Simulation ==============
# ========================================
# Simulation Setup
sim_time = 1000
plt_tic = np.linspace(0, sim_time, sim_time)
plt_states = np.zeros((nx, sim_time))
plt_actions = np.zeros((nu, sim_time))
for i in range(sim_time):
print("===================== sim_time :", i, "=====================")
tic = time.time()
# ========================================
# =========== Obstacle map ===============
# ========================================
tic = time.time()
ox, oy = obs_map.map_road(ox, oy, pos_cars)
# ox, oy = obs_map.map_maze(ox, oy)
toc = time.time()
print("Process time (Obstacle map):", (toc - tic))
# ================================================
# ======== Motion Planning (Hybrid A*) ===========
# ================================================
tic = time.time()
curr_pos = [x_vec[0], x_vec[1], x_vec[3]] # list for hybrid A*
goal_pos = goal
if i == 0:
path = hybrid_a_star.hybrid_a_star_planning(
curr_pos, goal_pos, ox, oy, XY_GRID_RESOLUTION, YAW_GRID_RESOLUTION)
# if i == 0:
# path_x, path_y = hybrid_a_star.a_star_planning(
# curr_pos, goal_pos, ox, oy, XY_GRID_RESOLUTION, YAW_GRID_RESOLUTION)
toc = time.time()
print("Process time (Hybrid A*):", (toc - tic))
# ========================================
# ========= Constraint Search ============
# ========================================
tic = time.time()
# lb_x, ub_x, lb_y, ub_y = hybrid_a_star.constraint_search(ox, oy, path)
# lb_x, ub_x, lb_y, ub_y = hybrid_a_star.constraint_search_(ox, oy, pred_x)
# lb_x, ub_x, lb_y, ub_y = hybrid_a_star.constraint_search__(ox, oy, path_x, path_y)
toc = time.time()
print("Process time (constraint_search):", (toc - tic))
# ========================================
# ========== Control (MPC) ===============
# ========================================
tic = time.time()
# Discrete time model of the vehicle lateral dynamics
# Reference states
# Xr, _ = mpc_path_tracking.reference_search(path_x, path_y, pred_x, DT, N)
Xr, _ = mpc_path_tracking.reference_search_(path.xlist, path.ylist, path.yawlist, pred_x, DT, N)
# Discrete time model of the vehicle lateral dynamics
Ad_mat, Bd_mat, gd_mat = vehicle.get_kinematics_model(x, u)
# ========== Constraints ==========
umin = np.array([-np.deg2rad(15), -3.]) # u : [steer, accel]
umax = np.array([ np.deg2rad(15), 1.])
xmin = np.array([-np.inf,-np.inf, -100., -2*np.pi]) # [X; Y; vel_x; Yaw]
xmax = np.array([ np.inf, np.inf, 100., 2*np.pi])
# # ========== Objective function ==========
# # MPC weight matrix
# Q = sparse.diags([1.0, 1.0, 5.0, 10.0]) # weight matrix for state
# # QN = Q
# QN = sparse.diags([10.0, 10.0, 50.0, 50.0]) # weight matrix for terminal state
# R = sparse.diags([0.1, 0.1]) # weight matrix for control input
# # R_before = 10*sparse.eye(nu) # weight matrix for control input
# Solve MPC
res = mpc_path_tracking.mpc(Ad_mat, Bd_mat, gd_mat, x_vec, Xr, Q, QN, R, N, xmin, xmax, umin, umax)
# res = mpc_path_tracking.mpc_(Ad_mat, Bd_mat, gd_mat, x_vec, Xr, Q, QN, R, N, lb_x, ub_x, lb_y, ub_y, umin, umax)
print("solution info")
print(res.info.obj_val)
# Check solver status
if res.info.status != 'solved':
print('OSQP did not solve the problem!')
# raise ValueError('OSQP did not solve the problem!')
plt.pause(5.0)
continue
# Apply first control input to the plant
ctrl = res.x[-N*nu:-(N-1)*nu]
toc = time.time()
print("Process time (MPC):", (toc - tic))
# for Plotting predictive states
sol_state = res.x[:-N*nu]
sol_control = res.x[-N*nu:]
for ii in range((N+1)*nx):
if ii % 4 == 0:
pred_x[0,ii//4] = sol_state[ii]
elif ii % 4 == 1:
pred_x[1,ii//4] = sol_state[ii]
elif ii % 4 == 2:
pred_x[2,ii//4] = sol_state[ii]
else: # ii % 4 == 3:
# Normalize angle
temp_yaw = sol_state[ii]
while temp_yaw > np.pi:
temp_yaw -= 2*np.pi
while temp_yaw < -np.pi:
temp_yaw += 2*np.pi
pred_x[3,ii//4] = temp_yaw
for jj in range((N)*nu):
if jj % 2 == 0:
pred_u[0,jj//2] = sol_control[jj]
elif jj % 2 == 1:
pred_u[1,jj//2] = sol_control[jj]
print("pred_u")
print(pred_u[0,:])
# ==================================================
# ========= Simulate Vehicle & Plotting ============
# ==================================================
# Plot
print("Current x :", x[0], "y :", x[1], "v :", x[2], "yaw :", x[3])
print("------------------------------------------------------------")
# print("Reference x :", xr[0], "y :", xr[1], "v :", xr[2], "yaw :", xr[3])
print("Reference x :", Xr[0,0], "y :", Xr[1,0], "v :", Xr[2,0], "yaw :", Xr[3,0])
print("------------------------------------------------------------")
print("steer :", u[0], "accel :", u[1])
plt.cla()
plt.cla()
plt.grid(True)
plt.axis("equal")
plt.plot(ox, oy, ".k") # plotting Obstacle map
plt.plot(plt_states[0,:i], plt_states[1,:i], "-b", label="Drived") # plotting driven path
plot_car(x[0], x[1], x[3], steer=u[0]) # plotting Vehicle w.r.t. rear axle.
plt.plot(pred_x[0,:], pred_x[1,:], "r") # Predictive Trajectory
plt.plot(path.xlist, path.ylist, label="Local_Path") # Path from Hybrid A*
# plt.plot(path_x, path_y, label="Local_Path") # Path from A*
plt.plot(Xr[0,:], Xr[1,:], "g") # Local Path for MPC
plt.pause(0.0001)
# Update States
u = np.expand_dims(ctrl, axis=1) # from (N,) to (N,1)
x_next = np.matmul(Ad_mat, x) + np.matmul(Bd_mat, u) + gd_mat
# Normalize angle
while x_next[3] > np.pi:
x_next[3] -= 2*np.pi
while x_next[3] < -np.pi:
x_next[3] += 2*np.pi
plt_states[:,i] = x.T
plt_actions[:,i] = u.T
x = x_next
x_vec = np.squeeze(x, axis=1) # (N,) shape for QP solver, NOT (N,1).