From 76df3ce81421b61b57f3f99f67589fbcb3295033 Mon Sep 17 00:00:00 2001 From: matssteinweg Date: Fri, 29 Nov 2019 09:36:30 +0100 Subject: [PATCH 01/36] initial commit LTV_MPC --- MPC.py | 329 +++++++++++++------ simulation.py | 89 ++--- spatial_bicycle_models.py | 674 ++++++-------------------------------- 3 files changed, 357 insertions(+), 735 deletions(-) diff --git a/MPC.py b/MPC.py index b122594..c54cbbc 100644 --- a/MPC.py +++ b/MPC.py @@ -1,31 +1,32 @@ import numpy as np import cvxpy as cp - +import osqp +import scipy as sp +from scipy import sparse ################## # MPC Controller # ################## class MPC: - def __init__(self, model, T, Q, R, Qf, StateConstraints, InputConstraints, - Reference): + def __init__(self, model, N, Q, R, QN, StateConstraints, InputConstraints): """ Constructor for the Model Predictive Controller. :param model: bicycle model object to be controlled :param T: time horizon | int :param Q: state cost matrix :param R: input cost matrix - :param Qf: final state cost matrix + :param QN: final state cost matrix :param StateConstraints: dictionary of state constraints :param InputConstraints: dictionary of input constraints :param Reference: reference values for state variables """ # Parameters - self.T = T # horizon + self.N = N # horizon self.Q = Q # weight matrix state vector self.R = R # weight matrix input vector - self.Qf = Qf # weight matrix terminal + self.QN = QN # weight matrix terminal # Model self.model = model @@ -34,11 +35,7 @@ class MPC: self.state_constraints = StateConstraints self.input_constraints = InputConstraints - # Reference - self.reference = Reference - # Current control and prediction - self.current_control = None self.current_prediction = None # Initialize Optimization Problem @@ -50,111 +47,71 @@ class MPC: time step. """ - # Instantiate optimization variables - self.x = cp.Variable((self.model.n_states+1, self.T + 1)) - self.u = cp.Variable((2, self.T)) + # number of input and state variables + nx = self.model.n_states + nu = 1 - # Instantiate optimization parameters - self.kappa = cp.Parameter(self.T+1) - self.x_0 = cp.Parameter(self.model.n_states+1, 1) - self.A = cp.Parameter(self.model.A.shape) - self.B = cp.Parameter(self.model.B.shape) + # system matrices + self.A = cp.Parameter(shape=(nx, nx*self.N)) + self.B = cp.Parameter(shape=(nx, nu*self.N)) + self.A.value = np.zeros(self.A.shape) + self.B.value = np.zeros(self.B.shape) - # Initialize cost - cost = 0 + # reference values + xr = np.array([0., 0., -1.0]) + self.ur = cp.Parameter((nu, self.N)) + self.ur.value = np.zeros(self.ur.shape) - # Initialize constraints - constraints = [self.x[:, 0] == self.x_0] + # constraints + umin = self.input_constraints['umin'] + umax = self.input_constraints['umax'] + xmin = self.state_constraints['xmin'] + xmax = self.state_constraints['xmax'] - for t in range(self.T): + # initial state + self.x_init = cp.Parameter(self.model.n_states) - # set dynamic constraints - constraints += [self.x[:-1, t + 1] == self.A[:-1, :] - @ self.x[:, t] + self.B[:-1, :] @ self.u[:, t], - self.x[-1, t + 1] == self.kappa[t+1]] - - # set input constraints - inputs = ['D', 'delta'] - for input_name, constraint in self.input_constraints.items(): - input_id = inputs.index(input_name) - if constraint[0] is not None: - constraints.append(-self.u[input_id, t] <= -constraint[0]) - if constraint[1] is not None: - constraints.append(self.u[input_id, t] <= constraint[1]) - - # Set state constraints - for state_name, constraint in self.state_constraints.items(): - state_id = self.model.spatial_state.list_states(). \ - index(state_name) - if constraint[0] is not None: - constraints.append(-self.x[state_id, t] <= -constraint[0]) - if constraint[1] is not None: - constraints.append(self.x[state_id, t] <= constraint[1]) - - # update cost function for states - for state_name, state_reference in self.reference.items(): - state_id = self.model.spatial_state.list_states(). \ - index(state_name) - cost += cp.norm(self.x[state_id, t] - state_reference, 2) * self.Q[ - state_id, state_id] - - # update cost function for inputs - cost += cp.norm(self.u[0, t], 2) * self.R[0, 0] - cost += cp.norm(self.u[1, t], 2) * self.R[1, 1] - - # set state constraints - for state_name, constraint in self.state_constraints.items(): - state_id = self.model.spatial_state.list_states(). \ - index(state_name) - if constraint[0] is not None: - constraints.append(-self.x[state_id, self.T] <= -constraint[0]) - if constraint[1] is not None: - constraints.append(self.x[state_id, self.T] <= constraint[1]) - - # update cost function for states - for state_name, state_reference in self.reference.items(): - state_id = self.model.spatial_state.list_states(). \ - index(state_name) - cost += cp.norm(self.x[state_id, self.T] - state_reference, 2) * \ - self.Qf[state_id, state_id] - - # sums problem objectives and concatenates constraints. - problem = cp.Problem(cp.Minimize(cost), constraints) + # Define problem + self.u = cp.Variable((nu, self.N)) + self.x = cp.Variable((nx, self.N + 1)) + objective = 0 + constraints = [self.x[:, 0] == self.x_init] + for n in range(self.N): + objective += cp.quad_form(self.x[:, n] - xr, self.Q) + cp.quad_form(self.u[:, n] - self.ur[:, n], self.R) + constraints += [self.x[:, n + 1] == self.A[:, n*nx:n*nx+nx] * self.x[:, n] + + self.B[:, n*nu] * (self.u[:, n] - self.ur[:, n])] + constraints += [umin <= self.u[:, n], self.u[:, n] <= umax] + objective += cp.quad_form(self.x[:, self.N] - xr, self.QN) + constraints += [xmin <= self.x[:, self.N], self.x[:, self.N] <= xmax] + problem = cp.Problem(cp.Minimize(objective), constraints) return problem - def get_control(self): + def get_control(self, v): """ Get control signal given the current position of the car. Solves a finite time optimization problem based on the linearized car model. """ - # get current waypoint curvature - kappa_ref = [wp.kappa for wp in self.model.reference_path.waypoints - [self.model.wp_id:self.model.wp_id+self.T+1]] + nx = self.model.n_states + nu = 1 - # Instantiate optimization parameters - self.kappa.value = kappa_ref - self.x_0.value = np.array(self.model.spatial_state[:] + [kappa_ref[0]]) - self.A.value = self.model.A - self.B.value = self.model.B + for n in range(self.N): + current_waypoint = self.model.reference_path.waypoints[self.model.wp_id+n] + next_waypoint = self.model.reference_path.waypoints[ + self.model.wp_id + n + 1] + delta_s = next_waypoint - current_waypoint + kappa_r = current_waypoint.kappa + self.A.value[:, n*nx:n*nx+nx], self.B.value[:, n*nu:n*nu+nu] = self.model.linearize(v, kappa_r, delta_s) + self.ur.value[:, n] = kappa_r - # Solve optimization problem - self.problem.solve(solver=cp.ECOS, warm_start=True) + self.x_init.value = np.array(self.model.spatial_state[:]) + self.problem.solve(solver=cp.OSQP, verbose=True) - # Store computed control signals and associated prediction - try: - self.current_control = self.u.value - self.current_prediction = self.update_prediction(self.x.value) - except TypeError: - print('No solution found!') - exit(1) + self.current_prediction = self.update_prediction(self.x.value) + delta = np.arctan(self.u.value[0, 0] * self.model.l) - # RCH - get first control signal - D_0 = self.u.value[0, 0] - delta_0 = self.u.value[1, 0] - - return D_0, delta_0 + return delta def update_prediction(self, spatial_state_prediction): """ @@ -168,12 +125,180 @@ class MPC: x_pred, y_pred = [], [] # get current waypoint ID - wp_id_ = np.copy(self.model.wp_id) + print('#########################') - for t in range(self.T): - associated_waypoint = self.model.reference_path.waypoints[wp_id_+t] + for n in range(self.N): + associated_waypoint = self.model.reference_path.waypoints[self.model.wp_id+n] predicted_temporal_state = self.model.s2t(associated_waypoint, - spatial_state_prediction[:, t]) + spatial_state_prediction[:, n]) + print('delta: ', np.arctan(self.u.value[0, n] * self.model.l)) + print('e_y: ', spatial_state_prediction[0, n]) + print('e_psi: ', spatial_state_prediction[1, n]) + print('t: ', spatial_state_prediction[2, n]) + print('+++++++++++++++++++++++') + + x_pred.append(predicted_temporal_state.x) + y_pred.append(predicted_temporal_state.y) + + return x_pred, y_pred + + +class MPC_OSQP: + def __init__(self, model, N, Q, R, QN, StateConstraints, InputConstraints): + """ + Constructor for the Model Predictive Controller. + :param model: bicycle model object to be controlled + :param T: 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 Reference: reference values for state variables + """ + + # Parameters + self.N = N # horizon + self.Q = Q # weight matrix state vector + self.R = R # weight matrix input vector + self.QN = QN # weight matrix terminal + + # Model + self.model = model + + # Constraints + self.state_constraints = StateConstraints + self.input_constraints = InputConstraints + + # Current control and prediction + self.current_prediction = None + + # Initialize Optimization Problem + self.optimizer = osqp.OSQP() + + def _init_problem(self, v): + """ + 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'] + xmin = self.state_constraints['xmin'] + xmax = self.state_constraints['xmax'] + + # LTV System Matrices + A = np.zeros((nx * (self.N + 1), nx * (self.N + 1))) + B = np.zeros((nx * (self.N + 1), nu * (self.N))) + # Reference vector for state and input variables + ur = np.zeros(self.N) + xr = np.array([0.0, 0.0, -1.0]) + # Offset for equality constraint (due to B * (u - ur)) + uq = np.zeros(self.N * nx) + + # Iterate over horizon + for n in range(self.N): + + # Get information about current waypoint + current_waypoint = self.model.reference_path.waypoints[ + self.model.wp_id + n] + next_waypoint = self.model.reference_path.waypoints[ + self.model.wp_id + n + 1] + delta_s = next_waypoint - current_waypoint + kappa_r = current_waypoint.kappa + + # Compute LTV matrices + A_lin, B_lin = self.model.linearize(v, kappa_r, delta_s) + A[nx + n * nx:n * nx + 2 * nx, n * nx:n * nx + nx] = A_lin + B[nx + n * nx:n * nx + 2 * nx, n * nu:n * nu + nu] = B_lin + + # Set kappa_r to reference for input signal + ur[n] = kappa_r + # Compute equality constraint offset (B*ur) + uq[n * nx:n * nx + nx] = B_lin[:, 0] * kappa_r + + # Get equality matrix + Ax = sparse.kron(sparse.eye(self.N + 1), + -sparse.eye(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) + # Combine constraint matrices + A = sparse.vstack([Aeq, Aineq], format='csc') + + # Get upper and lower bound vectors for equality constraints + lineq = np.hstack([np.kron(np.ones(self.N + 1), xmin), + np.kron(np.ones(self.N), umin)]) + uineq = np.hstack([np.kron(np.ones(self.N + 1), xmax), + np.kron(np.ones(self.N), umax)]) + # Get upper and lower bound vectors for inequality constraints + x0 = np.array(self.model.spatial_state[:]) + leq = np.hstack([-x0, uq]) + ueq = leq + # Combine upper and lower bound vectors + l = np.hstack([leq, lineq]) + u = np.hstack([ueq, uineq]) + + # Set cost matrices + 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), + -self.R.A[0, 0] * 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): + """ + Get control signal given the current position of the car. Solves a + finite time optimization problem based on the linearized car model. + """ + + # Number of state variables + nx = self.model.n_states + + # Initialize optimization problem + self._init_problem(v) + + # Solve optimization problem + dec = self.optimizer.solve() + x = np.reshape(dec.x[:(self.N+1)*nx], (self.N+1, nx)) + u = np.arctan(dec.x[-self.N] * self.model.l) + self.current_prediction = self.update_prediction(u, x) + + return u + + def update_prediction(self, u, spatial_state_prediction): + """ + Transform the predicted states to predicted x and y coordinates. + Mainly for visualization purposes. + :param spatial_state_prediction: list of predicted state variables + :return: lists of predicted x and y coordinates + """ + + # containers for x and y coordinates of predicted states + x_pred, y_pred = [], [] + + # get current waypoint ID + print('#########################') + + for n in range(self.N): + associated_waypoint = self.model.reference_path.waypoints[self.model.wp_id+n] + predicted_temporal_state = self.model.s2t(associated_waypoint, + spatial_state_prediction[n, :]) + 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('+++++++++++++++++++++++') + x_pred.append(predicted_temporal_state.x) y_pred.append(predicted_temporal_state.y) diff --git a/simulation.py b/simulation.py index 5cb46e5..1142a81 100644 --- a/simulation.py +++ b/simulation.py @@ -1,17 +1,17 @@ from map import Map import numpy as np from reference_path import ReferencePath -from spatial_bicycle_models import SimpleBicycleModel, ExtendedBicycleModel +from spatial_bicycle_models import BicycleModel import matplotlib.pyplot as plt -from MPC import MPC -from time import time +from MPC import MPC, MPC_OSQP +from scipy import sparse +import time + if __name__ == '__main__': # Select Simulation Mode | 'Race' or 'Q' sim_mode = 'Race' - # Select Model Type | 'Simple' or 'Extended' - model_type = 'Simple' # Create Map if sim_mode == 'Race': @@ -45,46 +45,23 @@ if __name__ == '__main__': # Initial state e_y_0 = 0.0 e_psi_0 = 0.0 - v_x_0 = 0.1 - v_y_0 = 0 - omega_0 = 0 - t_0 = 0 + t_0 = 0.0 + v = 1.0 - if model_type == 'Extended': - car = ExtendedBicycleModel(reference_path=reference_path, - e_y=e_y_0, e_psi=e_psi_0, v_x=v_x_0, v_y=v_y_0, - omega=omega_0, t=t_0) - elif model_type == 'Simple': - car = SimpleBicycleModel(reference_path=reference_path, - e_y=e_y_0, e_psi=e_psi_0, v=v_x_0) - else: - car = None - print('Invalid Model Type!') - exit(1) + car = BicycleModel(reference_path=reference_path, + e_y=e_y_0, e_psi=e_psi_0, t=t_0) ############## # Controller # ############## - if model_type == 'Extended': - Q = np.diag([1, 0, 0, 0, 0, 0]) - Qf = Q - R = np.diag([0, 0]) - Reference = {'e_y': 0, 'e_psi': 0, 'v_x': 1.0, 'v_y': 0, 'omega': 0, 't':0} - elif model_type == 'Simple': - Reference = {'e_y': 0, 'e_psi': 0, 'v': 4.0} - Q = np.diag([0.0005, 0.05, 0.5]) - Qf = Q - R = np.diag([0, 0]) - else: - Q, Qf, R, Reference = None, None, None, None - print('Invalid Model Type!') - exit(1) - - T = 5 - StateConstraints = {'e_y': (-0.2, 0.2), 'v': (0, 4)} - InputConstraints = {'D': (-1, 1), 'delta': (-0.44, 0.44)} - mpc = MPC(car, T, Q, R, Qf, StateConstraints, InputConstraints, Reference) + 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 # @@ -93,26 +70,22 @@ if __name__ == '__main__': # logging containers x_log = [car.temporal_state.x] y_log = [car.temporal_state.y] - psi_log = [car.temporal_state.psi] - v_log = [car.temporal_state.v_x] - D_log = [] - delta_log = [] # iterate over waypoints - for wp_id in range(len(car.reference_path.waypoints)-T-1): + for wp_id in range(len(car.reference_path.waypoints)-mpc.N-1): # get control signals - D, delta = mpc.get_control() + start = time.time() + delta = mpc.get_control(v) + end = time.time() + u = np.array([v, delta]) # drive car - car.drive(D, delta) + car.drive(u) - # log current state + # log x_log.append(car.temporal_state.x) y_log.append(car.temporal_state.y) - v_log.append(car.temporal_state.v_x) - D_log.append(D) - delta_log.append(delta) ################### # Plot Simulation # @@ -121,19 +94,13 @@ if __name__ == '__main__': car.reference_path.show() # plot car trajectory and velocity - plt.scatter(x_log, y_log, c='g', s=15) + plt.scatter(x_log[:-1], y_log[:-1], c='g', s=15) - # plot mpc prediction - if mpc.current_prediction is not None: - x_pred = mpc.current_prediction[0] - y_pred = mpc.current_prediction[1] - plt.scatter(x_pred, y_pred, c='b', s=10) + plt.scatter(mpc.current_prediction[0], mpc.current_prediction[1], c='b', s=5) - plt.title('MPC Simulation: Position: {:.2f} m, {:.2f} m, Velocity: ' - '{:.2f} m/s'.format(car.temporal_state.x, - car.temporal_state.y, car.temporal_state.v_x)) + 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.0000001) - + plt.pause(0.00000001) plt.close() diff --git a/spatial_bicycle_models.py b/spatial_bicycle_models.py index 60bd642..1f1b24f 100644 --- a/spatial_bicycle_models.py +++ b/spatial_bicycle_models.py @@ -7,7 +7,7 @@ from abc import ABC, abstractmethod ######################### class TemporalState: - def __init__(self, x, y, psi, v_x, v_y): + def __init__(self, x, y, psi): """ Temporal State Vector containing car pose (x, y, psi) and velocity :param x: x position in global coordinate system | [m] @@ -19,8 +19,6 @@ class TemporalState: self.x = x self.y = y self.psi = psi - self.v_x = v_x - self.v_y = v_y ######################## @@ -39,6 +37,9 @@ class SpatialState(ABC): def __getitem__(self, item): return list(vars(self).values())[item] + def __setitem__(self, key, value): + vars(self)[list(vars(self).keys())[key]] = value + def __len__(self): return len(vars(self)) @@ -60,40 +61,18 @@ class SpatialState(ABC): class SimpleSpatialState(SpatialState): - def __init__(self, e_y, e_psi, v): + def __init__(self, e_y, e_psi, t): """ Simplified Spatial State Vector containing orthogonal deviation from reference path (e_y), difference in orientation (e_psi) and velocity :param e_y: orthogonal deviation from center-line | [m] :param e_psi: yaw angle relative to path | [rad] - :param v: absolute velocity | [m/s] + :param t: time | [s] """ super(SimpleSpatialState, self).__init__() self.e_y = e_y self.e_psi = e_psi - self.v = v - - -class ExtendedSpatialState(SpatialState): - def __init__(self, e_y, e_psi, v_x, v_y, omega, t): - """ - Extended Spatial State Vector containing separate velocities in x and - y direction, angular velocity and time - :param e_y: orthogonal deviation from center-line | [m] - :param e_psi: yaw angle relative to path | [rad] - :param v_x: velocity in x direction (car frame) | [m/s] - :param v_y: velocity in y direction (car frame) | [m/s] - :param omega: anglular velocity of the car | [rad/s] - :param t: simulation time| [s] - """ - super(ExtendedSpatialState, self).__init__() - - self.e_y = e_y - self.e_psi = e_psi - self.v_x = v_x - self.v_y = v_y - self.omega = omega self.t = t @@ -161,45 +140,60 @@ class SpatialBicycleModel(ABC): return x, y, psi - def drive(self, D, delta): + def drive(self, input, state=None, kappa=None, delta_s=None): """ - Update states of spatial bicycle model. Model drive to the next - waypoint on the reference path. - :param D: acceleration command | [-1, 1] - :param delta: angular velocity | [rad] + Drive. + :param state: state vector for which to compute derivatives + :param input: input vector + :param kappa: curvature of corresponding waypoint + :return: numpy array with spatial derivatives for all state variables """ # Get spatial derivatives - spatial_derivatives = self.get_spatial_derivatives(D, delta) + if state is None and kappa is None and delta_s is None: + state = np.array(self.spatial_state[:]) + # Get delta_s | distance to next waypoint + next_waypoint = self.reference_path.waypoints[self.wp_id + 1] + delta_s = next_waypoint - self.current_waypoint + # Get current curvature + kappa = self.current_waypoint.kappa - # Get delta_s | distance to next waypoint - next_waypoint = self.reference_path.waypoints[self.wp_id+1] - delta_s = next_waypoint - self.current_waypoint + spatial_derivatives = self.get_spatial_derivatives(state, input, kappa) - # Update spatial state (Forward Euler Approximation) - self.spatial_state += spatial_derivatives * delta_s + # Update spatial state (Forward Euler Approximation) + self.spatial_state += spatial_derivatives * delta_s - # Assert that unique projections of car pose onto path exists - assert self.spatial_state.e_y < (1 / (self.current_waypoint.kappa + - self.eps)) + # Assert that unique projections of car pose onto path exists + #assert self.spatial_state.e_y < (1 / (self.current_waypoint.kappa + + # self.eps)) - # Increase waypoint ID - self.wp_id += 1 + # Increase waypoint ID + self.wp_id += 1 - # Update current waypoint - self.current_waypoint = self.reference_path.waypoints[self.wp_id] + # Update current waypoint + self.current_waypoint = self.reference_path.waypoints[self.wp_id] - # Update temporal_state to match spatial state - self.temporal_state = self.s2t() + # Update temporal_state to match spatial state + self.temporal_state = self.s2t() - # Update s | total driven distance along path - self.s += delta_s + # Update s | total driven distance along path + self.s += delta_s - # Linearize model around new operating point - self.A, self.B = self.linearize() + # Linearize model around new operating point + # self.A, self.B = self.linearize() + + else: + + spatial_derivatives = self.get_spatial_derivatives(state, input, + kappa) + + # Update spatial state (Forward Euler Approximation) + state += spatial_derivatives * delta_s + + return state @abstractmethod - def get_spatial_derivatives(self, D, delta): + def get_spatial_derivatives(self, state, input, kappa): pass @abstractmethod @@ -207,12 +201,12 @@ class SpatialBicycleModel(ABC): pass -######################## -# Simple Bicycle Model # -######################## +################# +# Bicycle Model # +################# -class SimpleBicycleModel(SpatialBicycleModel): - def __init__(self, reference_path, e_y, e_psi, v): +class BicycleModel(SpatialBicycleModel): + def __init__(self, reference_path, e_y, e_psi, t): """ Simplified Spatial Bicycle Model. Spatial Reformulation of Kinematic Bicycle Model. Uses Simplified Spatial State. @@ -223,24 +217,22 @@ class SimpleBicycleModel(SpatialBicycleModel): """ # Initialize base class - super(SimpleBicycleModel, self).__init__(reference_path) + super(BicycleModel, self).__init__(reference_path) # Constants - self.C1 = 0.5 - self.C2 = 17.06 - self.Cm1 = 12.0 - self.Cm2 = 2.17 - self.Cr2 = 0.1 - self.Cr0 = 0.6 + self.l = 0.06 # Initialize spatial state - self.spatial_state = SimpleSpatialState(e_y, e_psi, v) + self.spatial_state = SimpleSpatialState(e_y, e_psi, t) + + # Number of spatial state variables + self.n_states = len(self.spatial_state) # Initialize temporal state self.temporal_state = self.s2t() # Compute linear system matrices | Used for MPC - self.A, self.B = self.linearize() + # self.A, self.B = self.linearize() def s2t(self, reference_waypoint=None, reference_state=None): """ @@ -253,547 +245,85 @@ class SimpleBicycleModel(SpatialBicycleModel): if reference_state is None and reference_waypoint is None: # Get pose information from base class implementation - x, y, psi = super(SimpleBicycleModel, self).s2t() + x, y, psi = super(BicycleModel, self).s2t() # Compute simplified velocities - v_x = self.spatial_state.v - v_y = 0 else: # Get pose information from base class implementation - x, y, psi = super(SimpleBicycleModel, self).s2t(reference_waypoint, + x, y, psi = super(BicycleModel, self).s2t(reference_waypoint, reference_state) - v_x = reference_state[2] - v_y = 0 - return TemporalState(x, y, psi, v_x, v_y) + return TemporalState(x, y, psi) - def get_temporal_derivatives(self, D, delta): + def get_temporal_derivatives(self, state, input, kappa): """ Compute relevant temporal derivatives needed for state update. - :param D: duty-cycle of DC motor | [-1, 1] - :param delta: steering command | [rad] + :param state: state vector for which to compute derivatives + :param input: input vector + :param kappa: curvature of corresponding waypoint :return: temporal derivatives of distance, angle and velocity """ - # Compute velocity components | Approximation for small delta - v_x = self.spatial_state.v - v_y = self.spatial_state.v * delta * self.C1 - - # Compute velocity along waypoint direction - v_sigma = v_x * np.cos(self.spatial_state.e_psi) - v_y * np.sin( - self.spatial_state.e_psi) + e_y, e_psi, t = state + v, delta = input # Compute velocity along path - s_dot = 1 / (1 - (self.spatial_state.e_y * self.current_waypoint.kappa)) * v_sigma + s_dot = 1 / (1 - (e_y * kappa)) * v * np.cos(e_psi) # Compute yaw angle rate of change - psi_dot = self.spatial_state.v * delta * self.C2 + psi_dot = v / self.l * np.tan(delta) - # Compute acceleration - v_dot = (self.Cm1 - self.Cm2 * self.spatial_state.v) * D - self.Cr2 * ( - self.spatial_state.v ** 2) - self.Cr0 - ( - self.spatial_state.v * delta) ** 2 * self.C2 * self.C1 ** 2 + return s_dot, psi_dot - return s_dot, psi_dot, v_dot - - def get_spatial_derivatives(self, D, delta): + def get_spatial_derivatives(self, state, input, kappa): """ Compute spatial derivatives of all state variables for update. - :param D: duty-cycle of DC motor | [-1, 1] - :param delta: steering angle | [rad] + :param state: state vector for which to compute derivatives + :param input: input vector + :param kappa: curvature of corresponding waypoint :return: numpy array with spatial derivatives for all state variables """ + e_y, e_psi, t = state + v, delta = input + # Compute temporal derivatives - s_dot, psi_dot, v_dot = self.get_temporal_derivatives(D, delta) + s_dot, psi_dot = self.get_temporal_derivatives(state, input, kappa) # Compute spatial derivatives - d_e_y_d_s = (self.spatial_state.v * np.sin(self.spatial_state.e_psi) - + self.spatial_state.v * delta * self.C1 * np.cos( - self.spatial_state.e_psi)) / s_dot - d_e_psi_d_s = psi_dot / s_dot - self.current_waypoint.kappa - d_v_d_s = v_dot / s_dot + d_e_y_d_s = v * np.sin(e_psi) / s_dot + d_e_psi_d_s = psi_dot / s_dot - kappa + d_t_d_s = 1 / s_dot - return np.array([d_e_y_d_s, d_e_psi_d_s, d_v_d_s]) + return np.array([d_e_y_d_s, d_e_psi_d_s, d_t_d_s]) - def linearize(self, D=0, delta=0): + def linearize(self, v=None, kappa_r=None, delta_s=None): """ Linearize the system equations around the current state and waypoint. - :param delta: reference steering angle | [rad] - :param D: reference duty-cycle of DC-motor | [-1, 1] - """ + :param kappa_r: kappa of waypoint around which to linearize + """ - # Get current state | operating point to linearize around - e_y = self.spatial_state.e_y - e_psi = self.spatial_state.e_psi - v = self.spatial_state.v - - # Get curvature of current waypoint - kappa = self.reference_path.waypoints[self.wp_id].kappa - - # Get delta_s - next_waypoint = self.reference_path.waypoints[self.wp_id+1] - delta_s = next_waypoint - self.current_waypoint - - ############################## - # Helper Partial Derivatives # - ############################## - - # Compute velocity components - v_x = v - v_y = v * delta * self.C1 - - # Compute partial derivatives of s_dot w.r.t. each state variable, - # input variable and kappa - s_dot = 1 / (1 - e_y*kappa) * (v_x * np.cos(e_psi) - v_y * np.sin(e_psi)) - d_s_dot_d_e_y = kappa / (1-e_y*kappa)**2 * (v_x * np.cos(e_psi) - v_y * np.sin(e_psi)) - d_s_dot_d_e_psi = 1 / (1 - e_y*kappa) * (-v_x * np.sin(e_psi) - v_y * np.cos(e_psi)) - d_s_dot_d_v = 1 / (1 - e_y*kappa) * (np.cos(e_psi) - delta * self.C1 * np.sin(e_psi)) - # d_s_dot_d_D = 0 - d_s_dot_d_delta = 1 / (1 - e_y*kappa) * (- v * self.C1 * np.sin(e_psi)) - d_s_dot_d_kappa = e_y / (1-e_y*kappa)**2 * (v_x * np.cos(e_psi) - v_y * np.sin(e_psi)) - - # Compute partial derivatives of v_psi w.r.t. each state variable, - # input variable and kappa - v_psi = (v_x * np.sin(e_psi) + v_y * np.cos(e_psi)) - # d_v_psi_d_e_y = 0 - d_v_psi_d_e_psi = v_x * np.cos(e_psi) - v_y * np.sin(e_psi) - d_v_psi_d_v = np.sin(e_psi) + self.C1 * delta * np.cos(e_psi) - # d_v_psi_d_D = 0 - d_v_psi_d_delta = self.C1 * v * np.cos(e_psi) - # d_v_psi_d_kappa = 0 - - # Compute partial derivatives of psi_dot w.r.t. each state variable, - # input variable and kappa - psi_dot = v * delta * self.C2 - # d_psi_dot_d_e_y = 0 - # d_psi_dot_d_e_psi = 0 - d_psi_dot_d_v = delta * self.C2 - # d_psi_dot_d_D = 0 - d_psi_dot_d_delta = v * self.C2 - # d_psi_dot_d_kappa = 0 - - # Compute partial derivatives of v_dot w.r.t. each state variable, - # input variable and kappa - v_dot = (self.Cm1 - self.Cm2 * v) * D - self.Cr2 * (v ** 2) - self.Cr0 \ - - (v * delta) ** 2 * self.C2 * (self.C1 ** 2) - # d_v_dot_d_e_y = 0 - # d_v_dot_d_e_psi = 0 - d_v_dot_d_v = -self.Cm2 * D - 2 * self.Cr2 * v - 2 * v * (delta ** 2) \ - * self.C2 * (self.C1 ** 2) - d_v_dot_d_D = self.Cm1 - self.Cm2 * v - d_v_dot_d_delta = -2 * (v ** 2) * delta * self.C2 * self.C1 ** 2 - # d_v_dot_d_kappa = 0 - - ############################# - # State Partial Derivatives # - ############################# - - # Use pre-computed helper derivatives to compute spatial derivatives of - # all state variables using Quotient Rule - - # Compute partial derivatives of e_y w.r.t. each state variable, - # input variable and kappa - # e_y = v_psi / s_dot - d_e_y_d_e_y = - d_s_dot_d_e_y * v_psi / (s_dot**2) - d_e_y_d_e_psi = (d_v_psi_d_e_psi * s_dot - d_s_dot_d_e_psi * v_psi) / (s_dot**2) - d_e_y_d_v = (d_v_psi_d_v * s_dot - d_s_dot_d_v * v_psi) / (s_dot**2) - d_e_y_d_D = 0 - d_e_y_d_delta = (d_v_psi_d_delta * s_dot - d_s_dot_d_delta * v_psi) / (s_dot**2) - d_e_y_d_kappa = - d_s_dot_d_kappa * v_psi / (s_dot**2) - - # Compute partial derivatives of e_psi w.r.t. each state variable, - # input variable and kappa - # e_psi = psi_dot / s_dot - kappa - d_e_psi_d_e_y = - d_s_dot_d_e_y * psi_dot / (s_dot**2) - d_e_psi_d_e_psi = - d_s_dot_d_e_psi * psi_dot / (s_dot**2) - d_e_psi_d_v = (d_psi_dot_d_v * s_dot - d_s_dot_d_v * psi_dot) / (s_dot**2) - d_e_psi_d_D = 0 - d_e_psi_d_delta = (d_psi_dot_d_delta * s_dot - d_s_dot_d_delta * psi_dot) / (s_dot**2) - d_e_psi_d_kappa = - d_s_dot_d_kappa * psi_dot / (s_dot**2) - 1 - - # Compute partial derivatives of v w.r.t. each state variable, - # input variable and kappa - # v = v_dot / s_dot - d_v_d_e_y = - d_s_dot_d_e_y * v_dot / (s_dot**2) - d_v_d_e_psi = - d_s_dot_d_e_psi * v_dot / (s_dot**2) - d_v_d_v = (d_v_dot_d_v * s_dot - d_s_dot_d_v * v_dot) / (s_dot**2) - d_v_d_D = d_v_dot_d_D * s_dot / (s_dot**2) - d_v_d_delta = (d_v_dot_d_delta * s_dot - d_s_dot_d_delta * v_dot) / (s_dot**2) - d_v_d_kappa = - d_s_dot_d_kappa * v_dot / (s_dot**2) - - ############# - # Jacobians # - ############# - - # Construct Jacobian Matrix - a_1 = np.array([d_e_y_d_e_y, d_e_y_d_e_psi, d_e_y_d_v, d_e_y_d_kappa]) - a_2 = np.array([d_e_psi_d_e_y, d_e_psi_d_e_psi, d_e_psi_d_v, d_e_psi_d_kappa]) - a_3 = np.array([d_v_d_e_y, d_v_d_e_psi, d_v_d_v, d_v_d_kappa]) - - b_1 = np.array([d_e_y_d_D, d_e_y_d_delta]) - b_2 = np.array([d_e_psi_d_D, d_e_psi_d_delta]) - b_3 = np.array([d_v_d_D, d_v_d_delta]) - - # Add extra row for kappa | Allows for updating kappa during MPC - # optimization - a_4 = np.array([0, 0, 0, 0]) - b_4 = np.array([0, 0]) - - Ja = np.stack((a_1, a_2, a_3, a_4), axis=0) - Jb = np.stack((b_1, b_2, b_3, b_4), axis=0) + # Get linearization state + if kappa_r is None and delta_s is None: + # Get curvature of linearization waypoint + kappa_r = self.reference_path.waypoints[self.wp_id].kappa + # Get delta_s + next_waypoint = self.reference_path.waypoints[self.wp_id + 1] + delta_s = next_waypoint - self.current_waypoint ################### # System Matrices # ################### - # Construct system matrices from Jacobians. Multiply by sampling - # distance delta_s + add identity matrix (Forward Euler Approximation) - A = Ja * delta_s + np.identity(Ja.shape[1]) - B = Jb * delta_s + # Construct Jacobian Matrix + a_1 = np.array([1, delta_s, 0]) + a_2 = np.array([-kappa_r**2*delta_s, 1, 0]) + a_3 = np.array([-kappa_r/v*delta_s, 0, 0]) - return A, B + b_1 = np.array([0, ]) + b_2 = np.array([delta_s, ]) + b_3 = np.array([0, ]) + A = np.stack((a_1, a_2, a_3), axis=0) + B = np.stack((b_1, b_2, b_3), axis=0) -########################## -# Extended Bicycle Model # -########################## - -class ExtendedBicycleModel(SpatialBicycleModel): - def __init__(self, reference_path, e_y, e_psi, v_x, v_y, omega, t): - """ - Construct spatial bicycle model. - :param e_y: initial deviation from reference path | [m] - :param e_psi: initial heading offset from reference path | [rad] - :param v: initial velocity | [m/s] - :param reference_path: reference path model is supposed to follow - """ - super(ExtendedBicycleModel, self).__init__(reference_path) - - # Constants - self.m = 0.041 - self.Iz = 27.8e-6 - self.lf = 0.029 - self.lr = 0.033 - - self.Cm1 = 0.287 - self.Cm2 = 0.0545 - self.Cr2 = 0.0518 - self.Cr0 = 0.00035 - - self.Br = 3.3852 - self.Cr = 1.2691 - self.Dr = 0.1737 - self.Bf = 2.579 - self.Cf = 1.2 - self.Df = 0.192 - - # Spatial state - self.spatial_state = ExtendedSpatialState(e_y, e_psi, v_x, v_y, omega, t) - - # Temporal state - self.temporal_state = self.s2t() - - # Linear System Matrices - self.A, self.B = self.linearize() - - def s2t(self, reference_waypoint=None, predicted_state=None): - """ - Convert spatial state to temporal state - :return temporal state equivalent to self.spatial_state - """ - - # compute velocity information - if predicted_state is None and reference_waypoint is None: - # get information from base class - x, y, psi = super(ExtendedBicycleModel, self).s2t() - v_x = self.spatial_state.v_x - v_y = self.spatial_state.v_y - else: - # get information from base class - x, y, psi = super(ExtendedBicycleModel, self).s2t(reference_waypoint, - predicted_state) - v_x = predicted_state[2] - v_y = predicted_state[3] - - return TemporalState(x, y, psi, v_x, v_y) - - def get_forces(self, delta, D): - """ - Compute forces required for temporal derivatives of v_x and v_y - :param delta: - :param D: - :return: - """ - - F_rx = (self.Cm1 - self.Cm2 * self.spatial_state.v_x) * D - self.Cr0 - self.Cr2 * self.spatial_state.v_x ** 2 - - alpha_f = - np.arctan2(self.spatial_state.omega*self.lf + self.spatial_state.v_y, self.spatial_state.v_x) + delta - F_fy = self.Df * np.sin(self.Cf*np.arctan(self.Bf*alpha_f)) - - alpha_r = np.arctan2(self.spatial_state.omega*self.lr - self.spatial_state.v_y, self.spatial_state.v_x) - F_ry = self.Dr * np.sin(self.Cr * np.arctan(self.Br*alpha_r)) - - return F_rx, F_fy, F_ry, alpha_f, alpha_r - - def get_temporal_derivatives(self, delta, F_rx, F_fy, F_ry): - """ - Compute temporal derivatives needed for state update. - :param delta: steering command - :param D: duty-cycle of DC motor - :return: temporal derivatives of distance, angle and velocity - """ - - # velocity along path - s_dot = 1 / (1 - (self.spatial_state.e_y * self.current_waypoint.kappa)) \ - * (self.spatial_state.v_x * np.cos(self.spatial_state.e_psi) - + self.spatial_state.v_y * np.sin(self.spatial_state.e_psi)) - - # velocity in x and y direction - v_x_dot = (F_rx - F_fy * np.sin(delta) + self.m * self.spatial_state. - v_y * self.spatial_state.omega) / self.m - v_y_dot = (F_ry + F_fy * np.cos(delta) - self.m * self.spatial_state. - v_x * self.spatial_state.omega) / self.m - - # omega dot - omega_dot = (F_fy * self.lf * np.cos(delta) - F_ry * self.lr) / self.Iz - - return s_dot, v_x_dot, v_y_dot, omega_dot - - def get_spatial_derivatives(self, delta, D): - """ - Compute spatial derivatives of all state variables for update. - :param delta: steering angle - :param psi_dot: heading rate of change - :param s_dot: velocity along path - :param v_dot: acceleration - :return: spatial derivatives for all state variables - """ - - # get required forces - F_rx, F_fy, F_ry, _, _ = self.get_forces(delta, D) - - # Compute state derivatives - s_dot, v_x_dot, v_y_dot, omega_dot = \ - self.get_temporal_derivatives(delta, F_rx, F_fy, F_ry) - - - d_e_y = (self.spatial_state.v_x * np.sin(self.spatial_state.e_psi) - + self.spatial_state.v_y * np.cos(self.spatial_state.e_psi)) \ - / (s_dot + self.eps) - d_e_psi = (self.spatial_state.omega / (s_dot + self.eps) - self.current_waypoint.kappa) - - d_v_x = v_x_dot / (s_dot + self.eps) - d_v_y = v_y_dot / (s_dot + self.eps) - d_omega = omega_dot / (s_dot + self.eps) - d_t = 1 / (s_dot + self.eps) - - return np.array([d_e_y, d_e_psi, d_v_x, d_v_y, d_omega, d_t]) - - def linearize(self, delta=0, D=0): - """ - Linearize the system equations around the current state and waypoint. - :param delta: reference steering angle - :param D: reference dutycycle - """ - - # get current state - e_y = self.spatial_state.e_y - e_psi = self.spatial_state.e_psi - v_x = self.spatial_state.v_x - v_y = self.spatial_state.v_y - omega = self.spatial_state.omega - t = self.spatial_state.t - - # get information about current waypoint - kappa = self.reference_path.waypoints[self.wp_id].kappa - - # get delta_s - next_waypoint = self.reference_path.waypoints[self.wp_id + 1] - delta_s = next_waypoint - self.current_waypoint - - # get temporal derivatives - F_rx, F_fy, F_ry, alpha_f, alpha_r = self.get_forces(delta, D) - s_dot, v_x_dot, v_y_dot, omega_dot = self.\ - get_temporal_derivatives(delta, F_rx, F_fy, F_ry) - - ############################## - # Forces Partial Derivatives # - ############################## - - d_alpha_f_d_v_x = 1 / (1 + ((omega * self.lf + v_y) / v_x)**2) * (omega * self.lf + v_y) / (v_x**2) - d_alpha_f_d_v_y = - 1 / (1 + ((omega * self.lf + v_y) / v_x)**2) / v_x - d_alpha_f_d_omega = - 1 / (1 + ((omega * self.lf + v_y) / v_x)**2) * (self.lf / v_x) - d_alpha_f_d_delta = 1 - - d_alpha_r_d_v_x = - 1 / (1 + ((omega * self.lr - v_y) / v_x)**2) * (omega * self.lr - v_y) / (v_x**2) - d_alpha_r_d_v_y = - 1 / (1 + ((omega * self.lr - v_y) / v_x)**2) / v_x - d_alpha_r_d_omega = 1 / (1 + ((omega * self.lr - v_y) / v_x)**2) * (self.lr * v_x) - - d_F_fy_d_v_x = self.Df * np.cos(self.Cf * np.arctan(self.Bf * alpha_f)) * self.Cf / (1 + (self.Bf * alpha_f)**2) * self.Bf * d_alpha_f_d_v_x - d_F_fy_d_v_y = self.Df * np.cos(self.Cf * np.arctan(self.Bf * alpha_f)) * self.Cf / (1 + (self.Bf * alpha_f)**2) * self.Bf * d_alpha_f_d_v_y - d_F_fy_d_omega = self.Df * np.cos(self.Cf * np.arctan(self.Bf * alpha_f)) * self.Cf / (1 + (self.Bf * alpha_f)**2) * self.Bf * d_alpha_f_d_omega - d_F_fy_d_delta = self.Df * np.cos(self.Cf * np.arctan(self.Bf * alpha_f)) * self.Cf / (1 + (self.Bf * alpha_f)**2) * self.Bf * d_alpha_f_d_delta - - d_F_ry_d_v_x = self.Dr * np.cos(self.Cr * np.arctan(self.Br * alpha_r)) * self.Cr / (1 + (self.Br * alpha_r)**2) * self.Br * d_alpha_r_d_v_x - d_F_ry_d_v_y = self.Dr * np.cos(self.Cr * np.arctan(self.Br * alpha_r)) * self.Cr / (1 + (self.Br * alpha_r)**2) * self.Br * d_alpha_r_d_v_y - d_F_ry_d_omega = self.Dr * np.cos(self.Cr * np.arctan(self.Br * alpha_r)) * self.Cr / (1 + (self.Br * alpha_r)**2) * self.Br * d_alpha_r_d_omega - - d_F_rx_d_v_x = - self.Cm2 * D - 2 * self.Cr2 * v_x - d_F_rx_d_D = self.Cm1 - self.Cm2 * v_x - - ############################## - # Helper Partial Derivatives # - ############################## - - d_s_dot_d_e_y = kappa / (1-e_y*kappa)**2 * (v_x * np.cos(e_psi) - v_y * np.sin(e_psi)) - d_s_dot_d_e_psi = 1 / (1 - e_y*kappa) * (-v_x * np.sin(e_psi) - v_y * np.cos(e_psi)) - d_s_dot_d_v_x = 1 / (1 - e_y*kappa) * np.cos(e_psi) - d_s_dot_d_v_y = -1 / (1 - e_y*kappa) * np.sin(e_psi) - d_s_dot_d_omega = 0 - d_s_dot_d_t = 0 - d_s_dot_d_delta = 0 - d_s_dot_d_D = 0 - d_s_dot_d_kappa = e_y / (1-e_y*kappa)**2 * (v_x * np.cos(e_psi) - v_y * np.sin(e_psi)) - # Check - - c_1 = (v_x * np.sin(e_psi) + v_y * np.cos(e_psi)) - d_c_1_d_e_y = 0 - d_c_1_d_e_psi = v_x * np.cos(e_psi) - v_y * np.sin(e_psi) - d_c_1_d_v_x = np.sin(e_psi) - d_c_1_d_v_y = np.cos(e_psi) - d_c_1_d_omega = 0 - d_c_1_d_t = 0 - d_c_1_d_delta = 0 - d_c_1_d_D = 0 - d_c_1_d_kappa = 0 - # Check - - d_v_x_dot_d_e_y = 0 - d_v_x_dot_d_e_psi = 0 - d_v_x_dot_d_v_x = (d_F_rx_d_v_x - d_F_fy_d_v_x * np.sin(delta)) / self.m - d_v_x_dot_d_v_y = - (d_F_fy_d_v_y * np.sin(delta) + self.m * omega) / self.m - d_v_x_dot_d_omega = - (d_F_fy_d_omega * np.sin(delta) + self.m * v_y) / self.m - d_v_x_dot_d_t = 0 - d_v_x_dot_d_delta = - (F_fy * np.cos(delta) + d_F_fy_d_delta * np.sin(delta)) / self.m - d_v_x_dot_d_D = d_F_rx_d_D / self.m - d_v_x_dot_d_kappa = 0 - - d_v_y_dot_d_e_y = 0 - d_v_y_dot_d_e_psi = 0 - d_v_y_dot_d_v_x = (d_F_ry_d_v_x + d_F_fy_d_v_x * np.cos(delta) - self.m * omega) / self.m - d_v_y_dot_d_v_y = (d_F_ry_d_v_y + d_F_fy_d_v_y * np.cos(delta)) / self.m - d_v_y_dot_d_omega = (d_F_ry_d_omega + d_F_fy_d_omega * np.cos(delta) - self.m * v_x) / self.m - d_v_y_dot_d_t = 0 - d_v_y_dot_d_delta = d_F_fy_d_delta * np.cos(delta) / self.m - d_v_y_dot_d_D = 0 - d_v_y_dot_d_kappa = 0 - - d_omega_dot_d_e_y = 0 - d_omega_dot_d_e_psi = 0 - d_omega_dot_d_v_x = (d_F_fy_d_v_x * self.lf * np.cos(delta) - d_F_ry_d_v_x * self.lr) / self.Iz - d_omega_dot_d_v_y = (d_F_fy_d_v_y * self.lf * np.cos(delta) - d_F_fy_d_v_y * self.lr) / self.Iz - d_omega_dot_d_omega = (d_F_fy_d_omega * self.lf * np.cos(delta) - d_F_fy_d_omega * self.lr) / self.Iz - d_omega_dot_d_t = 0 - d_omega_dot_d_delta = (- F_fy * np.sin(delta) + d_F_fy_d_delta * np.cos(delta)) / self.Iz - d_omega_dot_d_D = 0 - d_omega_dot_d_kappa = 0 - - ####################### - # Partial Derivatives # - ####################### - - # derivatives for E_Y - d_e_y_d_e_y = -c_1 * d_s_dot_d_e_y / (s_dot**2) - d_e_y_d_e_psi = (d_c_1_d_e_psi * s_dot - d_s_dot_d_e_psi * c_1) / (s_dot**2) - d_e_y_d_v_x = (d_c_1_d_v_x * s_dot - d_s_dot_d_v_x * c_1) / (s_dot**2) - d_e_y_d_v_y = (d_c_1_d_v_y * s_dot - d_s_dot_d_v_y * c_1) / (s_dot**2) - d_e_y_d_omega = (d_c_1_d_omega * s_dot - d_s_dot_d_omega * c_1) / (s_dot**2) - d_e_y_d_t = 0 - d_e_y_d_D = 0 - d_e_y_d_delta = (d_c_1_d_delta * s_dot - d_s_dot_d_delta * c_1) / (s_dot**2) - d_e_y_d_kappa = -d_s_dot_d_kappa * c_1 / (s_dot**2) - - # derivatives for E_PSI - d_e_psi_d_e_y = - omega * d_s_dot_d_e_y / (s_dot**2) - d_e_psi_d_e_psi = - omega * d_s_dot_d_e_psi / (s_dot**2) - d_e_psi_d_v_x = (- omega * d_s_dot_d_v_x) / (s_dot**2) - d_e_psi_d_v_y = (- omega * d_s_dot_d_v_y) / (s_dot**2) - d_e_psi_d_omega = (s_dot - omega * d_s_dot_d_omega) / (s_dot**2) - d_e_psi_d_t = 0 - d_e_psi_d_delta = (- omega * d_s_dot_d_delta) / (s_dot**2) - d_e_psi_d_D = (- omega * d_s_dot_d_D) / (s_dot**2) - d_e_psi_d_kappa = -d_s_dot_d_kappa * omega / (s_dot**2) - 1 - - # derivatives for V_X - d_v_x_d_e_y = - d_s_dot_d_e_y * v_x_dot / (s_dot**2) - d_v_x_d_e_psi = - d_s_dot_d_e_psi * v_x_dot / (s_dot**2) - d_v_x_d_v_x = (d_v_x_dot_d_v_x * s_dot - d_s_dot_d_v_x * v_x_dot) / (s_dot**2) - d_v_x_d_v_y = (d_v_x_dot_d_v_y * s_dot - d_s_dot_d_v_y * v_x_dot) / (s_dot**2) - d_v_x_d_omega = (d_v_x_dot_d_omega * s_dot - d_s_dot_d_omega * v_x_dot) / (s_dot**2) - d_v_x_d_t = 0 - d_v_x_d_delta = (d_v_x_dot_d_delta * s_dot - d_s_dot_d_delta * v_x_dot) / (s_dot**2) - d_v_x_d_D = d_v_x_dot_d_D * s_dot / (s_dot**2) - d_v_x_d_kappa = -d_s_dot_d_kappa * v_x_dot / (s_dot**2) - - # derivatives for V_Y - d_v_y_d_e_y = - d_s_dot_d_e_y * v_y_dot / (s_dot ** 2) - d_v_y_d_e_psi = - d_s_dot_d_e_psi * v_y_dot / (s_dot ** 2) - d_v_y_d_v_x = (d_v_y_dot_d_v_x * s_dot - d_s_dot_d_v_x * v_y_dot) / ( - s_dot ** 2) - d_v_y_d_v_y = (d_v_y_dot_d_v_y * s_dot - d_s_dot_d_v_y * v_y_dot) / ( - s_dot ** 2) - d_v_y_d_omega = (d_v_y_dot_d_omega * s_dot - d_s_dot_d_omega * v_y_dot) / ( - s_dot ** 2) - d_v_y_d_t = 0 - d_v_y_d_delta = (d_v_y_dot_d_delta * s_dot - d_s_dot_d_delta * v_y_dot) / ( - s_dot ** 2) - d_v_y_d_D = d_v_y_dot_d_D * s_dot / (s_dot ** 2) - d_v_y_d_kappa = -d_s_dot_d_kappa * v_y_dot / (s_dot ** 2) - - # derivatives for Omega - d_omega_d_e_y = (d_omega_dot_d_e_y * s_dot - omega_dot * d_s_dot_d_e_y) / (s_dot**2) - d_omega_d_e_psi = (d_omega_dot_d_e_psi * s_dot - omega_dot * d_s_dot_d_e_psi) / (s_dot**2) - d_omega_d_v_x = (d_omega_dot_d_v_x * s_dot - omega_dot * d_s_dot_d_v_x) / (s_dot**2) - d_omega_d_v_y = (d_omega_dot_d_v_y * s_dot - omega_dot * d_s_dot_d_v_y) / (s_dot**2) - d_omega_d_omega = (d_omega_dot_d_omega * s_dot - omega_dot * d_s_dot_d_omega) / (s_dot**2) - d_omega_d_t = (d_omega_dot_d_t * s_dot - omega_dot * d_s_dot_d_t) / (s_dot**2) - d_omega_d_delta = (d_omega_dot_d_delta * s_dot - omega_dot * d_s_dot_d_delta) / (s_dot**2) - d_omega_d_D = (d_omega_dot_d_D * s_dot - omega_dot * d_s_dot_d_D) / (s_dot**2) - d_omega_d_kappa = (d_omega_dot_d_kappa * s_dot - omega_dot * d_s_dot_d_kappa) / (s_dot**2) - - # derivatives for T - d_t_d_e_y = - d_s_dot_d_e_y / (s_dot**2) - d_t_d_e_psi = - d_s_dot_d_e_psi / (s_dot ** 2) - d_t_d_v_x = - d_s_dot_d_v_x / (s_dot ** 2) - d_t_d_v_y = - d_s_dot_d_v_y / (s_dot ** 2) - d_t_d_omega = - d_s_dot_d_omega / (s_dot ** 2) - d_t_d_t = 0 - d_t_d_delta = - d_s_dot_d_delta / (s_dot ** 2) - d_t_d_D = 0 - d_t_d_kappa = - d_s_dot_d_kappa / (s_dot ** 2) - - a_1 = np.array([d_e_y_d_e_y, d_e_y_d_e_psi, d_e_y_d_v_x, d_e_y_d_v_y, d_e_y_d_omega, d_e_y_d_t, d_e_y_d_kappa]) - a_2 = np.array([d_e_psi_d_e_y, d_e_psi_d_e_psi, d_e_psi_d_v_x, d_e_psi_d_v_y, d_e_psi_d_omega, d_e_psi_d_t, d_e_psi_d_kappa]) - a_3 = np.array([d_v_x_d_e_y, d_v_x_d_e_psi, d_v_x_d_v_x, d_v_x_d_v_y, d_v_x_d_omega, d_v_x_d_t, d_v_x_d_kappa]) - a_4 = np.array([d_v_y_d_e_y, d_v_y_d_e_psi, d_v_y_d_v_x, d_v_y_d_v_y, d_v_y_d_omega, d_v_y_d_t, d_v_y_d_kappa]) - a_5 = np.array([d_omega_d_e_y, d_omega_d_e_psi, d_omega_d_v_x, d_omega_d_v_y, d_omega_d_omega, d_omega_d_t, d_omega_d_kappa]) - a_6 = np.array([d_t_d_e_y, d_t_d_e_psi, d_t_d_v_x, d_t_d_v_y, d_t_d_omega, d_t_d_t, d_t_d_kappa]) - a_7 = np.array([0, 0, 0, 0, 0, 0, 1]) - A = np.stack((a_1, a_2, a_3, a_4, a_5, a_6, a_7), axis=0) * delta_s - A[0, 0] += 1 - A[1, 1] += 1 - A[2, 2] += 1 - A[3, 3] += 1 - A[4, 4] += 1 - A[5, 5] += 1 - b_1 = np.array([d_e_y_d_D, d_e_y_d_delta]) - b_2 = np.array([d_e_psi_d_D, d_e_psi_d_delta]) - b_3 = np.array([d_v_x_d_D, d_v_x_d_delta]) - b_4 = np.array([d_v_y_d_D, d_v_y_d_delta]) - b_5 = np.array([d_omega_d_D, d_omega_d_delta]) - b_6 = np.array([d_t_d_D, d_t_d_delta]) - b_7 = np.array([0, 0]) - B = np.stack((b_1, b_2, b_3, b_4, b_5, b_6, b_7), axis=0) * delta_s - - # set system matrices - return A, B + return A, B \ No newline at end of file From e87596d5e89e7679c82b76114116983b824f07cf Mon Sep 17 00:00:00 2001 From: matssteinweg Date: Sun, 1 Dec 2019 01:56:15 +0100 Subject: [PATCH 02/36] Add comments and restructure reference path class. Add update_bounds method. --- reference_path.py | 341 +++++++++++++++++++++++++++++++++------------- 1 file changed, 250 insertions(+), 91 deletions(-) diff --git a/reference_path.py b/reference_path.py index 50d21cb..80378ab 100644 --- a/reference_path.py +++ b/reference_path.py @@ -24,7 +24,20 @@ class Waypoint: self.psi = psi self.kappa = kappa + # information about drivable area at waypoint + # upper and lower bound of drivable area orthogonal to + # waypoint orientation + self.lb = None + self.ub = None + self.border_cells = None + def __sub__(self, other): + """ + Overload subtract operator. Difference of two waypoints is equal to + their euclidean distance. + :param other: subtrahend + :return: euclidean distance between two waypoints + """ return ((self.x - other.x)**2 + (self.y - other.y)**2)**0.5 @@ -34,35 +47,46 @@ class Waypoint: class ReferencePath: - def __init__(self, map, wp_x, wp_y, resolution, smoothing_distance, width_info=False): + def __init__(self, map, wp_x, wp_y, resolution, smoothing_distance, max_width): """ Reference Path object. Create a reference trajectory from specified corner points with given resolution. Smoothing around corners can be - applied. + applied. Waypoints represent center-line of the path with specified + maximum width to both sides. + :param map: map object on which path will be placed + :param wp_x: x coordinates of corner points in global coordinates + :param wp_y: y coordinates of corner points in global coordinates + :param resolution: resolution of the path in m/wp + :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 """ - # precision + + # Precision self.eps = 1e-12 - # map + # Map self.map = map - # resolution of the path + # Resolution of the path self.resolution = resolution - # look ahead distance for path averaging + # Look ahead distance for path averaging self.smoothing_distance = smoothing_distance - # waypoints with x, y, psi, k - self.waypoints = self.construct_path(wp_x, wp_y) + # List of waypoint objects + self.waypoints = self._construct_path(wp_x, wp_y) - # path width - self.get_width_info = width_info - if self.get_width_info: - self.width_info = self.compute_width() - self.min_width = (np.min(self.width_info[0, :]), - np.min(self.width_info[3, :])) + # Compute path width (attribute of each waypoint) + self._compute_width(max_width=max_width) - def construct_path(self, wp_x, wp_y): + def _construct_path(self, wp_x, wp_y): + """ + Construct path from given waypoints. + :param wp_x: x coordinates of waypoints in global coordinates + :param wp_y: y coordinates of waypoints in global coordinates + :return: list of waypoint objects + """ # Number of waypoints n_wp = [int(np.sqrt((wp_x[i + 1] - wp_x[i]) ** 2 + @@ -78,7 +102,7 @@ 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] - # smooth path + # Smooth path wp_xs = [] wp_ys = [] for wp_id in range(self.smoothing_distance, len(wp_x) - @@ -88,147 +112,282 @@ class ReferencePath: wp_ys.append(np.mean(wp_y[wp_id - self.smoothing_distance:wp_id + self.smoothing_distance + 1])) + # Construct list of waypoint objects waypoints = list(zip(wp_xs, wp_ys)) - waypoints = self.spatial_reformulation(waypoints) + waypoints = self._construct_waypoints(waypoints) + return waypoints - def spatial_reformulation(self, waypoints): + def _construct_waypoints(self, waypoint_coordinates): """ Reformulate conventional waypoints (x, y) coordinates into waypoint - objects containing (x, y, psi, kappa) + objects containing (x, y, psi, kappa, ub, lb) + :param waypoint_coordinates: list of (x, y) coordinates of waypoints in + global coordinates :return: list of waypoint objects for entire reference path """ - waypoints_spatial = [] - for wp_id in range(len(waypoints) - 1): + # List containing waypoint objects + waypoints = [] - # get start and goal waypoints - current_wp = np.array(waypoints[wp_id]) - next_wp = np.array(waypoints[wp_id + 1]) + # Iterate over all waypoints + for wp_id in range(len(waypoint_coordinates) - 1): - # difference vector + # Get start and goal waypoints + current_wp = np.array(waypoint_coordinates[wp_id]) + next_wp = np.array(waypoint_coordinates[wp_id + 1]) + + # Difference vector dif_ahead = next_wp - current_wp - # angle ahead + # Angle ahead psi = np.arctan2(dif_ahead[1], dif_ahead[0]) - # distance to next waypoint + # Distance to next waypoint dist_ahead = np.linalg.norm(dif_ahead, 2) - # get x and y coordinates of current waypoint - x = current_wp[0] - y = current_wp[1] + # Get x and y coordinates of current waypoint + x, y = current_wp[0], current_wp[1] + # Compute local curvature at waypoint # first waypoint if wp_id == 0: kappa = 0 else: - prev_wp = np.array(waypoints[wp_id - 1]) + prev_wp = np.array(waypoint_coordinates[wp_id - 1]) dif_behind = current_wp - prev_wp angle_behind = np.arctan2(dif_behind[1], dif_behind[0]) angle_dif = np.mod(psi - angle_behind + math.pi, 2 * math.pi) \ - math.pi - kappa = np.abs(angle_dif / (dist_ahead + self.eps)) + kappa = angle_dif / (dist_ahead + self.eps) - waypoints_spatial.append(Waypoint(x, y, psi, kappa)) + waypoints.append(Waypoint(x, y, psi, kappa)) - return waypoints_spatial + return waypoints - def compute_width(self, max_dist=2.0): - max_dist = max_dist # m - width_info = np.zeros((6, len(self.waypoints))) + def _compute_width(self, max_width): + """ + Compute the width of the path by checking the maximum free space to + the left and right of the center-line. + :param max_width: maximum width of the path. + """ + + # Iterate over all waypoints for wp_id, wp in enumerate(self.waypoints): + # List containing information for current waypoint + width_info = [] + # Check width left and right of the center-line for i, dir in enumerate(['left', 'right']): - # get pixel coordinates of waypoint - wp_x, wp_y = self.map.w2m(wp.x, wp.y) - # get angle orthogonal to path in current direction + # Get angle orthogonal to path in current direction if dir == 'left': angle = np.mod(wp.psi + math.pi / 2 + math.pi, 2 * math.pi) - math.pi else: angle = np.mod(wp.psi - math.pi / 2 + math.pi, 2 * math.pi) - math.pi - # get closest cell to orthogonal vector - t_x, t_y = self.map.w2m(wp.x + max_dist * np.cos(angle), wp.y + max_dist * np.sin(angle)) - # compute path between cells - width_info[3*i:3*(i+1), wp_id] = self.get_min_dist(wp_x, wp_y, t_x, t_y, max_dist) - return width_info + # Get closest cell to orthogonal vector + t_x, t_y = self.map.w2m(wp.x + max_width * np.cos(angle), wp.y + + max_width * np.sin(angle)) + # Compute distance to orthogonal cell on path border + b_value, b_cell = self._get_min_width(wp, t_x, t_y, max_width) + # Add information to list for current waypoint + width_info.append(b_value) + width_info.append(b_cell) - def get_min_dist(self, wp_x, wp_y, t_x, t_y, max_dist): - # get neighboring cells (account for discretization) - neighbors_x, neighbors_y = [], [] + # Set waypoint attributes with width to the left and right + wp.ub = width_info[0] + wp.lb = -1 * width_info[2] # minus can be assumed as waypoints + # represent center-line of the path + # Set border cells of waypoint + wp.border_cells = (width_info[1], width_info[3]) + + def _get_min_width(self, wp, t_x, t_y, max_width): + """ + Compute the minimum distance between the current waypoint and the + orthogonal cell on the border of the path + :param wp: current waypoint + :param t_x: x coordinate of border cell in map coordinates + :param t_y: y coordinate of border cell in map coordinates + :param max_width: maximum path width in m + :return: min_width to border and corresponding cell + """ + + # Get neighboring cells of orthogonal cell (account for + # discretization inaccuracy) + tn_x, tn_y = [], [] for i in range(-1, 2, 1): for j in range(-1, 2, 1): - neighbors_x.append(t_x + i) - neighbors_y.append(t_y + j) + tn_x.append(t_x+i) + tn_y.append(t_y+j) - # get bresenham paths to all neighboring cells + # Get pixel coordinates of waypoint + wp_x, wp_y = self.map.w2m(wp.x, wp.y) + + # Get Bresenham paths to all possible cells paths = [] - for t_x, t_y in zip(neighbors_x, neighbors_y): + for t_x, t_y in zip(tn_x, tn_y): path = list(bresenham(wp_x, wp_y, t_x, t_y)) paths.append(path) - min_dist = max_dist + # Compute minimum distance to border cell + min_width = max_width + # map inspected cell to world coordinates min_cell = self.map.m2w(t_x, t_y) for path in paths: for cell in path: - t_x = cell[0] - t_y = cell[1] - # if path goes through occupied cell + t_x, t_y = cell[0], cell[1] + # If path goes through occupied cell if self.map.data[t_y, t_x] == 0: - # get world coordinates - x, y = self.map.m2w(wp_x, wp_y) + # Get world coordinates c_x, c_y = self.map.m2w(t_x, t_y) - cell_dist = np.sqrt((x - c_x) ** 2 + (y - c_y) ** 2) - if cell_dist < min_dist: - min_dist = cell_dist + cell_dist = np.sqrt((wp.x - c_x) ** 2 + (wp.y - c_y) ** 2) + if cell_dist < min_width: + min_width = cell_dist min_cell = (c_x, c_y) - dist_info = np.array([min_dist, min_cell[0], min_cell[1]]) - return dist_info - def show(self): + # decrease min_width by radius of circle around cell + min_width -= 1 / np.sqrt(2) * self.map.resolution - # plot map + return min_width, min_cell + + def update_bounds(self, wp_id): + """ + Compute upper and lower bounds of the drivable area orthogonal to + the given waypoint. + :param wp_id: ID of reference waypoint + """ + + # Get reference waypoint + wp = self.waypoints[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]) + lb_p = self.map.w2m(wp.border_cells[1][0], wp.border_cells[1][1]) + + # Compute path from left border cell to right border cell + path = list(bresenham(ub_p[0], ub_p[1], lb_p[0], lb_p[1])) + + # Initialize upper and lower bound of drivable area to + # upper bound of path + ub_o, lb_o = ub_p, ub_p + + # Initialize upper and lower bound of best segment to upper bound of + # path + ub_ls, lb_ls = ub_p, ub_p + + # Iterate over path from left border to right border + for x, y in path: + # If cell is free, update lower bound + if self.map.data[y, x] == 1: + lb_o = (x, y) + # If cell is occupied, end segment. Update best segment if current + # segment is larger than previous best segment. Then, reset upper + # and lower bound to current cell + elif self.map.data[y, x] == 0: + if np.sqrt((ub_o[0]-lb_o[0])**2+(ub_o[1]-lb_o[1])**2) > \ + np.sqrt((ub_ls[0]-lb_ls[0])**2+(ub_ls[1]-lb_ls[1])**2): + ub_ls = ub_o + lb_ls = lb_o + # Start new segment + ub_o = (x, y) + lb_o = (x, y) + + # If no segment was set (no obstacle between left and right border), + # return original bounds of path + if ub_ls == ub_p and lb_ls == ub_p: + return wp.lb, wp.ub + + # Transform upper and lower bound cells to world coordinates + ub_ls = self.map.m2w(ub_ls[0], ub_ls[1]) + lb_ls = self.map.m2w(lb_ls[0], lb_ls[1]) + # Check sign of upper and lower bound + angle_ub = np.mod(np.arctan2(ub_ls[1] - wp.y, ub_ls[0] - wp.x) + - wp.psi + math.pi, 2*math.pi) - math.pi + angle_lb = np.mod(np.arctan2(lb_ls[1] - wp.y, lb_ls[0] - wp.x) + - wp.psi + math.pi, 2*math.pi) - math.pi + sign_ub = np.sign(angle_ub) + sign_lb = np.sign(angle_lb) + # Compute upper and lower bound of largest drivable area + ub = sign_ub * np.sqrt((ub_ls[0]-wp.x)**2+(ub_ls[1]-wp.y)**2) + lb = sign_lb * np.sqrt((lb_ls[0]-wp.x)**2+(lb_ls[1]-wp.y)**2) + + # Update member variables of waypoint + wp.ub = ub + wp.lb = lb + wp.border_cells = (ub_ls, lb_ls) + + return lb, ub + + def show(self, display_drivable_area=True): + """ + Display path object on current figure. + :param display_drivable_area: If True, display arrows indicating width + of drivable area + """ + + # Clear figure plt.clf() - plt.imshow(np.flipud(self.map.data),cmap='gray', + + # Plot map in gray-scale and set extent to match world coordinates + plt.imshow(np.flipud(self.map.data), cmap='gray', extent=[self.map.origin[0], self.map.origin[0] + self.map.width * self.map.resolution, self.map.origin[1], self.map.origin[1] + self.map.height * self.map.resolution]) - # plot reference path + + # Get x and y coordinates for all waypoints wp_x = np.array([wp.x for wp in self.waypoints]) wp_y = np.array([wp.y for wp in self.waypoints]) - plt.scatter(wp_x, wp_y, color='k', s=5) - if self.get_width_info: - print('Min Width Left: {:f} m'.format(self.min_width[0])) - print('Min Width Right: {:f} m'.format(self.min_width[1])) - plt.quiver(wp_x, wp_y, self.width_info[1, :] - wp_x, - self.width_info[2, :] - wp_y, scale=1, units='xy', - width=0.05, color='#D4AC0D') - plt.quiver(wp_x, wp_y, self.width_info[4, :] - wp_x, - self.width_info[5, :] - wp_y, scale=1, units='xy', - width=0.05, color='#BA4A00') + # 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]) + 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]) + wp_lb_y = np.array([wp.border_cells[1][1] for wp in self.waypoints]) + + # Plot waypoints + plt.scatter(wp_x, wp_y, color='#99A3A4', s=3) + + # Plot arrows indicating drivable area + if display_drivable_area: + plt.quiver(wp_x, wp_y, wp_ub_x - wp_x, wp_ub_y - wp_y, scale=1, + units='xy', width=0.2*self.resolution, color='#2ECC71', + headwidth=1, headlength=2) + plt.quiver(wp_x, wp_y, wp_lb_x - wp_x, wp_lb_y - wp_y, scale=1, + units='xy', width=0.2*self.resolution, color='#2ECC71', + headwidth=1, headlength=2) if __name__ == '__main__': + # Select Path | 'Race' or 'Q' + path = 'Race' + # Create Map - map = Map(file_path='map_race.png', origin=[-1, -2], resolution=0.005) + 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 + reference_path = ReferencePath(map, wp_x, wp_y, path_resolution, + smoothing_distance=5, max_width=0.22) + 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] + # Specify path resolution + path_resolution = 0.20 # m / wp + reference_path = ReferencePath(map, wp_x, wp_y, path_resolution, + smoothing_distance=5, max_width=1.5) + else: + reference_path = None + print('Invalid path!') + exit(1) - # Specify waypoints - # Floor 2 - # wp_x = [-9.169, -2.7, 11.9, 7.3, -6.95] - # wp_y = [-15.678, -7.12, 10.9, 14.5, -3.31] - # Race Track - 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 - - # Smooth Path - reference_path = ReferencePath(map, wp_x, wp_y, path_resolution, - smoothing_distance=5) reference_path.show() plt.show() From a3b2b4d3f332dde3b916da8b1c4470c42d688e27 Mon Sep 17 00:00:00 2001 From: matssteinweg Date: Sun, 1 Dec 2019 01:58:14 +0100 Subject: [PATCH 03/36] Update m2w function. World coordinates are computed based on center of map cell. --- map.py | 20 +++++++------------- 1 file changed, 7 insertions(+), 13 deletions(-) diff --git a/map.py b/map.py index 12b6c4f..383220a 100644 --- a/map.py +++ b/map.py @@ -1,15 +1,7 @@ -#!/usr/bin/env python - -import sys -import os -import math import numpy as np import matplotlib.pyplot as plt from skimage.morphology import remove_small_holes from PIL import Image -dirname = os.path.dirname(__file__) -svea = os.path.join(dirname, '../../') -sys.path.append(os.path.abspath(svea)) class Map: @@ -24,7 +16,7 @@ class Map: self.threshold_occupied = threshold_occupied # instantiate member variables self.data = np.array(Image.open(file_path))[:, :, 0] # numpy array containing map data - #self.process_map() + self.process_map() self.height = self.data.shape[0] # height of the map in px self.width = self.data.shape[1] # width of the map in px self.resolution = resolution # resolution of the map in m/px @@ -52,14 +44,16 @@ class Map: :param y: y coordinate in global coordinate system :return: discrete x and y coordinates in px """ - x = dx * self.resolution + self.origin[0] - y = dy * self.resolution + self.origin[1] + x = (dx + 0.5) * self.resolution + self.origin[0] + y = (dy + 0.5) * self.resolution + self.origin[1] return x, y def process_map(self): - self.data = remove_small_holes(self.data, area_threshold=5, - connectivity=8).astype(np.int8) + #self.data = remove_small_holes(self.data, area_threshold=5, + # connectivity=8).astype(np.int8) + self.data = np.where(self.data >= 100, 1, 0) + if __name__ == '__main__': From 7940964b29c12b558b2ed864d1c4a306357a0ee7 Mon Sep 17 00:00:00 2001 From: matssteinweg Date: Sun, 1 Dec 2019 02:05:02 +0100 Subject: [PATCH 04/36] Change width of quiver plot to display drivable area instead of arrows. --- reference_path.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/reference_path.py b/reference_path.py index 80378ab..1659f82 100644 --- a/reference_path.py +++ b/reference_path.py @@ -351,17 +351,17 @@ class ReferencePath: # Plot arrows indicating drivable area if display_drivable_area: plt.quiver(wp_x, wp_y, wp_ub_x - wp_x, wp_ub_y - wp_y, scale=1, - units='xy', width=0.2*self.resolution, color='#2ECC71', - headwidth=1, headlength=2) + units='xy', width=1.5*self.resolution, color='#2ECC71', + headwidth=1, headlength=0) plt.quiver(wp_x, wp_y, wp_lb_x - wp_x, wp_lb_y - wp_y, scale=1, - units='xy', width=0.2*self.resolution, color='#2ECC71', - headwidth=1, headlength=2) + units='xy', width=1.5*self.resolution, color='#2ECC71', + headwidth=1, headlength=0) if __name__ == '__main__': # Select Path | 'Race' or 'Q' - path = 'Race' + path = 'Q' # Create Map if path == 'Race': From 6ec93febd3ab65118c392dc5e70f1aeee48b848f Mon Sep 17 00:00:00 2001 From: matssteinweg Date: Sun, 1 Dec 2019 15:38:17 +0100 Subject: [PATCH 05/36] Add obstacle class and function add_obstacles to add obstacles to path. Restructure class ReferencePath a bit. Add safety_margin to update_bounds function. --- reference_path.py | 133 +++++++++++++++++++++++++++++++++++++++++++--- 1 file changed, 125 insertions(+), 8 deletions(-) diff --git a/reference_path.py b/reference_path.py index 1659f82..e8fc8ed 100644 --- a/reference_path.py +++ b/reference_path.py @@ -3,6 +3,13 @@ import math from map import Map from bresenham import bresenham import matplotlib.pyplot as plt +import matplotlib.patches as plt_patches +from scipy.signal import savgol_filter + +# Colors +DRIVABLE_AREA = '#BDC3C7' +WAYPOINTS = '#D0D3D4' +OBSTACLE = '#2E4053' ############ @@ -41,13 +48,43 @@ 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) + ax = plt.gca() + ax.add_patch(circle) + + ################## # Reference Path # ################## class ReferencePath: - def __init__(self, map, wp_x, wp_y, resolution, smoothing_distance, max_width): + def __init__(self, map, wp_x, wp_y, resolution, smoothing_distance, + max_width): """ Reference Path object. Create a reference trajectory from specified corner points with given resolution. Smoothing around corners can be @@ -80,6 +117,9 @@ class ReferencePath: # Compute path width (attribute of each waypoint) self._compute_width(max_width=max_width) + # Obstacles on path + self.obstacles = list() + def _construct_path(self, wp_x, wp_y): """ Construct path from given waypoints. @@ -250,13 +290,17 @@ class ReferencePath: return min_width, min_cell - def update_bounds(self, wp_id): + def update_bounds(self, car, wp_id): """ Compute upper and lower bounds of the drivable area orthogonal to the given waypoint. + :param car: car model driving on the path :param wp_id: ID of reference waypoint """ + # Get car-dependent safety margin + safety_margin = car.safety_margin + # Get reference waypoint wp = self.waypoints[wp_id] @@ -311,6 +355,10 @@ class ReferencePath: ub = sign_ub * np.sqrt((ub_ls[0]-wp.x)**2+(ub_ls[1]-wp.y)**2) lb = sign_lb * np.sqrt((lb_ls[0]-wp.x)**2+(lb_ls[1]-wp.y)**2) + # Add safety margin to bounds + ub = ub - (safety_margin[1] + 2 * self.map.resolution) + lb = lb + (safety_margin[1] + 2 * self.map.resolution) + # Update member variables of waypoint wp.ub = ub wp.lb = lb @@ -318,6 +366,24 @@ class ReferencePath: return lb, ub + def add_obstacles(self, obstacles): + """ + Add obstacles to the path. + :param obstacles: list of obstacle objects + """ + + # Extend list of obstacles + self.obstacles.extend(obstacles) + + # Iterate over list of obstacles + for obstacle in obstacles: + radius_px = int(np.ceil(obstacle.radius / self.map.resolution)) + cx_px, cy_px = self.map.w2m(obstacle.cx, obstacle.cy) + y, x = np.ogrid[-radius_px: radius_px, -radius_px: radius_px] + index = x ** 2 + y ** 2 <= radius_px ** 2 + self.map.data[cy_px-radius_px:cy_px+radius_px, cx_px-radius_px: + cx_px+radius_px][index] = 0 + def show(self, display_drivable_area=True): """ Display path object on current figure. @@ -328,12 +394,18 @@ class ReferencePath: # Clear figure plt.clf() + # Disabled ticks + plt.xticks([]) + plt.yticks([]) + # Plot map in gray-scale and set extent to match world coordinates - plt.imshow(np.flipud(self.map.data), cmap='gray', + canvas = np.ones(self.map.data.shape) + plt.imshow(canvas, cmap='gray', extent=[self.map.origin[0], self.map.origin[0] + self.map.width * self.map.resolution, self.map.origin[1], self.map.origin[1] + - self.map.height * self.map.resolution]) + self.map.height * self.map.resolution], vmin=0.0, + vmax=1.0) # Get x and y coordinates for all waypoints wp_x = np.array([wp.x for wp in self.waypoints]) @@ -346,22 +418,56 @@ class ReferencePath: wp_lb_y = np.array([wp.border_cells[1][1] for wp in self.waypoints]) # Plot waypoints - plt.scatter(wp_x, wp_y, color='#99A3A4', s=3) + plt.scatter(wp_x, wp_y, color=WAYPOINTS, s=3) # Plot arrows indicating drivable area if display_drivable_area: plt.quiver(wp_x, wp_y, wp_ub_x - wp_x, wp_ub_y - wp_y, scale=1, - units='xy', width=1.5*self.resolution, color='#2ECC71', + units='xy', width=0.2*self.resolution, color=DRIVABLE_AREA, headwidth=1, headlength=0) plt.quiver(wp_x, wp_y, wp_lb_x - wp_x, wp_lb_y - wp_y, scale=1, - units='xy', width=1.5*self.resolution, color='#2ECC71', + units='xy', width=0.2*self.resolution, color=DRIVABLE_AREA, headwidth=1, headlength=0) + # Plot border of path + bl_x = np.array([wp.border_cells[0][0] for wp in + self.waypoints] + + [self.waypoints[0].border_cells[0][0]]) + bl_y = np.array([wp.border_cells[0][1] for wp in + self.waypoints] + + [self.waypoints[0].border_cells[0][1]]) + br_x = np.array([wp.border_cells[1][0] for wp in + self.waypoints] + + [self.waypoints[0].border_cells[1][0]]) + br_y = np.array([wp.border_cells[1][1] for wp in + self.waypoints] + + [self.waypoints[0].border_cells[1][1]]) + # Smooth border + # bl_x = savgol_filter(bl_x, 15, 9) + # bl_y = savgol_filter(bl_y, 15, 9) + # br_x = savgol_filter(br_x, 15, 9) + # br_y = savgol_filter(br_y, 15, 9) + + # If circular path, connect start and end point + if np.abs(self.waypoints[-1] - self.waypoints[0]) <= 2*self.resolution: + plt.plot(bl_x, bl_y, color=OBSTACLE) + plt.plot(br_x, br_y, color=OBSTACLE) + # If not circular, close path at start and end + else: + plt.plot(bl_x[:-1], bl_y[:-1], color=OBSTACLE) + plt.plot(br_x[:-1], br_y[:-1], color=OBSTACLE) + plt.plot((bl_x[-2],br_x[-2]), (bl_y[-2], br_y[-2]), color=OBSTACLE) + plt.plot((bl_x[0],br_x[0]), (bl_y[0], br_y[0]), color=OBSTACLE) + + # Plot obstacles + for obstacle in self.obstacles: + obstacle.show() + if __name__ == '__main__': # Select Path | 'Race' or 'Q' - path = 'Q' + path = 'Race' # Create Map if path == 'Race': @@ -388,6 +494,17 @@ if __name__ == '__main__': print('Invalid path!') exit(1) + obs1 = Obstacle(cx=0.0, cy=0.0, radius=0.05) + obs2 = Obstacle(cx=-0.8, cy=-0.5, radius=0.05) + obs3 = Obstacle(cx=-0.7, cy=-1.5, radius=0.05) + obs4 = Obstacle(cx=-0.3, cy=-1.0, radius=0.05) + obs5 = Obstacle(cx=0.3, cy=-1.0, radius=0.05) + obs6 = Obstacle(cx=0.75, cy=-1.5, radius=0.05) + obs7 = Obstacle(cx=0.7, cy=-0.9, radius=0.05) + obs8 = Obstacle(cx=1.2, cy=0.0, radius=0.05) + reference_path.add_obstacles([obs1, obs2, obs3, obs4, obs5, obs6, obs7, + obs8]) + reference_path.show() plt.show() From bbae14c519c81ceea8d8f93c50b3c0dd1c59ed9f Mon Sep 17 00:00:00 2001 From: matssteinweg Date: Sun, 1 Dec 2019 16:02:15 +0100 Subject: [PATCH 06/36] Add length, width and safety_margin to bicycle model ABC. Add show function to display car with safety margin ellipsoid. --- spatial_bicycle_models.py | 67 +++++++++++++++++++++++++++++++++------ 1 file changed, 57 insertions(+), 10 deletions(-) diff --git a/spatial_bicycle_models.py b/spatial_bicycle_models.py index 1f1b24f..9fc6582 100644 --- a/spatial_bicycle_models.py +++ b/spatial_bicycle_models.py @@ -1,6 +1,11 @@ import numpy as np from abc import ABC, abstractmethod +import matplotlib.pyplot as plt +import matplotlib.patches as plt_patches +# Colors +CAR = '#F1C40F' +CAR_OUTLINE = '#B7950B' ######################### # Temporal State Vector # @@ -81,7 +86,7 @@ class SimpleSpatialState(SpatialState): #################################### class SpatialBicycleModel(ABC): - def __init__(self, reference_path): + def __init__(self, reference_path, length, width): """ Abstract Base Class for Spatial Reformulation of Bicycle Model. :param reference_path: reference path object to follow @@ -90,6 +95,11 @@ class SpatialBicycleModel(ABC): # Precision self.eps = 1e-12 + # Car Parameters + self.l = length + self.w = width + self.safety_margin = self._compute_safety_margin() + # Reference Path self.reference_path = reference_path @@ -192,6 +202,47 @@ class SpatialBicycleModel(ABC): return state + def _compute_safety_margin(self): + """ + Compute safety margin for car if modeled by its center of gravity. + """ + + # Model ellipsoid around the car + length = self.l / np.sqrt(2) + width = self.w / np.sqrt(2) + + return length, width + + def show(self): + """ + Display car on current axis. + """ + + # Get car's center of gravity + cog = (self.temporal_state.x, self.temporal_state.y) + # Get current angle with respect to x-axis + yaw = np.rad2deg(self.temporal_state.psi) + # Draw rectangle + car = plt_patches.Rectangle(cog, width=self.l, height=self.w, + angle=yaw, facecolor=CAR, edgecolor=CAR_OUTLINE) + + # Shift center rectangle to match center of the car + car.set_x(car.get_x() - (self.l/2 * np.cos(self.temporal_state.psi) - + self.w/2 * np.sin(self.temporal_state.psi))) + car.set_y(car.get_y() - (self.w/2 * np.cos(self.temporal_state.psi) + + self.l/2 * np.sin(self.temporal_state.psi))) + + # Show safety margin + safety_margin = plt_patches.Ellipse(cog, width=2*self.safety_margin[0], + height=2*self.safety_margin[1], + angle=yaw, + fill=False, edgecolor=CAR) + + # Add rectangle to current axis + ax = plt.gca() + ax.add_patch(safety_margin) + ax.add_patch(car) + @abstractmethod def get_spatial_derivatives(self, state, input, kappa): pass @@ -206,21 +257,20 @@ class SpatialBicycleModel(ABC): ################# class BicycleModel(SpatialBicycleModel): - def __init__(self, reference_path, e_y, e_psi, t): + def __init__(self, length, width, reference_path, e_y, e_psi, t): """ Simplified Spatial Bicycle Model. Spatial Reformulation of Kinematic Bicycle Model. Uses Simplified Spatial State. + :param length: length of the car in m + :param width: with of the car in m :param reference_path: reference path model is supposed to follow :param e_y: deviation from reference path | [m] :param e_psi: heading offset from reference path | [rad] - :param v: initial velocity | [m/s] """ # Initialize base class - super(BicycleModel, self).__init__(reference_path) - - # Constants - self.l = 0.06 + super(BicycleModel, self).__init__(reference_path, length=length, + width=width) # Initialize spatial state self.spatial_state = SimpleSpatialState(e_y, e_psi, t) @@ -231,9 +281,6 @@ class BicycleModel(SpatialBicycleModel): # Initialize temporal state self.temporal_state = self.s2t() - # Compute linear system matrices | Used for MPC - # self.A, self.B = self.linearize() - def s2t(self, reference_waypoint=None, reference_state=None): """ Convert spatial state to temporal state. Either convert self.spatial_ From 12329708a760adc1195057e6987c0560608ab3be Mon Sep 17 00:00:00 2001 From: matssteinweg Date: Sun, 1 Dec 2019 16:04:18 +0100 Subject: [PATCH 07/36] Update and declutter simulation script. Add obstacles for Race path. --- simulation.py | 66 +++++++++++++++++++++++++++++++-------------------- 1 file changed, 40 insertions(+), 26 deletions(-) diff --git a/simulation.py b/simulation.py index 1142a81..df8ba01 100644 --- a/simulation.py +++ b/simulation.py @@ -1,11 +1,10 @@ from map import Map import numpy as np -from reference_path import ReferencePath +from reference_path import ReferencePath, Obstacle 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__': @@ -23,20 +22,36 @@ if __name__ == '__main__': -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.22) 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 + # Create smoothed reference path + reference_path = ReferencePath(map, wp_x, wp_y, path_resolution, + smoothing_distance=5, max_width=1.50) else: print('Invalid Simulation Mode!') - map, wp_x, wp_y, path_resolution = None, None, None, None + map, wp_x, wp_y, path_resolution, reference_path \ + = None, None, None, None, None exit(1) - # Create smoothed reference path - reference_path = ReferencePath(map, wp_x, wp_y, path_resolution, - smoothing_distance=5) + obs1 = Obstacle(cx=0.0, cy=0.0, radius=0.05) + obs2 = Obstacle(cx=-0.8, cy=-0.5, radius=0.05) + obs3 = Obstacle(cx=-0.7, cy=-1.5, radius=0.07) + obs4 = Obstacle(cx=-0.3, cy=-1.0, radius=0.07) + obs5 = Obstacle(cx=0.3, cy=-1.0, radius=0.05) + obs6 = Obstacle(cx=0.75, cy=-1.5, radius=0.07) + obs7 = Obstacle(cx=0.7, cy=-0.9, radius=0.08) + obs8 = Obstacle(cx=1.2, cy=0.0, radius=0.08) + obs9 = Obstacle(cx=0.7, cy=-0.1, radius=0.05) + obs10 = Obstacle(cx=1.1, cy=-0.6, radius=0.07) + reference_path.add_obstacles([obs1, obs2, obs3, obs4, obs5, obs6, obs7, + obs8, obs9, obs10]) ################ # Motion Model # @@ -48,19 +63,21 @@ if __name__ == '__main__': 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) + 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 = 20 - Q = sparse.diags([0.01, 0.0, 0.4]) - R = sparse.diags([0.01]) + N = 30 + Q = sparse.diags([1.0, 0.0, 0.1]) + R = sparse.diags([0.0001]) 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])} + InputConstraints = {'umin': np.array([-np.tan(0.66)/car.l]), + 'umax': np.array([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_OSQP(car, N, Q, R, QN, StateConstraints, InputConstraints) ############## @@ -72,13 +89,10 @@ if __name__ == '__main__': y_log = [car.temporal_state.y] # iterate over waypoints - for wp_id in range(len(car.reference_path.waypoints)-mpc.N-1): + for wp_id in range(len(car.reference_path.waypoints)-N-1): # get control signals - start = time.time() - delta = mpc.get_control(v) - end = time.time() - u = np.array([v, delta]) + u = mpc.get_control(v) # drive car car.drive(u) @@ -90,17 +104,17 @@ if __name__ == '__main__': ################### # 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) + # Plot path and drivable area + reference_path.show() - plt.scatter(mpc.current_prediction[0], mpc.current_prediction[1], c='b', s=5) + # Plot MPC prediction + mpc.show_prediction() + + # Plot car + car.show() 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.pause(0.05) plt.close() From 296d1db0305e268fa82ebb3d667c5a44e22d4e5c Mon Sep 17 00:00:00 2001 From: matssteinweg Date: Sun, 1 Dec 2019 16:05:25 +0100 Subject: [PATCH 08/36] Add dynamic constraints on e_y via update_bounds method. Add show_prediction function to display prediction. --- MPC.py | 38 ++++++++++++++++++++++++++++++-------- 1 file changed, 30 insertions(+), 8 deletions(-) diff --git a/MPC.py b/MPC.py index c54cbbc..d14f891 100644 --- a/MPC.py +++ b/MPC.py @@ -3,6 +3,10 @@ import cvxpy as cp import osqp import scipy as sp from scipy import sparse +import matplotlib.pyplot as plt + +# Colors +PREDICTION = '#BA4A00' ################## # MPC Controller # @@ -110,8 +114,9 @@ class MPC: self.current_prediction = self.update_prediction(self.x.value) delta = np.arctan(self.u.value[0, 0] * self.model.l) + u = np.array([v, delta]) - return delta + return u def update_prediction(self, spatial_state_prediction): """ @@ -200,6 +205,10 @@ class MPC_OSQP: # Offset for equality constraint (due to B * (u - ur)) uq = np.zeros(self.N * nx) + # Dynamic state constraints + xmin_dyn = np.kron(np.ones(self.N + 1), xmin) + xmax_dyn = np.kron(np.ones(self.N + 1), xmax) + # Iterate over horizon for n in range(self.N): @@ -220,6 +229,10 @@ class MPC_OSQP: ur[n] = kappa_r # Compute equality constraint offset (B*ur) uq[n * nx:n * nx + nx] = B_lin[:, 0] * kappa_r + lb, ub = self.model.reference_path.update_bounds(self.model, + self.model.wp_id + n) + xmin_dyn[nx * n] = lb + xmax_dyn[nx * n] = ub # Get equality matrix Ax = sparse.kron(sparse.eye(self.N + 1), @@ -232,10 +245,10 @@ class MPC_OSQP: A = sparse.vstack([Aeq, Aineq], format='csc') # Get upper and lower bound vectors for equality constraints - lineq = np.hstack([np.kron(np.ones(self.N + 1), xmin), - np.kron(np.ones(self.N), umin)]) - uineq = np.hstack([np.kron(np.ones(self.N + 1), xmax), - np.kron(np.ones(self.N), umax)]) + lineq = np.hstack([xmin_dyn, + np.kron(np.ones(self.N), umin)]) + uineq = np.hstack([xmax_dyn, + np.kron(np.ones(self.N), umax)]) # Get upper and lower bound vectors for inequality constraints x0 = np.array(self.model.spatial_state[:]) leq = np.hstack([-x0, uq]) @@ -270,8 +283,9 @@ class MPC_OSQP: # Solve optimization problem dec = self.optimizer.solve() x = np.reshape(dec.x[:(self.N+1)*nx], (self.N+1, nx)) - u = np.arctan(dec.x[-self.N] * self.model.l) - self.current_prediction = self.update_prediction(u, x) + delta = np.arctan(dec.x[-self.N] * self.model.l) + self.current_prediction = self.update_prediction(delta, x) + u = np.array([v, delta]) return u @@ -289,7 +303,7 @@ class MPC_OSQP: # get current waypoint ID print('#########################') - for n in range(self.N): + for n in range(2, self.N): associated_waypoint = self.model.reference_path.waypoints[self.model.wp_id+n] predicted_temporal_state = self.model.s2t(associated_waypoint, spatial_state_prediction[n, :]) @@ -304,3 +318,11 @@ class MPC_OSQP: return x_pred, y_pred + def show_prediction(self): + """ + Display predicted car trajectory in current axis. + """ + + plt.scatter(self.current_prediction[0], self.current_prediction[1], + c=PREDICTION, s=5) + From bf90a57adc260af70a543bbe47c6b0f7871e527b Mon Sep 17 00:00:00 2001 From: matssteinweg Date: Sun, 1 Dec 2019 16:40:04 +0100 Subject: [PATCH 09/36] Update MPC controller to use previously predicted control signal in case of infeasibility. If all predicted control signal exhausted and problem infeasible, stop execution --- MPC.py | 43 +++++++++++++++++++++++++++++++++++++++---- 1 file changed, 39 insertions(+), 4 deletions(-) diff --git a/MPC.py b/MPC.py index d14f891..c818bf0 100644 --- a/MPC.py +++ b/MPC.py @@ -178,6 +178,12 @@ class MPC_OSQP: # Current control and prediction self.current_prediction = None + # Counter for old control signals in case of infeasible problem + self.infeasibility_counter = 0 + + # Current control signals + self.current_control = None + # Initialize Optimization Problem self.optimizer = osqp.OSQP() @@ -282,10 +288,39 @@ class MPC_OSQP: # Solve optimization problem dec = self.optimizer.solve() - x = np.reshape(dec.x[:(self.N+1)*nx], (self.N+1, nx)) - delta = np.arctan(dec.x[-self.N] * self.model.l) - self.current_prediction = self.update_prediction(delta, x) - u = np.array([v, delta]) + + try: + # Get control signals + deltas = np.arctan(dec.x[-self.N:] * self.model.l) + delta = deltas[0] + + # Update control signals + self.current_control = deltas + + # 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) + + # Get current control signal + u = np.array([v, delta]) + + # if problem solved, reset infeasibility counter + self.infeasibility_counter = 0 + + except: + + print('Infeasible problem. Previously predicted' + ' control signal used!') + u = np.array([v, self.current_control + [self.infeasibility_counter+1]]) + + # increase infeasibility counter + self.infeasibility_counter += 1 + + if self.infeasibility_counter == (self.N - 1): + print('No control signal computed!') + exit(1) return u From 73d968c2f2d1e66b9581e66aad108854520ef887 Mon Sep 17 00:00:00 2001 From: matssteinweg Date: Sun, 1 Dec 2019 21:38:12 +0100 Subject: [PATCH 10/36] Remove MPC controller based on cvxpy --- MPC.py | 140 +-------------------------------------------------------- 1 file changed, 1 insertion(+), 139 deletions(-) diff --git a/MPC.py b/MPC.py index c818bf0..4113962 100644 --- a/MPC.py +++ b/MPC.py @@ -1,7 +1,5 @@ import numpy as np -import cvxpy as cp import osqp -import scipy as sp from scipy import sparse import matplotlib.pyplot as plt @@ -12,6 +10,7 @@ PREDICTION = '#BA4A00' # MPC Controller # ################## + class MPC: def __init__(self, model, N, Q, R, QN, StateConstraints, InputConstraints): """ @@ -23,143 +22,6 @@ class MPC: :param QN: final state cost matrix :param StateConstraints: dictionary of state constraints :param InputConstraints: dictionary of input constraints - :param Reference: reference values for state variables - """ - - # Parameters - self.N = N # horizon - self.Q = Q # weight matrix state vector - self.R = R # weight matrix input vector - self.QN = QN # weight matrix terminal - - # Model - self.model = model - - # Constraints - self.state_constraints = StateConstraints - self.input_constraints = InputConstraints - - # Current control and prediction - self.current_prediction = None - - # Initialize Optimization Problem - self.problem = self._init_problem() - - def _init_problem(self): - """ - Initialize parametrized optimization problem to be solved at each - time step. - """ - - # number of input and state variables - nx = self.model.n_states - nu = 1 - - # system matrices - self.A = cp.Parameter(shape=(nx, nx*self.N)) - self.B = cp.Parameter(shape=(nx, nu*self.N)) - self.A.value = np.zeros(self.A.shape) - self.B.value = np.zeros(self.B.shape) - - # reference values - xr = np.array([0., 0., -1.0]) - self.ur = cp.Parameter((nu, self.N)) - self.ur.value = np.zeros(self.ur.shape) - - # constraints - umin = self.input_constraints['umin'] - umax = self.input_constraints['umax'] - xmin = self.state_constraints['xmin'] - xmax = self.state_constraints['xmax'] - - # initial state - self.x_init = cp.Parameter(self.model.n_states) - - # Define problem - self.u = cp.Variable((nu, self.N)) - self.x = cp.Variable((nx, self.N + 1)) - objective = 0 - constraints = [self.x[:, 0] == self.x_init] - for n in range(self.N): - objective += cp.quad_form(self.x[:, n] - xr, self.Q) + cp.quad_form(self.u[:, n] - self.ur[:, n], self.R) - constraints += [self.x[:, n + 1] == self.A[:, n*nx:n*nx+nx] * self.x[:, n] - + self.B[:, n*nu] * (self.u[:, n] - self.ur[:, n])] - constraints += [umin <= self.u[:, n], self.u[:, n] <= umax] - objective += cp.quad_form(self.x[:, self.N] - xr, self.QN) - constraints += [xmin <= self.x[:, self.N], self.x[:, self.N] <= xmax] - problem = cp.Problem(cp.Minimize(objective), constraints) - - return problem - - def get_control(self, v): - """ - Get control signal given the current position of the car. Solves a - finite time optimization problem based on the linearized car model. - """ - - nx = self.model.n_states - nu = 1 - - for n in range(self.N): - current_waypoint = self.model.reference_path.waypoints[self.model.wp_id+n] - next_waypoint = self.model.reference_path.waypoints[ - self.model.wp_id + n + 1] - delta_s = next_waypoint - current_waypoint - kappa_r = current_waypoint.kappa - self.A.value[:, n*nx:n*nx+nx], self.B.value[:, n*nu:n*nu+nu] = self.model.linearize(v, kappa_r, delta_s) - self.ur.value[:, n] = kappa_r - - self.x_init.value = np.array(self.model.spatial_state[:]) - self.problem.solve(solver=cp.OSQP, verbose=True) - - self.current_prediction = self.update_prediction(self.x.value) - delta = np.arctan(self.u.value[0, 0] * self.model.l) - u = np.array([v, delta]) - - return u - - def update_prediction(self, spatial_state_prediction): - """ - Transform the predicted states to predicted x and y coordinates. - Mainly for visualization purposes. - :param spatial_state_prediction: list of predicted state variables - :return: lists of predicted x and y coordinates - """ - - # containers for x and y coordinates of predicted states - x_pred, y_pred = [], [] - - # get current waypoint ID - print('#########################') - - for n in range(self.N): - associated_waypoint = self.model.reference_path.waypoints[self.model.wp_id+n] - predicted_temporal_state = self.model.s2t(associated_waypoint, - spatial_state_prediction[:, n]) - print('delta: ', np.arctan(self.u.value[0, n] * self.model.l)) - print('e_y: ', spatial_state_prediction[0, n]) - print('e_psi: ', spatial_state_prediction[1, n]) - print('t: ', spatial_state_prediction[2, n]) - print('+++++++++++++++++++++++') - - x_pred.append(predicted_temporal_state.x) - y_pred.append(predicted_temporal_state.y) - - return x_pred, y_pred - - -class MPC_OSQP: - def __init__(self, model, N, Q, R, QN, StateConstraints, InputConstraints): - """ - Constructor for the Model Predictive Controller. - :param model: bicycle model object to be controlled - :param T: 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 Reference: reference values for state variables """ # Parameters From e6f006e515ce01db3133e135ecf3021a3e7362b6 Mon Sep 17 00:00:00 2001 From: matssteinweg Date: Sun, 1 Dec 2019 21:38:53 +0100 Subject: [PATCH 11/36] Add exemplary obstacles for Q path --- reference_path.py | 31 +++++++++++++++++++------------ 1 file changed, 19 insertions(+), 12 deletions(-) diff --git a/reference_path.py b/reference_path.py index e8fc8ed..b7cfde4 100644 --- a/reference_path.py +++ b/reference_path.py @@ -467,7 +467,7 @@ class ReferencePath: if __name__ == '__main__': # Select Path | 'Race' or 'Q' - path = 'Race' + path = 'Q' # Create Map if path == 'Race': @@ -481,6 +481,18 @@ if __name__ == '__main__': path_resolution = 0.05 # m / wp reference_path = ReferencePath(map, wp_x, wp_y, path_resolution, smoothing_distance=5, max_width=0.22) + # Add obstacles + obs1 = Obstacle(cx=0.0, cy=0.0, radius=0.05) + obs2 = Obstacle(cx=-0.8, cy=-0.5, radius=0.05) + obs3 = Obstacle(cx=-0.7, cy=-1.5, radius=0.05) + obs4 = Obstacle(cx=-0.3, cy=-1.0, radius=0.05) + obs5 = Obstacle(cx=0.3, cy=-1.0, radius=0.05) + obs6 = Obstacle(cx=0.75, cy=-1.5, radius=0.05) + obs7 = Obstacle(cx=0.7, cy=-0.9, radius=0.05) + obs8 = Obstacle(cx=1.2, cy=0.0, radius=0.05) + reference_path.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] @@ -489,22 +501,17 @@ if __name__ == '__main__': path_resolution = 0.20 # m / wp reference_path = ReferencePath(map, wp_x, wp_y, path_resolution, smoothing_distance=5, max_width=1.5) + obs1 = Obstacle(cx=-6.3, cy=-11.1, radius=0.20) + obs2 = Obstacle(cx=-2.2, cy=-6.8, radius=0.25) + obs3 = Obstacle(cx=1.7, cy=-1.0, radius=0.15) + obs4 = Obstacle(cx=2.0, cy=-1.2, radius=0.25) + reference_path.add_obstacles([obs1, obs2, obs3, obs4]) + else: reference_path = None print('Invalid path!') exit(1) - obs1 = Obstacle(cx=0.0, cy=0.0, radius=0.05) - obs2 = Obstacle(cx=-0.8, cy=-0.5, radius=0.05) - obs3 = Obstacle(cx=-0.7, cy=-1.5, radius=0.05) - obs4 = Obstacle(cx=-0.3, cy=-1.0, radius=0.05) - obs5 = Obstacle(cx=0.3, cy=-1.0, radius=0.05) - obs6 = Obstacle(cx=0.75, cy=-1.5, radius=0.05) - obs7 = Obstacle(cx=0.7, cy=-0.9, radius=0.05) - obs8 = Obstacle(cx=1.2, cy=0.0, radius=0.05) - reference_path.add_obstacles([obs1, obs2, obs3, obs4, obs5, obs6, obs7, - obs8]) - reference_path.show() plt.show() From 447bdf9f412258cf3dc04f138d5c44c6f12106a5 Mon Sep 17 00:00:00 2001 From: matssteinweg Date: Mon, 2 Dec 2019 00:12:22 +0100 Subject: [PATCH 12/36] MPC updates current waypoint and transforms temporal state into spatial state before computing current control signal --- MPC.py | 20 ++++++++++++++------ 1 file changed, 14 insertions(+), 6 deletions(-) diff --git a/MPC.py b/MPC.py index 4113962..b7cb144 100644 --- a/MPC.py +++ b/MPC.py @@ -2,6 +2,7 @@ import numpy as np import osqp from scipy import sparse import matplotlib.pyplot as plt +from time import time # Colors PREDICTION = '#BA4A00' @@ -145,6 +146,12 @@ class MPC: # Number of state variables nx = self.model.n_states + # Update current waypoint + self.model.get_current_waypoint() + + # Update spatial state + self.model.spatial_state = self.model.t2s() + # Initialize optimization problem self._init_problem(v) @@ -198,17 +205,18 @@ class MPC: x_pred, y_pred = [], [] # get current waypoint ID - print('#########################') + #print('#########################') for n in range(2, self.N): associated_waypoint = self.model.reference_path.waypoints[self.model.wp_id+n] predicted_temporal_state = self.model.s2t(associated_waypoint, spatial_state_prediction[n, :]) - 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('+++++++++++++++++++++++') + + #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('+++++++++++++++++++++++') x_pred.append(predicted_temporal_state.x) y_pred.append(predicted_temporal_state.y) From db90eb1055e040b346058631c8e6ba281c84df21 Mon Sep 17 00:00:00 2001 From: matssteinweg Date: Mon, 2 Dec 2019 00:13:41 +0100 Subject: [PATCH 13/36] Add attributes n_extension and circular. n_extensions specifies number of waypoints to be added in the end in order to allow for MPC control signals for last waypoints. Add function _compute_length to get length of paths and be able to get a waypoint based on the traveled distance s. --- reference_path.py | 61 ++++++++++++++++++++++++++++++++++++----------- 1 file changed, 47 insertions(+), 14 deletions(-) diff --git a/reference_path.py b/reference_path.py index b7cfde4..83c8fe8 100644 --- a/reference_path.py +++ b/reference_path.py @@ -84,7 +84,7 @@ class Obstacle: class ReferencePath: def __init__(self, map, wp_x, wp_y, resolution, smoothing_distance, - max_width): + max_width, n_extension, circular): """ Reference Path object. Create a reference trajectory from specified corner points with given resolution. Smoothing around corners can be @@ -97,6 +97,9 @@ 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 """ # Precision @@ -111,9 +114,18 @@ 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) + # Length of path + self.length, self.segment_lengths = self._compute_length() + # Compute path width (attribute of each waypoint) self._compute_width(max_width=max_width) @@ -142,6 +154,14 @@ 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 = [] @@ -205,6 +225,17 @@ class ReferencePath: return waypoints + def _compute_length(self): + """ + Compute length of center-line path as sum of euclidean distance between + waypoints. + :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)] + s = sum(segment_lengths) + return s, segment_lengths + def _compute_width(self, max_width): """ Compute the width of the path by checking the maximum free space to @@ -408,14 +439,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]) - wp_y = np.array([wp.y for wp in self.waypoints]) + wp_x = np.array([wp.x for wp in self.waypoints][:-self.n_extension]) + wp_y = np.array([wp.y for wp in self.waypoints][:-self.n_extension]) # 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]) - 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]) - wp_lb_y = np.array([wp.border_cells[1][1] for wp in self.waypoints]) + wp_ub_x = np.array([wp.border_cells[0][0] for wp in self.waypoints][:-self.n_extension]) + wp_ub_y = np.array([wp.border_cells[0][1] for wp in self.waypoints][:-self.n_extension]) + wp_lb_x = np.array([wp.border_cells[1][0] for wp in self.waypoints][:-self.n_extension]) + wp_lb_y = np.array([wp.border_cells[1][1] for wp in self.waypoints][:-self.n_extension]) # Plot waypoints plt.scatter(wp_x, wp_y, color=WAYPOINTS, s=3) @@ -431,16 +462,16 @@ class ReferencePath: # Plot border of path bl_x = np.array([wp.border_cells[0][0] for wp in - self.waypoints] + + self.waypoints][:-self.n_extension] + [self.waypoints[0].border_cells[0][0]]) bl_y = np.array([wp.border_cells[0][1] for wp in - self.waypoints] + + self.waypoints][:-self.n_extension] + [self.waypoints[0].border_cells[0][1]]) br_x = np.array([wp.border_cells[1][0] for wp in - self.waypoints] + + self.waypoints][:-self.n_extension] + [self.waypoints[0].border_cells[1][0]]) br_y = np.array([wp.border_cells[1][1] for wp in - self.waypoints] + + self.waypoints][:-self.n_extension] + [self.waypoints[0].border_cells[1][1]]) # Smooth border # bl_x = savgol_filter(bl_x, 15, 9) @@ -449,7 +480,7 @@ class ReferencePath: # br_y = savgol_filter(br_y, 15, 9) # If circular path, connect start and end point - if np.abs(self.waypoints[-1] - self.waypoints[0]) <= 2*self.resolution: + if self.circular: plt.plot(bl_x, bl_y, color=OBSTACLE) plt.plot(br_x, br_y, color=OBSTACLE) # If not circular, close path at start and end @@ -480,7 +511,8 @@ if __name__ == '__main__': # Specify path resolution path_resolution = 0.05 # m / wp reference_path = ReferencePath(map, wp_x, wp_y, path_resolution, - smoothing_distance=5, max_width=0.22) + smoothing_distance=5, max_width=0.22, n_extension=30, + 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.05) @@ -500,7 +532,8 @@ if __name__ == '__main__': # Specify path resolution path_resolution = 0.20 # m / wp reference_path = ReferencePath(map, wp_x, wp_y, path_resolution, - smoothing_distance=5, max_width=1.5) + smoothing_distance=5, max_width=1.5, + n_extension=30, circular=False) obs1 = Obstacle(cx=-6.3, cy=-11.1, radius=0.20) obs2 = Obstacle(cx=-2.2, cy=-6.8, radius=0.25) obs3 = Obstacle(cx=1.7, cy=-1.0, radius=0.15) From 15fa9f5c3369f701799117d147f4ff2c710114fb Mon Sep 17 00:00:00 2001 From: matssteinweg Date: Mon, 2 Dec 2019 00:15:30 +0100 Subject: [PATCH 14/36] Add function t2s to transform temporal state into spatial state. Modify drive function to work in temporal domain. Add function get_current_waypoint to set the waypoint of the reference path based on the traveled distance s of the car. --- spatial_bicycle_models.py | 137 ++++++++++++++++++++++++++------------ 1 file changed, 95 insertions(+), 42 deletions(-) diff --git a/spatial_bicycle_models.py b/spatial_bicycle_models.py index 9fc6582..e307456 100644 --- a/spatial_bicycle_models.py +++ b/spatial_bicycle_models.py @@ -2,6 +2,7 @@ import numpy as np from abc import ABC, abstractmethod import matplotlib.pyplot as plt import matplotlib.patches as plt_patches +import math # Colors CAR = '#F1C40F' @@ -25,6 +26,16 @@ class TemporalState: self.y = y self.psi = psi + def __iadd__(self, other): + """ + Overload Sum-Add operator. + :param other: numpy array to be added to state vector + """ + + for state_id, state in enumerate(vars(self).values()): + vars(self)[list(vars(self).keys())[state_id]] += other[state_id] + return self + ######################## # Spatial State Vector # @@ -106,6 +117,9 @@ class SpatialBicycleModel(ABC): # Set initial distance traveled self.s = 0.0 + # Set sampling time to None (Initialization required) + self.sampling_time = None + # Set initial waypoint ID self.wp_id = 0 @@ -150,57 +164,57 @@ class SpatialBicycleModel(ABC): return x, y, psi - def drive(self, input, state=None, kappa=None, delta_s=None): + def t2s(self): + """ + Convert spatial state to temporal state. Either convert self.spatial_ + state with current waypoint as reference or provide reference waypoint + and reference_state. + :return x, y, psi + """ + + # compute temporal state variables + e_y = np.cos(self.current_waypoint.psi) * \ + (self.temporal_state.y - self.current_waypoint.y) - \ + np.sin(self.current_waypoint.psi) * (self.temporal_state.x - + self.current_waypoint.x) + e_psi = self.temporal_state.psi - self.current_waypoint.psi + e_psi = np.mod(e_psi + math.pi, 2*math.pi) - math.pi + t = 0 + + return SimpleSpatialState(e_y, e_psi, t) + + def set_sampling_time(self, Ts): + """ + Set sampling time of bicycle model. + :param Ts: sampling time in s + """ + self.Ts = Ts + + def drive(self, u): """ Drive. - :param state: state vector for which to compute derivatives - :param input: input vector - :param kappa: curvature of corresponding waypoint + :param u: input vector :return: numpy array with spatial derivatives for all state variables """ - # Get spatial derivatives - if state is None and kappa is None and delta_s is None: - state = np.array(self.spatial_state[:]) - # Get delta_s | distance to next waypoint - next_waypoint = self.reference_path.waypoints[self.wp_id + 1] - delta_s = next_waypoint - self.current_waypoint - # Get current curvature - kappa = self.current_waypoint.kappa + # Get input signals + v, delta = u - spatial_derivatives = self.get_spatial_derivatives(state, input, kappa) + # Compute temporal state derivatives + x_dot = v * np.cos(self.temporal_state.psi) + y_dot = v * np.sin(self.temporal_state.psi) + psi_dot = v / self.l * np.tan(delta) + temporal_derivatives = np.array([x_dot, y_dot, psi_dot]) - # Update spatial state (Forward Euler Approximation) - self.spatial_state += spatial_derivatives * delta_s + # Update spatial state (Forward Euler Approximation) + self.temporal_state += temporal_derivatives * self.Ts - # Assert that unique projections of car pose onto path exists - #assert self.spatial_state.e_y < (1 / (self.current_waypoint.kappa + - # self.eps)) + # Compute velocity along path + s_dot = 1 / (1 - self.spatial_state.e_y * self.current_waypoint.kappa) \ + * v * np.cos(self.spatial_state.e_psi) - # Increase waypoint ID - self.wp_id += 1 - - # Update current waypoint - self.current_waypoint = self.reference_path.waypoints[self.wp_id] - - # Update temporal_state to match spatial state - self.temporal_state = self.s2t() - - # Update s | total driven distance along path - self.s += delta_s - - # Linearize model around new operating point - # self.A, self.B = self.linearize() - - else: - - spatial_derivatives = self.get_spatial_derivatives(state, input, - kappa) - - # Update spatial state (Forward Euler Approximation) - state += spatial_derivatives * delta_s - - return state + # Update distance travelled along reference path + self.s += s_dot * self.Ts def _compute_safety_margin(self): """ @@ -213,6 +227,45 @@ class SpatialBicycleModel(ABC): return length, width + def get_current_waypoint(self): + """ + Create waypoint on reference path at current location of car by + interpolation information from given path waypoints. + """ + + # Compute cumulative path length + length_cum = np.cumsum(self.reference_path.segment_lengths) + # Get first index with distance larger than distance traveled by car + # so far + greater_than_threshold = length_cum > self.s + next_wp_id = greater_than_threshold.searchsorted(True) + # Get previous index for interpolation + prev_wp_id = next_wp_id - 1 + + # Get distance traveled for both enclosing waypoints + s_next = length_cum[next_wp_id] + s_prev = length_cum[prev_wp_id] + + if np.abs(self.s - s_next) < np.abs(self.s - s_prev): + self.wp_id = next_wp_id + self.current_waypoint = self.reference_path.waypoints[next_wp_id] + else: + self.wp_id = prev_wp_id + self.current_waypoint = self.reference_path.waypoints[prev_wp_id] + # + # # Weight for next waypoint + # w = (s_next - self.s) / (s_next - s_prev) + # + # # Interpolate between the two waypoints + # prev_wp = self.reference_path.waypoints[prev_wp_id] + # next_wp = self.reference_path.waypoints[next_wp_id] + # x = w * next_wp.x + (1 - w) * prev_wp.x + # y = w * next_wp.y + (1 - w) * prev_wp.y + # psi = w * next_wp.psi + (1 - w) * prev_wp.psi + # kappa = w * next_wp.kappa + (1 - w) * prev_wp.kappa + + + def show(self): """ Display car on current axis. From 9b73084e092bbcf196d16b41ed922f636f67ad2c Mon Sep 17 00:00:00 2001 From: matssteinweg Date: Mon, 2 Dec 2019 09:48:34 +0100 Subject: [PATCH 15/36] Update simulation.py --- simulation.py | 36 ++++++++++++++++++++++++------------ 1 file changed, 24 insertions(+), 12 deletions(-) diff --git a/simulation.py b/simulation.py index df8ba01..d20bc39 100644 --- a/simulation.py +++ b/simulation.py @@ -3,7 +3,7 @@ import numpy as np from reference_path import ReferencePath, Obstacle from spatial_bicycle_models import BicycleModel import matplotlib.pyplot as plt -from MPC import MPC, MPC_OSQP +from MPC import MPC from scipy import sparse @@ -24,7 +24,8 @@ if __name__ == '__main__': 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.22) + smoothing_distance=5, max_width=0.22, + n_extension=50, circular=True) elif sim_mode == 'Q': map = Map(file_path='map_floor2.png') wp_x = [-9.169, 11.9, 7.3, -6.95] @@ -33,7 +34,8 @@ if __name__ == '__main__': 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) + smoothing_distance=5, max_width=1.50, + n_extension=50, circular=False) else: print('Invalid Simulation Mode!') map, wp_x, wp_y, path_resolution, reference_path \ @@ -71,25 +73,30 @@ if __name__ == '__main__': ############## N = 30 - Q = sparse.diags([1.0, 0.0, 0.1]) - R = sparse.diags([0.0001]) + Q = sparse.diags([1.0, 0.0, 0.0]) + R = sparse.diags([0.01]) QN = Q InputConstraints = {'umin': np.array([-np.tan(0.66)/car.l]), 'umax': np.array([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_OSQP(car, N, Q, R, QN, StateConstraints, InputConstraints) + mpc = MPC(car, N, Q, R, QN, StateConstraints, InputConstraints) ############## # Simulation # ############## - # logging containers + # Sampling time + Ts = 0.05 + t = 0 + car.set_sampling_time(Ts) + + # 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)-N-1): + # Until arrival at end of path + while car.s < reference_path.length: # get control signals u = mpc.get_control(v) @@ -114,7 +121,12 @@ if __name__ == '__main__': # Plot car car.show() - plt.title('MPC Simulation: Position: {:.2f} m, {:.2f} m'. - format(car.temporal_state.x, car.temporal_state.y)) - plt.pause(0.05) + t += Ts + + plt.title('MPC Simulation: Distance: {:.2f}m/{:.2f} m, Duration: {:.2f} s'. + format(car.s, car.reference_path.length, t)) + if t == Ts: + plt.show() + plt.pause(0.0001) + print('Final Time: {:.2f}'.format(t)) plt.close() From b1c49f987e56d32b9f7f3301c0731b89c272daab Mon Sep 17 00:00:00 2001 From: arne Date: Mon, 2 Dec 2019 19:23:10 +0100 Subject: [PATCH 16/36] compatibility with Python 2.7 --- map.py | 2 +- reference_path.py | 10 +++++----- spatial_bicycle_models.py | 35 ++++++++++++++++++++++++++--------- 3 files changed, 32 insertions(+), 15 deletions(-) diff --git a/map.py b/map.py index 383220a..bbb5c24 100644 --- a/map.py +++ b/map.py @@ -1,6 +1,6 @@ import numpy as np import matplotlib.pyplot as plt -from skimage.morphology import remove_small_holes +# from skimage.morphology import remove_small_holes from PIL import Image diff --git a/reference_path.py b/reference_path.py index 83c8fe8..35014db 100644 --- a/reference_path.py +++ b/reference_path.py @@ -1,7 +1,7 @@ import numpy as np import math from map import Map -from bresenham import bresenham +from skimage.draw import line import matplotlib.pyplot as plt import matplotlib.patches as plt_patches from scipy.signal import savgol_filter @@ -297,8 +297,8 @@ class ReferencePath: # Get Bresenham paths to all possible cells paths = [] for t_x, t_y in zip(tn_x, tn_y): - path = list(bresenham(wp_x, wp_y, t_x, t_y)) - paths.append(path) + x_list, y_list = line(wp_x, wp_y, t_x, t_y) + paths.append(zip(x_list, y_list)) # Compute minimum distance to border cell min_width = max_width @@ -340,7 +340,7 @@ class ReferencePath: lb_p = self.map.w2m(wp.border_cells[1][0], wp.border_cells[1][1]) # Compute path from left border cell to right border cell - path = list(bresenham(ub_p[0], ub_p[1], lb_p[0], lb_p[1])) + x_list, y_list = line(ub_p[0], ub_p[1], lb_p[0], lb_p[1]) # Initialize upper and lower bound of drivable area to # upper bound of path @@ -351,7 +351,7 @@ class ReferencePath: ub_ls, lb_ls = ub_p, ub_p # Iterate over path from left border to right border - for x, y in path: + for x, y in zip(x_list, y_list): # If cell is free, update lower bound if self.map.data[y, x] == 1: lb_o = (x, y) diff --git a/spatial_bicycle_models.py b/spatial_bicycle_models.py index e307456..74181f8 100644 --- a/spatial_bicycle_models.py +++ b/spatial_bicycle_models.py @@ -1,5 +1,13 @@ import numpy as np -from abc import ABC, abstractmethod +from abc import abstractmethod +try: + from abc import ABC +except: + # for Python 2.7 + from abc import ABCMeta + class ABC(object): + __metaclass__ = ABCMeta + pass import matplotlib.pyplot as plt import matplotlib.patches as plt_patches import math @@ -26,14 +34,16 @@ class TemporalState: self.y = y self.psi = psi + self.members = ['x', 'y', 'psi'] + def __iadd__(self, other): """ Overload Sum-Add operator. :param other: numpy array to be added to state vector """ - for state_id, state in enumerate(vars(self).values()): - vars(self)[list(vars(self).keys())[state_id]] += other[state_id] + for state_id in range(len(self.members)): + vars(self)[self.members[state_id]] += other[state_id] return self @@ -48,16 +58,21 @@ class SpatialState(ABC): @abstractmethod def __init__(self): + self.members = None pass def __getitem__(self, item): - return list(vars(self).values())[item] + if isinstance(item, int): + members = [self.members[item]] + else: + members = self.members[item] + return [vars(self)[key] for key in members] def __setitem__(self, key, value): - vars(self)[list(vars(self).keys())[key]] = value + vars(self)[self.members[key]] = value def __len__(self): - return len(vars(self)) + return len(self.members) def __iadd__(self, other): """ @@ -65,15 +80,15 @@ class SpatialState(ABC): :param other: numpy array to be added to state vector """ - for state_id, state in enumerate(vars(self).values()): - vars(self)[list(vars(self).keys())[state_id]] += other[state_id] + for state_id in range(len(self.members)): + vars(self)[self.members[state_id]] += other[state_id] return self def list_states(self): """ Return list of names of all states. """ - return list(vars(self).keys()) + return self.members class SimpleSpatialState(SpatialState): @@ -91,6 +106,8 @@ class SimpleSpatialState(SpatialState): self.e_psi = e_psi self.t = t + self.members = ['e_y', 'e_psi', 't'] + #################################### # Spatial Bicycle Model Base Class # From 120367902d17cd0f9387d23f505a547695893022 Mon Sep 17 00:00:00 2001 From: matssteinweg Date: Wed, 4 Dec 2019 14:12:33 +0100 Subject: [PATCH 17/36] update width computation. Border cell now updated to current upper and lower bound --- reference_path.py | 51 +++++++++++++++++++++++++++-------------------- 1 file changed, 29 insertions(+), 22 deletions(-) diff --git a/reference_path.py b/reference_path.py index 35014db..f7c81ec 100644 --- a/reference_path.py +++ b/reference_path.py @@ -316,22 +316,16 @@ class ReferencePath: min_width = cell_dist min_cell = (c_x, c_y) - # decrease min_width by radius of circle around cell - min_width -= 1 / np.sqrt(2) * self.map.resolution - return min_width, min_cell - def update_bounds(self, car, wp_id): + def update_bounds(self, wp_id, safety_margin): """ Compute upper and lower bounds of the drivable area orthogonal to the given waypoint. - :param car: car model driving on the path + :param safety_margin: safety margin of the car orthogonal to path in m :param wp_id: ID of reference waypoint """ - # Get car-dependent safety margin - safety_margin = car.safety_margin - # Get reference waypoint wp = self.waypoints[wp_id] @@ -351,14 +345,14 @@ class ReferencePath: ub_ls, lb_ls = ub_p, ub_p # Iterate over path from left border to right border - for x, y in zip(x_list, y_list): + for x, y in zip(x_list[1:], y_list[1:]): # If cell is free, update lower bound if self.map.data[y, x] == 1: lb_o = (x, y) # If cell is occupied, end segment. Update best segment if current # segment is larger than previous best segment. Then, reset upper - # and lower bound to current cell - elif self.map.data[y, x] == 0: + # and lower bound to current cell. + if self.map.data[y, x] == 0 or (x, y) == lb_p: if np.sqrt((ub_o[0]-lb_o[0])**2+(ub_o[1]-lb_o[1])**2) > \ np.sqrt((ub_ls[0]-lb_ls[0])**2+(ub_ls[1]-lb_ls[1])**2): ub_ls = ub_o @@ -367,14 +361,10 @@ class ReferencePath: ub_o = (x, y) lb_o = (x, y) - # If no segment was set (no obstacle between left and right border), - # return original bounds of path - if ub_ls == ub_p and lb_ls == ub_p: - return wp.lb, wp.ub - # Transform upper and lower bound cells to world coordinates ub_ls = self.map.m2w(ub_ls[0], ub_ls[1]) lb_ls = self.map.m2w(lb_ls[0], lb_ls[1]) + # Check sign of upper and lower bound angle_ub = np.mod(np.arctan2(ub_ls[1] - wp.y, ub_ls[0] - wp.x) - wp.psi + math.pi, 2*math.pi) - math.pi @@ -382,13 +372,27 @@ class ReferencePath: - wp.psi + math.pi, 2*math.pi) - math.pi sign_ub = np.sign(angle_ub) sign_lb = np.sign(angle_lb) + # Compute upper and lower bound of largest drivable area ub = sign_ub * np.sqrt((ub_ls[0]-wp.x)**2+(ub_ls[1]-wp.y)**2) lb = sign_lb * np.sqrt((lb_ls[0]-wp.x)**2+(lb_ls[1]-wp.y)**2) - # Add safety margin to bounds - ub = ub - (safety_margin[1] + 2 * self.map.resolution) - lb = lb + (safety_margin[1] + 2 * self.map.resolution) + # Add safety margin (attribute of car) to bounds + ub = ub - safety_margin + lb = lb + safety_margin + + # Check feasibility of the path + if ub < lb: + # Upper and lower bound of 0 indicate an infeasible path + # given the specified safety margin + ub, lb = 0.0, 0.0 + + # Compute absolute angle of bound cell + angle_ub = np.mod(math.pi/2 + wp.psi + math.pi, 2 * math.pi) - math.pi + angle_lb = np.mod(-math.pi/2 + wp.psi + math.pi, 2 * math.pi) - math.pi + # Compute cell on bound for computed distance ub and lb + 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 @@ -431,6 +435,7 @@ class ReferencePath: # Plot map in gray-scale and set extent to match world coordinates canvas = np.ones(self.map.data.shape) + canvas = np.flipud(self.map.data) plt.imshow(canvas, cmap='gray', extent=[self.map.origin[0], self.map.origin[0] + self.map.width * self.map.resolution, @@ -487,8 +492,8 @@ class ReferencePath: else: plt.plot(bl_x[:-1], bl_y[:-1], color=OBSTACLE) plt.plot(br_x[:-1], br_y[:-1], color=OBSTACLE) - plt.plot((bl_x[-2],br_x[-2]), (bl_y[-2], br_y[-2]), color=OBSTACLE) - plt.plot((bl_x[0],br_x[0]), (bl_y[0], br_y[0]), color=OBSTACLE) + plt.plot((bl_x[-2], br_x[-2]), (bl_y[-2], br_y[-2]), color=OBSTACLE) + plt.plot((bl_x[0], br_x[0]), (bl_y[0], br_y[0]), color=OBSTACLE) # Plot obstacles for obstacle in self.obstacles: @@ -535,7 +540,7 @@ if __name__ == '__main__': smoothing_distance=5, max_width=1.5, n_extension=30, circular=False) obs1 = Obstacle(cx=-6.3, cy=-11.1, radius=0.20) - obs2 = Obstacle(cx=-2.2, cy=-6.8, radius=0.25) + obs2 = Obstacle(cx=-2.2, cy=-6.8, radius=1.75) obs3 = Obstacle(cx=1.7, cy=-1.0, radius=0.15) obs4 = Obstacle(cx=2.0, cy=-1.2, radius=0.25) reference_path.add_obstacles([obs1, obs2, obs3, obs4]) @@ -545,6 +550,8 @@ if __name__ == '__main__': print('Invalid path!') exit(1) + [reference_path.update_bounds(wp_id=wp_id, safety_margin=0.02) + for wp_id in range(len(reference_path.waypoints))] reference_path.show() plt.show() From a5f5fd9f8e6b3084a9f84d9f693ab6d0894f35c7 Mon Sep 17 00:00:00 2001 From: matssteinweg Date: Wed, 4 Dec 2019 23:28:29 +0100 Subject: [PATCH 18/36] Add lidar model --- lidar_model.py | 148 +++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 148 insertions(+) create mode 100644 lidar_model.py diff --git a/lidar_model.py b/lidar_model.py new file mode 100644 index 0000000..82e8c68 --- /dev/null +++ b/lidar_model.py @@ -0,0 +1,148 @@ +from map import Map +import matplotlib.pyplot as plt +import numpy as np +import math +import time + +SCAN = '#5DADE2' + + +class LidarModel: + """ + Lidar Model + """ + def __init__(self, FoV, range, resolution): + """ + Constructor for Lidar object. + :param FoV: Sensor's Field of View in ° + :param range: range in m + :param resolution: resolution in ° + """ + + # set sensor parameters + self.FoV = FoV + self.range = range + self.resolution = resolution + + # number of measurements + self.n_measurements = int(self.FoV/self.resolution + 1) + + # construct measurement container + angles = np.linspace(-math.pi / 360 * self.FoV, + math.pi / 360 * self.FoV, + self.n_measurements) + ranges = np.ones(self.n_measurements) * self.range + self.measurements = np.stack((angles, ranges), axis=0) + + def scan(self, car, map): + """ + Get a Lidar Scan estimate + :param car: state containing x and y coordinates of the sensor + :param map: map object + :return: self with updated self.measurements + """ + + start = time.time() + # reset measurements + self.measurements[1, :] = np.ones(self.n_measurements) * self.range + + # flip map upside-down to allow for normal indexing of y axis + #map.data = np.flipud(map.data) + + # get sensor's map pose + x, y = map.w2m(car.x, car.y) + # get center of mass + xc = x + 0.5 + yc = y + 0.5 + + # get sensor range in px values + range_px = int(self.range / map.resolution) + + # iterate over area within sensor's range + for i in range(x - range_px, x + range_px + 1): + if 0 <= i < map.width: + for j in range(y - range_px, y + range_px + 1): + if 0 <= j < map.height: + # if obstacle detected + if map.data[j, i] == 0: + + # get center of mass of cell + xc_target = i + 0.5 + yc_target = j + 0.5 + + # check all corner's of cell + cell_angles = [] + for k in range(-1, 2): + for l in range(-1, 2): + dy = yc_target + l/2 - yc + dx = xc_target + k/2 - xc + cell_angle = np.arctan2(dy, dx) - car.psi + if cell_angle < - math.pi: + cell_angle = -np.mod(math.pi+cell_angle, 2*math.pi) + math.pi + else: + cell_angle = np.mod(math.pi+cell_angle, 2*math.pi) - math.pi + cell_angles.append(cell_angle) + + # get min and max angle hitting respective cell + min_angle = np.min(cell_angles) + max_angle = np.max(cell_angles) + + # get distance to mass center of cell + cell_distance = np.sqrt( + (xc - xc_target)**2 + (yc - yc_target)**2) + + # get IDs of all laser beams hitting cell + valid_beam_ids = [] + if min_angle < -math.pi/2 and max_angle > math.pi/2: + for beam_id in range(self.n_measurements): + if max_angle <= self.measurements[0, beam_id] <= min_angle: + valid_beam_ids.append(beam_id) + else: + for beam_id in range(self.n_measurements): + if min_angle <= self.measurements[0, beam_id] <= max_angle: + valid_beam_ids.append(beam_id) + + # update distance for all valid laser beams + for beam_id in valid_beam_ids: + if cell_distance < self.measurements[1, beam_id] / map.resolution: + self.measurements[1, beam_id] = cell_distance * map.resolution + + #map.data = np.flipud(map.data) + end = time.time() + print('Time elapsed: ', end - start) + + def plot_scan(self, car): + """ + Display current sensor measurements. + :param car: state containing x and y coordinate of sensor + """ + + start = time.time() + # get beam endpoints + beam_end_x = self.measurements[1, :] * np.cos(self.measurements[0, :] + car.psi) + beam_end_y = self.measurements[1, :] * np.sin(self.measurements[0, :] + car.psi) + + # plot all laser beams + for i in range(self.n_measurements): + plt.plot((car.x, car.x+beam_end_x[i]), (car.y, car.y+beam_end_y[i]), c=SCAN) + end = time.time() + print('Time elapsed: ', end - start) + + +if __name__ == '__main__': + + # Create Map + map = Map('map_floor2.png') + plt.imshow(map.data, cmap='gray', + extent=[map.origin[0], map.origin[0] + + map.width * map.resolution, + map.origin[1], map.origin[1] + + map.height * map.resolution]) + + car = BicycleModel(x=-4.9, y=-5.0, yaw=0.9) + sensor = LidarModel(FoV=180, range=5, resolution=1) + sensor.scan(car, map) + sensor.plot_scan(car) + + plt.axis('equal') + plt.show() From 3c305e46d49c7bc65f66a6a50551e6379d23196e Mon Sep 17 00:00:00 2001 From: matssteinweg Date: Wed, 4 Dec 2019 23:28:57 +0100 Subject: [PATCH 19/36] modify safety margin in update bounds --- MPC.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/MPC.py b/MPC.py index b7cb144..ac12f6c 100644 --- a/MPC.py +++ b/MPC.py @@ -98,8 +98,8 @@ class MPC: ur[n] = kappa_r # Compute equality constraint offset (B*ur) uq[n * nx:n * nx + nx] = B_lin[:, 0] * kappa_r - lb, ub = self.model.reference_path.update_bounds(self.model, - self.model.wp_id + n) + lb, ub = self.model.reference_path.update_bounds( + self.model.wp_id + n, self.model.safety_margin[1]) xmin_dyn[nx * n] = lb xmax_dyn[nx * n] = ub From 6b09b9ddc9f41c53035722546687958498b6d2d5 Mon Sep 17 00:00:00 2001 From: matssteinweg Date: Wed, 4 Dec 2019 23:29:22 +0100 Subject: [PATCH 20/36] Remove update of member variables in update bounds --- reference_path.py | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/reference_path.py b/reference_path.py index f7c81ec..357df29 100644 --- a/reference_path.py +++ b/reference_path.py @@ -395,9 +395,9 @@ class ReferencePath: 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) + #wp.ub = ub + #wp.lb = lb + #wp.border_cells = (ub_ls, lb_ls) return lb, ub @@ -435,7 +435,7 @@ class ReferencePath: # Plot map in gray-scale and set extent to match world coordinates canvas = np.ones(self.map.data.shape) - canvas = np.flipud(self.map.data) + # canvas = np.flipud(self.map.data) plt.imshow(canvas, cmap='gray', extent=[self.map.origin[0], self.map.origin[0] + self.map.width * self.map.resolution, @@ -503,7 +503,7 @@ class ReferencePath: if __name__ == '__main__': # Select Path | 'Race' or 'Q' - path = 'Q' + path = 'Race' # Create Map if path == 'Race': @@ -540,7 +540,7 @@ if __name__ == '__main__': smoothing_distance=5, max_width=1.5, n_extension=30, circular=False) obs1 = Obstacle(cx=-6.3, cy=-11.1, radius=0.20) - obs2 = Obstacle(cx=-2.2, cy=-6.8, radius=1.75) + obs2 = Obstacle(cx=-2.2, cy=-6.8, radius=0.25) obs3 = Obstacle(cx=1.7, cy=-1.0, radius=0.15) obs4 = Obstacle(cx=2.0, cy=-1.2, radius=0.25) reference_path.add_obstacles([obs1, obs2, obs3, obs4]) From 26c367d8806346439d1463afb6699849c6f26d4c Mon Sep 17 00:00:00 2001 From: matssteinweg Date: Wed, 4 Dec 2019 23:29:37 +0100 Subject: [PATCH 21/36] include lidar model --- simulation.py | 22 ++++++++++++++++++++-- 1 file changed, 20 insertions(+), 2 deletions(-) diff --git a/simulation.py b/simulation.py index d20bc39..cdd8422 100644 --- a/simulation.py +++ b/simulation.py @@ -5,6 +5,8 @@ from spatial_bicycle_models import BicycleModel import matplotlib.pyplot as plt from MPC import MPC from scipy import sparse +from time import time +from lidar_model import LidarModel if __name__ == '__main__': @@ -24,7 +26,7 @@ if __name__ == '__main__': 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.22, + smoothing_distance=5, max_width=0.23, n_extension=50, circular=True) elif sim_mode == 'Q': map = Map(file_path='map_floor2.png') @@ -82,6 +84,12 @@ if __name__ == '__main__': 'xmax': np.array([np.inf, np.inf, np.inf])} mpc = MPC(car, N, Q, R, QN, StateConstraints, InputConstraints) + ######### + # LiDAR # + ######### + + sensor = LidarModel(FoV=90, range=0.25, resolution=4.0) + ############## # Simulation # ############## @@ -99,11 +107,17 @@ if __name__ == '__main__': while car.s < reference_path.length: # get control signals + start = time() 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) @@ -115,6 +129,9 @@ 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() @@ -123,7 +140,8 @@ if __name__ == '__main__': t += Ts - plt.title('MPC Simulation: Distance: {:.2f}m/{:.2f} m, Duration: {:.2f} s'. + plt.title('MPC Simulation: Distance: {:.2f}m/{:.2f} m, Duration: ' + '{:.2f} s'. format(car.s, car.reference_path.length, t)) if t == Ts: plt.show() From c16b14f3cfdd54dc55f123b1f9667d02940d9b84 Mon Sep 17 00:00:00 2001 From: matssteinweg Date: Wed, 4 Dec 2019 23:29:53 +0100 Subject: [PATCH 22/36] Plot car on top of lidar scan --- spatial_bicycle_models.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/spatial_bicycle_models.py b/spatial_bicycle_models.py index 74181f8..48eb160 100644 --- a/spatial_bicycle_models.py +++ b/spatial_bicycle_models.py @@ -294,7 +294,7 @@ class SpatialBicycleModel(ABC): yaw = np.rad2deg(self.temporal_state.psi) # Draw rectangle car = plt_patches.Rectangle(cog, width=self.l, height=self.w, - angle=yaw, facecolor=CAR, edgecolor=CAR_OUTLINE) + angle=yaw, facecolor=CAR, edgecolor=CAR_OUTLINE, zorder=20) # Shift center rectangle to match center of the car car.set_x(car.get_x() - (self.l/2 * np.cos(self.temporal_state.psi) - @@ -306,7 +306,7 @@ class SpatialBicycleModel(ABC): safety_margin = plt_patches.Ellipse(cog, width=2*self.safety_margin[0], height=2*self.safety_margin[1], angle=yaw, - fill=False, edgecolor=CAR) + fill=False, edgecolor=CAR, zorder=20) # Add rectangle to current axis ax = plt.gca() From d8d596bb14778746c1a36d11479c09694815ce00 Mon Sep 17 00:00:00 2001 From: matssteinweg Date: Fri, 6 Dec 2019 10:45:20 +0100 Subject: [PATCH 23/36] Accumulate time in prediction horizon --- MPC.py | 6 +++--- simulation.py | 2 +- spatial_bicycle_models.py | 6 ++++-- 3 files changed, 8 insertions(+), 6 deletions(-) diff --git a/MPC.py b/MPC.py index ac12f6c..0a54163 100644 --- a/MPC.py +++ b/MPC.py @@ -90,14 +90,14 @@ class MPC: kappa_r = current_waypoint.kappa # Compute LTV matrices - A_lin, B_lin = self.model.linearize(v, kappa_r, delta_s) + f, A_lin, B_lin = self.model.linearize(v, kappa_r, delta_s) A[nx + n * nx:n * nx + 2 * nx, n * nx:n * nx + nx] = A_lin B[nx + n * nx:n * nx + 2 * nx, n * nu:n * nu + nu] = B_lin # Set kappa_r to reference for input signal ur[n] = kappa_r # Compute equality constraint offset (B*ur) - uq[n * nx:n * nx + nx] = B_lin[:, 0] * kappa_r + uq[n * nx:n * nx + nx] = B_lin[:, 0] * kappa_r - f lb, ub = self.model.reference_path.update_bounds( self.model.wp_id + n, self.model.safety_margin[1]) xmin_dyn[nx * n] = lb @@ -211,7 +211,7 @@ class MPC: associated_waypoint = self.model.reference_path.waypoints[self.model.wp_id+n] 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]) diff --git a/simulation.py b/simulation.py index cdd8422..510b318 100644 --- a/simulation.py +++ b/simulation.py @@ -77,7 +77,7 @@ if __name__ == '__main__': N = 30 Q = sparse.diags([1.0, 0.0, 0.0]) R = sparse.diags([0.01]) - QN = Q + QN = sparse.diags([0.0, 0.0, 1.0]) InputConstraints = {'umin': np.array([-np.tan(0.66)/car.l]), 'umax': np.array([np.tan(0.66)/car.l])} StateConstraints = {'xmin': np.array([-np.inf, -np.inf, -np.inf]), diff --git a/spatial_bicycle_models.py b/spatial_bicycle_models.py index 48eb160..f133716 100644 --- a/spatial_bicycle_models.py +++ b/spatial_bicycle_models.py @@ -434,13 +434,15 @@ class BicycleModel(SpatialBicycleModel): # Construct Jacobian Matrix a_1 = np.array([1, delta_s, 0]) a_2 = np.array([-kappa_r**2*delta_s, 1, 0]) - a_3 = np.array([-kappa_r/v*delta_s, 0, 0]) + a_3 = np.array([-kappa_r/v*delta_s, 0, 1]) b_1 = np.array([0, ]) b_2 = np.array([delta_s, ]) b_3 = np.array([0, ]) + f = np.array([0.0, 0.0, 1/v*delta_s]) + A = np.stack((a_1, a_2, a_3), axis=0) B = np.stack((b_1, b_2, b_3), axis=0) - return A, B \ No newline at end of file + return f, A, B \ No newline at end of file From 7e7ff06029c4931b2eff1f8fc6f1d135b01d68b9 Mon Sep 17 00:00:00 2001 From: matssteinweg Date: Fri, 6 Dec 2019 21:32:36 +0100 Subject: [PATCH 24/36] velocity as mpc input --- MPC.py | 84 +++++++++++++++++++++------------------ reference_path.py | 60 +++++++++++++--------------- simulation.py | 37 +++++++---------- spatial_bicycle_models.py | 6 +-- 4 files changed, 91 insertions(+), 96 deletions(-) diff --git a/MPC.py b/MPC.py index 0a54163..61d548f 100644 --- a/MPC.py +++ b/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))) - B = np.zeros((nx * (self.N + 1), nu * (self.N))) + A = np.zeros((self.nx * (self.N + 1), self.nx * (self.N + 1))) + B = np.zeros((self.nx * (self.N + 1), self.nu * (self.N))) # Reference vector for state and input variables - ur = np.zeros(self.N) - xr = np.array([0.0, 0.0, -1.0]) + ur = np.zeros(self.nu*self.N) + 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[ - self.model.wp_id + n] - next_waypoint = self.model.reference_path.waypoints[ - self.model.wp_id + n + 1] + current_waypoint = self.model.reference_path.get_waypoint(self.model.wp_id + n) + next_waypoint = self.model.reference_path.get_waypoint(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) - A[nx + n * nx:n * nx + 2 * nx, n * nx:n * nx + nx] = A_lin - B[nx + n * nx:n * nx + 2 * nx, n * nu:n * nu + nu] = B_lin + f, A_lin, B_lin = self.model.linearize(self.v_ref, kappa_ref, delta_s) + A[(n+1) * self.nx: (n+2)*self.nx, n * self.nx:(n+1)*self.nx] = A_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 - ur[n] = kappa_r + # Set reference for input signal + 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 - xmax_dyn[nx * n] = ub + xmin_dyn[self.nx * n] = lb + 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) - delta = deltas[0] + control_signals = np.array(dec.x[-self.N*nu:]) + 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 - [self.infeasibility_counter+1]]) + id = nu * (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]) diff --git a/reference_path.py b/reference_path.py index 357df29..475bada 100644 --- a/reference_path.py +++ b/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_y = np.array([wp.y 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]) # 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_y = np.array([wp.border_cells[0][1] for wp in self.waypoints][:-self.n_extension]) - wp_lb_x = np.array([wp.border_cells[1][0] for wp in self.waypoints][:-self.n_extension]) - wp_lb_y = np.array([wp.border_cells[1][1] 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]) + 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]) # 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': diff --git a/simulation.py b/simulation.py index 510b318..4d36677 100644 --- a/simulation.py +++ b/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]) - QN = sparse.diags([0.0, 0.0, 1.0]) - InputConstraints = {'umin': np.array([-np.tan(0.66)/car.l]), - 'umax': np.array([np.tan(0.66)/car.l])} + R = sparse.diags([1.0, 0.0]) + QN = sparse.diags([0.0, 0.0, 0.0]) + InputConstraints = {'umin': np.array([0.0, -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(v) - end = time() - print('Control time: ', end-start) + u = mpc.get_control() # 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)) - if t == Ts: - plt.show() - plt.pause(0.0001) + format(u[0], u[1], t)) + + plt.pause(0.01) print('Final Time: {:.2f}'.format(t)) plt.close() diff --git a/spatial_bicycle_models.py b/spatial_bicycle_models.py index f133716..85be2cb 100644 --- a/spatial_bicycle_models.py +++ b/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_2 = np.array([delta_s, ]) - b_3 = np.array([0, ]) + b_1 = np.array([0, 0]) + b_2 = np.array([0, delta_s]) + b_3 = np.array([-1/(v**2)*delta_s, 0]) f = np.array([0.0, 0.0, 1/v*delta_s]) From 4e9959a1b11277c3536c8904fc0cac4e6455d110 Mon Sep 17 00:00:00 2001 From: matssteinweg Date: Sat, 7 Dec 2019 00:29:55 +0100 Subject: [PATCH 25/36] Add speed profile to reference path. Include speed tracking in MPC --- MPC.py | 18 +++---- map.py | 1 + reference_path.py | 128 ++++++++++++++++++++++++++++++++++++---------- simulation.py | 32 ++++++------ 4 files changed, 126 insertions(+), 53 deletions(-) diff --git a/MPC.py b/MPC.py index 61d548f..2f28426 100644 --- a/MPC.py +++ b/MPC.py @@ -13,8 +13,7 @@ PREDICTION = '#BA4A00' class MPC: - def __init__(self, model, N, Q, R, QN, StateConstraints, InputConstraints, - velocity_reference): + def __init__(self, model, N, Q, R, QN, StateConstraints, InputConstraints): """ Constructor for the Model Predictive Controller. :param model: bicycle model object to be controlled @@ -24,7 +23,6 @@ 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 @@ -44,9 +42,6 @@ class MPC: self.state_constraints = StateConstraints self.input_constraints = InputConstraints - # Velocity reference - self.v_ref = velocity_reference - # Current control and prediction self.current_prediction = None @@ -54,7 +49,7 @@ class MPC: self.infeasibility_counter = 0 # Current control signals - self.current_control = np.ones((self.nu*self.N)) * velocity_reference + self.current_control = np.ones((self.nu*self.N)) # Initialize Optimization Problem self.optimizer = osqp.OSQP() @@ -90,19 +85,20 @@ class MPC: next_waypoint = self.model.reference_path.get_waypoint(self.model.wp_id + n + 1) delta_s = next_waypoint - current_waypoint kappa_ref = current_waypoint.kappa + v_ref = current_waypoint.v_ref # Compute LTV matrices - f, A_lin, B_lin = self.model.linearize(self.v_ref, kappa_ref, delta_s) + f, A_lin, B_lin = self.model.linearize(v_ref, kappa_ref, delta_s) A[(n+1) * self.nx: (n+2)*self.nx, n * self.nx:(n+1)*self.nx] = A_lin B[(n+1) * self.nx: (n+2)*self.nx, n * self.nu:(n+1)*self.nu] = B_lin # Set reference for input signal - ur[n*self.nu:(n+1)*self.nu] = np.array([self.v_ref, kappa_ref]) + ur[n*self.nu:(n+1)*self.nu] = np.array([v_ref, kappa_ref]) # Compute equality constraint offset (B*ur) uq[n * self.nx:(n+1)*self.nx] = B_lin.dot(np.array - ([self.v_ref, kappa_ref])) - f + ([v_ref, kappa_ref])) - f # Compute dynamic constraints on e_y - lb, ub = self.model.reference_path.update_bounds( + lb, ub, cells = self.model.reference_path.update_bounds( self.model.wp_id + n, self.model.safety_margin[1]) xmin_dyn[self.nx * n] = lb xmax_dyn[self.nx * n] = ub diff --git a/map.py b/map.py index bbb5c24..55bae67 100644 --- a/map.py +++ b/map.py @@ -56,6 +56,7 @@ class Map: + if __name__ == '__main__': map = Map('map_floor2.png') plt.imshow(map.data, cmap='gray') diff --git a/reference_path.py b/reference_path.py index 475bada..3ea4208 100644 --- a/reference_path.py +++ b/reference_path.py @@ -4,7 +4,8 @@ from map import Map from skimage.draw import line import matplotlib.pyplot as plt import matplotlib.patches as plt_patches -from scipy.signal import savgol_filter +from scipy import sparse +import osqp # Colors DRIVABLE_AREA = '#BDC3C7' @@ -31,7 +32,10 @@ class Waypoint: self.psi = psi self.kappa = kappa - # information about drivable area at waypoint + # Reference velocity at this waypoint according to speed profile + self.v_ref = None + + # Information about drivable area at waypoint # upper and lower bound of drivable area orthogonal to # waypoint orientation self.lb = None @@ -112,7 +116,7 @@ class ReferencePath: # Look ahead distance for path averaging self.smoothing_distance = smoothing_distance - # Circular + # Circular flag self.circular = circular # List of waypoint objects @@ -308,6 +312,88 @@ class ReferencePath: return min_width, min_cell + def compute_speed_profile(self, Constraints): + """ + Compute a speed profile for the path. Assign a reference velocity + to each waypoint based on its curvature. + """ + + # Set optimization horizon + N = self.n_waypoints - 1 + + # Constraints + a_min = np.ones(N-1) * Constraints['a_min'] + a_max = np.ones(N-1) * Constraints['a_max'] + v_min = np.ones(N) * Constraints['v_min'] + v_max = np.ones(N) * Constraints['v_max'] + + # Maximum lateral acceleration + ay_max = Constraints['ay_max'] + + # Inequality Matrix + D1 = np.zeros((N-1, N)) + + # Iterate over horizon + for i in range(N): + + # Get information about current waypoint + current_waypoint = self.get_waypoint(i) + next_waypoint = self.get_waypoint(i+1) + # distance between waypoints + li = next_waypoint - current_waypoint + # curvature of waypoint + ki = current_waypoint.kappa + + # Fill operator matrix + # dynamics of acceleration + if i < N-1: + D1[i, i:i+2] = np.array([-1/(2*li), 1/(2*li)]) + + # Compute dynamic constraint on velocity + v_max_dyn = np.sqrt(ay_max / (np.abs(ki) + self.eps)) + if v_max_dyn < v_max[i]: + v_max[i] = v_max_dyn + + # Construct inequality matrix + D1 = sparse.csc_matrix(D1) + D2 = sparse.eye(N) + D = sparse.vstack([D1, D2], format='csc') + + # Get upper and lower bound vectors for inequality constraints + l = np.hstack([a_min, v_min]) + u = np.hstack([a_max, v_max]) + + # Set cost matrices + P = sparse.eye(N, format='csc') + q = -1 * v_max + + # Solve optimization problem + problem = osqp.OSQP() + problem.setup(P=P, q=q, A=D, l=l, u=u, verbose=False) + speed_profile = problem.solve().x + + # Assign reference velocity to every waypoint + for i, wp in enumerate(self.waypoints[:-1]): + wp.v_ref = speed_profile[i] + self.waypoints[-1].v_ref = self.waypoints[-2].v_ref + + def get_waypoint(self, wp_id): + """ + Get waypoint corresponding to wp_id. Circular indexing supported. + :param wp_id: unique waypoint ID + :return: waypoint object + """ + + # Allow circular indexing if circular path + if wp_id >= self.n_waypoints and self.circular: + wp_id = np.mod(wp_id, self.n_waypoints) + # Terminate execution if end of path reached + elif wp_id >= self.n_waypoints and not self.circular: + print('Reached end of path!') + exit(1) + + return self.waypoints[wp_id] + def update_bounds(self, wp_id, safety_margin): """ Compute upper and lower bounds of the drivable area orthogonal to @@ -341,10 +427,10 @@ class ReferencePath: lb_o = (x, y) # If cell is occupied, end segment. Update best segment if current # segment is larger than previous best segment. Then, reset upper - # and lower bound to current cell. + # and lower bound to current cell if self.map.data[y, x] == 0 or (x, y) == lb_p: if np.sqrt((ub_o[0]-lb_o[0])**2+(ub_o[1]-lb_o[1])**2) > \ - np.sqrt((ub_ls[0]-lb_ls[0])**2+(ub_ls[1]-lb_ls[1])**2): + np.sqrt((ub_ls[0]-lb_ls[0])**2+(ub_ls[1]-lb_ls[1])**2): ub_ls = ub_o lb_ls = lb_o # Start new segment @@ -380,11 +466,13 @@ class ReferencePath: # Compute absolute angle of bound cell angle_ub = np.mod(math.pi/2 + wp.psi + math.pi, 2 * math.pi) - math.pi angle_lb = np.mod(-math.pi/2 + wp.psi + math.pi, 2 * math.pi) - math.pi + # Compute cell on bound for computed distance ub and lb 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) + border_cells = (ub_ls, lb_ls) - return lb, ub + return lb, ub, border_cells def add_obstacles(self, obstacles): """ @@ -397,8 +485,12 @@ class ReferencePath: # Iterate over list of obstacles for obstacle in obstacles: + # Compute radius of circular object in pixels radius_px = int(np.ceil(obstacle.radius / self.map.resolution)) + # Get center coordinates of obstacle in map coordinates cx_px, cy_px = self.map.w2m(obstacle.cx, obstacle.cy) + + # Add circular object to map y, x = np.ogrid[-radius_px: radius_px, -radius_px: radius_px] index = x ** 2 + y ** 2 <= radius_px ** 2 self.map.data[cy_px-radius_px:cy_px+radius_px, cx_px-radius_px: @@ -439,7 +531,7 @@ class ReferencePath: 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) + plt.scatter(wp_x, wp_y, color=WAYPOINTS, s=10) # Plot arrows indicating drivable area if display_drivable_area: @@ -463,11 +555,6 @@ class ReferencePath: br_y = np.array([wp.border_cells[1][1] for wp in self.waypoints] + [self.waypoints[0].border_cells[1][1]]) - # Smooth border - # bl_x = savgol_filter(bl_x, 15, 9) - # bl_y = savgol_filter(bl_y, 15, 9) - # br_x = savgol_filter(br_x, 15, 9) - # br_y = savgol_filter(br_y, 15, 9) # If circular path, connect start and end point if self.circular: @@ -484,22 +571,11 @@ 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 = 'Q' + path = 'Race' # Create Map if path == 'Race': @@ -512,7 +588,7 @@ if __name__ == '__main__': # Specify path resolution path_resolution = 0.05 # m / wp reference_path = ReferencePath(map, wp_x, wp_y, path_resolution, - smoothing_distance=5, max_width=0.22, n_extension=30, + smoothing_distance=5, max_width=0.15, circular=True) # Add obstacles obs1 = Obstacle(cx=0.0, cy=0.0, radius=0.05) @@ -534,7 +610,7 @@ if __name__ == '__main__': path_resolution = 0.20 # m / wp reference_path = ReferencePath(map, wp_x, wp_y, path_resolution, smoothing_distance=5, max_width=1.5, - n_extension=30, circular=False) + circular=False) obs1 = Obstacle(cx=-6.3, cy=-11.1, radius=0.20) obs2 = Obstacle(cx=-2.2, cy=-6.8, radius=0.25) obs3 = Obstacle(cx=1.7, cy=-1.0, radius=0.15) diff --git a/simulation.py b/simulation.py index 4d36677..33ef0a2 100644 --- a/simulation.py +++ b/simulation.py @@ -5,8 +5,6 @@ from spatial_bicycle_models import BicycleModel import matplotlib.pyplot as plt from MPC import MPC from scipy import sparse -from time import time -from lidar_model import LidarModel if __name__ == '__main__': @@ -65,6 +63,7 @@ if __name__ == '__main__': e_y_0 = 0.0 e_psi_0 = 0.0 t_0 = 0.0 + V_MAX = 2.5 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) @@ -78,18 +77,15 @@ if __name__ == '__main__': R = sparse.diags([1.0, 0.0]) QN = sparse.diags([0.0, 0.0, 0.0]) InputConstraints = {'umin': np.array([0.0, -np.tan(0.66)/car.l]), - 'umax': np.array([2.5, np.tan(0.66)/car.l])} + 'umax': np.array([V_MAX, 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])} - velocity_reference = 1.5 # m/s - mpc = MPC(car, N, Q, R, QN, StateConstraints, InputConstraints, - velocity_reference) + mpc = MPC(car, N, Q, R, QN, StateConstraints, InputConstraints) - ######### - # LiDAR # - ######### - - sensor = LidarModel(FoV=90, range=0.25, resolution=4.0) + # Compute speed profile + SpeedProfileConstraints = {'a_min': -2.0, 'a_max': 2.0, + 'v_min': 0, 'v_max': V_MAX, 'ay_max': 5.0} + car.reference_path.compute_speed_profile(SpeedProfileConstraints) ############## # Simulation # @@ -103,6 +99,7 @@ if __name__ == '__main__': # Logging containers x_log = [car.temporal_state.x] y_log = [car.temporal_state.y] + v_log = [0.0] # Until arrival at end of path while car.s < reference_path.length: @@ -116,6 +113,7 @@ if __name__ == '__main__': # log x_log.append(car.temporal_state.x) y_log.append(car.temporal_state.y) + v_log.append(u[0]) ################### # Plot Simulation # @@ -123,6 +121,8 @@ if __name__ == '__main__': # Plot path and drivable area reference_path.show() + plt.scatter(x_log, y_log, c=v_log, s=10) + plt.colorbar() # Plot MPC prediction mpc.show_prediction() @@ -130,12 +130,12 @@ if __name__ == '__main__': # Plot car car.show() + # Increase simulation time t += Ts + # Set figure title plt.title('MPC Simulation: v(t): {:.2f}, delta(t): {:.2f}, Duration: ' - '{:.2f} s'. - format(u[0], u[1], t)) + '{:.2f} s'.format(u[0], u[1], t)) - plt.pause(0.01) - print('Final Time: {:.2f}'.format(t)) - plt.close() + plt.pause(0.0001) + plt.show() From 7abc906af5879a40f6b58ea83d46fb169f1a8c12 Mon Sep 17 00:00:00 2001 From: matssteinweg Date: Sat, 7 Dec 2019 10:16:07 +0100 Subject: [PATCH 26/36] Add dynamic speed constraint based on predicted path curvature --- MPC.py | 15 +++++++++++++-- 1 file changed, 13 insertions(+), 2 deletions(-) diff --git a/MPC.py b/MPC.py index 2f28426..129cb63 100644 --- a/MPC.py +++ b/MPC.py @@ -42,6 +42,9 @@ class MPC: self.state_constraints = StateConstraints self.input_constraints = InputConstraints + # Maximum lateral acceleration + self.ay_max = 5.0 + # Current control and prediction self.current_prediction = None @@ -76,6 +79,11 @@ class MPC: # Dynamic state constraints xmin_dyn = np.kron(np.ones(self.N + 1), xmin) xmax_dyn = np.kron(np.ones(self.N + 1), xmax) + # Dynamic input constraints + 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 # Iterate over horizon for n in range(self.N): @@ -102,6 +110,10 @@ class MPC: self.model.wp_id + n, self.model.safety_margin[1]) xmin_dyn[self.nx * n] = lb xmax_dyn[self.nx * n] = ub + # Constrain maximum speed based on predicted car curvature + vmax_dyn = np.sqrt(self.ay_max / (np.abs(kappa_pred[n]) + 1e-12)) + if vmax_dyn < umax_dyn[self.nu*n]: + umax_dyn[self.nu*n] = vmax_dyn # Get equality matrix Ax = sparse.kron(sparse.eye(self.N + 1), @@ -116,8 +128,7 @@ class MPC: # Get upper and lower bound vectors for equality constraints lineq = np.hstack([xmin_dyn, np.kron(np.ones(self.N), umin)]) - uineq = np.hstack([xmax_dyn, - np.kron(np.ones(self.N), umax)]) + uineq = np.hstack([xmax_dyn, umax_dyn]) # Get upper and lower bound vectors for inequality constraints x0 = np.array(self.model.spatial_state[:]) leq = np.hstack([-x0, uq]) From df6e7351c82ce5d6de5209437a939a8d0c5e9c7d Mon Sep 17 00:00:00 2001 From: matssteinweg Date: Sun, 8 Dec 2019 11:33:25 +0100 Subject: [PATCH 27/36] Add function update_path_constraints to reference path for more robust computation of path constraints --- MPC.py | 12 +-- map.py | 1 - reference_path.py | 201 ++++++++++++++++++++++++++++++++++++++++++++-- simulation.py | 11 +-- 4 files changed, 208 insertions(+), 17 deletions(-) diff --git a/MPC.py b/MPC.py index 129cb63..1eafe2b 100644 --- a/MPC.py +++ b/MPC.py @@ -105,16 +105,18 @@ class MPC: # Compute equality constraint offset (B*ur) uq[n * self.nx:(n+1)*self.nx] = B_lin.dot(np.array ([v_ref, kappa_ref])) - f - # Compute dynamic constraints on e_y - lb, ub, cells = self.model.reference_path.update_bounds( - self.model.wp_id + n, self.model.safety_margin[1]) - xmin_dyn[self.nx * n] = lb - xmax_dyn[self.nx * n] = ub + # Constrain maximum speed based on predicted car curvature vmax_dyn = np.sqrt(self.ay_max / (np.abs(kappa_pred[n]) + 1e-12)) if vmax_dyn < umax_dyn[self.nu*n]: umax_dyn[self.nu*n] = vmax_dyn + # Compute dynamic constraints on e_y + ub, lb, cells = self.model.reference_path.update_path_constraints( + self.model.wp_id, self.N+1, 2*self.model.safety_margin[1], 0.05) + xmin_dyn[::self.nx] = lb + xmax_dyn[::self.nx] = ub + # Get equality matrix Ax = sparse.kron(sparse.eye(self.N + 1), -sparse.eye(self.nx)) + sparse.csc_matrix(A) diff --git a/map.py b/map.py index 55bae67..bbb5c24 100644 --- a/map.py +++ b/map.py @@ -56,7 +56,6 @@ class Map: - if __name__ == '__main__': map = Map('map_floor2.png') plt.imshow(map.data, cmap='gray') diff --git a/reference_path.py b/reference_path.py index 3ea4208..03abfa4 100644 --- a/reference_path.py +++ b/reference_path.py @@ -336,6 +336,8 @@ class ReferencePath: # Iterate over horizon for i in range(N): + look_ahead = 30 + # Get information about current waypoint current_waypoint = self.get_waypoint(i) next_waypoint = self.get_waypoint(i+1) @@ -343,6 +345,9 @@ class ReferencePath: li = next_waypoint - current_waypoint # curvature of waypoint ki = current_waypoint.kappa + if np.abs(ki) <= 0.1: + kis = [wp.kappa for wp in self.waypoints[i:i+look_ahead]] + ki = np.mean(kis) # Fill operator matrix # dynamics of acceleration @@ -511,8 +516,8 @@ class ReferencePath: plt.yticks([]) # Plot map in gray-scale and set extent to match world coordinates - canvas = np.ones(self.map.data.shape) - # canvas = np.flipud(self.map.data) + #canvas = np.ones(self.map.data.shape) + canvas = np.flipud(self.map.data) plt.imshow(canvas, cmap='gray', extent=[self.map.origin[0], self.map.origin[0] + self.map.width * self.map.resolution, @@ -571,11 +576,184 @@ class ReferencePath: for obstacle in self.obstacles: obstacle.show() + def _compute_free_segments(self, wp, min_width): + """ + Compute free path segments. + :param wp: waypoint object + :param min_width: minimum width of valid segment + :return: segment candidates as list of tuples (ub_cell, lb_cell) + """ + + # Candidate segments + free_segments = [] + + # Get waypoint's border cells in map coordinates + ub_p = self.map.w2m(wp.border_cells[0][0], + wp.border_cells[0][1]) + lb_p = self.map.w2m(wp.border_cells[1][0], + wp.border_cells[1][1]) + + # Compute path from left border cell to right border cell + x_list, y_list = line(ub_p[0], ub_p[1], lb_p[0], lb_p[1]) + + # Initialize upper and lower bound of drivable area to + # upper bound of path + ub_o, lb_o = ub_p, ub_p + + # Assume occupied path + free_cells = False + + # Iterate over path from left border to right border + for x, y in zip(x_list[1:], y_list[1:]): + # If cell is free, update lower bound + if self.map.data[y, x] == 1: + # Free cell detected + free_cells = True + lb_o = (x, y) + # If cell is occupied or end of path, end segment. Add segment + # to list of candidates. Then, reset upper and lower bound to + # current cell. + if (self.map.data[y, x] == 0 or (x, y) == lb_p) and free_cells: + # Set lower bound to border cell of segment + lb_o = (x, y) + # Transform upper and lower bound cells to world coordinates + ub_o = self.map.m2w(ub_o[0], ub_o[1]) + lb_o = self.map.m2w(lb_o[0], lb_o[1]) + # If segment larger than threshold, add to candidates + if np.sqrt((ub_o[0]-lb_o[0])**2 + (ub_o[1]-lb_o[1])**2) > \ + min_width: + free_segments.append((ub_o, lb_o)) + # Start new segment + ub_o = (x, y) + free_cells = False + elif self.map.data[y, x] == 0 and not free_cells: + ub_o = (x, y) + lb_o = (x, y) + + return free_segments + + def update_path_constraints(self, wp_id, N, min_width, safety_margin): + """ + Compute upper and lower bounds of the drivable area orthogonal to + the given waypoint. + """ + + # container for constraints and border cells + ub_hor = [] + lb_hor = [] + border_cells_hor = [] + + # Iterate over horizon + for n in range(N): + + # get corresponding waypoint + wp = self.waypoints[wp_id+n] + + # Get list of free segments + free_segments = self._compute_free_segments(wp, min_width) + + # First waypoint in horizon uses largest segment + if n == 0: + segment_lengths = [np.sqrt((seg[0][0]-seg[1][0])**2 + + (seg[0][1]-seg[1][1])**2) for seg in free_segments] + ls_id = segment_lengths.index(max(segment_lengths)) + ub_ls, lb_ls = free_segments[ls_id] + + else: + + # Get border cells of selected segment at previous waypoint + ub_pw, lb_pw = border_cells_hor[n-1] + ub_pw, lb_pw = list(ub_pw), list(lb_pw) + + # Project border cells onto new waypoint in path direction + wp_prev = self.waypoints[wp_id+n-1] + delta_s = wp_prev - wp + ub_pw[0] += delta_s * np.cos(wp_prev.psi) + ub_pw[1] += delta_s * np.cos(wp_prev.psi) + lb_pw[0] += delta_s * np.sin(wp_prev.psi) + lb_pw[1] += delta_s * np.sin(wp_prev.psi) + + # Iterate over free segments for current waypoint + if len(free_segments) >= 2: + + # container for overlap of segments with projection + segment_offsets = [] + + for free_segment in free_segments: + + # Get border cells of segment + ub_fs, lb_fs = free_segment + + # distance between upper bounds and lower bounds + d_ub = np.sqrt((ub_fs[0]-ub_pw[0])**2 + (ub_fs[1]-ub_pw[1])**2) + d_lb = np.sqrt((lb_fs[0]-lb_pw[0])**2 + (lb_fs[1]-lb_pw[1])**2) + mean_dist = (d_ub + d_lb) / 2 + + # Append offset to projected previous segment + segment_offsets.append(mean_dist) + + # Select segment with minimum offset to projected previous + # segment + ls_id = segment_offsets.index(min(segment_offsets)) + ub_ls, lb_ls = free_segments[ls_id] + + # Select free segment in case of only one candidate + elif len(free_segments) == 1: + ub_ls, lb_ls = free_segments[0] + + # Set waypoint coordinates as bound cells if no feasible + # segment available + else: + ub_ls, lb_ls = (wp.x, wp.y), (wp.x, wp.y) + + # Check sign of upper and lower bound + angle_ub = np.mod(np.arctan2(ub_ls[1] - wp.y, ub_ls[0] - wp.x) + - wp.psi + math.pi, 2 * math.pi) - math.pi + angle_lb = np.mod(np.arctan2(lb_ls[1] - wp.y, lb_ls[0] - wp.x) + - wp.psi + math.pi, 2 * math.pi) - math.pi + sign_ub = np.sign(angle_ub) + sign_lb = np.sign(angle_lb) + + # Compute upper and lower bound of largest drivable area + ub = sign_ub * np.sqrt( + (ub_ls[0] - wp.x) ** 2 + (ub_ls[1] - wp.y) ** 2) + lb = sign_lb * np.sqrt( + (lb_ls[0] - wp.x) ** 2 + (lb_ls[1] - wp.y) ** 2) + + # Subtract safety margin + ub -= safety_margin + lb += safety_margin + + # Check feasibility of the path + if ub < lb: + # Upper and lower bound of 0 indicate an infeasible path + # given the specified safety margin + ub, lb = 0.0, 0.0 + + # Compute absolute angle of bound cell + angle_ub = np.mod(math.pi / 2 + wp.psi + math.pi, + 2 * math.pi) - math.pi + angle_lb = np.mod(-math.pi / 2 + wp.psi + math.pi, + 2 * math.pi) - math.pi + # Compute cell on bound for computed distance ub and lb + 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) + bound_cells = (ub_ls, lb_ls) + + # Append results + ub_hor.append(ub) + lb_hor.append(lb) + border_cells_hor.append(list(bound_cells)) + + return np.array(ub_hor), np.array(lb_hor), border_cells_hor + if __name__ == '__main__': # Select Path | 'Race' or 'Q' - path = 'Race' + path = 'Q' # Create Map if path == 'Race': @@ -615,16 +793,27 @@ if __name__ == '__main__': obs2 = Obstacle(cx=-2.2, cy=-6.8, radius=0.25) obs3 = Obstacle(cx=1.7, cy=-1.0, radius=0.15) obs4 = Obstacle(cx=2.0, cy=-1.2, radius=0.25) - reference_path.add_obstacles([obs1, obs2, obs3, obs4]) + obs5 = Obstacle(cx=2.2, cy=-0.86, radius=0.1) + obs6 = Obstacle(cx=2.33, cy=-0.7, radius=0.1) + obs7 = Obstacle(cx=2.67, cy=-0.73, radius=0.1) + obs8 = Obstacle(cx=6.42, cy=3.97, radius=0.3) + obs9 = Obstacle(cx=7.42, cy=4.97, radius=0.3) + obs10 = Obstacle(cx=7.14, cy=5.7, radius=0.1) + reference_path.add_obstacles([obs1, obs2, obs3, obs4, obs5, obs6, obs7, obs8, obs9, obs10]) else: reference_path = None print('Invalid path!') exit(1) - [reference_path.update_bounds(wp_id=wp_id, safety_margin=0.02) - for wp_id in range(len(reference_path.waypoints))] + ub, lb, border_cells = reference_path.update_path_constraints(0, reference_path.n_waypoints, 0.60, 0.1) + # Get x and y locations of border cells for upper and lower bound + for wp_id in range(reference_path.n_waypoints): + if ub[wp_id] > 0.0 and lb[wp_id] > 0.0: + print(wp_id) + reference_path.waypoints[wp_id].border_cells = border_cells[wp_id] reference_path.show() + plt.show() diff --git a/simulation.py b/simulation.py index 33ef0a2..c7062ee 100644 --- a/simulation.py +++ b/simulation.py @@ -10,7 +10,7 @@ from scipy import sparse if __name__ == '__main__': # Select Simulation Mode | 'Race' or 'Q' - sim_mode = 'Race' + sim_mode = 'Q' # Create Map if sim_mode == 'Race': @@ -83,8 +83,8 @@ if __name__ == '__main__': mpc = MPC(car, N, Q, R, QN, StateConstraints, InputConstraints) # Compute speed profile - SpeedProfileConstraints = {'a_min': -2.0, 'a_max': 2.0, - 'v_min': 0, 'v_max': V_MAX, 'ay_max': 5.0} + SpeedProfileConstraints = {'a_min': -1.0, 'a_max': 1.0, + 'v_min': 0, 'v_max': V_MAX, 'ay_max': 1.0} car.reference_path.compute_speed_profile(SpeedProfileConstraints) ############## @@ -92,7 +92,7 @@ if __name__ == '__main__': ############## # Sampling time - Ts = 0.05 + Ts = 0.20 t = 0 car.set_sampling_time(Ts) @@ -137,5 +137,6 @@ if __name__ == '__main__': plt.title('MPC Simulation: v(t): {:.2f}, delta(t): {:.2f}, Duration: ' '{:.2f} s'.format(u[0], u[1], t)) - plt.pause(0.0001) + plt.pause(0.00001) + print(min(v_log)) plt.show() From dc7628ee8daf0f6b1f37ebdf8621c2b20250c8ff Mon Sep 17 00:00:00 2001 From: matssteinweg Date: Sun, 8 Dec 2019 11:58:22 +0100 Subject: [PATCH 28/36] Remove update bounds function --- reference_path.py | 84 ++--------------------------------------------- 1 file changed, 2 insertions(+), 82 deletions(-) diff --git a/reference_path.py b/reference_path.py index 03abfa4..3394579 100644 --- a/reference_path.py +++ b/reference_path.py @@ -399,86 +399,6 @@ class ReferencePath: return self.waypoints[wp_id] - def update_bounds(self, wp_id, safety_margin): - """ - Compute upper and lower bounds of the drivable area orthogonal to - the given waypoint. - :param safety_margin: safety margin of the car orthogonal to path in m - :param wp_id: ID of reference waypoint - """ - - # Get reference waypoint - 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]) - lb_p = self.map.w2m(wp.border_cells[1][0], wp.border_cells[1][1]) - - # Compute path from left border cell to right border cell - x_list, y_list = line(ub_p[0], ub_p[1], lb_p[0], lb_p[1]) - - # Initialize upper and lower bound of drivable area to - # upper bound of path - ub_o, lb_o = ub_p, ub_p - - # Initialize upper and lower bound of best segment to upper bound of - # path - ub_ls, lb_ls = ub_p, ub_p - - # Iterate over path from left border to right border - for x, y in zip(x_list[1:], y_list[1:]): - # If cell is free, update lower bound - if self.map.data[y, x] == 1: - lb_o = (x, y) - # If cell is occupied, end segment. Update best segment if current - # segment is larger than previous best segment. Then, reset upper - # and lower bound to current cell - if self.map.data[y, x] == 0 or (x, y) == lb_p: - if np.sqrt((ub_o[0]-lb_o[0])**2+(ub_o[1]-lb_o[1])**2) > \ - np.sqrt((ub_ls[0]-lb_ls[0])**2+(ub_ls[1]-lb_ls[1])**2): - ub_ls = ub_o - lb_ls = lb_o - # Start new segment - ub_o = (x, y) - lb_o = (x, y) - - # Transform upper and lower bound cells to world coordinates - ub_ls = self.map.m2w(ub_ls[0], ub_ls[1]) - lb_ls = self.map.m2w(lb_ls[0], lb_ls[1]) - - # Check sign of upper and lower bound - angle_ub = np.mod(np.arctan2(ub_ls[1] - wp.y, ub_ls[0] - wp.x) - - wp.psi + math.pi, 2*math.pi) - math.pi - angle_lb = np.mod(np.arctan2(lb_ls[1] - wp.y, lb_ls[0] - wp.x) - - wp.psi + math.pi, 2*math.pi) - math.pi - sign_ub = np.sign(angle_ub) - sign_lb = np.sign(angle_lb) - - # Compute upper and lower bound of largest drivable area - ub = sign_ub * np.sqrt((ub_ls[0]-wp.x)**2+(ub_ls[1]-wp.y)**2) - lb = sign_lb * np.sqrt((lb_ls[0]-wp.x)**2+(lb_ls[1]-wp.y)**2) - - # Add safety margin (attribute of car) to bounds - ub = ub - safety_margin - lb = lb + safety_margin - - # Check feasibility of the path - if ub < lb: - # Upper and lower bound of 0 indicate an infeasible path - # given the specified safety margin - ub, lb = 0.0, 0.0 - - # Compute absolute angle of bound cell - angle_ub = np.mod(math.pi/2 + wp.psi + math.pi, 2 * math.pi) - math.pi - angle_lb = np.mod(-math.pi/2 + wp.psi + math.pi, 2 * math.pi) - math.pi - - # Compute cell on bound for computed distance ub and lb - 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) - border_cells = (ub_ls, lb_ls) - - return lb, ub, border_cells - def add_obstacles(self, obstacles): """ Add obstacles to the path. @@ -753,7 +673,7 @@ class ReferencePath: if __name__ == '__main__': # Select Path | 'Race' or 'Q' - path = 'Q' + path = 'Race' # Create Map if path == 'Race': @@ -806,7 +726,7 @@ if __name__ == '__main__': print('Invalid path!') exit(1) - ub, lb, border_cells = reference_path.update_path_constraints(0, reference_path.n_waypoints, 0.60, 0.1) + ub, lb, border_cells = reference_path.update_path_constraints(0, reference_path.n_waypoints, 0.02, 0.01) # Get x and y locations of border cells for upper and lower bound for wp_id in range(reference_path.n_waypoints): if ub[wp_id] > 0.0 and lb[wp_id] > 0.0: From d25de2e0c263edf56cbc9157b6c044a5c0c69043 Mon Sep 17 00:00:00 2001 From: matssteinweg Date: Sun, 8 Dec 2019 14:57:35 +0100 Subject: [PATCH 29/36] Add attribute dynamic_border_cells to reference path. --- MPC.py | 11 +++-- reference_path.py | 98 +++++++++++++++++++++++---------------- simulation.py | 43 +++++++++-------- spatial_bicycle_models.py | 2 +- 4 files changed, 88 insertions(+), 66 deletions(-) diff --git a/MPC.py b/MPC.py index 1eafe2b..6c86b21 100644 --- a/MPC.py +++ b/MPC.py @@ -112,10 +112,13 @@ class MPC: umax_dyn[self.nu*n] = vmax_dyn # Compute dynamic constraints on e_y - ub, lb, cells = self.model.reference_path.update_path_constraints( - self.model.wp_id, self.N+1, 2*self.model.safety_margin[1], 0.05) - xmin_dyn[::self.nx] = lb - xmax_dyn[::self.nx] = ub + 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]) + 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 # Get equality matrix Ax = sparse.kron(sparse.eye(self.N + 1), diff --git a/reference_path.py b/reference_path.py index 3394579..38a9f6e 100644 --- a/reference_path.py +++ b/reference_path.py @@ -11,7 +11,7 @@ import osqp DRIVABLE_AREA = '#BDC3C7' WAYPOINTS = '#D0D3D4' OBSTACLE = '#2E4053' - +PATH_CONSTRAINTS = '#F5B041' ############ # Waypoint # @@ -40,7 +40,8 @@ class Waypoint: # waypoint orientation self.lb = None self.ub = None - self.border_cells = None + self.static_border_cells = None + self.dynamic_border_cells = None def __sub__(self, other): """ @@ -264,7 +265,8 @@ class ReferencePath: wp.lb = -1 * width_info[2] # minus can be assumed as waypoints # represent center-line of the path # Set border cells of waypoint - wp.border_cells = (width_info[1], width_info[3]) + wp.static_border_cells = (width_info[1], width_info[3]) + wp.dynamic_border_cells = (width_info[1], width_info[3]) def _get_min_width(self, wp, t_x, t_y, max_width): """ @@ -316,6 +318,8 @@ class ReferencePath: """ 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 """ # Set optimization horizon @@ -336,8 +340,6 @@ class ReferencePath: # Iterate over horizon for i in range(N): - look_ahead = 30 - # Get information about current waypoint current_waypoint = self.get_waypoint(i) next_waypoint = self.get_waypoint(i+1) @@ -345,9 +347,6 @@ class ReferencePath: li = next_waypoint - current_waypoint # curvature of waypoint ki = current_waypoint.kappa - if np.abs(ki) <= 0.1: - kis = [wp.kappa for wp in self.waypoints[i:i+look_ahead]] - ki = np.mean(kis) # Fill operator matrix # dynamics of acceleration @@ -436,8 +435,8 @@ class ReferencePath: plt.yticks([]) # Plot map in gray-scale and set extent to match world coordinates - #canvas = np.ones(self.map.data.shape) - canvas = np.flipud(self.map.data) + canvas = np.ones(self.map.data.shape) + #canvas = np.flipud(self.map.data) plt.imshow(canvas, cmap='gray', extent=[self.map.origin[0], self.map.origin[0] + self.map.width * self.map.resolution, @@ -450,10 +449,10 @@ class ReferencePath: 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]) - 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]) - wp_lb_y = np.array([wp.border_cells[1][1] for wp in self.waypoints]) + wp_ub_x = np.array([wp.static_border_cells[0][0] for wp in self.waypoints]) + wp_ub_y = np.array([wp.static_border_cells[0][1] for wp in self.waypoints]) + wp_lb_x = np.array([wp.static_border_cells[1][0] for wp in self.waypoints]) + wp_lb_y = np.array([wp.static_border_cells[1][1] for wp in self.waypoints]) # Plot waypoints plt.scatter(wp_x, wp_y, color=WAYPOINTS, s=10) @@ -468,18 +467,18 @@ class ReferencePath: headwidth=1, headlength=0) # Plot border of path - bl_x = np.array([wp.border_cells[0][0] for wp in + bl_x = np.array([wp.static_border_cells[0][0] for wp in self.waypoints] + - [self.waypoints[0].border_cells[0][0]]) - bl_y = np.array([wp.border_cells[0][1] for wp in + [self.waypoints[0].static_border_cells[0][0]]) + bl_y = np.array([wp.static_border_cells[0][1] for wp in self.waypoints] + - [self.waypoints[0].border_cells[0][1]]) - br_x = np.array([wp.border_cells[1][0] for wp in + [self.waypoints[0].static_border_cells[0][1]]) + br_x = np.array([wp.static_border_cells[1][0] for wp in self.waypoints] + - [self.waypoints[0].border_cells[1][0]]) - br_y = np.array([wp.border_cells[1][1] for wp in + [self.waypoints[0].static_border_cells[1][0]]) + br_y = np.array([wp.static_border_cells[1][1] for wp in self.waypoints] + - [self.waypoints[0].border_cells[1][1]]) + [self.waypoints[0].static_border_cells[1][1]]) # If circular path, connect start and end point if self.circular: @@ -492,6 +491,19 @@ class ReferencePath: plt.plot((bl_x[-2], br_x[-2]), (bl_y[-2], br_y[-2]), color=OBSTACLE) plt.plot((bl_x[0], br_x[0]), (bl_y[0], br_y[0]), color=OBSTACLE) + # Plot dynamic path constraints + # Get x and y locations of border cells for upper and lower bound + wp_ub_x = np.array( + [wp.dynamic_border_cells[0][0] for wp in self.waypoints]) + wp_ub_y = np.array( + [wp.dynamic_border_cells[0][1] for wp in self.waypoints]) + wp_lb_x = np.array( + [wp.dynamic_border_cells[1][0] for wp in self.waypoints]) + wp_lb_y = np.array( + [wp.dynamic_border_cells[1][1] for wp in self.waypoints]) + plt.plot(wp_ub_x, wp_ub_y, color=PATH_CONSTRAINTS) + plt.plot(wp_lb_x, wp_lb_y, color=PATH_CONSTRAINTS) + # Plot obstacles for obstacle in self.obstacles: obstacle.show() @@ -508,10 +520,10 @@ class ReferencePath: free_segments = [] # Get waypoint's border cells in map coordinates - ub_p = self.map.w2m(wp.border_cells[0][0], - wp.border_cells[0][1]) - lb_p = self.map.w2m(wp.border_cells[1][0], - wp.border_cells[1][1]) + ub_p = self.map.w2m(wp.static_border_cells[0][0], + wp.static_border_cells[0][1]) + lb_p = self.map.w2m(wp.static_border_cells[1][0], + wp.static_border_cells[1][1]) # Compute path from left border cell to right border cell x_list, y_list = line(ub_p[0], ub_p[1], lb_p[0], lb_p[1]) @@ -562,12 +574,13 @@ class ReferencePath: ub_hor = [] lb_hor = [] border_cells_hor = [] + border_cells_hor_sm = [] # Iterate over horizon for n in range(N): # get corresponding waypoint - wp = self.waypoints[wp_id+n] + wp = self.get_waypoint(wp_id+n) # Get list of free segments free_segments = self._compute_free_segments(wp, min_width) @@ -586,7 +599,7 @@ class ReferencePath: ub_pw, lb_pw = list(ub_pw), list(lb_pw) # Project border cells onto new waypoint in path direction - wp_prev = self.waypoints[wp_id+n-1] + wp_prev = self.get_waypoint(wp_id+n-1) delta_s = wp_prev - wp ub_pw[0] += delta_s * np.cos(wp_prev.psi) ub_pw[1] += delta_s * np.cos(wp_prev.psi) @@ -660,14 +673,24 @@ class ReferencePath: angle_ub) lb_ls = wp.x - lb * np.cos(angle_lb), wp.y - lb * np.sin( angle_lb) + bound_cells_sm = (ub_ls, lb_ls) + # Compute cell on bound for computed distance ub and lb + ub_ls = wp.x + (ub + safety_margin) * np.cos(angle_ub), wp.y + (ub + safety_margin) * np.sin( + angle_ub) + lb_ls = wp.x - (lb - safety_margin) * np.cos(angle_lb), wp.y - (lb - safety_margin) * np.sin( + angle_lb) bound_cells = (ub_ls, lb_ls) # Append results ub_hor.append(ub) lb_hor.append(lb) border_cells_hor.append(list(bound_cells)) + border_cells_hor_sm.append(list(bound_cells_sm)) - return np.array(ub_hor), np.array(lb_hor), border_cells_hor + # Assign dynamic border cells to waypoints + wp.dynamic_border_cells = bound_cells_sm + + return np.array(ub_hor), np.array(lb_hor) if __name__ == '__main__': @@ -711,29 +734,22 @@ if __name__ == '__main__': circular=False) obs1 = Obstacle(cx=-6.3, cy=-11.1, radius=0.20) obs2 = Obstacle(cx=-2.2, cy=-6.8, radius=0.25) - obs3 = Obstacle(cx=1.7, cy=-1.0, radius=0.15) - obs4 = Obstacle(cx=2.0, cy=-1.2, radius=0.25) - obs5 = Obstacle(cx=2.2, cy=-0.86, radius=0.1) - obs6 = Obstacle(cx=2.33, cy=-0.7, radius=0.1) - obs7 = Obstacle(cx=2.67, cy=-0.73, radius=0.1) - obs8 = Obstacle(cx=6.42, cy=3.97, radius=0.3) + 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) - obs10 = Obstacle(cx=7.14, cy=5.7, radius=0.1) - reference_path.add_obstacles([obs1, obs2, obs3, obs4, obs5, obs6, obs7, obs8, obs9, obs10]) + reference_path.add_obstacles([obs1, obs2, obs4, obs8, obs9]) else: reference_path = None print('Invalid path!') exit(1) - ub, lb, border_cells = reference_path.update_path_constraints(0, reference_path.n_waypoints, 0.02, 0.01) + ub, lb, border_cells = reference_path.update_path_constraints(0, + reference_path.n_waypoints, 0.02, 0.01) # Get x and y locations of border cells for upper and lower bound for wp_id in range(reference_path.n_waypoints): - if ub[wp_id] > 0.0 and lb[wp_id] > 0.0: - print(wp_id) reference_path.waypoints[wp_id].border_cells = border_cells[wp_id] reference_path.show() - plt.show() diff --git a/simulation.py b/simulation.py index c7062ee..d98f663 100644 --- a/simulation.py +++ b/simulation.py @@ -26,6 +26,17 @@ if __name__ == '__main__': 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.05) + obs3 = Obstacle(cx=-0.7, cy=-1.5, radius=0.05) + obs4 = Obstacle(cx=-0.3, cy=-1.0, radius=0.05) + obs5 = Obstacle(cx=0.3, cy=-1.0, radius=0.05) + obs6 = Obstacle(cx=0.75, cy=-1.5, radius=0.05) + obs7 = Obstacle(cx=0.7, cy=-0.9, radius=0.05) + obs8 = Obstacle(cx=1.2, cy=0.0, radius=0.05) + reference_path.add_obstacles([obs1, obs2, obs3, obs4, obs5, obs6, obs7, + obs8]) elif sim_mode == 'Q': map = Map(file_path='map_floor2.png') wp_x = [-9.169, 11.9, 7.3, -6.95] @@ -36,25 +47,18 @@ if __name__ == '__main__': 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) + reference_path.add_obstacles([obs1, obs2, obs4, obs8, obs9]) else: print('Invalid Simulation Mode!') map, wp_x, wp_y, path_resolution, reference_path \ = None, None, None, None, None exit(1) - obs1 = Obstacle(cx=0.0, cy=0.0, radius=0.05) - obs2 = Obstacle(cx=-0.8, cy=-0.5, radius=0.05) - obs3 = Obstacle(cx=-0.7, cy=-1.5, radius=0.07) - obs4 = Obstacle(cx=-0.3, cy=-1.0, radius=0.07) - obs5 = Obstacle(cx=0.3, cy=-1.0, radius=0.05) - obs6 = Obstacle(cx=0.75, cy=-1.5, radius=0.07) - obs7 = Obstacle(cx=0.7, cy=-0.9, radius=0.08) - obs8 = Obstacle(cx=1.2, cy=0.0, radius=0.08) - obs9 = Obstacle(cx=0.7, cy=-0.1, radius=0.05) - obs10 = Obstacle(cx=1.1, cy=-0.6, radius=0.07) - reference_path.add_obstacles([obs1, obs2, obs3, obs4, obs5, obs6, obs7, - obs8, obs9, obs10]) - ################ # Motion Model # ################ @@ -65,7 +69,7 @@ if __name__ == '__main__': t_0 = 0.0 V_MAX = 2.5 - car = BicycleModel(length=0.12, width=0.06, reference_path=reference_path, + car = BicycleModel(length=0.56, width=0.33, reference_path=reference_path, e_y=e_y_0, e_psi=e_psi_0, t=t_0) ############## @@ -83,7 +87,7 @@ if __name__ == '__main__': mpc = MPC(car, N, Q, R, QN, StateConstraints, InputConstraints) # Compute speed profile - SpeedProfileConstraints = {'a_min': -1.0, 'a_max': 1.0, + SpeedProfileConstraints = {'a_min': -0.05, 'a_max': 0.5, 'v_min': 0, 'v_max': V_MAX, 'ay_max': 1.0} car.reference_path.compute_speed_profile(SpeedProfileConstraints) @@ -92,7 +96,7 @@ if __name__ == '__main__': ############## # Sampling time - Ts = 0.20 + Ts = 0.05 t = 0 car.set_sampling_time(Ts) @@ -121,8 +125,8 @@ if __name__ == '__main__': # Plot path and drivable area reference_path.show() - plt.scatter(x_log, y_log, c=v_log, s=10) - plt.colorbar() + #plt.scatter(x_log, y_log, c=v_log, s=10) + #plt.colorbar() # Plot MPC prediction mpc.show_prediction() @@ -137,6 +141,5 @@ if __name__ == '__main__': plt.title('MPC Simulation: v(t): {:.2f}, delta(t): {:.2f}, Duration: ' '{:.2f} s'.format(u[0], u[1], t)) - plt.pause(0.00001) - print(min(v_log)) + plt.pause(0.000001) plt.show() diff --git a/spatial_bicycle_models.py b/spatial_bicycle_models.py index 85be2cb..ef6be94 100644 --- a/spatial_bicycle_models.py +++ b/spatial_bicycle_models.py @@ -240,7 +240,7 @@ class SpatialBicycleModel(ABC): # Model ellipsoid around the car length = self.l / np.sqrt(2) - width = self.w / np.sqrt(2) + width = self.w / np.sqrt(2) + 0.02 return length, width From b78f030378dd5243cc9924148a846e06841ac4e1 Mon Sep 17 00:00:00 2001 From: matssteinweg Date: Thu, 12 Dec 2019 08:34:14 +0100 Subject: [PATCH 30/36] Tidy up a bit --- MPC.py | 4 +- map.py | 45 ++++++++++++- reference_path.py | 130 ++++++++++++++++++++------------------ simulation.py | 56 ++++++++-------- spatial_bicycle_models.py | 6 +- 5 files changed, 142 insertions(+), 99 deletions(-) diff --git a/MPC.py b/MPC.py index 6c86b21..654de44 100644 --- a/MPC.py +++ b/MPC.py @@ -112,7 +112,7 @@ class MPC: umax_dyn[self.nu*n] = vmax_dyn # Compute dynamic constraints on e_y - ub, lb = self.model.reference_path.update_path_constraints( + 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]) xmin_dyn[0] = self.model.spatial_state.e_y @@ -249,5 +249,5 @@ class MPC: """ plt.scatter(self.current_prediction[0], self.current_prediction[1], - c=PREDICTION, s=5) + c=PREDICTION, s=30) diff --git a/map.py b/map.py index bbb5c24..fda48db 100644 --- a/map.py +++ b/map.py @@ -1,7 +1,8 @@ import numpy as np import matplotlib.pyplot as plt -# from skimage.morphology import remove_small_holes +from skimage.morphology import remove_small_holes from PIL import Image +from skimage.draw import line_aa class Map: @@ -23,6 +24,9 @@ class Map: self.origin = origin # x and y coordinates of map origin # (bottom-left corner) in m + self.obstacles = list() + self.boundaries = list() + def w2m(self, x, y): """ World2Map. Transform coordinates from global coordinate system to @@ -49,10 +53,45 @@ class Map: return x, y + def add_obstacles(self, obstacles): + """ + Add obstacles to the path. + :param obstacles: list of obstacle objects + """ + + # Extend list of obstacles + self.obstacles.extend(obstacles) + + # Iterate over list of obstacles + for obstacle in obstacles: + # Compute radius of circular object in pixels + radius_px = int(np.ceil(obstacle.radius / self.resolution)) + # Get center coordinates of obstacle in map coordinates + cx_px, cy_px = self.w2m(obstacle.cx, obstacle.cy) + + # Add circular object to map + y, x = np.ogrid[-radius_px: radius_px, -radius_px: radius_px] + index = x ** 2 + y ** 2 <= radius_px ** 2 + self.data[cy_px-radius_px:cy_px+radius_px, cx_px-radius_px: + cx_px+radius_px][index] = 0 + + def add_boundary(self, boundaries): + + # Extend list of boundaries + self.boundaries.extend(boundaries) + + # Iterate over list of boundaries + for boundary in boundaries: + sx = self.w2m(boundary[0][0], boundary[0][1]) + gx = self.w2m(boundary[1][0], boundary[1][1]) + path_x, path_y, _ = line_aa(sx[0], sx[1], gx[0], gx[1]) + for x, y in zip(path_x, path_y): + self.data[y, x] = 0 + def process_map(self): - #self.data = remove_small_holes(self.data, area_threshold=5, - # connectivity=8).astype(np.int8) self.data = np.where(self.data >= 100, 1, 0) + self.data = remove_small_holes(self.data, area_threshold=5, + connectivity=8).astype(np.int8) diff --git a/reference_path.py b/reference_path.py index 38a9f6e..08507fa 100644 --- a/reference_path.py +++ b/reference_path.py @@ -1,7 +1,7 @@ import numpy as np import math from map import Map -from skimage.draw import line +from skimage.draw import line_aa import matplotlib.pyplot as plt import matplotlib.patches as plt_patches from scipy import sparse @@ -77,7 +77,7 @@ class Obstacle: # Draw circle circle = plt_patches.Circle(xy=(self.cx, self.cy), radius= - self.radius, color=OBSTACLE) + self.radius, color=OBSTACLE, zorder=20) ax = plt.gca() ax.add_patch(circle) @@ -132,9 +132,6 @@ class ReferencePath: # Compute path width (attribute of each waypoint) self._compute_width(max_width=max_width) - # Obstacles on path - self.obstacles = list() - def _construct_path(self, wp_x, wp_y): """ Construct path from given waypoints. @@ -158,6 +155,8 @@ 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) - @@ -166,6 +165,8 @@ 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)) @@ -293,7 +294,7 @@ class ReferencePath: # Get Bresenham paths to all possible cells paths = [] for t_x, t_y in zip(tn_x, tn_y): - x_list, y_list = line(wp_x, wp_y, t_x, t_y) + x_list, y_list, _ = line_aa(wp_x, wp_y, t_x, t_y) paths.append(zip(x_list, y_list)) # Compute minimum distance to border cell @@ -398,28 +399,6 @@ class ReferencePath: return self.waypoints[wp_id] - def add_obstacles(self, obstacles): - """ - Add obstacles to the path. - :param obstacles: list of obstacle objects - """ - - # Extend list of obstacles - self.obstacles.extend(obstacles) - - # Iterate over list of obstacles - for obstacle in obstacles: - # Compute radius of circular object in pixels - radius_px = int(np.ceil(obstacle.radius / self.map.resolution)) - # Get center coordinates of obstacle in map coordinates - cx_px, cy_px = self.map.w2m(obstacle.cx, obstacle.cy) - - # Add circular object to map - y, x = np.ogrid[-radius_px: radius_px, -radius_px: radius_px] - index = x ** 2 + y ** 2 <= radius_px ** 2 - self.map.data[cy_px-radius_px:cy_px+radius_px, cx_px-radius_px: - cx_px+radius_px][index] = 0 - def show(self, display_drivable_area=True): """ Display path object on current figure. @@ -436,7 +415,7 @@ class ReferencePath: # Plot map in gray-scale and set extent to match world coordinates canvas = np.ones(self.map.data.shape) - #canvas = np.flipud(self.map.data) + # canvas = np.flipud(self.map.data) plt.imshow(canvas, cmap='gray', extent=[self.map.origin[0], self.map.origin[0] + self.map.width * self.map.resolution, @@ -455,7 +434,9 @@ class ReferencePath: wp_lb_y = np.array([wp.static_border_cells[1][1] for wp in self.waypoints]) # Plot waypoints - plt.scatter(wp_x, wp_y, color=WAYPOINTS, s=10) + + # colors = [wp.v_ref for wp in self.waypoints] + plt.scatter(wp_x, wp_y, c=WAYPOINTS, s=10) # Plot arrows indicating drivable area if display_drivable_area: @@ -482,8 +463,8 @@ class ReferencePath: # If circular path, connect start and end point if self.circular: - plt.plot(bl_x, bl_y, color=OBSTACLE) - plt.plot(br_x, br_y, color=OBSTACLE) + plt.plot(bl_x, bl_y, color='#5E5E5E') + plt.plot(br_x, br_y, color='#5E5E5E') # If not circular, close path at start and end else: plt.plot(bl_x[:-1], bl_y[:-1], color=OBSTACLE) @@ -494,19 +475,23 @@ class ReferencePath: # Plot dynamic path constraints # Get x and y locations of border cells for upper and lower bound wp_ub_x = np.array( - [wp.dynamic_border_cells[0][0] for wp in self.waypoints]) + [wp.dynamic_border_cells[0][0] for wp in self.waypoints]+ + [self.waypoints[0].static_border_cells[0][0]]) wp_ub_y = np.array( - [wp.dynamic_border_cells[0][1] for wp in self.waypoints]) + [wp.dynamic_border_cells[0][1] for wp in self.waypoints]+ + [self.waypoints[0].static_border_cells[0][1]]) wp_lb_x = np.array( - [wp.dynamic_border_cells[1][0] for wp in self.waypoints]) + [wp.dynamic_border_cells[1][0] for wp in self.waypoints]+ + [self.waypoints[0].static_border_cells[1][0]]) wp_lb_y = np.array( - [wp.dynamic_border_cells[1][1] for wp in self.waypoints]) - plt.plot(wp_ub_x, wp_ub_y, color=PATH_CONSTRAINTS) - plt.plot(wp_lb_x, wp_lb_y, color=PATH_CONSTRAINTS) + [wp.dynamic_border_cells[1][1] for wp in self.waypoints]+ + [self.waypoints[0].static_border_cells[1][1]]) + plt.plot(wp_ub_x, wp_ub_y, c=PATH_CONSTRAINTS) + plt.plot(wp_lb_x, wp_lb_y, c=PATH_CONSTRAINTS) # Plot obstacles - for obstacle in self.obstacles: - obstacle.show() + for obstacle in self.map.obstacles: + obstacle.show() def _compute_free_segments(self, wp, min_width): """ @@ -526,7 +511,7 @@ class ReferencePath: wp.static_border_cells[1][1]) # Compute path from left border cell to right border cell - x_list, y_list = line(ub_p[0], ub_p[1], lb_p[0], lb_p[1]) + x_list, y_list, _ = line_aa(ub_p[0], ub_p[1], lb_p[0], lb_p[1]) # Initialize upper and lower bound of drivable area to # upper bound of path @@ -690,13 +675,13 @@ class ReferencePath: # Assign dynamic border cells to waypoints wp.dynamic_border_cells = bound_cells_sm - return np.array(ub_hor), np.array(lb_hor) + return np.array(ub_hor), np.array(lb_hor), border_cells_hor_sm if __name__ == '__main__': # Select Path | 'Race' or 'Q' - path = 'Race' + path = 'Q' # Create Map if path == 'Race': @@ -713,31 +698,51 @@ if __name__ == '__main__': 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.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.05) + obs4 = Obstacle(cx=-0.3, cy=-1.0, radius=0.08) obs5 = Obstacle(cx=0.3, cy=-1.0, radius=0.05) obs6 = Obstacle(cx=0.75, cy=-1.5, radius=0.05) - obs7 = Obstacle(cx=0.7, cy=-0.9, radius=0.05) - obs8 = Obstacle(cx=1.2, cy=0.0, radius=0.05) - reference_path.add_obstacles([obs1, obs2, obs3, obs4, obs5, obs6, obs7, + obs7 = Obstacle(cx=0.7, cy=-0.9, radius=0.07) + obs8 = Obstacle(cx=1.2, cy=0.0, radius=0.08) + 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] + # wp_x = [-9.169, 11.9, 7.3, -6.95] + # wp_y = [-15.678, 10.9, 14.5, -3.31] + 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.20 # m / wp + path_resolution = 0.2 # m / wp reference_path = ReferencePath(map, wp_x, wp_y, path_resolution, - smoothing_distance=5, max_width=1.5, - 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) - reference_path.add_obstacles([obs1, obs2, obs4, obs8, obs9]) + 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 = ((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) else: reference_path = None @@ -745,10 +750,13 @@ if __name__ == '__main__': exit(1) ub, lb, border_cells = reference_path.update_path_constraints(0, - reference_path.n_waypoints, 0.02, 0.01) + reference_path.n_waypoints, 0.1, 0.01) + SpeedProfileConstraints = {'a_min': -0.1, 'a_max': 0.5, + 'v_min': 0, 'v_max': 1.0, 'ay_max': 4.0} + reference_path.compute_speed_profile(SpeedProfileConstraints) # Get x and y locations of border cells for upper and lower bound for wp_id in range(reference_path.n_waypoints): - reference_path.waypoints[wp_id].border_cells = border_cells[wp_id] + reference_path.waypoints[wp_id].dynamic_border_cells = border_cells[wp_id] reference_path.show() plt.show() diff --git a/simulation.py b/simulation.py index d98f663..5ec171d 100644 --- a/simulation.py +++ b/simulation.py @@ -10,7 +10,7 @@ from scipy import sparse if __name__ == '__main__': # Select Simulation Mode | 'Race' or 'Q' - sim_mode = 'Q' + sim_mode = 'Race' # Create Map if sim_mode == 'Race': @@ -28,15 +28,17 @@ if __name__ == '__main__': 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.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.05) - obs5 = Obstacle(cx=0.3, cy=-1.0, radius=0.05) - obs6 = Obstacle(cx=0.75, cy=-1.5, radius=0.05) - obs7 = Obstacle(cx=0.7, cy=-0.9, radius=0.05) - obs8 = Obstacle(cx=1.2, cy=0.0, radius=0.05) - reference_path.add_obstacles([obs1, obs2, obs3, obs4, obs5, obs6, obs7, - obs8]) + 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') wp_x = [-9.169, 11.9, 7.3, -6.95] @@ -52,7 +54,7 @@ if __name__ == '__main__': 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) - reference_path.add_obstacles([obs1, obs2, obs4, obs8, obs9]) + map.add_obstacles([obs1, obs2, obs4, obs8, obs9]) else: print('Invalid Simulation Mode!') map, wp_x, wp_y, path_resolution, reference_path \ @@ -67,9 +69,9 @@ if __name__ == '__main__': e_y_0 = 0.0 e_psi_0 = 0.0 t_0 = 0.0 - V_MAX = 2.5 + V_MAX = 1.0 - car = BicycleModel(length=0.56, width=0.33, reference_path=reference_path, + 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) ############## @@ -77,9 +79,9 @@ if __name__ == '__main__': ############## N = 30 - Q = sparse.diags([1.0, 0.0, 0.0]) - R = sparse.diags([1.0, 0.0]) - QN = sparse.diags([0.0, 0.0, 0.0]) + Q = sparse.diags([0.3, 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])} StateConstraints = {'xmin': np.array([-np.inf, -np.inf, -np.inf]), @@ -87,8 +89,8 @@ if __name__ == '__main__': mpc = MPC(car, N, Q, R, QN, StateConstraints, InputConstraints) # Compute speed profile - SpeedProfileConstraints = {'a_min': -0.05, 'a_max': 0.5, - 'v_min': 0, 'v_max': V_MAX, 'ay_max': 1.0} + SpeedProfileConstraints = {'a_min': -0.1, 'a_max': 0.5, + 'v_min': 0, 'v_max': V_MAX, 'ay_max': 4.0} car.reference_path.compute_speed_profile(SpeedProfileConstraints) ############## @@ -119,27 +121,21 @@ if __name__ == '__main__': y_log.append(car.temporal_state.y) v_log.append(u[0]) - ################### - # Plot Simulation # - ################### + # Increase simulation time + t += Ts # Plot path and drivable area reference_path.show() - #plt.scatter(x_log, y_log, c=v_log, s=10) - #plt.colorbar() - - # Plot MPC prediction - mpc.show_prediction() # Plot car car.show() - # Increase simulation time - t += Ts + # Plot MPC prediction + if mpc.current_prediction is not None: + mpc.show_prediction() # Set figure title plt.title('MPC Simulation: v(t): {:.2f}, delta(t): {:.2f}, Duration: ' '{:.2f} s'.format(u[0], u[1], t)) - - plt.pause(0.000001) - plt.show() + plt.axis('off') + plt.pause(0.001) diff --git a/spatial_bicycle_models.py b/spatial_bicycle_models.py index ef6be94..8687ef7 100644 --- a/spatial_bicycle_models.py +++ b/spatial_bicycle_models.py @@ -240,8 +240,8 @@ class SpatialBicycleModel(ABC): # Model ellipsoid around the car length = self.l / np.sqrt(2) - width = self.w / np.sqrt(2) + 0.02 - + width = self.w / np.sqrt(2) + widht = 0 return length, width def get_current_waypoint(self): @@ -310,7 +310,7 @@ class SpatialBicycleModel(ABC): # Add rectangle to current axis ax = plt.gca() - ax.add_patch(safety_margin) + #ax.add_patch(safety_margin) ax.add_patch(car) @abstractmethod From 0cb502445f8bd2a2d75921a05666ff60104050c2 Mon Sep 17 00:00:00 2001 From: matssteinweg Date: Wed, 1 Jan 2020 02:14:59 +0100 Subject: [PATCH 31/36] Tidy up a bit. Add comments. --- map.py | 67 ++++++++++++++++++++++++++++++++++++++++------------------ 1 file changed, 46 insertions(+), 21 deletions(-) diff --git a/map.py b/map.py index fda48db..41affc3 100644 --- a/map.py +++ b/map.py @@ -6,24 +6,36 @@ from skimage.draw import line_aa class Map: - """ - Handle for map message. Contains a subscriber to the map topic and - processes the map. Numpy array version of the - map available as member variable. - """ - def __init__(self, file_path, value_unknown=50, threshold_occupied=90, origin=[-30.0, -24.0], resolution=0.059999): + def __init__(self, file_path, 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. + :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 + """ - self.value_unknown = value_unknown + # Set binarization threshold self.threshold_occupied = threshold_occupied - # instantiate member variables - self.data = np.array(Image.open(file_path))[:, :, 0] # numpy array containing map data + + # Numpy array containing map data + self.data = np.array(Image.open(file_path))[:, :, 0] + + # Process raw map image self.process_map() + + # Store meta information self.height = self.data.shape[0] # height of the map in px self.width = self.data.shape[1] # width of the map in px self.resolution = resolution # resolution of the map in m/px self.origin = origin # x and y coordinates of map origin # (bottom-left corner) in m + # Containers for user-specified additional obstacles and boundaries self.obstacles = list() self.boundaries = list() @@ -35,18 +47,18 @@ class Map: :param y: y coordinate in global coordinate system :return: discrete x and y coordinates in px """ - d_x = np.floor((x - self.origin[0]) / self.resolution) - d_y = np.floor((y - self.origin[1]) / self.resolution) + dx = int(np.floor((x - self.origin[0]) / self.resolution)) + dy = int(np.floor((y - self.origin[1]) / self.resolution)) - return int(d_x), int(d_y) + return dx, dy def m2w(self, dx, dy): """ - World2Map. Transform coordinates from global coordinate system to - map coordinates. - :param x: x coordinate in global coordinate system - :param y: y coordinate in global coordinate system - :return: discrete x and y coordinates in px + Map2World. Transform coordinates from map coordinate system to + global coordinates. + :param dx: x coordinate in map coordinate system + :param dy: y coordinate in map coordinate system + :return: x and y coordinates of cell center in global coordinate system """ x = (dx + 0.5) * self.resolution + self.origin[0] y = (dy + 0.5) * self.resolution + self.origin[1] @@ -55,15 +67,16 @@ class Map: def add_obstacles(self, obstacles): """ - Add obstacles to the path. + Add obstacles to the map. :param obstacles: list of obstacle objects """ # Extend list of obstacles self.obstacles.extend(obstacles) - # Iterate over list of obstacles + # Iterate over list of new obstacles for obstacle in obstacles: + # Compute radius of circular object in pixels radius_px = int(np.ceil(obstacle.radius / self.resolution)) # Get center coordinates of obstacle in map coordinates @@ -76,6 +89,11 @@ class Map: cx_px+radius_px][index] = 0 def add_boundary(self, boundaries): + """ + Add boundaries to the map. + :param boundaries: list of tuples containing coordinates of boundaries' + start and end points + """ # Extend list of boundaries self.boundaries.extend(boundaries) @@ -89,12 +107,19 @@ class Map: self.data[y, x] = 0 def process_map(self): - self.data = np.where(self.data >= 100, 1, 0) + """ + 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') From 0cee9c73b8d5e15eb93c3a35366ac45be49c1bf0 Mon Sep 17 00:00:00 2001 From: matssteinweg Date: Wed, 1 Jan 2020 20:46:32 +0100 Subject: [PATCH 32/36] Create MPC_Framework.pdf --- Images/MPC_Framework.pdf | Bin 0 -> 20453 bytes 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 Images/MPC_Framework.pdf diff --git a/Images/MPC_Framework.pdf b/Images/MPC_Framework.pdf new file mode 100644 index 0000000000000000000000000000000000000000..f1ea73b4f631949bda40c04e4acf69a5d078a8b7 GIT binary patch literal 20453 zcmaHx1CXT6)}Y(AZQHh{-92sFwtL#PZQHhO+nBbE?eE?j|J~i#*odk(vrZo5$*MY8 z`PM@!FCs?ENXG(2x^Vt?4~mh1fxyv*vjGI8h#>v=<;733pr%0eiz{sG>$g0brMM)rNXJ=)sYHT27?nKYZ%udJ5 z_G2g|t|TK(LttfYX-x3T*wE7UM^VVu&0SID7dx2QwWb z6C;7Bm9e3dgRRX!@#z?u2^95B^c~Fq-X+B#V4Tm4sw<-bb8g8wM}TUA<^fB}kLTJm34)qmtN(gZ9}^ooK6 zP7cn-|8HMx|MewiZsqioCB2x{Pd*~XhPFmOHqyp6rcP!A9L#Kte0&6sPCxINHI!TC zrS?WN38#&BO--mNFebHvYcIntuG^D)7w`-4dNymM1+WhP>$Nu=Dx!jk@nRKr!xHN{ zDA>{@M36{aNTP^B#hb;qCeD|SdWg?k`U+K3^3lXf$hZ6MHC8|+Wl^hmxOtU^g~rzt zKK%C+{DjW|Vox|3t%0@nCot`3|M&NKPV&>p&ez*b2>D+oCH9AE9L}A?ZVq1!NGw=1WHA%e&dwM|=G21exd7!Ps!(+U)%=3d9M#G- z6S!0Z<~rp-S^5Bg`C7Yh6-vdbs#`BF+3J*488CCO%tsoVTdKs`N!-((RpNvkVJEPT4S+E}@5*BlYA%Pb ze);OK@gfz6;^ct9=B?Vyec5qHyB)7B0%7G4{0f-5IV1(AY{{&X2P7f7r=2y zBAhx~x=N`(oZb%3Vn07O8RJ}v{dp{rh!Hw zWA+JcpN`jV)2|+$4|1MFR+L(U4|?7(a*=zI{s)p`az@^;zY@}N1|CQY38~qGyAmHm zE?l#phecxMo}?e-!%@*%jmb?S;E6!Oy?MLyTG%cFy}}d0YUeG2s&8>m~J);;2$VAsaO@Wt}%!r9wI2JrPwG# zhP{-xknJL%>sUDJ;vyB2GGP_Yh1Zfe7}jAR zqiTC)m5x~c6wL|-Q2DX>!oQ#;ULxYJ^K`d#KBRn+=5g$=z%))yNfo+T9)d_MH|0O8 z=C~hWca(Bcc#=WW4Q^xQETowdo3Xb+Za*9(syEI;JpFe92$nkZ0W zp92gb3i3eDbsg65tspBzI3sK0^Zj7(p&Cwc6`WQ2iPzn{qDMQuL_{X+4G-zjq)=EG zqwrC2F8+CHRkkq0fiuoVa&aW7l${tiIA#gp8NQX}nb1IiY+j}R-c(&KTI=pV(QdjLS*qdbW{!s%bznpUPV<;^u0D`&?Z zo)+_i$CSVEweq(k9Z?B18VUg1MCCpngi5r2LL)uSk)BnFmzePaZMSYW&lbubo8Kl* z{J78OYZd9j{L5geK|8+!ZIX1`h5Wbp-zIgW<%C&Bi0_%6D)umboum5<_-84RD?&s* z-MaBLXI+ap!5f>!$FVLM=>_ekP?2-}ddWv9g@o0qEXR0-gkNf($%PDHR0*S2V7Nl* zOD&b)oIp8CEf=9qs01=p_Y}e67G4|kv>ym|GFZsW)xr&Jq7x`^xc;pY&~1$fH?*r6 zEGEZ=6_YrRw4NkzX`F>~P)UTdnT^RYOu>Z}wrSLVAauy1FzL8J+7g~vm_rsfCUNFD zYK%i`cFb4OIJ3&zPfQ+KBpPPFz7UnA09LWAw)U6|XHZUY$(xQ)9sm_~H7sOJK}Dh8 z63>T6hgl+C{rSe`)1>?i*9RD&Nf`NwY#djQBkqQ!!;9poC8S$#0s!RKQ`6_j^wCij z(gyP;=zd!;joK6mKRF~wRc(~GVjk&E;+t2os~>aIGM^xL#6L}DR^t{n|6D~283#nc zybVp$pbg_a&5B_O+o_~h^yW2OWL6%9cw$XgA2YL>79V?! zknH^Lb?={2c+34jk}Y%hDE%3OxDaA{gwku7l0)|T)*;co58}1xl0D?4cw;+rCzCu@ z0TmgKHE$h$y@21GsaNKCv3M(H!{2McF>Uls`my*YE~(SMylYeK<@Ew}g^Mh^V~G$> zeGE%8r7h;NAz{U<2UfTBUWxK@L~orsTveg0yZd9B`xw`;-_Svwp5Z}O*i`!(34x|F?;DpYwK>zP|NfpO5fD0PDT zs+iGnLmcpz&4B%oKhPY~qDmTBR^EA4YovxsyRD{|<f}51Pu71 z2YnwY^>%E4vQ*y#8B>Ai6{NF0u~5Cn5sq`)(nQ;eZgFHp8+P_aI)kJD9k@V)w^b_} zXmqOSZ=u!rK@!JS)?Q>i)*P_LS|pKLZzCB7nl{pEa)6>s%z-z-HFZ`X8H9Z+8ob?1 zK)=g~K1s7${bW)7%Zx;IVBaE+Q`TPE!z**3cGVJ<(L;tfo_v9Y!Nju8o=s4i<`vILT0K34{V= z7R3O<)OkLMy0y6XD&7@@fh+2u0FH@%V3%gQIzH9}47WFNsR8g~@ED9*05BlK*pWta zL^HU5+(J(kj9)`>*83oMM5r@?i8Yv^S>nVU&6!mEly%g4!O{ngWKAT12|pU!#R!0L zfO-JX2~hRz?CrG5cPSBYS$_6{S72;7`jHpMk`9~@u1fECB&yn0KR(U}2q|}Pydg7F z!Cms%)Cw2-#wUqLh&4Kl%<%dT5vy`2&=AjlXOJpvIcqE(7JBdqL6$fwskP9m=h|#P z;4`S;@dhYvneiblUG%@|b-su*S-w^(EhjcV**HOG_EBYePtsrx@IvaTCAyHo*i*$p z^AXTwE3pz~#-nV+!{uwS8%WMgG-o=|W5s z1_KcERj5KlYpNQg{i)#c>}>+NZ7hw%&HHSih}5>C@VNlpA1*k5|H|(N^rB%HXuyek z_d|!FfyN5XuzM`T9#Uq}ltZsoxk!&4CD9%O>h{uE|H^^GbJ(D2UD?u+8xoK(@1zp~ z2tv#apW_&y1oe3Z2$rRZl5Nv5f*(?*v)3@A1Oehz_scg|$9-mRop=gdojKo5#RH%S zNIm$4!3Nei?t7f*VQ6`lRCMMwK@!11v0F;m$yLS7o|=B zT-a%ev$;CH;6=AimN7#m5EbB_kFi`cQ3$AV zh(mTC$0g%E4~V!zqDXV7vE=WTtcHO2zrWq~0qN<9G0xf<@Rd8!vpUU16_ z*Q~+0XxLjr7q#_X3}zy7pJJ>riV%bFe6Ti5A9;W*_>%!6xNM}KRRfY#Pi+@(@qd#K zV>YI*s@c@=P-Y^q5n2hY2G;h_2X;LkB3z+o<9;AQya+mA&u@1L0vhd3CTW>zcky4qrp0hx*Wu@z$&Dg;Jf+AooSpT( zABm-ik9;vZ3ADa(0THxFM!Fv*AC9=Gj*iR81%-&g>)f%)-UZA#w3ANQqU&8~_hQb& zzO}e(0c$qJ><=Q_XKIGJ4C}>jI|C%vHAw1Yo#m49eQetTw}*iek%8o5jm!a%l83^6 zXH!AcvlP#~4=BkcEyWSN(&Ib$gyKtUX|r4qp@`m0(hYt+TobW|T07}R&bYC1VB*^# z8P{!D51iR?$%QQMIXfqu8%S-x(T(akAL}iz{tc|yaTj%g|IjG6lso6GaBcpb1WdX; zjdLpC4li=V$OSzxpy0+O4=@B(;q@B~H&4;HjFOp*sS7%~{$AysZzCyeDEg>}?~nq9 zYG9T7@izFZaSlUfa8_pW>Fk;ZGOVt(FHXfHW8nE(@%`DA4%614l!u}dLrSlX{TKyr zsu7$4euyY>bp-XK$p!l)T|W>q4Fous52Hl(BtZP`q^-8&X%{Ibta zqZuvadYjOCXGN7AnJ>pY&2;Tp$#a0A7$>wFl*m&HIe8uH zyX;97zoXEa8u{0aU0H5V($FH#dr?{ZF-U)zP_&rffF35~m)T1iXwqW5roj+L^5h5b zv6NS<6zNP$#5fgAxQ$kFx(MG+R{T4=uU)cS|5jf*CF>Jd@$__ro1{`pK)>##xo_m9 zC39-JyY6ud6WCU7y8C&mvy=hBnuH5&34yBIBoSwQDXRT~XSV)rW}-P)ZSi(AB?iki z@8S+pK(4tcO~kksP$G^vq~2JJh0;=L{;Y9S#+9 zj-;#$+eiVN6R!qy97K|4H&_gmwnZ8>&mM^7>BQZkYFmxP%)+q{z9e_L-=Pxs8k8I6 z*|jDW!iAcdgi4<**3i+V8#v!@OYmA0!<$PADK`qUPe}X6_0Gaxk%`_&hi3XyxnoNF zpixuAiNHEpJtdY?KL};5%?-ak_7t?wTfMvqO)$-Vf{WMI{_=&;mQ2oxDYG#hMA?K! zJs_5FPNOfx>=?*kE87t8oj(cYYToB?5Acen^$0MAzBpFdCvmkF>a=~-fNqxCGllv^ z3R1V@K(t#M5)6`>I*NC`EW``k612MEkfL+oYYynVUYI7oL@GFkl?s0qG?)P4#_5qR zujeYI9;jT5)~=>m)^9@?%{CD?QX~r$t);t4`y9yfoK)pAU)pmd-9Rf6GY6GkO;L0p zXp&+1Qe$Ou;>{IK8pVizt2uI`I-4;E_9j-0Dq*KGSb{%@w2eM>-iUbEngX*R|0?AN z*GzLO68a30r;ZT+!zy)@R%1*BHvVi&l52fK&F{kR_u?``zX2;u3wD)*-$>lEN{S^WXjKv)uVMJ z@Y`DbY<+G4}Ewb{9yaWD|W5hcRfmIL)Zz zLc5KncHe0lM@b|8h&v&AY(xn**YrkNy90At=NHh1jrN8Qd@gTplV7e_{TePA8`r*G zs=Rz|QZ1QgByn&&%59)^Jn!8~re(7}`M5l*p%sUVBfV#&47Pm}q(v#C6#W5%7wHDm zIXI2o_74;5W+)A9MsT>X8#OcAWy7v+r=m0SWfu4)DMrcbc7`+ z29wLmV!k&rJsm?tjYFs!(>m5o(A|{1q_r4o8yML)vDl7Kp=#`KZkN?78keZ2GCdKG z{^O<32kqCnI!CB}FEVKfh+JYO)04UJ#U;hscm?Ni5mf}~QS8Ft807#mc}b+V4yGr# z?41He&N?bjH01PevY(j$n$i9Es&00bSOzn)w)(G!e(G<9yZC!onp5^`@0{oE8PA=Q zuIu~lXZP!Ont^-npXV0T+yA@n3HD^FRmLcf(Z&9eb!t_dn{hP2ZHXBU|I;Z)0+I~g z&@sum=PBA_#`>PP6oGX3L~)ERNqs+JohB*B@uyVtlfM@WAop~lX7CBI av+0pYA z)%;VT1CA0VN6z$<%W~Wr6j~jMHU#5sr9~llQ^P>JF;6qEOW}F8wLsGyAMznrAvi|$ zivx`x=3h6mt-)UCb>tILjMy)FacIzI=Cx2g9=w`_`%N2hYkU1Qvl%^gt32=>GevRf z+3Uu8l1mI&D|6>*5{(~Q1CcB?{MFqNrK_Ue8ka^Aa5v->hU_}nip6vio3!-0eM>v_ zV0}m_ICw2(TTX&6hKL-({f369#)@i-0QUCk8HTjbm-j2GDwk<*pZHLYk*rl4F{qhH zxPj2QE^ot!H^#*QRmkk?b+H`pap*-PR1@blvAaEU)_XcvFaW5-7sQxm_VJXYtQ&yrs<%C z;rL#-h0(yfR%UGL`0~WN{&ji4hYJO}h=J#PAgLE6fK0WmZpx30V0fcKfzadEQxUua zXglq)yO+C4y|qs3}l27v6X zg&C@%wB_>YEBnBo!;R#^oT+MHYL!)vuL~v+OsJQ?)+-g4 zT$%@fh!c+`E5f-Z$VP4lfS@hfj)L`<<#%U* zF>@m>pn)ie6|2|klXGibXn?!>u#Uv8`$KS{ZZe`vhyH5rKD9H9g9wY0#FX>-92EtZ zel|BN>yO~EUnoytiLPFaA(p>$))%J31aawqi~G4^-AMW>@!CezQ^S@IT6}?C4^qq> zxhw$^H<+gh98sFupfiGOPi@k0=xs}ImR{a*YPbw*EpL?IMuzLKU#Kd3;*iLoRA6;d5q4is_)q3;~hKo9^iYl0CjF>dh9i9u*o<|sGrFQ#< ztQ?x24zP!dS>+8;l{PW!s!6Xd30(0c!M|gxc>e%&EIM4Q_TVf|*Kg~y3BU$D$YtB( z8Z>kBKxoT55)`OupBr4eg2Lq5645<-1jxvrmFhGJ`J0HEgqzBKrh9To`~`T1kY8 z(a>M$HSskY)0eRT;6a2h_eLKUQ8G!bd_v?yclWQ$H(rDnx_j%)M38Vx#ZdsPY3i`Z zp|NCe2$qw(AdyPtJ@8lz7PSxZvUGRYHp;Stc~SOzj7T+qhogMylxllomF*_8E~}BNlpFWFr7hlL%L5EQw9Db}iLqnS>2;eaSXFob?B^28+SH`@u=0 zv@Kz|uxjPz>?^~4*b+vMHoUGA*wm%eb<8HfSJYpE&;}I`)K?1#C`ZcY10r!0M;>DA z>r02W*Z46Ws{~8NR*N>r6==$v+`E2g`tV=;Z)bUUWkuu4E9E!^)0N)ql_4gb?;Ay+eL9>HKD?8 z&XV2YsqB4Dr~jQQJ&GU~TXmoSzpPtOIEM4JM6jGUuoVt0(%~QTywL70Hl~UF6vOWJ z{;96pHRqhrhg~Fa@Iw9KIw$LFi9E1JN3ac#RFN(I!i|!V434!DYNc2Doqr+5K?fSB zTM#Q~1TftfZW1t5TZUT4sddZ=o1KCg*NIvPo6}ZkgiE}?^R5Kjs(UYK=zH?`t@)Mk zt&E+K5xs0fzaCQVOqw%YtRfPVjWX1)%nxrBo2~NZ$M(1DeUKmUiY8(9I-MbH{%sz zB+fC?yG#||g%ZaE?M-*>@LR|4K?$t!XBqA>W%<5_!xu5$Wuk`w6Q&k4QYm(~mNOYV zG$;4##t&6831o~ZTA;_Dhc>6cZUUI24^0AH`!%EE-#}%K_`SIaU#5EAvgoC~SEENI zslwYn?7fO(r{u^pU(;C1#qe?#;Cs7+zJ;TB;8~+HsApxqQQb^Ug5@H+LwBdU8u_-s zM#KFD;?J5sZllYw^yBsg94=2zDf)|oKD~6{MRWTSTgy{x{EW-LT(GeAOGo+p&Q6HO zxNpYvz#TVp&Euxa&gI(}mRon16!a~o#{c4e&PNW|&BBx|w}V{jHkQ1fU>SdpwTlmY zUr~Vj+JAGTYC?Or-RZEt0e2Lm_^(1$uaw34iyXX;9d|2i|2RuO6IbPBf-bNQk=o1$@aL`;)O=N0g=lVQKc&}319pwtYG^CV-m1({m&bgL_kCe5?a-ygK;{o@!A-J6G za*nHxGUXq={FAeuzd7szirzyO*O)F@9kRAALo-X?lLa0zJ`caWyUri`AW5`k8UE(8 z*1ZW*6Ctjo9^L!LyP@_3PG)MH^jD~WPL3b9T`th3dAy8M{_LIc+1F|@%ssBI`GL1? zvcJgR7Ll^JjNG5;L8Y-*;!1!%a9dor17b=pJeJFI=#;0MZgX_)mU9B6Uy8SJhFvcU zcvC&K!?4$8dVjZVbKl+vw#=w%cX&LI#y{l?gC&Tr!YQ;yvY7-ol!fv4m%h#bQHD;%Nya9LRx_*}Q^`Z=%>hd^YNx$!Jo|10NpM6Vjc zWel5byD{AB?czPKH3YJ*o1LTa74B+iz*PSa-NU!Hot<8ia(%U5)3qRF$W=Q0v&KlC zH<09KJM6~tpBlH)z*vOJut{C+PMg#Ay1{42I5vcS*iEN*leO+07o6!7eeV?+;fmlB z?dK3dA*%-8Y!&~;gtrAp6SQGOPhT zl9m1%g!vnEi|hsIjtCWfje=SqG=YHn)EdRhu?>2}k=P{fkw?M2PAs-H@zjU+n_jc_ zILCAc6H!pSc%X0~@Dpj?28*n!7{(kGy{=8=iYHTD$V1PXnqk|bJKAZV<0`1>vXW13 zJxISahaA@`IyRM5IA4B6=oa#148rSMxe)V=<6#F7D_ZEArNBOw;zMU1%o%$T3Y>*O zCs+b{c6%IF4iA}$A=(L-jF!V@^2hP;L~W0MBajLsTM8j#E-92VWAm-FS`U2pC*AVR zCoyxaEh{L1YcBHgrD!_M$rzd0bFaE>nuniDFV2Xb4Sgd!;H57uFWkM1-7*;u_;L>r zDX^g?fF@7Pku7QiklO&yfV-GrrYRbuzbr z#F(_83F+<`yJ-aY8+21sX?-vCId{`~i?%`|j3>;>IJu8)V`um?jPch5i(((kJh<9> zOgix$V`r1{7_MjZ_G}BNy483AIblm z9I!U97GI~VfLQgt;Bli{2gSDJW%}O{o!&$csevr=o#F$2HqUnzB1^Jz$`O?wG^PaR zJ`K1EP3G{(37KFZqaY#82*3X+4bhL5<}-Xrav6lOa$y~ZG1qU0dYf>02wRRY)YRA%;(E5jkT5*lE!~Jgmr2KisxEyAC$EbW$4y?V{Kx2 z+QSCqeL2}1!h2v@guCidy6lMKHJ?5OShRk=5*-ii_bsP9dOQm^!M(Z z(F@8T6|ho4fP2L1jsa3w8}@TC5((G~OI`DMgOUvU;T2>Z>ILS7Y;j1@Q=|ZZp7Mq6 zN-FO%qP084JK&XWRT275=9YVxXV!Xl0!=pU1G?b^AQM~fWJz`*pMgKA9^rUIbExa0 zJ5r;UKD}E^;TSg8)l*{g40IcQfGa34j{Mv`7(5&C*fR~LxwmW1muvKN9s0y%&r{5L z8~Tn1{*JkuS(!LEQYDp~q$ML3`ULV7VmvS>w#NnWE~Nz%ZIr*j`fg_q4T1gz@2v}E zlpD$!#i)NdZ!%6Sk=ua3>zk)zLt!>Xh~GjC^QoRMBZO~VEBtS2|L|NkQ{-@0q{`wB z$~`qyoXH|&V$v&5UAj$=+s}tM!HxZNWCakrK&&e}-+5T0)6yWvP7LZ1&H3bxd#Lr- zd&zaMVDAG%LDo`=ArTC7#?|YpD~vAh(7Q24ZnrY?XJ;qdQv)}kQNPupdob#NNjaJt z0OaqI-wmz6!ect#Fb_$yHG;xzH1UHR@Zk)G^e=}^wO@b_UGhpQZ~@i$WbwZ<1)$&F z5z(aZjP19{Jm3l4SK<&gAu~Lo$FF73cHxip;K40Z3_Bt2*5HltEeAM0(S_71k@VWJ zn@&9fJqUNRpdAqBS_U}nYXyP283zmiC>XN+dOTpH38Z}WaQ+xLX^Dcs-TTf6KM~Zg z0XeY0(!Ju%jUpdXIs~_yMGl+OPR+V_@Z#nYKiIs%XXd;!K7;iH^(6FU>O5Bx@m%kz*!-ijaZ(245#I-jTc_zOp{fuQdiQ{|<7Q<7>~}24q38pK&LhL`nnHRwtPC++oT-x2v)dXx8d%`u19^`y=Ej7=S7PyJ-5xDk2~q(?};304|`GiuVMTorLsZi zruDU@5|__DA11+NIuM>`j;+mDu zkNDiDEIuK$RDE@W2@Wu$!5&tcd5tz}J3uNkTwRd>IBB`_RRW06iZwiN~G zd+1C6q~J{$LWpl>h67cm7%CjHQ#;6REycS>ap zP_4Z&7PAfHr-!nrNZA$2QmQ#(e^rysttnuYfkYi$g%2Dj{C(H&`10@Wh>~Jj%KbYj zCb-);&Qo)VrJY4iQ*{`)Oin}3qoP5%j=>5Nz;UJ0=IVsVXDprW@(i7^aj~85w8csQ zxY!DWl{*9_2_x4HwX_a4Y(lC*wuDBURsgjrS+<;oRuD6<+H=oo(y!X{&fE3d&;6=5 z*tZ^qRt~bS-g3{guILoE>`j_JLd|*2$$COQ7)6ic*y^J)32I_j#3OM+GzdFJbsRt< z-#Iv8sgVxJ3zG}S*hhiwfG^156J}zud#`86p8c-7w4Xs5KYQplufN`*d{F4BfDuTu z2tewSVo)324eAnwQP|w-)ZQQBzb#vha+~Fnp1;3{oRRiRi3d3&5YRKPn-TZ@S{=D1(Rw@GwsyDgqOowQ1sBRmYPYinJ*Ir|~gl zL}a&*5P?C=4kd|M6(@9$F{&a%QnfB)q>5)Etd1W{Rku}-S4u*n;$$XMlRyF+ETj@w zH%QsH(07Pj0mn66d_$6{y+l&V8!lXzwJ>=G_t$V!K&I`GE-w<3A4*8y`t*}%%2pCJ zUnJ4cvQKU7)h{6HI}XpV09_cUZ3j?I7y(U4 zP0`h(uVnn19g#7S%4XeBVPMT?OOaX}3fD_?Q}u(DE=iACwEq0PNZxQU*`Yj?;I zY7{6Kcun0U$!-stRSke^(*E6%S>RGv1q^n)*nW1FVJ6yM&6h-T#ACzF>Y3H;q3YdU zO%(rB9FHlNmc{I7QbVtvBe7;)NJAzzPQ`Sk0F{7R7oM8czR9PiJMe3KaQoOY=Zv$R zZwb@htN6s?Xv&=%ugt%d_Je{j613GbRSl)8^aq7g2x|Jnw3g zzUao;RSMRO1Dh_@d~0TSmZis^kFyAP%e3vQ@#0@LS8FMr7v;aq2&e`Xrt(*{7URvQ zyFAYA+no=(zSQq0YHlu3>XjA$9tPEn$wMFYe zNDrjV6Y*yxyTzH(CesB;4ExjvwGdYPKRA>RAgjWM!_W%hN`}!X!cDS|?8+MetCiaM zgwf?eCVoko13xN>j8FbGYNs0xai2SQDz*y@eoa1r^cdZLSc9ta7tcx5u%YOA?q=tT z4#jrZj}lTHDO(k?C@I;oKI9I)HWs zzlmeBCKV+CTDzFyA|DOL5;wX3T~R5$Hat#Dg3m$84oL2ZRQ@+o1;#@;TPcE62JGn8 zZDdLP4l_jM6nDi!Ur&XR;RM7u44|j{*Y643S>-C(rdaz-$>kDUXH@%iIC9V!O_mge z7|l=-zZJWByay5(=88_;vF4#{J)NElYP0ShC*AD$;@~;%&qZe?>{Xn2r6W$6k-pl_8E`_! zf7r*!Vt1GjG}VDB!xE>!Ecg9vS?P=1^;ahf;@pcmIg}G7vw?yDq6^z*LksPjsKQ)O zfE~k08&P1|f#6O`)99sg%?EKTIM-)OS>@8FtVT4!_Q_2^t3-(Qj3^(C&k}TE2BOdq zDt3wVu=JRx73h*^W)@fSblf&ch!POrC}_|5XXA9pgG8n3xaA-|U&SwH*;^hTt>2G_ zHImk$Ir=PaPjtMwpT(}2HzCuD3uqX2?8v`m{9H7MwC+uL47SzJVDD_P=`Jl z5hn%#C;jcPDb&2Wgv0+jSm(r3qyCk@^4t8?=3Qh>f60}-+W6;rqlA8(xrON2<7#SC zEcST#_|pk(>SJI*I+H*S6oMa!1$?ZDnSw4|unq(zg(;l(>9%qtkm%7@d}-fk`LHj0 z42m~?QK)pBELL7n;P+^eG(!*QHiXF0l8jOrzQoFo%FfpncfS2B)Mz9FQu8@>K$ z-47)I$QTNt&X_sRyEV&-@SLIeid7C2ivy;A6l##0Hvp#lTJm70Mj-=Yk_ot*g4eQl z5YboD`hq&!Et%hA?|Nih%HxJ(*_9*KPI);^MHlurvbXtWxXA11;=P^R6z#L0?}|{U z*+h!r*f6NeiK?59golzg)g?du+6sCA;Wc+2GUML`G0dvz+D_X{J#=t)(XC5#bVwcz zk8Ws^>62z5K>(-_=&n_4WjD)G!9&#dIvsE|Sn3Z!UzBG-1_?^9S0)6<027DBOwW7m z#-w89<~)XCnz=;kb0o(I6lB>jVjGo*K_d+BGe;5Z^GBjeR#D@qp7P^@&@21nJA(D0 zSsb;k85&sfJ5PTWrn8lv@kUiA|7ufH?iddgVSVR~chjD9qGuHYF;M(q8arB-RZPcB^ApsVM?1qP4MK8+MyzeIDMc?PlrDE* z)3Z}Lo!!(a6_%xCfW`@4=7`wxC||mjSjl7;k&>Zw@;_a^+`Ten#nEy+MSz7dzIn;@ za%KfOP0h*}wi~fEJ1*IAWBhF}=GqQ4w=a?+dXp$#mp$pXl=QofW2^gd2VgsT6R12B z-=nrl#Kv-CcCL2#z!6IErMuhKh-UfeGXVC!!!YS7duh+kZg1Yn*2&m(X1PykV=x5f zZQ*ZH1=a>Ti>jo5uX{v*;xk!JfM-KgWZZ?d1>(lR?X1lcaBpwyUX6`+d&->GZ|!nbNZa zt@ekXhEIHCd)Hr&+?aq#T+b(Ji2sVoP2s%10h z_xbW?04|}-mZf+_`?Q$`D-020ZFZQ!+I+}jq!$Kg&~ua2VjLM$BVilOUsmq}Q(r+z zb(xKqeJya07g?%ZYE+4nT7Fa33J$-Ty{uovsIKH^ET4H&svja4$f2Biq_|a-(mGO{Tqe{g1FWzFnzfA!-J^od#*?xf75%i7rsa|dK z`sTNe0n!+E8iY9^TAv$lKg>Y7(Ab9Xf}F zbTeE~rZr52+QGZ5wYX(7zDv8T2dVP{{p~TTvDT}fv*DcAuu2CSbQ=l(p?H1B@lDqg zL((UV%SbfgaU+*>;W40wl)xNe?j@a?a^ug9lxv=(wqSv4ijE^r18Uy{P5c+=y)`|a z1(bwHi8MiDmNUAV6X+8v7G>KIlumEHyfoZ!abIS!!DzB9nZjZ)FNk`Ma0(cc4~^t# zt{i0=3NA@iX{-z^HO3)Nd(rreIW;vYtt2rkEfAiG9b9VE;BTTn2!tB!RO_5S7GU@G z!iFQa=Tj5AO3{AOW6YMP83z&~&73@V6}CjnQ+Qq%HTMvHh)_UDNlucwu}@ZQq% zg+?E1Fx5iY8LRFp%3s^m6iw5Nq;*CnCa2Hz zXIFZ^qczTErC&pp(?GJf{L3&EGfIKw2{1f_T`Zz~(L)kZIKYntpZ$Ep@PdY$`SKO$ zp05;tskAtjy_p8A{PCNAPhn+sy54Nr1W!N6T5V^3zG9v^-rjOWC(k^qY>kZ?;qNef z94oQitRvPMv+96#D#3OtL2^X;G(j%0Wy5qtI-G_zv^flEy`Qa^mMwjVJO&QBl}LwG zLlQI=k)RcRqy(LymoYQWrLs@0f9=J*o9?Ai0@mY{1NueDrHqN;3eG^DH|o+QG7F1k zQR{2sD+A-f&@6%FidJ|S>z>tmF4E4FU9UWn>=kqX7DGjytlBe6)&8mAaH}mfvR%S$ ze!J{!F51JhWGrA0|&SG~tj6D=L|3RRh*q{Ot z-;^JEe-kZ;$c&%{#TAhj-q(h_${f@yWHo(TITd!-tiO8EsJ{RyZ=O^;2nk{2-l8k0 zo`Md9GkExbd5zlJG~-u;{2Sb4qqxzGju?~dTH1S3KeQuCQqIfJ+JiT$Xyrsl-A8=t zDB3p@|LXa-?Tifqm-oK9MNbIdlKR|57s6#E>uYyogu;7Fwocu_mpCb}ntHSMh=q1d zTBnrYeylTitS+~BiaiEs4?;bd+YL0KkqevQGSp;ne7SPWmnb-x2Z7ZTsDle>YNRo> zgBd&nTouBuY1l@Hz2sd@6<|u)Gv;py`yl7%Davcpk?eRP=y@O7$eOVX8zt;3Ne(n+i9#~`}`+NDbWf@dk zYxFRq)-f@PD`t;6y+F~?MKNlgyUJRhV@VHEBS+=kMaQ`gTds_BGz_g&Lc@^0D&6<+ zdR4mW&?u{J0|5>UJb%==8-$9=3MKRec|v@v0_7jRy|TXolK>+svcJNPt;@IO2t5vq zA>^Es81X@@)TPUs!4}&$vF1%pV~4QFYx?K+(B&Ox&7pCR{5qkVZWtU14WM|t|Esl( z#kyCEmN2|D6z>9C9AqaINZN@z9RId&a06yY2d+*&xW>J+&$Ec+h=L;AK?b%GK&y~s zVbbH5aWtI zHV$OO6YI)zW~Xf-eLaDGPff@7_M&Y1JHK4^=;ved$usZELG$|x&CYVOhg%~*^7PXT zEdRWNW}U;zcyu-HMm;XIxfF>K7F`&+own4@6r1X_jI&?o2PrOiGtbLX6z}s-HEd-e zfwF8P=R2FW@&kI?0OC~PektWN>WOPXKUcT-ZH{VEcCoi)z)^0Mr{ASiWh^1acX~-P zDxwx2ryC?8_T?0>FH!#Lp?3_}a-{E<2A)vI;28&z;=du;Z^cJ;0GS9|h*JJbZYV#t z%;etMW@uDdJ>GAcddNVlbrjdm5ql=|tuVW-zAibB7sixMdL%!onn>ab9PZK zGd676ORy1`#|a9LFOceVSFfCk!S15BwYL{hjSl`#RqQKkXB}wDS77S( zC|4(G_$`xToOu9kcQxPGVr9NFMVf}NRK;>94G0_CBRjh)Io2G5Vb)P^LP1@l1j?6u zTkY-eFxn?9@(%dF z`Z)7=sQxaFQ;$siXeDKvpU{FaGuNFZ^s^IfubK{uF%tT;q0ioxqcBoX?n>^-9soyk+@2L#Crf?J(i=;()~U&rl5w9+t&^ zV}oilAOGg$7}(I;GBO+?#wL zcB%|-vMBcW+QYt&`pWUXm%cu~w8&t{Pq1Iz)&FSO*3t(w89QiIn7*;UdYI{9{SQ&4BMkG4~S^s6DnN5?;q%-3>4-d>)Ho{&CAdU7=)o zs6fO{*om>D&4nOS7D&qe+?PametHnwH#0h6*%2tf&g>|(|H!oR={W7)a7D4rVBPR* z*G=A!t}nmrYGL>!p72u?=hhh>;RTjX%>U8kf>%fhlkpN{mSX1a*xVQOLih6n{rrcI zu^s`Q!V+7=4=M|hk0(*5WD(*bLQ%cZi1o!uqZ5FqP(=;pe0>zlRFR9&%4)WDTTD)v z1YS8l8{5t`cH7s9_7Lqd-b3XID55bA-@AX{*m02}_r4Tvde*dx?%ZPh>%Nrq2e~7Y zlG*82teZo1TyspFD(d{^VCy5(U`t>@&rPXp|9~7EVtiNYic{=_u8hu_Feha{)lUh_Dws~6j{;E0iKIJ6P0=5 zqQ@tS=HJKFck}oeExa$`dv(UtWq2rKAatAZg6(_BFi}ErE zA};LiO79mnHu|5&4(=PA`O2po>#^6dI9za+-$k*s;Y~qO>5)rJ3rzKF%qQHLFhx>F zw(#_#wLVL%-f``hAl>E2lw6> zn;sv~SVzn7!OZNdX82{L)v3qD)6XiCBT|&j-|FuKEqk_DoHo6t-QxIRMww6B(1|-k z+tqo=W(*5O5wwHx?Vx1z#-^S(ZB}N3}eiQb%S|@o3^$*9B9wjxIE`u*?Ore zd)}ckJD8f>ZMe_Z%pySh-Fdl^K>zLpEk3Vl*;DUkdfh*g0vv9DbIA`bI#e9g~RjpH!55YXJe+nVv@@M7Kgk$>+J?v7v&>jY_EUJ9YacHQ1=!LD(1 zcWX)mdX&bcXu>YRms+Y0SW4k2c+wFvx%6aNKtKV`=&Zv1v#-j>(H1Rb)S98M3 zqU{jo%K5i1MGqZMd5W;`6-^Nh>Nrry`9erK6utY=Mpi*figkbxoo6{^hrUyAyG7T9 z$!bBC6`k2I>l#$Z(*i{wa=GX%$pxzSyypVavp4=8i6@ z389rX>lM8@Omtt}4|^(3jVCXs{~0TFbK8VICuM!q^~gPp6Ld9!mshpD^SFBQ%^Z$; zjWe?Dlo{c1>NL^U)aG;tnFg|cmd;?2S8^PA=)H7vs&RM}&Hf~NdvnjmlY=@Rf(*Ef za%@EO{M`kbL(bVudhNxl2MRcLZt$wBm1o*mo;<{)yl>QzL0#!U_=!p#aPF!>C}!8i z9IZ(oVN;s2f)m;SLM|nFnz}wKTg84dNl(U^pM6Dp*MxVzbYtC^iae>F&7~^qwfsBV zhJi7%HPjc(ooP_qZRu1uNNH)6L(?O>K-inQuCXD7PPLaapt+D;p*PNC2X|O4NY&jI zLd%du08&E&2_TV29$o-80eCD5Ts0YbP+UwPJth!E1o3D9;>py>Zdw$ogQEu$M+8s= z1w0PNBTs_hF{-VqtAjHI7A1mDqqf4k$zd>n0t$}d03kUhEFMrmViZ6ew5tM+ z_;Y`VfrH~9pp1e=!oV2})P1AeFu$lttzvGl>uSKyw4rmtVUYe2*_{GA{GPp`r;W!7 z4X94ifIZ;FuuxJY4I|oNl7=qX4dVUmpdwH`yjRSI-tJHdNUruY80#~jI8Y(X&>Oi| z6;g4cK$1`TbUUcJt`5k(-@_oIp(iX`s!%R?VI<)62 z2Yx3jc$a#E*wS1yV8P3Gr~K;NfRePm?qb^kVeeT%>3-FbP*3S-Z{+?onIt&wJoSB&eriEfOUufm#c zvAiaoebq77(tO#im#Trw@6F>EaHt12=WYFD#Y#2GvlM)==*1K2Z~M8-q+0KKdmhQM zA8M!yAl>_%UbPVPPO0El%qB3yR!yybYpnBTfx{Z!7qvF3JvM@VN+PGKKfpM*X;Tp*t?&2o1mCycrW4#1-+B*jI%rD#?=C&$3TcwTP4%M} z<+Q$!Cp$7UBcRTOpw^YCU;ur3Bbu=*6_znWVpc%cpM&EJonp^}Mq&US^p6LL13>^p z+9QA1FwlwN2kH9VMt}y}KW%6L4XwV`4-Ej&s9Ix#cwRh&*RHXl(F(usK}7%VN5lXS zRlGKqh#^4DyvBwh;EDe{GlqzUM#n$=h#>Kovv| Date: Wed, 1 Jan 2020 21:04:49 +0100 Subject: [PATCH 33/36] Create Overview.svg --- Images/Overview.svg | 204 ++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 204 insertions(+) create mode 100644 Images/Overview.svg diff --git a/Images/Overview.svg b/Images/Overview.svg new file mode 100644 index 0000000..6c4f680 --- /dev/null +++ b/Images/Overview.svg @@ -0,0 +1,204 @@ + + + + + + + + + + Canvas + + Map + + + Content + TitleMap + + Image + + Road + + + + + + Obstacle1 + + + + + + + + Obstacle2 + + + + + + + + + + + Model + + + Content + TitleSpatial Bicycle​Model + + Centerline + + + Car + + + + + + + + CoordinateSystem + + + + + + + + MPC + + + Content + TitleMPC + + Image + + Road + + + + + + + Prediction + + + EgoCar + + + + + + + + Obstacle1 + + + + + + + + Obstacle2 + + + + + + + + + + + Path + + + Content + TitleReference Path + + Image + + Road + + + + + + Waypoints + + CoordinateSystem + + + + + + + CoordinateSystem + + + + + + + CoordinateSystem + + + + + + + CoordinateSystem + + + + + + + CoordinateSystem + + + + + + + + + + + Annotations + + MPC_Path + + Path2MPC + + State & Input Reference​State Constraints + + + + Path_Model + + + + Map_Path + Dynamic Drivable​Area Information + + State Reference + + + MPC_Model + + Model2MPC + + Current State​LTV System Dynamics + + + MPC2Model + + Velocity & Steering ​Commands + + + + + \ No newline at end of file From 281fc19b6d96b66471a26126379b0ef578ac1215 Mon Sep 17 00:00:00 2001 From: matssteinweg Date: Thu, 2 Jan 2020 17:09:32 +0100 Subject: [PATCH 34/36] Tidy up a bit. Remove inconsistencies. Modify s2t and t2s to work with both, state objects and np arrays. --- spatial_bicycle_models.py | 239 +++++++++++++++++--------------------- 1 file changed, 105 insertions(+), 134 deletions(-) diff --git a/spatial_bicycle_models.py b/spatial_bicycle_models.py index 8687ef7..0df8390 100644 --- a/spatial_bicycle_models.py +++ b/spatial_bicycle_models.py @@ -1,10 +1,13 @@ import numpy as np from abc import abstractmethod + try: from abc import ABC except: # for Python 2.7 from abc import ABCMeta + + class ABC(object): __metaclass__ = ABCMeta pass @@ -16,6 +19,7 @@ import math CAR = '#F1C40F' CAR_OUTLINE = '#B7950B' + ######################### # Temporal State Vector # ######################### @@ -23,12 +27,10 @@ CAR_OUTLINE = '#B7950B' class TemporalState: def __init__(self, x, y, psi): """ - Temporal State Vector containing car pose (x, y, psi) and velocity + Temporal State Vector containing car pose (x, y, psi) :param x: x position in global coordinate system | [m] :param y: y position in global coordinate system | [m] :param psi: yaw angle | [rad] - :param v_x: velocity in x direction (car frame) | [m/s] - :param v_y: velocity in y direction (car frame) | [m/s] """ self.x = x self.y = y @@ -41,7 +43,6 @@ class TemporalState: Overload Sum-Add operator. :param other: numpy array to be added to state vector """ - for state_id in range(len(self.members)): vars(self)[self.members[state_id]] += other[state_id] return self @@ -59,7 +60,8 @@ class SpatialState(ABC): @abstractmethod def __init__(self): self.members = None - pass + self.e_y = None + self.e_psi = None def __getitem__(self, item): if isinstance(item, int): @@ -92,7 +94,7 @@ class SpatialState(ABC): class SimpleSpatialState(SpatialState): - def __init__(self, e_y, e_psi, t): + def __init__(self, e_y=0.0, e_psi=0.0, t=0.0): """ Simplified Spatial State Vector containing orthogonal deviation from reference path (e_y), difference in orientation (e_psi) and velocity @@ -114,18 +116,21 @@ class SimpleSpatialState(SpatialState): #################################### class SpatialBicycleModel(ABC): - def __init__(self, reference_path, length, width): + def __init__(self, reference_path, length, width, Ts): """ Abstract Base Class for Spatial Reformulation of Bicycle Model. :param reference_path: reference path object to follow + :param length: length of car in m + :param width: width of car in m + :param Ts: sampling time of model """ # Precision self.eps = 1e-12 # Car Parameters - self.l = length - self.w = width + self.length = length + self.width = width self.safety_margin = self._compute_safety_margin() # Reference Path @@ -134,8 +139,8 @@ class SpatialBicycleModel(ABC): # Set initial distance traveled self.s = 0.0 - # Set sampling time to None (Initialization required) - self.sampling_time = None + # Set sampling time + self.Ts = Ts # Set initial waypoint ID self.wp_id = 0 @@ -149,69 +154,76 @@ class SpatialBicycleModel(ABC): # Declare temporal state variable | Initialization in sub-class self.temporal_state = None - # Declare system matrices of linearized model | Used for MPC - self.A, self.B = None, None - - def s2t(self, reference_waypoint=None, reference_state=None): + def s2t(self, reference_waypoint, reference_state): """ - Convert spatial state to temporal state. Either convert self.spatial_ - state with current waypoint as reference or provide reference waypoint - and reference_state. - :return x, y, psi + Convert spatial state to temporal state given a reference waypoint. + :param reference_waypoint: waypoint object to use as reference + :param reference_state: state vector as np.array to use as reference + :return Temporal State equivalent to reference state """ - # Compute spatial state for current waypoint if no waypoint given - if reference_waypoint is None and reference_state is None: - - # compute temporal state variables - x = self.current_waypoint.x - self.spatial_state.e_y * np.sin( - self.current_waypoint.psi) - y = self.current_waypoint.y + self.spatial_state.e_y * np.cos( - self.current_waypoint.psi) - psi = self.current_waypoint.psi + self.spatial_state.e_psi - - else: - - # compute temporal state variables + # Compute temporal state variables + if isinstance(reference_state, np.ndarray): x = reference_waypoint.x - reference_state[0] * np.sin( reference_waypoint.psi) y = reference_waypoint.y + reference_state[0] * np.cos( reference_waypoint.psi) psi = reference_waypoint.psi + reference_state[1] + elif isinstance(reference_state, SpatialState): + x = reference_waypoint.x - reference_state.e_y * np.sin( + reference_waypoint.psi) + y = reference_waypoint.y + reference_state.e_y * np.cos( + reference_waypoint.psi) + psi = reference_waypoint.psi + reference_state.e_psi + else: + print('Reference State type not supported!') + x, y, psi = None, None, None + exit(1) - return x, y, psi + return TemporalState(x, y, psi) - def t2s(self): + def t2s(self, reference_waypoint, reference_state): """ Convert spatial state to temporal state. Either convert self.spatial_ state with current waypoint as reference or provide reference waypoint and reference_state. - :return x, y, psi + :return Spatial State equivalent to reference state """ - # compute temporal state variables - e_y = np.cos(self.current_waypoint.psi) * \ - (self.temporal_state.y - self.current_waypoint.y) - \ - np.sin(self.current_waypoint.psi) * (self.temporal_state.x - - self.current_waypoint.x) - e_psi = self.temporal_state.psi - self.current_waypoint.psi - e_psi = np.mod(e_psi + math.pi, 2*math.pi) - math.pi - t = 0 + # Compute spatial state variables + if isinstance(reference_state, np.ndarray): + e_y = np.cos(reference_waypoint.psi) * \ + (reference_state[1] - reference_waypoint.y) - \ + np.sin(reference_waypoint.psi) * (reference_state[0] - + reference_waypoint.x) + e_psi = reference_state[2] - reference_waypoint.psi + + # Ensure e_psi is kept within range (-pi, pi] + e_psi = np.mod(e_psi + math.pi, 2 * math.pi) - math.pi + elif isinstance(reference_state, TemporalState): + e_y = np.cos(reference_waypoint.psi) * \ + (reference_state.y - reference_waypoint.y) - \ + np.sin(reference_waypoint.psi) * (reference_state.x - + reference_waypoint.x) + e_psi = reference_state.psi - reference_waypoint.psi + + # Ensure e_psi is kept within range (-pi, pi] + e_psi = np.mod(e_psi + math.pi, 2 * math.pi) - math.pi + else: + print('Reference State type not supported!') + e_y, e_psi = None, None + exit(1) + + # time state can be set to zero since it's only relevant for the MPC + # prediction horizon + t = 0.0 return SimpleSpatialState(e_y, e_psi, t) - def set_sampling_time(self, Ts): - """ - Set sampling time of bicycle model. - :param Ts: sampling time in s - """ - self.Ts = Ts - def drive(self, u): """ Drive. - :param u: input vector - :return: numpy array with spatial derivatives for all state variables + :param u: input vector containing [v, delta] """ # Get input signals @@ -220,7 +232,7 @@ class SpatialBicycleModel(ABC): # Compute temporal state derivatives x_dot = v * np.cos(self.temporal_state.psi) y_dot = v * np.sin(self.temporal_state.psi) - psi_dot = v / self.l * np.tan(delta) + psi_dot = v / self.length * np.tan(delta) temporal_derivatives = np.array([x_dot, y_dot, psi_dot]) # Update spatial state (Forward Euler Approximation) @@ -239,15 +251,13 @@ class SpatialBicycleModel(ABC): """ # Model ellipsoid around the car - length = self.l / np.sqrt(2) - width = self.w / np.sqrt(2) - widht = 0 - return length, width + safety_margin = self.width / np.sqrt(2) + + return safety_margin def get_current_waypoint(self): """ - Create waypoint on reference path at current location of car by - interpolation information from given path waypoints. + Get closest waypoint on reference path based on car's current location. """ # Compute cumulative path length @@ -256,7 +266,7 @@ class SpatialBicycleModel(ABC): # so far greater_than_threshold = length_cum > self.s next_wp_id = greater_than_threshold.searchsorted(True) - # Get previous index for interpolation + # Get previous index prev_wp_id = next_wp_id - 1 # Get distance traveled for both enclosing waypoints @@ -269,19 +279,6 @@ class SpatialBicycleModel(ABC): else: self.wp_id = prev_wp_id self.current_waypoint = self.reference_path.waypoints[prev_wp_id] - # - # # Weight for next waypoint - # w = (s_next - self.s) / (s_next - s_prev) - # - # # Interpolate between the two waypoints - # prev_wp = self.reference_path.waypoints[prev_wp_id] - # next_wp = self.reference_path.waypoints[next_wp_id] - # x = w * next_wp.x + (1 - w) * prev_wp.x - # y = w * next_wp.y + (1 - w) * prev_wp.y - # psi = w * next_wp.psi + (1 - w) * prev_wp.psi - # kappa = w * next_wp.kappa + (1 - w) * prev_wp.kappa - - def show(self): """ @@ -293,24 +290,22 @@ class SpatialBicycleModel(ABC): # Get current angle with respect to x-axis yaw = np.rad2deg(self.temporal_state.psi) # Draw rectangle - car = plt_patches.Rectangle(cog, width=self.l, height=self.w, - angle=yaw, facecolor=CAR, edgecolor=CAR_OUTLINE, zorder=20) + car = plt_patches.Rectangle(cog, width=self.length, height=self.width, + angle=yaw, facecolor=CAR, + edgecolor=CAR_OUTLINE, zorder=20) # Shift center rectangle to match center of the car - car.set_x(car.get_x() - (self.l/2 * np.cos(self.temporal_state.psi) - - self.w/2 * np.sin(self.temporal_state.psi))) - car.set_y(car.get_y() - (self.w/2 * np.cos(self.temporal_state.psi) + - self.l/2 * np.sin(self.temporal_state.psi))) - - # Show safety margin - safety_margin = plt_patches.Ellipse(cog, width=2*self.safety_margin[0], - height=2*self.safety_margin[1], - angle=yaw, - fill=False, edgecolor=CAR, zorder=20) + car.set_x(car.get_x() - (self.length / 2 * + np.cos(self.temporal_state.psi) - + self.width / 2 * + np.sin(self.temporal_state.psi))) + car.set_y(car.get_y() - (self.width / 2 * + np.cos(self.temporal_state.psi) + + self.length / 2 * + np.sin(self.temporal_state.psi))) # Add rectangle to current axis ax = plt.gca() - #ax.add_patch(safety_margin) ax.add_patch(car) @abstractmethod @@ -318,7 +313,7 @@ class SpatialBicycleModel(ABC): pass @abstractmethod - def linearize(self): + def linearize(self, v_ref, kappa_ref, delta_s): pass @@ -327,49 +322,29 @@ class SpatialBicycleModel(ABC): ################# class BicycleModel(SpatialBicycleModel): - def __init__(self, length, width, reference_path, e_y, e_psi, t): + def __init__(self, reference_path, length, width, Ts): """ 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 reference_path: reference path model is supposed to follow - :param e_y: deviation from reference path | [m] - :param e_psi: heading offset from reference path | [rad] + :param Ts: sampling time of model in s """ # Initialize base class super(BicycleModel, self).__init__(reference_path, length=length, - width=width) + width=width, Ts=Ts) # Initialize spatial state - self.spatial_state = SimpleSpatialState(e_y, e_psi, t) + self.spatial_state = SimpleSpatialState() # Number of spatial state variables self.n_states = len(self.spatial_state) # Initialize temporal state - self.temporal_state = self.s2t() - - def s2t(self, reference_waypoint=None, reference_state=None): - """ - Convert spatial state to temporal state. Either convert self.spatial_ - state with current waypoint as reference or provide reference waypoint - and reference_state. - :return temporal state equivalent to self.spatial_state or provided - reference state - """ - - if reference_state is None and reference_waypoint is None: - # Get pose information from base class implementation - x, y, psi = super(BicycleModel, self).s2t() - # Compute simplified velocities - else: - # Get pose information from base class implementation - x, y, psi = super(BicycleModel, self).s2t(reference_waypoint, - reference_state) - - return TemporalState(x, y, psi) + self.temporal_state = self.s2t(reference_state=self.spatial_state, + reference_waypoint=self.current_waypoint) def get_temporal_derivatives(self, state, input, kappa): """ @@ -380,6 +355,7 @@ class BicycleModel(SpatialBicycleModel): :return: temporal derivatives of distance, angle and velocity """ + # Get state and input variables e_y, e_psi, t = state v, delta = input @@ -387,7 +363,7 @@ class BicycleModel(SpatialBicycleModel): s_dot = 1 / (1 - (e_y * kappa)) * v * np.cos(e_psi) # Compute yaw angle rate of change - psi_dot = v / self.l * np.tan(delta) + psi_dot = v / self.length * np.tan(delta) return s_dot, psi_dot @@ -400,6 +376,7 @@ class BicycleModel(SpatialBicycleModel): :return: numpy array with spatial derivatives for all state variables """ + # Get state and input variables e_y, e_psi, t = state v, delta = input @@ -413,36 +390,30 @@ class BicycleModel(SpatialBicycleModel): return np.array([d_e_y_d_s, d_e_psi_d_s, d_t_d_s]) - def linearize(self, v=None, kappa_r=None, delta_s=None): + def linearize(self, v_ref, kappa_ref, delta_s): """ - Linearize the system equations around the current state and waypoint. - :param kappa_r: kappa of waypoint around which to linearize + Linearize the system equations around provided reference values. + :param v_ref: velocity reference around which to linearize + :param kappa_ref: kappa of waypoint around which to linearize + :param delta_s: distance between current waypoint and next waypoint """ - # Get linearization state - if kappa_r is None and delta_s is None: - # Get curvature of linearization waypoint - kappa_r = self.reference_path.waypoints[self.wp_id].kappa - # Get delta_s - next_waypoint = self.reference_path.waypoints[self.wp_id + 1] - delta_s = next_waypoint - self.current_waypoint - ################### # System Matrices # ################### # Construct Jacobian Matrix - a_1 = np.array([1, delta_s, 0]) - a_2 = np.array([-kappa_r**2*delta_s, 1, 0]) - a_3 = np.array([-kappa_r/v*delta_s, 0, 1]) + a_1 = np.array([1, delta_s, 0]) + a_2 = np.array([-kappa_ref ** 2 * delta_s, 1, 0]) + a_3 = np.array([-kappa_ref / v_ref * delta_s, 0, 1]) - b_1 = np.array([0, 0]) - b_2 = np.array([0, delta_s]) - b_3 = np.array([-1/(v**2)*delta_s, 0]) + b_1 = np.array([0, 0]) + b_2 = np.array([0, delta_s]) + b_3 = np.array([-1 / (v_ref ** 2) * delta_s, 0]) - f = np.array([0.0, 0.0, 1/v*delta_s]) + f = np.array([0.0, 0.0, 1 / v_ref * delta_s]) A = np.stack((a_1, a_2, a_3), axis=0) B = np.stack((b_1, b_2, b_3), axis=0) - return f, A, B \ No newline at end of file + return f, A, B From 58a9a112a4f624f159497833af521a2f3b3874f9 Mon Sep 17 00:00:00 2001 From: matssteinweg Date: Thu, 2 Jan 2020 17:10:14 +0100 Subject: [PATCH 35/36] Tidy up a bit. Add comments. --- MPC.py | 59 ++++++++++---------- lidar_model.py | 2 +- map.py | 70 ++++++++++++++++++------ reference_path.py | 86 +++++++++++------------------- simulation.py | 133 +++++++++++++++++++++++++++------------------- 5 files changed, 195 insertions(+), 155 deletions(-) diff --git a/MPC.py b/MPC.py index 654de44..77356e7 100644 --- a/MPC.py +++ b/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) diff --git a/lidar_model.py b/lidar_model.py index 82e8c68..aa6edbf 100644 --- a/lidar_model.py +++ b/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, diff --git a/map.py b/map.py index 41affc3..457403b 100644 --- a/map.py +++ b/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() diff --git a/reference_path.py b/reference_path.py index 08507fa..03805d4 100644 --- a/reference_path.py +++ b/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' - 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) diff --git a/simulation.py b/simulation.py index 5ec171d..676bc92 100644 --- a/simulation.py +++ b/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: ' From 97c03cf365f0661897ee6838d51649c33c6f4400 Mon Sep 17 00:00:00 2001 From: matssteinweg Date: Thu, 2 Jan 2020 17:12:32 +0100 Subject: [PATCH 36/36] remove old files --- .gitignore | 2 + Images/MPC_Framework.pdf | Bin 20453 -> 0 bytes Images/Overview.svg | 204 --------------------------------------- map_floor2.png | Bin 24342 -> 0 bytes map_race.png | Bin 19334 -> 0 bytes 5 files changed, 2 insertions(+), 204 deletions(-) delete mode 100644 Images/MPC_Framework.pdf delete mode 100644 Images/Overview.svg delete mode 100644 map_floor2.png delete mode 100644 map_race.png diff --git a/.gitignore b/.gitignore index ff7c126..827854f 100644 --- a/.gitignore +++ b/.gitignore @@ -9,3 +9,5 @@ __pycache__/map.cpython-37.pyc __pycache__/MPC.cpython-37.pyc __pycache__/reference_path.cpython-37.pyc __pycache__/spatial_bicycle_models.cpython-37.pyc +*.mp4 +*.png diff --git a/Images/MPC_Framework.pdf b/Images/MPC_Framework.pdf deleted file mode 100644 index f1ea73b4f631949bda40c04e4acf69a5d078a8b7..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 20453 zcmaHx1CXT6)}Y(AZQHh{-92sFwtL#PZQHhO+nBbE?eE?j|J~i#*odk(vrZo5$*MY8 z`PM@!FCs?ENXG(2x^Vt?4~mh1fxyv*vjGI8h#>v=<;733pr%0eiz{sG>$g0brMM)rNXJ=)sYHT27?nKYZ%udJ5 z_G2g|t|TK(LttfYX-x3T*wE7UM^VVu&0SID7dx2QwWb z6C;7Bm9e3dgRRX!@#z?u2^95B^c~Fq-X+B#V4Tm4sw<-bb8g8wM}TUA<^fB}kLTJm34)qmtN(gZ9}^ooK6 zP7cn-|8HMx|MewiZsqioCB2x{Pd*~XhPFmOHqyp6rcP!A9L#Kte0&6sPCxINHI!TC zrS?WN38#&BO--mNFebHvYcIntuG^D)7w`-4dNymM1+WhP>$Nu=Dx!jk@nRKr!xHN{ zDA>{@M36{aNTP^B#hb;qCeD|SdWg?k`U+K3^3lXf$hZ6MHC8|+Wl^hmxOtU^g~rzt zKK%C+{DjW|Vox|3t%0@nCot`3|M&NKPV&>p&ez*b2>D+oCH9AE9L}A?ZVq1!NGw=1WHA%e&dwM|=G21exd7!Ps!(+U)%=3d9M#G- z6S!0Z<~rp-S^5Bg`C7Yh6-vdbs#`BF+3J*488CCO%tsoVTdKs`N!-((RpNvkVJEPT4S+E}@5*BlYA%Pb ze);OK@gfz6;^ct9=B?Vyec5qHyB)7B0%7G4{0f-5IV1(AY{{&X2P7f7r=2y zBAhx~x=N`(oZb%3Vn07O8RJ}v{dp{rh!Hw zWA+JcpN`jV)2|+$4|1MFR+L(U4|?7(a*=zI{s)p`az@^;zY@}N1|CQY38~qGyAmHm zE?l#phecxMo}?e-!%@*%jmb?S;E6!Oy?MLyTG%cFy}}d0YUeG2s&8>m~J);;2$VAsaO@Wt}%!r9wI2JrPwG# zhP{-xknJL%>sUDJ;vyB2GGP_Yh1Zfe7}jAR zqiTC)m5x~c6wL|-Q2DX>!oQ#;ULxYJ^K`d#KBRn+=5g$=z%))yNfo+T9)d_MH|0O8 z=C~hWca(Bcc#=WW4Q^xQETowdo3Xb+Za*9(syEI;JpFe92$nkZ0W zp92gb3i3eDbsg65tspBzI3sK0^Zj7(p&Cwc6`WQ2iPzn{qDMQuL_{X+4G-zjq)=EG zqwrC2F8+CHRkkq0fiuoVa&aW7l${tiIA#gp8NQX}nb1IiY+j}R-c(&KTI=pV(QdjLS*qdbW{!s%bznpUPV<;^u0D`&?Z zo)+_i$CSVEweq(k9Z?B18VUg1MCCpngi5r2LL)uSk)BnFmzePaZMSYW&lbubo8Kl* z{J78OYZd9j{L5geK|8+!ZIX1`h5Wbp-zIgW<%C&Bi0_%6D)umboum5<_-84RD?&s* z-MaBLXI+ap!5f>!$FVLM=>_ekP?2-}ddWv9g@o0qEXR0-gkNf($%PDHR0*S2V7Nl* zOD&b)oIp8CEf=9qs01=p_Y}e67G4|kv>ym|GFZsW)xr&Jq7x`^xc;pY&~1$fH?*r6 zEGEZ=6_YrRw4NkzX`F>~P)UTdnT^RYOu>Z}wrSLVAauy1FzL8J+7g~vm_rsfCUNFD zYK%i`cFb4OIJ3&zPfQ+KBpPPFz7UnA09LWAw)U6|XHZUY$(xQ)9sm_~H7sOJK}Dh8 z63>T6hgl+C{rSe`)1>?i*9RD&Nf`NwY#djQBkqQ!!;9poC8S$#0s!RKQ`6_j^wCij z(gyP;=zd!;joK6mKRF~wRc(~GVjk&E;+t2os~>aIGM^xL#6L}DR^t{n|6D~283#nc zybVp$pbg_a&5B_O+o_~h^yW2OWL6%9cw$XgA2YL>79V?! zknH^Lb?={2c+34jk}Y%hDE%3OxDaA{gwku7l0)|T)*;co58}1xl0D?4cw;+rCzCu@ z0TmgKHE$h$y@21GsaNKCv3M(H!{2McF>Uls`my*YE~(SMylYeK<@Ew}g^Mh^V~G$> zeGE%8r7h;NAz{U<2UfTBUWxK@L~orsTveg0yZd9B`xw`;-_Svwp5Z}O*i`!(34x|F?;DpYwK>zP|NfpO5fD0PDT zs+iGnLmcpz&4B%oKhPY~qDmTBR^EA4YovxsyRD{|<f}51Pu71 z2YnwY^>%E4vQ*y#8B>Ai6{NF0u~5Cn5sq`)(nQ;eZgFHp8+P_aI)kJD9k@V)w^b_} zXmqOSZ=u!rK@!JS)?Q>i)*P_LS|pKLZzCB7nl{pEa)6>s%z-z-HFZ`X8H9Z+8ob?1 zK)=g~K1s7${bW)7%Zx;IVBaE+Q`TPE!z**3cGVJ<(L;tfo_v9Y!Nju8o=s4i<`vILT0K34{V= z7R3O<)OkLMy0y6XD&7@@fh+2u0FH@%V3%gQIzH9}47WFNsR8g~@ED9*05BlK*pWta zL^HU5+(J(kj9)`>*83oMM5r@?i8Yv^S>nVU&6!mEly%g4!O{ngWKAT12|pU!#R!0L zfO-JX2~hRz?CrG5cPSBYS$_6{S72;7`jHpMk`9~@u1fECB&yn0KR(U}2q|}Pydg7F z!Cms%)Cw2-#wUqLh&4Kl%<%dT5vy`2&=AjlXOJpvIcqE(7JBdqL6$fwskP9m=h|#P z;4`S;@dhYvneiblUG%@|b-su*S-w^(EhjcV**HOG_EBYePtsrx@IvaTCAyHo*i*$p z^AXTwE3pz~#-nV+!{uwS8%WMgG-o=|W5s z1_KcERj5KlYpNQg{i)#c>}>+NZ7hw%&HHSih}5>C@VNlpA1*k5|H|(N^rB%HXuyek z_d|!FfyN5XuzM`T9#Uq}ltZsoxk!&4CD9%O>h{uE|H^^GbJ(D2UD?u+8xoK(@1zp~ z2tv#apW_&y1oe3Z2$rRZl5Nv5f*(?*v)3@A1Oehz_scg|$9-mRop=gdojKo5#RH%S zNIm$4!3Nei?t7f*VQ6`lRCMMwK@!11v0F;m$yLS7o|=B zT-a%ev$;CH;6=AimN7#m5EbB_kFi`cQ3$AV zh(mTC$0g%E4~V!zqDXV7vE=WTtcHO2zrWq~0qN<9G0xf<@Rd8!vpUU16_ z*Q~+0XxLjr7q#_X3}zy7pJJ>riV%bFe6Ti5A9;W*_>%!6xNM}KRRfY#Pi+@(@qd#K zV>YI*s@c@=P-Y^q5n2hY2G;h_2X;LkB3z+o<9;AQya+mA&u@1L0vhd3CTW>zcky4qrp0hx*Wu@z$&Dg;Jf+AooSpT( zABm-ik9;vZ3ADa(0THxFM!Fv*AC9=Gj*iR81%-&g>)f%)-UZA#w3ANQqU&8~_hQb& zzO}e(0c$qJ><=Q_XKIGJ4C}>jI|C%vHAw1Yo#m49eQetTw}*iek%8o5jm!a%l83^6 zXH!AcvlP#~4=BkcEyWSN(&Ib$gyKtUX|r4qp@`m0(hYt+TobW|T07}R&bYC1VB*^# z8P{!D51iR?$%QQMIXfqu8%S-x(T(akAL}iz{tc|yaTj%g|IjG6lso6GaBcpb1WdX; zjdLpC4li=V$OSzxpy0+O4=@B(;q@B~H&4;HjFOp*sS7%~{$AysZzCyeDEg>}?~nq9 zYG9T7@izFZaSlUfa8_pW>Fk;ZGOVt(FHXfHW8nE(@%`DA4%614l!u}dLrSlX{TKyr zsu7$4euyY>bp-XK$p!l)T|W>q4Fous52Hl(BtZP`q^-8&X%{Ibta zqZuvadYjOCXGN7AnJ>pY&2;Tp$#a0A7$>wFl*m&HIe8uH zyX;97zoXEa8u{0aU0H5V($FH#dr?{ZF-U)zP_&rffF35~m)T1iXwqW5roj+L^5h5b zv6NS<6zNP$#5fgAxQ$kFx(MG+R{T4=uU)cS|5jf*CF>Jd@$__ro1{`pK)>##xo_m9 zC39-JyY6ud6WCU7y8C&mvy=hBnuH5&34yBIBoSwQDXRT~XSV)rW}-P)ZSi(AB?iki z@8S+pK(4tcO~kksP$G^vq~2JJh0;=L{;Y9S#+9 zj-;#$+eiVN6R!qy97K|4H&_gmwnZ8>&mM^7>BQZkYFmxP%)+q{z9e_L-=Pxs8k8I6 z*|jDW!iAcdgi4<**3i+V8#v!@OYmA0!<$PADK`qUPe}X6_0Gaxk%`_&hi3XyxnoNF zpixuAiNHEpJtdY?KL};5%?-ak_7t?wTfMvqO)$-Vf{WMI{_=&;mQ2oxDYG#hMA?K! zJs_5FPNOfx>=?*kE87t8oj(cYYToB?5Acen^$0MAzBpFdCvmkF>a=~-fNqxCGllv^ z3R1V@K(t#M5)6`>I*NC`EW``k612MEkfL+oYYynVUYI7oL@GFkl?s0qG?)P4#_5qR zujeYI9;jT5)~=>m)^9@?%{CD?QX~r$t);t4`y9yfoK)pAU)pmd-9Rf6GY6GkO;L0p zXp&+1Qe$Ou;>{IK8pVizt2uI`I-4;E_9j-0Dq*KGSb{%@w2eM>-iUbEngX*R|0?AN z*GzLO68a30r;ZT+!zy)@R%1*BHvVi&l52fK&F{kR_u?``zX2;u3wD)*-$>lEN{S^WXjKv)uVMJ z@Y`DbY<+G4}Ewb{9yaWD|W5hcRfmIL)Zz zLc5KncHe0lM@b|8h&v&AY(xn**YrkNy90At=NHh1jrN8Qd@gTplV7e_{TePA8`r*G zs=Rz|QZ1QgByn&&%59)^Jn!8~re(7}`M5l*p%sUVBfV#&47Pm}q(v#C6#W5%7wHDm zIXI2o_74;5W+)A9MsT>X8#OcAWy7v+r=m0SWfu4)DMrcbc7`+ z29wLmV!k&rJsm?tjYFs!(>m5o(A|{1q_r4o8yML)vDl7Kp=#`KZkN?78keZ2GCdKG z{^O<32kqCnI!CB}FEVKfh+JYO)04UJ#U;hscm?Ni5mf}~QS8Ft807#mc}b+V4yGr# z?41He&N?bjH01PevY(j$n$i9Es&00bSOzn)w)(G!e(G<9yZC!onp5^`@0{oE8PA=Q zuIu~lXZP!Ont^-npXV0T+yA@n3HD^FRmLcf(Z&9eb!t_dn{hP2ZHXBU|I;Z)0+I~g z&@sum=PBA_#`>PP6oGX3L~)ERNqs+JohB*B@uyVtlfM@WAop~lX7CBI av+0pYA z)%;VT1CA0VN6z$<%W~Wr6j~jMHU#5sr9~llQ^P>JF;6qEOW}F8wLsGyAMznrAvi|$ zivx`x=3h6mt-)UCb>tILjMy)FacIzI=Cx2g9=w`_`%N2hYkU1Qvl%^gt32=>GevRf z+3Uu8l1mI&D|6>*5{(~Q1CcB?{MFqNrK_Ue8ka^Aa5v->hU_}nip6vio3!-0eM>v_ zV0}m_ICw2(TTX&6hKL-({f369#)@i-0QUCk8HTjbm-j2GDwk<*pZHLYk*rl4F{qhH zxPj2QE^ot!H^#*QRmkk?b+H`pap*-PR1@blvAaEU)_XcvFaW5-7sQxm_VJXYtQ&yrs<%C z;rL#-h0(yfR%UGL`0~WN{&ji4hYJO}h=J#PAgLE6fK0WmZpx30V0fcKfzadEQxUua zXglq)yO+C4y|qs3}l27v6X zg&C@%wB_>YEBnBo!;R#^oT+MHYL!)vuL~v+OsJQ?)+-g4 zT$%@fh!c+`E5f-Z$VP4lfS@hfj)L`<<#%U* zF>@m>pn)ie6|2|klXGibXn?!>u#Uv8`$KS{ZZe`vhyH5rKD9H9g9wY0#FX>-92EtZ zel|BN>yO~EUnoytiLPFaA(p>$))%J31aawqi~G4^-AMW>@!CezQ^S@IT6}?C4^qq> zxhw$^H<+gh98sFupfiGOPi@k0=xs}ImR{a*YPbw*EpL?IMuzLKU#Kd3;*iLoRA6;d5q4is_)q3;~hKo9^iYl0CjF>dh9i9u*o<|sGrFQ#< ztQ?x24zP!dS>+8;l{PW!s!6Xd30(0c!M|gxc>e%&EIM4Q_TVf|*Kg~y3BU$D$YtB( z8Z>kBKxoT55)`OupBr4eg2Lq5645<-1jxvrmFhGJ`J0HEgqzBKrh9To`~`T1kY8 z(a>M$HSskY)0eRT;6a2h_eLKUQ8G!bd_v?yclWQ$H(rDnx_j%)M38Vx#ZdsPY3i`Z zp|NCe2$qw(AdyPtJ@8lz7PSxZvUGRYHp;Stc~SOzj7T+qhogMylxllomF*_8E~}BNlpFWFr7hlL%L5EQw9Db}iLqnS>2;eaSXFob?B^28+SH`@u=0 zv@Kz|uxjPz>?^~4*b+vMHoUGA*wm%eb<8HfSJYpE&;}I`)K?1#C`ZcY10r!0M;>DA z>r02W*Z46Ws{~8NR*N>r6==$v+`E2g`tV=;Z)bUUWkuu4E9E!^)0N)ql_4gb?;Ay+eL9>HKD?8 z&XV2YsqB4Dr~jQQJ&GU~TXmoSzpPtOIEM4JM6jGUuoVt0(%~QTywL70Hl~UF6vOWJ z{;96pHRqhrhg~Fa@Iw9KIw$LFi9E1JN3ac#RFN(I!i|!V434!DYNc2Doqr+5K?fSB zTM#Q~1TftfZW1t5TZUT4sddZ=o1KCg*NIvPo6}ZkgiE}?^R5Kjs(UYK=zH?`t@)Mk zt&E+K5xs0fzaCQVOqw%YtRfPVjWX1)%nxrBo2~NZ$M(1DeUKmUiY8(9I-MbH{%sz zB+fC?yG#||g%ZaE?M-*>@LR|4K?$t!XBqA>W%<5_!xu5$Wuk`w6Q&k4QYm(~mNOYV zG$;4##t&6831o~ZTA;_Dhc>6cZUUI24^0AH`!%EE-#}%K_`SIaU#5EAvgoC~SEENI zslwYn?7fO(r{u^pU(;C1#qe?#;Cs7+zJ;TB;8~+HsApxqQQb^Ug5@H+LwBdU8u_-s zM#KFD;?J5sZllYw^yBsg94=2zDf)|oKD~6{MRWTSTgy{x{EW-LT(GeAOGo+p&Q6HO zxNpYvz#TVp&Euxa&gI(}mRon16!a~o#{c4e&PNW|&BBx|w}V{jHkQ1fU>SdpwTlmY zUr~Vj+JAGTYC?Or-RZEt0e2Lm_^(1$uaw34iyXX;9d|2i|2RuO6IbPBf-bNQk=o1$@aL`;)O=N0g=lVQKc&}319pwtYG^CV-m1({m&bgL_kCe5?a-ygK;{o@!A-J6G za*nHxGUXq={FAeuzd7szirzyO*O)F@9kRAALo-X?lLa0zJ`caWyUri`AW5`k8UE(8 z*1ZW*6Ctjo9^L!LyP@_3PG)MH^jD~WPL3b9T`th3dAy8M{_LIc+1F|@%ssBI`GL1? zvcJgR7Ll^JjNG5;L8Y-*;!1!%a9dor17b=pJeJFI=#;0MZgX_)mU9B6Uy8SJhFvcU zcvC&K!?4$8dVjZVbKl+vw#=w%cX&LI#y{l?gC&Tr!YQ;yvY7-ol!fv4m%h#bQHD;%Nya9LRx_*}Q^`Z=%>hd^YNx$!Jo|10NpM6Vjc zWel5byD{AB?czPKH3YJ*o1LTa74B+iz*PSa-NU!Hot<8ia(%U5)3qRF$W=Q0v&KlC zH<09KJM6~tpBlH)z*vOJut{C+PMg#Ay1{42I5vcS*iEN*leO+07o6!7eeV?+;fmlB z?dK3dA*%-8Y!&~;gtrAp6SQGOPhT zl9m1%g!vnEi|hsIjtCWfje=SqG=YHn)EdRhu?>2}k=P{fkw?M2PAs-H@zjU+n_jc_ zILCAc6H!pSc%X0~@Dpj?28*n!7{(kGy{=8=iYHTD$V1PXnqk|bJKAZV<0`1>vXW13 zJxISahaA@`IyRM5IA4B6=oa#148rSMxe)V=<6#F7D_ZEArNBOw;zMU1%o%$T3Y>*O zCs+b{c6%IF4iA}$A=(L-jF!V@^2hP;L~W0MBajLsTM8j#E-92VWAm-FS`U2pC*AVR zCoyxaEh{L1YcBHgrD!_M$rzd0bFaE>nuniDFV2Xb4Sgd!;H57uFWkM1-7*;u_;L>r zDX^g?fF@7Pku7QiklO&yfV-GrrYRbuzbr z#F(_83F+<`yJ-aY8+21sX?-vCId{`~i?%`|j3>;>IJu8)V`um?jPch5i(((kJh<9> zOgix$V`r1{7_MjZ_G}BNy483AIblm z9I!U97GI~VfLQgt;Bli{2gSDJW%}O{o!&$csevr=o#F$2HqUnzB1^Jz$`O?wG^PaR zJ`K1EP3G{(37KFZqaY#82*3X+4bhL5<}-Xrav6lOa$y~ZG1qU0dYf>02wRRY)YRA%;(E5jkT5*lE!~Jgmr2KisxEyAC$EbW$4y?V{Kx2 z+QSCqeL2}1!h2v@guCidy6lMKHJ?5OShRk=5*-ii_bsP9dOQm^!M(Z z(F@8T6|ho4fP2L1jsa3w8}@TC5((G~OI`DMgOUvU;T2>Z>ILS7Y;j1@Q=|ZZp7Mq6 zN-FO%qP084JK&XWRT275=9YVxXV!Xl0!=pU1G?b^AQM~fWJz`*pMgKA9^rUIbExa0 zJ5r;UKD}E^;TSg8)l*{g40IcQfGa34j{Mv`7(5&C*fR~LxwmW1muvKN9s0y%&r{5L z8~Tn1{*JkuS(!LEQYDp~q$ML3`ULV7VmvS>w#NnWE~Nz%ZIr*j`fg_q4T1gz@2v}E zlpD$!#i)NdZ!%6Sk=ua3>zk)zLt!>Xh~GjC^QoRMBZO~VEBtS2|L|NkQ{-@0q{`wB z$~`qyoXH|&V$v&5UAj$=+s}tM!HxZNWCakrK&&e}-+5T0)6yWvP7LZ1&H3bxd#Lr- zd&zaMVDAG%LDo`=ArTC7#?|YpD~vAh(7Q24ZnrY?XJ;qdQv)}kQNPupdob#NNjaJt z0OaqI-wmz6!ect#Fb_$yHG;xzH1UHR@Zk)G^e=}^wO@b_UGhpQZ~@i$WbwZ<1)$&F z5z(aZjP19{Jm3l4SK<&gAu~Lo$FF73cHxip;K40Z3_Bt2*5HltEeAM0(S_71k@VWJ zn@&9fJqUNRpdAqBS_U}nYXyP283zmiC>XN+dOTpH38Z}WaQ+xLX^Dcs-TTf6KM~Zg z0XeY0(!Ju%jUpdXIs~_yMGl+OPR+V_@Z#nYKiIs%XXd;!K7;iH^(6FU>O5Bx@m%kz*!-ijaZ(245#I-jTc_zOp{fuQdiQ{|<7Q<7>~}24q38pK&LhL`nnHRwtPC++oT-x2v)dXx8d%`u19^`y=Ej7=S7PyJ-5xDk2~q(?};304|`GiuVMTorLsZi zruDU@5|__DA11+NIuM>`j;+mDu zkNDiDEIuK$RDE@W2@Wu$!5&tcd5tz}J3uNkTwRd>IBB`_RRW06iZwiN~G zd+1C6q~J{$LWpl>h67cm7%CjHQ#;6REycS>ap zP_4Z&7PAfHr-!nrNZA$2QmQ#(e^rysttnuYfkYi$g%2Dj{C(H&`10@Wh>~Jj%KbYj zCb-);&Qo)VrJY4iQ*{`)Oin}3qoP5%j=>5Nz;UJ0=IVsVXDprW@(i7^aj~85w8csQ zxY!DWl{*9_2_x4HwX_a4Y(lC*wuDBURsgjrS+<;oRuD6<+H=oo(y!X{&fE3d&;6=5 z*tZ^qRt~bS-g3{guILoE>`j_JLd|*2$$COQ7)6ic*y^J)32I_j#3OM+GzdFJbsRt< z-#Iv8sgVxJ3zG}S*hhiwfG^156J}zud#`86p8c-7w4Xs5KYQplufN`*d{F4BfDuTu z2tewSVo)324eAnwQP|w-)ZQQBzb#vha+~Fnp1;3{oRRiRi3d3&5YRKPn-TZ@S{=D1(Rw@GwsyDgqOowQ1sBRmYPYinJ*Ir|~gl zL}a&*5P?C=4kd|M6(@9$F{&a%QnfB)q>5)Etd1W{Rku}-S4u*n;$$XMlRyF+ETj@w zH%QsH(07Pj0mn66d_$6{y+l&V8!lXzwJ>=G_t$V!K&I`GE-w<3A4*8y`t*}%%2pCJ zUnJ4cvQKU7)h{6HI}XpV09_cUZ3j?I7y(U4 zP0`h(uVnn19g#7S%4XeBVPMT?OOaX}3fD_?Q}u(DE=iACwEq0PNZxQU*`Yj?;I zY7{6Kcun0U$!-stRSke^(*E6%S>RGv1q^n)*nW1FVJ6yM&6h-T#ACzF>Y3H;q3YdU zO%(rB9FHlNmc{I7QbVtvBe7;)NJAzzPQ`Sk0F{7R7oM8czR9PiJMe3KaQoOY=Zv$R zZwb@htN6s?Xv&=%ugt%d_Je{j613GbRSl)8^aq7g2x|Jnw3g zzUao;RSMRO1Dh_@d~0TSmZis^kFyAP%e3vQ@#0@LS8FMr7v;aq2&e`Xrt(*{7URvQ zyFAYA+no=(zSQq0YHlu3>XjA$9tPEn$wMFYe zNDrjV6Y*yxyTzH(CesB;4ExjvwGdYPKRA>RAgjWM!_W%hN`}!X!cDS|?8+MetCiaM zgwf?eCVoko13xN>j8FbGYNs0xai2SQDz*y@eoa1r^cdZLSc9ta7tcx5u%YOA?q=tT z4#jrZj}lTHDO(k?C@I;oKI9I)HWs zzlmeBCKV+CTDzFyA|DOL5;wX3T~R5$Hat#Dg3m$84oL2ZRQ@+o1;#@;TPcE62JGn8 zZDdLP4l_jM6nDi!Ur&XR;RM7u44|j{*Y643S>-C(rdaz-$>kDUXH@%iIC9V!O_mge z7|l=-zZJWByay5(=88_;vF4#{J)NElYP0ShC*AD$;@~;%&qZe?>{Xn2r6W$6k-pl_8E`_! zf7r*!Vt1GjG}VDB!xE>!Ecg9vS?P=1^;ahf;@pcmIg}G7vw?yDq6^z*LksPjsKQ)O zfE~k08&P1|f#6O`)99sg%?EKTIM-)OS>@8FtVT4!_Q_2^t3-(Qj3^(C&k}TE2BOdq zDt3wVu=JRx73h*^W)@fSblf&ch!POrC}_|5XXA9pgG8n3xaA-|U&SwH*;^hTt>2G_ zHImk$Ir=PaPjtMwpT(}2HzCuD3uqX2?8v`m{9H7MwC+uL47SzJVDD_P=`Jl z5hn%#C;jcPDb&2Wgv0+jSm(r3qyCk@^4t8?=3Qh>f60}-+W6;rqlA8(xrON2<7#SC zEcST#_|pk(>SJI*I+H*S6oMa!1$?ZDnSw4|unq(zg(;l(>9%qtkm%7@d}-fk`LHj0 z42m~?QK)pBELL7n;P+^eG(!*QHiXF0l8jOrzQoFo%FfpncfS2B)Mz9FQu8@>K$ z-47)I$QTNt&X_sRyEV&-@SLIeid7C2ivy;A6l##0Hvp#lTJm70Mj-=Yk_ot*g4eQl z5YboD`hq&!Et%hA?|Nih%HxJ(*_9*KPI);^MHlurvbXtWxXA11;=P^R6z#L0?}|{U z*+h!r*f6NeiK?59golzg)g?du+6sCA;Wc+2GUML`G0dvz+D_X{J#=t)(XC5#bVwcz zk8Ws^>62z5K>(-_=&n_4WjD)G!9&#dIvsE|Sn3Z!UzBG-1_?^9S0)6<027DBOwW7m z#-w89<~)XCnz=;kb0o(I6lB>jVjGo*K_d+BGe;5Z^GBjeR#D@qp7P^@&@21nJA(D0 zSsb;k85&sfJ5PTWrn8lv@kUiA|7ufH?iddgVSVR~chjD9qGuHYF;M(q8arB-RZPcB^ApsVM?1qP4MK8+MyzeIDMc?PlrDE* z)3Z}Lo!!(a6_%xCfW`@4=7`wxC||mjSjl7;k&>Zw@;_a^+`Ten#nEy+MSz7dzIn;@ za%KfOP0h*}wi~fEJ1*IAWBhF}=GqQ4w=a?+dXp$#mp$pXl=QofW2^gd2VgsT6R12B z-=nrl#Kv-CcCL2#z!6IErMuhKh-UfeGXVC!!!YS7duh+kZg1Yn*2&m(X1PykV=x5f zZQ*ZH1=a>Ti>jo5uX{v*;xk!JfM-KgWZZ?d1>(lR?X1lcaBpwyUX6`+d&->GZ|!nbNZa zt@ekXhEIHCd)Hr&+?aq#T+b(Ji2sVoP2s%10h z_xbW?04|}-mZf+_`?Q$`D-020ZFZQ!+I+}jq!$Kg&~ua2VjLM$BVilOUsmq}Q(r+z zb(xKqeJya07g?%ZYE+4nT7Fa33J$-Ty{uovsIKH^ET4H&svja4$f2Biq_|a-(mGO{Tqe{g1FWzFnzfA!-J^od#*?xf75%i7rsa|dK z`sTNe0n!+E8iY9^TAv$lKg>Y7(Ab9Xf}F zbTeE~rZr52+QGZ5wYX(7zDv8T2dVP{{p~TTvDT}fv*DcAuu2CSbQ=l(p?H1B@lDqg zL((UV%SbfgaU+*>;W40wl)xNe?j@a?a^ug9lxv=(wqSv4ijE^r18Uy{P5c+=y)`|a z1(bwHi8MiDmNUAV6X+8v7G>KIlumEHyfoZ!abIS!!DzB9nZjZ)FNk`Ma0(cc4~^t# zt{i0=3NA@iX{-z^HO3)Nd(rreIW;vYtt2rkEfAiG9b9VE;BTTn2!tB!RO_5S7GU@G z!iFQa=Tj5AO3{AOW6YMP83z&~&73@V6}CjnQ+Qq%HTMvHh)_UDNlucwu}@ZQq% zg+?E1Fx5iY8LRFp%3s^m6iw5Nq;*CnCa2Hz zXIFZ^qczTErC&pp(?GJf{L3&EGfIKw2{1f_T`Zz~(L)kZIKYntpZ$Ep@PdY$`SKO$ zp05;tskAtjy_p8A{PCNAPhn+sy54Nr1W!N6T5V^3zG9v^-rjOWC(k^qY>kZ?;qNef z94oQitRvPMv+96#D#3OtL2^X;G(j%0Wy5qtI-G_zv^flEy`Qa^mMwjVJO&QBl}LwG zLlQI=k)RcRqy(LymoYQWrLs@0f9=J*o9?Ai0@mY{1NueDrHqN;3eG^DH|o+QG7F1k zQR{2sD+A-f&@6%FidJ|S>z>tmF4E4FU9UWn>=kqX7DGjytlBe6)&8mAaH}mfvR%S$ ze!J{!F51JhWGrA0|&SG~tj6D=L|3RRh*q{Ot z-;^JEe-kZ;$c&%{#TAhj-q(h_${f@yWHo(TITd!-tiO8EsJ{RyZ=O^;2nk{2-l8k0 zo`Md9GkExbd5zlJG~-u;{2Sb4qqxzGju?~dTH1S3KeQuCQqIfJ+JiT$Xyrsl-A8=t zDB3p@|LXa-?Tifqm-oK9MNbIdlKR|57s6#E>uYyogu;7Fwocu_mpCb}ntHSMh=q1d zTBnrYeylTitS+~BiaiEs4?;bd+YL0KkqevQGSp;ne7SPWmnb-x2Z7ZTsDle>YNRo> zgBd&nTouBuY1l@Hz2sd@6<|u)Gv;py`yl7%Davcpk?eRP=y@O7$eOVX8zt;3Ne(n+i9#~`}`+NDbWf@dk zYxFRq)-f@PD`t;6y+F~?MKNlgyUJRhV@VHEBS+=kMaQ`gTds_BGz_g&Lc@^0D&6<+ zdR4mW&?u{J0|5>UJb%==8-$9=3MKRec|v@v0_7jRy|TXolK>+svcJNPt;@IO2t5vq zA>^Es81X@@)TPUs!4}&$vF1%pV~4QFYx?K+(B&Ox&7pCR{5qkVZWtU14WM|t|Esl( z#kyCEmN2|D6z>9C9AqaINZN@z9RId&a06yY2d+*&xW>J+&$Ec+h=L;AK?b%GK&y~s zVbbH5aWtI zHV$OO6YI)zW~Xf-eLaDGPff@7_M&Y1JHK4^=;ved$usZELG$|x&CYVOhg%~*^7PXT zEdRWNW}U;zcyu-HMm;XIxfF>K7F`&+own4@6r1X_jI&?o2PrOiGtbLX6z}s-HEd-e zfwF8P=R2FW@&kI?0OC~PektWN>WOPXKUcT-ZH{VEcCoi)z)^0Mr{ASiWh^1acX~-P zDxwx2ryC?8_T?0>FH!#Lp?3_}a-{E<2A)vI;28&z;=du;Z^cJ;0GS9|h*JJbZYV#t z%;etMW@uDdJ>GAcddNVlbrjdm5ql=|tuVW-zAibB7sixMdL%!onn>ab9PZK zGd676ORy1`#|a9LFOceVSFfCk!S15BwYL{hjSl`#RqQKkXB}wDS77S( zC|4(G_$`xToOu9kcQxPGVr9NFMVf}NRK;>94G0_CBRjh)Io2G5Vb)P^LP1@l1j?6u zTkY-eFxn?9@(%dF z`Z)7=sQxaFQ;$siXeDKvpU{FaGuNFZ^s^IfubK{uF%tT;q0ioxqcBoX?n>^-9soyk+@2L#Crf?J(i=;()~U&rl5w9+t&^ zV}oilAOGg$7}(I;GBO+?#wL zcB%|-vMBcW+QYt&`pWUXm%cu~w8&t{Pq1Iz)&FSO*3t(w89QiIn7*;UdYI{9{SQ&4BMkG4~S^s6DnN5?;q%-3>4-d>)Ho{&CAdU7=)o zs6fO{*om>D&4nOS7D&qe+?PametHnwH#0h6*%2tf&g>|(|H!oR={W7)a7D4rVBPR* z*G=A!t}nmrYGL>!p72u?=hhh>;RTjX%>U8kf>%fhlkpN{mSX1a*xVQOLih6n{rrcI zu^s`Q!V+7=4=M|hk0(*5WD(*bLQ%cZi1o!uqZ5FqP(=;pe0>zlRFR9&%4)WDTTD)v z1YS8l8{5t`cH7s9_7Lqd-b3XID55bA-@AX{*m02}_r4Tvde*dx?%ZPh>%Nrq2e~7Y zlG*82teZo1TyspFD(d{^VCy5(U`t>@&rPXp|9~7EVtiNYic{=_u8hu_Feha{)lUh_Dws~6j{;E0iKIJ6P0=5 zqQ@tS=HJKFck}oeExa$`dv(UtWq2rKAatAZg6(_BFi}ErE zA};LiO79mnHu|5&4(=PA`O2po>#^6dI9za+-$k*s;Y~qO>5)rJ3rzKF%qQHLFhx>F zw(#_#wLVL%-f``hAl>E2lw6> zn;sv~SVzn7!OZNdX82{L)v3qD)6XiCBT|&j-|FuKEqk_DoHo6t-QxIRMww6B(1|-k z+tqo=W(*5O5wwHx?Vx1z#-^S(ZB}N3}eiQb%S|@o3^$*9B9wjxIE`u*?Ore zd)}ckJD8f>ZMe_Z%pySh-Fdl^K>zLpEk3Vl*;DUkdfh*g0vv9DbIA`bI#e9g~RjpH!55YXJe+nVv@@M7Kgk$>+J?v7v&>jY_EUJ9YacHQ1=!LD(1 zcWX)mdX&bcXu>YRms+Y0SW4k2c+wFvx%6aNKtKV`=&Zv1v#-j>(H1Rb)S98M3 zqU{jo%K5i1MGqZMd5W;`6-^Nh>Nrry`9erK6utY=Mpi*figkbxoo6{^hrUyAyG7T9 z$!bBC6`k2I>l#$Z(*i{wa=GX%$pxzSyypVavp4=8i6@ z389rX>lM8@Omtt}4|^(3jVCXs{~0TFbK8VICuM!q^~gPp6Ld9!mshpD^SFBQ%^Z$; zjWe?Dlo{c1>NL^U)aG;tnFg|cmd;?2S8^PA=)H7vs&RM}&Hf~NdvnjmlY=@Rf(*Ef za%@EO{M`kbL(bVudhNxl2MRcLZt$wBm1o*mo;<{)yl>QzL0#!U_=!p#aPF!>C}!8i z9IZ(oVN;s2f)m;SLM|nFnz}wKTg84dNl(U^pM6Dp*MxVzbYtC^iae>F&7~^qwfsBV zhJi7%HPjc(ooP_qZRu1uNNH)6L(?O>K-inQuCXD7PPLaapt+D;p*PNC2X|O4NY&jI zLd%du08&E&2_TV29$o-80eCD5Ts0YbP+UwPJth!E1o3D9;>py>Zdw$ogQEu$M+8s= z1w0PNBTs_hF{-VqtAjHI7A1mDqqf4k$zd>n0t$}d03kUhEFMrmViZ6ew5tM+ z_;Y`VfrH~9pp1e=!oV2})P1AeFu$lttzvGl>uSKyw4rmtVUYe2*_{GA{GPp`r;W!7 z4X94ifIZ;FuuxJY4I|oNl7=qX4dVUmpdwH`yjRSI-tJHdNUruY80#~jI8Y(X&>Oi| z6;g4cK$1`TbUUcJt`5k(-@_oIp(iX`s!%R?VI<)62 z2Yx3jc$a#E*wS1yV8P3Gr~K;NfRePm?qb^kVeeT%>3-FbP*3S-Z{+?onIt&wJoSB&eriEfOUufm#c zvAiaoebq77(tO#im#Trw@6F>EaHt12=WYFD#Y#2GvlM)==*1K2Z~M8-q+0KKdmhQM zA8M!yAl>_%UbPVPPO0El%qB3yR!yybYpnBTfx{Z!7qvF3JvM@VN+PGKKfpM*X;Tp*t?&2o1mCycrW4#1-+B*jI%rD#?=C&$3TcwTP4%M} z<+Q$!Cp$7UBcRTOpw^YCU;ur3Bbu=*6_znWVpc%cpM&EJonp^}Mq&US^p6LL13>^p z+9QA1FwlwN2kH9VMt}y}KW%6L4XwV`4-Ej&s9Ix#cwRh&*RHXl(F(usK}7%VN5lXS zRlGKqh#^4DyvBwh;EDe{GlqzUM#n$=h#>Kovv| - - - - - - - - - Canvas - - Map - - - Content - TitleMap - - Image - - Road - - - - - - Obstacle1 - - - - - - - - Obstacle2 - - - - - - - - - - - Model - - - Content - TitleSpatial Bicycle​Model - - Centerline - - - Car - - - - - - - - CoordinateSystem - - - - - - - - MPC - - - Content - TitleMPC - - Image - - Road - - - - - - - Prediction - - - EgoCar - - - - - - - - Obstacle1 - - - - - - - - Obstacle2 - - - - - - - - - - - Path - - - Content - TitleReference Path - - Image - - Road - - - - - - Waypoints - - CoordinateSystem - - - - - - - CoordinateSystem - - - - - - - CoordinateSystem - - - - - - - CoordinateSystem - - - - - - - CoordinateSystem - - - - - - - - - - - Annotations - - MPC_Path - - Path2MPC - - State & Input Reference​State Constraints - - - - Path_Model - - - - Map_Path - Dynamic Drivable​Area Information - - State Reference - - - MPC_Model - - Model2MPC - - Current State​LTV System Dynamics - - - MPC2Model - - Velocity & Steering ​Commands - - - - - \ No newline at end of file diff --git a/map_floor2.png b/map_floor2.png deleted file mode 100644 index f3d4a442166bc247fd3dfa1588342f1bfb88df03..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 24342 zcmcFr`8$+f*dJ>|A=}7KzLDKzOSWu9DlN9L#MrWitYa;e7+YoR6e-2f7{)qCjD2Uu zSS#5YJ2S%w@6+{O@1OAcf$Msn^E~G~=RW5?pZjxvZm;g#GCOnn{Amyfbmqo&B70+ZxuIXUs*rb55pm> zBz(xd=Q>flvTqqwt^hYX2uiP@`jF|y)U}nNgSpHzSC~~m`&lZi&E5?+%x*~NO!vcn zlhB-9k)f8^cUVEOY_K*TK;W~e5J>VM$D3u-7 z1~dEl$2Y=dHROgTbe5*4gWR4R47FyC8v*Y5l};0glTi!4<%b9tnG#g69o-YuHlLvS z1C24T_G$RFL&F}o&`4;u)X<)*bjVMjD;Wesmfw2z{)KeQG0I|+M;9n=4R`=+AHTM? zNzkwROY2{oJS^<@A7GekV2IO?`S@t3hs^<7h`K^QIVaPK;fxHJv5~Ltmt0zjqJ$QL zew+r(qX>n#!cSuNno3(CC;dC~W@$y*%@wsRW|L!GSL~C#v7B)kz@iY-&f}TpF!jX9 zgdO{nJ>lGv6;b?;;o6>{JMH~oX2B!-Jd6;!iI}#o4b0&|nAM@e)AIM9J}7Vc2eyhC zWpwF)#@#$P&o^E<H?~<b;#!Th1ja!%fEx;rRC=xkigaV*s_!vnIwT0IDV!YkS{7MgoAjqTo@s3; zNX`nI_37`@nIs?(*d1B(ku13pB0;N@a?}Eo{@K zHfjcQ@)3mz4qSB5?gF{VAPnDA?#h`sH)vnabP~kUJo~!N8}_MEKNJ-W-gG)BK-oRQ zxPjb$=Bmo_mDcx(m|{a8L@i7ezs}?EegW}~#ms`AhMj9ECmL8ur|T+W;p@{=g-WVX z5WaQA+stt{749W)Nb$jSqsXmc_f}9R^M{yE#WYv~TBDEeNpX=4$W{%)8s(4UT^u>t zJR;35L-qc1FQ!b)p)N41f~tA&Kcr~fM(4&JV8dVgnY2J_?*MKSx5dI1%(A<_}2MzkOQLI0+zeB#%s=0rA*x@7kvl2#F{$IyKZ{46gOd1>d9Y>T}} zfZ{=Zth0F6-A3}FjbF}x?>XZf0CWEIg|BNG84(zF&9$Hcp&MkG4<6q@-8aEZ?RGD!QKZzh$mYV&lv-D4lnbN5lXmd%jMK=BR815m>S=p0 zo%`RO4>wS??YaWWr22e7O%!!EMwt0{bb`8XfuHsA#Sdi;-tu#-x)1Xx$HUfU6CyX! za441yl+TNQS8bL2r%7jWVxMGnnD5uZ%^<7V~H z(9)f^@V%hOhzevh_zxII$3*dgzst!7trMx6+41G&UDK?fVPzN2OInxqqwlr)RXwbn zbw0hT%I*$wlR$jyGnHlY5nISir|$0V5{p`SqRg~nadw-Pa@h>rnJTOw?gZWbTzA_4 zd6W{g?rQeQOarsTITu71pJuCF>V7IN#_^=#*mD5R3j!SGRW(7KB(b?5cVC=P_Z)b^ zNpn-~%r1z5ub+se zmcN979*VSsO|YTBZLbjjI{(j)L!UWAqyur${`qiF@i~0GpCh+8|J4!}wa$5+4FuX< zwmibgRCp{PvPro>{$C5N2Wp28)GVy4e=>?_+ZWsJG|9kn^fLYFj38&=ZcWRrwUuQB z=wCuvr+d%2&f3L!;#$=R{g}f!VbRK22h<^rK>LudpfN>)-N*YutR8#6s@;5VjXi-| zr_A(EG67b=E5bVm2o;@TEW4N7$HIfMQ z=O+Sd*Kho+*ov?TU|Gui+In1o+TJ&5?Z@KY>+G&wj)tL-@+rf1c_Snhb5#C4I z4gufRx~;anYcaoMqn(WgVlHV9ndt;Fw&f+L9tV2n43AWIjxgC7<4$34KWh00V`5^i zJ@<$ySJxO4>uNalPR0e%s_=d6a;D%VGZ%7iDk7?i_%QUcd?K5=f z?FN&V2UJp-&w{*egBQlU;Ca+;h93p2LASGvvZ3|Z@AW%D4P}FY={g;-5 zDw1yyw|j9dE3|Uv_8Q6n>u+%%HBF=+(A?O^L3`g=#h9R*Z}-Quh`mv`Z>Ff_d%rs$ z(~xJzo~(=b&xSYFvSD0F*_}*36g}{)Hqr*V1u{DD8O4B^d*q(^th?i#Xpy=wChN~$ z-!9t2iy=_fHfRFcaD8_8;Kuugx^HH$aPG;T@fB2xx2L|Do|%PfY)p**0H0%Yfie3} zyUv@KhLRQHZIwZw)mGd)bzK#+x z&9V1X2z0x_!`HyZ|92EEZ?CQ;UcZTlB5ISgtwXb&hTn3vEgHiqUm#$5NN)b{%ka%i z(BsS;U_F$xp&EJ&Q+#?-lX@;b_gK}O8A-Nd`Kb%F2(23o+1ri15Io)LhUbk_&Jhx} zT~!z+6-BRCoN@O3y7lBmMTmHdV!IaU?o_4MdK#f{q~d#{ohry%6{7KVaft=$J@^(C z`w9O-GVG+~#U~4XxWb&2|3b^&ld%-*<%%x!>w%!FW+Ky|-|ilW6YV^fWcvDFr>06i zbXXc0C6iUd?_OcXRnOK3x(Wjt*bTj1cQm?JvOgJ#n6fxFPbMl?s!M0FE#K!(cJVP4 z|JN_RvI64n6X2YF@|%w4JnEuhLxV0L?RCl-RWu;5HG0s;HOqh@C-?cCC}uO*CcWQ_;@?;f>n)$c|j^?`}i8x{vh{Jr{`X$=vK~b zHSB3uB?+*4dHVbv7Zc}U^5RPH0&Q^P!+)>EIP+X1A#)jgH_;X*tGrX^I_Q$w40p`I zB}ww{6_PN*4SEqX@={dGs^Zj664d$JB=-l=Z=7+`?cfLQ9PRGbc|g_R>?S*C}{XRelc<49e1f6ofed*8e9vs~-=){Db-lK}CDMmSUu|Ua*iEobg9W9tAfara#fWF`T+GG+n7=I$5vgRV zyK%9ySzqL6ZNyK#xJb~D^M|7cet*0S;vRNdJ=|!h3PY&e^FxxVDXI0Kall%)2O#dw zz6*|1h03xqJ*GdSXEPu3ySTUIaQZQKZvcVeLx)P2gZ0&0Vd|$#am7A+!DA&6pL2o_ zQ*d$E+5~Oy74%h}AG#iRuZ@+)>O$k@7~klD-k`3!>+4b9uP&K_;Xa<7wO>PL3!rw! zVsN2rQU+WO5K}E@fp|raVjULl<6~bY zyKa$-$6gm{MRJIcNt~Te97a6FQ$*teY(HMNEm3erTnc)>|KcXhFW}W;(6f9B^0$px zmJgP`?z^P(N3HFXq;G=EAV}TrB=>D4{0KG3?wKIjW;7_wdt^XC$38Ro!y<^Zo;7Q@$O%Dl%KHkmrSt5K3-b?juRDkkAHQA&bGICMoLX8%rH2F|L28#j!`EmrosS%+c+M)%c+z zFJO^uhn8;=b`sr1USzy_5iwHQ|K1mgJHmB87Te%E5K1lARNQ!^Wx1%yR0o&@4!ojI zptnD@^Yvok4RW#0SoUVm2wv2AC<&+QPfQEwXPA*js3xH92-J3`PAR!U)AC9J?=KvO znx#R$l%(jiL^)9IX-p$KeyefB2%{P*}!LI|u|EtPD-&hIdG1sij0DNr*q47u5xqd0< zlbzo$DiCCp-Gc>{Isl^TZ06w^3`E<(as`gbftu+ilm`;`(kVR=-pW8=URzF+YTB9n zhaso_96t3;L>Rdmw1C+_}#Vtz0M`7YV#U zZIm-daTAslqgTmVz0$>NSKb0GgkfAkXm;3>PaYD<*cmU>gyxTy2z`OTikoAhajn5h z1t+2ARSXUQR?ASA!P(m{X8VV)Rd?VEe7+uA3r zvstT1pJZ9dP%*9ZW>I5VoiF6`E!lH_E2>PZkEy7v8l;d8a)(x};FUG%J7Ct8=PIVe z!3`5a-}9}$0G#v?-F2z;P+_r zeDNY@vTDeH@G`ykqo^4{a=L-#ALsPnCZ2P#3}ojNjINV;!fM0Ou0wm+M2C~;U$MSj zCvFj&JF04;!DkfPZ3kjf>75J)RSBvuo_u=TV{Pjs_oamm_FLF!Y;VoM^XlA1`+=lP zS#80m;5M4VJ-6#&NvWJbV4W!t_|#Unk6eX3@jbdJ5Vr9kgyIQlqL+4!j1FPk-@}JO zLLz=y9fJ0eIjS!-+Ut%yryb{U?4cj^5-<2<fAWH z;X2AY$F;6u;wR*J@~Z;l_$oG-%52ky^u2F8HD%pTD(u{Vueq<0S}i)3=gq(Ctn^9j z=x@R7vor5Bh=>t&@ei@njYZ>mtzGB};sa5BdWoY=IJV+3SwqN7zv=G@YCiUj1a;HT z(xZfSW~=bTUK3P#{(T#~uYd$L>4vKfrdp20jO1?HdY7Y{A8&5YUEXPs&tdPQ+q3-Q z#e3bVrxP-ZuWfI)lmsk9Z1lCvMi!P2Il^wBXLFRDk6F+`!Oocq4J_fwDyA7XO{5mRS>k*Ri(hD_hIPcu+|^kz#fJF_ zZK8S3aKBQ~_-duMZ>Bw1zC_9{7px%KCUv$B4w~K1m_GEF_MSfn^BGI6I}Gh;3nUat zQ`v`+DukRe6CPDj77#6y?nE`PhE;$Amiv~y!Kk8v7YtVHFm{IngF)+#kvS(Gl*%LN zX-bq8@UlE}v8La+@BmZ#x*a-knQZCtJ+6CZPdok~Z|B@o+vwmEXKgN0{=icqoB5>k zkFm`ogNOwbRo(yTWH4D+D?sS8eq44xLM8*P)#JZu)VNIQ!X2)=;j{xoN4lnV)XzP7 zscWpjv*St zOS+tireIijAJ@|2sRKJ%@_|t_c@j&15tcGOKaz%~lB1PXj8CVNYI4uy%?n!~O&BZK zVVs*4vX|K1rLBCU4`3x-2v^Hu$OvjJ7^^-1K0$nwMj`uS0`qQ`gaHQO6d5O%T+rXn3< z`_|t`SKd*t=IkT0JVj%Lk3$G8>*D0<_h#eheD{UaDZ|hXA1L(sS4wdIH{XN#fI6m? zQ8D-E#*IoJh<^F zmNI+_ql~;Tm%|QC4TQKDShm|r$*|aviz{9@>HV%QIu+#Q2%d{LuA3HKuN6e;TH!=H z3KPB(svz&2+ig9oV^-4kB#A~33mdv_aiXfk^FC@U&{4D$`K}!|`D1yDD{!Ym*#rjZ z-~sT!UNdEr^fFetOGG_kc748oxzv3m++9SZR&_M|%Se`)5>gYGyHgX~M>JgX)&BK2 zY%VQM00^dP_K~3s<4yw{1Dm+4^kDe-OzxFnL+BOxd%NNArn6Nq2(XoPW$;|%hC(m+&m;ETH5BQq zyrl2JRq2>p465v~9+dv|xxXO&!;hikT-euot-N=tsh&3^njmj9psBTpquosFK7Kd{ zk786{6{@OPT{!e&f_scJ3ta{wGwvI(aQyzmLlmK6a&E};tXszjR6_Uxk`|94eYH=MOJLCbS{r=| zNfgXi3}|TDo&9_Gcf0fTaBiN%o-0Wl@0Mc?^CCwSDHQpt%L};Vn|K+$(!4fFfQjNX zv2#{?uS&=>aeC~q8|y6Pl;`uF3~OuG0>zo9D`zY!kQIUyqHo=H_U)e3P?^}eo z3%HS#qi>Th0T?1`d#X~NVQr;F-q%OFFI_d=Bp_gDA-dsac@_y>J{0m_?}7dmGpnh3 zGq$G6PcVkUzyaG+gy#dZ0q;;KePZPIxBYzqA6H}EXB=chlOtz;7UU(+6np}m%4a?M z&en-(PJ0rf5Ks#qj3~#QG{8-mS2AEY(FIe{aO(QwCuFHVy?2Kyxf3Fxz#FD_TUy3;YZtYL2z-FcO2UMXWQwk!+_C97hn`C7_cBH;V!|Kd6*0dLe+~3HXlQ27k<@idykcEx{RbAu6o`~lk8ts zy7NqhASccmQZrew{zsi}6yde$h# z{{^lFAS*g8_p5T6%O269BM{?1me)65oFvw<@#1TL-VOGL0GWe6yKk`4Rls$$_yU}B zdUWx{x!sdZRs1}UYC2AF!PdU;lk(Kbg{U>a^HTS$0vc3!+u{Ha@%5*F;1vDY6{w4@ zD>zYhR2v*|b8UTtbR}`(v`b%+-;q;;35dy_8~;!S&XbK!uQf&&B<6i-eo#S-?AN4! z4&iPRHm+PtIKLf|Ra#)snxJYMe%CF!Hn0mgHkz{T<2Ald+n+)=N%~sUJRUOwk$-~Y zfW*+gK8_{+=FcVBbUBq49>0i(>)fv&S+A8#gnDWy_^|bfIZ@{Xvtq@Lu3UYXr@}h6 z%P8MoF-b>PM@yLDQr#|$Fxq{tnwo}llNA%(&p!10DgSH7^@kV$n<#WcbZY3;xiG~VLw!ZUi=HH0`2G!DgSfs+5f^*z6m8-BYIn5-U9CX&3FHb#bqRz9k zh0Qe};VYY=1Pr9p#FbYd>H-j-M&(uyO->&7gZ+_y)PYS}vELW9UT`9JM|Hy2rf<~` znk?G0d^}uk{tI>Px>5J8)=r#dI-M7UV}5rY;~jvDX_0M)pLVHDTeJ-GXo-v%U?2jh z_%q>U$VaY0S&T?20RzdQHwtEWJTL<@)qc0@S*Ya%&Z{%JXC5jByp0xUi4-3)5CXo= zSXKcFfT%2cfge1{+rCE;Hg6YZ8Y};lIa*Z#9NUadYUziqfM~rmq?8%gLr;toRDai` z2{8Tfv?6a9!3Q*AuyO55Ax!ONW-E;2($A(L`; zq5JOeICQoUjBO-IzH+*;@<*E9l%jJDrL1WK844eK+XtXS8 zZHW&!0=SDfO`O#S%ATQPb?uYOHd(rk(GIC5B9K0;u{SZN0Y%mb5FHYBmK`r-Pu@A7 zJAHTRV}r6cjopA90(&BF?!rA1WU4UbA0Z>9U?pPr;karRsqD-Iw>Vv=kz6)WIa&br zafU^7W(Io`IDi~RiA4ZEJ5L>^D)4>8hhL53k*}s~h+HO{IXR&}|4$3>sp?wD2brfr zU7J~hbPuw~9O=R`N4wy#FpxSflI;xnj5*{>Ss~{X$ihCFN+drPsda3)2&81{*0PYq z7mQJc;1V5=&MAEYv6%TO53uHiaHKH!rX7^%XxgH^C-#$LCH~CSm0rPVDc-&;VRP`k zL#e%iH=vPAQR6W{9F8qCoNKgR@3UJfhv7|y4bp(%GmzGz;hZ>*JV!cRfHOd@YgH%J ze0RkX?fuAhRhY*Q4ygh*__m3HvkZ|J5F^j)IKC!6Z_De-4>U^P=`y&GV|0grb;gOU z^$cZ3E^IsSp5D6&mev&?-RwT!U0vI7(TX1XmX=_Z3ZUF=Mi@8#hw-LxSpKU!4$f#! z=A^n!cMdmo|1;Z}*ZmG&mnInu0bBsarAr6-{Px>l^rWeHh3RcG?)O83gV8;cUZN{m zN0W_;HZ$5Mee2g99mPzpL{31-!E|(S>B&s%k#)6ZLK)@jW78dg_-AzVJ;I#kM%ir3&FA9)gT7i{oygZs zIC(r|J@0tbUP3t2$e?x(&xUgQp30N7%4X7SEi6?-{(|t^xbg9LXq-Uk*J1)VA|Arn zuhveDC7hrnlsM&f)q}`foB8bX1SL6EV{;B8ObfboV$dXKD(v`O+XDLA5<1s9^F9*Pl5K)F+dhs}bFk zY2<6rZ#0ww3||j=r{RLQgMB*L&2onT8@lVDoO9qM-Fr3}H2hS>6f7eg+Wg?F$xMC8 z658pJ^Xe`&e9St&;!Hee91!It{Lq6$_lS9$7G?Mu#zHI^wN-!l$VAS+h>yuC^*t7& zC#Stp($zf~0427$UrqIua<21K5aF!>^#ZJKaH-~LrGkUT!}nFT`_|N_>Om?ib{)jk znU8>mM~L^0kaFC`e#G`?oJ8oN$53t3ud*gttToKK9KU97+$WVEV7#_F@~q)23jnlK z_3oYTpvGW-Z<@2aZeAf5RdnX>9_vQWA?=jo0JL*CCnon1rg)JeRPZ6IYl$ zNrO&XjNL}y1$CRJlbMUJ%mA?hpA4<}LAXbOd-j0e(A05-)T#FZ%*t`i393$L8->{% zIj8{HrtYRqO(&)f?m>_m1s~obtDgJmkVj!$NmjvoZa7@Du71?zbf;WbUf?~q&(HFO zL6Po{pQG=9O%jMdZICT&mb-VEx(K%h4IWAC{3AiM;+;FXTrvtNmS)Q6I za1`#zs@{87DSk~ffU^8%`)H|piU&Ugh@+0_p5#piQwfAsXXY85)Z-}fChhc_QA^#z zyL{t;7u_6>avHuSb9a|5^!k5s_R%F&+n%bzY=t~YQTex>#HJ71u5wtJnQtuLqsOCc zlNNgqw3-&bS-B9eqzi>njFo$R3dw5=A1{O#9O<=P|M$ctF>l4&zPj=n)tUiF$Dp}E zeA~{Zw7_FVQm_zR`PbxO@7Rx8O1OoU^E5~u$UvhNayMAfk1kEuzz&uJhLYr%=N?S8 znk}nFuDqJTa;i16mZW_<@6B2@I>CdKXO!l~mwC)x{kak1x(j>j|8y9>JvIvMyp57c zvJo%m7ml3CLJt*UwR=^Gh1*k+f6wbP4KukQ{!o#M$iIV`rJf$0L1hzZWD@aU-Uiu& zl}MizTogfeM;6YXb+Qprv>3W9@Gt%jH|eSFEFE2Z1x>rHJ94N_4l;TETiq^svo32x zmtQX2Pa8TXhQfi=|K1$?J)2^a$y`pcrrK0#Pt%(&b`F$@(*p#)nQ2C}(08HJ1)j1$ zAT{V(G5X4&YBLXBjkYkd3^KS4Y^lcA>pfMzQ&xA!58F1z)Dlob5ii;yJ8O>2x|D&; z#VPev9eM?k|F%XvW49@If!A#MO_uLeARob7@Uh++`Ac-n61k&VV1to_7ITUi))((fZUDWBXW!5O+fC zWngM?5D0!53H_0!aN8kvr84>aui!%LYSw%tr|Vda{+r+$=Bj0(DtT-rhvV^VcQn%; zun==b;Yp&;tEMajqT80=s1JjETHSSHO9xx7R_|>%dtzMZr~moBd!Cv}PII@tbNiE0 z{I#K9|87>mT=vLad_-TloHGy|*1c4cIqdP}i+{N%nj$8)D$?2HCh;`_;|(&RRO z2-iE!cCbrDST{r-KNX3ns4DNdsswOfsaBpEraD7m>jSx|S{`vNfTPAZHQK7lQ{Qs! zhYyC@&v3X4CC~QzwZg_FG^Qe2e0*-Wt`0G}{X2!Ke>kyL%WK|4jj;ZjI_;ppgqwgz5KFR*mG&V|Q~mzXqu8 z+<<%nK2pH#VEJ~1Q6U#4pXzZJ(=$VremXh7*QstsOsDKeGT!9{g%R~=dx{n>cO|8@O>x~SXdB@ZpL>m3di4!P=y zJ+4EkUPxfTox_7iM|`un_F~-G)Xj>qj;anrYUzFhoZHHq3kDwT;H}2%&#eds0-w3E z6f7zNd@c^-0(FM2dx(qyT24R&pl2x5+vw#G!{nDU0Vh0PESQ%Mq3-8+1{EVl`lGWu zGtrw@eR=Q3uk<5So85Uz-U%5k?TWZ~1dK~iLt3lm^RUNuCnBJRzr;p{Eg8tee!wFJ ziqv>FaZ_=ame2)Q+zjyK1rrVo0dq4fTH6RaBNtdh98_B|&d$7guEEcl_fEV5@QFOQ zbl2&;&igMI5Bu}{#I+MF$3hLo1Dg@y{#akX@N?@94FA|p_keOQ@kv;+?Zd3esgSA{ zBSw>#kAypr&!aoGcL=uK52KwDR3vzt9n;suq#9>M|b)HPOXCN}AP(JBO4T#pz6w%$$?nPRzzc zFW`q{hOz=VMJU^EN{e-;8})n-PGnleQTmXypYjQ+5;_nV=R)t@$~g{js=E`Rv5+*K z%K&LP2|w4N=DWgR;Ol`0n-Vy7;IG|102)`Q%WMf7>#i%di5ObkG=?h+xo{eDd;{6! z2*JScY0)YUe({a%noIvd1koOCsS|8~bI|1#GFcLus})OScnF+^j|Vr5O5)t*ex18q zXgB1cTQHK_b-iPY7i}39?#vn~gHXM!d8+hn;OqG-;S)~yEXi~eZ=25uKUP^8#J4AU zQ2eBnz@!7F*2-+4G~cR9IM2lQ-sjh|h78r`@DEK?V12U&9tyQ}f*szxQw;UJ{z;Dw zmjbddO?QEGR-t(oHq%#Nx3&zU5%8tfs_V@%;)3gR%Zxj}#CZtpaXeS7xraVM)28r5 zHKt`kD$(-Z;V|yvpZ(-1XOJ^D9;9@M-3?BZu2o1hlZ&K5Kr@S6vqeH(cQ=)zhct=Wr^xnybV!aWzzQPIi60{1@pe^xL@Td zSJe8>b%RW|0Ts5vCf~F9gT|`mr=jI5Y4!1DLrB94>e2T|2rc95>EXg0RoSe#Kf*Ji-sMWVc9-fj?pA>Iv>5HBBVycpg6^OsmRS7yXrzPuh&L5bXI zWQ^PeFy?Kw`Wuq_I5sHWS&l6wW0~szyCQ(>JyK;;bPvoFDUC3cIsHALA{i}kx{Agg^{*us)3Yk}4r+Uh?Uz*6pZj6AbV zaF5u~XWi0s;nWKXJ@fqOC*?kFjT8N8s>YvT&-HmhX0?Ctb?fP`RpFNXf6tJCwcVJRyRyVqCi0qaKveuDac@@fzX*NE#`^{xyrH zOd}O{QYeCGydWHCPqLELncgVeA3S~a_>Ajr>^&v>SQDS298~+{(Mfn8A$*rK2nag$ zsx_h5k1Fp3`-D^s*BS~n_rk|UIG>}h`#lBQklL8e-{TyXCceaZ+D+DOjD7NpXu6JT zyuVf!Zhb%4lzZVU1U5Vwgs)i4!*X$Y^K{JdKf3p>d^zMukG51V%kGLgOp9T2opJZz zWW3k*Iz@t7kO*;u$1juf-es|(b0Zyk%S@vzF>EvW?%&KpvgS0M5c);5Pcy0szQ~z&GC~d4taX(c`mUM_7x7`#!!{fQ zh#CHpZ8UjPzx1{-%_D_Q;pIpX8wG!sv|mO>upTvO?y zhMGFG5C$uS)>d)J)C5&F-)Pl?{!`!T)dUl8{wrw8R7HcEk`H&@gkCBzOj7V^w}Z@t ziqWEY`vleN#nf6Jc%*FVtMWPnEWe6(iypi|uYzzkxmIJMeyY%&q43B7@x_C??{A^W z)pjtmHB34QHLsuRe$Q(Q>y(s(dbelFO*R=&+?;R8nxyNFz7T70%vnFzv7d`tmy~=C z((M+}nDd2G-Cuc9=^GW@DD?B_p0zGJoh%?vVS~&TOR+RW)Um9P zagU91{vaO~)#a=@cGz%(RZ^3pG=E)bYLX+B-Id};S)Eg_PzSz$C{u5aP;{^BiWV_E zAMzB8Vuz``o0{hwn~QweUQ$HLO?j+G!_DeT(@r99`ll>hG+bLR!Z@JC3=vi&OCHwC zo1^u|FPqJiCs0NAZek%p+<705?9dl=?kgScc{yYW9Gfk7htfit`8!fWc?CKWfsFCi zc@}e#TBNsC7n^X%TYIX>K%i}7=E1~#odde%e4TE6<93pSDIc&I&Oi4MYoBbHiY}=- zc_5!pm?>M>*uDGFZ5Jt@=Ke^^4$z#dB$iw=(wd~2TAe&__PnIv(~nRXni%PyC|I66 z6our|#%EHh3DF&DI42-!tNMdK>5c7Rk|{<#c1+xF=8UE~@{^-`@*$iM9U5keUW!OS z0!sBK-Zjs!< zWpYC5$?sooN?%yeSo1I~hvTT#A*X{Tj40nwjQ$sEN#CRV+s`I5q2hon10QTwl%N`S z)=|!ysITdo@oTCmJk{glAl`eGx)|2_p<7EBd)C-l)M#Kpjad~SD;b_F-S`UDPG@%w zTub@>`r2P#=`>2P$g0Z?*K%$ReHuYKYRC2|7>ABOwRQTrpRjrzTh+GKmhpOEy-j}B z%xwiI&-|4H5z6WWmdzDlHJO~=C!Wj+Kld5vA{USc<^n^sUDkusQE_S927{s4X1cA- zGuxEqSsx2j*V7#LB#>yhv{njOp>Qr7NgwpMJ6M_4+?4Eum3gI@C0#kKs()fnkkFu^ zl*_`Osjx;}x)=DSbDE|7`f|mD=EjUoP%IiV|2f3*p1jqy$!gKto-5c5jxbL=%9MLL zGL63*pca^}`zl$_H8T7u>kBeZBaU{^W=K3wPVXXnV28 zPTXnsD@-}j5ohvkOad7Zl@2HvfvcvOjpG=J+eCbI4_M0s&J3_`tM{3?8QH?JL{f~%xTh}PHI*n z>PqZpOS)1tkThRKv;32a&Txe@vA|xN2!0Y~=XOKi<2>ZIB!Z#nZucCGN^y}XE@*XU zuxYy9+w$3n%bdw>wR#kHlBQAVX(n0WcC{tY!7S}yZ>oh7=vW{FC+k|WEm$Ju7>knZ z_l#{0oh)vjZYb;H_izP{cPJdBfGWQ&|7o0P*khkdiCET@TsxsB2v9>PZaYM=q zDAM}GS3_(Lv>Y#xdbFu3RWM{9ukZ^Uc+_0MaVTxoJ~_rM@bf%ce%*_w!@S6dZ9)ED zUsf|t7zI73lI6@ffct&a*aDv(tsasVo9?bmZRmzQ@jE&{f4b1~+fUR&Gkxyf74n=~ zBI@L@2iATMs9PIicVDjTXKX%5iCA4Q)&3Ob*;y=l>d<^>;*HRcs-$g9f!I*<>>%Kd z-)x0Y4wjZH4#Mxe0rn~{TqpKm>yS8+a-~|qk=`C;TE16bSIH$^`Fi*5Pm#ib1knRm zqgQnrCt9an;axQpU{_|Jx$IJ%!S2HH&WPg=2O5c?Xc7JIi9P;}KwqtEoJ1Usd zH+p2$h)uphDInUPio8@QP8-Y$FLm}ZiQII(34C$Yy_rHdEXQG;{N21zezZYvdRW>u zPcAIqg?odFQ78Wk_da6E%V9^=E1rDma8q+(GeL=we5?I zmDxGhiV~{MjlLxeh56$Sy%Kie*^CYVld0F?Ep1`?~ggml9hgzWYT-hZ}n=wU9#YNCg(;;;Y+L z+5+u~fJ4v=d2(jxx*7{$+_3LN*<~$zcz72*ISyxlrRk7cTj{yI;C7GN@3wCx2#wB= z2F#7qA^*BlGG`by$4Z1gsM!@p={Caxf)gpNKmlOS?p#E^_SYJ*ES{&6bLW2H3=|V5 zRB5HkU#9*QFL~_Qp*2&F`kpSVOr34we?2ibI%VHP71ecS4ofY$km#6mFg5MBeG6Jb z3ApS@T){Pz$W8;Lb`bcygZQdUGcH7gr+qR5$b)AEnuyw*9&x%hd3iz?Swf@fFs$Fw zk){SR+q7ICdpXaGkGqX%j&BF@JTi#cqv(C})&C^I#5IfUyY8LqNEHiEPMn=hdA$Sr zI6nonC8An+6ptQnNY>+`Ih|wE^$z~iJ#6?8>#3tK7g13b$?iIGcwoV3d+Q(lkc)B6 z!OEQ!N87beUhdR)zD6aG`Z=`QfnVs_Dy(g~IAsjA z_~y2QZsltU>qhUNf9~QjSVTL*1;|~$d(|}GZ(8W$Gx71Ji*N2P{>HB|zqI@SgfIj> z_qCE4-DJup;?W9_Kn#|dzQN5G?@IWPkiI}uSAoW_0*~^s_Ql}>HO%N%U3PQE$!s8W zPhjgDbVM1sBy%m<+B)vL~Z9O&bT+k|xzhfx?*%~?c$)2Ajz*+wsx{5>`()gl1g7(K^OVnaQ4~d@&eYx-OpAkBD4ByWNNeIH8A{*fvFFm z#Ym=B%v%(#-P*SX@wRjWrRk%Im!&n5?;;c_&G*sNg(x|Dh9Stvpy#SjF+S)gW#FBK z*kQSQBBL#*dh6a=0Z>0@RT5Wr^pm@k8|r<)u)nxAnKg6@sum-+_2RX%i-+RonI27~ zbHBF!vacW4hdZ!^mtuN+ky&V*7%Tr?PH1#rm8Ii1S}ZsI8j~GS=FcFnwI};cnHbRnB}8=85CYp ze6vXl!N9tCKilB+F#61Qs8nnl;_LlDj~*#!E8AnTd6C1B-OgwAL+x{b4Q!AY89fmG zoz{}YoK1}U(YfEf6jzVdpy^!xg?ts(gwpOoGMURwFlqPsNETk{``y(;3`hgHctCA& zFa)muwHBVr{I04XvFQK_+3?eR)ng! zTZbv}uTlA>S~p6zJ-p&pIe)Rcs*cz(4J_pM4iQYWZ=%pXu7x>I#l(d^p$|Id91CnL zHMG6_{v_RnTvd?X_U~&)<*j3lWo{xPUG}IHjduy$p#?dr5}dRmp*}(4sL4LINHse} zaap%n+@rG)0u1$CoD0f2uw3N_MDzftev1KsENI{(Kpvnq5b`x!IA}j zx&;OLSQLaQhz3;q!!vdx>-da?NvQSy@4xct2cvJcmpCp~2fkTe2AuTgQOA~*u^>Wl z?y)GBP2JgfVR@v{b}7yvbkAvtZmPDc9bE*#(T{^kTmf6K$-Fb=j`^3u4@L=DoV#G( zK=GFA$Rh(ijg7J{%c6LXRG`(Lg-EzfBM*o3lbCj+6PXc51tOLYWho99zHj=3|KL$cZW6A*U$eeg{Cf|%k|iD)u-n$TE97# z0MtkS0D{L-EVe#&!DyZW+p46%n{(baV_VvSl!}09jT+3;w8Y``XZ%IfuBAms5 z-1@Rcy<1eITR~l44t5!RpjpPd%6Q+pFF^`l!$oa z#ZGS%CRz8Q?qn)|Tghi7SLp28uRUM9P#NJw~f~4A@R&x;FYb zVAqTr*b_f$$c&@uoa7<*de0V0NAry#YQVasK==z#k^^R-n<6qB$%;St$-@+qY@wZU zlCy$q3Aq1HKs$<T(Qc!;Wd$~-)ijH#1667>{jmJU zyyz3q17M(qrRJ?>UxM^eSBUNRO5*MXCi+&zy0AV0=dka~aGQ*ni^S?-e-t7|lR-Y^ zuaNQ;zzv`Y2HWE2&1th_t`3ECP1lUd+UHyX@$4E9ufu2MEzi9B8?x%ZJVw-R^uW%M1-s#VQHNb4fo3gI?{p39`EO)lO>%)QTv>4oj<|5lx+h z{WR)OouS$7=0{5kS94rXdqavY&=OKhDxyYYG zf6w)aPg$T-fcNVE;TioxP2Rf)-?R#7yet+$xH2xJWssMVLejd;l9!S2m>nexsKCNQF^HWxw z98~J%L;jA7F^gHnkBp|<^UXP&Q2Y9vFK9nr4%*JAk0V4883B0LioTHC7+kSZXuV{I zD%b{nJ0J%fwWsn*T;;HhIrGvP9?G;qFM14Ef?!DdLNph`FNtK8r9Kj6G;n5gik6J+ ze2m=8pE^xfc#ql_NE2Os7A3=`uu+_!mp%sj=f?Hqgw zliVYBMcMmUf(mDRfx_UD=tO`h8@|Ha_m*^u3{1gR zJx@~l$f!P|l5gox9(BN7N>Ra~@yR>htowr% zQ}d3l<&a2fs()BCb&3Ks3>3e@Sp9E?_3?f8qeAPdc4?gm-}C1vK7Bw0OlMaxV}V1A zuP~L;ol{fNJqkVn7}7juurtJ?{n~glfpMVRS`Dnwc=843#17x6$!gu}kK0&@EVGW# zl-w2v4*}otHFofT&;_FY@?x)>=}=%oa2p>klc)qLq4PU#fpisQEa+>|!UaNeLSaEi zI&g@L)T?P4xm*)8bv(QXCwn_Pc9-N+V%zc=eCumXYEmA`%OHjgu}X&+D2nS_8svrd zXAiF1&N26`Q&`;q;1aOzfRamuHk$9d2)zP+?%ll#X_NBnybLiZc#p1LkDk*~y6f%b zV_kDZYV}MmS0haWo35k)#`GI&6k7K3)QiPhH>}1I=d&;kwpmi^ATZd8WOTmwp7VBJ0@VMx|wB2)He{~}A zLIn3BuZmijpOuPx?{g=D_A|7MYd>}9{R-OsV$xUO@gSM3vd;NUJwa3EuL&Jn8RL%* zLQe4au~H(v?WBG+wZNZz?OOI3T!m6dUE(Xm(+l=(1Q_y8W&Lb@{t$1iN96at&LuyoK~&H^SqA?%Sa4*2DB@5 z(b_Swrr8+1S3}P?kW23QRBkP_0yyd-N``hx(k+|xp_lcAjmn*P^@95WBK`JOgEMt1 zG%~A8@02>w?X9Gh<=W7VE!)w*+xX`MU*A|^)QQSR>Xj5i+61!<1!VVT86rkA?f?Dz z*O$GelvlE?C%0$K^P|Jzc-Hw2SZoaN72*?tZ@1U5N{(b!w-sAz=V&LP`3xNZpFyjy zhysfhe|$I0{(7#)WBE=ixKoFpXUa=)1DG+y{*+tgjmqQT08PhBl7p$AB33pg^1vsL zsQ``ORs4f_K!S9l$FTn+N<^Li3F~glS8I>m4XuFQs{()~inWs&uD4}+dn=@au)IUV z6&W%pzlp_^2M9;gNp|egOoTi$!UPQt*cfOeFQWWl#p_d?yT9uV>_dak%f81398WEc zu8l;GhAykO@DM2g5I~3IAmvt-sf761W7e^T-y+m>XkTv_r&p{LH0)0`Qs{bhK;{o( zv#V+Skgs%h%Ed&u98eeuAnN+WAi$$kn9{w-BWjA7ZRd+=Fnn6|vg$PtL2{zTa^Bh%Y?E zy8#trFHU0{ZhZ10ep<-Kb~J4&%s4N&O$^YbbGd5!1xu8mM5H5o0_XVT_`tnF&*;sw zNlP?PI12IVQuIbOPT})8X_EQ6Hr&sZGE&{j?o@18^`PX&Pj)~~&})Rjc!SkYsh($C zkf!&{Xz<-L(=UwF^}{a19!v}!hWA=CLiIJ(x7cMOs&ZwbF}tvNF%fXd!-XiFbmAMw z%wYaq^WqqQJRRuUGot}31~^DDbg7m}Uz;eSia&q17=CNKCz4WLd#9klL>mFCC+6=7 z{iE(LZ;?)S71gf)Pi36wU}NWMi3x?*j6nuZjc69&9tBn?Y}y-{`1M9o1#Jp=0bU*Q zFHBtJz;aMtnate9f9svvPYK-rrvuN|)9z3#fo_tzE|N0X?B(5^%n6!qK@54vl$lm} zaf~<>UA13tD>hNJLA?NZ5AhBM2BY_*cX)jD?+QhG+_^ttQ>6N`468MMDShd3N?^r5 zSo9fedBT#FZLAYNIdpy#h+R+hA#JCD>oNsPB7kIb%_Bj#WQEKd{!> z5uP2-mdX(ulTLHnVPuw4j^@3ec7oELP3xhc-v%9&jAzAdI=3M$myX4vU#)y8&-wW% z3dnOz1<^i7h^rf=%jP!pRAmOx_g7EEF58)Cqq#xZiMPpJVrnNqokFb#}K9Z|BaoGO>d7q?%xfW?mKYP8>{5ryz%jF4OAI^I>rn#XN zsDNC^YAj8ohYr2eC*L*L>s3UZ2t*guNqbLJt}ShPU8g z>kIhpq2M@GxtzLB4##GK;4(Ceqapyl#`dnh)SxFR_erU5x7hc`?VMM>O#AeN_5zIu z$)N5_2?u!mNXpZ05w}@*6zJE2HfVF|hAoBn%U`<9pV9-@T^;Cd%xus4?yqVg&EQ(C zU-0cvHy?xcH-_KNo7;xVyNE=P%<|IrWo%|Ffb$Ibp?VduDn?0N@)aR^S`?JVnU}d~ z&)}Vz%_J^$6fwUn{j;e3qqR}!^}zVQjU7@wg$ZW|-^Notgh-duFDwQ&6adzf4i-Rj zCW~!qb{@X_I#o_=dY9b&fhLytAW8l9^8tQXE^eyuG-V#^E_Hr&-i)Sd-spX#dZAK% z9~{k4`WY?)r{`6KRitB^R>eTp^Un3)85Z|7u;EB9&v^IZqR8hi3Gqn<8zxU>tL$V1 z{zSHXHQ;Pv{N@N4&Gf4!9k0^1?$Kjr(hS{GK|G?TmtV{i#<_laEOdlK!cX)}uOtth zCdgtqxN+|VzKCrE6t9KU{xOv{UgXSo-2VM4OimglFfSX}&#l{g$rR1@1d@%X2i zX-c4&04iNvIM<&U5}q<%(N&RvGmI_2rQV#+WvkFJ#|h+cF^dV*cNlOHqZ}Isd8~9$ zOZOc>0LI*F2X3ApFYGmORTUA! z(*3zd4-GMl=%=iih^&c?^)$m8SAc@j33da>>BH93JbtybgT4ID!Vsx&A$LKm_@&M#EER4o2xnj>zuA|KFdE& z+-8;Wt5);AKSV5pCNX2tM0k%30QP+vl+vJ^sg|+=Y9OJl-l^&BxCys|l$mJU8f>^w zZgy7xMeO3eVD$Cd7u0FF02agk#&+q+Lty*IkkVGLciqnu8(W00b|{Zlaq0H0NT|46 zwy&F%2q-*>xdvg zG-XoCI%z;;M4ojiu$WCy>wf-WAKO}AckN~~28%Mcri}AeC#8;8R33Gn$DAqw2_;A- z>86{3H|a65gKzF(E*QPozt+N07rdR2na1S}j^~)WX?}`lwA+-4?Rcs`d;s2D|H}3` z{76HRf&%fPRlds0B;L00m5`n9T3{`iNqSp~?Jp%#_^k_)ntY=+)?`od=E6=mGLR%a zEYAI&*puPT;$;zE9(I0MJ;;^EI*mV)_#VJ{;M)TU$#Z!DAjfeN& zYJuG~`Zo=4~igu!G_IO%Iyd@rH^L8D#U;AwmNQ*qN2-5f#)h=sM_>jzI~CibQUm$`uC(_8R8`jb{o<$8R3E(br!~KSIN|`! zOIpYOM?N@MoK#6h3gH*MUngR`YVPHB=oH_d69T*1`mP|Q*~_`GWT8`;aB;BPI%U3D z%EI3bHsT}Cxx@-Y{aUE#(k@rj1tD+8%Ln%KD8GX`KM0ad16G zXc1Mu{S9L7$^}xpUor$8od3)A$)T}`!lFjF7@ni%0%A$huQ7gi$HN&H>-aizrk zQl`)n_eaDoj!4*Y0|OS=C-Zle{5ENQ2NOI?0dhFFhsb&XJC%44utz2aN_HCxe6sptD@IgdfT; zOxT_>UvvcMV@MfsjEqna#cKS1i8USiUu7j@IeJc1CsYvhvw>osh5d3xwMu#%P&@Ec zX&vL#pU?c`0UfJwc7bmfrY$2-E*~jwzgjTWDE2TuBf?>u zsF0$3ytktS84NFp!!QoJG9TRUP`mE0tSpY10*wfv7^h#fjJn~XQp>J8KAPMWfkGr$ zBkWlmhmsJp+PB^IAtI`2%m{?IUr2i8*@ z)^QJ%uLx{Q+ltKP9R7Yf#q~(sdx6aVwqpg3risY5BW#P9anc| z?)tF`oGVv5VZ99rawNYTN}K=-gbZaqE$1LNLB~k2lmXV!fwV1ldXyIY)LpSJsUOo> z+&~*&2J17nL7 zt>#Zal92wWW5WnKCq`69zn3d)ARCGdB?w)A%{7|&Rq*niwD!yn3?M;95>lVB<^JBu zE=gFlPKGtI_&GoQsQ%!&tAR*FFS8H?eRh*J-(uam(K&RTV`LtHb-`HYCaA7K5eiH( z)9(D2HPd0EQsfb6!l9PSZjN~#muXH-{ij6FKAXB1JqNj zvGOutV-`;urUcoF)xA>&vlpJyI10&TVD!+pp(KAa=f^KR zHXGDQ0$+C3G-&D!6)NzlXe4pqwrxprPkna}MUdmlFjD!N?AYJ0)qRd4x@Q>f080yE z5qFUjss z%U`NJ{$yLOY-DRqZyxsgZq22pML72596c4?7-%9JML3lCho74^Oq_)cKC3?0$y>+6 zCk(G?2@+qxX))FbkRbW5VR2|JlxM=_=4OSC%yrfO=gDvaiqUqT^w4mPS)K}Hv#-x$ zgJ<^QJLbt0tq~-m;1iY#EFkD1i5HEqDn2|mn6u^jyR{Ln8F8?Q7K3DFhk3YIA|n(2 z4RR1_o}G5;`X0)w5Hx!c<(bPUj{_K1sMRr!e>(vjEl{v~p2VrMpW6UqLiSaeN60V} z&6;+^xF%rOV3pGU0z>zB4Zd1vczCtz@T$aU1}8`@LQt~%F%^%Fq;7bCcKLV#1y@}V z<%asPH@;=A!K<-*EIJD;q{Dx?)s}Vr1Q4*Im^>-Bp^HAX3~%JZ0}*{?SS|^AEJ)DE zJlOcG#1=ebGPm=EE4*=yFwOvi{?aZb9d+JlmDdBgSRc+b#Pzf4urXHp>raegVlybB z%snHn35XW+ziAe?2RxcMkg{Fcldr6oKDWQU6xJmU1g@?)S^cfUO_${sWxEf%FHmJ3Lrz4( z2Vs!#fmZnhf?9>)uRt=uzad2sbNPS&yLv!Zz$<#`q4Z`kpdfTr&sewOiqnJt1NQrL A6aWAK diff --git a/map_race.png b/map_race.png deleted file mode 100644 index 3a412dde4f330e4f080c5e9236596e21dc2299b5..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 19334 zcmeFZRX|+J(kKiA1c$*bz~DiGySoGnkl+&BW$@r6!QI`02X_cTf(CaDPSC*M67(+i z*+;(ffA{r&y1-)fbXQe(byr)-guPag!$c!RgM)*^RFIcehl7JZe|k_rK#p|1{4(%D zw3JYmfP<@vLBBIb0zTK9Yb#hNE5k7XX%skQcw9IHAO#Qn!oicmJwX5|I1_lXf2D2U zU;GUNkcH=fdm@evJkIc(f2Hw(^oI~@I7Hz6gqsXJzz;mj)4zYdUt4>c+u0ktIyytR zSOwtVIN3P_1UWbaIXNN!NO1GR!GUr?|D*zco{RAB^bo}UwI~_{0ugpLvk+95mi-$Y_#{GYv6$;KfdAi&1X$;Qd~5`cK=^4`JK*z=`>3(Y@>{F9Efxr?c@wUev0 zqXXoLuCa-uo2v*l^;1Rv`uRsY%}oDQkCU6T-CwnsnX;MNncJH?xVo@$uyU~dZ+HN= z|3Y+d`MWoOeqr-8b^?&t|5qe)PwRiP^*^wlO8&2CuGSX+7qq96f1v%0ya)GkH`dGsp%Jv`G!fgL5v@pP*pt7TxwZ(gBV^?$0|3#35+5QXW-@W@6 zwv@B^lOVovbhH!wpK|#R%>OL^57eig5c~^XTiO~J0~cTrMS0lSh1veslK&M`=l_cN zA0_{dDdlMA=&a#nY-%pb@ponak@X+YPn|9(VqhB|A}R-)OuCNLUe8|d?l;x04f=gW zM}M(Fs&kC@S|OYrK9z)?T|~eweGitwwbfy!RIpg({{8lZcbQ7tj!4JV zl7Eh%nu3APTeQbOIFtZ5lpiwUNoD*mFrU%^ooXPk$20;M4jvpE3)f@NmHSy90tbS? zLFLGSJUDn%RPiM`45n~IAnytEDewP*{=Xjh|Dy+T5#RwKWd)1R5bJgli^@H9)PnYq!~))-k8*jeV!MGeWfVA#)z8ODoJ?Qhmk zsuDyGptZltgWcRWbz4t#TH_utlwLtbwCLPU-cI88DN0N-rY!1Ijbr7=h$-H)@3p2B ze<&9CRm8XW=cT)GM#4`kqU<_S`QINif`UR(U!$-2_#R7|u7ONrSI($M$rozqsux)A zX6@ehq@RV(ysF@6>}%0e3~!F%n^c{OxeIbHQ0Dpb*?)pj*qB$$p+VTX(as6YCrbim zi1JNW%qMQ*(JTMx?Nk$5+q?uRJnA^x3Bycuj^@w35XM~Z2mN^cCDm!!UB35?9Iny4 z_&PLgmGZ9{*1ESd<%VZNDg;g&$oD0wEKF>^7?oh_CsO!iO@xfPa+vlEmR-1Sw#|&* zmStrW?3Tr8EcM?=jm;pBaRn3h|5{F4#1gA?znees92l+nQeKVs2`+tKVz>87XP>jJ z{~%s`qY(G1#U_YUXPV@d!Do&`?EAKvyZgbV)-NtG&0!S3w_jksi7>Rq&^N=-4=)nz zXFnd*1%;(ZFIj)>%9!+ZRG}uco^zqJL`|!@e{6Wfs3_*{5J*`u{!nFl@?5txG zx8qE+UXNIkzYTje=R1Rye#mQ2t>)NcxTIy%%RGr(sYIz=ff<3e5M#t0b!s$p_BfTP z_!32IY%|RZg_~)%b?SB{Wg+DIq*pZ^-63Up(b3!OWt}Xc=Ve!W#1TRj2tIl@YeHPN zbO{$>9jTb^fnTPvnX{mF{7NjLKXOVbj~0hnZ74$zXni(S0{V0vt6Zm+kYU-LuHuRp z<(H{%GR>w(zY5mvoc*ys8aep=#@rCoGt)->kuBNPbj?p}D;o5k_-K2%-^!|D6 zK9_=~H>zSL6hVr`jUMXWlPf{Ng@uW1?0@sVwi$>u zT1U@(sL)0AYUWROQwQ$|6I8N`YI0l6a>d98)`pw-1fkuc?opLjw+z-ad|QS$Zz`Ke zFx?puo-5>)oQ6IsMus%!lwvQ(uijJ+Kgfd^yy%O6cQfqK+&iSZ_kowSzTbJ9-hkg0 z3{8a~o}t5f;sbyhi9>IHP9ZxC*o&;j8iRz&n7+xsd@SS;|M{t^CZinD&X6=yPX#KP zzI>8&r|DIU<$y^j{w}ifnhjb(r*Bz0zM<059b@j{!_i?*Wj_@87)>XRETM0QQO~^n ztk@Xr-A{>bImt&F@!;c5=GF_YS7{z)lGuUEbcknfO|=xqz57qYmTH0y6F z#3n|=s!eOWX;T-gef_e-ifGsBgEb!A?X#C3o9Q|ZMbdl+`T{0H6a)Oiri15UzdVE% zoLo-r45XpO0ycbGjlRM^6>JaCP~DH>Txk^V7Nt^VR#se>whTEey*71CKzO< z%Kt_s-*OKZUD_!sMP-wu>c#b7VLH)bC>f~MCBrG!{KJfNR;)4?%_MaA*t^7u@j+Y} zR8JeENnWHLj$&{O3KJqsnl5LhU>nt7_9rzV_^kQ6(9QaVcS)+T4`<-F zjqyBIYK&l1Dptu2&{Hq5OIU!W!)gG zd;U&G_Q;NAjg+pDmJ};DT*4&J60SN1oIK4Dn0YE=?t$@SkbM=R6}w%W=E^SqrHf!z zQeAV0Z~|4^95Z{tnRtMA#p#-+cZVJ(r>C7du+}j;{6IVNMMl>gp`)p5 z!N4s2!wN_(KNUBKgBQ_rPlysq>Fptl9-+LPwt_bE#i8N!+j1xLjiuL-_DWtHnQrN) zKL;#nU4%j)eO=w>J>*BD2m8YZ$tC_p8eET7dk=>G_uwCHD0+*S$gxXw!Sh+ri@&Uh+;~__$NO$kIT0pQ@#gcNgQM@|{b?U57ni3l zX!J6_L0S!~ndq{bn8CkfEXN5G`kO1J)qR;(Qn%8i z;tGpA_pHm2PooktTCI*=p0PCWg3R2-^VsS3Gwj$>(UMEnRdRwB6d^b+5qH1cE<3=i%NV~6PaiAA==u4(U;Ug!?(f(@ zFaph4c=$u=GODoQU5dB6+|sg^(Hd{DQE+3& zz>Zgxo*w*JT*hHS_bW$Cm?(jZ!bEIBK!-p$0~If% zEcc}_M9h|)JAkb&JoS8?^AoX$2KW0+`k%Zn8dH&4sC~unqG>oLzLyc>SZR$YrZv`R zhYMMJ=`EpGcD^{=LpTE0x&)^m90gWBqem}aUl~^sB|hVacI1R%|!Lw^PljB z=UUtj+{iC2boqmiB?F#yu5>vlh;c!SWi(Sux9R-4it@hSL}+90A+j8t?7v`oD<14A zY=~IWMLA}*%ufa_-gPDnn&faRhiF?>^B44r#~CAv2;^?0IsvYE473*L1+4@o;aw>Cw}PLxuiG43&Lm z5k5r=gF8F9f%qAgV2=+*LkN#qPB7)*XC7zVy2ZFD| zVuID%8FUUzz~|B4oz$J@p?l_3d$Bqq;~FBr_JJXOctiw+-TE}#d2#=2nfm~K&YxrJ z&*z4jMbx~23nEBG4BSQ0Po>DEl#IOt0RP-TEX1GXu^rS++n{V(5yxy5wzv20CY+$4 z575$$SG*a;;O!8OZRR4GOycG~VI@4=byly|l!`qyJ$(!I^3)^+?I8ZnJZFQYjzN^k zzv#qL$lB)GlIE;aw{c_!iKdajt!tW~yxuX3%_dpqEBQ2RNepdJb~oei`a3=d%Bi+{SIc7o~EX456%zhx&NdgYZoiuV7G{iu?S;7sg%%#!5#$XOZ2a zXqborbmp^S|E*1O*=~4->cacpQAVtL;0W{H=6w>=<5;X9Lute*QtNl4Pxlet$KUn+ zDdf(k1IonA8Xre(d}nQzHh$fbo3a=QMwzwf3L8)sZ2(J(f)5L7mDsy)E5x5mwS+Pc z94iD=Qf^9gV5IU@mXB9y6F9p#N;#tlm5ub5Vk zxW&wQR?~IGHJ-64Ufm}_oq`BmTfR?6HXCuM`=rMMv?Y=JdS?8s8^uf-MI9Qh9YTZl zjlV^z-jlR1^H*n*-b~(mBtuivg??s!i;4e(??Kv^R>6hUao&TlT|H zO23~j?DS$l*rdU)vH8HG;l@~4-M4!2p?u*ya^}27-w}Nj`AWV*lM|rZ;DhQK#Wb@M zKIBK1tBy1%8i%d18M!U$L)CitJdBrq|D<)hd+d$;z-;PWpWXc%=zFj^!kj{V)1xB>W}I$t$*`lyPwSnxB`e zW0K}MUr;&hRD+j0;P=GKlLX(gZ^y3h6%|)@5fyrKn!Cs))SyZUY@D&b~+faO$Z_teAV%if*=@#{%XP!;mj(E|vx)JlWclm1Ee*Cn?FWc-M zwXTnoPV>)r>>`2%8MAFQ9y;hz=TO&jh%1o@eVdE_ebd5)OPy~pBD95|H-KqNLaGT$ z#aWl5n(@;35zG4OLz-`=K^n^Iy8Heath877SGD7kv#m^d=dH3zeL!V5x-jnaS5i$l zL@Wgdv;3I##v1Z{{lk;>9*dMr(=6}wpM$1xLUhzOKA8?D^zf0ko4L;mndBEajgkXlq{;QU> zoqLCZy=6)-<^mCewqm(g)L_`q8>>)j?1^-tk)UKM3~xGdbgWPxMgU?>rEQkoAd;w>r9H<_2D*s~C(wHim2HePu6ZonMJ-351_S53YC zLZCp#ap2$^jUhECIAYB=2DTLe<%uc1 zPL(fZH4&?HUB9kdF+uZ<6!5R^9W2qH`adDyV-^=sy;@5p zElkx3w^w8kG9{07md;coVZFWh>Ulmt;o);%_Gky>bAhl83b2krjAPx|JuCgddvp4J zeZ8tenXmFFnU)xaCg*bTN!n@ROWjd&RT<0aKalX(O@iFO$4YC?oA2sO-m9>xN``tj z*F1N|NPkb_Kv9hag0N!-Dn>nASzIc+aPg+xW8z6JNntxBa-q;ZkupY=iWBDxk{QLaZN3rPJ3^h#XBjpDw-_xsC z1o2$#=pa-ory%H>X^Wu%+HXf$o*t3&T{(d>&hst^0yq>Q&?WM_>P}-*5iN<_VZ;3} zK)*llOP3s=&>ce@9gkVcT)sr}oj=2qWDx#Ubbn}r3MCS(bizUu&t(Pe>`NI9I<|Ml zTFy+$3U!RkCHE=_>sD8O{m=vGg`reV<~N-TwQa?FavK}fL}E?zNd5?4z#eR#anBIN z#jQZJyGJTV9jtZC#r%>&x*aY*?A39NejMGj6`u-b99++<%-(#D>qe zh{QHfR20~(YW{&4s6i6XN@HZp=ET+aTHp8BB;PUmTPwO=c$6%j0-w6V?1?dhNlt#sWs2Z2e+g_3S zQ@{MHwL8mMVrW^&1~`|2k6#4{_M{}_mh~oFtk3NKD7`rrZrHdmF1W=DQ6Gb*s z;4fUOH|1rMPYNr=R(ARnNh>=4VH;E{GGHP=D3AkbadTlQwp^`N`IWSUqIaBEf-S|M zVJAu#@CJonZY%>riSt}RF|edPLt1dh!SH>{}|IcOJc%Gdkudg0XFwmcwW{p`?*frtvW-iJCX zAr1lo+8@dh*f+sCxev9WMxOu;BWCoNy&pp|)`n=@GtrN5C`$Cledl>G=H5p6*vjpJ zN9$nE`ONMf_MR+MSz>s=nlX-L1YGAI|>a1LICgD>x8-^*m}ZXAyigQ1uK{D!b15W4rjpe zJl|8bFk*DZbPKL0i$Gx>K5!mSTnnQg{P5l8Yt`cSarjUnsZ@3(5C?rgC2{-rTSM~| zO@?t5wIKDo89pNTB!6JByuu7a?}$V6B6r3ilPAtRY^HP$9kS>SrJhn(8;q zzQR-9_U<0Tcga7NMO3ygUtLuXX-AP{B%ttzgFvTvfIiZnx8oiSAw%di@}h|e-TEz) z3pNBc9M=Uw>G=qkW$UHr{@YyENO&n}T9H)x-r>CjagatgJ6Z-HXdm!$d2iLsRizf* zKeI{6XlbZ*3UCV<){Yx@-XYQbg$dF}c`YV%(U&_iiuBN<)S;$}FAEeCTD7Q3M~y)s zVu9rwOR>v=e(tci_I%FA%U3VRIZb6gHoIVRJckRENA-c!-6EtJ#94$57lE%UW=>VN zf@HKZoI@6nLWQ^z*%3h-c>G3`cUBG|*M5VC>r{OA4W}DB2$ThUTy&wg!#++#h`*@D zOSCC}z;p`|w;0uh6;G2UKj*xSLI3Fy>OKF-m=aN%%IU4wYLc~*_0IF?pF`;F20Fxn zc;G7(2n{-TP4!)xt`8-zcsl(vyYk1k+nppg zIJ}MTdWiy&hZ{4(6%A`$+OsQfsfJyu$Ve#Q0u>Oa6NUD|t&CJ>sg>P|F}o^94$`YM_n8sY>mcH7963xc^=xIDw56t0q89U+#U=TMC>P}tQeY}h=QCq{P-FMVgNF5l zeG<}^JTKCjy6oTl)leNbo=ez)38D2cV7_U>{HAT9_*c{FJ~odHBYQ|dR1h_7dOa*I z`v*2tZ*L-(uN{++9@X|p-Gipo*Pn;KFJBi$Ii_`zx#_gAPc23c8)zNlY1c4xvUG#V zwn8lOs9+|2NQQyp%ZMcl4_F8zmtR}{c&>%aj2>xj16RL4p1iek5LjHEtn8EU*)&eI zAFgl5dHx<-jPiIxxTLcuNNp%KT(Bm-!~p+Y3mi%pxH+Si8AEFGtLG>r?Tt_9EiL34 z4OByWT;xjW^|L!dpXJ7wo`7;g6cwfYgZLfF@LOibK3>lCoAhAr`q9>h?RUyuAtiP33{3*M$;%6Y1J=tdh#1Xt8&%oEyMukoc#gM$?(tnVbJ39vtiKb;~yFzQ}&BP-mXCtgWnWBv7`9 z3bmBMbr*-&fXHol_E~5Kjygsz{i{arbuOB5v?6Gw1h@j5he)&%nNSha@%3s=umt+% zn9+|)hKr-!2Eqw_p%B;vms~WYGgw^IyoQ4|m|*|yqtAQB!%Ws-p_(hu2E{G-Gh7(m zIE&g)79rvCYr}_k4I(uYW`<5lEi%Nwxh6C~Ji=y@gdE2VUxLtF>%RXK_l1z&G}xn2 z*ccO$nz~w>kuAY|Gtg1%>z;OGXeS*#S*W*(q=BUb-Ks~v3Mq*GXd>J`oH5t*;;XbC z($d?Y+PMh(%X9G60tp>gP`Y!1&T(_1R~*Ywir+i;%9`|z4I`$W!a@Dt2IZo*}@_Hd$imR(1}ISo%2I%bcEa3h-@uPHN3B(7`Gqi^30_i56w;4905#Lh8&W}Tqzw-*IG7JYc$nl+CYJBT|(Au2^a6B~T>!UX5alx@Y~IlnTt?c(s;w0m4Yt$BAJ zeS{l+VzeA~6Z)KND(0WC-?vi6s^8TQ47YY$>l>ws9_J0K1XrGN_7;ZFwky8q>pmx1 zjN_`WUc}sNp*=(B5eaX0`Lk8`avr&^RX%iKu^{X)=f|Z5K}dxT>umFRY>yXeuV?B? zswnpZ2_-RZKr`1TfeJ>}jyvnN6E~U+XZg@EbH3V}SNA`5oY{irer}c}K2UB6M3bL#xwr$pYeyt_o}0VtL{()26E8D$-B+^Cv*T|p9~{^JRBiF) z7>)@e-&{Pn_)T4%vAyOR@iwfHZy9x{7a$+#VqRt6I4ySXroTC(3_d|zoW|4M-i=W5 z&A-j`JQ@{5)m4E$P_=H3>`7f|2~srb&FMw)pv&(#jEMexFW345ee=i1!nWV2-@ zrr1ki*?nQ5yHZ7Wv&gC+FGWSF5Yu4Kj84#B3TCjlmt}`32Bi#p_IMbvp^t2;@q3VJT&L zvM(Bavfg*utnYU6%AF=$?l#0|))Wu*bh=+SDc7M8TGk+i1KgzX##6J_0Y?Y2FB8CaYj$kKhwJoQIfliudPUq6v^usAzmG%fi9>DEv zbt#4|yVzVoG25VVPyT+jgkxgKrEXL2NOMw@&kN%zg_29e_o;Wsbhx1B3=+{PM~r1H za|16m6*skxjWt2s3RiA6kG~)4mC_UMIa;@%HWL{y$)1ZwOjx+h%Yi~$v8_ZYaU@WD zzDD=k;LO_*WpDAJIxpoD9R#nYzKq}kUqNXvd~?Yw_EwFGuO>U^|K$zCGiN_-nc`tn z{yK^kM2kp^Jh2s$DM^2fo;Vj0uErvm*j#l?`a^>hjOC4aj=PmC5X(g?Y4h5G17W~!i}`E-K1{Me zhq?}o^4xi@CC6P$aQZ9R&mV_5v#8CbErN2nWAH6?Bs*|!ss=a@(da|V8ro)Vsnd$RAUz>zJf&Y+WxYXoK?Ly_OrSZARrX zD5RGJRX=0lKB_CCs$g-Od^LEcf~4rVidwJ~+luBcJQ zvT}ZTy|sj^j>-V)hA5IkJ<;xthpqWKE0IXSg!5IV-Zw!rdcxllb@z?VS3S(v&*=i0 zBM|_x5>XbzH7~+D`_Qaf@O_8NZPX{fK2&w(hZ`@b=67tGcCJvFiwr-r&1c218=tg^ ztC$x1Vq2ec@(osL^VYW706~QyCnOam2)G|B4HZ_*yBH+IgidexeJ2$4TXar4Al}h# zhX>)6mw=*zzqpi_Tt#d1ClwdYd=`POdPLkznEdAlABA+)TXI063yO!1-nXvXZuOct z=yGO_IUB_Q+7nN!=y!{^{OEp^@~e&mjuFj|G_1{Ksu~!?-32eSa(Fl$4X{|kW*Wsp=LqbfsusqgHt@DXT_V8tcO7Mgb!+TlY*t}O;NILTfBx(9?gIrg<7ut>A*2oG+OaO{A$?LPq3IV?If$g`gEY*3@8i)J3~PoY)VsvSG?_^_uwR_9Kg=v#i53f0S}koL3AR&QgiD^uo>8YJ1Ez|~H+aD2&j?Ftn* z&!U?W`}r!jR%3!=^K$HD=kC!_nev-pD+4V91V$BTZg;7;)l11#xft?5Qly_ydSHsu zZ?QB<%&GC2U6U^m@bxgv4dHZowqfYO?kOUEc-29s6L#`e^Dd$Mz!by!Jhd;@nl_C< zef~t^UAt}zeQT>kjjyoK84dUs}UK#z2 zZyR-_fD?LT*CLnZCPT!~iS2oRdUISdo!M-@ob0+Y$AH>dC+eOgs){3umE2XKU*o_GUt!eiKiFaQji-#!osK9 z`-(IK2CMw?(}3nW>Vc_I4b8KQ{QjxnO5rRpo8u(C`in_Mb>@?Ac#AZPV(}_0?~i^A zxGv?6M-Of-q3-YMO(N?gDLnaOI{XH9hN|(9wqhbKx^{sGDC5QcM-`@R6fCxu@JD`m)B&} zhWp~SQXp7gv$M$;>n`v8+6{TW#MvXlFIB?=SWHy)M!v>-1^G#{j8UI#%gscqrX;WG zq$z&ER|pRK@hSZnJI%gzSwBTLt(eo$HUZ_s1wLohJa+0KO{L+O6wjpOlC5~yEnWKM zRqjmV)QH=Q86+DzSntX1BL#!Y(8M$Ev1jMi-qM)Vv z^E`iEF{p&yES-1HX1ckXYC=_{*_5pU^S;;Rq)UX0zMR-UBJoR;u}T%r{P zk}}dg0P3!ng>=&v{x~8BYHwTYRURbj>{Aj%R4OKmg4N73a(#5){MMRj3@8htf}PG0 z>vp|zoQg@U;C?N&{Qi8BK`yQI{(9u}Gh1S^s9A&7L^p-~>(Z3%827XM?dM)R1@2NQ z2UWonlgbTDv!tzYeKj9{p+m7H*;K|ZTdhey6bF*-kLDBlmf5Hwg=@gmU%xuzduaJ~ zjJ`!zkIXp&pXG#1Nkb=oyf<@fT;`UNkdNBDTw6~@9mKC}(Ubh;iknZw080?^n-Px| zH@H)j!9~lB^*(!yO@H~z{aee>Z;|_!LXwj6qnVrah{%UIGT*96>><6S zaYfldzjR&j#+$n zJu8T91gz68G=)^7+Phh|3do@sA63%78u9-8qwM}&g)wg%YjYhZS8R;U11>q*9WL7* zBP*Z$&=Q4B?hi}OXG~khxvMI;4OXKh6y47pNyQWyg}QsR_JF62es1zkX_7-jtt6Sf zh7u*oH2w*>ihD-Zkz1V<)-xGj@u+^9P4WQKdyHvPx)DJc$~f&n7H8zR1p+wVKCbYHugrG#f9+oNi zeJ@8`^m`I9ZmwT&O;dMRd((?;lZK^yf<1pBh8Lo3ETdtEnag2F=T~XrM@JG~XfHih zcaI@J3=)q1RHneYq9#REXF_0o1YZdN;dTR z7N>{8S99k9rPo^g3o zN%QM(f0c2Dbb_BbmS7M6rDJ@QfPiL1*cW$&x`Y?@!{eWC7RFIGSRIgpECD0h1IoDT zHD&4p-_S*hwY=(tU{P=TrBZNn2tH0K5YL=&ZV^Kp54eMWHq>{)_>7a$Z*hBe3RFppd6;&Rm`4_Z&dxF2et908tqP{#!pd8!!JO?)@nM@Fw& zWtUoL1y-Ojd8~m1rd7Nekax9w6z$$m|*WR{`9=}?Jk0u?GV$^Sr*D!#N zZ~+xtbr0J+bL={%o7`_?f4RkB<+m6;=eb!MMg@$O;*eQD&Hg*7wtoFiS)*L9R8F1K zuE!;9!Xs{i^7mKt9l&yL0dZ>dyB7AfdU9`)!Nb2-HZ9d#x^IJo_QiQwtR$_dJ~F49 z9wQ@)2cQP>ks)tsf3II)Jr@+9RN5!>@Ml926-K{}yE4CgkZR;B<~O!Pyal=u2hUU0 z>*%)5n|hSH;J{rVt-HObX8I-U`~&2>U{MXb59`(B+*!t0Y@sP@!B?a@2$&7zs6+{b zR%j)d+wY6^?;NX-`6EeVu~62(scNeE_uz~49`0YKy1wtSHK7`VRD1?WCe8e$ zu0N0vrD@pZY98oYcZv=g3DSjKvi#p1dUn~I1nO2@k$6eu&VAzg?W?qQuGlciP`~oK zPKbg_@6n2ms9I1p8#IH z`mSud5<^j319>xB#))(N<I?2^H@bhJ*nPrj}+`k?g!)rd$w30gfR<>8{Vw|CDM@bgT2wUJ(%wB!`F zxp2URBuH1c0z09{#fk0<3t(}|IQY=9bQjusG>7QVdg>dV%+~QrYVpx#v!4BQe5|H~7SADi>7=r4D#j*WAOzuN z0(RTRJa{V4SpJr#6^HXImt_qqMg}kC+!Ul$oehbn83z}rgF3#pde-P)ctHa@PZ$3f zRp%9q2uKC+*N$3VLu$R4ebj#+DrDYB2pk0jC;+n!96UDStWJ~!z1c1P6_5L7@-^ka zFL<{2r!Y}CN)Y_danata#CxbwMA5alWaSevFf*v2W;z&aqgEAK%tHCG85ut2$rnZZ zofl}@3u|6z!)PgoThD55`@DS}>uKEx2v7xm*Bg82_5G;f*<*LH$4`OqS4e4UFkC}M z>4or!a#hNGgwWP;wLrhEs66shiy>=Ke-$3QJghJM5Z20r-2G+hb1&u-tH&|{3atXM z%~O}LVx{ia&J@`Hh;jukb}zR>UOvmB4(!RgyRmK%`2j=bc@?bqL@)LoXxhZ0w!-T* zv{<zF;q64LIh*lzx9^G?&;NG@_FGkF4Hv;)>VR$;oudM*C7ElhHIh!;I zvDlZbbLsQJy;-OEBB&0GFMSvLvblKHH>ptq#5Yi0`@4B^nbA*TXe}`o7ng6dB`1Fp zbYQO$zQ%YO?=IZtiphJnB$(CGl)6Y+_o#J(fs@9djI94W$#>DG4nR>{^Q${^DJ%@h zbmgXyrGmYQZ_qStHECBD!&NSgMgI#%W-Z0y_L24SwIUy96*aC0SD}Lw>DPuqCVTGM zS}K~SK>!DK`ez*SC40@v2;C6JHRG*H%;h3v5L$0->8c?v9m79K0X5|K-9<@lRhZ^N zsXh?yWA5HL0vI2G;3v*qZ$vjV9iTzS^nWWm;+Tb!gw&zMcb7sWi#m23=UEe~olqmAa-H|0 zNZ5#hRq7z`Xp^H88E9}C%6PdPD@{oPu``~>sj1Q`ku|YV7CycYaQX>lhSx2XK3ky4 zzR<=aR0X6xdcRd;SCcuFQin^p8JG=V8c;npY-cd8@=ZECmL-&7%<5tI%w4M3!o+t9 z1KRZxs38t@{8A@}`QiD9fIiE8pc_U&o1H7TaZ*{+4RUv>-3H`u3Sw6o3_krO_=GYP71z+zaY;9qOc{l~V z*ibnrPdrVWshxFTdNGSS|G^@7@sl0^Xae@^w7Ye+?meoo;8+sqLl5ln9j-EDxU#MD zg~%vhPtbF($Dk6nKS_Y}J3EtM-BvY74t+CqW^;mLVd9Nw!VBOVLY^@_eHTFUjO6Ws z8Nn?r_d0_#Iwc_5h*aYJ4&&PgY7Xmqj^AbQuWC47od=^pTUDuqdS3sRlQe=4vcKe2htwt zldhwlEtLhV?SqPf5|icA;aUaQ-@51hYelI;6`c?OFHZ;1@54U86aVV1P@~`<;v{a1 z4N+M_T*`dXXIc`Fc0?dd8OG5Ht8$-Ybgp5RVd?daJWcHZtfD~NVKli1KQOHzoPg(u z#+8?nxi5xYWUp^ITz^?U5JJ-?;CpQ6EC1^&Xcdrk6Nxp{vaODPIFJx0v|sF%^cgPo zqw#80=mSBJHG9-tol&y#Qoo|lXWmnr`{RW^;D8F|jiBuR zfDLr4DI)LBpw)El{;x?7htS$n<2cQwekqx!Yq8H4ULUmkDY@;ktrNX#K*nNUfa*0Z zqGp;$Z3-}rvm3~o`kb&LuRI6XV4E!UiLL5n26+Uf8tqaQg!n(@TWki&RXeW`UV6}2 zPBJ#BHMAz?%V9S%(i^433IQkG0NpJhaQwo(+p9Qk6lu|lpvOpm5V3W%=M_ujJmVHp zYv4oJk$r>>tLC7HE;LS;@c1b98gAV<9+jWbhLAD8(tTXP@yW$S*#1aH?6H7DA-1{y zMZHvX4X-&$Ym!x@eAipBJPUjvR?sUujMq+2i_tq3xQ)^0j2M6)Yyg};@Qn};9RFtx z?4ay(U=DEQqh%0i0SE)*Kj31AKgkUIz{qlFumrAsEM8&&%-al1e}KnH{5;Me2h1Fx z#xrQ3GEo3je*FN>oqq0Tv;kE*jL(4D!7CO)g_b%);d6QB3ZNAXOlN@FXDYWkf?V>z zALtT$whurn7+7WiwHv#0HG^F;ALx>Lo(G`A12_zT+SB_)*g#pP!5-+6{}Knl$6bti hWHd&GMAG^%zjub?`_DW68Guff@^tlcS?83{1OV0P=5qi5