2020-01-03 00:10:14 +08:00
|
|
|
from map import Map, Obstacle
|
2019-11-24 05:27:57 +08:00
|
|
|
import numpy as np
|
2020-01-03 00:10:14 +08:00
|
|
|
from reference_path import ReferencePath
|
2019-11-29 16:36:30 +08:00
|
|
|
from spatial_bicycle_models import BicycleModel
|
2019-11-24 05:27:57 +08:00
|
|
|
import matplotlib.pyplot as plt
|
2019-12-02 16:48:34 +08:00
|
|
|
from MPC import MPC
|
2019-11-29 16:36:30 +08:00
|
|
|
from scipy import sparse
|
|
|
|
|
2019-11-24 05:27:57 +08:00
|
|
|
|
|
|
|
if __name__ == '__main__':
|
|
|
|
|
2020-01-03 00:10:14 +08:00
|
|
|
# Select Simulation Mode | 'Sim_Track' or 'Real_Track'
|
|
|
|
sim_mode = 'Sim_Track'
|
|
|
|
|
|
|
|
# Simulation Environment. Mini-Car on track specifically designed to show-
|
|
|
|
# case time-optimal driving.
|
|
|
|
if sim_mode == 'Sim_Track':
|
|
|
|
|
|
|
|
# Load map file
|
|
|
|
map = Map(file_path='sim_map.png', origin=[-1, -2], resolution=0.005)
|
2019-11-24 05:27:57 +08:00
|
|
|
|
2019-11-24 05:56:09 +08:00
|
|
|
# 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]
|
2020-01-03 00:10:14 +08:00
|
|
|
|
2019-11-24 05:56:09 +08:00
|
|
|
# Specify path resolution
|
|
|
|
path_resolution = 0.05 # m / wp
|
2020-01-03 00:10:14 +08:00
|
|
|
|
2019-12-01 23:04:18 +08:00
|
|
|
# Create smoothed reference path
|
|
|
|
reference_path = ReferencePath(map, wp_x, wp_y, path_resolution,
|
2019-12-05 06:29:37 +08:00
|
|
|
smoothing_distance=5, max_width=0.23,
|
2019-12-07 04:32:36 +08:00
|
|
|
circular=True)
|
2020-01-03 00:10:14 +08:00
|
|
|
|
2019-12-08 21:57:35 +08:00
|
|
|
# Add obstacles
|
2020-01-03 00:10:14 +08:00
|
|
|
use_obstacles = False
|
|
|
|
if use_obstacles:
|
|
|
|
obs1 = Obstacle(cx=0.0, cy=0.0, radius=0.05)
|
|
|
|
obs2 = Obstacle(cx=-0.8, cy=-0.5, radius=0.08)
|
|
|
|
obs3 = Obstacle(cx=-0.7, cy=-1.5, radius=0.05)
|
|
|
|
obs4 = Obstacle(cx=-0.3, cy=-1.0, radius=0.08)
|
|
|
|
obs5 = Obstacle(cx=0.27, cy=-1.0, radius=0.05)
|
|
|
|
obs6 = Obstacle(cx=0.78, cy=-1.47, radius=0.05)
|
|
|
|
obs7 = Obstacle(cx=0.73, cy=-0.9, radius=0.07)
|
|
|
|
obs8 = Obstacle(cx=1.2, cy=0.0, radius=0.08)
|
|
|
|
obs9 = Obstacle(cx=0.67, cy=-0.05, radius=0.06)
|
|
|
|
map.add_obstacles([obs1, obs2, obs3, obs4, obs5, obs6, obs7,
|
|
|
|
obs8, obs9])
|
|
|
|
|
|
|
|
# Instantiate motion model
|
|
|
|
car = BicycleModel(length=0.12, width=0.06,
|
|
|
|
reference_path=reference_path, Ts=0.05)
|
|
|
|
|
|
|
|
# Real-World Environment. Track used for testing the algorithm on a 1:12
|
|
|
|
# RC car.
|
|
|
|
elif sim_mode == 'Real_Track':
|
|
|
|
|
|
|
|
# Load map file
|
|
|
|
map = Map(file_path='real_map.png', origin=(-30.0, -24.0),
|
|
|
|
resolution=0.06)
|
|
|
|
|
|
|
|
# Specify waypoints
|
2019-11-24 05:56:09 +08:00
|
|
|
wp_x = [-9.169, 11.9, 7.3, -6.95]
|
|
|
|
wp_y = [-15.678, 10.9, 14.5, -3.31]
|
2020-01-03 00:10:14 +08:00
|
|
|
|
2019-11-24 05:56:09 +08:00
|
|
|
# Specify path resolution
|
|
|
|
path_resolution = 0.20 # m / wp
|
2020-01-03 00:10:14 +08:00
|
|
|
|
2019-12-01 23:04:18 +08:00
|
|
|
# Create smoothed reference path
|
|
|
|
reference_path = ReferencePath(map, wp_x, wp_y, path_resolution,
|
2019-12-02 16:48:34 +08:00
|
|
|
smoothing_distance=5, max_width=1.50,
|
2019-12-07 04:32:36 +08:00
|
|
|
circular=False)
|
2020-01-03 00:10:14 +08:00
|
|
|
|
|
|
|
# Add obstacles
|
|
|
|
add_obstacles = False
|
|
|
|
if add_obstacles:
|
|
|
|
obs1 = Obstacle(cx=-6.3, cy=-11.1, radius=0.20)
|
|
|
|
obs2 = Obstacle(cx=-2.2, cy=-6.8, radius=0.25)
|
|
|
|
obs4 = Obstacle(cx=2.0, cy=-0.2, radius=0.25)
|
|
|
|
obs8 = Obstacle(cx=6.0, cy=5.0, radius=0.3)
|
|
|
|
obs9 = Obstacle(cx=7.42, cy=4.97, radius=0.3)
|
|
|
|
map.add_obstacles([obs1, obs2, obs4, obs8, obs9])
|
|
|
|
|
|
|
|
# Instantiate motion model
|
|
|
|
car = BicycleModel(length=0.30, width=0.20,
|
|
|
|
reference_path=reference_path, Ts=0.05)
|
|
|
|
|
2019-11-24 05:56:09 +08:00
|
|
|
else:
|
|
|
|
print('Invalid Simulation Mode!')
|
2020-01-03 00:10:14 +08:00
|
|
|
map, wp_x, wp_y, path_resolution, reference_path, car \
|
|
|
|
= None, None, None, None, None, None
|
2019-11-24 05:56:09 +08:00
|
|
|
exit(1)
|
|
|
|
|
2019-11-24 05:27:57 +08:00
|
|
|
##############
|
|
|
|
# Controller #
|
|
|
|
##############
|
|
|
|
|
2019-12-01 23:04:18 +08:00
|
|
|
N = 30
|
2020-01-03 00:10:14 +08:00
|
|
|
Q = sparse.diags([1.0, 0.0, 0.0])
|
2019-12-12 15:34:14 +08:00
|
|
|
R = sparse.diags([0.5, 0.0])
|
2020-01-03 00:10:14 +08:00
|
|
|
QN = sparse.diags([1.0, 0.0, 0.0])
|
|
|
|
|
|
|
|
v_max = 1.0 # m/s
|
|
|
|
delta_max = 0.66 # rad
|
|
|
|
ay_max = 4.0 # m/s^2
|
|
|
|
InputConstraints = {'umin': np.array([0.0, -np.tan(delta_max)/car.length]),
|
|
|
|
'umax': np.array([v_max, np.tan(delta_max)/car.length])}
|
2019-12-01 23:04:18 +08:00
|
|
|
StateConstraints = {'xmin': np.array([-np.inf, -np.inf, -np.inf]),
|
|
|
|
'xmax': np.array([np.inf, np.inf, np.inf])}
|
2020-01-03 00:10:14 +08:00
|
|
|
mpc = MPC(car, N, Q, R, QN, StateConstraints, InputConstraints, ay_max)
|
2019-11-24 05:27:57 +08:00
|
|
|
|
2019-12-07 07:29:55 +08:00
|
|
|
# Compute speed profile
|
2020-01-03 00:10:14 +08:00
|
|
|
a_min = -0.1 # m/s^2
|
|
|
|
a_max = 0.5 # m/s^2
|
|
|
|
SpeedProfileConstraints = {'a_min': a_min, 'a_max': a_max,
|
|
|
|
'v_min': 0.0, 'v_max': v_max, 'ay_max': ay_max}
|
2019-12-07 07:29:55 +08:00
|
|
|
car.reference_path.compute_speed_profile(SpeedProfileConstraints)
|
2019-12-05 06:29:37 +08:00
|
|
|
|
2019-11-24 05:27:57 +08:00
|
|
|
##############
|
|
|
|
# Simulation #
|
|
|
|
##############
|
|
|
|
|
2020-01-03 00:10:14 +08:00
|
|
|
# Set simulation time to zero
|
|
|
|
t = 0.0
|
2019-12-02 16:48:34 +08:00
|
|
|
|
|
|
|
# Logging containers
|
2019-11-24 05:27:57 +08:00
|
|
|
x_log = [car.temporal_state.x]
|
|
|
|
y_log = [car.temporal_state.y]
|
2019-12-07 07:29:55 +08:00
|
|
|
v_log = [0.0]
|
2019-11-24 05:27:57 +08:00
|
|
|
|
2019-12-02 16:48:34 +08:00
|
|
|
# Until arrival at end of path
|
|
|
|
while car.s < reference_path.length:
|
2019-11-24 05:27:57 +08:00
|
|
|
|
|
|
|
# get control signals
|
2019-12-07 04:32:36 +08:00
|
|
|
u = mpc.get_control()
|
2019-11-24 05:27:57 +08:00
|
|
|
|
|
|
|
# drive car
|
2019-11-29 16:36:30 +08:00
|
|
|
car.drive(u)
|
2019-11-24 05:27:57 +08:00
|
|
|
|
2019-11-29 16:36:30 +08:00
|
|
|
# log
|
2019-11-24 05:27:57 +08:00
|
|
|
x_log.append(car.temporal_state.x)
|
|
|
|
y_log.append(car.temporal_state.y)
|
2019-12-07 07:29:55 +08:00
|
|
|
v_log.append(u[0])
|
2019-11-24 05:27:57 +08:00
|
|
|
|
2019-12-12 15:34:14 +08:00
|
|
|
# Increase simulation time
|
2020-01-03 00:10:14 +08:00
|
|
|
t += car.Ts
|
2019-11-24 05:27:57 +08:00
|
|
|
|
2019-12-01 23:04:18 +08:00
|
|
|
# Plot path and drivable area
|
|
|
|
reference_path.show()
|
|
|
|
|
|
|
|
# Plot car
|
|
|
|
car.show()
|
2019-11-24 05:27:57 +08:00
|
|
|
|
2019-12-12 15:34:14 +08:00
|
|
|
# Plot MPC prediction
|
2020-01-03 00:10:14 +08:00
|
|
|
mpc.show_prediction()
|
2019-12-02 16:48:34 +08:00
|
|
|
|
2019-12-07 07:29:55 +08:00
|
|
|
# Set figure title
|
2019-12-07 04:32:36 +08:00
|
|
|
plt.title('MPC Simulation: v(t): {:.2f}, delta(t): {:.2f}, Duration: '
|
2019-12-07 07:29:55 +08:00
|
|
|
'{:.2f} s'.format(u[0], u[1], t))
|
2019-12-12 15:34:14 +08:00
|
|
|
plt.axis('off')
|
|
|
|
plt.pause(0.001)
|