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)
