107 lines
3.0 KiB
Python
107 lines
3.0 KiB
Python
from map import Map
|
|
import numpy as np
|
|
from reference_path import ReferencePath
|
|
from spatial_bicycle_models import BicycleModel
|
|
import matplotlib.pyplot as plt
|
|
from MPC import MPC, MPC_OSQP
|
|
from scipy import sparse
|
|
import time
|
|
|
|
|
|
if __name__ == '__main__':
|
|
|
|
# Select Simulation Mode | 'Race' or 'Q'
|
|
sim_mode = 'Race'
|
|
|
|
# Create Map
|
|
if sim_mode == 'Race':
|
|
map = Map(file_path='map_race.png', origin=[-1, -2], resolution=0.005)
|
|
# Specify waypoints
|
|
wp_x = [-0.75, -0.25, -0.25, 0.25, 0.25, 1.25, 1.25, 0.75, 0.75, 1.25,
|
|
1.25, -0.75, -0.75, -0.25]
|
|
wp_y = [-1.5, -1.5, -0.5, -0.5, -1.5, -1.5, -1, -1, -0.5, -0.5, 0, 0,
|
|
-1.5, -1.5]
|
|
# Specify path resolution
|
|
path_resolution = 0.05 # m / wp
|
|
elif sim_mode == 'Q':
|
|
map = Map(file_path='map_floor2.png')
|
|
wp_x = [-9.169, 11.9, 7.3, -6.95]
|
|
wp_y = [-15.678, 10.9, 14.5, -3.31]
|
|
# Specify path resolution
|
|
path_resolution = 0.20 # m / wp
|
|
else:
|
|
print('Invalid Simulation Mode!')
|
|
map, wp_x, wp_y, path_resolution = None, None, None, None
|
|
exit(1)
|
|
|
|
# Create smoothed reference path
|
|
reference_path = ReferencePath(map, wp_x, wp_y, path_resolution,
|
|
smoothing_distance=5)
|
|
|
|
################
|
|
# Motion Model #
|
|
################
|
|
|
|
# Initial state
|
|
e_y_0 = 0.0
|
|
e_psi_0 = 0.0
|
|
t_0 = 0.0
|
|
v = 1.0
|
|
|
|
car = BicycleModel(reference_path=reference_path,
|
|
e_y=e_y_0, e_psi=e_psi_0, t=t_0)
|
|
|
|
##############
|
|
# Controller #
|
|
##############
|
|
|
|
N = 20
|
|
Q = sparse.diags([0.01, 0.0, 0.4])
|
|
R = sparse.diags([0.01])
|
|
QN = Q
|
|
InputConstraints = {'umin': np.array([-np.tan(0.44)/car.l]), 'umax': np.array([np.tan(0.44)/car.l])}
|
|
StateConstraints = {'xmin': np.array([-0.2, -np.inf, 0]), 'xmax': np.array([0.2, np.inf, np.inf])}
|
|
mpc = MPC_OSQP(car, N, Q, R, QN, StateConstraints, InputConstraints)
|
|
|
|
##############
|
|
# Simulation #
|
|
##############
|
|
|
|
# logging containers
|
|
x_log = [car.temporal_state.x]
|
|
y_log = [car.temporal_state.y]
|
|
|
|
# iterate over waypoints
|
|
for wp_id in range(len(car.reference_path.waypoints)-mpc.N-1):
|
|
|
|
# get control signals
|
|
start = time.time()
|
|
delta = mpc.get_control(v)
|
|
end = time.time()
|
|
u = np.array([v, delta])
|
|
|
|
# drive car
|
|
car.drive(u)
|
|
|
|
# log
|
|
x_log.append(car.temporal_state.x)
|
|
y_log.append(car.temporal_state.y)
|
|
|
|
###################
|
|
# Plot Simulation #
|
|
###################
|
|
# plot path
|
|
car.reference_path.show()
|
|
|
|
# plot car trajectory and velocity
|
|
plt.scatter(x_log[:-1], y_log[:-1], c='g', s=15)
|
|
|
|
plt.scatter(mpc.current_prediction[0], mpc.current_prediction[1], c='b', s=5)
|
|
|
|
plt.title('MPC Simulation: Position: {:.2f} m, {:.2f} m'.
|
|
format(car.temporal_state.x, car.temporal_state.y))
|
|
plt.xticks([])
|
|
plt.yticks([])
|
|
plt.pause(0.00000001)
|
|
plt.close()
|