def mpcPlot(self):
dy = Dynamics.VehicleDynamics()
ref = dy.reference_trajectory(
Numpy2Torch(self.state_history[:, -1],
self.state_history[:, -1].shape))
self.state_r_history = self.state_r_history.reshape([-1, 5])
plt.figure(1)
plt.plot(self.state_history[:, -1],
self.state_history[:, 0],
label="trajectory")
# plt.plot(state_r_history[:,-1],state_r_history[:,0], label="$trajectory_r$")
# plt.plot(self.state_history[:,-1],self.config.a_curve * np.sin(self.config.k_curve*self.state_history[:,-1]), label="reference")
plt.plot(self.state_history[:, -1], ref[:, 0], label="reference")
plt.legend(loc="upper right")
plt.figure(2)
plt.plot(self.state_history[:, -1],
self.state_history[:, 2],
label="trajectory")
# plt.plot(state_r_history[:,-1],state_r_history[:,0], label="$trajectory_r$")
# plt.plot(self.state_history[:,-1],self.config.a_curve * np.sin(self.config.k_curve*self.state_history[:,-1]), label="reference")
plt.plot(self.state_history[:, -1], ref[:, 2], label="reference")
plt.legend(loc="upper right")
plt.figure(3)
plt.plot(self.state_history[0:-1, -1], self.control_history)
plt.show()
def mpcSolution(self):
# initialize state model
statemodel_plt = Dynamics.VehicleDynamics()
state = self.initial_state
# state = torch.tensor([[0.0, 0.0, self.psi_init, 0.0, 0.0]])
state.requires_grad_(False)
x_ref = statemodel_plt.reference_trajectory(state[:, -1])
state_r = state.detach().clone() # relative state
state_r[:, 0:4] = state_r[:, 0:4] - x_ref
self.state_history = state.detach().numpy()
plot_length = self.config.SIMULATION_STEPS
self.control_history = []
self.state_r_history = state_r
cal_time = 0
plt.figure(0)
for i in range(plot_length):
x = state_r.tolist()[0]
time_start = time.time()
temp, control = self.solver.mpcSolver(x, self.config.NP)
plt.plot(temp[:, -1], temp[:, 0])
cal_time += time.time() - time_start
u = Numpy2Torch(control[0], (-1, self.config.ACTION_DIM))
state, state_r = step_relative(statemodel_plt, state, u)
self.state_history = np.append(self.state_history,
state.detach().numpy(),
axis=0)
self.control_history = np.append(self.control_history,
u.detach().numpy())
self.state_r_history = np.append(self.state_history,
state_r.detach().numpy())
print("MPC calculating time: {:.3f}".format(cal_time) + "s")
self.mpcSaveTraj()
# control_history = np.empty([0, 1])
# time_init = time.time()
# for i in range(config.NP_TOTAL):
# state, control = solver.mpc_solver(x, 60)
# x = state[1]
# u = control[0]
# state_history = np.append(state_history, x)
# control_history = np.append(control_history, u)
# x = x.tolist()
# print("steps:{:3d}".format(i) + " | state: " + str(x))
# print("MPC calculating time: {:.3f}".format(time.time() - time_init) + "s")
# state_history = state_history.reshape(-1,config.DYNAMICS_DIM)
# # np.savetxt(os.path.join(log_dir, 'structured_MPC_state.txt'), state_history)
# # np.savetxt(os.path.join(log_dir, 'structured_MPC_control.txt'), control_history)
statemodel_plt = Dynamics.VehicleDynamics()
if config.tire_model == 'Fiala':
state = torch.tensor([[1.0, 0.0, config.psi_init, 0.0, config.u, 0.0]])
else:
state = torch.tensor([[0.0, 0.0, config.psi_init, 0.0, 0.0]])
state.requires_grad_(False)
x_ref = statemodel_plt.reference_trajectory(state[:, -1])
state_r = state.detach().clone()
state_r[:, 0:4] = state_r[:, 0:4] - x_ref
state_history = state.detach().numpy()
x = np.array([0.])
plot_length = 300
control_history = []
state_r_history = state_r
cal_time = 0
plt.figure()
def simulation(methods, log_dir, simu_dir):
policy = Actor(S_DIM, A_DIM)
value = Critic(S_DIM, A_DIM)
config = DynamicsConfig()
solver = Solver()
load_dir = log_dir
policy.load_parameters(load_dir)
value.load_parameters(load_dir)
statemodel_plt = Dynamics.VehicleDynamics()
plot_length = config.SIMULATION_STEPS
# initial_state = torch.tensor([[0.5, 0.0, config.psi_init, 0.0, 0.0]])
# baseline = Baseline(initial_state, simu_dir)
# baseline.mpcSolution()
# baseline.openLoopSolution()
# Open-loop reference
x_init = [0.0, 0.0, config.psi_init, 0.0, 0.0]
op_state, op_control = solver.openLoopMpcSolver(x_init, config.NP_TOTAL)
np.savetxt(os.path.join(simu_dir, 'Open_loop_control.txt'), op_control)
for method in methods:
cal_time = 0
state = torch.tensor([[0.0, 0.0, config.psi_init, 0.0, 0.0]])
# state = torch.tensor([[0.0, 0.0, 0.0, 0.0, 0.0]])
state.requires_grad_(False)
# x_ref = statemodel_plt.reference_trajectory(state[:, -1])
x_ref = statemodel_plt.reference_trajectory(state[:, -1])
state_r = state.detach().clone()
state_r[:, 0:4] = state_r[:, 0:4] - x_ref
state_history = state.detach().numpy()
control_history = []
print('\nCALCULATION TIME:')
for i in range(plot_length):
if method == 'ADP':
time_start = time.time()
u = policy.forward(state_r[:, 0:4])
cal_time += time.time() - time_start
elif method == 'MPC':
x = state_r.tolist()[0]
time_start = time.time()
_, control = solver.mpcSolver(x, config.NP) # todo:retreve
cal_time += time.time() - time_start
u = np.array(control[0],
dtype='float32').reshape(-1, config.ACTION_DIM)
u = torch.from_numpy(u)
else:
u = np.array(op_control[i],
dtype='float32').reshape(-1, config.ACTION_DIM)
u = torch.from_numpy(u)
state, state_r = step_relative(statemodel_plt, state, u)
# state_next, deri_state, utility, F_y1, F_y2, alpha_1, alpha_2 = statemodel_plt.step(state, u)
# state_r_old, _, _, _, _, _, _ = statemodel_plt.step(state_r, u)
# state_r = state_r_old.detach().clone()
# state_r[:, [0, 2]] = state_next[:, [0, 2]]
# x_ref = statemodel_plt.reference_trajectory(state_next[:, -1])
# state_r[:, 0:4] = state_r[:, 0:4] - x_ref
# state = state_next.clone().detach()
# s = state_next.detach().numpy()
state_history = np.append(state_history,
state.detach().numpy(),
axis=0)
control_history = np.append(control_history, u.detach().numpy())
if method == 'ADP':
print(" ADP: {:.3f}".format(cal_time) + "s")
np.savetxt(os.path.join(simu_dir, 'ADP_state.txt'), state_history)
np.savetxt(os.path.join(simu_dir, 'ADP_control.txt'),
control_history)
elif method == 'MPC':
print(" MPC: {:.3f}".format(cal_time) + "s")
np.savetxt(os.path.join(simu_dir, 'structured_MPC_state.txt'),
state_history)
np.savetxt(os.path.join(simu_dir, 'structured_MPC_control.txt'),
control_history)
else:
np.savetxt(os.path.join(simu_dir, 'Open_loop_state.txt'),
state_history)
adp_simulation_plot(simu_dir)
plot_comparison(simu_dir, methods)
LR_V = 3e-3 # learning rate of value net
# tasks
TRAIN_FLAG = 1
LOAD_PARA_FLAG = 0
SIMULATION_FLAG = 1
# Set random seed
np.random.seed(0)
torch.manual_seed(0)
# initialize policy and value net, model of vehicle dynamics
config = GeneralConfig()
policy = Actor(config.STATE_DIM, config.ACTION_DIM, lr=LR_P)
value = Critic(config.STATE_DIM, 1, lr=LR_V)
vehicleDynamics = Dynamics.VehicleDynamics()
state_batch = vehicleDynamics.initialize_state()
writer = SummaryWriter()
# Training
iteration_index = 0
if LOAD_PARA_FLAG == 1:
print(
"********************************* LOAD PARAMETERS *********************************"
)
# load pre-trained parameters
load_dir = "./Results_dir/2020-10-09-14-42-10000"
policy.load_parameters(load_dir)
value.load_parameters(load_dir)
if TRAIN_FLAG == 1:
def plot_comparison(simu_dir, methods):
'''
Plot comparison figure among ADP, MPC & open-loop solution.
Trajectory, tracking error and control signal plot
Parameters
----------
picture_dir: string
location of figure saved.
'''
num_methods = len(methods)
picture_dir = simu_dir + "/Figures"
config = DynamicsConfig()
trajectory_data = []
dy = Dynamics.VehicleDynamics()
if os.path.exists(os.path.join(simu_dir, 'structured_MPC_state.txt')) == 0:
print('No comparison state data!')
else:
if 'MPC' in methods:
mpc_state = np.loadtxt(
os.path.join(simu_dir, 'structured_MPC_state.txt'))
mpc_trajectory = (mpc_state[:, 4], mpc_state[:, 0])
trajectory_data.append(mpc_trajectory)
if 'ADP' in methods:
adp_state = np.loadtxt(os.path.join(simu_dir, 'ADP_state.txt'))
adp_trajectory = (adp_state[:, 4], adp_state[:, 0])
trajectory_data.append(adp_trajectory)
if 'OP' in methods:
open_loop_state = np.loadtxt(
os.path.join(simu_dir, 'Open_loop_state.txt'))
open_loop_trajectory = (open_loop_state[:, 4], open_loop_state[:,
0])
trajectory_data.append(open_loop_trajectory)
myplot(trajectory_data,
num_methods,
"xy",
fname=os.path.join(picture_dir, 'trajectory.png'),
xlabel="longitudinal position [m]",
ylabel="Lateral position [m]",
legend=methods,
legend_loc="lower left")
heading_angle = []
if 'MPC' in methods:
mpc_state = np.loadtxt(
os.path.join(simu_dir, 'structured_MPC_state.txt'))
mpc_trajectory = (mpc_state[:, 4], 180 / np.pi * mpc_state[:, 2])
heading_angle.append(mpc_trajectory)
if 'ADP' in methods:
adp_state = np.loadtxt(os.path.join(simu_dir, 'ADP_state.txt'))
adp_trajectory = (adp_state[:, 4], 180 / np.pi * adp_state[:, 2])
heading_angle.append(adp_trajectory)
if 'OP' in methods:
open_loop_state = np.loadtxt(
os.path.join(simu_dir, 'Open_loop_state.txt'))
open_loop_trajectory = (open_loop_state[:, 4], open_loop_state[:,
2])
heading_angle.append(open_loop_trajectory)
myplot(heading_angle,
num_methods,
"xy",
fname=os.path.join(picture_dir, 'trajectory_heading_angle.png'),
xlabel="longitudinal position [m]",
ylabel="Heading angle [degree]",
legend=methods,
legend_loc="lower left")
error_data = []
print('\nMAX TRACKING ERROR:')
print(' MAX LATERAL POSITION ERROR:')
if 'MPC' in methods:
mpc_ref = dy.reference_trajectory(
Numpy2Torch(mpc_state[:, 4], mpc_state[:, 4].shape)).numpy()
mpc_error = (mpc_state[:, 4], mpc_state[:, 0] - mpc_ref[:, 0])
print(' Max MPC lateral error: {:0.3E}m'.format(
float(max(abs(mpc_state[:, 0] - mpc_ref[:, 0])))))
error_data.append(mpc_error)
if 'ADP' in methods:
adp_ref = dy.reference_trajectory(
Numpy2Torch(adp_state[:, 4], adp_state[:, 4].shape)).numpy()
adp_error = (adp_state[:, 4], adp_state[:, 0] - adp_ref[:, 0])
print(' Max ADP lateral error: {:0.3E}m'.format(
float(max(abs(adp_state[:, 0] - adp_ref[:, 0])))))
error_data.append(adp_error)
if 'OP' in methods:
open_loop_error = (open_loop_state[:, 4],
open_loop_state[:, 0] - config.a_curve *
np.sin(config.k_curve * open_loop_state[:, 4]))
error_data.append(open_loop_error)
myplot(error_data,
num_methods,
"xy",
fname=os.path.join(picture_dir, 'trajectory_error.png'),
xlabel="longitudinal position error [m]",
ylabel="Lateral position [m]",
legend=methods,
legend_loc="upper left")
# mpc_error = (mpc_state[:, 4], mpc_state[:, 0] - config.a_curve * np.sin(config.k_curve * mpc_state[:, 4]))
# open_loop_error = (open_loop_state[:, 4], open_loop_state[:, 0] - config.a_curve * np.sin(config.k_curve * open_loop_state[:, 4]))
# adp_error = (adp_state[:, 4], 1 * (adp_state[:, 0] - config.a_curve * np.sin(config.k_curve * adp_state[:, 4]))) # TODO: safe delete
#error_data = [mpc_error, adp_error, open_loop_error]
# myplot(error_data, 3, "xy",
# fname=os.path.join(picture_dir,'trajectory_error.png'),
# xlabel="longitudinal position [m]",
# ylabel="Lateral position error [m]",
# legend=["MPC", "ADP", "Open-loop"],
# legend_loc="lower left"
# )
psi_error_data = []
print(' MAX YAW ANGLE ERROR:')
if 'MPC' in methods:
mpc_psi_error = (mpc_state[:, 4],
180 / np.pi * (mpc_state[:, 2] - mpc_ref[:, 2]))
print(' MPC: {:0.3E} degree'.format(
float(max(abs(180 / np.pi *
(mpc_state[:, 2] - mpc_ref[:, 2]))))))
psi_error_data.append(mpc_psi_error)
if 'ADP' in methods:
adp_psi_error = (adp_state[:, 4],
180 / np.pi * (adp_state[:, 2] - adp_ref[:, 2]))
print(' ADP: {:0.3E} degree'.format(
float(max(abs(180 / np.pi *
(adp_state[:, 2] - adp_ref[:, 2]))))))
psi_error_data.append(adp_psi_error)
if 'OP' in methods:
open_loop_psi_error = (
open_loop_state[:, 4], 180 / np.pi *
(open_loop_state[:, 2] -
np.arctan(config.a_curve * config.k_curve *
np.cos(config.k_curve * open_loop_state[:, 4]))))
psi_error_data.append(open_loop_psi_error)
myplot(psi_error_data,
num_methods,
"xy",
fname=os.path.join(picture_dir, 'head_angle_error.png'),
xlabel="longitudinal position [m]",
ylabel="head angle error [degree]",
legend=["MPC", "ADP", "Open-loop"],
legend_loc="lower left")
control_plot_data = []
if 'MPC' in methods:
mpc_control = np.loadtxt(
os.path.join(simu_dir, 'structured_MPC_control.txt'))
mpc_control_tuple = (mpc_state[1:, 4], 180 / np.pi * mpc_control)
control_plot_data.append(mpc_control_tuple)
if 'ADP' in methods:
adp_control = np.loadtxt(os.path.join(simu_dir, 'ADP_control.txt'))
adp_control_tuple = (adp_state[1:, 4], 180 / np.pi * adp_control)
control_plot_data.append(adp_control_tuple)
if 'OP' in methods:
open_loop_control = np.loadtxt(
os.path.join(simu_dir, 'Open_loop_control.txt'))
open_loop_control_tuple = (open_loop_state[1:, 4],
180 / np.pi * open_loop_control)
control_plot_data.append(open_loop_control_tuple)
myplot(control_plot_data,
3,
"xy",
fname=os.path.join(picture_dir, 'control.png'),
xlabel="longitudinal position [m]",
ylabel="steering angle [degree]",
legend=["MPC", "ADP", "Open-loop"],
legend_loc="upper left")
if 'OP' in methods:
y_avs_error = []
for [i, d] in enumerate(error_data):
y_avs_error.append(np.mean(np.abs(d[1])))
print("Tracking error of lateral position:")
print("MPC:{:.3e} | ".format(y_avs_error[0]) +
"ADP:{:.3e} | ".format(y_avs_error[1]) +
"Open-loop:{:.3e} | ".format(y_avs_error[2]))
psi_avs_error = []
for [i, d] in enumerate(psi_error_data):
psi_avs_error.append(np.mean(np.abs(d[1])))
print("Tracking error of heading angle:")
print("MPC:{:.3e} | ".format(psi_avs_error[0]) +
"ADP:{:.3e} | ".format(psi_avs_error[1]) +
"Open-loop:{:.3e} | ".format(psi_avs_error[2]))
mpc_control_error = mpc_control - open_loop_control
adp_control_error = adp_control - open_loop_control
print("Control error:")
print("MPC:{:.3e} | ".format(np.mean(np.abs(mpc_control_error))) +
"ADP:{:.3e} | ".format(np.mean(np.abs(adp_control_error))))
