Multi-Purpose-MPC/simulation.py

163 lines
5.3 KiB
Python
Raw Normal View History

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
# 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
# Specify path resolution
path_resolution = 0.05 # m / wp
2020-01-03 00:10:14 +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
# 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
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
# Specify path resolution
path_resolution = 0.20 # m / wp
2020-01-03 00:10:14 +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)
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
exit(1)
2019-11-24 05:27:57 +08:00
##############
# Controller #
##############
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])}
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
# 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}
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]
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)
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
# 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
# Set figure title
2019-12-07 04:32:36 +08:00
plt.title('MPC Simulation: v(t): {:.2f}, delta(t): {:.2f}, Duration: '
'{:.2f} s'.format(u[0], u[1], t))
2019-12-12 15:34:14 +08:00
plt.axis('off')
plt.pause(0.001)