Tidy up a bit. Add comments.
matssteinweg
parent
281fc19b6d
commit
58a9a112a4
59
MPC.py
59
MPC.py
|
|
@ -2,7 +2,6 @@ import numpy as np
|
|||
import osqp
|
||||
from scipy import sparse
|
||||
import matplotlib.pyplot as plt
|
||||
from time import time
|
||||
|
||||
# Colors
|
||||
PREDICTION = '#BA4A00'
|
||||
|
|
@ -13,16 +12,18 @@ PREDICTION = '#BA4A00'
|
|||
|
||||
|
||||
class MPC:
|
||||
def __init__(self, model, N, Q, R, QN, StateConstraints, InputConstraints):
|
||||
def __init__(self, model, N, Q, R, QN, StateConstraints, InputConstraints,
|
||||
ay_max):
|
||||
"""
|
||||
Constructor for the Model Predictive Controller.
|
||||
:param model: bicycle model object to be controlled
|
||||
:param T: time horizon | int
|
||||
:param N: time horizon | int
|
||||
:param Q: state cost matrix
|
||||
:param R: input cost matrix
|
||||
:param QN: final state cost matrix
|
||||
:param StateConstraints: dictionary of state constraints
|
||||
:param InputConstraints: dictionary of input constraints
|
||||
:param ay_max: maximum allowed lateral acceleration in curves
|
||||
"""
|
||||
|
||||
# Parameters
|
||||
|
|
@ -43,7 +44,7 @@ class MPC:
|
|||
self.input_constraints = InputConstraints
|
||||
|
||||
# Maximum lateral acceleration
|
||||
self.ay_max = 5.0
|
||||
self.ay_max = ay_max
|
||||
|
||||
# Current control and prediction
|
||||
self.current_prediction = None
|
||||
|
|
@ -52,7 +53,7 @@ class MPC:
|
|||
self.infeasibility_counter = 0
|
||||
|
||||
# Current control signals
|
||||
self.current_control = np.ones((self.nu*self.N))
|
||||
self.current_control = np.zeros((self.nu*self.N))
|
||||
|
||||
# Initialize Optimization Problem
|
||||
self.optimizer = osqp.OSQP()
|
||||
|
|
@ -73,7 +74,7 @@ class MPC:
|
|||
B = np.zeros((self.nx * (self.N + 1), self.nu * (self.N)))
|
||||
# Reference vector for state and input variables
|
||||
ur = np.zeros(self.nu*self.N)
|
||||
xr = np.array([0.0, 0.0, 0.0])
|
||||
xr = np.zeros(self.nx*(self.N+1))
|
||||
# Offset for equality constraint (due to B * (u - ur))
|
||||
uq = np.zeros(self.N * self.nx)
|
||||
# Dynamic state constraints
|
||||
|
|
@ -83,7 +84,7 @@ class MPC:
|
|||
umax_dyn = np.kron(np.ones(self.N), umax)
|
||||
# Get curvature predictions from previous control signals
|
||||
kappa_pred = np.tan(np.array(self.current_control[3::] +
|
||||
self.current_control[-1:])) / self.model.l
|
||||
self.current_control[-1:])) / self.model.length
|
||||
|
||||
# Iterate over horizon
|
||||
for n in range(self.N):
|
||||
|
|
@ -113,13 +114,16 @@ class MPC:
|
|||
|
||||
# Compute dynamic constraints on e_y
|
||||
ub, lb, _ = self.model.reference_path.update_path_constraints(
|
||||
self.model.wp_id+1, self.N, 2*self.model.safety_margin[1],
|
||||
self.model.safety_margin[1])
|
||||
self.model.wp_id+1, self.N, 2*self.model.safety_margin,
|
||||
self.model.safety_margin)
|
||||
xmin_dyn[0] = self.model.spatial_state.e_y
|
||||
xmax_dyn[0] = self.model.spatial_state.e_y
|
||||
xmin_dyn[self.nx::self.nx] = lb
|
||||
xmax_dyn[self.nx::self.nx] = ub
|
||||
|
||||
# Set reference for state as center-line of drivable area
|
||||
xr[self.nx::self.nx] = (lb + ub) / 2
|
||||
|
||||
# Get equality matrix
|
||||
Ax = sparse.kron(sparse.eye(self.N + 1),
|
||||
-sparse.eye(self.nx)) + sparse.csc_matrix(A)
|
||||
|
|
@ -146,8 +150,9 @@ class MPC:
|
|||
P = sparse.block_diag([sparse.kron(sparse.eye(self.N), self.Q), self.QN,
|
||||
sparse.kron(sparse.eye(self.N), self.R)], format='csc')
|
||||
q = np.hstack(
|
||||
[np.kron(np.ones(self.N), -self.Q.dot(xr)), -self.QN.dot(xr),
|
||||
-np.tile(np.array([self.R.A[0, 0], self.R.A[1, 1]]), self.N) * ur])
|
||||
[-np.tile(np.diag(self.Q.A), self.N) * xr[:-self.nx],
|
||||
-self.QN.dot(xr[-self.nx:]),
|
||||
-np.tile(np.diag(self.R.A), self.N) * ur])
|
||||
|
||||
# Initialize optimizer
|
||||
self.optimizer = osqp.OSQP()
|
||||
|
|
@ -167,7 +172,9 @@ class MPC:
|
|||
self.model.get_current_waypoint()
|
||||
|
||||
# Update spatial state
|
||||
self.model.spatial_state = self.model.t2s()
|
||||
self.model.spatial_state = self.model.t2s(reference_state=
|
||||
self.model.temporal_state, reference_waypoint=
|
||||
self.model.current_waypoint)
|
||||
|
||||
# Initialize optimization problem
|
||||
self._init_problem()
|
||||
|
|
@ -178,7 +185,8 @@ class MPC:
|
|||
try:
|
||||
# Get control signals
|
||||
control_signals = np.array(dec.x[-self.N*nu:])
|
||||
control_signals[1::2] = np.arctan(control_signals[1::2] * self.model.l)
|
||||
control_signals[1::2] = np.arctan(control_signals[1::2] *
|
||||
self.model.length)
|
||||
v = control_signals[0]
|
||||
delta = control_signals[1]
|
||||
|
||||
|
|
@ -189,7 +197,7 @@ class MPC:
|
|||
x = np.reshape(dec.x[:(self.N+1)*nx], (self.N+1, nx))
|
||||
|
||||
# Update predicted temporal states
|
||||
self.current_prediction = self.update_prediction(delta, x)
|
||||
self.current_prediction = self.update_prediction(x)
|
||||
|
||||
# Get current control signal
|
||||
u = np.array([v, delta])
|
||||
|
|
@ -213,7 +221,7 @@ class MPC:
|
|||
|
||||
return u
|
||||
|
||||
def update_prediction(self, u, spatial_state_prediction):
|
||||
def update_prediction(self, spatial_state_prediction):
|
||||
"""
|
||||
Transform the predicted states to predicted x and y coordinates.
|
||||
Mainly for visualization purposes.
|
||||
|
|
@ -221,23 +229,19 @@ class MPC:
|
|||
:return: lists of predicted x and y coordinates
|
||||
"""
|
||||
|
||||
# containers for x and y coordinates of predicted states
|
||||
# Containers for x and y coordinates of predicted states
|
||||
x_pred, y_pred = [], []
|
||||
|
||||
# get current waypoint ID
|
||||
#print('#########################')
|
||||
|
||||
# Iterate over prediction horizon
|
||||
for n in range(2, self.N):
|
||||
associated_waypoint = self.model.reference_path.get_waypoint(self.model.wp_id+n)
|
||||
# Get associated waypoint
|
||||
associated_waypoint = self.model.reference_path.\
|
||||
get_waypoint(self.model.wp_id+n)
|
||||
# Transform predicted spatial state to temporal state
|
||||
predicted_temporal_state = self.model.s2t(associated_waypoint,
|
||||
spatial_state_prediction[n, :])
|
||||
#print(spatial_state_prediction[n, 2])
|
||||
#print('delta: ', u)
|
||||
#print('e_y: ', spatial_state_prediction[n, 0])
|
||||
#print('e_psi: ', spatial_state_prediction[n, 1])
|
||||
#print('t: ', spatial_state_prediction[n, 2])
|
||||
#print('+++++++++++++++++++++++')
|
||||
|
||||
# Save predicted coordinates in world coordinate frame
|
||||
x_pred.append(predicted_temporal_state.x)
|
||||
y_pred.append(predicted_temporal_state.y)
|
||||
|
||||
|
|
@ -248,6 +252,7 @@ class MPC:
|
|||
Display predicted car trajectory in current axis.
|
||||
"""
|
||||
|
||||
plt.scatter(self.current_prediction[0], self.current_prediction[1],
|
||||
if self.current_prediction is not None:
|
||||
plt.scatter(self.current_prediction[0], self.current_prediction[1],
|
||||
c=PREDICTION, s=30)
|
||||
|
||||
|
|
|
|||
|
|
@ -132,7 +132,7 @@ class LidarModel:
|
|||
if __name__ == '__main__':
|
||||
|
||||
# Create Map
|
||||
map = Map('map_floor2.png')
|
||||
map = Map('real_map.png')
|
||||
plt.imshow(map.data, cmap='gray',
|
||||
extent=[map.origin[0], map.origin[0] +
|
||||
map.width * map.resolution,
|
||||
|
|
|
|||
70
map.py
70
map.py
|
|
@ -3,11 +3,46 @@ import matplotlib.pyplot as plt
|
|||
from skimage.morphology import remove_small_holes
|
||||
from PIL import Image
|
||||
from skimage.draw import line_aa
|
||||
import matplotlib.patches as plt_patches
|
||||
|
||||
# Colors
|
||||
OBSTACLE = '#2E4053'
|
||||
|
||||
|
||||
############
|
||||
# Obstacle #
|
||||
############
|
||||
|
||||
class Obstacle:
|
||||
def __init__(self, cx, cy, radius):
|
||||
"""
|
||||
Constructor for a circular obstacle to be placed on a map.
|
||||
:param cx: x coordinate of center of obstacle in world coordinates
|
||||
:param cy: y coordinate of center of obstacle in world coordinates
|
||||
:param radius: radius of circular obstacle in m
|
||||
"""
|
||||
self.cx = cx
|
||||
self.cy = cy
|
||||
self.radius = radius
|
||||
|
||||
def show(self):
|
||||
"""
|
||||
Display obstacle on current axis.
|
||||
"""
|
||||
|
||||
# Draw circle
|
||||
circle = plt_patches.Circle(xy=(self.cx, self.cy), radius=
|
||||
self.radius, color=OBSTACLE, zorder=20)
|
||||
ax = plt.gca()
|
||||
ax.add_patch(circle)
|
||||
|
||||
|
||||
#######
|
||||
# Map #
|
||||
#######
|
||||
|
||||
class Map:
|
||||
def __init__(self, file_path, threshold_occupied=100,
|
||||
origin=(-30.0, -24.0), resolution=0.06):
|
||||
def __init__(self, file_path, origin, resolution, threshold_occupied=100):
|
||||
"""
|
||||
Constructor for map object. Map contains occupancy grid map data of
|
||||
environment as well as meta information.
|
||||
|
|
@ -65,6 +100,19 @@ class Map:
|
|||
|
||||
return x, y
|
||||
|
||||
def process_map(self):
|
||||
"""
|
||||
Process raw map image. Binarization and removal of small holes in map.
|
||||
"""
|
||||
|
||||
# Binarization using specified threshold
|
||||
# 1 corresponds to free, 0 to occupied
|
||||
self.data = np.where(self.data >= self.threshold_occupied, 1, 0)
|
||||
|
||||
# Remove small holes in map corresponding to spurious measurements
|
||||
self.data = remove_small_holes(self.data, area_threshold=5,
|
||||
connectivity=8).astype(np.int8)
|
||||
|
||||
def add_obstacles(self, obstacles):
|
||||
"""
|
||||
Add obstacles to the map.
|
||||
|
|
@ -106,21 +154,9 @@ class Map:
|
|||
for x, y in zip(path_x, path_y):
|
||||
self.data[y, x] = 0
|
||||
|
||||
def process_map(self):
|
||||
"""
|
||||
Process raw map image. Binarization and removal of small holes in map.
|
||||
"""
|
||||
|
||||
# Binarization using specified threshold
|
||||
# 1 corresponds to free, 0 to occupied
|
||||
self.data = np.where(self.data >= self.threshold_occupied, 1, 0)
|
||||
|
||||
# Remove small holes in map corresponding to spurious measurements
|
||||
self.data = remove_small_holes(self.data, area_threshold=5,
|
||||
connectivity=8).astype(np.int8)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
map = Map('map_floor2.png')
|
||||
plt.imshow(map.data, cmap='gray')
|
||||
map = Map('real_map.png')
|
||||
# map = Map('sim_map.png')
|
||||
plt.imshow(np.flipud(map.data), cmap='gray')
|
||||
plt.show()
|
||||
|
|
|
|||
|
|
@ -1,17 +1,17 @@
|
|||
import numpy as np
|
||||
import math
|
||||
from map import Map
|
||||
from map import Map, Obstacle
|
||||
from skimage.draw import line_aa
|
||||
import matplotlib.pyplot as plt
|
||||
import matplotlib.patches as plt_patches
|
||||
from scipy import sparse
|
||||
import osqp
|
||||
|
||||
# Colors
|
||||
DRIVABLE_AREA = '#BDC3C7'
|
||||
WAYPOINTS = '#D0D3D4'
|
||||
OBSTACLE = '#2E4053'
|
||||
PATH_CONSTRAINTS = '#F5B041'
|
||||
OBSTACLE = '#2E4053'
|
||||
|
||||
|
||||
############
|
||||
# Waypoint #
|
||||
|
|
@ -21,7 +21,9 @@ class Waypoint:
|
|||
def __init__(self, x, y, psi, kappa):
|
||||
"""
|
||||
Waypoint object containing x, y location in global coordinate system,
|
||||
orientation of waypoint psi and local curvature kappa
|
||||
orientation of waypoint psi and local curvature kappa. Waypoint further
|
||||
contains an associated reference velocity computed by the speed profile
|
||||
and a path width specified by upper and lower bounds.
|
||||
:param x: x position in global coordinate system | [m]
|
||||
:param y: y position in global coordinate system | [m]
|
||||
:param psi: orientation of waypoint | [rad]
|
||||
|
|
@ -37,7 +39,9 @@ class Waypoint:
|
|||
|
||||
# Information about drivable area at waypoint
|
||||
# upper and lower bound of drivable area orthogonal to
|
||||
# waypoint orientation
|
||||
# waypoint orientation.
|
||||
# Upper bound: free drivable area to the left of center-line in m
|
||||
# Lower bound: free drivable area to the right of center-line in m
|
||||
self.lb = None
|
||||
self.ub = None
|
||||
self.static_border_cells = None
|
||||
|
|
@ -53,35 +57,6 @@ class Waypoint:
|
|||
return ((self.x - other.x)**2 + (self.y - other.y)**2)**0.5
|
||||
|
||||
|
||||
############
|
||||
# Obstacle #
|
||||
############
|
||||
|
||||
class Obstacle:
|
||||
def __init__(self, cx, cy, radius):
|
||||
"""
|
||||
Constructor for a circular obstacle to be place on a path.
|
||||
:param cx: x coordinate of center of obstacle in world coordinates
|
||||
:param cy: y coordinate of center of obstacle in world coordinates
|
||||
:param radius: radius of circular obstacle in m
|
||||
"""
|
||||
|
||||
self.cx = cx
|
||||
self.cy = cy
|
||||
self.radius = radius
|
||||
|
||||
def show(self):
|
||||
"""
|
||||
Display obstacle.
|
||||
"""
|
||||
|
||||
# Draw circle
|
||||
circle = plt_patches.Circle(xy=(self.cx, self.cy), radius=
|
||||
self.radius, color=OBSTACLE, zorder=20)
|
||||
ax = plt.gca()
|
||||
ax.add_patch(circle)
|
||||
|
||||
|
||||
##################
|
||||
# Reference Path #
|
||||
##################
|
||||
|
|
@ -155,8 +130,6 @@ class ReferencePath:
|
|||
wp_y = [wp for segment in wp_y for wp in segment] + [gp_y]
|
||||
|
||||
# Smooth path
|
||||
#wp_xs = wp_x[:self.smoothing_distance]
|
||||
#wp_ys = wp_y[:self.smoothing_distance]
|
||||
wp_xs = []
|
||||
wp_ys = []
|
||||
for wp_id in range(self.smoothing_distance, len(wp_x) -
|
||||
|
|
@ -165,8 +138,6 @@ class ReferencePath:
|
|||
+ self.smoothing_distance + 1]))
|
||||
wp_ys.append(np.mean(wp_y[wp_id - self.smoothing_distance:wp_id
|
||||
+ self.smoothing_distance + 1]))
|
||||
#wp_xs += wp_x[-self.smoothing_distance:]
|
||||
#wp_ys += wp_y[-self.smoothing_distance:]
|
||||
|
||||
# Construct list of waypoint objects
|
||||
waypoints = list(zip(wp_xs, wp_ys))
|
||||
|
|
@ -434,7 +405,6 @@ class ReferencePath:
|
|||
wp_lb_y = np.array([wp.static_border_cells[1][1] for wp in self.waypoints])
|
||||
|
||||
# Plot waypoints
|
||||
|
||||
# colors = [wp.v_ref for wp in self.waypoints]
|
||||
plt.scatter(wp_x, wp_y, c=WAYPOINTS, s=10)
|
||||
|
||||
|
|
@ -680,22 +650,28 @@ class ReferencePath:
|
|||
|
||||
if __name__ == '__main__':
|
||||
|
||||
# Select Path | 'Race' or 'Q'
|
||||
path = 'Q'
|
||||
# Select Track | 'Real_Track' or 'Sim_Track'
|
||||
path = 'Sim_Track'
|
||||
|
||||
if path == 'Sim_Track':
|
||||
|
||||
# Load map file
|
||||
map = Map(file_path='sim_map.png', origin=[-1, -2], resolution=0.005)
|
||||
|
||||
# Create Map
|
||||
if path == '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
|
||||
|
||||
# Create reference path
|
||||
reference_path = ReferencePath(map, wp_x, wp_y, path_resolution,
|
||||
smoothing_distance=5, max_width=0.15,
|
||||
circular=True)
|
||||
|
||||
# Add obstacles
|
||||
obs1 = Obstacle(cx=0.0, cy=0.0, radius=0.05)
|
||||
obs2 = Obstacle(cx=-0.8, cy=-0.5, radius=0.08)
|
||||
|
|
@ -708,19 +684,26 @@ if __name__ == '__main__':
|
|||
reference_path.map.add_obstacles([obs1, obs2, obs3, obs4, obs5, obs6, obs7,
|
||||
obs8])
|
||||
|
||||
elif path == '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]
|
||||
elif path == 'Real_Track':
|
||||
|
||||
# Load map file
|
||||
map = Map(file_path='real_map.png', origin=(-30.0, -24.0),
|
||||
resolution=0.06)
|
||||
|
||||
# Specify waypoints
|
||||
wp_x = [-1.62, -6.04, -6.6, -5.36, -2.0, 5.9,
|
||||
11.9, 7.3, 0.0, -1.62]
|
||||
wp_y = [3.24, -1.4, -3.0, -5.36, -6.65, 3.5,
|
||||
10.9, 14.5, 5.2, 3.24]
|
||||
|
||||
# Specify path resolution
|
||||
path_resolution = 0.2 # m / wp
|
||||
|
||||
# Create reference path
|
||||
reference_path = ReferencePath(map, wp_x, wp_y, path_resolution,
|
||||
smoothing_distance=5, max_width=2.0,
|
||||
circular=True)
|
||||
|
||||
# Add obstacles and bounds to map
|
||||
cone1 = Obstacle(-5.9, -2.9, 0.2)
|
||||
cone2 = Obstacle(-2.3, -5.9, 0.2)
|
||||
|
|
@ -731,16 +714,11 @@ if __name__ == '__main__':
|
|||
table3 = Obstacle(4.30, 3.22, 0.2)
|
||||
obstacle_list = [cone1, cone2, cone3, cone4, table1, table2, table3]
|
||||
map.add_obstacles(obstacle_list)
|
||||
# bound1 = ((2.25, -2.5), (1.55, 1.0))
|
||||
# bound2 = ((1.56, 1.25), (3.64, 0.75))
|
||||
# bound3 = ((4.46, 3.06), (7.51, 6.9))
|
||||
# bound4 = ((4.18, 3.03), (1.95, 3.26))
|
||||
# bound5 = ((-3.26, -0.21), (7.29, 13.16))
|
||||
|
||||
bound1 = ((-0.02, -2.72), (1.5, 1.0))
|
||||
bound2 = ((4.43, 3.07), (1.5, 1.0))
|
||||
bound3 = ((4.43, 3.07), (7.5, 6.93))
|
||||
bound4 = ((7.28, 13.37), (-3.32, -0.12))
|
||||
|
||||
boundary_list = [bound1, bound2, bound3, bound4]
|
||||
map.add_boundary(boundary_list)
|
||||
|
||||
|
|
|
|||
133
simulation.py
133
simulation.py
|
|
@ -1,6 +1,6 @@
|
|||
from map import Map
|
||||
from map import Map, Obstacle
|
||||
import numpy as np
|
||||
from reference_path import ReferencePath, Obstacle
|
||||
from reference_path import ReferencePath
|
||||
from spatial_bicycle_models import BicycleModel
|
||||
import matplotlib.pyplot as plt
|
||||
from MPC import MPC
|
||||
|
|
@ -9,98 +9,120 @@ from scipy import sparse
|
|||
|
||||
if __name__ == '__main__':
|
||||
|
||||
# Select Simulation Mode | 'Race' or 'Q'
|
||||
sim_mode = 'Race'
|
||||
# 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)
|
||||
|
||||
# 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
|
||||
|
||||
# Create smoothed reference path
|
||||
reference_path = ReferencePath(map, wp_x, wp_y, path_resolution,
|
||||
smoothing_distance=5, max_width=0.23,
|
||||
circular=True)
|
||||
# Add 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])
|
||||
elif sim_mode == 'Q':
|
||||
map = Map(file_path='map_floor2.png')
|
||||
# Add obstacles
|
||||
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]
|
||||
|
||||
# Specify path resolution
|
||||
path_resolution = 0.20 # m / wp
|
||||
|
||||
# Create smoothed reference path
|
||||
reference_path = ReferencePath(map, wp_x, wp_y, path_resolution,
|
||||
smoothing_distance=5, max_width=1.50,
|
||||
circular=False)
|
||||
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])
|
||||
|
||||
# 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!')
|
||||
map, wp_x, wp_y, path_resolution, reference_path \
|
||||
= None, None, None, None, None
|
||||
map, wp_x, wp_y, path_resolution, reference_path, car \
|
||||
= None, None, None, None, None, None
|
||||
exit(1)
|
||||
|
||||
################
|
||||
# Motion Model #
|
||||
################
|
||||
|
||||
# Initial state
|
||||
e_y_0 = 0.0
|
||||
e_psi_0 = 0.0
|
||||
t_0 = 0.0
|
||||
V_MAX = 1.0
|
||||
|
||||
car = BicycleModel(length=0.12, width=0.06, reference_path=reference_path,
|
||||
e_y=e_y_0, e_psi=e_psi_0, t=t_0)
|
||||
|
||||
##############
|
||||
# Controller #
|
||||
##############
|
||||
|
||||
N = 30
|
||||
Q = sparse.diags([0.3, 0.0, 0.0])
|
||||
Q = sparse.diags([1.0, 0.0, 0.0])
|
||||
R = sparse.diags([0.5, 0.0])
|
||||
QN = sparse.diags([0.3, 0.0, 0.0])
|
||||
InputConstraints = {'umin': np.array([0.0, -np.tan(0.66)/car.l]),
|
||||
'umax': np.array([V_MAX, np.tan(0.66)/car.l])}
|
||||
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])}
|
||||
mpc = MPC(car, N, Q, R, QN, StateConstraints, InputConstraints)
|
||||
mpc = MPC(car, N, Q, R, QN, StateConstraints, InputConstraints, ay_max)
|
||||
|
||||
# Compute speed profile
|
||||
SpeedProfileConstraints = {'a_min': -0.1, 'a_max': 0.5,
|
||||
'v_min': 0, 'v_max': V_MAX, 'ay_max': 4.0}
|
||||
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)
|
||||
|
||||
##############
|
||||
# Simulation #
|
||||
##############
|
||||
|
||||
# Sampling time
|
||||
Ts = 0.05
|
||||
t = 0
|
||||
car.set_sampling_time(Ts)
|
||||
# Set simulation time to zero
|
||||
t = 0.0
|
||||
|
||||
# Logging containers
|
||||
x_log = [car.temporal_state.x]
|
||||
|
|
@ -122,7 +144,7 @@ if __name__ == '__main__':
|
|||
v_log.append(u[0])
|
||||
|
||||
# Increase simulation time
|
||||
t += Ts
|
||||
t += car.Ts
|
||||
|
||||
# Plot path and drivable area
|
||||
reference_path.show()
|
||||
|
|
@ -131,8 +153,7 @@ if __name__ == '__main__':
|
|||
car.show()
|
||||
|
||||
# Plot MPC prediction
|
||||
if mpc.current_prediction is not None:
|
||||
mpc.show_prediction()
|
||||
mpc.show_prediction()
|
||||
|
||||
# Set figure title
|
||||
plt.title('MPC Simulation: v(t): {:.2f}, delta(t): {:.2f}, Duration: '
|
||||
|
|
|
|||
Loading…
Reference in New Issue