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")