def compute_obstacle_constraints( self, params, ovehicles, X, Delta, Omicron, segments ): constraints = [] M_big = self.__params.M_big T = self.__control_horizon L, K = self.__params.L, params.K diag = self.__params.diag if self.road_boundary_constraints: S_big = M_big * np.sum(Omicron[~segments.mask], axis=0) S_big = np.repeat(S_big[..., None], L, axis=1) else: S_big = np.zeros(T, L, dtype=float) vertices = self.__compute_vertices(params, ovehicles) A_union, b_union = self.__compute_overapproximations( params, ovehicles, vertices ) 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(T): 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]) + S_big[t] ) rhs = b_obs + diag constraints.extend([l >= r for (l, r) in zip(lhs, rhs)]) constraints.extend([np.sum(Delta[indices, t]) >= 1]) return constraints, vertices, A_union, b_union
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)
def compute_boundary_constraints(self, p_x, p_y): """ Parameters ========== p_x : np.array of docplex.mp.vartype.VarType Has p_x = <p_x_1, p_x_2, ..., p_x_T> where timesteps T is the extent of motion plan to enforce constraints in the x axis. p_y : np.array of docplex.mp.vartype.VarType Has p_y = <p_y_1, p_y_2, ..., p_y_T> where timesteps T is the extent of motion plan to enforce constraints in the y axis. """ b_length, f_length, r_width, l_width = self.__road_seg_params mtx = util.rotation_2d(self.__road_seg_starting[2]) shift = self.__road_seg_starting[:2] if self.plot_boundary: self.__plot_boundary() pos = np.stack([p_x, p_y], axis=1) pos = util.obj_matmul((pos - shift), mtx.T) constraints = [] constraints.extend([-b_length <= z for z in pos[:, 0]]) constraints.extend([z <= f_length for z in pos[:, 0]]) constraints.extend([-r_width <= z for z in pos[:, 1]]) constraints.extend([z <= l_width for z in pos[:, 1]]) return constraints
def compute_road_boundary_constraints(self, params, X, Omicron, segments): constraints = [] T = self.__control_horizon M_big = self.__params.M_big # diag = self.__params.diag for t in range(T): for seg_idx, (A, b) in enumerate(segments.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""" constraints.extend([l <= r for (l, r) in zip(lhs, rhs)]) constraints.extend([np.sum(Omicron[:, t]) >= 1]) return constraints
def compute_boundary_constraints(self, p_x, p_y): """ Parameters ========== p_x : np.array of docplex.mp.vartype.VarType p_y : np.array of docplex.mp.vartype.VarType """ b_length, f_length, r_width, l_width = self.__road_seg_params mtx = util.rotation_2d(self.__road_seg_starting[2]) shift = self.__road_seg_starting[:2] if self.plot_boundary: self.__plot_boundary() pos = np.stack([p_x, p_y], axis=1) pos = util.obj_matmul((pos - shift), mtx.T) constraints = [] constraints.extend([-b_length <= z for z in pos[:, 0]]) constraints.extend([z <= f_length for z in pos[:, 0]]) constraints.extend([-r_width <= z for z in pos[:, 1]]) constraints.extend([z <= l_width for z in pos[:, 1]]) return constraints
def compute_obstacle_constraints(self, params, ovehicles, X, Delta, Omicron, segments): """Compute obstacle constraints. Parameters ========== X : np.array of docplex.mp.vartype.ContinuousVarType State space plan of shape (N_select, T, nx) Delta : np.array of docplex.mp.vartype.BinaryVarType OV obstacle slack variables of shape (N_select, T, O, L) Omicron : np.array of docplex.mp.vartype.BinaryVarType Road boundary slack variables of shape (N_select, I, T) """ constraints = [] N_select, M_big = params.N_select, self.__params.M_big T = self.__control_horizon L = self.__params.L diag = self.__params.diag if self.road_boundary_constraints: S_big = M_big * np.sum(Omicron[:, ~segments.mask], axis=1) S_big = np.repeat(S_big[..., None], L, axis=2) else: S_big = np.zeros(N_select, T, L, dtype=float) vertices = self.__compute_vertices(params, ovehicles) A_unions, b_unions = self.__compute_overapproximations( params, ovehicles, vertices) for n in range(N_select): # select outerapprox. by index n A_union, b_union = A_unions[n], b_unions[n] for t in range(T): As, bs = A_union[t], b_union[t] for o, (A, b) in enumerate(zip(As, bs)): lhs = util.obj_matmul(A, X[ n, t, :2]) + M_big * (1 - Delta[n, t, o]) + S_big[n, t] rhs = b + diag constraints.extend([l >= r for (l, r) in zip(lhs, rhs)]) constraints.extend([np.sum(Delta[n, t, o]) >= 1]) return constraints, vertices, A_unions, b_unions
def do_highlevel_control(self, params): """Decide parameters. TODO finish description """ segs_polytopes, goal, seg_psi_bounds = self.compute_segs_polytopes_and_goal( params) """Apply motion planning problem""" n_segs = len(segs_polytopes) Gamma, nu, nx = params.Gamma, params.nu, params.nx T = self.__control_horizon max_a, max_delta = self.__params.max_a, self.__params.max_delta model = docplex.mp.model.Model(name="proposed_problem") min_u = np.vstack((np.zeros(T), np.full(T, -0.5 * max_delta))).T.ravel() max_u = np.vstack( (np.full(T, max_a), np.full(T, 0.5 * max_delta))).T.ravel() u = np.array(model.continuous_var_list(nu * T, lb=min_u, ub=max_u, name='u'), dtype=object) # State variables X = (params.initial_rollout.local + util.obj_matmul(Gamma, u)).reshape( T + 1, nx) X = params.transform(X) X = X[1:] # Control variables U = u.reshape(T, nu) """Apply motion dynamics constraints""" model.add_constraints( self.compute_dyamics_constraints(params, X[:, 3], X[:, 4])) """Apply road boundary constraints""" if self.road_boundary_constraints: # Slack variables from road obstacles Omicron = np.array(model.binary_var_list(n_segs * T, name="omicron"), dtype=object).reshape(n_segs, T) M_big, diag = self.__params.M, self.__params.diag psi_0 = params.initial_state.world[2] for t in range(self.__control_horizon): 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""" # 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) """Start from current vehicle position and minimize the objective""" w_ch_accel = 0. w_ch_turning = 0. w_accel = 0. w_turning = 0. # final destination objective cost = (X[-1, 0] - goal[0])**2 + (X[-1, 1] - goal[1])**2 # change in acceleration objective for u1, u2 in util.pairwise(U[:, 0]): _u = (u1 - u2) cost += w_ch_accel * _u * _u # change in turning rate objective for u1, u2 in util.pairwise(U[:, 1]): _u = (u1 - u2) cost += w_ch_turning * _u * _u cost += w_accel * np.sum(U[:, 0]**2) # turning rate objective cost += w_turning * np.sum(U[:, 1]**2) model.minimize(cost) # TODO: warmstarting # if 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.__control_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 delta_0 as hotfix to MIP warmstart as it needs # # at least 1 integer value set. # warm_start.add_var_value('delta_0', 0) # model.add_mip_start(warm_start) # 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.solve() # model.print_solution() f = lambda x: x if isinstance(x, numbers.Number) else x.solution_value cost = cost.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)
def do_highlevel_control(self, params, ovehicles): """Decide parameters. Since we're doing multiple coinciding, we want to compute... TODO finish description """ vertices = self.__compute_vertices(params, ovehicles) A_unions, b_unions = self.__compute_overapproximations( vertices, params, ovehicles) """Apply motion planning problem""" N_traj, L, T, K, O, Gamma, nu, nx = params.N_traj, self.__params.L, self.__params.T, params.K, \ params.O, self.__params.Gamma, self.__params.nu, self.__params.nx model = docplex.mp.model.Model(name='obstacle_avoidance') # set up controls variables for each trajectory u = np.array( model.continuous_var_list(N_traj * T * nu, lb=-np.inf, ub=np.inf, name='control')) Delta = np.array( model.binary_var_list(O * L * N_traj * T, name='delta')).reshape(N_traj, T, O, L) # U has shape (N_traj, T*nu) U = u.reshape(N_traj, -1) # Gamma has shape (nx*(T + 1), nu*T) so X has shape (N_traj, nx*(T + 1)) X = util.obj_matmul(U, Gamma.T) # X, U have shapes (N_traj, T, nx) and (N_traj, T, nu) resp. X = (X + params.States_free_init).reshape(N_traj, -1, nx)[..., 1:, :] U = U.reshape(N_traj, -1, nu) for _U, _X in zip(U, X): # _X, _U have shapes (T, nx) and (T, nu) resp. p_x, p_y = _X[..., 0], _X[..., 1] v_x, v_y = _X[..., 2], _X[..., 3] u_x, u_y = _U[..., 0], _U[..., 1] model.add_constraints(self.compute_boundary_constraints(p_x, p_y)) model.add_constraints(self.compute_velocity_constraints(v_x, v_y)) model.add_constraints( self.compute_acceleration_constraints(u_x, u_y)) # set up obstacle constraints M_big, N_traj, T, diag = self.__params.M_big, params.N_traj, self.__params.T, self.__params.diag for n in range(N_traj): # for each obstacle/trajectory A_union, b_union = A_unions[n], b_unions[n] for t in range(T): # for each timestep As, bs = A_union[t], b_union[t] for o, (A, b) in enumerate(zip(As, bs)): lhs = util.obj_matmul( A, X[n, t, :2]) + M_big * (1 - Delta[n, t, o]) rhs = b + diag model.add_constraints([l >= r for (l, r) in zip(lhs, rhs)]) model.add_constraint(np.sum(Delta[n, t, o]) >= 1) # set up coinciding constraints for t in range(0, self.__n_coincide): for x1, x2 in util.pairwise(X[:, t]): model.add_constraints([l == r for (l, r) in zip(x1, x2)]) # start from current vehicle position and minimize the objective p_x, p_y, _ = carlautil.to_location_ndarray(self.__ego_vehicle, flip_y=True) if self.__goal.is_relative: goal_x, goal_y = p_x + self.__goal.x, p_y + self.__goal.y else: goal_x, goal_y = self.__goal.x, self.__goal.y start = np.array([p_x, p_y]) goal = np.array([goal_x, goal_y]) cost = np.sum((X[:, -1, :2] - goal)**2) model.minimize(cost) # model.print_information() if self.log_cplex: model.parameters.mip.display = 5 s = model.solve(log_output=True) else: model.solve() # model.print_solution() f = lambda x: x if isinstance(x, numbers.Number) else x.solution_value U_star = util.obj_vectorize(f, U) cost = cost.solution_value X_star = util.obj_vectorize(f, X) return util.AttrDict(cost=cost, U_star=U_star, X_star=X_star, A_unions=A_unions, b_unions=b_unions, vertices=vertices, start=start, goal=goal)
def do_single_control(self, params, ovehicles): """Decide parameters. TODO: acceleration limit is hardcoded to 8 m/s^2 TODO finish description """ vertices = self.__compute_vertices(params, ovehicles) A_union, b_union = self.__compute_overapproximations( vertices, params, ovehicles) segs_polytopes, goal = self.compute_segs_polytopes_and_goal(params) """Apply motion planning problem""" n_segs = len(segs_polytopes) L, T, K, Gamma, nu, nx = self.__params.L, self.__params.T, params.K, \ self.__params.Gamma, self.__params.nu, self.__params.nx u_max = self.__params.u_max model = docplex.mp.model.Model(name="proposed_problem") u = np.array(model.continuous_var_list(nu * T, lb=-u_max, ub=u_max, name='u'), dtype=object) Delta = np.array(model.binary_var_list(L * np.sum(K) * T, name='delta'), dtype=object).reshape(np.sum(K), T, L) Omicron = np.array(model.binary_var_list(n_segs * T, name="omicron"), dtype=object).reshape(n_segs, T) X = (params.States_free_init + util.obj_matmul(Gamma, u)).reshape( T + 1, nx) X = X[1:] U = u.reshape(T, nu) """Apply motion dynamics constraints""" model.add_constraints( self.compute_velocity_constraints(X[:, 2], X[:, 3])) model.add_constraints( self.compute_acceleration_constraints(U[:, 0], U[:, 1])) """Apply road boundaries constraints""" M_big, T, diag = self.__params.M_big, self.__params.T, self.__params.diag 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 model.add_constraints([l <= r for (l, r) in zip(lhs, rhs)]) model.add_constraint(np.sum(Omicron[:, t]) >= 1) """Apply collision constraints""" T, K, diag, M_big = self.__params.T, params.K, \ self.__params.diag, self.__params.M_big 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(T): 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) """Start from current vehicle position and minimize the objective""" start = params.x0[:2] cost = (X[-1, 0] - goal[0])**2 + (X[-1, 1] - goal[1])**2 model.minimize(cost) # TODO: warmstarting # if 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.__control_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 delta_0 as hotfix to MIP warmstart as it needs # # at least 1 integer value set. # warm_start.add_var_value('delta_0', 0) # model.add_mip_start(warm_start) # 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.solve() # model.print_solution() f = lambda x: x if isinstance(x, numbers.Number) else x.solution_value cost = cost.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, A_union=A_union, b_union=b_union, vertices=vertices, start=start, goal=goal)
def do_multiple_control(self, params, ovehicles): """Decide parameters. Since we're doing multiple coinciding, we want to compute... TODO finish description """ vertices = self.__compute_vertices(params, ovehicles) A_unions, b_unions = self.__compute_overapproximations( vertices, params, ovehicles) segs_polytopes, goal = self.compute_segs_polytopes_and_goal(params) """Optimize all trajectories if MCC, else partial trajectories.""" # params.N_select params.N_traj params.subtraj_indices N_select = params.N_select if self.__agent_type == "rmcc" else params.N_traj """Common to MCC+RMCC: setup CPLEX variables""" L, T, K, O, Gamma, nu, nx = self.__params.L, self.__params.T, \ params.K, params.O, self.__params.Gamma, self.__params.nu, self.__params.nx n_segs = len(segs_polytopes) u_max = self.__params.u_max model = docplex.mp.model.Model(name='obstacle_avoidance') # set up controls variables for each trajectory u = np.array( model.continuous_var_list(N_select * T * nu, lb=-u_max, ub=u_max, name='control')) Delta = np.array( model.binary_var_list(O * L * N_select * T, name='delta')).reshape(N_select, T, O, L) Omicron = np.array(model.binary_var_list(N_select * n_segs * T, name="omicron"), dtype=object).reshape(N_select, n_segs, T) """Common to MCC+RMCC: compute state from input variables""" # U has shape (N_select, T*nu) U = u.reshape(N_select, -1) # Gamma has shape (nx*(T + 1), nu*T) so X has shape (N_select, nx*(T + 1)) X = util.obj_matmul(U, Gamma.T) # X, U have shapes (N_select, T, nx) and (N_select, T, nu) resp. X = (X + params.States_free_init).reshape(N_select, -1, nx)[..., 1:, :] U = U.reshape(N_select, -1, nu) """Common to MCC+RMCC: apply dynamics constraints to trajectories""" for _U, _X in zip(U, X): # _X, _U have shapes (T, nx) and (T, nu) resp. v_x, v_y = _X[..., 2], _X[..., 3] u_x, u_y = _U[..., 0], _U[..., 1] model.add_constraints(self.compute_velocity_constraints(v_x, v_y)) model.add_constraints( self.compute_acceleration_constraints(u_x, u_y)) """Common to MCC+RMCC: apply road boundaries constraints to trajectories""" M_big, T, diag = self.__params.M_big, self.__params.T, self.__params.diag for n in range(N_select): # for each trajectory for t in range(T): for seg_idx, (A, b) in enumerate(segs_polytopes): lhs = util.obj_matmul(A, X[n, t, :2]) - np.array( M_big * (1 - Omicron[n, seg_idx, t])) rhs = b + diag model.add_constraints([l <= r for (l, r) in zip(lhs, rhs)]) model.add_constraint(np.sum(Omicron[n, :, t]) >= 1) """Apply collision constraints""" traj_indices = params.subtraj_indices if self.__agent_type == "rmcc" else range( N_select) M_big, T, diag = self.__params.M_big, self.__params.T, self.__params.diag for n, i in enumerate(traj_indices): # select outerapprox. by index i # if MCC then iterates through every combination of obstacles A_union, b_union = A_unions[i], b_unions[i] for t in range(T): # for each timestep As, bs = A_union[t], b_union[t] for o, (A, b) in enumerate(zip(As, bs)): lhs = util.obj_matmul( A, X[n, t, :2]) + M_big * (1 - Delta[n, t, o]) rhs = b + diag model.add_constraints([l >= r for (l, r) in zip(lhs, rhs)]) model.add_constraint(np.sum(Delta[n, t, o]) >= 1) """Common to MCC+RMCC: set up coinciding constraints""" for t in range(0, self.__n_coincide): for x1, x2 in util.pairwise(X[:, t]): model.add_constraints([l == r for (l, r) in zip(x1, x2)]) """Common to MCC+RMCC: set objective and run solver""" start = params.x0[:2] cost = np.sum((X[:, -1, :2] - goal)**2) model.minimize(cost) # model.print_information() if self.log_cplex: model.parameters.mip.display = 5 s = model.solve(log_output=True) else: model.solve() # model.print_solution() f = lambda x: x if isinstance(x, numbers.Number) else x.solution_value U_star = util.obj_vectorize(f, U) cost = cost.solution_value X_star = util.obj_vectorize(f, X) return util.AttrDict(cost=cost, U_star=U_star, X_star=X_star, A_unions=A_unions, b_unions=b_unions, vertices=vertices, start=start, goal=goal)
def do_highlevel_control(self, params, ovehicles): """Decide parameters""" # TODO: assign eps^k_o and beta^k_o to the vehicles. # Skipping that for now. vertices = self.__compute_vertices(params, ovehicles) A_union, b_union = self.__compute_overapproximations( vertices, params, ovehicles) """Apply motion planning problem""" L, T, K, Gamma, nu, nx = self.__params.L, self.__params.T, params.K, \ self.__params.Gamma, self.__params.nu, self.__params.nx u_max = self.__params.u_max model = docplex.mp.model.Model(name="proposed_problem") u = np.array(model.continuous_var_list(nu * T, lb=-u_max, ub=u_max, name='u'), dtype=object) Delta = np.array(model.binary_var_list(L * np.sum(K) * T, name='delta'), dtype=object).reshape(np.sum(K), T, L) X = (params.States_free_init + util.obj_matmul(Gamma, u)).reshape( T + 1, nx) X = X[1:] U = u.reshape(T, nu) """Apply motion constraints""" model.add_constraints( self.compute_boundary_constraints(X[:, 0], X[:, 1])) model.add_constraints( self.compute_velocity_constraints(X[:, 2], X[:, 3])) model.add_constraints( self.compute_acceleration_constraints(U[:, 0], U[:, 1])) """Apply collision constraints""" T, K, diag, M_big = self.__params.T, params.K, \ self.__params.diag, self.__params.M_big 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(T): 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) """Start from current vehicle position and minimize the objective""" p_x, p_y, _ = carlautil.actor_to_location_ndarray(self.__ego_vehicle, flip_y=True) if self.__goal.is_relative: goal_x, goal_y = p_x + self.__goal.x, p_y + self.__goal.y else: goal_x, goal_y = self.__goal.x, self.__goal.y start = np.array([p_x, p_y]) goal = np.array([goal_x, goal_y]) cost = (X[-1, 0] - goal_x)**2 + (X[-1, 1] - goal_y)**2 model.minimize(cost) if 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.__control_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 delta_0 as hotfix to MIP warmstart as it needs # at least 1 integer value set. warm_start.add_var_value('delta_0', 0) model.add_mip_start(warm_start) # 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.solve() # model.print_solution() f = lambda x: x if isinstance(x, numbers.Number) else x.solution_value cost = cost.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, A_union=A_union, b_union=b_union, vertices=vertices, start=start, goal=goal)
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""" N_select = params.N_select 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 min_u = np.repeat(min_u[None], N_select, axis=0).ravel() max_u = np.vstack((np.full(T, max_a), np.full(T, max_delta))).T max_u = np.repeat(max_u[None], N_select, axis=0).ravel() # Slack variables for control u = model.continuous_var_list(N_select * nu * T, lb=min_u, ub=max_u, name="u") # U has shape (N_select, T*nu) U = np.array(u, dtype=object).reshape(N_select, -1) U_delta = U - u_bar x = util.obj_matmul(U_delta, Gamma.T) + x_bar # State variables x, y, psi, v X = x.reshape(N_select, T, nx) # Control variables a, delta U = U.reshape(N_select, T, nu) """Apply state constraints""" for _X in X: 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_select * n_segs * T, name="omicron") Omicron = np.array(Omicron, dtype=object).reshape(N_select, 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 n in range(N_select): for t in range(T): for seg_idx, (A, b) in enumerate(segs_polytopes): lhs = util.obj_matmul(A, X[n, t, :2]) - np.array( M_big * (1 - Omicron[n, 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[n, t, 2] - psi_0 - M_big * ( 1 - Omicron[seg_idx, t]) model.add_constraint( lhs <= seg_psi_bounds[n, seg_idx][1]) lhs = X[n, t, 2] - psi_0 + M_big * ( 1 - Omicron[n, seg_idx, t]) model.add_constraint( lhs >= seg_psi_bounds[seg_idx][0]) model.add_constraint(np.sum(Omicron[n, :, t]) >= 1) """Apply vehicle collision constraints""" if params.O > 0: vertices = self.__compute_vertices(params, ovehicles) A_unions, b_unions = self.__compute_overapproximations( vertices, params, ovehicles) O, L = params.O, self.__params.L # Slack variables for vehicle obstacles Delta = model.binary_var_list(O * L * N_select * T, name='delta') Delta = np.array(Delta, dtype=object).reshape(N_select, T, O, L) M_big = self.__params.M_big diag = self.__params.diag # for n, i in enumerate(params.subtraj_indices): # # select outerapprox. by index i # A_union, b_union = A_unions[i], b_unions[i] # for t in range(T): # As, bs = A_union[t], b_union[t] # for o, (A, b) in enumerate(zip(As, bs)): # lhs = util.obj_matmul(A, X[n,t,:2]) + M_big*(1 - Delta[n,t,o]) # rhs = b + diag # model.add_constraints([l >= r for (l,r) in zip(lhs, rhs)]) # model.add_constraint(np.sum(Delta[n,t,o]) >= 1) for n in range(N_select): # select outerapprox. by index n A_union, b_union = A_unions[n], b_unions[n] for t in range(T): As, bs = A_union[t], b_union[t] for o, (A, b) in enumerate(zip(As, bs)): lhs = util.obj_matmul( A, X[n, t, :2]) + M_big * (1 - Delta[n, t, o]) rhs = b + diag model.add_constraints( [l >= r for (l, r) in zip(lhs, rhs)]) model.add_constraint(np.sum(Delta[n, t, o]) >= 1) else: vertices = A_unions = b_unions = None """Set up coinciding constraints""" for t in range(0, self.__n_coincide): for u1, u2 in util.pairwise(U[:, t]): model.add_constraints([l == r for (l, r) in zip(u1, u2)]) """Compute and minimize objective""" cost = 0 for _U, _X in zip(U, X): cost += self.compute_objective(_X, _U, goal) model.minimize(cost) # TODO: add warmstarting to MCC # 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""" 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_unions=A_unions, b_unions=b_unions, vertices=vertices)
def do_highlevel_control(self, params, ovehicles): """Decide parameters.""" """Compute road segments and goal.""" segments, goal = 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: T = self.__control_horizon I = len(segments.polytopes) # Slack variables from road obstacles Omicron = model.binary_var_list(I * T, name="omicron") Omicron = np.array(Omicron, dtype=object).reshape(I, T) model.add_constraints( self.compute_road_boundary_constraints(params, X, Omicron, segments) ) else: Omicron = None if params.O > 0: T = self.__control_horizon 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) ( constraints, vertices, A_union, b_union, ) = self.compute_obstacle_constraints( params, ovehicles, X, Delta, Omicron, segments ) model.add_constraints(constraints) 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, segments=segments, ), 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, segments=segments, ), InSimulationException("Optimizer failed to find a solution")
def do_highlevel_control(self, params): """Decide parameters.""" segs_polytopes, goal, seg_psi_bounds = self.compute_segs_polytopes_and_goal( params) """Apply motion planning problem""" n_segs = len(segs_polytopes) Gamma, nu, nx = params.Gamma, params.nu, params.nx T = self.__control_horizon max_a, min_a = self.__params.max_a, self.__params.min_a max_delta = self.__params.max_delta 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 = np.array(model.continuous_var_list(nu * T, lb=min_u, ub=max_u, name='u'), dtype=object) # State variables x, y, psi, v X = (params.initial_rollout.local + util.obj_matmul(Gamma, u)).reshape( T + 1, nx) X = params.transform(X) X = X[1:] # Control variables a, delta U = u.reshape(T, nu) """Apply motion dynamics constraints""" model.add_constraints(self.compute_dyamics_constraints( params, X[:, 3])) """Apply road boundary constraints""" if self.road_boundary_constraints: # Slack variables from road obstacles Omicron = np.array(model.binary_var_list(n_segs * T, name="omicron"), dtype=object).reshape(n_segs, T) M_big, diag = self.__params.M_big, self.__params.diag psi_0 = params.initial_state.world[2] for t in range(self.__control_horizon): 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) """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() 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)
def do_highlevel_control(self, params, ovehicles): """Decide parameters.""" """Compute road segments and goal.""" segments, goal = self.compute_segs_polytopes_and_goal(params) """Apply motion planning problem""" N_select = params.N_select 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 min_u = np.repeat(min_u[None], N_select, axis=0).ravel() max_u = np.vstack((np.full(T, max_a), np.full(T, max_delta))).T max_u = np.repeat(max_u[None], N_select, axis=0).ravel() # Slack variables for control u = model.continuous_var_list(N_select * nu * T, lb=min_u, ub=max_u, name="u") # U has shape (N_select, T*nu) U = np.array(u, dtype=object).reshape(N_select, -1) U_delta = U - u_bar x = util.obj_matmul(U_delta, Gamma.T) + x_bar # State variables x, y, psi, v X = x.reshape(N_select, T, nx) # Control variables a, delta U = U.reshape(N_select, T, nu) """Apply state constraints""" model.add_constraints(self.compute_state_constraints(params, X)) """Apply road boundary constraints""" if self.road_boundary_constraints: N_select = params.N_select T = self.__control_horizon I = len(segments.polytopes) # Slack variables from road obstacles Omicron = model.binary_var_list(N_select * I * T, name="omicron") Omicron = np.array(Omicron, dtype=object).reshape(N_select, I, T) model.add_constraints( self.compute_road_boundary_constraints(params, X, Omicron, segments)) else: Omicron = None """Apply vehicle collision constraints""" if params.O > 0: N_select = params.N_select T = self.__control_horizon O, L = params.O, self.__params.L # Slack variables for vehicle obstacles Delta = model.binary_var_list(O * L * N_select * T, name='delta') Delta = np.array(Delta, dtype=object).reshape(N_select, T, O, L) (constraints, vertices, A_unions, b_unions) = self.compute_obstacle_constraints( params, ovehicles, X, Delta, Omicron, segments) model.add_constraints(constraints) else: Delta = None vertices = A_unions = b_unions = None """Set up coinciding constraints""" for t in range(0, self.__n_coincide): for u1, u2 in util.pairwise(U[:, t]): model.add_constraints([l == r for (l, r) in zip(u1, u2)]) # TODO: constraints on binary variables actually makes optimization slower. Why? # if Omicron is not None: # for o1, o2 in util.pairwise(Omicron[:,:,t]): # model.add_constraints([l == r for (l, r) in zip(o1, o2)]) # if Delta is not None: # for d1, d2 in util.pairwise(Delta[:,t]): # d1 = d1.reshape(-1) # d2 = d2.reshape(-1) # model.add_constraints([l == r for (l, r) in zip(d1, d2)]) """Compute and minimize objective""" cost = self.compute_mean_objective(X, U, goal) model.minimize(cost) # TODO: add warmstarting to MCC # 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_unions=A_unions, b_unions=b_unions, vertices=vertices, segments=segments), 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_unions=A_unions, b_unions=b_unions, vertices=vertices, segments=segments), InSimulationException( "Optimizer failed to find a solution")
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")