def at_bounding_box_to_label(self, actor): actor_location = carlautil.actor_to_location_ndarray(actor) dist = np.linalg.norm(self.CENTER_COORDS - actor_location) if dist < self.REGION_RADIUS: return BoundingRegionLabel.BOUNDED else: return BoundingRegionLabel.NONE
def __compute_prediction_controls(self, frame): pred_result = self.do_prediction(frame) ovehicles = self.make_ovehicles(pred_result) params = self.make_highlevel_params(ovehicles) ctrl_result = self.do_highlevel_control(params, ovehicles) """Get trajectory""" trajectory = [] x, y, _ = carlautil.actor_to_location_ndarray(self.__ego_vehicle) X = np.concatenate((np.array([x, y])[None], ctrl_result.X_star)) n_steps = X.shape[0] headings = [] for t in range(1, n_steps): x, y = X[t] y = -y yaw = np.arctan2(X[t, 1] - X[t - 1, 1], X[t, 0] - X[t - 1, 0]) headings.append(yaw) yaw = util.reflect_radians_about_x_axis(yaw) transform = carla.Transform(carla.Location(x=x, y=y), carla.Rotation(yaw=yaw)) trajectory.append(transform) """Plot scenario""" ctrl_result.headings = headings lon, lat, _ = carlautil.actor_to_bbox_ndarray(self.__ego_vehicle) ego_bbox = [lon, lat] plot_lcss_prediction(pred_result, ovehicles, params, ctrl_result, self.__prediction_horizon, ego_bbox) return trajectory
def __plot_overapproximations(self, params, ovehicles, vertices, A_union, b_union): fig, axes = plt.subplots(1, 2, figsize=(15, 7)) ax = axes[1] X = vertices[-1][0][0][:, 0:2].T ax.scatter(X[0], X[1], color='r', s=2) X = vertices[-1][0][0][:, 2:4].T ax.scatter(X[0], X[1], color='b', s=2) X = vertices[-1][0][0][:, 4:6].T ax.scatter(X[0], X[1], color='g', s=2) X = vertices[-1][0][0][:, 6:8].T ax.scatter(X[0], X[1], color='m', s=2) A = A_union[-1][0][0] b = b_union[-1][0][0] try: util.npu.plot_h_polyhedron(ax, A, b, fc='none', ec='k') except scipy.spatial.qhull.QhullError as e: print(f"Failed to plot polyhedron of OV") x, y, _ = carlautil.actor_to_location_ndarray(self.__ego_vehicle, flip_y=True) ax = axes[0] ax.scatter(x, y, marker='*', c='k', s=100) ovehicle_colors = get_ovehicle_color_set( [ov.n_states for ov in ovehicles]) for ov_idx, ovehicle in enumerate(ovehicles): for latent_idx in range(ovehicle.n_states): logging.info( f"Plotting OV {ov_idx} latent value {latent_idx}.") color = ovehicle_colors[ov_idx][latent_idx] for t in range(self.__prediction_horizon): X = vertices[t][latent_idx][ov_idx][:, 0:2].T ax.scatter(X[0], X[1], color=color, s=2) X = vertices[t][latent_idx][ov_idx][:, 2:4].T ax.scatter(X[0], X[1], color=color, s=2) X = vertices[t][latent_idx][ov_idx][:, 4:6].T ax.scatter(X[0], X[1], color=color, s=2) X = vertices[t][latent_idx][ov_idx][:, 6:8].T ax.scatter(X[0], X[1], color=color, s=2) A = A_union[t][latent_idx][ov_idx] b = b_union[t][latent_idx][ov_idx] try: util.npu.plot_h_polyhedron(ax, A, b, fc='none', ec=color) #, alpha=0.3) except scipy.spatial.qhull.QhullError as e: print( f"Failed to plot polyhedron of OV {ov_idx} latent value {latent_idx} timestep t={t}" ) for ax in axes: ax.set_aspect('equal') filename = f"agent{self.__ego_vehicle.id}_frame{params.frame}_overapprox" fig.savefig(os.path.join('out', f"{filename}.png")) fig.clf()
def at_slope_to_label(self, actor): """Check wheter actor (i.e. vehicle) is near a slope, returning a ScenarioSlopeLabel. TODO: make wp_locations array size smaller after debugging. Don't need to check non-sloped waypoints. """ actor_location = carlautil.actor_to_location_ndarray(actor) actor_xy = actor_location[:2] actor_z = actor_location[-1] """Want to ignore waypoints above and below (i.e. in the case actor is on a bridge).""" upperbound_z = actor.bounding_box.extent.z * 2 lowerbound_z = -1 xy_distances_to_wps = np.linalg.norm(self.wp_locations[:, :2] - actor_xy, axis=1) z_displacement_to_wps = self.wp_locations[:, -1] - actor_z """get waypoints close to vehicle filter""" wps_filter = np.logical_and( xy_distances_to_wps < self.SLOPE_FIND_RADIUS, np.logical_and(z_displacement_to_wps < upperbound_z, z_displacement_to_wps > lowerbound_z)) ##### """obtain extra slope information""" wp_pitches = self.wp_pitches[wps_filter] if wp_pitches.size == 0: max_wp_pitch = 0.0 else: wp_pitches = np.min(np.vstack(( wp_pitches, np.abs(wp_pitches - 360.), )), axis=0) max_wp_pitch = np.max(wp_pitches) ##### if self._debug: nearby_slopes = np.logical_and(self.wp_is_sloped == True, wps_filter) for wp_location in self.wp_locations[nearby_slopes]: loc = carlautil.ndarray_to_location(wp_location) carlautil.debug_point(self.carla_world, loc) if np.any(self.wp_is_sloped[wps_filter]): return ScenarioSlopeLabel.SLOPES, max_wp_pitch else: return ScenarioSlopeLabel.NONE, max_wp_pitch
def at_intersection_to_label(self, actor): """Retrieve the label corresponding to the actor's location in the map based on proximity to intersections. Parameters ---------- actor : carla.Actor Returns """ actor_location = carlautil.actor_to_location_ndarray(actor) distances_to_uncontrolled = np.linalg.norm( self.uncontrolled_junction_locations - actor_location, axis=1) if np.any(distances_to_uncontrolled < self.TLIGHT_DETECT_RADIUS): return ScenarioIntersectionLabel.UNCONTROLLED distances_to_controlled = np.linalg.norm( self.controlled_junction_locations - actor_location, axis=1) if np.any(distances_to_controlled < self.TLIGHT_DETECT_RADIUS): return ScenarioIntersectionLabel.CONTROLLED return ScenarioIntersectionLabel.NONE
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 __plot_vertices(self, ovehicles, vertices): fig, axes = plt.subplots(1, 2, figsize=(15, 7)) ax = axes[1] X = vertices[-1][0][0][:, 0:2].T ax.scatter(X[0], X[1], c='r', s=2) X = vertices[-1][0][0][:, 2:4].T ax.scatter(X[0], X[1], c='b', s=2) X = vertices[-1][0][0][:, 4:6].T ax.scatter(X[0], X[1], c='g', s=2) X = vertices[-1][0][0][:, 6:8].T ax.scatter(X[0], X[1], c='m', s=2) x, y, _ = carlautil.actor_to_location_ndarray(self.__ego_vehicle, flip_y=True) ax = axes[0] ax.scatter(x, y, marker='*', c='k', s=100) ovehicle_colors = get_ovehicle_color_set( [ov.n_states for ov in ovehicles]) for ov_idx, ovehicle in enumerate(ovehicles): for latent_idx in range(ovehicle.n_states): logging.info( f"Plotting OV {ov_idx} latent value {latent_idx}.") color = ovehicle_colors[ov_idx][latent_idx] for t in range(self.__params.T): X = vertices[t][latent_idx][ov_idx][:, 0:2].T ax.scatter(X[0], X[1], color=color, s=2) X = vertices[t][latent_idx][ov_idx][:, 2:4].T ax.scatter(X[0], X[1], color=color, s=2) X = vertices[t][latent_idx][ov_idx][:, 4:6].T ax.scatter(X[0], X[1], color=color, s=2) X = vertices[t][latent_idx][ov_idx][:, 6:8].T ax.scatter(X[0], X[1], color=color, s=2) for ax in axes: ax.set_aspect('equal') plt.show()
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. """Compute the approximation of the union of obstacle sets""" # Find vertices of sampled obstacle sets T, K, n_ov, truck = params.T, params.K, params.O, params.truck vertices = np.empty((T, np.max(K), n_ov), dtype=object).tolist() for ov_idx, ovehicle in enumerate(ovehicles): for latent_idx in range(ovehicle.n_states): for t in range(T): ps = ovehicle.pred_positions[latent_idx] yaws = ovehicle.pred_yaws[latent_idx] n_p = ps.shape[0] vertices[t][latent_idx][ov_idx] = np.zeros((n_p, 8)) for k in range(n_p): vertices[t][latent_idx][ov_idx][ k] = get_vertices_from_center( ps[k, t], yaws[k, t], truck.d) plot_vertices = False if plot_vertices: latent_idx = 0 ov_idx = 0 fig, axes = plt.subplots(2, 2, figsize=(10, 10)) axes = axes.ravel() for t, ax in zip(range(1, params.T, 2), axes): # for ovehicle in scene.ovehicles: X = vertices[t][latent_idx][ov_idx][:, 0:2].T ax.scatter(X[0], X[1], c='r', s=2) X = vertices[t][latent_idx][ov_idx][:, 2:4].T ax.scatter(X[0], X[1], c='b', s=2) X = vertices[t][latent_idx][ov_idx][:, 4:6].T ax.scatter(X[0], X[1], c='g', s=2) X = vertices[t][latent_idx][ov_idx][:, 6:8].T ax.scatter(X[0], X[1], c='orange', s=2) ax.set_title(f"t = {t}") ax.set_aspect('equal') plt.show() """Cluster the samples""" T, K, n_ov = params.T, params.K, params.O A_union = np.empty(( T, np.max(K), n_ov, ), dtype=object).tolist() b_union = np.empty(( T, np.max(K), n_ov, ), dtype=object).tolist() for ov_idx, ovehicle in enumerate(ovehicles): for latent_idx in range(ovehicle.n_states): for t in range(T): yaws = ovehicle.pred_yaws[latent_idx][:, t] vertices_k = vertices[t][latent_idx][ov_idx] mean_theta_k = np.mean(yaws) A_union_k, b_union_k = get_approx_union( mean_theta_k, vertices_k) A_union[t][latent_idx][ov_idx] = A_union_k b_union[t][latent_idx][ov_idx] = b_union_k """Plot the overapproximation""" plot_overapprox = False if plot_overapprox: fig, ax = plt.subplots() t = 7 ov_idx = 0 ovehicle = ovehicles[ov_idx] for latent_idx in range(ovehicle.n_states): ps = ovehicle.pred_positions[latent_idx][:, t].T ax.scatter(ps[0], ps[1], c='r', s=2) try: plot_h_polyhedron(ax, A_union[t][latent_idx][ov_idx], b_union[t][0][ov_idx], ec='k', alpha=0.3) except scipy.spatial.qhull.QhullError as e: print(e) ax.set_aspect('equal') plt.show() """Apply motion planning problem""" model = docplex.mp.model.Model(name="proposed_problem") L, T, K, Gamma, nu, nx = params.L, params.T, params.K, params.Gamma, params.nu, params.nx u = np.array(model.continuous_var_list(nu * T, lb=-np.inf, name='u'), dtype=object) delta_tmp = model.binary_var_matrix(L * np.sum(K), T, name='delta') delta = np.empty(( L * np.sum(K), T, ), dtype=object) for k, v in delta_tmp.items(): delta[k] = v x_future = params.States_free_init + obj_matmul(Gamma, u) big_M = 1000 # conservative upper-bound x1 = x_future[nx::nx] x2 = x_future[nx + 1::nx] x3 = x_future[nx + 2::nx] x4 = x_future[nx + 3::nx] # 3rd and 4th states have COUPLED CONSTRAINTS # x4 - c1*x3 <= - c3 # x4 - c1*x3 >= - c2 # x4 + c1*x3 <= c2 # x4 + c1*x3 >= c3 c1, c2, c3 = params.c1, params.c2, params.c3 model.add_constraints([z <= -c3 for z in x4 - c1 * x3]) model.add_constraints([z <= c2 for z in -x4 + c1 * x3]) model.add_constraints([z <= c2 for z in x4 + c1 * x3]) model.add_constraints([z <= -c3 for z in -x4 - c1 * x3]) u1 = u[0::nu] u2 = u[1::nu] # Inputs have COUPLED CONSTRAINTS: # u2 - t1*u1 <= - t3 # u2 - t1*u1 >= - t2 # u2 + t1*u1 <= t2 # u2 + t1*u1 >= t3 t1, t2, t3 = params.t1, params.t2, params.t3 model.add_constraints([z <= -t3 for z in u2 - t1 * u1]) model.add_constraints([z <= t2 for z in -u2 + t1 * u1]) model.add_constraints([z <= t2 for z in u2 + t1 * u1]) model.add_constraints([z <= -t3 for z in -u2 - t1 * u1]) X = np.stack([x1, x2], axis=1) T, K, diag = params.T, params.K, 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(T): A_obs = A_union[t][latent_idx][ov_idx] b_obs = b_union[t][latent_idx][ov_idx] indices = 4 * (sum_clu + latent_idx) + np.arange(0, 4) lhs = obj_matmul(A_obs, X[t]) + big_M * (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) p_y = -p_y # need to flip about x-axis 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 = (x1[-1] - goal_x)**2 + (x2[-1] - goal_y)**2 model.minimize(cost) # model.print_information() s = model.solve() # model.print_solution() u_star = np.array([ui.solution_value for ui in u]) cost = cost.solution_value x1 = np.array([x1i.solution_value for x1i in x1]) x2 = np.array([x2i.solution_value for x2i in x2]) X_star = np.stack((x1, x2)).T 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 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 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""" # 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""" model = docplex.mp.model.Model(name="proposed_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 = np.array(model.continuous_var_list(nu * T, lb=-np.inf, name='u'), dtype=object) delta_tmp = model.binary_var_matrix(L * np.sum(K), T, name='delta') delta = np.empty(( L * np.sum(K), T, ), dtype=object) for k, v in delta_tmp.items(): delta[k] = v x_future = params.States_free_init + obj_matmul(Gamma, u) # TODO: hardcoded value, need to specify better big_M = 1000 # conservative upper-bound x1 = x_future[nx::nx] x2 = x_future[nx + 1::nx] x3 = x_future[nx + 2::nx] x4 = x_future[nx + 3::nx] u1 = u[0::nu] u2 = u[1::nu] model.add_constraints(self.compute_boundary_constraints(x1, x2)) model.add_constraints(self.compute_velocity_constraints(x3, x4)) model.add_constraints(self.compute_acceleration_constraints(u1, u2)) X = np.stack([x1, x2], axis=1) T, K, diag = self.__params.T, params.K, 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(T): A_obs = A_union[t][latent_idx][ov_idx] b_obs = b_union[t][latent_idx][ov_idx] indices = 4 * (sum_clu + latent_idx) + np.arange(0, 4) lhs = obj_matmul(A_obs, X[t]) + big_M * (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) p_y = -p_y # need to flip about x-axis 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 = (x1[-1] - goal_x)**2 + (x2[-1] - goal_y)**2 model.minimize(cost) # model.print_information() # model.parameters.read.datacheck = 1 if self.log_cplex: model.parameters.mip.display = 5 s = model.solve(log_output=True) else: model.solve() # model.print_solution() u_star = np.array([ui.solution_value for ui in u]) cost = cost.solution_value x1 = np.array([x1i.solution_value for x1i in x1]) x2 = np.array([x2i.solution_value for x2i in x2]) X_star = np.stack((x1, x2)).T 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)