def perform_action(sim, agent_id, vehicle, action, controller_state, sensor_state, action_space): if action is None: return if action_space == ActionSpaceType.Continuous: vehicle.control( throttle=np.clip(action[0], 0.0, 1.0), brake=np.clip(action[1], 0.0, 1.0), steering=np.clip(action[2], -1, 1), ) elif action_space == ActionSpaceType.ActuatorDynamic: ActuatorDynamicController.perform_action(vehicle, action, controller_state, dt_sec=sim.timestep_sec) elif action_space == ActionSpaceType.Trajectory: TrajectoryTrackingController.perform_trajectory_tracking_PD( action, vehicle, controller_state, dt_sec=sim.timestep_sec, ) elif action_space == ActionSpaceType.MPC: TrajectoryTrackingController.perform_trajectory_tracking_MPC( action, vehicle, controller_state, sim.timestep_sec) elif action_space == ActionSpaceType.LaneWithContinuousSpeed: LaneFollowingController.perform_lane_following( sim, agent_id, vehicle, controller_state, sensor_state, action[0], action[1], ) elif action_space == ActionSpaceType.Lane: perform_lane_following = partial( LaneFollowingController.perform_lane_following, sim=sim, agent_id=agent_id, vehicle=vehicle, controller_state=controller_state, sensor_state=sensor_state, ) # 12.5 m/s (45 km/h) is used as the nominal speed for lane change. # For keep_lane, the nominal speed is set to 15 m/s (54 km/h). if action == "keep_lane": perform_lane_following(target_speed=15, lane_change=0) elif action == "slow_down": perform_lane_following(target_speed=0, lane_change=0) elif action == "change_lane_left": perform_lane_following(target_speed=12.5, lane_change=1) elif action == "change_lane_right": perform_lane_following(target_speed=12.5, lane_change=-1) else: raise ValueError( f"perform_action(action_space={action_space}, ...) has failed " "inside controller")
def act(self, obs): ego = obs.ego_vehicle_state lane_index = min(len(obs.waypoint_paths) - 1, ego.lane_index) wps = obs.waypoint_paths[lane_index] for wp_path in obs.waypoint_paths: if wp_path[0].lane_index == lane_index: wps = wp_path break # wps = min( # obs.waypoint_paths, # key=lambda wps: abs(wps[0].signed_lateral_error(ego.position)), # ) num_trajectory_points = len(wps) trajectory = [ [wps[i].pos[0] for i in range(num_trajectory_points)], [wps[i].pos[1] for i in range(num_trajectory_points)], [wps[i].heading for i in range(num_trajectory_points)], ] curvature = abs( TrajectoryTrackingController.curvature_calculation(trajectory, num_points=10)) gains = { "theta": 3.0 * 0.05, "position": 1.0 * 100, "obstacle": 0.0, "u_accel": 0.01, "u_yaw_rate": 0.1, "terminal": 0.01, "impatience": 0.0, "speed": 0.5 * 100, "rate": 1 * 10, } self.gain = Gain(**gains) if curvature < 100: gains = { "theta": 16 * 0.05, "position": 1 * 120, "obstacle": 3.0, "u_accel": 0.01, "u_yaw_rate": 0.1, "terminal": 0.0001, "impatience": 0.01, "speed": 0.5 * 100, "rate": 0.1 * 1, } self.gain = Gain(**gains) if curvature < 10: gains = { "theta": 1 * 5, "position": 40 * 6, "obstacle": 3.0, "u_accel": 0.1 * 1, "u_yaw_rate": 0.01, "terminal": 0.01, "impatience": 0.01, "speed": 0.3 * 400, "rate": 0.001, } self.gain = Gain(**gains) if curvature > 200: gains = { "theta": 0.1 * 0.05, "position": 1 * 10, "obstacle": 3.0, "u_accel": 0.1, "u_yaw_rate": 0.1, "terminal": 0.01, "impatience": 0.01, "speed": 0.5 * 250 * 2, "rate": 10, } self.gain = Gain(**gains) wps = wps[:self.WP_N] # repeat the last waypoint to fill out self.WP_N waypoints wps += [wps[-1]] * (self.WP_N - len(wps)) flat_wps = [ wp_param for wp in wps for wp_param in [wp.pos[0], wp.pos[1], float(wp.heading) + math.pi * 0.5] ] if self.SV_N == 0: flat_svs = [] elif len(obs.neighborhood_vehicle_states) == 0 and self.SV_N > 0: # We have no social vehicles in the scene, create placeholders far away flat_svs = [ ego.position[0] + 100000, ego.position[1] + 100000, 0, 0, ] * self.SV_N else: # Give the closest SV_N social vehicles to the planner social_vehicles = sorted( obs.neighborhood_vehicle_states, key=lambda sv: np.linalg.norm(sv.position - ego.position), )[:self.SV_N] # repeat the last social vehicle to ensure SV_N social vehicles social_vehicles += [social_vehicles[-1] ] * (self.SV_N - len(social_vehicles)) flat_svs = [ sv_param for sv in social_vehicles for sv_param in [ sv.position[0], sv.position[1], float(sv.heading) + math.pi * 0.5, sv.speed, ] ] ego_params = [ ego.position[0], ego.position[1], float(ego.heading) + math.pi * 0.5, ego.speed, ] impatience = self.update_impatience(ego) solver_params = (list(self.gain) + ego_params + flat_svs + flat_wps + [impatience, wps[0].speed_limit]) neutral_bias = [0 for i in range(12)] resp = self.solver.run(solver_params, initial_guess=neutral_bias) self.last_position = ego.position if resp is not None: u_star = resp.solution self.prev_solution = u_star ego_model = VehicleModel(*ego_params) xs = [] ys = [] headings = [] speeds = [] for u in zip(u_star[::2], u_star[1::2]): ego_model.step(U(*u), self.ts) headings.append(Heading(ego_model.theta - math.pi * 0.5)) xs.append(ego_model.x) ys.append(ego_model.y) speeds.append(ego_model.speed) traj = [xs, ys, headings, speeds] if self.debug: self._draw_debug_panel(xs, ys, wps, flat_svs, ego, u_star) return traj else: # Failed to find a solution. # re-init the planner and stay still, hopefully once we've re-initialized, we can recover self.init_planner() return None
def perform_lane_following( cls, sim, agent_id, vehicle, controller_state, sensor_state, target_speed=12.5, lane_change=0, ): assert isinstance(vehicle.chassis, AckermannChassis) state = controller_state # This lookahead value is coupled with a few calculations below, changing it # may affect stability of the controller. print(f"start wp_paths") wp_paths = sensor_state.mission_planner.waypoint_paths_at(sim, vehicle.pose, lookahead=30) print(f"in l_f_c, wps {wp_paths}") current_lane = LaneFollowingController.find_current_lane( wp_paths, vehicle.position) wp_path = wp_paths[np.clip(current_lane + lane_change, 0, len(wp_paths) - 1)] # we compute a road "curviness" to inform our throttle activation. # We should move slowly when we are on curvy roads. ewma_road_curviness = 0.0 for wp_a, wp_b in reversed(list(zip(wp_path, wp_path[1:]))): ewma_road_curviness = lerp( ewma_road_curviness, math.degrees(abs(wp_a.relative_heading(wp_b.heading))), 0.03, ) # print(f"[wp] point is {wp_a}") road_curviness_normalization = 2.5 road_curviness = np.clip( ewma_road_curviness / road_curviness_normalization, 0, 1) # Number of trajectory point used for curvature calculation. num_trajectory_points = min([10, len(wp_path)]) trajectory = [ [wp_path[i].pos[0] for i in range(num_trajectory_points)], [wp_path[i].pos[1] for i in range(num_trajectory_points)], [wp_path[i].heading for i in range(num_trajectory_points)], ] # The following calculates the radius of curvature for the 4th # waypoints in the waypoint list. Value 4 is chosen to ensure # that the heading error correction is triggered before the vehicle # reaches to a sharp turn defined be min_curvature. look_ahead_curvature = abs( TrajectoryTrackingController.curvature_calculation(trajectory, 4)) # Minimum curvature limit for pushing forward the waypoint # which is used for heading error calculation. min_curvature = 2 # If the look_ahead_curvature is less than the min_curvature, then # update the location of the points which its curvature is less than # min_curvature. if look_ahead_curvature <= min_curvature: state.min_curvature_location = (wp_path[4].pos[0], wp_path[4].pos[1]) # LOOK AHEAD ERROR SETTING # look_ahead_wp_num is the ahead waypoint which is used to # calculate the lateral error TODO: use adaptive setting to # choose the ahead waypoint # if the road_curviness is high(i.e. > 0.5), we reduce the # look_ahead_wp_num to calculate a more accurate lateral error # normal look ahead distant is set to 8 meters which is reduced # to 6 meters when the curvature increases. # Note: waypoints are spaced at roughly 1 meter apart if road_curviness > 0.5: look_ahead_wp_num = 3 else: look_ahead_wp_num = 4 look_ahead_wp_num = min(look_ahead_wp_num, len(wp_path) - 1) reference_heading = wp_path[0].heading look_ahead_wp = wp_path[look_ahead_wp_num] look_ahead_dist = look_ahead_wp.dist_to(vehicle.position) vehicle_look_ahead_pt = [ vehicle.position[0] - look_ahead_dist * math.sin(vehicle.heading), vehicle.position[1] + look_ahead_dist * math.cos(vehicle.heading), ] # 5.56 m/s (20 km/h), 6.94 m/s (25 km/h) are desired speed for different thresholds # for road curviness # 0.5 , 0.8 are dimensionless thresholds for road_curviness. # 1.8 and 0.6 are the longitudinal velocity controller # proportional gains for different road curvinesss. if road_curviness < 0.3: raw_throttle = (-METER_PER_SECOND_TO_KM_PER_HR * 1.8 * (vehicle.speed - target_speed)) elif road_curviness > 0.3 and road_curviness < 0.8: raw_throttle = (-0.6 * METER_PER_SECOND_TO_KM_PER_HR * (vehicle.speed - np.clip(target_speed, 0, 6.94))) else: raw_throttle = (-0.6 * METER_PER_SECOND_TO_KM_PER_HR * (vehicle.speed - np.clip(target_speed, 0, 5.56))) speed_error = vehicle.speed - target_speed state.integral_speed_error += speed_error * sim.timestep_sec velocity_error_damping_term = (speed_error - state.speed_error) / sim.timestep_sec # 0.2 is the coefficent of d-controller for speed tracking # 0.1 is the coefficent of I-controller for speed tracking # 5.5 is the gain of feedforward term. This term is directly # related to the steering angle, this is added to further enhance # the speed tracking performance. raw_throttle += ( -0.2 * velocity_error_damping_term - 0.1 * state.integral_speed_error + abs(5.5 * math.sin(state.steering_state * vehicle.max_steering_wheel))) state.speed_error = speed_error # If the distance of the vehicle to the ahead point for which # the waypoint curvature is less than min_curvature is less than # 2 meters, then push forward the waypoint which is used to # calculate the heading error. if (state.min_curvature_location != (None, None)) and math.sqrt( (vehicle.position[0] - state.min_curvature_location[0])**2 + (vehicle.position[1] - state.min_curvature_location[1])**2) < 2: reference_heading = wp_path[look_ahead_wp_num].heading # Desired closed loop poles of the lateral dynamics # The higher the absolute value, the closed loop response will # be faster for that state, the four states of that are used for # Linearization of the lateral dynamics are: # [lateral error, heading error, yaw_rate, side_slip angle] desired_poles = np.array([ cls.lateral_error, cls.heading_error, cls.yaw_rate, cls.side_slip_angle, ]) LaneFollowingController.calculate_lateral_gains( sim, state, vehicle, desired_poles, target_speed) # LOOK AHEAD CONTROLLER controller_lat_error = wp_path[look_ahead_wp_num].signed_lateral_error( vehicle_look_ahead_pt) abs_heading_error = min( abs((vehicle.heading % (2 * math.pi)) - reference_heading), abs(2 * math.pi - abs((vehicle.heading % (2 * math.pi)) - reference_heading)), ) curvature_radius = TrajectoryTrackingController.curvature_calculation( trajectory) brake_norm = 0 if raw_throttle < 0: brake_norm = np.clip(-raw_throttle, 0, 1) throttle_norm = 0 else: # The term involving absolute value of the lateral speed is # added as a traction control strategy, The traction controller # gain is set to 4.5, the lower the value, the vehicle becomes # more agile but may result in instability in harsh curves # with high speeds. if vehicle.speed > 70 / 3.6 and abs(curvature_radius) <= 1e3: traction_gain = 4.5 elif 40 / 3.6 <= vehicle.speed <= 70 / 3.6 and abs( curvature_radius) <= 3: traction_gain = 2.5 else: traction_gain = 0.5 throttle_norm = np.clip( raw_throttle - traction_gain * METER_PER_SECOND_TO_KM_PER_HR * abs(vehicle.chassis.longitudinal_lateral_speed[1]), 0, 1, ) # The feedback term involving yaw rate is added to reduce # the oscillation in vehicle heading, the proportional gain for # yaw rate is set to 2.75, the higher value results in less # oscillation in heading angle. 0.3 is the integral controller # gain for lateral error. The feedforward term based on the # curvature is added to enhance the transient performance when # the road curvature changes locally. state.lateral_integral_error += sim.timestep_sec * controller_lat_error # The feed forward term for the steering controller. This # term is proportionate to Ux^2/R. The coefficient 0.15 is # chosen to enhance the transient tracking performance. # This coefficient also depends on the inertia properties # and the cornering stiffness of the tires. See: # https://www.tandfonline.com/doi/full/10.1080/00423114.2015.1055279 steering_feed_forward_gain = 0.15 if abs(curvature_radius) < 7: steering_feed_forward_gain = 0.45 steering_controller_feed_forward = (1 * steering_feed_forward_gain * (1 / curvature_radius) * (vehicle.speed)**2) normalized_speed = np.clip(vehicle.speed * 3.6 / 100, 0, 1) heading_speed_gain = lerp(0.5, 14, normalized_speed) yaw_rate_speed_gain = lerp(5.75, 11.75, normalized_speed) lateral_speed_gain = np.clip(lerp(-1, 14, normalized_speed), 1, 2) steering_norm = np.clip( -heading_speed_gain * math.degrees(state.heading_error_gain) * (abs_heading_error * np.sign(reference_heading - (vehicle.heading % (2 * math.pi)))) + lateral_speed_gain * state.lateral_error_gain * (controller_lat_error) + yaw_rate_speed_gain * vehicle.chassis.yaw_rate[2] + 0.3 * state.lateral_integral_error - steering_controller_feed_forward, -1, 1, ) # The steering low pass filter, 5.5 is the constant of the # first order linear low pass filter. steering_filter_constant = 5.5 state.steering_state = low_pass_filter( steering_norm, state.steering_state, steering_filter_constant, sim.timestep_sec, ) # The Throttle low pass filter, 2 is the constant of the # first order linear low pass filter. # TODO: Add low pass filter for brake. throttle_filter_constant = 2 state.throttle_state = low_pass_filter( throttle_norm, state.throttle_state, throttle_filter_constant, sim.timestep_sec, lower_bound=0, ) # Applying control actions to the vehicle vehicle.control( throttle=state.throttle_state, brake=brake_norm, steering=state.steering_state, ) LaneFollowingController._update_target_lane_if_reached_end_of_lane( agent_id, vehicle, controller_state, sensor_state)
def perform_action( sim, agent_id, vehicle, action, controller_state, sensor_state, action_space, vehicle_type, ): print("in controller") if action is None: return if vehicle_type == "bus": assert action_space == ActionSpaceType.Trajectory if action_space == ActionSpaceType.Continuous: vehicle.control( throttle=np.clip(action[0], 0.0, 1.0), brake=np.clip(action[1], 0.0, 1.0), steering=np.clip(action[2], -1, 1), ) elif action_space == ActionSpaceType.ActuatorDynamic: ActuatorDynamicController.perform_action(vehicle, action, controller_state, dt_sec=sim.timestep_sec) elif action_space == ActionSpaceType.Trajectory: TrajectoryTrackingController.perform_trajectory_tracking_PD( action, vehicle, controller_state, dt_sec=sim.timestep_sec, ) elif action_space == ActionSpaceType.MPC: TrajectoryTrackingController.perform_trajectory_tracking_MPC( action, vehicle, controller_state, sim.timestep_sec) elif action_space == ActionSpaceType.LaneWithContinuousSpeed: change_lane_set = [-1, 0, 1] assert action[1] in change_lane_set, "target lane is not valid" print(f"in Controllers: speed {action[0]}; lane {action[1]}") LaneFollowingController.perform_lane_following( sim, agent_id, vehicle, controller_state, sensor_state, action[0], int(action[1]), ) elif action_space == ActionSpaceType.AnchorPoint: # TODO: check action (AnchorPoint) is valid # assert action is change_lane_set, "target lane is not valid" target_speed = 15 LaneFollowingController.perform_lane_following_with_anchor( sim, agent_id, vehicle, controller_state, sensor_state, target_speed, action, ) elif action_space == ActionSpaceType.Lane: print("in controller-dis") perform_lane_following = partial( LaneFollowingController.perform_lane_following, sim=sim, agent_id=agent_id, vehicle=vehicle, controller_state=controller_state, sensor_state=sensor_state, ) # 12.5 m/s (45 km/h) is used as the nominal speed for lane change. # For keep_lane, the nominal speed is set to 15 m/s (54 km/h). if action == "keep_lane": perform_lane_following(target_speed=15, lane_change=0) elif action == "slow_down": perform_lane_following(target_speed=0, lane_change=0) elif action == "change_lane_left": perform_lane_following(target_speed=12.5, lane_change=1) elif action == "change_lane_right": perform_lane_following(target_speed=12.5, lane_change=-1) # # 20.0 m/s (72 km/h) is used as the nominal speed for lane change. # # For keep_lane, the nominal speed is set to 33.3 m/s (120 km/h). # if action == "keep_lane": # perform_lane_following(target_speed=33.3, lane_change=0) # elif action == "slow_down": # perform_lane_following(target_speed=0, lane_change=0) # elif action == "change_lane_left": # perform_lane_following(target_speed=20.0, lane_change=1) # elif action == "change_lane_right": # perform_lane_following(target_speed=20.0, lane_change=-1) else: raise ValueError( f"perform_action(action_space={action_space}, ...) has failed " "inside controller")
def perform_action( sim, agent_id, vehicle, action, controller_state, sensor_state, action_space, vehicle_type, ): """Calls control for the given vehicle based on a given action space and action. Args: sim: A simulation instance. agent_id: An agent within the simulation that is associated with a vehicle. vehicle: A vehicle within the simulation that is associated with an agent. action: The action for the controller to perform. controller_state: The last vehicle controller state as relates to its action space. sensor_state: The state of a vehicle sensor as relates to vehicle sensors. action_space: The action space of the provided action. vehicle_type: Vehicle type information about the given vehicle. """ if action is None: return if vehicle_type == "bus": assert action_space == ActionSpaceType.Trajectory if action_space == ActionSpaceType.Continuous: vehicle.control( throttle=np.clip(action[0], 0.0, 1.0), brake=np.clip(action[1], 0.0, 1.0), steering=np.clip(action[2], -1, 1), ) elif action_space == ActionSpaceType.ActuatorDynamic: ActuatorDynamicController.perform_action(vehicle, action, controller_state, dt_sec=sim.last_dt) elif action_space == ActionSpaceType.Trajectory: TrajectoryTrackingController.perform_trajectory_tracking_PD( action, vehicle, controller_state, dt_sec=sim.last_dt, ) elif action_space == ActionSpaceType.MPC: TrajectoryTrackingController.perform_trajectory_tracking_MPC( action, vehicle, controller_state, sim.last_dt) elif action_space == ActionSpaceType.LaneWithContinuousSpeed: LaneFollowingController.perform_lane_following( sim, agent_id, vehicle, controller_state, sensor_state, action[0], action[1], ) elif action_space == ActionSpaceType.Lane: perform_lane_following = partial( LaneFollowingController.perform_lane_following, sim=sim, agent_id=agent_id, vehicle=vehicle, controller_state=controller_state, sensor_state=sensor_state, ) # 12.5 m/s (45 km/h) is used as the nominal speed for lane change. # For keep_lane, the nominal speed is set to 15 m/s (54 km/h). if action == "keep_lane": perform_lane_following(target_speed=15, lane_change=0) elif action == "slow_down": perform_lane_following(target_speed=0, lane_change=0) elif action == "change_lane_left": perform_lane_following(target_speed=12.5, lane_change=1) elif action == "change_lane_right": perform_lane_following(target_speed=12.5, lane_change=-1) elif action_space == ActionSpaceType.Imitation: ImitationController.perform_action(sim.last_dt, vehicle, action) else: # Note: TargetPose and MultiTargetPose use a MotionPlannerProvider directly raise ValueError( f"perform_action(action_space={action_space}, ...) has failed " "inside controller")