def add_variables_by_blocks(indexing_set, lb, ub, var_type, model, relaxation=False, docplex_var_name=None):
if docplex_var_name is None:
indexing_set = len(indexing_set)
if relaxation or var_type == "C":
var_list = model.continuous_var_list(indexing_set, lb, ub, name=docplex_var_name)
elif var_type == "B":
var_list = model.binary_var_list(indexing_set, name=docplex_var_name)
elif var_type == "I":
var_list = model.integer_var_list(indexing_set, lb, ub, name=docplex_var_name)
return np.array(var_list)
def do_highlevel_control(self, params, ovehicles):
"""Decide parameters."""
"""Compute road segments and goal."""
segs_polytopes, goal, seg_psi_bounds = self.compute_segs_polytopes_and_goal(
params
)
"""Apply motion planning problem"""
x_bar, u_bar, Gamma = params.x_bar, params.u_bar, params.Gamma
nx, nu = params.nx, params.nu
T = self.__control_horizon
max_a, min_a = self.__params.max_a, self.__params.min_a
max_delta = self.__params.max_delta
"""Model, control and state variables"""
model = docplex.mp.model.Model(name="proposed_problem")
min_u = np.vstack((np.full(T, min_a), np.full(T, -max_delta))).T.ravel()
max_u = np.vstack((np.full(T, max_a), np.full(T, max_delta))).T.ravel()
# Slack variables for control
u = model.continuous_var_list(nu * T, lb=min_u, ub=max_u, name="u")
u = np.array(u, dtype=object)
u_delta = u - u_bar
x = util.obj_matmul(Gamma, u_delta) + x_bar
# State variables x, y, psi, v
X = x.reshape(T, nx)
# Control variables a, delta
U = u.reshape(T, nu)
"""Apply state constraints"""
model.add_constraints(self.compute_state_constraints(params, X))
"""Apply road boundary constraints"""
if self.road_boundary_constraints:
n_segs = len(segs_polytopes)
# Slack variables from road obstacles
Omicron = model.binary_var_list(n_segs*T, name="omicron")
Omicron = np.array(Omicron, dtype=object).reshape(n_segs, T)
M_big = self.__params.M_big
# diag = self.__params.diag
psi_0 = params.initial_state.world[2]
T = self.__control_horizon
for t in range(T):
for seg_idx, (A, b) in enumerate(segs_polytopes):
lhs = util.obj_matmul(A, X[t, :2]) - np.array(
M_big * (1 - Omicron[seg_idx, t])
)
rhs = b # - diag
"""Constraints on road boundaries"""
model.add_constraints([l <= r for (l, r) in zip(lhs, rhs)])
"""Constraints on angle boundaries"""
if self.angle_boundary_constraints:
lhs = X[t, 2] - psi_0 - M_big * (1 - Omicron[seg_idx, t])
model.add_constraint(lhs <= seg_psi_bounds[seg_idx][1])
lhs = X[t, 2] - psi_0 + M_big * (1 - Omicron[seg_idx, t])
model.add_constraint(lhs >= seg_psi_bounds[seg_idx][0])
model.add_constraint(np.sum(Omicron[:, t]) >= 1)
"""Apply vehicle collision constraints"""
if params.O > 0:
vertices = self.__compute_vertices(params, ovehicles)
A_union, b_union = self.__compute_overapproximations(vertices, params, ovehicles)
L, K = self.__params.L, params.K
# Slack variables for vehicle obstacles
Delta = model.binary_var_list(L*np.sum(K)*T, name='delta')
Delta = np.array(Delta, dtype=object).reshape(np.sum(K), T, L)
M_big = self.__params.M_big
diag = self.__params.diag
for ov_idx, ovehicle in enumerate(ovehicles):
n_states = ovehicle.n_states
sum_clu = np.sum(K[:ov_idx])
for latent_idx in range(n_states):
for t in range(self.__control_horizon):
A_obs = A_union[t][latent_idx][ov_idx]
b_obs = b_union[t][latent_idx][ov_idx]
indices = sum_clu + latent_idx
lhs = util.obj_matmul(A_obs, X[t, :2]) + M_big*(1 - Delta[indices, t])
rhs = b_obs + diag
model.add_constraints([l >= r for (l,r) in zip(lhs, rhs)])
model.add_constraint(np.sum(Delta[indices, t]) >= 1)
else:
vertices = A_union = b_union = None
"""Compute and minimize objective"""
cost = self.compute_objective(X, U, goal)
model.minimize(cost)
if self.road_boundary_constraints and self.__U_warmstarting is not None:
# Warm start inputs if past iteration was run.
warm_start = model.new_solution()
for i, u in enumerate(self.__U_warmstarting[self.__step_horizon :]):
warm_start.add_var_value(f"u_{2*i}", u[0])
warm_start.add_var_value(f"u_{2*i + 1}", u[1])
# add omicron_0 as hotfix to MIP warmstart as it needs
# at least 1 integer value set.
warm_start.add_var_value("omicron_0", 1)
model.add_mip_start(
warm_start, write_level=docplex.mp.constants.WriteLevel.AllVars
)
# model.print_information()
# model.parameters.read.datacheck = 1
if self.log_cplex:
model.parameters.mip.display = 2
s = model.solve(log_output=True)
else:
model.parameters.mip.display = 0
model.solve()
# model.print_solution()
"""Extract solution"""
try:
cost = cost.solution_value
f = lambda x: x if isinstance(x, numbers.Number) else x.solution_value
U_star = util.obj_vectorize(f, U)
X_star = util.obj_vectorize(f, X)
return util.AttrDict(
cost=cost, U_star=U_star, X_star=X_star, goal=goal,
A_union=A_union, b_union=b_union, vertices=vertices
), None
except docplex.mp.utils.DOcplexException as e:
# TODO: check model for infeasible solution
# docplex.util.environment.logger:environment.py:627 Notify end solve,
# status=JobSolveStatus.INFEASIBLE_SOLUTION, solve_time=None
return util.AttrDict(
cost=None, U_star=None, X_star=None,
goal=goal, A_union=A_union, b_union=b_union, vertices=vertices
), InSimulationException("Optimizer failed to find a solution")
