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) v_0_x, v_0_y, _ = carlautil.actor_to_velocity_ndarray( self.__ego_vehicle, flip_y=True) # 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, v_0_x, v_0_y])) params.initial_state = initial_state A, B = self.__params.A, self.__params.B nx, nu = self.__params.nx, self.__params.nu T = self.__control_horizon # make state computation account for initial position and velocity initial_rollout = np.concatenate([ np.linalg.matrix_power(A, t) @ initial_state.world for t in range(T + 1) ]) params.initial_rollout = util.AttrDict(world=initial_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 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 Local LCSS parameters that are environment dependent.""" params = util.AttrDict() """General local params.""" params.frame = frame # 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.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)]) """Local params for (random) multiple coinciding controls.""" # N_traj - number of planned trajectories possible to compute params.N_traj = np.prod(params.K) max_K = np.max(params.K) # TODO: figure out a way to specify number of subtrajectories params.N_select = min(int(1.5 * max_K), params.N_traj) # subtraj_indices - the indices of the sub-trajectory to optimize for params.subtraj_indices = np.random.choice(np.arange(params.N_traj), size=params.N_select, replace=False) return params
def visualize_distances(): config = parse_arguments() client = carla.Client(config.host, config.port) client.set_timeout(10.0) world = client.get_world() carla_map = world.get_map() blueprints = world.get_blueprint_library().filter("vehicle.*") blueprints = [x for x in blueprints if int(x.get_attribute("number_of_wheels")) == 4] blueprints = [x for x in blueprints if not x.id.endswith('isetta')] blueprints = [x for x in blueprints if not x.id.endswith('carlacola')] blueprints = [x for x in blueprints if not x.id.endswith('firetruck')] blueprints = [x for x in blueprints if not x.id.endswith('cybertruck')] blueprints = [x for x in blueprints if not x.id.endswith('ambulance')] blueprints = [x for x in blueprints if not x.id.endswith('sprinter')] blueprints = [x for x in blueprints if not x.id.endswith('t2')] # blueprint = world.get_blueprint_library().find("vehicle.audi.a2") blueprint = random.choice(blueprints) spawn_point = carla_map.get_spawn_points()[0] vehicle = None try: vehicle = world.spawn_actor(blueprint, spawn_point) transform = carlautil.spherical_to_camera_watcher_transform( 3, math.pi, math.pi*(1/6), pin=spawn_point.location) world.get_spectator().set_transform(transform) thickness = 0.2 color = carla.Color(r=255, g=0, b=0, a=100) life_time = 6 loc = carlautil.to_location_ndarray(spawn_point.location) scale = 2. z = loc[2] + 0.5 box = np.array([[-1., -1.], [1., -1.], [1., 1.], [-1., 1.]]) box = (box*scale) + loc[:2] world.debug.draw_line( carla.Location(box[0, 0], box[0, 1], z), carla.Location(box[1, 0], box[1, 1], z), thickness=thickness, color=color, life_time=life_time) world.debug.draw_line( carla.Location(box[1, 0], box[1, 1], z), carla.Location(box[2, 0], box[2, 1], z), thickness=thickness, color=color, life_time=life_time) world.debug.draw_line( carla.Location(box[2, 0], box[2, 1], z), carla.Location(box[3, 0], box[3, 1], z), thickness=thickness, color=color, life_time=life_time) world.debug.draw_line( carla.Location(box[3, 0], box[3, 1], z), carla.Location(box[0, 0], box[0, 1], z), thickness=thickness, color=color, life_time=life_time) time.sleep(life_time) world.wait_for_tick() finally: if vehicle: vehicle.destroy()
def to_point(wp, flip_x=False, flip_y=False): """Convert waypoint to a 2D point. Parameters ========== wp : carla.Waypoint Returns ======= list of float """ x, y, _ = carlautil.to_location_ndarray(wp, flip_x=flip_x, flip_y=flip_y) return [x, y]
def extract_junction_with_portals(self): carla_topology = self.carla_map.get_topology() junctions = carlautil.get_junctions_from_topology_graph(carla_topology) _junctions = [] for junction in junctions: jx, jy, _ = carlautil.to_location_ndarray(junction, flip_y=True) wps = junction.get_waypoints(carla.LaneType.Driving) _wps = [] for wp1, wp2 in wps: # wp1 is the waypoint entering into the intersection # wp2 is the waypoint exiting out of the intersection x, y, _ = carlautil.to_location_ndarray(wp1, flip_y=True) _, yaw, _ = carlautil.to_rotation_ndarray(wp1, flip_y=True) _wp1 = (x, y, yaw, wp1.lane_width) x, y, _ = carlautil.to_location_ndarray(wp2, flip_y=True) _, yaw, _ = carlautil.to_rotation_ndarray(wp2, flip_y=True) _wp2 = (x, y, yaw, wp2.lane_width) _wps.append((_wp1, _wp2)) _junctions.append( util.AttrDict(pos=np.array([jx, jy]), waypoints=np.array(_wps))) return _junctions
def __plot_segs_polytopes(self, params, segs_polytopes, goal): fig, ax = plt.subplots(figsize=(7, 7)) x_min, y_min = np.min(self.__road_segs.positions, axis=0) x_max, y_max = np.max(self.__road_segs.positions, axis=0) self.__map_reader.render_map( ax, extent=(x_min - 20, x_max + 20, y_min - 20, y_max + 20) ) x, y, _ = carlautil.to_location_ndarray(self.__ego_vehicle, flip_y=True) ax.scatter(x, y, c="r", zorder=10) x, y = goal ax.scatter(x, y, c="g", marker="*", zorder=10) for A, b in segs_polytopes: util.npu.plot_h_polyhedron(ax, A, b, fc="b", ec="b", alpha=0.3) filename = f"agent{self.__ego_vehicle.id}_frame{params.frame}_boundary" fig.savefig(os.path.join("out", f"{filename}.png")) fig.clf()
def __compute_prediction_controls(self, frame): pred_result = self.do_prediction(frame) ovehicles = self.make_ovehicles(pred_result) params = self.make_local_params(ovehicles) ctrl_result = self.do_highlevel_control(params, ovehicles) """use control input next round for warm starting.""" self.U_warmstarting = ctrl_result.U_star """Get trajectory""" trajectory = [] x, y, _ = carlautil.to_location_ndarray(self.__ego_vehicle, flip_y=True) X = np.concatenate((np.array([x, y])[None], ctrl_result.X_star[:, :2])) n_steps = X.shape[0] headings = [] for t in range(1, n_steps): x, y = X[t] y = -y # flip about x-axis again to move back to UE coordinates yaw = np.arctan2(X[t, 1] - X[t - 1, 1], X[t, 0] - X[t - 1, 0]) headings.append(yaw) # flip about x-axis again to move back to UE coordinates yaw = util.reflect_radians_about_x_axis(yaw) transform = carla.Transform(carla.Location(x=x, y=y), carla.Rotation(yaw=yaw)) trajectory.append(transform) if self.plot_scenario: """Plot scenario""" filename = f"agent{self.__ego_vehicle.id}_frame{frame}_lcss_control" ctrl_result.headings = headings lon, lat, _ = carlautil.actor_to_bbox_ndarray(self.__ego_vehicle) ego_bbox = [lon, lat] params.update(self.__params) plot_lcss_prediction(pred_result, ovehicles, params, ctrl_result, self.__prediction_horizon, ego_bbox, filename=filename) if self.plot_simulation: """Save planned trajectory for final plotting""" self.__plot_simulation_data.planned_trajectories[ frame] = np.concatenate((params.x0[None], ctrl_result.X_star)) self.__plot_simulation_data.planned_controls[ frame] = ctrl_result.U_star return trajectory
def get_straight_line(start_wp, start_yaw, tol=2.0): """Get the points corresponding to the straight part of the road lane starting from start_wp. Parameters ========== start_wp : carla.Waypoint The starting point of the road lane. start_yaw : float The angle of the road in radians. This is passed as the lanes on the road may not be parallel. tol : float (optional) The tolerance to select straight part of the road in meters. Returns ======= np.array Array of points of shape (N, 2). Each (x, y) point is in meters in world coordinates. """ x_start, y_start, _ = carlautil.to_location_ndarray(start_wp) FACTOR = 10 x_end, y_end = x_start + FACTOR * np.cos( start_yaw), y_start + FACTOR * np.sin(start_yaw) def inner(wp): wps = wp.next_until_lane_end(PRECISION) points = np.array(util.map_to_ndarray(to_point, wps)) distances = util.distances_from_line_2d(points, x_start, y_start, x_end, y_end) wps_mask = np.nonzero(distances > tol)[0] if wps_mask.size > 0: # waypoints veer off a straight line idx = wps_mask[0] return points[:idx], list() else: # waypoints stay one straight line. Get the next set of waypoints return points, wps[-1].next(PRECISION) dq = collections.deque([start_wp]) point_collection = [] while len(dq) > 0: wp = dq.popleft() points, next_wps = inner(wp) dq.extend(next_wps) point_collection.append(points) return np.concatenate(point_collection)
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] plot_bbox_constraints = False if plot_bbox_constraints: fig, axes = plt.subplots(1, 2, figsize=(10, 6)) axes = axes.ravel() ego_x, ego_y, _ = carlautil.to_location_ndarray(self.__ego_vehicle) ego_y = -ego_y wp_x, wp_y = self.__road_seg_starting[:2] axes[0].plot(ego_x, ego_y, 'g*') axes[0].plot(wp_x, wp_y, 'b*') patch = patches.Polygon(self.__road_seg_enclosure, ec='b', fc='none') axes[0].add_patch(patch) axes[0].set_aspect('equal') ego_x, ego_y = mtx @ (np.array([ego_x, ego_y]) - shift) axes[1].plot(ego_x, ego_y, 'g*') axes[1].plot([0, -b_length], [0, 0], '-bo') axes[1].plot([0, f_length], [0, 0], '-bo') axes[1].plot([0, 0], [0, -r_width], '-bo') axes[1].plot([0, 0], [0, l_width], '-bo') axes[1].set_aspect('equal') fig.tight_layout() plt.show() pos = np.stack([p_x, p_y], axis=1) pos = 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 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 __plot_boundary(self): fig, axes = plt.subplots(1, 2, figsize=(10, 6)) axes = axes.ravel() ego_x, ego_y, _ = carlautil.to_location_ndarray(self.__ego_vehicle) ego_y = -ego_y wp_x, wp_y = self.__road_seg_starting[:2] axes[0].plot(ego_x, ego_y, 'g*') axes[0].plot(wp_x, wp_y, 'b*') patch = patches.Polygon(self.__road_seg_enclosure, ec='b', fc='none') axes[0].add_patch(patch) axes[0].set_aspect('equal') 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] ego_x, ego_y = mtx @ (np.array([ego_x, ego_y]) - shift) axes[1].plot(ego_x, ego_y, 'g*') axes[1].plot([0, -b_length], [0, 0], '-bo') axes[1].plot([0, f_length], [0, 0], '-bo') axes[1].plot([0, 0], [0, -r_width], '-bo') axes[1].plot([0, 0], [0, l_width], '-bo') axes[1].set_aspect('equal') fig.tight_layout() plt.show()
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 # N_traj - number of planned trajectories to compute params.N_traj = np.prod(params.K) p_0_x, p_0_y, _ = carlautil.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]) 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, 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
def get_road_segment_enclosure(start_wp, tol=2.0): """Get rectangle that tightly inner approximates of the road segment containing the starting waypoint. Parameters ========== start_wp : carla.Waypoint A starting waypoint of the road. tol : float (optional) The tolerance to select straight part of the road in meters. Returns ======= np.array The position and the heading angle of the starting waypoint of the road of form [x, y, angle] in (meters, meters, radians). np.array The 2D bounding box enclosure in world coordinates of shape (4, 2) enclosing the road segment. np.array The parameters of the enclosure of form (b_length, f_length, r_width, l_width) If the enclosure is in the reference frame such that the starting waypoint points along +x-axis, then the enclosure has these length and widths: ____________________________________ | l_width | | | | | | | | b_length -- (x, y)-> -- f_length | | | | | | | | r_width | ------------------------------------ """ _LENGTH = -2 _, start_yaw, _ = carlautil.to_rotation_ndarray(start_wp) adj_wps = get_adjacent_waypoints(start_wp) # mtx : np.array # Rotation matrix from world coordinates, both frames in UE orientation mtx = util.rotation_2d(-start_yaw) # rev_mtx : np.array # Rotation matrix to world coordinates, both frames in UE orientation rev_mtx = util.rotation_2d(start_yaw) s_x, s_y, _ = carlautil.to_location_ndarray(start_wp) # Get points of lanes f = lambda wp: get_straight_line(wp, start_yaw, tol=tol) pc = util.map_to_list(f, adj_wps) # Get length of bb for lanes def g(points): points = points - np.array([s_x, s_y]) points = (rev_mtx @ points.T)[0] return np.abs(np.max(points) - np.min(points)) lane_lengths = util.map_to_ndarray(g, pc) length = np.min(lane_lengths) # Get width of bb for lanes lwp, rwp = adj_wps[0], adj_wps[-1] l_x, l_y, _ = carlautil.to_location_ndarray(lwp) r_x, r_y, _ = carlautil.to_location_ndarray(rwp) points = np.array([[l_x, l_y], [s_x, s_y], [r_x, r_y]]) points = points @ rev_mtx.T l_width = np.abs(points[0, 1] - points[1, 1]) + lwp.lane_width / 2. r_width = np.abs(points[1, 1] - points[2, 1]) + rwp.lane_width / 2. # construct bounding box of road segment x, y, _ = carlautil.to_location_ndarray(start_wp) vec = np.array([[0, 0], [_LENGTH, 0]]) @ mtx.T dx0, dy0 = vec[1, 0], vec[1, 1] vec = np.array([[0, 0], [length, 0]]) @ mtx.T dx1, dy1 = vec[1, 0], vec[1, 1] vec = np.array([[0, 0], [0, -l_width]]) @ mtx.T dx2, dy2 = vec[1, 0], vec[1, 1] vec = np.array([[0, 0], [0, r_width]]) @ mtx.T dx3, dy3 = vec[1, 0], vec[1, 1] bbox = np.array([[x + dx0 + dx3, y + dy0 + dy3], [x + dx1 + dx3, y + dy1 + dy3], [x + dx1 + dx2, y + dy1 + dy2], [x + dx0 + dx2, y + dy0 + dy2]]) start_wp_spec = np.array([s_x, s_y, start_yaw]) bbox_spec = np.array([_LENGTH, length, r_width, l_width]) return start_wp_spec, bbox, bbox_spec
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 __capture_agents_within_radius(self, frame_id): labels = self.__map_reader.get_actor_labels(self.__ego_vehicle) if self.__should_exclude_dataset_sample(labels): self.__remove_scene_builder() player_location = carlautil.to_location_ndarray(self.__ego_vehicle) player_z = player_location[2] if len(self.__other_vehicles): other_ids = list(self.__other_vehicles.keys()) other_vehicles = self.__other_vehicles.values() others_data = carlautil.actors_to_Lxyz_Vxyz_Axyz_Rpyr_ndarray( other_vehicles).T other_locations = others_data[:3].T distances = np.linalg.norm(other_locations - player_location, axis=1) df = pd.DataFrame({ "frame_id": np.full((len(other_ids), ), frame_id), "type": [self.__scene_config.node_type.VEHICLE] * len(other_ids), "node_id": util.map_to_list(str, other_ids), "robot": [False] * len(other_ids), "distances": distances, "x": others_data[0], "y": others_data[1], "z": others_data[2], "v_x": others_data[3], "v_y": others_data[4], "v_z": others_data[5], "a_x": others_data[6], "a_y": others_data[7], "a_z": others_data[8], "length": others_data[9], "width": others_data[10], "height": others_data[11], "heading": others_data[13], }) df = df[df["distances"] < self.__radius] df = df[df["z"].between( player_z - self.Z_LOWERBOUND, player_z + self.Z_UPPERBOUND, inclusive="neither", )] del df["distances"] else: df = pd.DataFrame(columns=[ "frame_id", "type", "node_id", "robot", "x", "y", "z", "length", "width", "height", "heading", ]) ego_data = carlautil.actor_to_Lxyz_Vxyz_Axyz_Rpyr_ndarray( self.__ego_vehicle) data_point = pd.DataFrame.from_records([{ "frame_id": frame_id, "type": self.__scene_config.node_type.VEHICLE, "node_id": "ego", "robot": True, "x": ego_data[0], "y": ego_data[1], "z": ego_data[2], "v_x": ego_data[3], "v_y": ego_data[4], "v_z": ego_data[5], "a_x": ego_data[6], "a_y": ego_data[7], "a_z": ego_data[8], "length": ego_data[9], "width": ego_data[10], "height": ego_data[11], "heading": ego_data[13], }]) df = pd.concat((df, data_point), ignore_index=True) # df = df.append(data_point, ignore_index=True) if self.__trajectory_data is None: self.__trajectory_data = df else: self.__trajectory_data = pd.concat((self.__trajectory_data, df), ignore_index=True)