def __plot_scenario(self, pred_result, ovehicles, params, ctrl_result): filename = f"agent{self.__ego_vehicle.id}_frame{params.frame}_lcss_control" if self.__agent_type == "oa": 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) elif self.__agent_type == "mcc": 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) elif self.__agent_type == "rmcc": pass else: raise NotImplementedError()
def __plot_simulation(self): if len(self.__plot_simulation_data.planned_trajectories) == 0: return if self.__agent_type == "oa": filename = f"agent{self.__ego_vehicle.id}_oa_simulation" lon, lat, _ = carlautil.actor_to_bbox_ndarray(self.__ego_vehicle) plot_oa_simulation( self.__scene_builder.get_scene(), self.__plot_simulation_data.actual_trajectory, self.__plot_simulation_data.planned_trajectories, self.__plot_simulation_data.planned_controls, self.__road_segs, [lon, lat], self.__control_horizon, filename=filename) elif self.__agent_type == "mcc": filename = f"agent{self.__ego_vehicle.id}_mcc_simulation" lon, lat, _ = carlautil.actor_to_bbox_ndarray(self.__ego_vehicle) plot_multiple_simulation( self.__scene_builder.get_scene(), self.__plot_simulation_data.actual_trajectory, self.__plot_simulation_data.planned_trajectories, self.__plot_simulation_data.planned_controls, self.__road_segs, [lon, lat], self.__control_horizon, filename=filename) else: raise NotImplementedError()
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 __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_scenario(self, pred_result, ovehicles, params, ctrl_result, error=None): lon, lat, _ = carlautil.actor_to_bbox_ndarray(self.__ego_vehicle) ego_bbox = np.array([lon, lat]) params.update(self.__params) if error: filename = f"agent{self.__ego_vehicle.id}_frame{params.frame}_mcc_fail" PlotPredictiveControl( pred_result, ovehicles, params, ctrl_result, self.__control_horizon, ego_bbox, T_coin=self.__n_coincide).plot_mcc_failure(filename=filename) else: filename = f"agent{self.__ego_vehicle.id}_frame{params.frame}_mcc_predict" PlotPredictiveControl( pred_result, ovehicles, params, ctrl_result, self.__control_horizon, ego_bbox, T_coin=self.__n_coincide).plot_mcc_prediction( filename=filename)
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_ovehicles(self, result): scene, timestep, nodes = result.scene, result.timestep, result.nodes predictions, latent_probs, z = result.predictions, result.latent_probs, result.z past_dict, ground_truth_dict = result.past_dict, result.ground_truth_dict """Preprocess predictions""" minpos = np.array([scene.x_min, scene.y_min]) ovehicles = [] for idx, node in enumerate(nodes): if node.id == 'ego': continue lon, lat, _ = carlautil.actor_to_bbox_ndarray( self.__other_vehicles[int(node.id)]) veh_bbox = [lon, lat] veh_gt = ground_truth_dict[timestep][node] + minpos veh_past = past_dict[timestep][node] + minpos veh_predict = predictions[idx] + minpos veh_latent_pmf = latent_probs[idx] n_states = veh_latent_pmf.size zn = z[idx] veh_latent_predictions = [[] for x in range(n_states)] for jdx, p in enumerate(veh_predict): veh_latent_predictions[zn[jdx]].append(p) for jdx in range(n_states): veh_latent_predictions[jdx] = np.array( veh_latent_predictions[jdx]) ovehicle = OVehicle.from_trajectron(node, self.__prediction_horizon, veh_gt, veh_past, veh_latent_pmf, veh_latent_predictions, bbox=veh_bbox) ovehicles.append(ovehicle) return ovehicles
def make_ovehicles(self, result): scene, timestep, nodes = result.scene, result.timestep, result.nodes predictions, latent_probs, z = result.predictions, result.latent_probs, result.z past_dict, ground_truth_dict = result.past_dict, result.ground_truth_dict """Preprocess predictions""" minpos = np.array([scene.x_min, scene.y_min]) ovehicles = [] for idx, node in enumerate(nodes): if node.id == "ego": lon, lat, _ = carlautil.actor_to_bbox_ndarray( self.__ego_vehicle) else: lon, lat, _ = carlautil.actor_to_bbox_ndarray( self.__other_vehicles[int(node.id)]) veh_bbox = np.array([lon, lat]) veh_gt = ground_truth_dict[timestep][node] + minpos veh_past = past_dict[timestep][node] + minpos veh_predict = predictions[idx] + minpos veh_latent_pmf = latent_probs[idx] n_states = veh_latent_pmf.size zn = z[idx] veh_latent_predictions = [[] for x in range(n_states)] for jdx, p in enumerate(veh_predict): veh_latent_predictions[zn[jdx]].append(p) for jdx in range(n_states): veh_latent_predictions[jdx] = np.array( veh_latent_predictions[jdx]) ovehicle = OVehicle.from_trajectron( node, self.__prediction_horizon, veh_gt, veh_past, veh_latent_pmf, veh_latent_predictions, bbox=veh_bbox, ) # if node.id != "ego": # vehicle = self.__other_vehicles[int(node.id)] # pos = carlautil.to_location_ndarray(vehicle, flip_y=True) # out = f"id: {node.id}, pos: {pos}, last: {ovehicle.past[-1]}, gt: {ovehicle.ground_truth}" # logging.info(out) ovehicles.append(ovehicle) return ovehicles
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_simulation(self): if len(self.__plot_simulation_data.planned_trajectories) == 0: return filename = f"agent{self.__ego_vehicle.id}_oa_simulation" lon, lat, _ = carlautil.actor_to_bbox_ndarray(self.__ego_vehicle) plot_oa_simulation(self.__scene_builder.get_scene(), self.__plot_simulation_data.actual_trajectory, self.__plot_simulation_data.planned_trajectories, self.__plot_simulation_data.planned_controls, [lon, lat], self.__control_horizon, filename=filename)
def __plot_scenario(self, pred_result, ovehicles, params, ctrl_result): if self.__agent_type == "oa": filename = f"agent{self.__ego_vehicle.id}_frame{params.frame}_lcss_control" 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)
def __plot_simulation(self): if len(self.__plot_simulation_data.planned_trajectories) == 0: return filename = f"agent{self.__ego_vehicle.id}_oa_simulation" lon, lat, _ = carlautil.actor_to_bbox_ndarray(self.__ego_vehicle) plot_oa_simulation( self.__map_reader.map_data, self.__plot_simulation_data.actual_trajectory, self.__plot_simulation_data.planned_trajectories, self.__plot_simulation_data.planned_controls, self.__plot_simulation_data.goals, self.__road_segs, [lon, lat], self.__step_horizon, filename=filename, road_boundary_constraints=self.road_boundary_constraints)
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 __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 __plot_scenario( self, pred_result, ovehicles, params, ctrl_result, error=None ): lon, lat, _ = carlautil.actor_to_bbox_ndarray(self.__ego_vehicle) ego_bbox = np.array([lon, lat]) params.update(self.__params) if error: filename = f"agent{self.__ego_vehicle.id}_frame{params.frame}_optim_fail" plot_predictive_control_failure( pred_result, ovehicles, params, ctrl_result, self.__control_horizon, ego_bbox, filename=filename ) else: filename = f"agent{self.__ego_vehicle.id}_frame{params.frame}_lcss_control" plot_lcss_prediction( pred_result, ovehicles, params, ctrl_result, self.__control_horizon, ego_bbox, filename=filename )
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 inspect_entity_dimensions(): config = parse_arguments() client = carla.Client(config.host, config.port) client.set_timeout(5.0) world = client.get_world() carla_map = world.get_map() spawn_point = carla_map.get_spawn_points()[0] # vehicle blueprints 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')] # walker blueprints walker_blueprints = world.get_blueprint_library().filter("walker.*") vehicle = None pedestrian = None spectator = world.get_spectator() logging = util.AttrDict( vehicle=util.AttrDict( xs=[], d2s=[], d3s=[] ), pedestrian=util.AttrDict( xs=[], d2s=[], d3s=[] ) ) for blueprint in blueprints: try: vehicle = world.spawn_actor(blueprint, spawn_point) world.wait_for_tick() transform = carlautil.spherical_to_camera_watcher_transform( 5, math.pi, math.pi*(1/6), pin=spawn_point.location) spectator.set_transform(transform) world.wait_for_tick() x = carlautil.actor_to_bbox_ndarray(vehicle) d2 = np.linalg.norm(x[:2] / 2) d3 = np.linalg.norm(x / 2) logging.vehicle.xs.append(x) logging.vehicle.d2s.append(d2) logging.vehicle.d3s.append(d3) print(blueprint.id) print(x) time.sleep(0.1) finally: if vehicle: vehicle.destroy() vehicle = None world.wait_for_tick() for blueprint in walker_blueprints: try: pedestrian = world.spawn_actor(blueprint, spawn_point) world.wait_for_tick() transform = carlautil.spherical_to_camera_watcher_transform( 5, math.pi, math.pi*(1/6), pin=spawn_point.location) spectator.set_transform(transform) world.wait_for_tick() x = carlautil.actor_to_bbox_ndarray(pedestrian) d2 = np.linalg.norm(x[:2] / 2) d3 = np.linalg.norm(x / 2) logging.pedestrian.xs.append(x) logging.pedestrian.d2s.append(d2) logging.pedestrian.d3s.append(d3) finally: if pedestrian: pedestrian.destroy() pedestrian = None for k, entity in logging.items(): entity.xs = np.stack(entity.xs) entity.d2s = np.stack(entity.d2s) entity.d3s = np.stack(entity.d3s) print(f"Vehicle dimensions { np.max(logging.vehicle.xs, 0) }") print(f" max 2D distance from origin { np.max(logging.vehicle.d2s) }") print(f" max 3D distance from origin { np.max(logging.vehicle.d3s) }") print(f"Pedestrian dimensions { np.mean(logging.pedestrian.xs, 0) }") print(f" max 2D distance from origin { np.max(logging.pedestrian.d2s) }") print(f" max 3D distance from origin { np.max(logging.pedestrian.d3s) }")