velocity as mpc input

MPC.py

@@ -13,7 +13,8 @@ PREDICTION = '#BA4A00'
 
 
 class MPC:
-    def __init__(self, model, N, Q, R, QN, StateConstraints, InputConstraints):
+    def __init__(self, model, N, Q, R, QN, StateConstraints, InputConstraints,
+                 velocity_reference):
         """
         Constructor for the Model Predictive Controller.
         :param model: bicycle model object to be controlled
@@ -23,6 +24,7 @@ class MPC:
         :param QN: final state cost matrix
         :param StateConstraints: dictionary of state constraints
         :param InputConstraints: dictionary of input constraints
+        :param velocity_reference: reference value for velocity
         """
 
         # Parameters
@@ -34,10 +36,17 @@ class MPC:
         # Model
         self.model = model
 
+        # Dimensions
+        self.nx = self.model.n_states
+        self.nu = 2
+
         # Constraints
         self.state_constraints = StateConstraints
         self.input_constraints = InputConstraints
 
+        # Velocity reference
+        self.v_ref = velocity_reference
+
         # Current control and prediction
         self.current_prediction = None
 
@@ -45,20 +54,16 @@ class MPC:
         self.infeasibility_counter = 0
 
         # Current control signals
-        self.current_control = None
+        self.current_control = np.ones((self.nu*self.N)) * velocity_reference
 
         # Initialize Optimization Problem
         self.optimizer = osqp.OSQP()
 
-    def _init_problem(self, v):
+    def _init_problem(self):
         """
         Initialize optimization problem for current time step.
         """
 
-        # Number of state and input variables
-        nx = self.model.n_states
-        nu = 1
-
         # Constraints
         umin = self.input_constraints['umin']
         umax = self.input_constraints['umax']
@@ -66,14 +71,13 @@ class MPC:
         xmax = self.state_constraints['xmax']
 
         # LTV System Matrices
-        A = np.zeros((nx * (self.N + 1), nx * (self.N + 1)))
+        A = np.zeros((self.nx * (self.N + 1), self.nx * (self.N + 1)))
-        B = np.zeros((nx * (self.N + 1), nu * (self.N)))
+        B = np.zeros((self.nx * (self.N + 1), self.nu * (self.N)))
         # Reference vector for state and input variables
-        ur = np.zeros(self.N)
+        ur = np.zeros(self.nu*self.N)
-        xr = np.array([0.0, 0.0, -1.0])
+        xr = np.array([0.0, 0.0, 0.0])
         # Offset for equality constraint (due to B * (u - ur))
-        uq = np.zeros(self.N * nx)
+        uq = np.zeros(self.N * self.nx)
-
         # Dynamic state constraints
         xmin_dyn = np.kron(np.ones(self.N + 1), xmin)
         xmax_dyn = np.kron(np.ones(self.N + 1), xmax)
@@ -82,34 +86,34 @@ class MPC:
         for n in range(self.N):
 
             # Get information about current waypoint
-            current_waypoint = self.model.reference_path.waypoints[
+            current_waypoint = self.model.reference_path.get_waypoint(self.model.wp_id + n)
-                self.model.wp_id + n]
+            next_waypoint = self.model.reference_path.get_waypoint(self.model.wp_id + n + 1)
-            next_waypoint = self.model.reference_path.waypoints[
-                self.model.wp_id + n + 1]
             delta_s = next_waypoint - current_waypoint
-            kappa_r = current_waypoint.kappa
+            kappa_ref = current_waypoint.kappa
 
             # Compute LTV matrices
-            f, A_lin, B_lin = self.model.linearize(v, kappa_r, delta_s)
+            f, A_lin, B_lin = self.model.linearize(self.v_ref, kappa_ref, delta_s)
-            A[nx + n * nx:n * nx + 2 * nx, n * nx:n * nx + nx] = A_lin
+            A[(n+1) * self.nx: (n+2)*self.nx, n * self.nx:(n+1)*self.nx] = A_lin
-            B[nx + n * nx:n * nx + 2 * nx, n * nu:n * nu + nu] = B_lin
+            B[(n+1) * self.nx: (n+2)*self.nx, n * self.nu:(n+1)*self.nu] = B_lin
 
-            # Set kappa_r to reference for input signal
+            # Set reference for input signal
-            ur[n] = kappa_r
+            ur[n*self.nu:(n+1)*self.nu] = np.array([self.v_ref, kappa_ref])
             # Compute equality constraint offset (B*ur)
-            uq[n * nx:n * nx + nx] = B_lin[:, 0] * kappa_r - f
+            uq[n * self.nx:(n+1)*self.nx] = B_lin.dot(np.array
+                                            ([self.v_ref, kappa_ref])) - f
+            # Compute dynamic constraints on e_y
             lb, ub = self.model.reference_path.update_bounds(
                 self.model.wp_id + n, self.model.safety_margin[1])
-            xmin_dyn[nx * n] = lb
+            xmin_dyn[self.nx * n] = lb
-            xmax_dyn[nx * n] = ub
+            xmax_dyn[self.nx * n] = ub
 
         # Get equality matrix
         Ax = sparse.kron(sparse.eye(self.N + 1),
-                         -sparse.eye(nx)) + sparse.csc_matrix(A)
+                         -sparse.eye(self.nx)) + sparse.csc_matrix(A)
         Bu = sparse.csc_matrix(B)
         Aeq = sparse.hstack([Ax, Bu])
         # Get inequality matrix
-        Aineq = sparse.eye((self.N + 1) * nx + self.N * nu)
+        Aineq = sparse.eye((self.N + 1) * self.nx + self.N * self.nu)
         # Combine constraint matrices
         A = sparse.vstack([Aeq, Aineq], format='csc')
 
@@ -131,13 +135,13 @@ class MPC:
              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),
-             -self.R.A[0, 0] * ur])
+             -np.tile(np.array([self.R.A[0, 0], self.R.A[1, 1]]), self.N) * ur])
 
         # Initialize optimizer
         self.optimizer = osqp.OSQP()
         self.optimizer.setup(P=P, q=q, A=A, l=l, u=u, verbose=False)
 
-    def get_control(self, v):
+    def get_control(self):
         """
         Get control signal given the current position of the car. Solves a
         finite time optimization problem based on the linearized car model.
@@ -145,6 +149,7 @@ class MPC:
 
         # Number of state variables
         nx = self.model.n_states
+        nu = 2
 
         # Update current waypoint
         self.model.get_current_waypoint()
@@ -153,21 +158,24 @@ class MPC:
         self.model.spatial_state = self.model.t2s()
 
         # Initialize optimization problem
-        self._init_problem(v)
+        self._init_problem()
 
         # Solve optimization problem
         dec = self.optimizer.solve()
 
         try:
             # Get control signals
-            deltas = np.arctan(dec.x[-self.N:] * self.model.l)
+            control_signals = np.array(dec.x[-self.N*nu:])
-            delta = deltas[0]
+            control_signals[1::2] = np.arctan(control_signals[1::2] * self.model.l)
+            v = control_signals[0]
+            delta = control_signals[1]
 
             # Update control signals
-            self.current_control = deltas
+            self.current_control = control_signals
 
             # Get predicted spatial states
             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)
 
@@ -181,8 +189,8 @@ class MPC:
 
             print('Infeasible problem. Previously predicted'
                   ' control signal used!')
-            u = np.array([v, self.current_control
+            id = nu * (self.infeasibility_counter + 1)
-            [self.infeasibility_counter+1]])
+            u = np.array(self.current_control[id:id+2])
 
             # increase infeasibility counter
             self.infeasibility_counter += 1
@@ -208,10 +216,10 @@ class MPC:
         #print('#########################')
 
         for n in range(2, self.N):
-            associated_waypoint = self.model.reference_path.waypoints[self.model.wp_id+n]
+            associated_waypoint = self.model.reference_path.get_waypoint(self.model.wp_id+n)
             predicted_temporal_state = self.model.s2t(associated_waypoint,
                                             spatial_state_prediction[n, :])
-            print(spatial_state_prediction[n, 2])
+            #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])

reference_path.py

@@ -84,7 +84,7 @@ class Obstacle:
 
 class ReferencePath:
     def __init__(self, map, wp_x, wp_y, resolution, smoothing_distance,
-                 max_width, n_extension, circular):
+                 max_width, circular):
         """
         Reference Path object. Create a reference trajectory from specified
         corner points with given resolution. Smoothing around corners can be
@@ -97,8 +97,6 @@ class ReferencePath:
         :param smoothing_distance: number of waypoints used for smoothing the
         path by averaging neighborhood of waypoints
         :param max_width: maximum width of path to both sides in m
-        :param n_extension: number of samples used for path extension to allow
-        for MPC
         :param circular: True if path circular
         """
 
@@ -114,15 +112,15 @@ class ReferencePath:
         # Look ahead distance for path averaging
         self.smoothing_distance = smoothing_distance
 
-        # Number of waypoints used to extend path at the end
-        self.n_extension = n_extension
-
         # Circular
         self.circular = circular
 
         # List of waypoint objects
         self.waypoints = self._construct_path(wp_x, wp_y)
 
+        # Number of waypoints
+        self.n_waypoints = len(self.waypoints)
+
         # Length of path
         self.length, self.segment_lengths = self._compute_length()
 
@@ -154,14 +152,6 @@ class ReferencePath:
                     tolist() for i in range(len(wp_y) - 1)]
         wp_y = [wp for segment in wp_y for wp in segment] + [gp_y]
 
-        if self.n_extension is not None:
-            if self.circular:
-                wp_x += wp_x[:self.n_extension]
-                wp_y += wp_y[:self.n_extension]
-            else:
-                wp_x += wp_x[-self.n_extension:]
-                wp_y += wp_y[-self.n_extension:]
-
         # Smooth path
         wp_xs = []
         wp_ys = []
@@ -232,7 +222,7 @@ class ReferencePath:
         :return: length of center-line path in m
         """
         segment_lengths = [0.0] + [self.waypoints[wp_id+1] - self.waypoints
-                    [wp_id] for wp_id in range(len(self.waypoints)-self.n_extension-1)]
+                    [wp_id] for wp_id in range(len(self.waypoints)-1)]
         s = sum(segment_lengths)
         return s, segment_lengths
 
@@ -327,7 +317,7 @@ class ReferencePath:
         """
 
         # Get reference waypoint
-        wp = self.waypoints[wp_id]
+        wp = self.get_waypoint(wp_id)
 
         # Get waypoint's border cells in map coordinates
         ub_p = self.map.w2m(wp.border_cells[0][0], wp.border_cells[0][1])
@@ -394,11 +384,6 @@ class ReferencePath:
         ub_ls = wp.x + ub * np.cos(angle_ub), wp.y + ub * np.sin(angle_ub)
         lb_ls = wp.x - lb * np.cos(angle_lb), wp.y - lb * np.sin(angle_lb)
 
-        # Update member variables of waypoint
-        #wp.ub = ub
-        #wp.lb = lb
-        #wp.border_cells = (ub_ls, lb_ls)
-
         return lb, ub
 
     def add_obstacles(self, obstacles):
@@ -444,14 +429,14 @@ class ReferencePath:
                    vmax=1.0)
 
         # Get x and y coordinates for all waypoints
-        wp_x = np.array([wp.x for wp in self.waypoints][:-self.n_extension])
+        wp_x = np.array([wp.x for wp in self.waypoints])
-        wp_y = np.array([wp.y for wp in self.waypoints][:-self.n_extension])
+        wp_y = np.array([wp.y for wp in self.waypoints])
 
         # Get x and y locations of border cells for upper and lower bound
-        wp_ub_x = np.array([wp.border_cells[0][0] for wp in self.waypoints][:-self.n_extension])
+        wp_ub_x = np.array([wp.border_cells[0][0] for wp in self.waypoints])
-        wp_ub_y = np.array([wp.border_cells[0][1] for wp in self.waypoints][:-self.n_extension])
+        wp_ub_y = np.array([wp.border_cells[0][1] for wp in self.waypoints])
-        wp_lb_x = np.array([wp.border_cells[1][0] for wp in self.waypoints][:-self.n_extension])
+        wp_lb_x = np.array([wp.border_cells[1][0] for wp in self.waypoints])
-        wp_lb_y = np.array([wp.border_cells[1][1] for wp in self.waypoints][:-self.n_extension])
+        wp_lb_y = np.array([wp.border_cells[1][1] for wp in self.waypoints])
 
         # Plot waypoints
         plt.scatter(wp_x, wp_y, color=WAYPOINTS, s=3)
@@ -467,16 +452,16 @@ class ReferencePath:
 
         # Plot border of path
         bl_x = np.array([wp.border_cells[0][0] for wp in
-                         self.waypoints][:-self.n_extension] +
+                         self.waypoints] +
                         [self.waypoints[0].border_cells[0][0]])
         bl_y = np.array([wp.border_cells[0][1] for wp in
-                         self.waypoints][:-self.n_extension] +
+                         self.waypoints] +
                         [self.waypoints[0].border_cells[0][1]])
         br_x = np.array([wp.border_cells[1][0] for wp in
-                         self.waypoints][:-self.n_extension] +
+                         self.waypoints] +
                         [self.waypoints[0].border_cells[1][0]])
         br_y = np.array([wp.border_cells[1][1] for wp in
-                         self.waypoints][:-self.n_extension] +
+                         self.waypoints] +
                         [self.waypoints[0].border_cells[1][1]])
         # Smooth border
         # bl_x = savgol_filter(bl_x, 15, 9)
@@ -499,11 +484,22 @@ class ReferencePath:
         for obstacle in self.obstacles:
             obstacle.show()
 
+    def get_waypoint(self, wp_id):
+        if wp_id >= self.n_waypoints and self.circular:
+            wp_id = np.mod(wp_id, self.n_waypoints)
+        elif wp_id >= self.n_waypoints and not self.circular:
+            print('Reached end of path!')
+            exit(1)
+
+        return self.waypoints[wp_id]
+
+
+
 
 if __name__ == '__main__':
 
     # Select Path | 'Race' or 'Q'
-    path = 'Race'
+    path = 'Q'
 
     # Create Map
     if path == 'Race':

simulation.py

@@ -27,7 +27,7 @@ if __name__ == '__main__':
         # Create smoothed reference path
         reference_path = ReferencePath(map, wp_x, wp_y, path_resolution,
                                        smoothing_distance=5, max_width=0.23,
-                                       n_extension=50, circular=True)
+                                       circular=True)
     elif sim_mode == 'Q':
         map = Map(file_path='map_floor2.png')
         wp_x = [-9.169, 11.9, 7.3, -6.95]
@@ -37,7 +37,7 @@ if __name__ == '__main__':
         # Create smoothed reference path
         reference_path = ReferencePath(map, wp_x, wp_y, path_resolution,
                                        smoothing_distance=5, max_width=1.50,
-                                       n_extension=50, circular=False)
+                                       circular=False)
     else:
         print('Invalid Simulation Mode!')
         map, wp_x, wp_y, path_resolution, reference_path \
@@ -65,7 +65,6 @@ if __name__ == '__main__':
     e_y_0 = 0.0
     e_psi_0 = 0.0
     t_0 = 0.0
-    v = 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)
@@ -76,13 +75,15 @@ if __name__ == '__main__':
 
     N = 30
     Q = sparse.diags([1.0, 0.0, 0.0])
-    R = sparse.diags([0.01])
+    R = sparse.diags([1.0, 0.0])
-    QN = sparse.diags([0.0, 0.0, 1.0])
+    QN = sparse.diags([0.0, 0.0, 0.0])
-    InputConstraints = {'umin': np.array([-np.tan(0.66)/car.l]),
+    InputConstraints = {'umin': np.array([0.0, -np.tan(0.66)/car.l]),
-                        'umax': np.array([np.tan(0.66)/car.l])}
+                        'umax': np.array([2.5, np.tan(0.66)/car.l])}
     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)
+    velocity_reference = 1.5  # m/s
+    mpc = MPC(car, N, Q, R, QN, StateConstraints, InputConstraints,
+              velocity_reference)
 
     #########
     # LiDAR #
@@ -107,17 +108,11 @@ if __name__ == '__main__':
     while car.s < reference_path.length:
 
         # get control signals
-        start = time()
+        u = mpc.get_control()
-        u = mpc.get_control(v)
-        end = time()
-        print('Control time: ', end-start)
 
         # drive car
         car.drive(u)
 
-        # scan
-        scan = sensor.scan(car.temporal_state, map)
-
         # log
         x_log.append(car.temporal_state.x)
         y_log.append(car.temporal_state.y)
@@ -129,9 +124,6 @@ if __name__ == '__main__':
         # Plot path and drivable area
         reference_path.show()
 
-        # Plot scan
-        sensor.plot_scan(car.temporal_state)
-
         # Plot MPC prediction
         mpc.show_prediction()
 
@@ -140,11 +132,10 @@ if __name__ == '__main__':
 
         t += Ts
 
-        plt.title('MPC Simulation: Distance: {:.2f}m/{:.2f} m, Duration: '
+        plt.title('MPC Simulation: v(t): {:.2f}, delta(t): {:.2f}, Duration: '
                   '{:.2f} s'.
-                  format(car.s, car.reference_path.length, t))
+                  format(u[0], u[1], t))
-        if t == Ts:
+
-            plt.show()
+        plt.pause(0.01)
-        plt.pause(0.0001)
     print('Final Time: {:.2f}'.format(t))
     plt.close()

spatial_bicycle_models.py

@@ -436,9 +436,9 @@ class BicycleModel(SpatialBicycleModel):
         a_2 = np.array([-kappa_r**2*delta_s,  1,            0])
         a_3 = np.array([-kappa_r/v*delta_s,   0,            1])
 
-        b_1 = np.array([0, ])
+        b_1 = np.array([0,          0])
-        b_2 = np.array([delta_s, ])
+        b_2 = np.array([0,   delta_s])
-        b_3 = np.array([0, ])
+        b_3 = np.array([-1/(v**2)*delta_s,          0])
 
         f = np.array([0.0, 0.0, 1/v*delta_s])