def compute_velocity_constraints(self, v_x, v_y): """Velocity states have coupled constraints. Generate docplex constraints for velocity for double integrators. Street speed limit is 30 km/h == 8.33.. m/s Parameters ========== v_x : np.array of docplex.mp.vartype.VarType Has v_x = <v_x_1, v_x_2, ..., v_x_T> where timesteps T is the extent of motion plan to enforce constraints in the x axis. v_y : np.array of docplex.mp.vartype.VarType Has v_y = <v_y_1, v_y_2, ..., v_y_T> where timesteps T is the extent of motion plan to enforce constraints in the y axis. """ v_lim = self.__ego_vehicle.get_speed_limit() # is m/s _, theta, _ = carlautil.actor_to_rotation_ndarray(self.__ego_vehicle, flip_y=True) r = v_lim / 2 v_1 = r v_2 = 0.75 * v_lim c1 = v_2*((v_x - r*np.cos(theta))*np.cos(theta) \ + (v_y - r*np.sin(theta))*np.sin(theta)) c2 = v_1*((v_y - r*np.sin(theta))*np.cos(theta) \ - (v_x - r*np.cos(theta))*np.sin(theta)) c3 = np.abs(v_1 * v_2) constraints = [] constraints.extend([z <= c3 for z in c1 + c2]) constraints.extend([z <= c3 for z in -c1 + c2]) constraints.extend([z <= c3 for z in c1 - c2]) constraints.extend([z <= c3 for z in -c1 - c2]) return constraints
def compute_acceleration_constraints(self, u_x, u_y): """Accelaration control inputs have coupled constraints. Generate docplex constraints for velocity for double integrators. Present performance cars are capable of going from 0 to 60 mph in under 5 seconds. Reference: https://en.wikipedia.org/wiki/0_to_60_mph Parameters ========== u_x : np.array of docplex.mp.vartype.VarType u_y : np.array of docplex.mp.vartype.VarType """ _, theta, _ = carlautil.actor_to_rotation_ndarray(self.__ego_vehicle, flip_y=True) r = -2.5 a_1 = 7.5 a_2 = 5.0 c1 = a_2*((u_x - r*np.cos(theta))*np.cos(theta) \ + (u_y - r*np.sin(theta))*np.sin(theta)) c2 = a_1*((u_y - r*np.sin(theta))*np.cos(theta) \ - (u_x - r*np.cos(theta))*np.sin(theta)) c3 = np.abs(a_1 * a_2) constraints = [] constraints.extend([z <= c3 for z in c1 + c2]) constraints.extend([z <= c3 for z in -c1 + c2]) constraints.extend([z <= c3 for z in c1 - c2]) constraints.extend([z <= c3 for z in -c1 - c2]) return constraints
def compute_velocity_constraints(self, v_x, v_y): """Velocity states have coupled constraints. Generate docplex constraints for velocity for double integrators. Street speed limit is 30 km/h == 8.33.. m/s Parameters ========== v_x : np.array of docplex.mp.vartype.VarType v_y : np.array of docplex.mp.vartype.VarType """ v_lim = self.__ego_vehicle.get_speed_limit() # is m/s _, theta, _ = carlautil.actor_to_rotation_ndarray(self.__ego_vehicle, flip_y=True) r = v_lim / 2 v_1 = r v_2 = 0.75 * v_lim # TODO: this isn't a good setting c1 = v_2*((v_x - r*np.cos(theta))*np.cos(theta) \ + (v_y - r*np.sin(theta))*np.sin(theta)) c2 = v_1*((v_y - r*np.sin(theta))*np.cos(theta) \ - (v_x - r*np.cos(theta))*np.sin(theta)) c3 = np.abs(v_1 * v_2) constraints = [] constraints.extend([z <= c3 for z in c1 + c2]) constraints.extend([z <= c3 for z in -c1 + c2]) constraints.extend([z <= c3 for z in c1 - c2]) constraints.extend([z <= c3 for z in -c1 - c2]) return constraints
def make_local_params(self, frame, ovehicles): """Get the local optimization parameters used for current MPC step.""" """Get parameters to construct control and state variables.""" params = util.AttrDict() params.frame = frame p_0_x, p_0_y, _ = carlautil.to_location_ndarray(self.__ego_vehicle, flip_y=True) _, psi_0, _ = carlautil.actor_to_rotation_ndarray( self.__ego_vehicle, flip_y=True ) v_0_mag = self.get_current_velocity() x_init = np.array([p_0_x, p_0_y, psi_0, v_0_mag]) initial_state = util.AttrDict(world=x_init, local=np.array([0, 0, 0, v_0_mag])) params.initial_state = initial_state # try: # u_init = self.__U_warmstarting[self.__step_horizon] # except TypeError: # u_init = np.array([0., 0.]) # Using previous control doesn't work u_init = np.array([0.0, 0.0]) x_bar, u_bar, Gamma, nx, nu = self.__vehicle_model.get_optimization_ltv( x_init, u_init ) params.x_bar, params.u_bar, params.Gamma = x_bar, u_bar, Gamma params.nx, params.nu = nx, nu """Get controls for other vehicles.""" # O - number of obstacles params.O = len(ovehicles) # K - for each o=1,...,O K[o] is the number of outer approximations for vehicle o params.K = np.zeros(params.O, dtype=int) for idx, vehicle in enumerate(ovehicles): params.K[idx] = vehicle.n_states return params
def make_local_params(self, frame, ovehicles): """Get the local optimization parameters used for current MPC step.""" """Get parameters to construct control and state variables.""" params = util.AttrDict() params.frame = frame p_0_x, p_0_y, _ = carlautil.to_location_ndarray(self.__ego_vehicle, flip_y=True) _, psi_0, _ = carlautil.actor_to_rotation_ndarray(self.__ego_vehicle, flip_y=True) v_0_mag = self.get_current_velocity() x_init = np.array([p_0_x, p_0_y, psi_0, v_0_mag]) initial_state = util.AttrDict(world=x_init, local=np.array([0, 0, 0, v_0_mag])) params.initial_state = initial_state """Get controls for other vehicles.""" # O - number of total vehicles EV and OVs params.O = len(ovehicles) # K - for each o=1,...,O K[o] is the number of outer approximations for vehicle o params.K = np.zeros(params.O, dtype=int) for idx, vehicle in enumerate(ovehicles): params.K[idx] = vehicle.n_states return params
def make_local_params(self, frame): """Get the local optimization parameters used for current MPC step.""" params = util.AttrDict() params.frame = frame p_0_x, p_0_y, _ = carlautil.to_location_ndarray(self.__ego_vehicle, flip_y=True) _, psi_0, _ = carlautil.actor_to_rotation_ndarray(self.__ego_vehicle, flip_y=True) v_0_mag = self.get_current_velocity() x_init = np.array([p_0_x, p_0_y, psi_0, v_0_mag]) initial_state = util.AttrDict(world=x_init, local=np.array([0, 0, 0, v_0_mag])) params.initial_state = initial_state # try: # u_init = self.__U_warmstarting[self.__step_horizon] # except TypeError: # u_init = np.array([0., 0.]) # Using previous control doesn't work u_init = np.array([0.0, 0.0]) x_bar, u_bar, Gamma, nx, nu = self.__vehicle_model.get_optimization_ltv( x_init, u_init) params.x_bar, params.u_bar, params.Gamma = x_bar, u_bar, Gamma params.nx, params.nu = nx, nu return params
def make_local_params(self, frame): """Get the linearized bicycle model using the vehicle's immediate orientation.""" params = util.AttrDict() params.frame = frame """Dynamics parameters""" p_0_x, p_0_y, _ = carlautil.to_location_ndarray(self.__ego_vehicle, flip_y=True) _, psi_0, _ = carlautil.actor_to_rotation_ndarray(self.__ego_vehicle, flip_y=True) v_0_mag = self.get_current_velocity() delta_0 = self.get_current_steering() # initial_state - state at current frame in world/local coordinates # Local coordinates has initial position and heading at 0 initial_state = util.AttrDict( world=np.array([p_0_x, p_0_y, psi_0, v_0_mag, delta_0]), local=np.array([0, 0, 0, v_0_mag, delta_0])) params.initial_state = initial_state # transform - transform points from local coordinates back to world coordinates. M = np.array([ [math.cos(psi_0), -math.sin(psi_0), p_0_x], [math.sin(psi_0), math.cos(psi_0), p_0_y], ]) def transform(X): points = np.pad(X[:, :2], [(0, 0), (0, 1)], mode="constant", constant_values=1) points = util.obj_matmul(points, M.T) psis = X[:, 2] + psi_0 return np.concatenate((points, psis[..., None], X[:, 3:]), axis=1) params.transform = transform # longitudinal and lateral dimensions of car are normally 3.70 m, 1.79 m resp. bbox_lon = self.__params.bbox_lon # TODO: use center of gravity from API instead A = get_state_matrix(0, 0, 0, v_0_mag, delta_0, l_r=0.5 * bbox_lon, L=bbox_lon) B = get_input_matrix() C = get_output_matrix() D = get_feedforward_matrix() sys = control.matlab.c2d(control.matlab.ss(A, B, C, D), self.__timestep) A = np.array(sys.A) B = np.array(sys.B) # nx, nu - size of state variable and control input respectively. nx, nu = sys.B.shape params.nx, params.nu = nx, nu T = self.__control_horizon # Closed form solution to states given inputs # C1 has shape (nx, T*nx) C1 = np.zeros(( nx, T * nx, )) # C2 has shape (nx*(T - 1), nx*(T-1)) as A has shape (nx, nx) C2 = np.kron(np.eye(T - 1), A) # C3 has shape (nx*(T - 1), nx) C3 = np.zeros(( (T - 1) * nx, nx, )) # C, Abar have shape (nx*T, nx*T) C = np.concatenate(( C1, np.concatenate(( C2, C3, ), axis=1), ), axis=0) Abar = np.eye(T * nx) - C # Bbar has shape (nx*T, nu*T) as B has shape (nx, nu) Bbar = np.kron(np.eye(T), B) # Gamma has shape (nx*(T + 1), nu*T) as Abar\Bbar has shape (nx*T, nu*T) Gamma = np.concatenate(( np.zeros(( nx, T * nu, )), np.linalg.solve(Abar, Bbar), )) params.Gamma = Gamma # make state computation account for initial position and velocity initial_local_rollout = np.concatenate([ np.linalg.matrix_power(A, t) @ initial_state.local for t in range(T + 1) ]) params.initial_rollout = util.AttrDict(local=initial_local_rollout) return params
def make_local_params(self, frame, ovehicles): """Get the local optimization parameters used for current MPC step.""" """Get parameters to construct control and state variables.""" params = util.AttrDict() params.frame = frame p_0_x, p_0_y, _ = carlautil.to_location_ndarray(self.__ego_vehicle, flip_y=True) _, psi_0, _ = carlautil.actor_to_rotation_ndarray(self.__ego_vehicle, flip_y=True) v_0_mag = self.get_current_velocity() x_init = np.array([p_0_x, p_0_y, psi_0, v_0_mag]) initial_state = util.AttrDict(world=x_init, local=np.array([0, 0, 0, v_0_mag])) params.initial_state = initial_state # try: # u_init = self.__U_warmstarting[self.__step_horizon] # except TypeError: # u_init = np.array([0., 0.]) # Using previous control doesn't work u_init = np.array([0.0, 0.0]) x_bar, u_bar, Gamma, nx, nu = self.__vehicle_model.get_optimization_ltv( x_init, u_init) params.x_bar, params.u_bar, params.Gamma = x_bar, u_bar, Gamma params.nx, params.nu = nx, nu """Get parameters for other vehicles.""" # O - number of obstacles params.O = len(ovehicles) # K - for each o=1,...,O K[o] is the number of outer approximations for vehicle o params.K = np.zeros(params.O, dtype=int) for idx, vehicle in enumerate(ovehicles): params.K[idx] = vehicle.n_states """Get parameters for (random) multiple coinciding control.""" # N_traj : int # Number of planned trajectories possible to compute params.N_traj = np.prod(params.K) if self.__random_mcc: """How does random multiple coinciding control work? each item in the product set S_1 X S_2 X S_3 X S_4 represents the particular choices each vehicles make get the subset of the product set S_1 X S_2 X S_3 X S_4 such that for each i = 1..4, for each j in S_i there is a tuple in the subset with j in the i-th place. """ vehicle_n_states = [ovehicle.n_states for ovehicle in ovehicles] n_states_max = max(vehicle_n_states) vehicle_state_ids = [ util.range_to_list(n_states) for n_states in vehicle_n_states ] def preprocess_state_ids(state_ids): state_ids = state_ids + random.choices( state_ids, k=n_states_max - len(state_ids)) random.shuffle(state_ids) return state_ids vehicle_state_ids = util.map_to_list(preprocess_state_ids, vehicle_state_ids) # sublist_joint_decisions : list of (list of int) # Subset of S_1 X S_2 X S_3 X S_4 of joint decisions params.sublist_joint_decisions = np.array( vehicle_state_ids).T.tolist() # N_select : int # Number of planned trajectories to compute params.N_select = len(params.sublist_joint_decisions) else: # sublist_joint_decisions : list of (list of int) # Entire set of S_1 X S_2 X S_3 X S_4 of joint decision outcomes params.sublist_joint_decisions = util.product_list_of_list([ util.range_to_list(ovehicle.n_states) for ovehicle in ovehicles ]) # N_select : int # Number of planned trajectories to compute, equal to N_traj. params.N_select = len(params.sublist_joint_decisions) return params