Tidy up a bit. Add comments.

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.wp_id+1, self.N, 2*self.model.safety_margin,
-            self.model.safety_margin[1])
+            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.diag(self.Q.A), self.N) * xr[:-self.nx],
-             -np.tile(np.array([self.R.A[0, 0], self.R.A[1, 1]]), self.N) * ur])
+             -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
+        # Iterate over prediction horizon
-        #print('#########################')
-
         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.
         """
 
+        if self.current_prediction is not None:
             plt.scatter(self.current_prediction[0], self.current_prediction[1],
                     c=PREDICTION, s=30)
 

lidar_model.py

@@ -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,

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,
+    def __init__(self, file_path, origin, resolution, threshold_occupied=100):
-                 origin=(-30.0, -24.0), resolution=0.06):
         """
         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')
+    map = Map('real_map.png')
-    plt.imshow(map.data, cmap='gray')
+    # map = Map('sim_map.png')
+    plt.imshow(np.flipud(map.data), cmap='gray')
     plt.show()

reference_path.py

@@ -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'
+    # Select Track | 'Real_Track' or 'Sim_Track'
-    path = 'Q'
+    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':
+    elif path == 'Real_Track':
-        map = Map(file_path='map_floor2.png')
+
-        # wp_x = [-9.169, 11.9, 7.3, -6.95]
+        # Load map file
-        # wp_y = [-15.678, 10.9, 14.5, -3.31]
+        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)
 

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,24 +9,33 @@ from scipy import sparse
 
 if __name__ == '__main__':
 
-    # Select Simulation Mode | 'Race' or 'Q'
+    # Select Simulation Mode | 'Sim_Track' or 'Real_Track'
-    sim_mode = 'Race'
+    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
+        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)
@@ -36,71 +45,84 @@ if __name__ == '__main__':
             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')
+        # 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)
+
+        # 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 \
+        map, wp_x, wp_y, path_resolution, reference_path, car \
-            = None, None, None, None, None
+            = 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])
+    QN = sparse.diags([1.0, 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])}
+    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,
+    a_min = -0.1  # m/s^2
-                               'v_min': 0, 'v_max': V_MAX, 'ay_max': 4.0}
+    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
+    # Set simulation time to zero
-    Ts = 0.05
+    t = 0.0
-    t = 0
-    car.set_sampling_time(Ts)
 
     # 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,7 +153,6 @@ if __name__ == '__main__':
         car.show()
 
         # Plot MPC prediction
-        if mpc.current_prediction is not None:
         mpc.show_prediction()
 
         # Set figure title