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) v_0_x, v_0_y, _ = carlautil.actor_to_velocity_ndarray( self.__ego_vehicle, flip_y=True) # 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, v_0_x, v_0_y])) params.initial_state = initial_state A, B = self.__params.A, self.__params.B nx, nu = self.__params.nx, self.__params.nu T = self.__control_horizon # make state computation account for initial position and velocity initial_rollout = np.concatenate([ np.linalg.matrix_power(A, t) @ initial_state.world for t in range(T + 1) ]) params.initial_rollout = util.AttrDict(world=initial_rollout) return params
def get_current_velocity(self): """ Get current velocity of vehicle in """ v_0_x, v_0_y, _ = carlautil.actor_to_velocity_ndarray( self.__ego_vehicle, flip_y=True) return np.sqrt(v_0_x**2 + v_0_y**2)
def make_local_params(self, frame, ovehicles): """Get Local LCSS parameters that are environment dependent.""" params = util.AttrDict() """General local params.""" params.frame = frame # 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 p_0_x, p_0_y, _ = carlautil.to_location_ndarray(self.__ego_vehicle, flip_y=True) v_0_x, v_0_y, _ = carlautil.actor_to_velocity_ndarray( self.__ego_vehicle, flip_y=True) # x0 : np.array # Initial state x0 = np.array([p_0_x, p_0_y, v_0_x, v_0_y]) params.x0 = x0 A, T = self.__params.A, self.__params.T # States_free_init has shape (nx*(T+1)) params.States_free_init = np.concatenate( [np.linalg.matrix_power(A, t) @ x0 for t in range(T + 1)]) """Local params for (random) multiple coinciding controls.""" # N_traj - number of planned trajectories possible to compute params.N_traj = np.prod(params.K) max_K = np.max(params.K) # TODO: figure out a way to specify number of subtrajectories params.N_select = min(int(1.5 * max_K), params.N_traj) # subtraj_indices - the indices of the sub-trajectory to optimize for params.subtraj_indices = np.random.choice(np.arange(params.N_traj), size=params.N_select, replace=False) return params
def make_local_params(self, ovehicles): """Get Local LCSS parameters that are environment dependent.""" params = util.AttrDict() params.O = len(ovehicles) # number of obstacles params.K = np.zeros(params.O, dtype=int) for idx, vehicle in enumerate(ovehicles): params.K[idx] = vehicle.n_states p_0_x, p_0_y, _ = carlautil.actor_to_location_ndarray( self.__ego_vehicle) p_0_y = -p_0_y # need to flip about x-axis v_0_x, v_0_y, _ = carlautil.actor_to_velocity_ndarray( self.__ego_vehicle) v_0_y = -v_0_y # need to flip about x-axis # x0 : np.array # Initial state x0 = np.array([p_0_x, p_0_y, v_0_x, v_0_y]) A, T = self.__params.A, self.__params.T # States_free_init has shape (nx*(T+1)) params.States_free_init = np.concatenate( [np.linalg.matrix_power(A, t) @ x0 for t in range(T + 1)]) return params
def make_local_params(self, ovehicles): """Get Local LCSS parameters that are environment dependent.""" params = util.AttrDict() # 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 p_0_x, p_0_y, _ = carlautil.actor_to_location_ndarray( self.__ego_vehicle, flip_y=True) v_0_x, v_0_y, _ = carlautil.actor_to_velocity_ndarray( self.__ego_vehicle, flip_y=True) # x0 : np.array # Initial state x0 = np.array([p_0_x, p_0_y, v_0_x, v_0_y]) params.x0 = x0 A, T = self.__params.A, self.__params.T # States_free_init has shape (nx*(T+1)) params.States_free_init = np.concatenate( [np.linalg.matrix_power(A, t) @ x0 for t in range(T + 1)]) return params
def make_highlevel_params(self, ovehicles): p_0_x, p_0_y, _ = carlautil.actor_to_location_ndarray( self.__ego_vehicle) p_0_y = -p_0_y # need to flip about x-axis v_0_x, v_0_y, _ = carlautil.actor_to_velocity_ndarray( self.__ego_vehicle) v_0_y = -v_0_y # need to flip about x-axis """Get LCSS parameters""" # TODO: refactor this long chain params = util.AttrDict() params.Ts = 0.5 # A, sys.A both have shape (4, 4) A = np.diag([1, 1], k=2) # B, sys.B both have shape (4, 2) B = np.concatenate(( np.diag([0, 0]), np.diag([1, 1]), )) # C has shape (2, 4) C = np.concatenate(( np.diag([1, 1]), np.diag([0, 0]), ), axis=1) # D has shape (2, 2) D = np.diag([0, 0]) sys = control.matlab.c2d(control.matlab.ss(A, B, C, D), params.Ts) params.A = np.array(sys.A) params.B = np.array(sys.B) # number of state variables x, number of input variables u # nx = 4, nu = 2 params.nx, params.nu = sys.B.shape # TODO: remove car, truck magic numbers # only truck.d is used params.car = util.AttrDict() params.car.d = np.array([4.5, 2.5]) # truck should be renamed to car, and only truck.d is used params.truck = util.AttrDict() params.truck.d = [4.5, 2.5] # params.x0 : np.array # Initial state params.x0 = np.array([p_0_x, p_0_y, v_0_x, v_0_y]) params.diag = np.sqrt(params.car.d[1]**2 + params.car.d[0]**2) / 2. # TODO: remove constraint of v, u magic numbers # Vehicle dynamics restrictions params.vmin = np.array([0 / 3.6, -20 / 3.6]) # lower bounds params.vmax = np.array([80 / 3.6, 20 / 3.6]) # upper bounds params.umin = np.array([-10, -5]) params.umax = np.array([3, 5]) # Prediction parameters params.T = self.__prediction_horizon params.O = len(ovehicles) # number of obstacles params.L = 4 # number of faces of obstacle sets params.K = np.zeros(params.O, dtype=int) for idx, vehicle in enumerate(ovehicles): params.K[idx] = vehicle.n_states """ 3rd and 4th states have COUPLED CONSTRAINTS %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % x4 - c1*x3 <= - c3 % x4 - c1*x3 >= - c2 % x4 + c1*x3 <= c2 % x4 + c1*x3 >= c3 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% """ vmin, vmax = params.vmin, params.vmax # they work only assuming vmax(2) = -vmin(2) params.c1 = vmax[1] / (0.5 * (vmax[0] - vmin[0])) params.c2 = params.c1 * vmax[0] params.c3 = params.c1 * vmin[0] """ Inputs have COUPLED CONSTRAINTS %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % u2 - t1*u1 <= - t3 % u2 - t1*u1 >= - t2 % u2 + t1*u1 <= t2 % u2 + t1*u1 >= t3 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% """ umin, umax = params.umin, params.umax params.t1 = umax[1] / (0.5 * (umax[0] - umin[0])) params.t2 = params.t1 * umax[0] params.t3 = params.t1 * umin[0] A, B, T, nx, nu = params.A, params.B, params.T, params.nx, params.nu # 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.Abar = Abar params.Bbar = Bbar params.Gamma = Gamma A, T, x0 = params.A, params.T, params.x0 # States_free_init has shape (nx*(T+1)) params.States_free_init = np.concatenate( [np.linalg.matrix_power(A, t) @ x0 for t in range(T + 1)]) return params
def get_speed(vehicle): """Get velocity of vehicle in m/s.""" v_0_x, v_0_y, _ = carlautil.actor_to_velocity_ndarray(vehicle, flip_y=True) return np.sqrt(v_0_x**2 + v_0_y**2)