def environment_setup(map_file, use_obstacles=True):
# Load map file
map = Map(file_path=map_file, 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
if use_obstacles:
obs1 = Obstacle(cx=0.0, cy=0.05, radius=0.05)
obs2 = Obstacle(cx=-0.85, cy=-0.5, radius=0.08)
obs3 = Obstacle(cx=-0.75, cy=-1.5, radius=0.05)
obs4 = Obstacle(cx=-0.35, cy=-1.0, radius=0.08)
obs5 = Obstacle(cx=0.35, 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])
return reference_path
max_width=0.23,
circular=True)
# Add obstacles
use_obstacles = True
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='maps/real_map.png',
origin=(-30.0, -24.0),
resolution=0.06)
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)
cone3 = Obstacle(10.9, 10.7, 0.2)
cone4 = Obstacle(7.4, 13.5, 0.2)
table1 = Obstacle(-0.30, -1.75, 0.2)
table2 = Obstacle(1.55, 1.00, 0.2)
table3 = Obstacle(4.30, 3.22, 0.2)
obstacle_list = [cone1, cone2, cone3, cone4, table1, table2, table3]
map.add_obstacles(obstacle_list)
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)
else:
reference_path = None
print('Invalid path!')
exit(1)
ub, lb, border_cells = reference_path.update_path_constraints(
0, reference_path.n_waypoints, 0.1, 0.01)
def setupMPC():
global returnUDPMessage
global mpc
global car
global x_log
global y_log
global v_log
global t
global reference_path
global map
print("go")
# 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='maps/sim_map5.png', origin=[-1, -2],
resolution=0.005) #picture is 35x35m
#500x500px
"""
Constructor for map object. Map contains occupancy grid map data of
environment as well as meta information.
:param file_path: path to image of map
:param threshold_occupied: threshold value for binarization of map
image
:param origin: x and y coordinates of map origin in world coordinates
[m]
:param resolution: resolution in m/px
"""
# 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
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])
#map.remove_obstacle(obs4)
"""
Simplified Spatial Bicycle Model. Spatial Reformulation of Kinematic
Bicycle Model. Uses Simplified Spatial State.
:param reference_path: reference path model is supposed to follow
:param length: length of the car in m
:param width: with of the car in m
:param Ts: sampling time of model in s
"""
# Instantiate motion model
car = BicycleModel(length=0.12, width=0.06, #12cm. 6cm
reference_path=reference_path, Ts=0.008333333333)
else:
print('Invalid Simulation Mode!')
map, wp_x, wp_y, path_resolution, reference_path, car \
= None, None, None, None, None, None
exit(1)
##############
# Controller #
##############
"""
Constructor for the Model Predictive Controller.
:param model: bicycle model object to be controlled
: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
"""
N = 30
Q = sparse.diags([1.0, 0.0, 0.0])
R = sparse.diags([0.5, 0.0])
QN = sparse.diags([1.0, 0.0, 0.0])
v_max = 1 # m/s
delta_max = .66 # rad
ay_max = 4000 # m/s^2 #max laterial accel (how far it's actually capable of going)
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, ay_max)
# Compute speed profile
a_min = -0.1 # m/s^2
a_max = 0.5 # m/s^2 ##################what inputs the car can actually accept
"""
Compute a speed profile for the path. Assign a reference velocity
to each waypoint based on its curvature.
:param Constraints: constraints on acceleration and velocity
curvature of the path
"""
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 #
##############
# Set simulation time to zero
t = 0.0
# Logging containers
x_log = [car.temporal_state.x]
y_log = [car.temporal_state.y]
v_log = [0.0]
returnUDPMessage="DONE SETUP"
print(returnUDPMessage)
