def __compute_prediction_controls(self, frame): params = self.make_local_params(frame) ctrl_result = self.do_highlevel_control(params) """use control input next round for warm starting.""" # self.U_warmstarting = ctrl_result.U_star """Get trajectory and velocity""" trajectory = [] velocity = [] X = np.concatenate( (params.initial_state.world[None], ctrl_result.X_star)) n_steps = X.shape[0] for t in range(1, n_steps): x, y, yaw = X[t, :3] y = -y # flip about x-axis again to move back to UE coordinates # 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]) yaw = np.rad2deg(util.reflect_radians_about_x_axis(yaw)) transform = carla.Transform(carla.Location(x=x, y=y), carla.Rotation(yaw=yaw)) trajectory.append(transform) velocity.append(X[t, 3]) if self.plot_simulation: """Save planned trajectory for final plotting""" self.__plot_simulation_data.planned_trajectories[frame] = X self.__plot_simulation_data.planned_controls[ frame] = ctrl_result.U_star self.__plot_simulation_data.goals[frame] = ctrl_result.goal return trajectory, velocity
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 compute_acceleration_constraints(self, u_x, u_y): """Accelaration control inputs have coupled constraints. Generate docplex constraints for velocity for double integrators. Present performance cars are capable of going from 0 to 60 mph in under 5 seconds. Reference: https://en.wikipedia.org/wiki/0_to_60_mph Parameters ========== u_x : np.array of docplex.mp.vartype.VarType u_y : np.array of docplex.mp.vartype.VarType """ _, theta, _ = carlautil.actor_to_rotation_ndarray(self.__ego_vehicle) theta = util.reflect_radians_about_x_axis(theta) r = -2.5 a_1 = 7.5 a_2 = 5.0 c1 = a_2*((u_x - r*np.cos(theta))*np.cos(theta) \ + (u_y - r*np.sin(theta))*np.sin(theta)) c2 = a_1*((u_y - r*np.sin(theta))*np.cos(theta) \ - (u_x - r*np.cos(theta))*np.sin(theta)) c3 = np.abs(a_1 * a_2) constraints = [] constraints.extend([z <= c3 for z in c1 + c2]) constraints.extend([z <= c3 for z in -c1 + c2]) constraints.extend([z <= c3 for z in c1 - c2]) constraints.extend([z <= c3 for z in -c1 - c2]) return constraints
def compute_velocity_constraints(self, v_x, v_y): """Velocity states have coupled constraints. Generate docplex constraints for velocity for double integrators. Street speed limit is 30 km/h == 8.33.. m/s Parameters ========== v_x : np.array of docplex.mp.vartype.VarType v_y : np.array of docplex.mp.vartype.VarType """ v_lim = self.__ego_vehicle.get_speed_limit() # is m/s _, theta, _ = carlautil.actor_to_rotation_ndarray(self.__ego_vehicle) theta = util.reflect_radians_about_x_axis(theta) r = v_lim / 2 v_1 = r v_2 = 0.75 * v_lim c1 = v_2*((v_x - r*np.cos(theta))*np.cos(theta) \ + (v_y - r*np.sin(theta))*np.sin(theta)) c2 = v_1*((v_y - r*np.sin(theta))*np.cos(theta) \ - (v_x - r*np.cos(theta))*np.sin(theta)) c3 = np.abs(v_1 * v_2) constraints = [] constraints.extend([z <= c3 for z in c1 + c2]) constraints.extend([z <= c3 for z in -c1 + c2]) constraints.extend([z <= c3 for z in c1 - c2]) constraints.extend([z <= c3 for z in -c1 - c2]) return constraints
def __build_scene_data(self): """Does post/mid collection processing of the data. This method copies the data and does processing on the copy if __scene_config is a SceneConfig, or directly processes the data if __scene_config is a OnlineConfig. Reflects all coordinates data about the x-axis in-place. This is needed because CARLA 0.9.11 uses a flipped y-axis. Returns ======= np.array Current overhead points collected by scene builder of shape (# points, 3) that has been flipped about the x-axis. pd.DataFrame Current trajectory data collected by scene builder with relevant data (position, velocity, acceleration, heading) flipped about the x-axis. """ scene_data = SceneBuilderData( self.__save_directory, self.__scene_name, self.__map_reader.map_name, self.__world.get_settings().fixed_delta_seconds, self.__trajectory_data, self.__overhead_points, self.__overhead_labels, self.__overhead_ids, self.__map_reader.map_data, self.__vehicle_visibility, self.__sensor_loc_at_t0, self.__first_frame, self.__scene_config, ) if self.is_online: """Copies the scene data. WARNING: map_data, scene_config are passed as reference.""" attr_names = [ "trajectory_data", "overhead_points", "overhead_labels", "overhead_ids", "sensor_loc_at_t0", "vehicle_visibility", ] for name in attr_names: setattr(scene_data, name, copy.deepcopy(getattr(scene_data, name))) """Reflect and return scene raw data.""" scene_data.overhead_points[:, 1] *= -1 scene_data.trajectory_data[["y", "v_y", "a_y"]] *= -1 scene_data.trajectory_data[ "heading"] = util.reflect_radians_about_x_axis( scene_data.trajectory_data["heading"]) """Sort Trajectory data""" scene_data.trajectory_data.sort_values("frame_id", inplace=True) return scene_data
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 __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 __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) """Get trajectory""" # X has shape (T+1, nx) X = np.concatenate( (ctrl_result.start[None], ctrl_result.X_star[0, :, :2])) trajectory = [] for t in range(1, self.__n_coincide + 1): 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]) # 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.log_agent: logging.info( f"Optimized {params.N_select}/{params.N_traj} trajectories avoiding {params.O} vehicles." ) logging.info( f"Selected trajectory indices are: {params.subtraj_indices}") if self.plot_scenario: """Plot scenario""" filename = f"agent{self.__ego_vehicle.id}_frame{frame}_lcss_control" lon, lat, _ = carlautil.actor_to_bbox_ndarray(self.__ego_vehicle) ego_bbox = [lon, lat] params.update(self.__params) plot_multiple_coinciding_controls(pred_result, ovehicles, params, ctrl_result, ego_bbox, filename=filename) return trajectory
def __compute_prediction_controls(self, frame): pred_result = self.do_prediction(frame) ovehicles = self.make_ovehicles(pred_result) params = self.make_local_params(frame, 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 = [] velocity = [] X = np.concatenate((params.x0[None], ctrl_result.X_star)) n_steps = X.shape[0] headings = [] for t in range(1, n_steps): x, y = X[t, :2] 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 = np.rad2deg(util.reflect_radians_about_x_axis(yaw)) transform = carla.Transform(carla.Location(x=x, y=y), carla.Rotation(yaw=yaw)) trajectory.append(transform) velocity.append(math.sqrt(X[t, 2]**2 + X[t, 3]**2)) if self.plot_scenario: """Plot scenario""" ctrl_result.headings = headings self.__plot_scenario(pred_result, ovehicles, params, ctrl_result) if self.plot_simulation: """Save planned trajectory for final plotting""" if self.__agent_type == "oa": 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 else: raise NotImplementedError() return trajectory, velocity
def __init__(self, ego_vehicle, map_reader, other_vehicle_ids, eval_stg, control_horizon=6, n_burn_interval=4, prediction_horizon=8, n_predictions=100, n_coincide=6, scene_builder_cls=TrajectronPlusPlusSceneBuilder, scene_config=OnlineConfig(), log_cplex=False, log_agent=False, plot_scenario=False, plot_boundary=False, plot_vertices=False, plot_overapprox=False, **kwargs): assert n_coincide <= prediction_horizon assert control_horizon <= n_coincide 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.__scene_builder_cls = scene_builder_cls # __control_horizon : int # Number of predictions timesteps to conduct control over. self.__control_horizon = control_horizon # __n_burn_interval : int # Interval in prediction timesteps to skip prediction and control. self.__n_burn_interval = n_burn_interval # __prediction_horizon : int # Number of predictions timesteps to predict other vehicles over. self.__prediction_horizon = prediction_horizon self.__n_predictions = n_predictions self.__n_coincide = n_coincide 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.__prediction_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.__goal = util.AttrDict(x=50, y=0, is_relative=True) # __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 self.log_cplex = log_cplex self.log_agent = log_agent self.plot_scenario = plot_scenario self.plot_boundary = plot_boundary self.plot_vertices = plot_vertices self.plot_overapprox = plot_overapprox
def __compute_prediction_controls(self, frame): pred_result = self.do_prediction(frame) ovehicles = self.make_ovehicles(pred_result) params = self.make_local_params(frame, ovehicles) if self.__agent_type == "oa": ctrl_result = self.do_single_control(params, ovehicles) elif self.__agent_type == "mcc" or self.__agent_type == "rmcc": ctrl_result = self.do_multiple_control(params, ovehicles) else: raise NotImplementedError() """use control input next round for warm starting.""" # self.U_warmstarting = ctrl_result.U_star """Get trajectory""" trajectory = [] velocity = [] # X has shape (T+1, nx) if self.__agent_type == "oa": X = np.concatenate((params.x0[None], ctrl_result.X_star)) n_steps = X.shape[0] elif self.__agent_type == "mcc" or self.__agent_type == "rmcc": X = np.concatenate((params.x0[None], ctrl_result.X_star[0])) n_steps = self.__n_coincide + 1 if self.log_agent: logging.info(f"Optimized {params.N_traj} " f"trajectories avoiding {params.O} vehicles.") else: raise NotImplementedError() headings = [] for t in range(1, n_steps): x, y = X[t, :2] 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 = np.rad2deg(util.reflect_radians_about_x_axis(yaw)) transform = carla.Transform(carla.Location(x=x, y=y), carla.Rotation(yaw=yaw)) trajectory.append(transform) velocity.append(math.sqrt(X[t, 2]**2 + X[t, 3]**2)) if self.plot_scenario: """Plot scenario""" ctrl_result.headings = headings self.__plot_scenario(pred_result, ovehicles, params, ctrl_result) if self.plot_simulation: """Save planned trajectory for final plotting""" if self.__agent_type == "oa": 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 elif self.__agent_type == "mcc" or self.__agent_type == "rmcc": N_select = params.N_select if self.__agent_type == "rmcc" else params.N_traj self.__plot_simulation_data.planned_trajectories[ frame] = np.concatenate( (np.repeat(params.x0[None], N_select, axis=0)[:, None], ctrl_result.X_star), axis=1) self.__plot_simulation_data.planned_controls[ frame] = ctrl_result.U_star else: pass return trajectory, velocity
def __init__( self, ego_vehicle, map_reader, other_vehicle_ids, eval_stg, n_burn_interval=4, control_horizon=6, prediction_horizon=8, n_predictions=100, scene_builder_cls=TrajectronPlusPlusSceneBuilder, scene_config=OnlineConfig(), ######################### # Controller type setting agent_type="oa", n_coincide=6, ####################### # Logging and debugging log_cplex=False, log_agent=False, plot_scenario=False, plot_simulation=False, plot_boundary=False, plot_vertices=False, plot_overapprox=False, ####################### # Planned path settings turn_choices=[], max_distance=100, ####################### **kwargs): self.__ego_vehicle = ego_vehicle self.__map_reader = map_reader self.__eval_stg = eval_stg # __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 predictions timesteps to conduct control over. self.__control_horizon = control_horizon # __prediction_horizon : int # Number of predictions timesteps to predict other vehicles over. self.__prediction_horizon = prediction_horizon # __n_predictions : int # Number of predictions to generate on each control step. self.__n_predictions = n_predictions self.__scene_builder_cls = scene_builder_cls self.__scene_config = scene_config if agent_type == "oa": assert control_horizon <= prediction_horizon elif agent_type == "mcc" or agent_type == "rmcc": assert n_coincide <= prediction_horizon assert control_horizon <= n_coincide else: raise ValueError(f"Agent type {agent_type} is not recognized.") self.__agent_type = agent_type # __n_coincide : int # Number of steps in motion plan that coincide. # Used for multiple coinciding control. self.__n_coincide = n_coincide # __first_frame : int # First frame in simulation. Used to find current timestep. self.__first_frame = None self.__scene_builder = None self.__world = self.__ego_vehicle.get_world() 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.__prediction_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() #################################################### # rectangular boundary constraints are not used here #################################################### # __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 ############################################ # curved road segmented boundary constraints ############################################ # __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 # Goal destination the vehicle should navigates to. # Not used for motion planning. self.__goal = util.AttrDict(x=x, y=y, is_relative=False) self.log_cplex = log_cplex self.log_agent = log_agent self.plot_scenario = plot_scenario self.plot_simulation = plot_simulation self.plot_boundary = plot_boundary self.plot_vertices = plot_vertices self.plot_overapprox = plot_overapprox if self.plot_simulation: self.__plot_simulation_data = util.AttrDict( actual_trajectory=collections.OrderedDict(), planned_trajectories=collections.OrderedDict(), planned_controls=collections.OrderedDict())