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_global_params(self): """Get scenario wide parameters used across all loops""" params = util.AttrDict() # Slack variable for solver params.M_big = 10_000 # Control variable for solver, setting max/min acceleration/speed params.max_a = 1 params.min_a = -7 params.max_v = 5 # objective : util.AttrDict # Parameters in objective function. params.objective = util.AttrDict(w_final=2., w_ch_accel=0.5, w_ch_turning=0.5, w_accel=0.5, w_turning=0.5) # Maximum steering angle physics_control = self.__ego_vehicle.get_physics_control() wheels = physics_control.wheels params.limit_delta = np.deg2rad(wheels[0].max_steer_angle) # Max steering # We fix max turning angle to make reasonable planned turns. params.max_delta = 0.5 * params.limit_delta # longitudinal and lateral dimensions of car are normally 3.70 m, 1.79 m resp. bbox = util.AttrDict() bbox.lon, bbox.lat, _ = carlautil.actor_to_bbox_ndarray( self.__ego_vehicle) params.bbox = bbox # Number of faces of obstacle sets params.L = 4 # Minimum distance from vehicle to avoid collision. # Assumes that car is a circle. # TODO: remove this. Improve bounds instead params.diag = np.sqrt(bbox.lon**2 + bbox.lat**2) / 2. 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 set_goal(self, x=None, y=None, distance=None, is_relative=True, **kwargs): if x is not None and y is not None: self.__goal = util.AttrDict(x=x, y=y, is_relative=is_relative) elif distance is not None: point = self.__road_boundary.get_point_from_start(distance) self.__goal = util.AttrDict(x=point[0], y=point[1], is_relative=False) else: raise NotImplementedError("Unknown method of setting motion planner goal.")
def __setup_road_boundary_conditions(self, turn_choices, max_distance): """Set up a generic road boundary configuration. TODO: extend RoadBoundaryConstraint so it takes over the role of __setup_curved_road_segmented_boundary_conditions() and __setup_rectangular_boundary_conditions() """ # __turn_choices : list of int # List of choices of turns to make at intersections, # starting with the first intersection to the last. self.__turn_choices = turn_choices # __max_distance : number # Maximum distance from road self.__max_distance = max_distance # __road_boundary : RoadBoundaryConstraint # Factory for road boundary constraints. self.__road_boundary = self.__map_reader.road_boundary_constraints_from_actor( self.__ego_vehicle, self.__max_distance, choices=self.__turn_choices, flip_y=True, ) n_polytopes = len(self.__road_boundary.road_segs.polytopes) logging.info( f"created {n_polytopes} polytopes covering " f"a distance of {np.round(self.__max_distance, 2)} m in total." ) x, y = self.__road_boundary.points[-1] # __goal # Not used for motion planning when using this BC. self.__goal = util.AttrDict(x=x, y=y, is_relative=False)
def __setup_curved_road_segmented_boundary_conditions( self, turn_choices, max_distance ): # __turn_choices : list of int # List of choices of turns to make at intersections, # starting with the first intersection to the last. self.__turn_choices = turn_choices # __max_distance : number # Maximum distance from road self.__max_distance = max_distance # __road_segs : util.AttrDict # Container of road segment properties. self.__road_segs = self.__map_reader.curved_road_segments_enclosure_from_actor( self.__ego_vehicle, self.__max_distance, choices=self.__turn_choices, flip_y=True, ) logging.info( f"max curvature of planned path is {self.__road_segs.max_k}; " f"created {len(self.__road_segs.polytopes)} polytopes covering " f"a distance of {np.round(self.__max_distance, 2)} m in total." ) x, y = self.__road_segs.spline(self.__road_segs.distances[-1]) # __goal # Not used for motion planning when using this BC. self.__goal = util.AttrDict(x=x, y=y, is_relative=False)
def __init__(self): """Load map data from cache and collect shapes of all the intersections.""" self.map_datum = {} # map to Shapely MultiPolygon for junction entrance/exits self.map_to_smpolys = {} # map to Shapely Circle covering junction region self.map_to_scircles = {} logger.info("Retrieve map from cache.") for map_name in CARLA_MAP_NAMES: cachepath = f"{CACHEDIR}/map_data.{map_name}.pkl" with open(cachepath, "rb") as f: payload = dill.load(f, encoding="latin1") self.map_datum[map_name] = util.AttrDict( road_polygons=payload["road_polygons"], white_lines=payload["white_lines"], yellow_lines=payload["yellow_lines"], junctions=payload["junctions"], ) logger.info("Retrieving some data from map.") for map_name in CARLA_MAP_NAMES: for _junction in self.map_datum[map_name].junctions: f = lambda x, y, yaw, l: util.vertices_from_bbox( np.array([x, y]), yaw, np.array([5.0, 0.95 * l])) vertex_set = util.map_to_ndarray( lambda wps: util.starmap(f, wps), _junction.waypoints) smpolys = util.map_to_list(vertex_set_to_smpoly, vertex_set) util.setget_list_from_dict(self.map_to_smpolys, map_name).extend(smpolys) x, y = _junction.pos scircle = shapely.geometry.Point(x, y).buffer( self.TLIGHT_DETECT_RADIUS) util.setget_list_from_dict(self.map_to_scircles, map_name).append(scircle)
def extract_junction_points(self, tlight_find_radius=25.): carla_topology = self.carla_map.get_topology() junctions = carlautil.get_junctions_from_topology_graph(carla_topology) tlights = util.filter_to_list(lambda a: 'traffic_light' in a.type_id, self.carla_world.get_actors()) tlight_distances = np.zeros(( len(tlights), len(junctions), )) f = lambda j: carlautil.location_to_ndarray(j.bounding_box.location) junction_locations = util.map_to_ndarray(f, junctions) g = lambda tl: carlautil.transform_to_location_ndarray(tl. get_transform()) tlight_locations = util.map_to_ndarray(g, tlights) for idx, junction in enumerate(junctions): tlight_distances[:, idx] = np.linalg.norm(tlight_locations - junction_locations[idx], axis=1) is_controlled_junction = (tlight_distances < tlight_find_radius).any( axis=0) is_uncontrolled_junction = np.logical_not(is_controlled_junction) controlled_junction_locations \ = junction_locations[is_controlled_junction] uncontrolled_junction_locations \ = junction_locations[is_uncontrolled_junction] return util.AttrDict(controlled=controlled_junction_locations, uncontrolled=uncontrolled_junction_locations)
def __init__( self, vehicle, dt=0.03, break_prop=0.1, coeff=PIDCoefficients(K_P=1.0, K_I=0.0, K_D=0.0), ): """Constructor method. Parameters ========== vehicle : carla.Vehicle Actor to apply to local planner logic onto dt : float Time differential in seconds coeff : PIDCoefficients Throttle/break PID coefficients. """ self.__vehicle = vehicle self.__coeff = coeff self.__dt = dt self.__break_prop = break_prop self.__clip = util.Clip(low=-1, high=1) self.__error_buffer = collections.deque(maxlen=8_000) self.__stats = util.AttrDict(pe=0., ie=0., de=0.) self.__should_hotfix_mpc = False
def __init__( self, vehicle, max_steering=1.0, dt=0.03, coeff=PIDCoefficients(K_P=1.0, K_I=0.0, K_D=0.0), ): """Constructor method. Parameters ========== vehicle : carla.Vehicle Actor to apply to local planner logic onto max_steering : float The maximum steering angle in radians. For CARLA version 0.9 the Audi A2 can steer a maximum of 57.5 degrees, 1.00 radians (average of two wheels). dt : float Time differential in seconds coeff : PIDCoefficients PID coefficients. """ self.__vehicle = vehicle self.__max_steering = max_steering self.__coeff = coeff self.__dt = dt self.__clip = util.Clip(low=-1, high=1) self.__error_buffer = collections.deque(maxlen=8_000) self.__stats = util.AttrDict(pe=0., ie=0., de=0.) self.__should_hotfix_mpc = False
def __setup_curved_road_segmented_boundary_conditions( self, turn_choices, max_distance): # __turn_choices : list of int # List of choices of turns to make at intersections, # starting with the first intersection to the last. self.__turn_choices = turn_choices # __max_distance : number # Maximum distance from road self.__max_distance = max_distance # __road_segs.spline : scipy.interpolate.CubicSpline # The spline representing the path the vehicle should motion plan on. # __road_segs.polytopes : list of (ndarray, ndarray) # List of polytopes in H-representation (A, b) where x is in polytope if Ax <= b. # __road_segs.distances : ndarray # The distances along the spline to follow from nearest endpoint # before encountering corresponding covering polytope in index. # __road_segs.positions : ndarray # The 2D positions of center of the covering polytope in index. self.__road_segs = self.__map_reader.curved_road_segments_enclosure_from_actor( self.__ego_vehicle, self.__max_distance, choices=self.__turn_choices, flip_y=True) logging.info( f"max curvature of planned path is {self.__road_segs.max_k}; " f"created {len(self.__road_segs.polytopes)} polytopes covering " f"a distance of {np.round(self.__max_distance, 2)} m in total.") x, y = self.__road_segs.spline(self.__road_segs.distances[-1]) # __goal # Not used for motion planning when using this BC. self.__goal = util.AttrDict(x=x, y=y, is_relative=False)
def do_prediction(self, frame): """Get processed scene object from scene builder, input the scene to a model to generate the predictions, and then return the predictions and the latents variables. """ """Construct online scene""" scene = self.__scene_builder.get_scene() """Extract Predictions""" frame_id = int( (frame - self.__first_frame) / self.__scene_config.record_interval) timestep = frame_id # we use this as the timestep timesteps = np.array([timestep]) with torch.no_grad(): z, predictions, nodes, predictions_dict, latent_probs = generate_vehicle_latents( self.__eval_stg, scene, timesteps, num_samples=self.__n_predictions, ph=self.__prediction_horizon, z_mode=False, gmm_mode=False, full_dist=False, all_z_sep=False) _, past_dict, ground_truth_dict = \ prediction_output_to_trajectories( predictions_dict, dt=scene.dt, max_h=10, ph=self.__prediction_horizon, map=None) return util.AttrDict(scene=scene, timestep=timestep, nodes=nodes, predictions=predictions, z=z, latent_probs=latent_probs, past_dict=past_dict, ground_truth_dict=ground_truth_dict)
def __make_global_params(self): """Get global parameters used across all loops""" params = util.AttrDict() # Big M variable for Slack variables in solver params.M = 1000 # Control variable for solver, setting max acceleration params.max_a = 2.5 # Maximum steering angle physics_control = self.__ego_vehicle.get_physics_control() wheels = physics_control.wheels params.max_delta = np.deg2rad(wheels[0].max_steer_angle) # longitudinal and lateral dimensions of car are normally 3.70 m, 1.79 m resp. params.bbox_lon, params.bbox_lat, _ = carlautil.actor_to_bbox_ndarray( self.__ego_vehicle) params.diag = np.sqrt(params.bbox_lon**2 + params.bbox_lat**2) / 2. params.A, params.B = self.__get_state_space_representation( self.__timestep) # number of state variables x, number of input variables u # nx = 4, nu = 2 params.nx, params.nu = params.B.shape # Closed for solution of control without obstacles A, B, nx, nu = params.A, params.B, params.nx, params.nu T = self.__control_horizon # 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 return params
def __init__(self, vehicle, max_steering=1.0, K_P=1.0, K_I=0.0, K_D=0.0, dt=0.03): """Constructor method. Parameters ========== vehicle : carla.Vehicle Actor to apply to local planner logic onto max_steering : float The maximum steering angle in radians. For CARLA version 0.9 the Audi A2 can steer a maximum of 57.5 degrees, 1.00 radians (average of two wheels). K_P : float Proportional term K_I : float Integral term K_D : float Differential term dt : float Time differential in seconds """ self.__vehicle = vehicle self.__max_steering = max_steering self.__k_p = K_P self.__k_i = K_I self.__k_d = K_D self.__dt = dt self.__clip = util.Clip(low=-1, high=1) self.__error_buffer = collections.deque(maxlen=8_000) self.__stats = util.AttrDict(pe=0., ie=0., de=0.)
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 __init__( self, ego_vehicle, map_reader, n_burn_interval=4, control_horizon=6, step_horizon=1, scene_config=OnlineConfig(), road_boundary_constraints=True, ####################### # Logging and debugging log_cplex=False, log_agent=False, plot_simulation=False, plot_boundary=False, ####################### # Planned path settings turn_choices=[], max_distance=100, ####################### **kwargs): self.__ego_vehicle = ego_vehicle self.__map_reader = map_reader # __n_burn_interval : int # Interval in prediction timesteps to skip prediction and control. self.__n_burn_interval = n_burn_interval # __control_horizon : int # Number of timesteps to optimize control over. self.__control_horizon = control_horizon # __step_horizon : int # Number of steps to take at each iteration of MPC. self.__step_horizon = step_horizon # __first_frame : int # First frame in simulation. Used to find current timestep. self.__first_frame = None self.__scene_config = scene_config self.__world = self.__ego_vehicle.get_world() self.__timestep = self.__scene_config.record_interval \ * self.__world.get_settings().fixed_delta_seconds self.__local_planner = LocalPlanner(self.__ego_vehicle) self.__params = self.__make_global_params() self.__setup_curved_road_segmented_boundary_conditions( turn_choices, max_distance) self.road_boundary_constraints = road_boundary_constraints self.log_cplex = log_cplex self.log_agent = log_agent self.plot_simulation = plot_simulation self.plot_boundary = plot_boundary if self.plot_simulation: self.__plot_simulation_data = util.AttrDict( actual_trajectory=collections.OrderedDict(), planned_trajectories=collections.OrderedDict(), planned_controls=collections.OrderedDict(), goals=collections.OrderedDict())
def extract_road_polygons_and_lines(self, sampling_precision=0.05): """Extract road white and yellow dividing lines, and road polygons. Parameters ========== sampling_precision : float Space between sampling points to create road data. Returns ======= util.AttrDict Payload of road network information with the following keys: - road_polygons: list of road polygons (closed), each represented as array of shape (?, 2) - yellow_lines: list of white road lines, each represented as array of shape (?, 2) - white_lines: list of white road lines, each represented as array of shape (?, 2) """ road_polygons = [] yellow_lines = [] white_lines = [] topology = [x[0] for x in self.carla_map.get_topology()] topology = sorted(topology, key=lambda w: w.transform.location.z) for waypoint in topology: waypoints = [waypoint] nxt = waypoint.next(sampling_precision)[0] while nxt.road_id == waypoint.road_id: waypoints.append(nxt) nxt = nxt.next(sampling_precision)[0] left_marking = carlautil.locations_to_ndarray([ self.__lateral_shift(w.transform, -w.lane_width * 0.5) for w in waypoints ], flip_y=True) right_marking = carlautil.locations_to_ndarray([ self.__lateral_shift(w.transform, w.lane_width * 0.5) for w in waypoints ], flip_y=True) road_polygon = np.concatenate( (left_marking, np.flipud(right_marking)), axis=0) if len(road_polygon) > 2: road_polygons.append(road_polygon) if not waypoint.is_intersection: sample = waypoints[int(len(waypoints) / 2)] if self.__is_yellow_line(sample, -sample.lane_width * 1.1): yellow_lines.append(left_marking) else: white_lines.append(left_marking) if self.__is_yellow_line(sample, sample.lane_width * 1.1): yellow_lines.append(right_marking) else: white_lines.append(right_marking) return util.AttrDict(road_polygons=road_polygons, yellow_lines=yellow_lines, white_lines=white_lines)
def __make_global_params(self): """Get Global LCSS parameters used across all loops""" params = util.AttrDict() params.M_big = 1000 params.u_max = 3. params.A, params.B = self.__get_state_space_representation( self.__prediction_timestep) # number of state variables x, number of input variables u # nx = 4, nu = 2 params.nx, params.nu = params.B.shape # longitudinal and lateral dimensions of car are normally 3.70 m, 1.79 m resp. bbox_lon, bbox_lat, _ = carlautil.actor_to_bbox_ndarray( self.__ego_vehicle) params.diag = np.sqrt(bbox_lon**2 + bbox_lat**2) / 2. # Prediction parameters params.T = self.__prediction_horizon params.L = 4 # number of faces of obstacle sets # Closed for solution of control without obstacles 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 return params
def _plot_multiple_coinciding_controls(pred_result, ovehicles, params, ctrl_result, ego_bbox, filename='lcss_control'): """ Parameters ========== predict_result : util.AttrDict Payload containing scene, timestep, nodes, predictions, z, latent_probs, past_dict, ground_truth_dict ovehicles : list of OVehicle params : util.AttrDict ctrl_result : util.AttrDict ego_bbox : list of int filename : str """ """Plots for paper""" for traj_idx, latent_indices in enumerate( util.product_list_of_list( [range(ovehicle.n_states) for ovehicle in ovehicles])): T = params.T fig, axes = plt.subplots(T // 2 + (T % 2), 2, figsize=(10, (10 / 4) * T)) axes = axes.ravel() """Get scene bitmap""" scene = pred_result.scene map_mask = scene.map['VISUALIZATION'].as_image() map_data = util.AttrDict() map_data.road_bitmap = np.max(map_mask, axis=2) map_data.road_div_bitmap = map_mask[..., 1] map_data.lane_div_bitmap = map_mask[..., 0] map_data.extent = (scene.x_min, scene.x_max, scene.y_min, scene.y_max) """Get control trajectory data""" X = np.concatenate( (ctrl_result.start[None], ctrl_result.X_star[traj_idx, :, :2]), axis=0) headings = [] for t in range(1, T + 1): heading = np.arctan2(X[t, 1] - X[t - 1, 1], X[t, 0] - X[t - 1, 0]) headings.append(heading) headings = np.array(headings) for t, ax in zip(range(T), axes): _plot_multiple_coinciding_controls_timestep( ax, map_data, ovehicles, params, ctrl_result, X, headings, t, traj_idx, latent_indices, ego_bbox) fig.tight_layout() fig.savefig(os.path.join('out', f"{filename}_traj{traj_idx + 1}.png")) fig.clf()
def test_group_split(): """Test by manual inspection""" config = util.AttrDict(n_groups=4) group_creator = SampleGroupCreator(config) split_creator = CrossValidationSplitCreator(config) sample_ids = MOCK_IDS groups = group_creator.make_groups(sample_ids) splits = split_creator.make_splits(groups) split = splits[0] print() pp.pprint(groups) pp.pprint(split)
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 __setup_rectangular_boundary_conditions(self): # __road_segment_enclosure : np.array # Array of shape (4, 2) enclosing the road segment # __road_seg_starting : np.array # The position and the heading angle of the starting waypoint # of the road of form [x, y, angle] in (meters, meters, radians). self.__road_seg_starting, self.__road_seg_enclosure, self.__road_seg_params \ = self.__map_reader.road_segment_enclosure_from_actor(self.__ego_vehicle) self.__road_seg_starting[1] *= -1 # need to flip about x-axis self.__road_seg_starting[2] = util.reflect_radians_about_x_axis( self.__road_seg_starting[2]) # need to flip about x-axis self.__road_seg_enclosure[:, 1] *= -1 # need to flip about x-axis # __goal # Goal destination the vehicle should navigates to. self.__goal = util.AttrDict(x=50, y=0, is_relative=True)
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_global_params(self): """Get global parameters used across all loops""" params = util.AttrDict() # Big M variable for Slack variables in solver params.M = 1000 # Control variable for solver, setting max acceleration params.max_a = 2.5 # Maximum steering angle physics_control = self.__ego_vehicle.get_physics_control() wheels = physics_control.wheels params.max_delta = np.deg2rad(wheels[0].max_steer_angle) # longitudinal and lateral dimensions of car are normally 3.70 m, 1.79 m resp. params.bbox_lon, params.bbox_lat, _ = carlautil.actor_to_bbox_ndarray( self.__ego_vehicle) params.diag = np.sqrt(params.bbox_lon**2 + params.bbox_lat**2) / 2. return params
def __inspect_node(self, map_name, nodeid): """Label the given node by inspecting the node's properties.""" (is_complete_intersection) = self.__inspect_intersection_completedness( map_name, nodeid) (is_at_intersection) = self.__inspect_intersectedness(map_name, nodeid) (is_significant_car, is_stopped_car) = self.__inspect_distance(map_name, nodeid) (is_major_turn, is_minor_turn) = self.__inspect_curvature(map_name, nodeid) return util.AttrDict(node_id=nodeid, is_complete_intersection=is_complete_intersection, is_at_intersection=is_at_intersection, is_significant_car=is_significant_car, is_stopped_car=is_stopped_car, is_major_turn=is_major_turn, is_minor_turn=is_minor_turn)
def __init__(self, ego_vehicle, map_reader, other_vehicle_ids, eval_stg, predict_interval=6, n_burn_interval=4, prediction_horizon=8, n_predictions=100, scene_config=OnlineConfig()): self.__ego_vehicle = ego_vehicle self.__map_reader = map_reader self.__world = self.__ego_vehicle.get_world() # __first_frame : int # First frame in simulation. Used to find current timestep. self.__first_frame = None self.__scene_builder = None self.__scene_config = scene_config self.__predict_interval = predict_interval self.__n_burn_interval = n_burn_interval self.__prediction_horizon = prediction_horizon self.__n_predictions = n_predictions self.__eval_stg = eval_stg vehicles = self.__world.get_actors(other_vehicle_ids) # __other_vehicles : list of carla.Vehicle # List of IDs of vehicles not including __ego_vehicle. # Use this to track other vehicles in the scene at each timestep. self.__other_vehicles = dict(zip(other_vehicle_ids, vehicles)) # __sensor : carla.Sensor # Segmentation sensor. Data points will be used to construct overhead. self.__sensor = self.__create_segmentation_lidar_sensor() # __lidar_feeds : collections.OrderedDict # Where int key is frame index and value # is a carla.LidarMeasurement or carla.SemanticLidarMeasurement self.__lidar_feeds = collections.OrderedDict() self.__local_planner = LocalPlanner(self.__ego_vehicle) self.__goal = util.AttrDict(x=50, y=0, is_relative=True)
def get_current(self): """Get reference and measurements after step has been taken step, and control is applied to vehicle. Returns ======= util.AttrDict Contains the following at current - measurement - reference - control : the last applied control. """ control = self.__vehicle.get_control() control = util.AttrDict( throttle=control.throttle, brake=control.brake, steer=control.steer ) error = util.AttrDict( speed=self.longitudinal_controller.get_current(), angle=self.lateral_controller.get_current() ) speed = carlautil.actor_to_speed(self.__vehicle) _, angle, _ = carlautil.to_rotation_ndarray(self.__vehicle) if not self.step_to_speed \ or self.__step_idx - 1 >= len(self.step_to_speed): return util.AttrDict( measurement=util.AttrDict(speed=speed, angle=angle), reference=util.AttrDict(speed=speed, angle=angle), error=error, control=control ) reference_speed = self.step_to_speed[self.__step_idx - 1] reference_angle = self.step_to_angle[self.__step_idx - 1] return util.AttrDict( measurement=util.AttrDict(speed=speed, angle=angle), reference=util.AttrDict( speed=reference_speed, angle=reference_angle, ), error=error, control=control )
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 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