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
