def run_in_series(self, next_waypoint: Transform, **kwargs) -> VehicleControl: """ Execute one step of control invoking both lateral and longitudinal PID controllers to reach a target waypoint at a given target_speed. Args: vehicle: New vehicle state next_waypoint: target location encoded as a waypoint **kwargs: Returns: Next Vehicle Control """ curr_speed = Vehicle.get_speed(self.agent.vehicle) if curr_speed < 60: self._lat_controller.k_d = OPTIMIZED_LATERAL_PID_VALUES[60].K_D self._lat_controller.k_i = OPTIMIZED_LATERAL_PID_VALUES[60].K_I self._lat_controller.k_p = OPTIMIZED_LATERAL_PID_VALUES[60].K_P elif curr_speed < 100: self._lat_controller.k_d = OPTIMIZED_LATERAL_PID_VALUES[100].K_D self._lat_controller.k_i = OPTIMIZED_LATERAL_PID_VALUES[100].K_I self._lat_controller.k_p = OPTIMIZED_LATERAL_PID_VALUES[100].K_P elif curr_speed < 150: self._lat_controller.k_d = OPTIMIZED_LATERAL_PID_VALUES[150].K_D self._lat_controller.k_i = OPTIMIZED_LATERAL_PID_VALUES[150].K_I self._lat_controller.k_p = OPTIMIZED_LATERAL_PID_VALUES[150].K_P acceptable_target_speed = self.target_speed if abs(self.agent.vehicle.control.steering) < 0.05: acceptable_target_speed += 20 # eco boost acceleration = self._lon_controller.run_step(acceptable_target_speed) current_steering = self._lat_controller.run_step(next_waypoint) control = VehicleControl() if acceleration >= 0.0: control.throttle = min(acceleration, self.max_throttle) # control.brake = 0.0 else: control.throttle = 0 # control.brake = min(abs(acceleration), self.max_brake) # Steering regulation: changes cannot happen abruptly, can't steer too much. if current_steering > self.past_steering + 0.1: current_steering = self.past_steering + 0.1 elif current_steering < self.past_steering - 0.1: current_steering = self.past_steering - 0.1 if current_steering >= 0: steering = min(self.max_steer, current_steering) else: steering = max(-self.max_steer, current_steering) if abs(current_steering) > 0.03 and curr_speed > 110: # if i am doing a sharp (>0.5) turn, i do not want to step on full gas control.throttle = -1 control.steering = steering self.past_steering = steering return control
def run_in_series(self, next_waypoint: Transform, **kwargs) -> VehicleControl: """ Execute one step of control invoking both lateral and longitudinal PID controllers to reach a target waypoint at a given target_speed. Args: vehicle: New vehicle state next_waypoint: target location encoded as a waypoint **kwargs: Returns: Next Vehicle Control """ self.update_lon_control_k_values() self.update_lat_control_k_values() acceleration = self._lon_controller.run_step(self.target_speed) current_steering = self._lat_controller.run_step(next_waypoint) control = VehicleControl() if acceleration >= 0.0: control.throttle = min(acceleration, self.max_throttle) # control.brake = 0.0 else: control.throttle = 0 # control.brake = min(abs(acceleration), self.max_brake) # Steering regulation: changes cannot happen abruptly, can't steer too much. if current_steering > self.past_steering + 0.1: current_steering = self.past_steering + 0.1 elif current_steering < self.past_steering - 0.1: current_steering = self.past_steering - 0.1 if current_steering >= 0: steering = min(self.max_steer, current_steering) else: steering = max(-self.max_steer, current_steering) control.steering = steering self.past_steering = steering abs_throttle = 0.2 return self.throttle_regularization(control=control, min_throttle=-abs_throttle, max_throttle=abs_throttle)
def long_pid_control(self, next_waypoint: Transform, control: VehicleControl): dist_to_car = next_waypoint.location.z if dist_to_car < self.distance_to_keep: self.logger.info("TOO CLOSE BRAKING!") control.brake = True control.throttle = 0 else: error = dist_to_car - self.distance_to_keep error_dt = 0 if len(self.long_error_queue ) == 0 else error - self.long_error_queue[-1] self.long_error_queue.append(error) error_it = sum(self.long_error_queue) e_p = self.lon_kp * error e_d = self.lon_kd * error_dt e_i = self.lon_ki * error_it long_control = np.clip(e_p + e_d + e_i, -1, self.max_throttle) control.throttle = long_control
def run_step(self, sensors_data: SensorsData, vehicle: Vehicle) -> VehicleControl: super(DepthE2EAgent, self).run_step(sensors_data, vehicle) control = VehicleControl(throttle=0.5, steering=0) if self.front_depth_camera.data is not None: depth_image = self.front_depth_camera.data.copy() data = np.expand_dims(np.expand_dims(depth_image, 2), 0) output = self.model.predict(data) throttle, steering = output[0] control.throttle, control.steering = float(throttle), float( steering) return control
def long_pid_control(self, next_waypoint, control: VehicleControl): self_point = self.agent.vehicle.transform.to_array() target_point = next_waypoint.location.to_array() x_diff = target_point[0] - self_point[0] y_diff = target_point[1] - self_point[1] z_diff = target_point[2] - self_point[2] dist_to_car = math.sqrt(x_diff * x_diff + z_diff * z_diff) if dist_to_car < self.distance_to_keep: self.logger.info( f"TOO CLOSE BRAKING! dist_to_car: {dist_to_car} < self.distance_to_keep:{self.distance_to_keep}" ) control.brake = True control.throttle = 0 else: error = dist_to_car - self.distance_to_keep error_dt = 0 if len(self.long_error_queue ) == 0 else error - self.long_error_queue[-1] self.long_error_queue.append(error) error_it = sum(self.long_error_queue) e_p = self.lon_kp * error e_d = self.lon_kd * error_dt e_i = self.lon_ki * error_it long_control = np.clip(e_p + e_d + e_i, -1, self.max_throttle) control.throttle = long_control
def smoothen_control(self, control: VehicleControl): if abs(control.throttle) > abs(self.prev_control.throttle ) and self.prev_control.throttle > 0.15: # ensure slower increase, faster decrease. 0.15 barely drives the car control.throttle = (self.prev_control.throttle * self.throttle_smoothen_factor + control.throttle) / \ (self.throttle_smoothen_factor + 1) if abs(control.steering) < abs(self.prev_control.steering): # slowly turn back control.steering = ( self.prev_control.steering * self.steering_smoothen_factor_backward + control.steering) / \ (self.steering_smoothen_factor_backward + 1) elif abs(control.steering) < abs(self.prev_control.steering): control.steering = (self.prev_control.steering * self.steering_smoothen_factor_forward + control.steering) / \ (self.steering_smoothen_factor_backward + 1) self.prev_control = control return control
def run_in_series(self, next_waypoint: Transform) -> VehicleControl: super(VehicleMPCController, self).run_in_series(next_waypoint) # get vehicle location (x, y) # location = self.vehicle.transform.location location = self.agent.vehicle.control.location x, y = location.x, location.y # get vehicle rotation # rotation = self.vehicle.transform.rotation rotation = self.agent.vehicle.control.rotation ψ = rotation.yaw / 180 * np.pi # transform into radient cos_ψ = np.cos(ψ) sin_ψ = np.sin(ψ) # get vehicle speed # v = Vehicle.get_speed(self.vehicle) v = Vehicle.get_speed(self.agent.vehicle) # get next waypoint location wx, wy = next_waypoint.location.x, next_waypoint.location.y # debug logging # self.logger.debug(f"car location: ({x}, {y})") # self.logger.debug(f"car ψ: {ψ}") # self.logger.debug(f"car speed: {v}") # self.logger.debug(f"next waypoint: ({wx}, {wy})") ### 3D ### # get the index of next waypoint # waypoint_index = self.get_closest_waypoint_index_3D(location, # next_waypoint.location) # # find more waypoints index to fit a polynomial # waypoint_index_shifted = waypoint_index - 2 # indeces = waypoint_index_shifted + self.steps_poly * np.arange( # self.poly_degree + 1) # indeces = indeces % self.track_DF.shape[0] # # get waypoints for polynomial fitting # pts = np.array([[self.track_DF.iloc[i][0], self.track_DF.iloc[i][ # 1]] for i in indeces]) ### 2D ### index_2D = self.get_closest_waypoint_index_2D(location, next_waypoint.location) index_2D_shifted = index_2D - 5 indeces_2D = index_2D_shifted + self.steps_poly * np.arange( self.poly_degree + 1) indeces_2D = indeces_2D % self.pts_2D.shape[0] pts = self.pts_2D[indeces_2D] # self.logger.debug(f'\nwaypoint index:\n {index_2D}') # self.logger.debug(f'\nindeces:\n {indeces_2D}') # transform waypoints from world to car coorinate pts_car = VehicleMPCController.transform_into_cars_coordinate_system( pts, x, y, cos_ψ, sin_ψ ) # fit the polynomial poly = np.polyfit(pts_car[:, 0], pts_car[:, 1], self.poly_degree) # Debug # self.logger.debug(f'\nwaypoint index:\n {waypoint_index}') # self.logger.debug(f'\nindeces:\n {indeces}') # self.logger.debug(f'\npts for poly_fit:\n {pts}') # self.logger.debug(f'\npts_car:\n {pts_car}') ########### cte = poly[-1] eψ = -np.arctan(poly[-2]) init = (0, 0, 0, v, cte, eψ, *poly) self.state0 = self.get_state0(v, cte, eψ, self.steer, self.throttle, poly) result = self.minimize_cost(self.bounds, self.state0, init) # self.steer = -0.6 * cte - 5.5 * (cte - self.prev_cte) # self.prev_cte = cte # self.throttle = VehicleMPCController.clip_throttle(self.throttle, # v, self.target_speed) control = VehicleControl() if 'success' in result.message: self.steer = result.x[-self.steps_ahead] self.throttle = result.x[-2 * self.steps_ahead] else: self.logger.debug('Unsuccessful optimization') control.steering = self.steer control.throttle = self.throttle return control
def throttle_regularization(self, control: VehicleControl, min_throttle, max_throttle): control.throttle = np.interp(x=control.throttle, xp=[-1, 1], fp=[min_throttle, max_throttle]) return control