Multi-Purpose-MPC/simulation.py

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()